AP_Math: vector3 perpendicular function

libraries/AP_Math/tests/test_perpendicular.cpp

@@ -1,6 +1,7 @@
 #include <AP_gtest.h>
 
 #include <AP_Math/vector2.h>
+#include <AP_Math/vector3.h>
 
 #define EXPECT_VECTOR2F_EQ(v1, v2)              \
     do {                                        \
@@ -8,26 +9,63 @@
         EXPECT_FLOAT_EQ(v1[1], v2[1]);          \
     } while (false);
 
-#define PERP_TEST(px,py, vx,vy, ex,ey)              \
+#define EXPECT_VECTOR3F_EQ(v1, v2)              \
+    do {                                        \
+        EXPECT_FLOAT_EQ(v1[0], v2[0]);          \
+        EXPECT_FLOAT_EQ(v1[1], v2[1]);          \
+        EXPECT_FLOAT_EQ(v1[2], v2[2]);          \
+    } while (false);
+
+
+#define PERP_TEST_2D(px,py, vx,vy, ex,ey)               \
     do {                                                \
-        Vector2f p(px,py);                           \
-        Vector2f v(vx,vy);                           \
+        Vector2f p(px,py);                              \
+        Vector2f v(vx,vy);                              \
         Vector2f expected(ex,ey);                       \
         Vector2f result;                                \
-        result = Vector2f::perpendicular(p, v);   \
-        EXPECT_VECTOR2F_EQ(result, expected);           \
+        result = Vector2f::perpendicular(p, v);         \
+        EXPECT_VECTOR2F_EQ(expected, result);           \
     } while (false)
 
+#define PERP_TEST_3D(px,py,pz, vx,vy,vz, ex,ey,ez)      \
+    do {                                                \
+        Vector3f p(px,py,pz);                           \
+        Vector3f v(vx,vy,vz);                           \
+        Vector3f expected(ex,ey,ez);                    \
+        Vector3f result;                                \
+        result = Vector3f::perpendicular(p, v);         \
+        EXPECT_VECTOR3F_EQ(expected, result);           \
+    } while (false)
+
+void foo() { } 
 TEST(ThreatTests, Distance)
 {
 
-    PERP_TEST( 0.0f,0.0f, 0.0f,0.0f, 0.0f,0.0f);
-    PERP_TEST( 0.0f,0.0f, 1.0f,0.0f, 0.0f,-1.0f);
-    PERP_TEST( 2.0f,0.0f, 1.0f,0.0f, 0.0f,-1.0f);
-    PERP_TEST( 0.0f,2.0f, 1.0f,0.0f, 0.0f,1.0f);
+    PERP_TEST_2D( 0.0f,0.0f, 0.0f,0.0f, 0.0f,0.0f);
+    PERP_TEST_2D( 0.0f,0.0f, 1.0f,0.0f, 0.0f,-1.0f);
+    PERP_TEST_2D( 2.0f,0.0f, 1.0f,0.0f, 0.0f,-1.0f);
+    PERP_TEST_2D( 0.0f,2.0f, 1.0f,0.0f, 0.0f,1.0f);
+    PERP_TEST_2D( 0.0f,2.0f, 1.0f,2.0f, -2.0f,1.0f);
+    PERP_TEST_2D( 2.0f,0.0f, 1.0f,2.0f, 2.0f,-1.0f);
+
+    // 2D cases for the 3D code:
+    PERP_TEST_3D( 0.0f,0.0f,0.0f, 0.0f,0.0f,0.0f, 0.0f,0.0f,0.0f);
+    PERP_TEST_3D( 0.0f,0.0f,0.0f, 1.0f,0.0f,0.0f, 0.0f,0.0f,0.0f);
+    PERP_TEST_3D( 2.0f,0.0f,0.0f, 1.0f,0.0f,0.0f, 0.0f,0.0f,0.0f);
+    foo();
+    PERP_TEST_3D( 0.0f,2.0f,0.0f, 1.0f,0.0f,0.0f, 0.0f,2.0f,0.0f);
+    PERP_TEST_3D( 0.0f,2.0f,0.0f, 1.0f,2.0f,0.0f, -0.8f,0.4f,0.0f);
+    PERP_TEST_3D( 2.0f,0.0f,0.0f, 1.0f,2.0f,0.0f, 1.6f,-0.8f,0.0f);
+
+
+    // 3D-specific tests
+    PERP_TEST_3D( 1.0f,1.0f,1.0f, 1.0f,0.0f,0.0f, 0.0f,1.0f,1.0f);
+    PERP_TEST_3D( -1.0f,-1.0f,-1.0f, -1.0f,0.0f,0.0f, 0.0f,-1.0f,-1.0f);
+    PERP_TEST_3D(1.0f,1.0f,0.0f, 1.0f,0.0f,0.0f, 0.0f,1.0f,0.0f);
+
+    PERP_TEST_3D(1.0f,1.0f,1.0f, 1.0f,-1.0f,0.0f, 1.0f,1.0f,1.0f);
 
-    PERP_TEST( 0.0f,2.0f, 1.0f,2.0f, -2.0f,1.0f);
-    PERP_TEST( 2.0f,0.0f, 1.0f,2.0f, 2.0f,-1.0f);
+    PERP_TEST_3D(1.0f,1.0f,1.0f, 1.0f,-1.0f,1.0f, 0.66666666f,1.33333333f,0.66666666f);
 }
 
 AP_GTEST_MAIN()

libraries/AP_Math/vector3.h

@@ -211,6 +211,21 @@ public:
         return v * (*this * v)/(v*v);
     }
 
+    // given a position p1 and a velocity v1 produce a vector
+    // perpendicular to v1 maximising distance from p1.  If p1 is the
+    // zero vector the return from the function will always be the
+    // zero vector - that should be checked for.
+    static Vector3<T> perpendicular(const Vector3<T> &p1, const Vector3<T> &v1)
+    {
+        T d = p1 * v1;
+        if (fabsf(d) < FLT_EPSILON) {
+            return p1;
+        }
+        Vector3<T> parallel = (v1 * d) / v1.length_squared();
+        Vector3<T> perpendicular = p1 - parallel;
+
+        return perpendicular;
+    }
 
 };