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: add tests for line intersection

Browse Source
This commit is contained in:
Peter Barker 2018-08-13 13:40:22 +10:00 committed by Peter Barker
parent 1b46104a98
commit 626467db14
2 changed files with 103 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
libraries/AP_Math/tests/test_segment_intersection.cpp Normal file
View File

@ -0,0 +1,65 @@
#include <AP_gtest.h>
#include <AP_Math/vector2.h>
#define EXPECT_VECTOR2F_EQ(v1, v2) \
do { \
EXPECT_FLOAT_EQ(v1[0], v2[0]); \
EXPECT_FLOAT_EQ(v1[1], v2[1]); \
} while (false);
#define TEST_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y, ix,iy, expected) \
do { \
const Vector2f s1p1(s1p1x,s1p1y); \
const Vector2f s1p2(s1p2x,s1p2y); \
const Vector2f s2p1(s2p1x,s2p1y); \
const Vector2f s2p2(s2p2x,s2p2y); \
Vector2f calculated_intersection; \
bool result = Vector2f::segment_intersection(s1p1, s1p2, s2p1, s2p2, calculated_intersection); \
EXPECT_EQ(expected, result); \
if (result == expected && expected) { \
Vector2f expected_intersection{ix, iy}; \
EXPECT_VECTOR2F_EQ(calculated_intersection, expected_intersection); \
} \
result = Vector2f::segment_intersection(s2p1, s2p2, s1p1, s1p2, calculated_intersection); \
EXPECT_EQ(expected, result); \
if (expected) { \
Vector2f expected_intersection{ix, iy}; \
EXPECT_VECTOR2F_EQ(calculated_intersection, expected_intersection); \
EXPECT_EQ(Vector2f::point_on_segment(calculated_intersection,s1p1, s1p2), true); \
EXPECT_EQ(Vector2f::point_on_segment(calculated_intersection,s2p1, s2p2), true); \
} \
} while (false);
#define SHOULD_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y, ix, iy) \
TEST_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y, ix, iy, true)
#define SHOULD_NOT_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y) \
TEST_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y, 0, 0, false)
TEST(SegmentIntersectionTests, Simple)
{
SHOULD_INTERSECT(0,0,2,2, 2,0,0,2, 1,1);
SHOULD_INTERSECT(0,0,5,5, 2,0,0,2, 1,1);
}
TEST(SegmentIntersectionTests, Parallel)
{
SHOULD_NOT_INTERSECT(0,0,0,1, 2,0,2,1);
}
// the following should probably intersect but don't:
// TEST(SegmentIntersectionTests, Subsegment)
// {
// SHOULD_INTERSECT(0,0,0,2, 0,0,0,1, 1,1);
// }
// TEST(SegmentIntersectionTests, Identical)
// {
// SHOULD_INTERSECT(0,0,0,2, 0,0,0,2, 1,1);
// }
TEST(SegmentIntersectionTests, NonIntersecting)
{
SHOULD_NOT_INTERSECT(0,0,0,2, 1,1,1,2);
SHOULD_NOT_INTERSECT(0,0,0,1, -2,2,2,2)
}
AP_GTEST_MAIN()

38
libraries/AP_Math/vector2.h
View File

@ -230,6 +230,44 @@ struct Vector2
// returns true if they intersect and intersection argument is updated with intersection closest to seg_start
static bool circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, float radius, Vector2<T>& intersection);
static bool point_on_segment(const Vector2<T>& point,
const Vector2<T>& seg_start,
const Vector2<T>& seg_end) {
const float expected_run = seg_end.x-seg_start.x;
const float intersection_run = point.x-seg_start.x;
// check slopes are identical:
if (fabsf(expected_run) < FLT_EPSILON) {
if (fabsf(intersection_run) > FLT_EPSILON) {
return false;
}
} else {
const float expected_slope = (seg_end.y-seg_start.y)/expected_run;
const float intersection_slope = (point.y-seg_start.y)/intersection_run;
if (expected_slope != intersection_slope) {
return false;
}
}
// check for presence in bounding box
if (seg_start.x < seg_end.x) {
if (point.x < seg_start.x || point.x > seg_end.x) {
return false;
}
} else {
if (point.x < seg_end.x || point.x > seg_start.x) {
return false;
}
}
if (seg_start.y < seg_end.y) {
if (point.y < seg_start.y || point.y > seg_end.y) {
return false;
}
} else {
if (point.y < seg_end.y || point.y > seg_start.y) {
return false;
}
}
return true;
}
};
typedef Vector2<int16_t> Vector2i;