Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Add segment to plane intersection function

This commit is contained in:
Rishabh 2021-05-27 00:33:22 +05:30 committed by Randy Mackay
parent cb911a01e2
commit 547f0efd57
2 changed files with 46 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Math/vector3.cpp
View File

@ -512,12 +512,12 @@ Vector3<T> Vector3<T>::point_on_line_closest_to_other_point(const Vector3<T> &w1
return (closest_point + w1);
}
// Shortest distance between two line segments
// Closest point between two line segments
// This implementation is borrowed from: http://geomalgorithms.com/a07-_distance.html
// INPUT: 4 points corresponding to start and end of two line segments
// OUTPUT: shortest distance, and closest point on segment 2, from segment 1, gets passed on reference as "intersection"
// OUTPUT: closest point on segment 2, from segment 1, gets passed on reference as "closest_point"
template <typename T>
float Vector3<T>::segment_to_segment_dist(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& intersection)
void Vector3<T>::segment_to_segment_closest_point(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& closest_point)
{
// direction vectors
const Vector3<T> line1 = seg1_end - seg1_start;
@ -532,7 +532,7 @@ float Vector3<T>::segment_to_segment_dist(const Vector3<T>& seg1_start, const Ve
const float e = line2*diff;
const float discriminant = (a*c) - (b*b);
float sc, sN, sD = discriminant; // sc = sN / sD, default sD = D >= 0
float sN, sD = discriminant; // default sD = D >= 0
float tc, tN, tD = discriminant; // tc = tN / tD, default tD = D >= 0
if (discriminant < FLT_EPSILON) {
@ -582,14 +582,39 @@ float Vector3<T>::segment_to_segment_dist(const Vector3<T>& seg1_start, const Ve
sD = a;
}
}
// finally do the division to get sc and tc
sc = (fabsf(sN) < FLT_EPSILON ? 0.0 : sN / sD);
// finally do the division to get tc
tc = (fabsf(tN) < FLT_EPSILON ? 0.0 : tN / tD);
const Vector3<T> closest_line_segment = diff + (line1*sc) - (line2*tc);
const float len = closest_line_segment.length();
intersection = seg2_start + line2*tc;
return len;
// closest point on seg2
closest_point = seg2_start + line2*tc;
}
// Returns true if the passed 3D segment passes through a plane defined by plane normal, and a point on the plane
template <typename T>
bool Vector3<T>::segment_plane_intersect(const Vector3<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point)
{
Vector3<T> u = seg_end - seg_start;
Vector3<T> w = seg_start - plane_point;
float D = plane_normal * u;
float N = -(plane_normal * w);
if (fabsf(D) < FLT_EPSILON) {
if (::is_zero(N)) {
// segment lies in this plane
return true;
} else {
// does not intersect
return false;
}
}
const float sI = N / D;
if (sI < 0 || sI > 1) {
// does not intersect
return false;
}
// intersects at unique point
return true;
}
// define for float and double

7
libraries/AP_Math/vector3.h
View File

@ -277,8 +277,11 @@ public:
// This implementation is borrowed from: http://geomalgorithms.com/a07-_distance.html
// INPUT: 4 points corresponding to start and end of two line segments
// OUTPUT: shortest distance between segments, and closest point on segment 2, from segment 1, gets passed on reference as "intersection"
static float segment_to_segment_dist(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& intersection) WARN_IF_UNUSED;
// OUTPUT: closest point on segment 2, from segment 1, gets passed on reference as "closest_point"
static void segment_to_segment_closest_point(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& closest_point);
// Returns true if the passed 3D segment passes through a plane defined by plane normal, and a point on the plane
static bool segment_plane_intersect(const Vector3<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point);
};
typedef Vector3<int16_t> Vector3i;