| Bsoft 2.0.5 Bernard's software package

Matrix3.h
Go to the documentation of this file.
1
9 #ifndef _Matrix3_
10 #define _Matrix3_
11
12 #include "Vector3.h"
13 #include "Quaternion.h"
14 #include "Matrix.h"
15
16
17 class Matrix3
18 {
19 private:
20 Vector3<double> d[3];
21 void check() {
22 for ( long i=0; i<3; i++ ) {
23 for ( long j=0; j<3; j++ ) {
24 if ( fabs(d[i][j]) < 1e-12 ) d[i][j] = 0;
25 if ( d[i][j] > 1 ) d[i][j] = 1;
26 if ( d[i][j] < -1 ) d[i][j] = -1;
27 }
28 }
29 }
30 void from_quaternion(Quaternion q) {
31 d[0][0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
32 d[0][1] = 2*q[1]*q[2] - 2*q[0]*q[3];
33 d[0][2] = 2*q[1]*q[3] + 2*q[0]*q[2];
34 d[1][0] = 2*q[1]*q[2] + 2*q[0]*q[3];
35 d[1][1] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
36 d[1][2] = 2*q[2]*q[3] - 2*q[0]*q[1];
37 d[2][0] = 2*q[1]*q[3] - 2*q[0]*q[2];
38 d[2][1] = 2*q[2]*q[3] + 2*q[0]*q[1];
39 d[2][2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
40 check();
41 // for ( long i=0; i<3; i++ ) cout << d[i] << endl;
42 }
43 public:
44 Matrix3() { }
45 // Diagonal matrix
46 Matrix3(const double v) { for ( long i=0; i<3; i++ ) d[i][i] = v; }
47 Matrix3(const double* v) {
48 long i, j, k;
49 for ( i=k=0; i<3; i++ )
50 for ( j=0; j<3; j++, k++ )
51 d[i][j] = v[k];
52 }
53 Matrix3(const vector<double>& v) {
54 long i, j, k;
55 for ( i=k=0; i<3; i++ )
56 for ( j=0; j<3; j++, k++ )
57 d[i][j] = v[k];
58 }
59 Matrix3(const double d0, const double d1, const double d2,
60 const double d3, const double d4, const double d5,
61 const double d6, const double d7, const double d8) {
62 d[0][0] = d0; d[0][1] = d1; d[0][2] = d2;
63 d[1][0] = d3; d[1][1] = d4; d[1][2] = d5;
64 d[2][0] = d6; d[2][1] = d7; d[2][2] = d8;
65 }
66 Matrix3(const Matrix3& m) { for ( long i=0; i<3; i++ ) d[i] = m.d[i]; }
67 // Matrix from a quaternion
68 Matrix3(Quaternion q) {
69 from_quaternion(q);
70 }
71 // Matrix from a rotation axis and angle
72 template <typename T>
73 Matrix3(Vector3<T> axis, double angle) {
74 Quaternion q(axis, angle);
75 from_quaternion(q);
76 }
77 // Matrix from a tilt angle and axis
78 Matrix3(double tilt, double axis) {
79 double tilt_2 = tilt/2.0L;
80 double sin_tilt = sin(tilt_2);
81 Quaternion q(cos(tilt_2), cos(axis)*sin_tilt, sin(axis)*sin_tilt, 0);
82 from_quaternion(q);
83 }
84 template <typename Type1, typename Type2>
85 Matrix3(Vector3<Type1>& from_vec, Vector3<Type2>& to_vec) {
86 Quaternion q(from_vec, to_vec);
87 from_quaternion(q);
88 }
89 /* Quaternion quaternion() const {
90 Quaternion q;
91 double t = 1 + data[0] + data[4] + data[8];
92 if ( t > 1 ) {
93 t = 0.5*sqrt(t);
94 q.s(t);
95 t = 0.25/t;
96 q.x((data[7] - data[5])*t);
97 q.y((data[2] - data[6])*t);
98 q.z((data[3] - data[1])*t);
99 // puts("** 1 **");
100 } else if ( data[0] > data[4] && data[0] > data[8] ) {
101 t = 0.5*sqrt(1 + data[0] - data[4] - data[8]);
102 q.x(t);
103 t = 0.25/t;
104 q.s(-(data[5] - data[7])*t);
105 q.y((data[1] + data[3])*t);
106 q.z((data[2] + data[6])*t);
107 // puts("** 2 **");
108 } else if ( data[4] > data[8] ) {
109 t = 0.5*sqrt(1 - data[0] + data[4] - data[8]);
110 q.y(t);
111 t = 0.25/t;
112 // q.s(-(data[2] - data[6])*t);
113 q.s((data[2] - data[6])*t);
114 q.x((data[1] + data[3])*t);
115 q.z((data[5] + data[7])*t);
116 // puts("** 3 **");
117 } else {
118 t = 0.5*sqrt(1 - data[0] - data[4] + data[8]);
119 q.z(t);
120 t = 0.25/t;
121 q.s(-(data[1] - data[3])*t);
122 q.x((data[2] + data[6])*t);
123 q.y((data[5] + data[7])*t);
124 // puts("** 4 **");
125 }
126 q.normalize();
127 return q;
128 }*/
129 Quaternion quaternion() const {
130 Quaternion q;
131 double t = 1 + d[0][0] + d[1][1] + d[2][2];
132 if ( t > 1 ) {
133 t = 0.5*sqrt(t);
134 q[0] = t;
135 t = 0.25/t;
136 q[1] = (d[2][1] - d[1][2])*t;
137 q[2] = (d[0][2] - d[2][0])*t;
138 q[3] = (d[1][0] - d[0][1])*t;
139 // puts("** 1 **");
140 } else if ( d[0][0] > d[1][1] && d[0][0] > d[2][2] ) {
141 t = 0.5*sqrt(1 + d[0][0] - d[1][1] - d[2][2]);
142 q[1] = t;
143 t = 0.25/t;
144 q[0] = -(d[1][2] - d[2][1])*t;
145 q[2] = (d[0][1] + d[1][0])*t;
146 q[3] = (d[0][2] + d[2][0])*t;
147 // puts("** 2 **");
148 } else if ( d[1][1] > d[2][2] ) {
149 t = 0.5*sqrt(1 - d[0][0] + d[1][1] - d[2][2]);
150 q[2] = t;
151 t = 0.25/t;
152 // q[0] = -(d[0][2] - d[2][0])*t;
153 q[0] = (d[0][2] - d[2][0])*t;
154 q[1] = (d[0][1] + d[1][0])*t;
155 q[3] = (d[1][2] + d[2][1])*t;
156 // puts("** 3 **");
157 } else {
158 t = 0.5*sqrt(1 - d[0][0] - d[1][1] + d[2][2]);
159 q[3] = t;
160 t = 0.25/t;
161 q[0] = -(d[0][1] - d[1][0])*t;
162 q[1] = (d[0][2] + d[2][0])*t;
163 q[2] = (d[1][2] + d[2][1])*t;
164 // puts("** 4 **");
165 }
166 q.normalize();
167 return q;
168 }
169 Vector3<double>& operator[](long i) { while ( i>2 ) i-=3; return d[i]; }
170 // This asignment operator does not work!
171 // Matrix3 operator=(const Matrix3& m) { Matrix3 mn(m); return mn; }
172 Matrix3 operator=(const Matrix3& m) {
173 for ( long i=0; i<3; i++ ) d[i] = m.d[i];
174 return *this;
175 }
176 Matrix3 operator+=(const Matrix3& m) {
177 for ( long i=0; i<3; i++ ) d[i] += m.d[i];
178 return *this;
179 }
180 Matrix3 operator+(const Matrix3& m) {
181 Matrix3 mn(*this);
182 return mn += m;
183 }
184 Matrix3 operator-=(const Matrix3& m) {
185 for ( int i=0; i<3; i++ ) d[i] -= m.d[i];
186 return *this;
187 }
188 Matrix3 operator-(const Matrix3& m) {
189 Matrix3 mn(*this);
190 return mn -= m;
191 }
192 Matrix3 operator*=(const Matrix3& m) {
193 *this = *this * m;
194 return *this;
195 }
196 Matrix3 operator*(const Matrix3& m) {
197 long i, j, k;
198 Matrix3 mn;
199 for ( i=0; i<3; i++ )
200 for ( j=0; j<3; j++ )
201 for ( k=0; k<3; k++ )
202 mn.d[i][j] += d[i][k] * m.d[k][j];
203 return mn;
204 }
205 /* Matrix3 operator*(const Matrix3& m) const {
206 long i, j;
207 Matrix3 mt = m.transpose();
208 Matrix3 mn;
209 for ( i=0; i<3; i++ )
210 for ( j=0; j<3; j++ )
211 mn.d[i][j] = d[i].scalar(m.d[j]);
212 return mn;
213 }*/
214 Matrix3 operator*=(const double v) {
215 for ( long i=0; i<3; i++ ) d[i] *= v;
216 return *this;
217 }
218 Matrix3 operator*(const double v) {
219 Matrix3 mn;
220 for ( long i=0; i<3; i++ ) mn.d[i] = d[i] * v;
221 return mn;
222 }
223 Matrix3 operator/=(const double v) {
224 double div = 1/v;
225 return *this *= div;
226 }
227 Matrix3 operator/(const double v) {
228 double div = 1/v;
229 return *this * div;
230 }
231 bool operator==(const Matrix3& m) {
232 int e = 0;
233 for ( long i=0; i<3; i++ )
234 e += ( d[i] == m.d[i] );
235 return ( e == 3 );
236 }
237 template <typename T>
238 Vector3<T> operator*(Vector3<T>& v) {
239 Vector3<T> vp;
240 for ( long i=0; i<3; i++ ) vp[i] = v.scalar(d[i]);
241 return vp;
242 }
243 template <typename T>
244 vector<T> operator*(vector<T>& v) {
245 vector<T> vp(3);
246 Vector3<T> vo(v[0], v[1], v[2]);
247 for ( long i=0; i<3; i++ ) vp[i] = vo.scalar(d[i]);
248 return vp;
249 }
250 template <typename T>
251 Matrix3 operator/=(Vector3<T>& v) {
252 for ( long i=0; i<3; i++ ) d[i] /= v[i];
253 return *this;
254 }
255 template <typename T>
256 Matrix3 operator/(Vector3<T>& v) {
257 Matrix3 mn(*this);
258 return mn /= v;
259 }
260 double determinant() {
261 return d[0][0]*(d[1][1]*d[2][2]-d[1][2]*d[2][1])
262 - d[0][1]*(d[1][0]*d[2][2]-d[1][2]*d[2][0])
263 + d[0][2]*(d[1][0]*d[2][1]-d[1][1]*d[2][0]);
264 }
265 double trace() {
266 return d[0][0] + d[1][1] + d[2][2];
267 }
268 Matrix3 transpose() const {
269 Matrix3 m;
270 for ( long i=0; i<3; i++ )
271 for ( long j=0; j<3; j++ )
272 m[i][j] = d[j][i];
273 return m;
274 }
275 /* void normalize() {
276 long i;
277 double s;
278 for ( i=0, s=0; i<9; i+=3 ) s += d[i]*d[i]; // First column
279 s = sqrt(s);
280 for ( i=0; i<9; i+=3 ) d[i] /= s;
281 for ( i=0, s=0; i<9; i+=3 ) s += d[i]*d[i+1]; // Second column
282 for ( i=0; i<9; i+=3 ) d[i+1] -= s*d[i];
283 for ( i=1, s=0; i<9; i+=3 ) s += d[i]*d[i];
284 s = sqrt(s);
285 for ( i=1; i<9; i+=3 ) d[i] /= s;
286 for ( i=0, s=0; i<9; i+=3 ) s += d[i]*d[i+2]; // Third column
287 for ( i=0; i<9; i+=3 ) d[i+2] -= s*d[i];
288 for ( i=1, s=0; i<9; i+=3 ) s += d[i]*d[i+1];
289 for ( i=1; i<9; i+=3 ) d[i+1] -= s*d[i];
290 for ( i=2, s=0; i<9; i+=3 ) s += d[i]*d[i];
291 s = sqrt(s);
292 for ( i=2; i<9; i+=3 ) d[i] /= s;
293 }*/
294 void normalize() {
295 long i;
296 double s;
297 for ( i=0, s=0; i<3; i++ ) s += d[i][0]*d[i][0]; // First column
298 s = sqrt(s);
299 for ( i=0; i<3; i++ ) d[i][0] /= s;
300 for ( i=0, s=0; i<3; i++ ) s += d[i][0]*d[i][1]; // Second column
301 for ( i=0; i<3; i++ ) d[i][1] -= s*d[i][0];
302 for ( i=0, s=0; i<3; i++ ) s += d[i][1]*d[i][1];
303 s = sqrt(s);
304 for ( i=0; i<3; i++ ) d[i][1] /= s;
305 for ( i=0, s=0; i<3; i++ ) s += d[i][0]*d[i][2]; // Third column
306 for ( i=0; i<3; i++ ) d[i][2] -= s*d[i][0];
307 for ( i=0, s=0; i<3; i++ ) s += d[i][1]*d[i][2];
308 for ( i=0; i<3; i++ ) d[i][2] -= s*d[i][1];
309 for ( i=0, s=0; i<3; i++ ) s += d[i][2]*d[i][2];
310 s = sqrt(s);
311 for ( i=0; i<3; i++ ) d[i][2] /= s;
312 }
313 Matrix3 rotation(const Matrix3& mat1, const Matrix3& mat2) {
314 Quaternion q1 = mat1.quaternion();
315 Quaternion q2 = mat2.quaternion();
316 Quaternion q = q1 * q2.invert();
317 Matrix3 mat(q);
318 return mat;
319 }
320 Matrix3 singular_value_decomposition() {
321 long i, j;
322 Matrix mat(3,3);
323 Matrix3 mat3;
324 for ( i=0; i<3; i++ ) for ( j=0; j<3; j++ ) mat[i][j] = d[i][j];
325 mat.singular_value_decomposition();
326 for ( i=0; i<3; i++ ) for ( j=0; j<3; j++ ) mat3[i][j] = mat[i][j];
327 return mat3;
328 }
329 };
330
331 template <typename Type>
332 inline Matrix3 operator*(Vector3<Type>& v, Matrix3& m)
333 {
334 Matrix3 mn;
335 for ( long i=0; i<3; i++ )
336 mn[i] = m[i] * v[i];
337
338 return mn;
339 }
340
341 /*template <typename Type>
342 inline Matrix3 operator/=(Matrix3& m, Vector3<Type>& v)
343 {
344 // Type* d = (Type *) &v;
345 for ( long i=0; i<3; i++ )
346 m[i] /= v[i];
347
348 return m;
349 }
350 */
351 /*template <typename Type>
352 inline Matrix3 operator/(const Matrix3& m, Vector3<Type>& v)
353 {
354 Matrix3 mn(m);
355 return mn /= v;
356 }
357 */
358 #endif
359
360 ostream& operator<<(ostream& output, Matrix3& m);
361
362 // Function prototypes
363 int matrix3_show_hp(Matrix3& mat);
364
365
Matrix3::Matrix3
Matrix3(const double v)
Definition: Matrix3.h:46
Matrix3::determinant
double determinant()
Definition: Matrix3.h:260
Matrix3::Matrix3
Matrix3(Vector3< T > axis, double angle)
Definition: Matrix3.h:73
Matrix3::trace
double trace()
Definition: Matrix3.h:265
Matrix3::Matrix3
Matrix3(const double d0, const double d1, const double d2, const double d3, const double d4, const double d5, const double d6, const double d7, const double d8)
Definition: Matrix3.h:59
Matrix3::rotation
Matrix3 rotation(const Matrix3 &mat1, const Matrix3 &mat2)
Definition: Matrix3.h:313
Matrix3::operator+=
Matrix3 operator+=(const Matrix3 &m)
Definition: Matrix3.h:176
Matrix3::Matrix3
Matrix3(Vector3< Type1 > &from_vec, Vector3< Type2 > &to_vec)
Definition: Matrix3.h:85
Quaternion::invert
double invert()
Definition: Quaternion.h:161
Matrix3
Definition: Matrix3.h:17
Quaternion
Definition: Quaternion.h:25
Matrix.h
Generalized matrix class.
Matrix3::operator *
Matrix3 operator *(const Matrix3 &m)
Definition: Matrix3.h:196
Matrix3::operator/
Matrix3 operator/(const double v)
Definition: Matrix3.h:227
Matrix3::quaternion
Quaternion quaternion() const
Definition: Matrix3.h:129
Matrix3::Matrix3
Matrix3(const vector< double > &v)
Definition: Matrix3.h:53
Matrix::singular_value_decomposition
double singular_value_decomposition()
Singular value decomposition of a matrix .
Definition: matrix.cpp:177
Matrix3::operator=
Matrix3 operator=(const Matrix3 &m)
Definition: Matrix3.h:172
Matrix3::operator/=
Matrix3 operator/=(Vector3< T > &v)
Definition: Matrix3.h:251
Vector3::scalar
double scalar(const Vector3 &v) const
Definition: Vector3.h:247
matrix3_show_hp
int matrix3_show_hp(Matrix3 &mat)
Print out a matrix.
Definition: Matrix3.cpp:31
Quaternion::normalize
double normalize()
Definition: Quaternion.h:151
Quaternion.h
Class for quaternions.
operator<<
ostream & operator<<(ostream &output, Matrix3 &m)
Definition: Matrix3.cpp:15
Matrix3::operator *=
Matrix3 operator *=(const Matrix3 &m)
Definition: Matrix3.h:192
Matrix3::operator-
Matrix3 operator-(const Matrix3 &m)
Definition: Matrix3.h:188
Matrix
Definition: Matrix.h:21
Matrix3::Matrix3
Matrix3(Quaternion q)
Definition: Matrix3.h:68
Matrix3::normalize
void normalize()
Definition: Matrix3.h:294
Matrix3::transpose
Matrix3 transpose() const
Definition: Matrix3.h:268
Vector3< double >
Matrix3::singular_value_decomposition
Matrix3 singular_value_decomposition()
Definition: Matrix3.h:320
Vector3.h
Class for 3-value vectors.
Matrix3::Matrix3
Matrix3(const double *v)
Definition: Matrix3.h:47
Matrix3::Matrix3
Matrix3(double tilt, double axis)
Definition: Matrix3.h:78
Matrix3::operator[]
Vector3< double > & operator[](long i)
Definition: Matrix3.h:169
Matrix3::operator-=
Matrix3 operator-=(const Matrix3 &m)
Definition: Matrix3.h:184
operator *
Matrix3 operator *(Vector3< Type > &v, Matrix3 &m)
Definition: Matrix3.h:332
Matrix3::operator==
bool operator==(const Matrix3 &m)
Definition: Matrix3.h:231
Matrix3::Matrix3
Matrix3()
Definition: Matrix3.h:44
Matrix3::Matrix3
Matrix3(const Matrix3 &m)
Definition: Matrix3.h:66
Matrix3::operator/
Matrix3 operator/(Vector3< T > &v)
Definition: Matrix3.h:256
Matrix3::operator+
Matrix3 operator+(const Matrix3 &m)
Definition: Matrix3.h:180
Matrix3::operator/=
Matrix3 operator/=(const double v)
Definition: Matrix3.h:223