mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
AP_Math: Remove an unneeded safe_sqrtf, leverage is_zero
This commit is contained in:
parent
6e84a31613
commit
48610ea0a0
@ -191,7 +191,7 @@ bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const
|
||||
const float delta = sq(b) - (4.0f * a * c);
|
||||
|
||||
// check for invalid data
|
||||
if (fabsf(a) < FLT_EPSILON) {
|
||||
if (::is_zero(a)) {
|
||||
return false;
|
||||
}
|
||||
if (isnan(a) || isnan(b) || isnan(c) || isnan(delta)) {
|
||||
@ -203,7 +203,7 @@ bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const
|
||||
return false;
|
||||
}
|
||||
|
||||
const float delta_sqrt = safe_sqrt(delta);
|
||||
const float delta_sqrt = sqrtf(delta);
|
||||
const float t1 = (-b + delta_sqrt) / (2.0f * a);
|
||||
const float t2 = (-b - delta_sqrt) / (2.0f * a);
|
||||
|
||||
|
@ -415,7 +415,7 @@ float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3
|
||||
float c = (seg_end-*this).length();
|
||||
|
||||
// protect against divide by zero later
|
||||
if (fabsf(b) < FLT_EPSILON) {
|
||||
if (::is_zero(b)) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user