From 779f78d4717a09584df43ec713bc03202ed352f7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 20 Jun 2016 09:30:35 +1000 Subject: [PATCH] AP_Math: vector3 perpendicular function --- .../AP_Math/tests/test_perpendicular.cpp | 60 +++++++++++++++---- libraries/AP_Math/vector3.h | 15 +++++ 2 files changed, 64 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Math/tests/test_perpendicular.cpp b/libraries/AP_Math/tests/test_perpendicular.cpp index a42e80357e..dd91636a59 100644 --- a/libraries/AP_Math/tests/test_perpendicular.cpp +++ b/libraries/AP_Math/tests/test_perpendicular.cpp @@ -1,6 +1,7 @@ #include #include +#include #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); - 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); + // 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_3D(1.0f,1.0f,1.0f, 1.0f,-1.0f,1.0f, 0.66666666f,1.33333333f,0.66666666f); } AP_GTEST_MAIN() diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 842c4ec626..16c3022554 100644 --- a/libraries/AP_Math/vector3.h +++ b/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 perpendicular(const Vector3 &p1, const Vector3 &v1) + { + T d = p1 * v1; + if (fabsf(d) < FLT_EPSILON) { + return p1; + } + Vector3 parallel = (v1 * d) / v1.length_squared(); + Vector3 perpendicular = p1 - parallel; + + return perpendicular; + } };