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

uncrustify libraries/AP_Math/vector3.h

Browse Source
This commit is contained in:
uncrustify 2012-08-16 23:20:14 -07:00 committed by Pat Hickey
parent d50c606c97
commit 57d4db2be4
1 changed files with 177 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
libraries/AP_Math/vector3.h
View File

@ -9,34 +9,34 @@
// Derived closely from:
/****************************************
* 3D Vector Classes
* By Bill Perone (billperone@yahoo.com)
* Original: 9-16-2002
* Revised: 19-11-2003
* 11-12-2003
* 18-12-2003
* 06-06-2004
*
* © 2003, This code is provided "as is" and you can use it freely as long as
* credit is given to Bill Perone in the application it is used in
*
* Notes:
* if a*b = 0 then a & b are orthogonal
* a%b = -b%a
* a*(b%c) = (a%b)*c
* a%b = a(cast to matrix)*b
* (a%b).length() = area of parallelogram formed by a & b
* (a%b).length() = a.length()*b.length() * sin(angle between a & b)
* (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0
* a * (b%c) = volume of parallelpiped formed by a, b, c
* vector triple product: a%(b%c) = b*(a*c) - c*(a*b)
* scalar triple product: a*(b%c) = c*(a%b) = b*(c%a)
* vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c)
* if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0
* vectors a1...an are linearly dependant if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0
* or if the matrix (A) * b = 0
*
****************************************/
* 3D Vector Classes
* By Bill Perone (billperone@yahoo.com)
* Original: 9-16-2002
* Revised: 19-11-2003
* 11-12-2003
* 18-12-2003
* 06-06-2004
*
* © 2003, This code is provided "as is" and you can use it freely as long as
* credit is given to Bill Perone in the application it is used in
*
* Notes:
* if a*b = 0 then a & b are orthogonal
* a%b = -b%a
* a*(b%c) = (a%b)*c
* a%b = a(cast to matrix)*b
* (a%b).length() = area of parallelogram formed by a & b
* (a%b).length() = a.length()*b.length() * sin(angle between a & b)
* (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0
* a * (b%c) = volume of parallelpiped formed by a, b, c
* vector triple product: a%(b%c) = b*(a*c) - c*(a*b)
* scalar triple product: a*(b%c) = c*(a%b) = b*(c%a)
* vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c)
* if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0
* vectors a1...an are linearly dependant if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0
* or if the matrix (A) * b = 0
*
****************************************/
#ifndef VECTOR3_H
#define VECTOR3_H
@ -51,34 +51,49 @@ public:
T x, y, z;
// trivial ctor
Vector3<T>() { x = y = z = 0; }
Vector3<T>() {
x = y = z = 0;
}
// setting ctor
Vector3<T>(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {}
Vector3<T>(const T x0, const T y0, const T z0) : x(x0), y(y0), z(z0) {
}
// function call operator
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
bool operator==(const Vector3<T> &v)
{ return (x==v.x && y==v.y && z==v.z); }
bool operator ==(const Vector3<T> &v)
{
return (x==v.x && y==v.y && z==v.z);
}
// test for inequality
bool operator!=(const Vector3<T> &v)
{ return (x!=v.x || y!=v.y || z!=v.z); }
bool operator !=(const Vector3<T> &v)
{
return (x!=v.x || y!=v.y || z!=v.z);
}
// negation
Vector3<T> operator -(void) const
{ return Vector3<T>(-x,-y,-z); }
{
return Vector3<T>(-x,-y,-z);
}
// addition
Vector3<T> operator +(const Vector3<T> &v) const
{ return Vector3<T>(x+v.x, y+v.y, z+v.z); }
{
return Vector3<T>(x+v.x, y+v.y, z+v.z);
}
// subtraction
Vector3<T> operator -(const Vector3<T> &v) const
{ return Vector3<T>(x-v.x, y-v.y, z-v.z); }
{
return Vector3<T>(x-v.x, y-v.y, z-v.z);
}
// uniform scaling
Vector3<T> operator *(const T num) const
@ -130,22 +145,30 @@ public:
// gets the length of this vector squared
T length_squared() const
{ return (T)(*this * *this); }
{
return (T)(*this * *this);
}
// gets the length of this vector
float length(void) const;
// normalizes this vector
void normalize()
{ *this/=length(); }
{
*this/=length();
}
// zero the vector
void zero()
{ x = y = z = 0.0; }
{
x = y = z = 0.0;
}
// returns the normalized version of this vector
Vector3<T> normalized() const
{ return *this/length(); }
{
return *this/length();
}
// reflects this vector about n
void reflect(const Vector3<T> &n)
@ -157,31 +180,45 @@ public:
// projects this vector onto 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
Vector3<T> projected(const Vector3<T> &v)
{ 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)acos((v1*v2) / (v1.length()*v2.length())); }
{
return (T)acos((v1*v2) / (v1.length()*v2.length()));
}
// computes the angle between this vector and another vector
T angle(const Vector3<T> &v2)
{ return (T)acos(((*this)*v2) / (this->length()*v2.length())); }
{
return (T)acos(((*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)acos(v1*v2); }
{
return (T)acos(v1*v2);
}
// check if any elements are NAN
bool is_nan(void)
{ return isnan(x) || isnan(y) || isnan(z); }
{
return isnan(x) || isnan(y) || isnan(z);
}
// check if any elements are infinity
bool is_inf(void)
{ return isinf(x) || isinf(y) || isinf(z); }
{
return isinf(x) || isinf(y) || isinf(z);
}
// rotate by a standard rotation
void rotate(enum Rotation rotation);