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: Update segment_to_segment_dis test

Browse Source
This commit is contained in:
Rishabh 2021-06-03 00:27:24 +05:30 committed by Randy Mackay
parent 730cf0ad9b
commit b9fbf1a661
1 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Math/tests/test_3d_lines.cpp
View File

@ -14,7 +14,7 @@
TEST(Lines3dTests, ClosestDistBetweenLinePoint) TEST(Lines3dTests, ClosestDistBetweenLinePoint)
{ {
// check if the 2-d and 3-d variant of this method is same if the third-dimension is zero // check if the 2-d and 3-d variant of this method is same if the third-dimension is zero
float dist_3d = Vector3f::closest_distance_between_line_and_point(Vector3f{0.0f, 1.0f, 0.0f}, Vector3f{0.0f, 10.0f, 0.0f}, Vector3f{6.0f, 5.0f, 0.0f}); float dist_3d = Vector3f::closest_distance_between_line_and_point(Vector3f{0.0f, 1.0f, 0.0f}, Vector3f{0.0f, 10.0f, 0.0f}, Vector3f{6.0f, 5.0f, 0.0f});
float dist_2d = Vector2f::closest_distance_between_line_and_point(Vector2f{0.0f, 1.0f}, Vector2f{0.0f, 10.0f}, Vector2f{6.0f, 5.0f}); float dist_2d = Vector2f::closest_distance_between_line_and_point(Vector2f{0.0f, 1.0f}, Vector2f{0.0f, 10.0f}, Vector2f{6.0f, 5.0f});
@ -29,20 +29,18 @@ TEST(Lines3dTests, ClosestDistBetweenLinePoint)
EXPECT_VECTOR3F_EQ((Vector3f{0.0f, 0.0f, 0.0f}), intersection_null); EXPECT_VECTOR3F_EQ((Vector3f{0.0f, 0.0f, 0.0f}), intersection_null);
} }
TEST(Lines3dTests, SegmentToSegmentDistance) TEST(Lines3dTests, SegmentToSegmentCloestPoint)
{ {
// random segments test // random segments test
Vector3f intersection; Vector3f intersection;
float dist = Vector3f::segment_to_segment_dist(Vector3f{-10.0f,0.0f,0.0f}, Vector3f{10.0f,0.0f,0.0f}, Vector3f{0.0f, -5.0f, 1.0}, Vector3f{0.0f, 5.0f, 1.0f}, intersection); Vector3f::segment_to_segment_closest_point(Vector3f{-10.0f,0.0f,0.0f}, Vector3f{10.0f,0.0f,0.0f}, Vector3f{0.0f, -5.0f, 1.0}, Vector3f{0.0f, 5.0f, 1.0f}, intersection);
EXPECT_FLOAT_EQ(dist, 1.0f);
EXPECT_VECTOR3F_EQ(intersection, (Vector3f{0.0f, 0.0f, 1.0f})); EXPECT_VECTOR3F_EQ(intersection, (Vector3f{0.0f, 0.0f, 1.0f}));
// check for intersecting segments. Verify with the 2-d variant // check for intersecting segments. Verify with the 2-d variant
dist = Vector3f::segment_to_segment_dist(Vector3f{}, Vector3f{10.0f,10.0f,0.0f}, Vector3f{2.0f, -10.0f, 0.0}, Vector3f{3.0f, 10.0f, 0.0f}, intersection); Vector3f::segment_to_segment_closest_point(Vector3f{}, Vector3f{10.0f,10.0f,0.0f}, Vector3f{2.0f, -10.0f, 0.0}, Vector3f{3.0f, 10.0f, 0.0f}, intersection);
Vector2f intersection_2d; Vector2f intersection_2d;
const bool result = Vector2f::segment_intersection(Vector2f{}, Vector2f{10.0f,10.0}, Vector2f{2.0f, -10.0f}, Vector2f{3.0f, 10.0f}, intersection_2d); const bool result = Vector2f::segment_intersection(Vector2f{}, Vector2f{10.0f,10.0}, Vector2f{2.0f, -10.0f}, Vector2f{3.0f, 10.0f}, intersection_2d);
EXPECT_EQ(true, result); EXPECT_EQ(true, result);
EXPECT_FLOAT_EQ(dist, 0.0f);
EXPECT_VECTOR3F_EQ(intersection, (Vector3f(intersection_2d.x, intersection_2d.y, 0.0f))); EXPECT_VECTOR3F_EQ(intersection, (Vector3f(intersection_2d.x, intersection_2d.y, 0.0f)));
} }