Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: moved a lot of vector templates to cpp from .h

Browse Source 
this reduces the code size quite a lot on AVR
This commit is contained in:
Andrew Tridgell 2013-04-12 10:04:18 +10:00
parent 78eb12a4ac
commit 43c3c60de2
4 changed files with 278 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
libraries/AP_Math/vector2.cpp
View File

@ -25,4 +25,116 @@ float Vector2<T>::length(void) const
return pythagorous2(x, y); return pythagorous2(x, y);
} }
// dot product
template <typename T>
T Vector2<T>::operator *(const Vector2<T> &v) const
{
return x*v.x + y*v.y;
}
template <typename T>
Vector2<T> &Vector2<T>::operator *=(const T num)
{
x*=num; y*=num;
return *this;
}
template <typename T>
Vector2<T> &Vector2<T>::operator /=(const T num)
{
x /= num; y /= num;
return *this;
}
template <typename T>
Vector2<T> &Vector2<T>::operator -=(const Vector2<T> &v)
{
x -= v.x; y -= v.y;
return *this;
}
template <typename T>
bool Vector2<T>::is_nan(void) const
{
return isnan(x) || isnan(y);
}
template <typename T>
bool Vector2<T>::is_inf(void) const
{
return isinf(x) || isinf(y);
}
template <typename T>
Vector2<T> &Vector2<T>::operator +=(const Vector2<T> &v)
{
x+=v.x; y+=v.y;
return *this;
}
template <typename T>
Vector2<T> Vector2<T>::operator /(const T num) const
{
return Vector2<T>(x/num, y/num);
}
template <typename T>
Vector2<T> Vector2<T>::operator *(const T num) const
{
return Vector2<T>(x*num, y*num);
}
template <typename T>
Vector2<T> Vector2<T>::operator -(const Vector2<T> &v) const
{
return Vector2<T>(x-v.x, y-v.y);
}
template <typename T>
Vector2<T> Vector2<T>::operator +(const Vector2<T> &v) const
{
return Vector2<T>(x+v.x, y+v.y);
}
template <typename T>
Vector2<T> Vector2<T>::operator -(void) const
{
return Vector2<T>(-x,-y);
}
template <typename T>
bool Vector2<T>::operator ==(const Vector2<T> &v) const
{
return (x==v.x && y==v.y);
}
template <typename T>
bool Vector2<T>::operator !=(const Vector2<T> &v) const
{
return (x!=v.x && y!=v.y);
}
template <typename T>
float Vector2<T>::angle(const Vector2<T> &v2) const
{
return acosf(((*this)*v2) / (this->length()*v2.length()));
}
// only define for float
template float Vector2<float>::length(void) const; template float Vector2<float>::length(void) const;
template float Vector2<float>::operator *(const Vector2<float> &v) const;
template Vector2<float> &Vector2<float>::operator *=(const float num);
template Vector2<float> &Vector2<float>::operator /=(const float num);
template Vector2<float> &Vector2<float>::operator -=(const Vector2<float> &v);
template Vector2<float> &Vector2<float>::operator +=(const Vector2<float> &v);
template Vector2<float> Vector2<float>::operator /(const float num) const;
template Vector2<float> Vector2<float>::operator *(const float num) const;
template Vector2<float> Vector2<float>::operator +(const Vector2<float> &v) const;
template Vector2<float> Vector2<float>::operator -(const Vector2<float> &v) const;
template Vector2<float> Vector2<float>::operator -(void) const;
template bool Vector2<float>::operator ==(const Vector2<float> &v) const;
template bool Vector2<float>::operator !=(const Vector2<float> &v) const;
template bool Vector2<float>::is_nan(void) const;
template bool Vector2<float>::is_inf(void) const;
template float Vector2<float>::angle(const Vector2<float> &v) const;

106
libraries/AP_Math/vector2.h
View File

@ -46,82 +46,55 @@ struct Vector2
} }
// test for equality // test for equality
bool operator==(const Vector2<T> &v) bool operator ==(const Vector2<T> &v) const;
{
return (x==v.x && y==v.y);
}
// test for inequality // test for inequality
bool operator!=(const Vector2<T> &v) bool operator !=(const Vector2<T> &v) const;
{
return (x!=v.x || y!=v.y);
}
// negation // negation
Vector2<T> operator -(void) const Vector2<T> operator -(void) const;
{
return Vector2<T>(-x, -y);
}
// addition // addition
Vector2<T> operator +(const Vector2<T> &v) const Vector2<T> operator +(const Vector2<T> &v) const;
{
return Vector2<T>(x+v.x, y+v.y);
}
// subtraction // subtraction
Vector2<T> operator -(const Vector2<T> &v) const Vector2<T> operator -(const Vector2<T> &v) const;
{
return Vector2<T>(x-v.x, y-v.y);
}
// uniform scaling // uniform scaling
Vector2<T> operator *(const T num) const Vector2<T> operator *(const T num) const;
{
Vector2<T> temp(*this);
return temp*=num;
}
// uniform scaling // uniform scaling
Vector2<T> operator /(const T num) const Vector2<T> operator /(const T num) const;
{
Vector2<T> temp(*this);
return temp/=num;
}
// addition // addition
Vector2<T> &operator +=(const Vector2<T> &v) Vector2<T> &operator +=(const Vector2<T> &v);
{
x+=v.x; y+=v.y;
return *this;
}
// subtraction // subtraction
Vector2<T> &operator -=(const Vector2<T> &v) Vector2<T> &operator -=(const Vector2<T> &v);
{
x-=v.x; y-=v.y;
return *this;
}
// uniform scaling // uniform scaling
Vector2<T> &operator *=(const T num) Vector2<T> &operator *=(const T num);
{
x*=num; y*=num;
return *this;
}
// uniform scaling // uniform scaling
Vector2<T> &operator /=(const T num) Vector2<T> &operator /=(const T num);
{
x/=num; y/=num;
return *this;
}
// dot product // dot product
T operator *(const Vector2<T> &v) const T operator *(const Vector2<T> &v) const;
{
return x*v.x + y*v.y; // cross product
} Vector2<T> operator %(const Vector2<T> &v) const;
// computes the angle between this vector and another vector
float angle(const Vector2<T> &v2) const;
// computes the angle in radians between the origin and this vector
T angle(void) const;
// check if any elements are NAN
bool is_nan(void) const;
// check if any elements are infinity
bool is_inf(void) const;
// gets the length of this vector squared // gets the length of this vector squared
T length_squared() const T length_squared() const
@ -163,31 +136,6 @@ struct Vector2
{ {
return v * (*this * v)/(v*v); return v * (*this * v)/(v*v);
} }
// computes the angle between 2 arbitrary vectors
T angle(const Vector2<T> &v1, const Vector2<T> &v2)
{
return (T)acosf((v1*v2) / (v1.length()*v2.length()));
}
// computes the angle in radians between the origin and this vector
T angle(void)
{
return (T)atan2f(y, x);
}
// computes the angle between this vector and another vector
T angle(const Vector2<T> &v2)
{
return (T)acosf(((*this)*v2) / (this->length()*v2.length()));
}
// computes the angle between 2 normalized arbitrary vectors
T angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2)
{
return (T)acosf(v1*v2);
}
}; };
typedef Vector2<int16_t> Vector2i; typedef Vector2<int16_t> Vector2i;

104
libraries/AP_Math/vector3.cpp
View File

@ -184,8 +184,110 @@ float Vector3<T>::length(void) const
return pythagorous3(x, y, z); return pythagorous3(x, y, z);
} }
// only define for signed numbers template <typename T>
Vector3<T> &Vector3<T>::operator *=(const T num)
{
x*=num; y*=num; z*=num;
return *this;
}
template <typename T>
Vector3<T> &Vector3<T>::operator /=(const T num)
{
x /= num; y /= num; z /= num;
return *this;
}
template <typename T>
Vector3<T> &Vector3<T>::operator -=(const Vector3<T> &v)
{
x -= v.x; y -= v.y; z -= v.z;
return *this;
}
template <typename T>
bool Vector3<T>::is_nan(void) const
{
return isnan(x) || isnan(y) || isnan(z);
}
template <typename T>
bool Vector3<T>::is_inf(void) const
{
return isinf(x) || isinf(y) || isinf(z);
}
template <typename T>
Vector3<T> &Vector3<T>::operator +=(const Vector3<T> &v)
{
x+=v.x; y+=v.y; z+=v.z;
return *this;
}
template <typename T>
Vector3<T> Vector3<T>::operator /(const T num) const
{
return Vector3<T>(x/num, y/num, z/num);
}
template <typename T>
Vector3<T> Vector3<T>::operator *(const T num) const
{
return Vector3<T>(x*num, y*num, z*num);
}
template <typename T>
Vector3<T> Vector3<T>::operator -(const Vector3<T> &v) const
{
return Vector3<T>(x-v.x, y-v.y, z-v.z);
}
template <typename T>
Vector3<T> Vector3<T>::operator +(const Vector3<T> &v) const
{
return Vector3<T>(x+v.x, y+v.y, z+v.z);
}
template <typename T>
Vector3<T> Vector3<T>::operator -(void) const
{
return Vector3<T>(-x,-y,-z);
}
template <typename T>
bool Vector3<T>::operator ==(const Vector3<T> &v) const
{
return (x==v.x && y==v.y && z==v.z);
}
template <typename T>
bool Vector3<T>::operator !=(const Vector3<T> &v) const
{
return (x!=v.x && y!=v.y && z!=v.z);
}
template <typename T>
float Vector3<T>::angle(const Vector3<T> &v2) const
{
return acosf(((*this)*v2) / (this->length()*v2.length()));
}
// only define for float
template void Vector3<float>::rotate(enum Rotation); template void Vector3<float>::rotate(enum Rotation);
template float Vector3<float>::length(void) const; template float Vector3<float>::length(void) const;
template Vector3<float> Vector3<float>::operator %(const Vector3<float> &v) const; template Vector3<float> Vector3<float>::operator %(const Vector3<float> &v) const;
template float Vector3<float>::operator *(const Vector3<float> &v) const; template float Vector3<float>::operator *(const Vector3<float> &v) const;
template Vector3<float> &Vector3<float>::operator *=(const float num);
template Vector3<float> &Vector3<float>::operator /=(const float num);
template Vector3<float> &Vector3<float>::operator -=(const Vector3<float> &v);
template Vector3<float> &Vector3<float>::operator +=(const Vector3<float> &v);
template Vector3<float> Vector3<float>::operator /(const float num) const;
template Vector3<float> Vector3<float>::operator *(const float num) const;
template Vector3<float> Vector3<float>::operator +(const Vector3<float> &v) const;
template Vector3<float> Vector3<float>::operator -(const Vector3<float> &v) const;
template Vector3<float> Vector3<float>::operator -(void) const;
template bool Vector3<float>::operator ==(const Vector3<float> &v) const;
template bool Vector3<float>::operator !=(const Vector3<float> &v) const;
template bool Vector3<float>::is_nan(void) const;
template bool Vector3<float>::is_inf(void) const;
template float Vector3<float>::angle(const Vector3<float> &v) const;

131
libraries/AP_Math/vector3.h
View File

@ -60,168 +60,109 @@ public:
} }
// function call operator // function call operator
void operator ()(const T x0, const T y0, const T z0) void operator ()(const T x0, const T y0, const T z0)
{ {
x= x0; y= y0; z= z0; x= x0; y= y0; z= z0;
} }
// test for equality // test for equality
bool operator ==(const Vector3<T> &v) bool operator ==(const Vector3<T> &v) const;
{
return (x==v.x && y==v.y && z==v.z);
}
// test for inequality // test for inequality
bool operator !=(const Vector3<T> &v) bool operator !=(const Vector3<T> &v) const;
{
return (x!=v.x || y!=v.y || z!=v.z);
}
// negation // negation
Vector3<T> operator -(void) const Vector3<T> operator -(void) const;
{
return Vector3<T>(-x,-y,-z);
}
// addition // addition
Vector3<T> operator +(const Vector3<T> &v) const Vector3<T> operator +(const Vector3<T> &v) const;
{
return Vector3<T>(x+v.x, y+v.y, z+v.z);
}
// subtraction // subtraction
Vector3<T> operator -(const Vector3<T> &v) const Vector3<T> operator -(const Vector3<T> &v) const;
{
return Vector3<T>(x-v.x, y-v.y, z-v.z);
}
// uniform scaling // uniform scaling
Vector3<T> operator *(const T num) const Vector3<T> operator *(const T num) const;
{
Vector3<T> temp(*this);
return temp*=num;
}
// uniform scaling // uniform scaling
Vector3<T> operator /(const T num) const Vector3<T> operator /(const T num) const;
{
Vector3<T> temp(*this);
return temp/=num;
}
// addition // addition
Vector3<T> &operator +=(const Vector3<T> &v) Vector3<T> &operator +=(const Vector3<T> &v);
{
x+=v.x; y+=v.y; z+=v.z;
return *this;
}
// subtraction // subtraction
Vector3<T> &operator -=(const Vector3<T> &v) Vector3<T> &operator -=(const Vector3<T> &v);
{
x-=v.x; y-=v.y; z-=v.z;
return *this;
}
// uniform scaling // uniform scaling
Vector3<T> &operator *=(const T num) Vector3<T> &operator *=(const T num);
{
x*=num; y*=num; z*=num;
return *this;
}
// uniform scaling // uniform scaling
Vector3<T> &operator /=(const T num) Vector3<T> &operator /=(const T num);
{
x/=num; y/=num; z/=num;
return *this;
}
// dot product // dot product
T operator *(const Vector3<T> &v) const; T operator *(const Vector3<T> &v) const;
// cross product // cross product
Vector3<T> operator %(const Vector3<T> &v) const; Vector3<T> operator %(const Vector3<T> &v) const;
// computes the angle between this vector and another vector
float angle(const Vector3<T> &v2) const;
// check if any elements are NAN
bool is_nan(void) const;
// check if any elements are infinity
bool is_inf(void) const;
// rotate by a standard rotation
void rotate(enum Rotation rotation);
// gets the length of this vector squared // gets the length of this vector squared
T length_squared() const T length_squared() const
{ {
return (T)(*this * *this); return (T)(*this * *this);
} }
// gets the length of this vector // gets the length of this vector
float length(void) const; float length(void) const;
// normalizes this vector // normalizes this vector
void normalize() void normalize()
{ {
*this/=length(); *this /= length();
} }
// zero the vector // zero the vector
void zero() void zero()
{ {
x = y = z = 0.0; x = y = z = 0.0;
} }
// returns the normalized version of this vector // returns the normalized version of this vector
Vector3<T> normalized() const Vector3<T> normalized() const
{ {
return *this/length(); return *this/length();
} }
// reflects this vector about n // reflects this vector about n
void reflect(const Vector3<T> &n) void reflect(const Vector3<T> &n)
{ {
Vector3<T> orig(*this); Vector3<T> orig(*this);
project(n); project(n);
*this= *this*2 - orig; *this = *this*2 - orig;
} }
// projects this vector onto v // projects this vector onto v
void project(const Vector3<T> &v) void project(const Vector3<T> &v)
{ {
*this= v * (*this * v)/(v*v); *this= v * (*this * v)/(v*v);
} }
// returns this vector projected onto v // returns this vector projected onto v
Vector3<T> projected(const Vector3<T> &v) Vector3<T> projected(const Vector3<T> &v) const
{ {
return v * (*this * v)/(v*v); return v * (*this * v)/(v*v);
} }
// computes the angle between 2 arbitrary vectors
T angle(const Vector3<T> &v1, const Vector3<T> &v2)
{
return (T)acosf((v1*v2) / (v1.length()*v2.length()));
}
// computes the angle between this vector and another vector
T angle(const Vector3<T> &v2)
{
return (T)acosf(((*this)*v2) / (this->length()*v2.length()));
}
// computes the angle between 2 arbitrary normalized vectors
T angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2)
{
return (T)acosf(v1*v2);
}
// check if any elements are NAN
bool is_nan(void)
{
return isnan(x) || isnan(y) || isnan(z);
}
// check if any elements are infinity
bool is_inf(void)
{
return isinf(x) || isinf(y) || isinf(z);
}
// rotate by a standard rotation
void rotate(enum Rotation rotation);
}; };