AP_Math: add Vector3f::distance_squared

libraries/AP_Math/vector3.h

@@ -210,6 +210,14 @@ public:
         return v * (*this * v)/(v*v);
     }
 
+    // distance from the tip of this vector to another vector squared (so as to avoid the sqrt calculation)
+    float distance_squared(const Vector3<T> &v) const {
+        float dist_x = x-v.x;
+        float dist_y = y-v.y;
+        float dist_z = z-v.z;
+        return (dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
+    }
+
     // 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