From e6328c350df359df1ae47f12e2692b7f6e39b877 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 26 Aug 2017 13:53:43 +0900 Subject: [PATCH] AP_Math: add Vector3f::distance_squared --- libraries/AP_Math/vector3.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index e5b5b55b42..1df67c947a 100644 --- a/libraries/AP_Math/vector3.h +++ b/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 &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