Loading...
Searching...
No Matches
Matrix3.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2012 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17#ifndef IGNITION_MATH_MATRIX3_HH_
18#define IGNITION_MATH_MATRIX3_HH_
19
20#include <algorithm>
21#include <cstring>
25#include <ignition/math/config.hh>
26
27namespace ignition
28{
29 namespace math
30 {
31 inline namespace IGNITION_MATH_VERSION_NAMESPACE
32 {
33 template <typename T> class Quaternion;
34
37 template<typename T>
38 class Matrix3
39 {
41 public: static const Matrix3<T> Identity;
42
44 public: static const Matrix3<T> Zero;
45
47 public: Matrix3()
48 {
49 std::memset(this->data, 0, sizeof(this->data[0][0])*9);
50 }
51
54 public: Matrix3(const Matrix3<T> &_m)
55 {
56 std::memcpy(this->data, _m.data, sizeof(this->data[0][0])*9);
57 }
58
69 public: Matrix3(T _v00, T _v01, T _v02,
70 T _v10, T _v11, T _v12,
71 T _v20, T _v21, T _v22)
72 {
73 this->data[0][0] = _v00;
74 this->data[0][1] = _v01;
75 this->data[0][2] = _v02;
76 this->data[1][0] = _v10;
77 this->data[1][1] = _v11;
78 this->data[1][2] = _v12;
79 this->data[2][0] = _v20;
80 this->data[2][1] = _v21;
81 this->data[2][2] = _v22;
82 }
83
86 public: explicit Matrix3(const Quaternion<T> &_q)
87 {
88 Quaternion<T> qt = _q;
89 qt.Normalize();
90 this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
91 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
92 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
93 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
94 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
95 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
96 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
97 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
98 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
99 }
100
102 public: virtual ~Matrix3() {}
103
114 public: void Set(T _v00, T _v01, T _v02,
115 T _v10, T _v11, T _v12,
116 T _v20, T _v21, T _v22)
117 {
118 this->data[0][0] = _v00;
119 this->data[0][1] = _v01;
120 this->data[0][2] = _v02;
121 this->data[1][0] = _v10;
122 this->data[1][1] = _v11;
123 this->data[1][2] = _v12;
124 this->data[2][0] = _v20;
125 this->data[2][1] = _v21;
126 this->data[2][2] = _v22;
127 }
128
133 public: void Axes(const Vector3<T> &_xAxis,
134 const Vector3<T> &_yAxis,
135 const Vector3<T> &_zAxis)
136 {
137 this->Col(0, _xAxis);
138 this->Col(1, _yAxis);
139 this->Col(2, _zAxis);
140 }
141
145 public: void Axis(const Vector3<T> &_axis, T _angle)
146 {
147 T c = cos(_angle);
148 T s = sin(_angle);
149 T C = 1-c;
150
151 this->data[0][0] = _axis.X()*_axis.X()*C + c;
152 this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
153 this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
154
155 this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
156 this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
157 this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
158
159 this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
160 this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
161 this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
162 }
163
170 public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
171 {
172 const T _v1LengthSquared = _v1.SquaredLength();
173 if (_v1LengthSquared <= 0.0)
174 {
175 // zero vector - we can't handle this
176 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
177 return;
178 }
179
180 const T _v2LengthSquared = _v2.SquaredLength();
181 if (_v2LengthSquared <= 0.0)
182 {
183 // zero vector - we can't handle this
184 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
185 return;
186 }
187
188 const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
189 if (fabs(dot - 1.0) <= 1e-6)
190 {
191 // the vectors are parallel
192 this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
193 return;
194 }
195 else if (fabs(dot + 1.0) <= 1e-6)
196 {
197 // the vectors are opposite
198 this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
199 return;
200 }
201
202 const Vector3<T> cross = _v1.Cross(_v2).Normalize();
203
204 this->Axis(cross, acos(dot));
205 }
206
211 public: void Col(unsigned int _c, const Vector3<T> &_v)
212 {
213 unsigned int c = clamp(_c, 0u, 2u);
214
215 this->data[0][c] = _v.X();
216 this->data[1][c] = _v.Y();
217 this->data[2][c] = _v.Z();
218 }
219
223 public: Matrix3<T> &operator=(const Matrix3<T> &_mat)
224 {
225 memcpy(this->data, _mat.data, sizeof(this->data[0][0])*9);
226 return *this;
227 }
228
230 public: Matrix3<T> operator-(const Matrix3<T> &_m) const
231 {
232 return Matrix3<T>(
233 this->data[0][0] - _m(0, 0),
234 this->data[0][1] - _m(0, 1),
235 this->data[0][2] - _m(0, 2),
236 this->data[1][0] - _m(1, 0),
237 this->data[1][1] - _m(1, 1),
238 this->data[1][2] - _m(1, 2),
239 this->data[2][0] - _m(2, 0),
240 this->data[2][1] - _m(2, 1),
241 this->data[2][2] - _m(2, 2));
242 }
243
245 public: Matrix3<T> operator+(const Matrix3<T> &_m) const
246 {
247 return Matrix3<T>(
248 this->data[0][0]+_m(0, 0),
249 this->data[0][1]+_m(0, 1),
250 this->data[0][2]+_m(0, 2),
251 this->data[1][0]+_m(1, 0),
252 this->data[1][1]+_m(1, 1),
253 this->data[1][2]+_m(1, 2),
254 this->data[2][0]+_m(2, 0),
255 this->data[2][1]+_m(2, 1),
256 this->data[2][2]+_m(2, 2));
257 }
258
260 public: Matrix3<T> operator*(const T &_s) const
261 {
262 return Matrix3<T>(
263 _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
264 _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
265 _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
266 }
267
271 public: Matrix3<T> operator*(const Matrix3<T> &_m) const
272 {
273 return Matrix3<T>(
274 // first row
275 this->data[0][0]*_m(0, 0)+
276 this->data[0][1]*_m(1, 0)+
277 this->data[0][2]*_m(2, 0),
278
279 this->data[0][0]*_m(0, 1)+
280 this->data[0][1]*_m(1, 1)+
281 this->data[0][2]*_m(2, 1),
282
283 this->data[0][0]*_m(0, 2)+
284 this->data[0][1]*_m(1, 2)+
285 this->data[0][2]*_m(2, 2),
286
287 // second row
288 this->data[1][0]*_m(0, 0)+
289 this->data[1][1]*_m(1, 0)+
290 this->data[1][2]*_m(2, 0),
291
292 this->data[1][0]*_m(0, 1)+
293 this->data[1][1]*_m(1, 1)+
294 this->data[1][2]*_m(2, 1),
295
296 this->data[1][0]*_m(0, 2)+
297 this->data[1][1]*_m(1, 2)+
298 this->data[1][2]*_m(2, 2),
299
300 // third row
301 this->data[2][0]*_m(0, 0)+
302 this->data[2][1]*_m(1, 0)+
303 this->data[2][2]*_m(2, 0),
304
305 this->data[2][0]*_m(0, 1)+
306 this->data[2][1]*_m(1, 1)+
307 this->data[2][2]*_m(2, 1),
308
309 this->data[2][0]*_m(0, 2)+
310 this->data[2][1]*_m(1, 2)+
311 this->data[2][2]*_m(2, 2));
312 }
313
318 public: Vector3<T> operator*(const Vector3<T> &_vec) const
319 {
320 return Vector3<T>(
321 this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
322 this->data[0][2]*_vec.Z(),
323 this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
324 this->data[1][2]*_vec.Z(),
325 this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
326 this->data[2][2]*_vec.Z());
327 }
328
333 public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
334 {
335 return _m * _s;
336 }
337
344 public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
345 const Matrix3<T> &_m)
346 {
347 return Vector3<T>(
348 _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
349 _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
350 _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
351 }
352
358 public: bool Equal(const Matrix3 &_m, const T &_tol) const
359 {
360 return equal<T>(this->data[0][0], _m(0, 0), _tol)
361 && equal<T>(this->data[0][1], _m(0, 1), _tol)
362 && equal<T>(this->data[0][2], _m(0, 2), _tol)
363 && equal<T>(this->data[1][0], _m(1, 0), _tol)
364 && equal<T>(this->data[1][1], _m(1, 1), _tol)
365 && equal<T>(this->data[1][2], _m(1, 2), _tol)
366 && equal<T>(this->data[2][0], _m(2, 0), _tol)
367 && equal<T>(this->data[2][1], _m(2, 1), _tol)
368 && equal<T>(this->data[2][2], _m(2, 2), _tol);
369 }
370
374 public: bool operator==(const Matrix3<T> &_m) const
375 {
376 return this->Equal(_m, static_cast<T>(1e-6));
377 }
378
383 {
384 return *this = Matrix3<T>(_q);
385 }
386
390 public: bool operator!=(const Matrix3<T> &_m) const
391 {
392 return !(*this == _m);
393 }
394
399 public: inline const T &operator()(size_t _row, size_t _col) const
400 {
401 return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
403 }
404
409 public: inline T &operator()(size_t _row, size_t _col)
410 {
411 return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
413 }
414
417 public: T Determinant() const
418 {
419 T t0 = this->data[2][2]*this->data[1][1]
420 - this->data[2][1]*this->data[1][2];
421
422 T t1 = -(this->data[2][2]*this->data[1][0]
423 -this->data[2][0]*this->data[1][2]);
424
425 T t2 = this->data[2][1]*this->data[1][0]
426 - this->data[2][0]*this->data[1][1];
427
428 return t0 * this->data[0][0]
429 + t1 * this->data[0][1]
430 + t2 * this->data[0][2];
431 }
432
435 public: Matrix3<T> Inverse() const
436 {
437 T t0 = this->data[2][2]*this->data[1][1] -
438 this->data[2][1]*this->data[1][2];
439
440 T t1 = -(this->data[2][2]*this->data[1][0] -
441 this->data[2][0]*this->data[1][2]);
442
443 T t2 = this->data[2][1]*this->data[1][0] -
444 this->data[2][0]*this->data[1][1];
445
446 T invDet = 1.0 / (t0 * this->data[0][0] +
447 t1 * this->data[0][1] +
448 t2 * this->data[0][2]);
449
450 return invDet * Matrix3<T>(
451 t0,
452 - (this->data[2][2] * this->data[0][1] -
453 this->data[2][1] * this->data[0][2]),
454 + (this->data[1][2] * this->data[0][1] -
455 this->data[1][1] * this->data[0][2]),
456 t1,
457 + (this->data[2][2] * this->data[0][0] -
458 this->data[2][0] * this->data[0][2]),
459 - (this->data[1][2] * this->data[0][0] -
460 this->data[1][0] * this->data[0][2]),
461 t2,
462 - (this->data[2][1] * this->data[0][0] -
463 this->data[2][0] * this->data[0][1]),
464 + (this->data[1][1] * this->data[0][0] -
465 this->data[1][0] * this->data[0][1]));
466 }
467
469 public: void Transpose()
470 {
471 std::swap(this->data[0][1], this->data[1][0]);
472 std::swap(this->data[0][2], this->data[2][0]);
473 std::swap(this->data[1][2], this->data[2][1]);
474 }
475
478 public: Matrix3<T> Transposed() const
479 {
480 return Matrix3<T>(
481 this->data[0][0], this->data[1][0], this->data[2][0],
482 this->data[0][1], this->data[1][1], this->data[2][1],
483 this->data[0][2], this->data[1][2], this->data[2][2]);
484 }
485
490 public: friend std::ostream &operator<<(
491 std::ostream &_out, const ignition::math::Matrix3<T> &_m)
492 {
493 _out << precision(_m(0, 0), 6) << " "
494 << precision(_m(0, 1), 6) << " "
495 << precision(_m(0, 2), 6) << " "
496 << precision(_m(1, 0), 6) << " "
497 << precision(_m(1, 1), 6) << " "
498 << precision(_m(1, 2), 6) << " "
499 << precision(_m(2, 0), 6) << " "
500 << precision(_m(2, 1), 6) << " "
501 << precision(_m(2, 2), 6);
502
503 return _out;
504 }
509 public: friend std::istream &operator>>(
510 std::istream &_in, ignition::math::Matrix3<T> &_m)
511 {
512 // Skip white spaces
513 _in.setf(std::ios_base::skipws);
514 T d[9];
515 _in >> d[0] >> d[1] >> d[2]
516 >> d[3] >> d[4] >> d[5]
517 >> d[6] >> d[7] >> d[8];
518
519 _m.Set(d[0], d[1], d[2],
520 d[3], d[4], d[5],
521 d[6], d[7], d[8]);
522 return _in;
523 }
524
526 private: T data[3][3];
527 };
528
529 template<typename T>
531 1, 0, 0,
532 0, 1, 0,
533 0, 0, 1);
534
535 template<typename T>
537 0, 0, 0,
538 0, 0, 0,
539 0, 0, 0);
540
544 }
545 }
546}
547
548#endif
A 3x3 matrix class.
Definition Quaternion.hh:32
Matrix3< T > operator+(const Matrix3< T > &_m) const
returns the element wise sum of two matrices
Definition Matrix3.hh:245
Matrix3< T > & operator=(const Quaternion< T > &_q)
Set the matrix3 from a quaternion.
Definition Matrix3.hh:382
Matrix3< T > Inverse() const
Return the inverse matrix.
Definition Matrix3.hh:435
Matrix3< T > & operator=(const Matrix3< T > &_mat)
Equal operator.
Definition Matrix3.hh:223
friend Matrix3< T > operator*(T _s, const Matrix3< T > &_m)
Matrix multiplication operator for scaling.
Definition Matrix3.hh:333
virtual ~Matrix3()
Desctructor.
Definition Matrix3.hh:102
Matrix3< T > operator*(const Matrix3< T > &_m) const
Matrix multiplication operator.
Definition Matrix3.hh:271
Matrix3(const Quaternion< T > &_q)
Construct Matrix3 from a quaternion.
Definition Matrix3.hh:86
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator with Vector3 on the right treated like a column vector.
Definition Matrix3.hh:318
Matrix3(const Matrix3< T > &_m)
Copy constructor.
Definition Matrix3.hh:54
void Transpose()
Transpose this matrix.
Definition Matrix3.hh:469
void Set(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Set values.
Definition Matrix3.hh:114
void From2Axes(const Vector3< T > &_v1, const Vector3< T > &_v2)
Set the matrix to represent rotation from vector _v1 to vector _v2, so that _v2.Normalize() == this *...
Definition Matrix3.hh:170
T & operator()(size_t _row, size_t _col)
Array subscript operator.
Definition Matrix3.hh:409
bool operator!=(const Matrix3< T > &_m) const
Inequality test operator.
Definition Matrix3.hh:390
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix3< T > &_m)
Stream insertion operator.
Definition Matrix3.hh:490
T Determinant() const
Return the determinant of the matrix.
Definition Matrix3.hh:417
Matrix3(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Constructor.
Definition Matrix3.hh:69
Matrix3()
Constructor.
Definition Matrix3.hh:47
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition Matrix3.hh:478
const T & operator()(size_t _row, size_t _col) const
Array subscript operator.
Definition Matrix3.hh:399
bool operator==(const Matrix3< T > &_m) const
Equality test operator.
Definition Matrix3.hh:374
static const Matrix3< T > Identity
Identity matrix.
Definition Matrix3.hh:41
void Axis(const Vector3< T > &_axis, T _angle)
Set the matrix from an axis and angle.
Definition Matrix3.hh:145
friend Vector3< T > operator*(const Vector3< T > &_v, const Matrix3< T > &_m)
Matrix left multiplication operator for Vector3.
Definition Matrix3.hh:344
Matrix3< T > operator*(const T &_s) const
returns the element wise scalar multiplication
Definition Matrix3.hh:260
bool Equal(const Matrix3 &_m, const T &_tol) const
Equality test with tolerance.
Definition Matrix3.hh:358
void Axes(const Vector3< T > &_xAxis, const Vector3< T > &_yAxis, const Vector3< T > &_zAxis)
Set the matrix from three axis (1 per column)
Definition Matrix3.hh:133
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix3< T > &_m)
Stream extraction operator.
Definition Matrix3.hh:509
void Col(unsigned int _c, const Vector3< T > &_v)
Set a column.
Definition Matrix3.hh:211
static const Matrix3< T > Zero
Zero matrix.
Definition Matrix3.hh:44
Matrix3< T > operator-(const Matrix3< T > &_m) const
returns the element wise difference of two matrices
Definition Matrix3.hh:230
A quaternion class.
Definition Quaternion.hh:38
void Normalize()
Normalize the quaternion.
Definition Quaternion.hh:223
const T & X() const
Get the x component.
Definition Quaternion.hh:955
const T & Y() const
Get the y component.
Definition Quaternion.hh:962
const T & Z() const
Get the z component.
Definition Quaternion.hh:969
const T & W() const
Get the w component.
Definition Quaternion.hh:948
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
T Z() const
Get the z value.
Definition Vector3.hh:661
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition Vector3.hh:123
T Y() const
Get the y value.
Definition Vector3.hh:654
Vector3 Normalize()
Normalize the vector length.
Definition Vector3.hh:132
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition Vector3.hh:198
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition Vector3.hh:188
T X() const
Get the x value.
Definition Vector3.hh:647
Matrix3< int > Matrix3i
Definition Matrix3.hh:541
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition Helpers.hh:579
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition Helpers.hh:395
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition Helpers.hh:222
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition Helpers.hh:545
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition Helpers.hh:216
Matrix3< double > Matrix3d
Definition Matrix3.hh:542
Matrix3< float > Matrix3f
Definition Matrix3.hh:543
Definition Angle.hh:40