AP_Math: add Vector2f::segment_intersection

libraries/AP_Math/vector2.cpp

@@ -140,6 +140,39 @@ float Vector2<T>::angle(const Vector2<T> &v2) const
     return acosf(cosv);
 }
 
+// find the intersection between two line segments
+// returns true if they intersect, false if they do not
+// the point of intersection is returned in the intersection argument
+template <typename T>
+bool Vector2<T>::segment_intersection(const Vector2<T>& seg1_start, const Vector2<T>& seg1_end, const Vector2<T>& seg2_start, const Vector2<T>& seg2_end, Vector2<T>& intersection)
+{
+    // implementation borrowed from http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
+    const Vector2<T> r1 = seg1_end - seg1_start;
+    const Vector2<T> r2 = seg2_end - seg2_start;
+    const Vector2<T> ss2_ss1 = seg2_start - seg1_start;
+    const float r1xr2 = r1 % r2;
+    const float q_pxr = ss2_ss1 % r1;
+    if (fabsf(r1xr2) < FLT_EPSILON) {
+        // either collinear or parallel and non-intersecting
+        return false;
+    } else {
+        // t = (q - p) * s / (r * s)
+        // u = (q - p) * r / (r * s)
+        float t = (ss2_ss1 % r2) / r1xr2;
+        float u = q_pxr / r1xr2;
+        if ((u >= 0) && (u <= 1) && (t >= 0)) {
+            // lines intersect
+            // t can be any non-negative value because (p, p + r) is a ray
+            // u must be between 0 and 1 because (q, q + s) is a line segment
+            intersection = seg1_start + (r1*t);
+            return true;
+        } else {
+            // non-parallel and non-intersecting
+            return false;
+        }
+    }
+}
+
 // only define for float
 template float Vector2<float>::length(void) const;
 template float Vector2<float>::operator *(const Vector2<float> &v) const;
@@ -158,6 +191,7 @@ template bool Vector2<float>::operator !=(const Vector2<float> &v) const;
 template bool Vector2<float>::is_nan(void) const;
 template bool Vector2<float>::is_inf(void) const;
 template float Vector2<float>::angle(const Vector2<float> &v) const;
+template bool Vector2<float>::segment_intersection(const Vector2<float>& seg1_start, const Vector2<float>& seg1_end, const Vector2<float>& seg2_start, const Vector2<float>& seg2_end, Vector2<float>& intersection);
 
 template bool Vector2<long>::operator ==(const Vector2<long> &v) const;
 

libraries/AP_Math/vector2.h

@@ -221,6 +221,11 @@ struct Vector2
         return delta.length();
     }
 
+    // find the intersection between two line segments
+    // returns true if they intersect, false if they do not
+    // the point of intersection is returned in the intersection argument
+    static bool segment_intersection(const Vector2<T>& seg1_start, const Vector2<T>& seg1_end, const Vector2<T>& seg2_start, const Vector2<T>& seg2_end, Vector2<T>& intersection);
+
 };
 
 typedef Vector2<int16_t>        Vector2i;