diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp index 0892165d6e..093e230a95 100644 --- a/libraries/AP_Math/vector2.cpp +++ b/libraries/AP_Math/vector2.cpp @@ -248,9 +248,63 @@ bool Vector2::circle_segment_intersection(const Vector2& seg_start, const return false; } +// normalizes this vector +template +void Vector2::normalize() +{ + *this /= length(); +} + +// returns the normalized vector +template +Vector2 Vector2::normalized() const +{ + return *this/length(); +} + +// reflects this vector about n +template +void Vector2::reflect(const Vector2 &n) +{ + const Vector2 orig(*this); + project(n); + *this = *this*2 - orig; +} + +// projects this vector onto v +template +void Vector2::project(const Vector2 &v) +{ + *this= v * (*this * v)/(v*v); +} + +// returns this vector projected onto v +template +Vector2 Vector2::projected(const Vector2 &v) +{ + return v * (*this * v)/(v*v); +} + +// given a position pos_delta and a velocity v1 produce a vector +// perpendicular to v1 maximising distance from p1 +template +Vector2 Vector2::perpendicular(const Vector2 &pos_delta, const Vector2 &v1) +{ + const Vector2 perpendicular1 = Vector2(-v1[1], v1[0]); + const Vector2 perpendicular2 = Vector2(v1[1], -v1[0]); + const T d1 = perpendicular1 * pos_delta; + const T d2 = perpendicular2 * pos_delta; + if (d1 > d2) { + return perpendicular1; + } + return perpendicular2; +} + // only define for float template float Vector2::length_squared(void) const; template float Vector2::length(void) const; +template Vector2 Vector2::normalized() const; +template void Vector2::normalize(); template float Vector2::operator *(const Vector2 &v) const; template float Vector2::operator %(const Vector2 &v) const; template Vector2 &Vector2::operator *=(const float num); diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h index ca7960db9a..d07986160f 100644 --- a/libraries/AP_Math/vector2.h +++ b/libraries/AP_Math/vector2.h @@ -137,50 +137,23 @@ struct Vector2 float length(void) const; // normalizes this vector - void normalize() - { - *this/=length(); - } + void normalize(); // returns the normalized vector - Vector2 normalized() const - { - return *this/length(); - } + Vector2 normalized() const; // reflects this vector about n - void reflect(const Vector2 &n) - { - const Vector2 orig(*this); - project(n); - *this= *this*2 - orig; - } + void reflect(const Vector2 &n); // projects this vector onto v - void project(const Vector2 &v) - { - *this= v * (*this * v)/(v*v); - } + void project(const Vector2 &v); // returns this vector projected onto v - Vector2 projected(const Vector2 &v) - { - return v * (*this * v)/(v*v); - } + Vector2 projected(const Vector2 &v); // given a position p1 and a velocity v1 produce a vector // perpendicular to v1 maximising distance from p1 - static Vector2 perpendicular(const Vector2 &pos_delta, const Vector2 &v1) - { - const Vector2 perpendicular1 = Vector2(-v1[1], v1[0]); - const Vector2 perpendicular2 = Vector2(v1[1], -v1[0]); - const T d1 = perpendicular1 * pos_delta; - const T d2 = perpendicular2 * pos_delta; - if (d1 > d2) { - return perpendicular1; - } - return perpendicular2; - } + static Vector2 perpendicular(const Vector2 &pos_delta, const Vector2 &v1); /* * Returns the point closest to p on the line segment (v,w).