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

@@ -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"

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

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 <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