Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add Vector3f::distance_to_segment

Browse Source
This commit is contained in:
Randy Mackay 2017-08-30 18:25:18 +09:00
parent 1a9bccaaa7
commit e892bbbac0
2 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Math/vector3.cpp
View File

@ -390,6 +390,32 @@ Matrix3<T> Vector3<T>::mul_rowcol(const Vector3<T> &v2) const
v1.z * v2.x, v1.z * v2.y, v1.z * v2.z);
}
// distance from the tip of this vector to a line segment specified by two vectors
template <typename T>
float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
{
// triangle side lengths
float a = (*this-seg_start).length();
float b = (seg_start-seg_end).length();
float c = (seg_end-*this).length();
// protect against divide by zero later
if (fabsf(b) < FLT_EPSILON) {
return 0.0f;
}
// semiperimeter of triangle
float s = (a+b+c)/2.0f;
float area_squared = s*(s-a)*(s-b)*(s-c);
// area must be constrained above 0 because a triangle could have 3 points could be on a line and float rounding could push this under 0
if (area_squared < 0.0f) {
area_squared = 0.0f;
}
float area = safe_sqrt(area_squared);
return 2.0f*area/b;
}
// define for float
template void Vector3<float>::rotate(enum Rotation);
template void Vector3<float>::rotate_inverse(enum Rotation);
@ -412,6 +438,7 @@ template bool Vector3<float>::operator !=(const Vector3<float> &v) const;
template bool Vector3<float>::is_nan(void) const;
template bool Vector3<float>::is_inf(void) const;
template float Vector3<float>::angle(const Vector3<float> &v) const;
template float Vector3<float>::distance_to_segment(const Vector3<float> &seg_start, const Vector3<float> &seg_end) const;
// define needed ops for Vector3l
template Vector3<int32_t> &Vector3<int32_t>::operator +=(const Vector3<int32_t> &v);

3
libraries/AP_Math/vector3.h
View File

@ -218,6 +218,9 @@ public:
return (dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
}
// distance from the tip of this vector to a line segment specified by two vectors
float distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const;
// 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