Merge branch 'fredb/winfix'

[simgear.git] / simgear / timing / geocoord.cxx

diff --git a/simgear/timing/geocoord.cxx b/simgear/timing/geocoord.cxx
index b904ca054e0d6eaf0edccd1790f5e37db955a4e9..d3f522600643e38167c34d165e08b6cae7fcd529 100644 (file)
--- a/simgear/timing/geocoord.cxx
+++ b/simgear/timing/geocoord.cxx
@@ -12,10 +12,9 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  * Library General Public License for more details.
  *
- * You should have received a copy of the GNU Library General Public
- * License along with this library; if not, write to the
- * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA  02111-1307, USA.
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  *
  **************************************************************************/
 
@@ -28,8 +27,12 @@
  * written for FlightGear, in order to store Timezone control points. 
  *
  ************************************************************************/
+#ifdef HAVE_CONFIG_H
+#  include <simgear_config.h>
+#endif
+
+#include <simgear/math/SGMath.hxx>
 #include "geocoord.h"
-#include <plib/sg.h>
 
 SGGeoCoord::SGGeoCoord(const SGGeoCoord& other)
 {
@@ -37,57 +40,20 @@ SGGeoCoord::SGGeoCoord(const SGGeoCoord& other)
   lon = other.lon;
 }
 
-// double SGGeoCoord::getAngle(const SGGeoCoord& other) const
-// {
-//   Vector first(      getX(),       getY(),       getZ());
-//   Vector secnd(other.getX(), other.getY(), other.getZ());
-//     double
-//       dot = VecDot(first, secnd),
-//       len1 = first.VecLen(),
-//       len2 = secnd.VecLen(),
-//       len = len1 * len2,
-//       angle = 0;
-//     //printf ("Dot: %f, len1: %f len2: %f\n", dot, len1, len2);
-//     /*Vector pPos = prevPos - Reference->prevPos;
-//       Vector pVel = prevVel - Reference->prevVel;*/
-
-
-//     if ( ( (dot / len) < 1) && (dot / len > -1) && len )
-//             angle = acos(dot / len);
-//     return angle;
-// }
-
-// SGGeoCoord* SGGeoCoordContainer::getNearest(const SGGeoCoord& ref) const
-// {
-//   float angle, maxAngle = 180;
-
-//   SGGeoCoordVectorConstIterator i, nearest;
-//   for (i = data.begin(); i != data.end(); i++)
-//     {
-//       angle = SGD_RADIANS_TO_DEGREES * (*i)->getAngle(ref);
-//       if (angle < maxAngle)
-//     {
-//       maxAngle = angle;
-//       nearest = i;
-//     }
-//     }
-//   return *nearest;
-// }
-
-
 SGGeoCoord* SGGeoCoordContainer::getNearest(const SGGeoCoord& ref) const
 {
-  sgVec3 first, secnd;
-  float dist, maxDist=SG_MAX;
-  sgSetVec3( first, ref.getX(), ref.getY(), ref.getZ());
+  if (data.empty())
+    return 0;
+
+  float maxCosAng = -2;
+  SGVec3f refVec(ref.getX(), ref.getY(), ref.getZ());
   SGGeoCoordVectorConstIterator i, nearest;
-  for (i = data.begin(); i != data.end(); i++)
+  for (i = data.begin(); i != data.end(); ++i)
     {
-      sgSetVec3(secnd, (*i)->getX(), (*i)->getY(), (*i)->getZ());
-      dist = sgDistanceSquaredVec3(first, secnd);
-      if (dist < maxDist)
+      float cosAng = dot(refVec, SGVec3f((*i)->getX(), (*i)->getY(), (*i)->getZ()));
+      if (maxCosAng < cosAng)
        {
-         maxDist = dist;
+         maxCosAng = cosAng;
          nearest = i;
        }
     }