diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 71ffc08944..921c852ed7 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -617,6 +617,13 @@ bool Vector3::segment_plane_intersect(const Vector3& seg_start, const Vect return true; } +// return xy components of a vector3 +template +Vector2 Vector3::xy() +{ + return Vector2{x,y}; +} + // define for float and double template class Vector3; template class Vector3; diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 751303dadd..150c040332 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -63,6 +63,9 @@ template class Matrix3; +template +class Vector2; + template class Vector3 { @@ -189,6 +192,9 @@ public: // rotate vector by angle in radians in xy plane leaving z untouched void rotate_xy(float rotation_rad); + // return xy components of a vector3 + Vector2 xy(); + // gets the length of this vector squared T length_squared() const {