Browse Source

AP_Math: judge isnan judgment before operation

zr-v5.1
murata 5 years ago committed by Peter Barker
parent
commit
4ffc559ccc
  1. 12
      libraries/AP_Math/vector2.cpp

12
libraries/AP_Math/vector2.cpp

@ -199,15 +199,21 @@ bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const @@ -199,15 +199,21 @@ bool Vector2<T>::circle_segment_intersection(const Vector2<T>& seg_start, const
const float a = sq(seg_end_minus_start.x) + sq(seg_end_minus_start.y);
const float b = 2 * ((seg_end_minus_start.x * seg_start_local.x) + (seg_end_minus_start.y * seg_start_local.y));
const float c = sq(seg_start_local.x) + sq(seg_start_local.y) - sq(radius);
if (isnan(a) || isnan(b) || isnan(c)) {
return false;
}
const float delta = sq(b) - (4.0f * a * c);
if (isnan(delta)) {
return false;
}
// check for invalid data
if (::is_zero(a)) {
return false;
}
if (isnan(a) || isnan(b) || isnan(c) || isnan(delta)) {
return false;
}
// check for invalid delta (i.e. discriminant)
if (delta < 0.0f) {

Loading…
Cancel
Save