Beginnings of a math library for ArduPilot(Mega) systems.

The vector classes are light adaptations of work by Bill Perone
(billperone@yahoo.com), the Matrix3 class draws on them for
inspiration.

Bill's matrix classes are too heavyweight and not templated, so
they're less suitable for us here.

This code compiles, and some trivial tests seem to work, but
it should not be considered "golden" yet.



git-svn-id: https://arducopter.googlecode.com/svn/trunk@441 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-09-08 08:21:46 +00:00
parent ffa25b0846
commit 097161cd8d
5 changed files with 627 additions and 0 deletions

View File

@ -0,0 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Assorted useful math operations for ArduPilot(Mega)
#include "vector2.h"
#include "vector3.h"
#include "matrix3.h"

View File

@ -0,0 +1,29 @@
Vector2 KEYWORD1
Vector2i KEYWORD1
Vector2ui KEYWORD1
Vector2l KEYWORD1
Vector2ul KEYWORD1
Vector2f KEYWORD1
Vector3 KEYWORD1
Vector3i KEYWORD1
Vector3ui KEYWORD1
Vector3l KEYWORD1
Vector3ul KEYWORD1
Vector3f KEYWORD1
Matrix3 KEYWORD1
Matrix3i KEYWORD1
Matrix3ui KEYWORD1
Matrix3l KEYWORD1
Matrix3ul KEYWORD1
Matrix3f KEYWORD1
length_squared KEYWORD2
length KEYWORD2
normalize KEYWORD2
normalized KEYWORD2
reflect KEYWORD2
project KEYWORD2
projected KEYWORD2
angle KEYWORD2
angle_normalized KEYWORD2
rotate KEYWORD2
rotated KEYWORD2

153
libraries/AP_Math/matrix3.h Normal file
View File

@ -0,0 +1,153 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Copyright 2010 Michael Smith, all rights reserved.
// This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// Inspired by:
/****************************************
* 3D Vector Classes
* By Bill Perone (billperone@yahoo.com)
*/
#ifndef MATRIX3_H
#define MATRIX3_H
#include "vector3.h"
// 3x3 matrix with elements of type T
template <typename T>
class Matrix3 {
public:
// Vectors comprising the rows of the matrix
Vector3<T> a, b, c;
// trivial ctor
Matrix3<T>() {}
// setting ctor
Matrix3<T>(const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0): a(a0), b(b0), c(c0) {}
// function call operator
void operator () (const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0)
{ a = a0; b = b0; c = c0; }
// test for equality
bool operator == (const Matrix3<T> &m)
{ return (a==m.a && b==m.b && c==m.c); }
// test for inequality
bool operator != (const Matrix3<T> &m)
{ return (a!=m.a || b!=m.b || c!=m.c); }
// set to value
const Matrix3<T> &operator = (const Matrix3<T> &m)
{
a= m.a; b= m.b; c= m.c;
return *this;
}
// negation
const Matrix3<T> operator - (void) const
{ return Matrix3<T>(-a,-b,-c); }
// addition
const Matrix3<T> operator + (const Matrix3<T> &m) const
{ return Matrix3<T>(a+m.a, b+m.b, c+m.c); }
const Matrix3<T> &operator += (const Matrix3<T> &m)
{ return *this = *this + m; }
// subtraction
const Matrix3<T> operator - (const Matrix3<T> &m) const
{ return Matrix3<T>(a-m.a, b-m.b, c-m.c); }
const Matrix3<T> &operator -= (const Matrix3<T> &m)
{ return *this = *this - m; }
// uniform scaling
const Matrix3<T> operator * (const T num) const
{ return Matrix3<T>(a*num, b*num, c*num); }
const Matrix3<T> operator *= (const T num)
{ return *this = *this * num; }
const Matrix3<T> operator / (const T num) const
{ return Matrix3<T>(a/num, b/num, c/num); }
const Matrix3<T> operator /= (const T num)
{ return *this = *this / num; }
// multiplication by a vector
const Vector3<T> operator *(const Vector3<T> &v) const
{
return Vector3<T>(a.x * v.x + a.y * v.x + a.z * v.x,
b.x * v.y + b.y * v.y + b.z * v.y,
c.x * v.z + c.y * v.z + c.z * v.z);
}
// multiplication by another Matrix3
const Matrix3<T> operator *(const Matrix3<T> &m) const
{
Matrix3<T> temp = (Vector3<T>(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x,
a.x * m.a.y + a.y * m.b.y + a.z * m.c.y,
a.x * m.a.z + a.y * m.b.z + a.z * m.c.z),
Vector3<T>(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x,
b.x * m.a.y + b.y * m.b.y + b.z * m.c.y,
b.x * m.a.z + b.y * m.b.z + b.z * m.c.z),
Vector3<T>(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x,
c.x * m.a.y + c.y * m.b.y + c.z * m.c.y,
c.x * m.a.z + c.y * m.b.z + c.z * m.c.z));
return temp;
}
const Matrix3<T> operator *=(const Matrix3<T> &m)
{ return *this = this * m; }
// rotation (90 degrees left)
const Matrix3<T> rotated(void)
{
return Matrix3<T>(Vector3<T>(a.z, b.z, c.z),
Vector3<T>(a.y, b.y, c.y),
Vector3<T>(a.x, b.x, c.x));
}
const Matrix3<T> rotate(void)
{ return *this = rotated(); }
};
// macro to make creating the ctors for derived matrices easier
#define MATRIX3_CTORS(name, type) \
/* trivial ctor */ \
name() {} \
/* make a,b,c combination into a matrix */ \
name(Vector3<type> &a0, Vector3<type> &b0, Vector3<type> &c0) : Matrix3<type>(a0, b0, c0) {}
class Matrix3i : public Matrix3<int>
{
public:
MATRIX3_CTORS(Matrix3i, int)
};
class Matrix3ui : public Matrix3<unsigned int>
{
public:
MATRIX3_CTORS(Matrix3ui, unsigned int)
};
class Matrix3l : public Matrix3<long>
{
public:
MATRIX3_CTORS(Matrix3l, long)
};
class Matrix3ul : public Matrix3<unsigned long>
{
public:
MATRIX3_CTORS(Matrix3ul, unsigned long)
};
class Matrix3f : public Matrix3<float>
{
public:
MATRIX3_CTORS(Matrix3f, float)
};
#endif // MATRIX3_H

208
libraries/AP_Math/vector2.h Normal file
View File

@ -0,0 +1,208 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Copyright 2010 Michael Smith, all rights reserved.
// This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// Derived closely from:
/****************************************
* 2D Vector Classes
* By Bill Perone (billperone@yahoo.com)
* Original: 9-16-2002
* Revised: 19-11-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
****************************************/
#ifndef VECTOR2_H
#define VECTOR2_H
#include <math.h>
template <typename T>
struct Vector2
{
T x, y;
// trivial ctor
Vector2<T>() {}
// setting ctor
Vector2<T>(const T x0, const T y0): x(x0), y(y0) {}
// array indexing
T &operator [](unsigned int i)
{ return *(&x+i); }
// array indexing
const T &operator [](unsigned int i) const
{ return *(&x+i); }
// function call operator
void operator ()(const T x0, const T y0)
{ x= x0; y= y0; }
// test for equality
bool operator==(const Vector2<T> &v)
{ return (x==v.x && y==v.y); }
// test for inequality
bool operator!=(const Vector2<T> &v)
{ return (x!=v.x || y!=v.y); }
// set to value
const Vector2<T> &operator =(const Vector2<T> &v)
{
x= v.x; y= v.y;
return *this;
}
// negation
const Vector2<T> operator -(void) const
{ return Vector2<T>(-x, -y); }
// addition
const Vector2<T> operator +(const Vector2<T> &v) const
{ return Vector2<T>(x+v.x, y+v.y); }
// subtraction
const Vector2<T> operator -(const Vector2<T> &v) const
{ return Vector2<T>(x-v.x, y-v.y); }
// uniform scaling
const Vector2<T> operator *(const T num) const
{
Vector2<T> temp(*this);
return temp*=num;
}
// uniform scaling
const Vector2<T> operator /(const T num) const
{
Vector2<T> temp(*this);
return temp/=num;
}
// addition
const Vector2<T> &operator +=(const Vector2<T> &v)
{
x+=v.x; y+=v.y;
return *this;
}
// subtraction
const Vector2<T> &operator -=(const Vector2<T> &v)
{
x-=v.x; y-=v.y;
return *this;
}
// uniform scaling
const Vector2<T> &operator *=(const T num)
{
x*=num; y*=num;
return *this;
}
// uniform scaling
const Vector2<T> &operator /=(const T num)
{
x/=num; y/=num;
return *this;
}
// dot product
T operator *(const Vector2<T> &v) const
{ return x*v.x + y*v.y; }
// gets the length of this vector squared
T length_squared() const
{ return (T)(*this * *this); }
// gets the length of this vector
T length() const
{ return (T)sqrt(*this * *this); }
// normalizes this vector
void normalize()
{ *this/=length(); }
// returns the normalized vector
Vector2<T> normalized() const
{ return *this/length(); }
// reflects this vector about n
void reflect(const Vector2<T> &n)
{
Vector2<T> orig(*this);
project(n);
*this= *this*2 - orig;
}
// projects this vector onto v
void project(const Vector2<T> &v)
{ *this= v * (*this * v)/(v*v); }
// returns this vector projected onto v
Vector2<T> projected(const Vector2<T> &v)
{ return v * (*this * v)/(v*v); }
// computes the angle between 2 arbitrary vectors
static inline T angle(const Vector2<T> &v1, const Vector2<T> &v2)
{ return (T)acosf((v1*v2) / (v1.length()*v2.length())); }
// computes the angle between 2 normalized arbitrary vectors
static inline T angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2)
{ return (T)acosf(v1*v2); }
};
// macro to make creating the ctors for derived vectors easier
#define VECTOR2_CTORS(name, type) \
/* trivial ctor */ \
name() {} \
/* down casting ctor */ \
name(const Vector2<type> &v): Vector2<type>(v.x, v.y) {} \
/* make x,y combination into a vector */ \
name(type x0, type y0): Vector2<type>(x0, y0) {}
struct Vector2i: public Vector2<int>
{
VECTOR2_CTORS(Vector2i, int)
};
struct Vector2ui: public Vector2<unsigned int>
{
VECTOR2_CTORS(Vector2ui, unsigned int)
};
struct Vector2l: public Vector2<long>
{
VECTOR2_CTORS(Vector2l, long)
};
struct Vector2ul: public Vector2<unsigned long>
{
VECTOR2_CTORS(Vector2ul, unsigned long)
};
struct Vector2f: public Vector2<float>
{
VECTOR2_CTORS(Vector2f, float)
};
#endif // VECTOR2_H

230
libraries/AP_Math/vector3.h Normal file
View File

@ -0,0 +1,230 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Copyright 2010 Michael Smith, all rights reserved.
// This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// 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
*
****************************************/
#ifndef VECTOR3_H
#define VECTOR3_H
#include <math.h>
template <typename T>
class Vector3
{
public:
T x, y, z;
// trivial ctor
Vector3<T>() {}
// setting ctor
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; }
// test for equality
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); }
// set to value
const Vector3<T> &operator =(const Vector3<T> &v)
{
x= v.x; y= v.y; z= v.z;
return *this;
}
// negation
const Vector3<T> operator -(void) const
{ return Vector3<T>(-x,-y,-z); }
// addition
const Vector3<T> operator +(const Vector3<T> &v) const
{ return Vector3<T>(x+v.x, y+v.y, z+v.z); }
// subtraction
const Vector3<T> operator -(const Vector3<T> &v) const
{ return Vector3<T>(x-v.x, y-v.y, z-v.z); }
// uniform scaling
const Vector3<T> operator *(const T num) const
{
Vector3<T> temp(*this);
return temp*=num;
}
// uniform scaling
const Vector3<T> operator /(const T num) const
{
Vector3<T> temp(*this);
return temp/=num;
}
// addition
const Vector3<T> &operator +=(const Vector3<T> &v)
{
x+=v.x; y+=v.y; z+=v.z;
return *this;
}
// subtraction
const Vector3<T> &operator -=(const Vector3<T> &v)
{
x-=v.x; y-=v.y; z-=v.z;
return *this;
}
// uniform scaling
const Vector3<T> &operator *=(const T num)
{
x*=num; y*=num; z*=num;
return *this;
}
// uniform scaling
const Vector3<T> &operator /=(const T num)
{
x/=num; y/=num; z/=num;
return *this;
}
// dot product
T operator *(const Vector3<T> &v) const
{ return x*v.x + y*v.y + z*v.z; }
// cross product
const Vector3<T> operator %(const Vector3<T> &v) const
{
Vector3<T> temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
return temp;
}
// gets the length of this vector squared
T length_squared() const
{ return (T)(*this * *this); }
// gets the length of this vector
float length() const
{ return (T)sqrt(*this * *this); }
// normalizes this vector
void normalize()
{ *this/=length(); }
// returns the normalized version of this vector
Vector3<T> normalized() const
{ return *this/length(); }
// reflects this vector about n
void reflect(const Vector3<T> &n)
{
Vector3<T> orig(*this);
project(n);
*this= *this*2 - orig;
}
// projects this vector onto v
void project(const Vector3<T> &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); }
// computes the angle between 2 arbitrary vectors
static inline T angle(const Vector3<T> &v1, const Vector3<T> &v2)
{ return (T)acosf((v1*v2) / (v1.length()*v2.length())); }
// computes the angle between 2 arbitrary normalized vectors
static inline T angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2)
{ return (T)acosf(v1*v2); }
};
// macro to make creating the ctors for derived vectors easier
#define VECTOR3_CTORS(name, type) \
/* trivial ctor */ \
name() {} \
/* down casting ctor */ \
name(const Vector3<type> &v): Vector3<type>(v.x, v.y, v.z) {} \
/* make x,y,z combination into a vector */ \
name(type x0, type y0, type z0): Vector3<type>(x0, y0, z0) {}
class Vector3i: public Vector3<int>
{
public:
VECTOR3_CTORS(Vector3i, int)
};
class Vector3ui: public Vector3<unsigned int>
{
public:
VECTOR3_CTORS(Vector3ui, unsigned int)
};
class Vector3l: public Vector3<long>
{
public:
VECTOR3_CTORS(Vector3l, long)
};
class Vector3ul: public Vector3<unsigned long>
{
public:
VECTOR3_CTORS(Vector3ul, unsigned long)
};
class Vector3f: public Vector3<float>
{
public:
VECTOR3_CTORS(Vector3f, float)
};
#endif // VECTOR3_H