Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2189 - 1
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
2
 
3
// Copyright 2010 Michael Smith, all rights reserved.
4
 
5
//      This library is free software; you can redistribute it and / or
6
//      modify it under the terms of the GNU Lesser General Public
7
//      License as published by the Free Software Foundation; either
8
//      version 2.1 of the License, or (at your option) any later version.
9
 
10
// Inspired by:
11
/****************************************
12
 * 3D Vector Classes
13
 * By Bill Perone (billperone@yahoo.com)
14
 */
15
 
16
//
17
// 3x3 matrix implementation.
18
//
19
// Note that the matrix is organised in row-normal form (the same as
20
// applies to array indexing).
21
//
22
// In addition to the template, this header defines the following types:
23
//
24
// Matrix3i             3x3 matrix of signed integers
25
// Matrix3ui    3x3 matrix of unsigned integers
26
// Matrix3l             3x3 matrix of signed longs
27
// Matrix3ul    3x3 matrix of unsigned longs
28
// Matrix3f             3x3 matrix of signed floats
29
//
30
 
31
#ifndef MATRIX3_H
32
#define MATRIX3_H
33
 
34
#include "Vector3.h"
35
#include "Constants.h"
36
 
37
// 3x3 matrix with elements of type T
38
template <typename T>
39
class Matrix3 {
40
public:
41
 
42
        // Vectors comprising the rows of the matrix
43
        Vector3<T>      a, b, c;
44
 
45
        // trivial ctor
46
        // note that the Vector3 ctor will zero the vector elements
47
        Matrix3<T>() {}
48
 
49
        // setting ctor
50
        Matrix3<T>(const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0): a(a0), b(b0), c(c0) {}
51
 
52
        // setting ctor
53
        Matrix3<T>(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz): a(ax,ay,az), b(bx,by,bz), c(cx,cy,cz) {}
54
 
55
        // function call operator
56
        void operator () (const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0)
57
        {       a = a0; b = b0; c = c0;  }
58
 
59
        // test for equality
60
        bool operator == (const Matrix3<T> &m)
61
        {       return (a==m.a && b==m.b && c==m.c);    }
62
 
63
        // test for inequality
64
        bool operator != (const Matrix3<T> &m)
65
        {       return (a!=m.a || b!=m.b || c!=m.c);    }
66
 
67
        // negation
68
        Matrix3<T> operator - (void) const
69
        {       return Matrix3<T>(-a,-b,-c);    }
70
 
71
        // addition
72
        Matrix3<T> operator + (const Matrix3<T> &m) const
73
        {   return Matrix3<T>(a+m.a, b+m.b, c+m.c);      }
74
        Matrix3<T> &operator += (const Matrix3<T> &m)
75
        {       return *this = *this + m;       }
76
 
77
        // subtraction
78
        Matrix3<T> operator - (const Matrix3<T> &m) const
79
        {   return Matrix3<T>(a-m.a, b-m.b, c-m.c);      }
80
        Matrix3<T> &operator -= (const Matrix3<T> &m)
81
        {       return *this = *this - m;       }
82
 
83
        // uniform scaling
84
        Matrix3<T> operator * (const T num) const
85
        {       return Matrix3<T>(a*num, b*num, c*num); }
86
        Matrix3<T> &operator *= (const T num)
87
        {       return *this = *this * num;     }
88
         Matrix3<T> operator / (const T num) const
89
        {       return Matrix3<T>(a/num, b/num, c/num); }
90
        Matrix3<T> &operator /= (const T num)
91
        {       return *this = *this / num;     }
92
 
93
        // multiplication by a vector
94
        Vector3<T> operator *(const Vector3<T> &v) const;
95
 
96
        // multiplication of transpose by a vector
97
        Vector3<T> mul_transpose(const Vector3<T> &v) const;
98
 
99
        // multiplication by another Matrix3<T>
100
        Matrix3<T> operator *(const Matrix3<T> &m) const;
101
 
102
        Matrix3<T> &operator *=(const Matrix3<T> &m)
103
        {       return *this = *this * m;       }
104
 
105
        // transpose the matrix
106
        Matrix3<T> transposed(void) const
107
        {
108
                return Matrix3<T>(Vector3<T>(a.x, b.x, c.x),
109
                                                  Vector3<T>(a.y, b.y, c.y),
110
                                                  Vector3<T>(a.z, b.z, c.z));
111
        }
112
        Matrix3<T> transpose(void)
113
        {       return *this = transposed();    }
114
 
115
        // zero the matrix
116
        void zero(void) {
117
                a.x = a.y = a.z = 0;
118
                b.x = b.y = b.z = 0;
119
                c.x = c.y = c.z = 0;
120
        }
121
 
122
        // setup the identity matrix
123
        void identity(void) {
124
                a.x = b.y = c.z = 1;
125
                a.y = a.z = 0;
126
                b.x = b.z = 0;
127
                c.x = c.y = 0;
128
        }
129
 
130
        // check if any elements are NAN
131
        bool is_nan(void)
132
                {   return a.is_nan() || b.is_nan() || c.is_nan(); }
133
 
134
        // fill in the matrix with a standard rotation
135
        void rotation(enum Rotation rotation);
136
 
137
    // create a rotation matrix from Euler angles
138
        void from_euler(float roll, float pitch, float yaw);
139
 
140
    // create eulers from a rotation matrix
141
        void to_euler(float *roll, float *pitch, float *yaw);
142
 
143
    // apply an additional rotation from a body frame gyro vector
144
    // to a rotation matrix.
145
        void rotate(const Vector3<T> &g);
146
};
147
 
148
typedef Matrix3<int16_t>                Matrix3i;
149
typedef Matrix3<uint16_t>               Matrix3ui;
150
typedef Matrix3<int32_t>                Matrix3l;
151
typedef Matrix3<uint32_t>               Matrix3ul;
152
typedef Matrix3<float>                  Matrix3f;
153
 
154
#endif // MATRIX3_H