diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 631667e586..6ea5922e3b 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -272,11 +272,32 @@ float Vector3::angle(const Vector3 &v2) const return acosf(((*this)*v2) / (this->length()*v2.length())); } +// multiplication of transpose by a vector +template +Vector3 Vector3::operator *(const Matrix3 &m) const +{ + return Vector3(*this * m.colx(), + *this * m.coly(), + *this * m.colz()); +} + +// multiply a column vector by a row vector, returning a 3x3 matrix +template +Matrix3 Vector3::mul_rowcol(const Vector3 &v2) const +{ + const Vector3 v1 = *this; + return Matrix3(v1.x * v2.x, v1.x * v2.y, v1.x * v2.z, + v1.y * v2.x, v1.y * v2.y, v1.y * v2.z, + v1.z * v2.x, v1.z * v2.y, v1.z * v2.z); +} + // only define for float template void Vector3::rotate(enum Rotation); template float Vector3::length(void) const; template Vector3 Vector3::operator %(const Vector3 &v) const; template float Vector3::operator *(const Vector3 &v) const; +template Vector3 Vector3::operator *(const Matrix3 &m) const; +template Matrix3 Vector3::mul_rowcol(const Vector3 &v) const; template Vector3 &Vector3::operator *=(const float num); template Vector3 &Vector3::operator /=(const float num); template Vector3 &Vector3::operator -=(const Vector3 &v); diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 43fe67c185..7b7043ce98 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -44,9 +44,13 @@ #include #include +template +class Matrix3; + template class Vector3 { + public: T x, y, z; @@ -101,6 +105,12 @@ public: // dot product T operator *(const Vector3 &v) const; + // multiply a row vector by a matrix, to give a row vector + Vector3 operator *(const Matrix3 &m) const; + + // multiply a column vector by a row vector, returning a 3x3 matrix + Matrix3 mul_rowcol(const Vector3 &v) const; + // cross product Vector3 operator %(const Vector3 &v) const;