Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: vector3 perpendicular function

Browse Source
This commit is contained in:
Peter Barker 2016-06-20 09:30:35 +10:00 committed by Randy Mackay
parent f6cb0ffb6f
commit 779f78d471
2 changed files with 64 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_Math/tests/test_perpendicular.cpp
View File

@ -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 expected(ex,ey); \
Vector2f result; \
result = Vector2f::perpendicular(p, v); \
EXPECT_VECTOR2F_EQ(result, expected); \
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()

15
libraries/AP_Math/vector3.h
View File

@ -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;
}
};