Added point3d.hxx to replace cheezy fgPoint3d struct.

[simgear.git] / Math / vector.cxx

diff --git a/Math/vector.cxx b/Math/vector.cxx
index 4da0af44f0a8db910db21cfa44b566a1e46b7bc3..641c056e7fe5bf7b3bbc6564129503c410322d59 100644 (file)
--- a/Math/vector.cxx
+++ b/Math/vector.cxx
@@ -27,13 +27,14 @@
 #include <math.h>
 #include <stdio.h>
 
-#include <Include/fg_types.h>
+// #include <Include/fg_types.h>
 
 #include "vector.hxx"
 
 #include "mat3.h"
 
 
+#if !defined( USE_XTRA_MAT3_INLINES )
 /* Map a vector onto the plane specified by normal */
 void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec,
                                    MAT3vec result)
@@ -78,6 +79,7 @@ void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec,
     /* printf("  result = %.2f, %.2f, %.2f\n", 
        result[0], result[1], result[2]); */
 }
+#endif // !defined( USE_XTRA_MAT3_INLINES )
 
 
 // Given a point p, and a line through p0 with direction vector d,
@@ -107,12 +109,47 @@ double fgPointLine(MAT3vec p, MAT3vec p0, MAT3vec d) {
 }
 
 
+// Given a point p, and a line through p0 with direction vector d,
+// find the shortest distance (squared) from the point to the line
+double fgPointLineSquared(MAT3vec p, MAT3vec p0, MAT3vec d) {
+    MAT3vec u, u1, v;
+    double ud, dd, tmp;
+    
+    // u = p - p0
+    MAT3_SUB_VEC(u, p, p0);
+
+    // calculate the projection, u1, of u along d.
+    // u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d;
+    ud = MAT3_DOT_PRODUCT(u, d);
+    dd = MAT3_DOT_PRODUCT(d, d);
+    tmp = ud / dd;
+
+    MAT3_SCALE_VEC(u1, d, tmp);;
+
+    // v = u - u1 = vector from closest point on line, p1, to the
+    // original point, p.
+    MAT3_SUB_VEC(v, u, u1);
+
+    return ( MAT3_DOT_PRODUCT(v, v) );
+}
+
+
 /* $Log$
-/* Revision 1.1  1998/07/08 14:40:10  curt
-/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx
-/* Updated fg_geodesy comments to reflect that routines expect and produce
-/*   meters.
+/* Revision 1.4  1998/10/16 00:50:31  curt
+/* Added point3d.hxx to replace cheezy fgPoint3d struct.
 /*
+ * Revision 1.3  1998/08/24 20:04:12  curt
+ * Various "inline" code optimizations contributed by Norman Vine.
+ *
+ * Revision 1.2  1998/07/24 21:34:38  curt
+ * fgPointLine() rewritten into fgPointLineSquared() ... this ultimately saves
+ * us from doing a sqrt().
+ *
+ * Revision 1.1  1998/07/08 14:40:10  curt
+ * polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx
+ * Updated fg_geodesy comments to reflect that routines expect and produce
+ *   meters.
+ *
  * Revision 1.3  1998/05/07 23:04:28  curt
  * Added a blank formating line!
  *