From 097161cd8dbae18e31dee759d255f7c2af35ab9f Mon Sep 17 00:00:00 2001 From: "DrZiplok@gmail.com" Date: Wed, 8 Sep 2010 08:21:46 +0000 Subject: [PATCH] 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 --- libraries/AP_Math/AP_Math.h | 7 + libraries/AP_Math/keywords.txt | 29 +++++ libraries/AP_Math/matrix3.h | 153 ++++++++++++++++++++++ libraries/AP_Math/vector2.h | 208 +++++++++++++++++++++++++++++ libraries/AP_Math/vector3.h | 230 +++++++++++++++++++++++++++++++++ 5 files changed, 627 insertions(+) create mode 100644 libraries/AP_Math/AP_Math.h create mode 100644 libraries/AP_Math/keywords.txt create mode 100644 libraries/AP_Math/matrix3.h create mode 100644 libraries/AP_Math/vector2.h create mode 100644 libraries/AP_Math/vector3.h diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h new file mode 100644 index 0000000000..b94ed1c696 --- /dev/null +++ b/libraries/AP_Math/AP_Math.h @@ -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" diff --git a/libraries/AP_Math/keywords.txt b/libraries/AP_Math/keywords.txt new file mode 100644 index 0000000000..a5b7938f47 --- /dev/null +++ b/libraries/AP_Math/keywords.txt @@ -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 diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h new file mode 100644 index 0000000000..23bf28ba5d --- /dev/null +++ b/libraries/AP_Math/matrix3.h @@ -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 +class Matrix3 { +public: + + // Vectors comprising the rows of the matrix + Vector3 a, b, c; + + // trivial ctor + Matrix3() {} + + // setting ctor + Matrix3(const Vector3 a0, const Vector3 b0, const Vector3 c0): a(a0), b(b0), c(c0) {} + + // function call operator + void operator () (const Vector3 a0, const Vector3 b0, const Vector3 c0) + { a = a0; b = b0; c = c0; } + + // test for equality + bool operator == (const Matrix3 &m) + { return (a==m.a && b==m.b && c==m.c); } + + // test for inequality + bool operator != (const Matrix3 &m) + { return (a!=m.a || b!=m.b || c!=m.c); } + + // set to value + const Matrix3 &operator = (const Matrix3 &m) + { + a= m.a; b= m.b; c= m.c; + return *this; + } + + // negation + const Matrix3 operator - (void) const + { return Matrix3(-a,-b,-c); } + + // addition + const Matrix3 operator + (const Matrix3 &m) const + { return Matrix3(a+m.a, b+m.b, c+m.c); } + const Matrix3 &operator += (const Matrix3 &m) + { return *this = *this + m; } + + // subtraction + const Matrix3 operator - (const Matrix3 &m) const + { return Matrix3(a-m.a, b-m.b, c-m.c); } + const Matrix3 &operator -= (const Matrix3 &m) + { return *this = *this - m; } + + // uniform scaling + const Matrix3 operator * (const T num) const + { return Matrix3(a*num, b*num, c*num); } + const Matrix3 operator *= (const T num) + { return *this = *this * num; } + const Matrix3 operator / (const T num) const + { return Matrix3(a/num, b/num, c/num); } + const Matrix3 operator /= (const T num) + { return *this = *this / num; } + + // multiplication by a vector + const Vector3 operator *(const Vector3 &v) const + { + return Vector3(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 operator *(const Matrix3 &m) const + { + Matrix3 temp = (Vector3(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(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(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 operator *=(const Matrix3 &m) + { return *this = this * m; } + + // rotation (90 degrees left) + const Matrix3 rotated(void) + { + return Matrix3(Vector3(a.z, b.z, c.z), + Vector3(a.y, b.y, c.y), + Vector3(a.x, b.x, c.x)); + } + const Matrix3 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 &a0, Vector3 &b0, Vector3 &c0) : Matrix3(a0, b0, c0) {} + +class Matrix3i : public Matrix3 +{ +public: + MATRIX3_CTORS(Matrix3i, int) +}; + +class Matrix3ui : public Matrix3 +{ +public: + MATRIX3_CTORS(Matrix3ui, unsigned int) +}; + +class Matrix3l : public Matrix3 +{ +public: + MATRIX3_CTORS(Matrix3l, long) +}; +class Matrix3ul : public Matrix3 +{ +public: + MATRIX3_CTORS(Matrix3ul, unsigned long) +}; + +class Matrix3f : public Matrix3 +{ +public: + MATRIX3_CTORS(Matrix3f, float) +}; + +#endif // MATRIX3_H diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h new file mode 100644 index 0000000000..e00df5d05f --- /dev/null +++ b/libraries/AP_Math/vector2.h @@ -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 + +template +struct Vector2 +{ + T x, y; + + + // trivial ctor + Vector2() {} + + // setting ctor + Vector2(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 &v) + { return (x==v.x && y==v.y); } + + // test for inequality + bool operator!=(const Vector2 &v) + { return (x!=v.x || y!=v.y); } + + // set to value + const Vector2 &operator =(const Vector2 &v) + { + x= v.x; y= v.y; + return *this; + } + + // negation + const Vector2 operator -(void) const + { return Vector2(-x, -y); } + + // addition + const Vector2 operator +(const Vector2 &v) const + { return Vector2(x+v.x, y+v.y); } + + // subtraction + const Vector2 operator -(const Vector2 &v) const + { return Vector2(x-v.x, y-v.y); } + + // uniform scaling + const Vector2 operator *(const T num) const + { + Vector2 temp(*this); + return temp*=num; + } + + // uniform scaling + const Vector2 operator /(const T num) const + { + Vector2 temp(*this); + return temp/=num; + } + + // addition + const Vector2 &operator +=(const Vector2 &v) + { + x+=v.x; y+=v.y; + return *this; + } + + // subtraction + const Vector2 &operator -=(const Vector2 &v) + { + x-=v.x; y-=v.y; + return *this; + } + + // uniform scaling + const Vector2 &operator *=(const T num) + { + x*=num; y*=num; + return *this; + } + + // uniform scaling + const Vector2 &operator /=(const T num) + { + x/=num; y/=num; + return *this; + } + + // dot product + T operator *(const Vector2 &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 normalized() const + { return *this/length(); } + + // reflects this vector about n + void reflect(const Vector2 &n) + { + Vector2 orig(*this); + project(n); + *this= *this*2 - orig; + } + + // projects this vector onto v + void project(const Vector2 &v) + { *this= v * (*this * v)/(v*v); } + + // returns this vector projected onto v + Vector2 projected(const Vector2 &v) + { return v * (*this * v)/(v*v); } + + // computes the angle between 2 arbitrary vectors + static inline T angle(const Vector2 &v1, const Vector2 &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 &v1, const Vector2 &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 &v): Vector2(v.x, v.y) {} \ + /* make x,y combination into a vector */ \ + name(type x0, type y0): Vector2(x0, y0) {} + + + +struct Vector2i: public Vector2 +{ + VECTOR2_CTORS(Vector2i, int) +}; + + +struct Vector2ui: public Vector2 +{ + VECTOR2_CTORS(Vector2ui, unsigned int) +}; + + +struct Vector2l: public Vector2 +{ + VECTOR2_CTORS(Vector2l, long) +}; + + +struct Vector2ul: public Vector2 +{ + VECTOR2_CTORS(Vector2ul, unsigned long) +}; + + +struct Vector2f: public Vector2 +{ + VECTOR2_CTORS(Vector2f, float) +}; + +#endif // VECTOR2_H diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h new file mode 100644 index 0000000000..b2873232d7 --- /dev/null +++ b/libraries/AP_Math/vector3.h @@ -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 + +template +class Vector3 +{ +public: + T x, y, z; + + // trivial ctor + Vector3() {} + + // setting ctor + Vector3(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 &v) + { return (x==v.x && y==v.y && z==v.z); } + + // test for inequality + bool operator!=(const Vector3 &v) + { return (x!=v.x || y!=v.y || z!=v.z); } + + // set to value + const Vector3 &operator =(const Vector3 &v) + { + x= v.x; y= v.y; z= v.z; + return *this; + } + + // negation + const Vector3 operator -(void) const + { return Vector3(-x,-y,-z); } + + // addition + const Vector3 operator +(const Vector3 &v) const + { return Vector3(x+v.x, y+v.y, z+v.z); } + + // subtraction + const Vector3 operator -(const Vector3 &v) const + { return Vector3(x-v.x, y-v.y, z-v.z); } + + // uniform scaling + const Vector3 operator *(const T num) const + { + Vector3 temp(*this); + return temp*=num; + } + + // uniform scaling + const Vector3 operator /(const T num) const + { + Vector3 temp(*this); + return temp/=num; + } + + // addition + const Vector3 &operator +=(const Vector3 &v) + { + x+=v.x; y+=v.y; z+=v.z; + return *this; + } + + // subtraction + const Vector3 &operator -=(const Vector3 &v) + { + x-=v.x; y-=v.y; z-=v.z; + return *this; + } + + // uniform scaling + const Vector3 &operator *=(const T num) + { + x*=num; y*=num; z*=num; + return *this; + } + + // uniform scaling + const Vector3 &operator /=(const T num) + { + x/=num; y/=num; z/=num; + return *this; + } + + // dot product + T operator *(const Vector3 &v) const + { return x*v.x + y*v.y + z*v.z; } + + // cross product + const Vector3 operator %(const Vector3 &v) const + { + Vector3 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 normalized() const + { return *this/length(); } + + // reflects this vector about n + void reflect(const Vector3 &n) + { + Vector3 orig(*this); + project(n); + *this= *this*2 - orig; + } + + // projects this vector onto v + void project(const Vector3 &v) + { *this= v * (*this * v)/(v*v); } + + // returns this vector projected onto v + Vector3 projected(const Vector3 &v) + { return v * (*this * v)/(v*v); } + + // computes the angle between 2 arbitrary vectors + static inline T angle(const Vector3 &v1, const Vector3 &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 &v1, const Vector3 &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 &v): Vector3(v.x, v.y, v.z) {} \ + /* make x,y,z combination into a vector */ \ + name(type x0, type y0, type z0): Vector3(x0, y0, z0) {} + + + + +class Vector3i: public Vector3 +{ +public: + VECTOR3_CTORS(Vector3i, int) +}; + + +class Vector3ui: public Vector3 +{ +public: + VECTOR3_CTORS(Vector3ui, unsigned int) +}; + + +class Vector3l: public Vector3 +{ +public: + VECTOR3_CTORS(Vector3l, long) +}; + + +class Vector3ul: public Vector3 +{ +public: + VECTOR3_CTORS(Vector3ul, unsigned long) +}; + + +class Vector3f: public Vector3 +{ +public: + VECTOR3_CTORS(Vector3f, float) +}; + +#endif // VECTOR3_H