Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1702 - 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
 
36
// 3x3 matrix with elements of type T
37
template <typename T>
38
class Matrix3 {
39
public:
40
 
41
        // Vectors comprising the rows of the matrix
42
        Vector3<T>      a, b, c;
43
 
44
        // trivial ctor
45
        // note that the Vector3 ctor will zero the vector elements
46
        Matrix3<T>() {}
47
 
48
        // setting ctor
49
        Matrix3<T>(const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0): a(a0), b(b0), c(c0) {}
50
 
51
        // setting ctor
52
        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) {}
53
 
54
        // function call operator
55
        void operator () (const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0)
56
        {       a = a0; b = b0; c = c0;  }
57
 
58
        // test for equality
59
        bool operator == (const Matrix3<T> &m)
60
        {       return (a==m.a && b==m.b && c==m.c);    }
61
 
62
        // test for inequality
63
        bool operator != (const Matrix3<T> &m)
64
        {       return (a!=m.a || b!=m.b || c!=m.c);    }
65
 
66
        // negation
67
        Matrix3<T> operator - (void) const
68
        {       return Matrix3<T>(-a,-b,-c);    }
69
 
70
        // addition
71
        Matrix3<T> operator + (const Matrix3<T> &m) const
72
        {   return Matrix3<T>(a+m.a, b+m.b, c+m.c);      }
73
        Matrix3<T> &operator += (const Matrix3<T> &m)
74
        {       return *this = *this + m;       }
75
 
76
        // subtraction
77
        Matrix3<T> operator - (const Matrix3<T> &m) const
78
        {   return Matrix3<T>(a-m.a, b-m.b, c-m.c);      }
79
        Matrix3<T> &operator -= (const Matrix3<T> &m)
80
        {       return *this = *this - m;       }
81
 
82
        // uniform scaling
83
        Matrix3<T> operator * (const T num) const
84
        {       return Matrix3<T>(a*num, b*num, c*num); }
85
        Matrix3<T> &operator *= (const T num)
86
        {       return *this = *this * num;     }
87
         Matrix3<T> operator / (const T num) const
88
        {       return Matrix3<T>(a/num, b/num, c/num); }
89
        Matrix3<T> &operator /= (const T num)
90
        {       return *this = *this / num;     }
91
 
92
        // multiplication by a vector
93
        Vector3<T> operator *(const Vector3<T> &v) const;
94
 
95
        // multiplication of transpose by a vector
96
        Vector3<T> mul_transpose(const Vector3<T> &v) const;
97
 
98
        // multiplication by another Matrix3<T>
99
        Matrix3<T> operator *(const Matrix3<T> &m) const;
100
 
101
        Matrix3<T> &operator *=(const Matrix3<T> &m)
102
        {       return *this = *this * m;       }
103
 
104
        // transpose the matrix
105
        Matrix3<T> transposed(void) const
106
        {
107
                return Matrix3<T>(Vector3<T>(a.x, b.x, c.x),
108
                                                  Vector3<T>(a.y, b.y, c.y),
109
                                                  Vector3<T>(a.z, b.z, c.z));
110
        }
111
        Matrix3<T> transpose(void)
112
        {       return *this = transposed();    }
113
 
114
        // zero the matrix
115
        void zero(void) {
116
                a.x = a.y = a.z = 0;
117
                b.x = b.y = b.z = 0;
118
                c.x = c.y = c.z = 0;
119
        }
120
 
121
        // setup the identity matrix
122
        void identity(void) {
123
                a.x = b.y = c.z = 1;
124
                a.y = a.z = 0;
125
                b.x = b.z = 0;
126
                c.x = c.y = 0;
127
        }
128
 
129
        // check if any elements are NAN
130
        bool is_nan(void)
131
                {   return a.is_nan() || b.is_nan() || c.is_nan(); }
132
 
133
        // fill in the matrix with a standard rotation
134
        void rotation(enum Rotation rotation);
135
 
136
    // create a rotation matrix from Euler angles
137
        void from_euler(float roll, float pitch, float yaw);
138
 
139
    // create eulers from a rotation matrix
140
        void to_euler(float *roll, float *pitch, float *yaw);
141
 
142
    // apply an additional rotation from a body frame gyro vector
143
    // to a rotation matrix.
144
        void rotate(const Vector3<T> &g);
145
};
146
 
147
typedef Matrix3<int16_t>                Matrix3i;
148
typedef Matrix3<uint16_t>               Matrix3ui;
149
typedef Matrix3<int32_t>                Matrix3l;
150
typedef Matrix3<uint32_t>               Matrix3ul;
151
typedef Matrix3<float>                  Matrix3f;
152
 
153
#endif // MATRIX3_H