All Classes Namespaces Files Functions Variables Typedefs Friends Macros Pages
Matrix3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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 
18 #ifndef _IGNITION_MATRIX3_HH_
19 #define _IGNITION_MATRIX3_HH_
20 
21 #include <cstring>
22 #include <ignition/math/Vector3.hh>
24 
25 namespace ignition
26 {
27  namespace math
28  {
31  template<typename T>
32  class Matrix3
33  {
35  public: static const Matrix3<T> Identity;
36 
38  public: static const Matrix3<T> Zero;
39 
41  public: Matrix3()
42  {
43  std::memset(this->data, 0, sizeof(this->data[0][0])*9);
44  }
45 
48  public: Matrix3(const Matrix3<T> &_m)
49  {
50  std::memcpy(this->data, _m.data, sizeof(this->data[0][0])*9);
51  }
52 
63  public: Matrix3(T _v00, T _v01, T _v02,
64  T _v10, T _v11, T _v12,
65  T _v20, T _v21, T _v22)
66  {
67  this->data[0][0] = _v00;
68  this->data[0][1] = _v01;
69  this->data[0][2] = _v02;
70  this->data[1][0] = _v10;
71  this->data[1][1] = _v11;
72  this->data[1][2] = _v12;
73  this->data[2][0] = _v20;
74  this->data[2][1] = _v21;
75  this->data[2][2] = _v22;
76  }
77 
80  public: Matrix3(const Quaternion<T> &_q)
81  {
82  Quaternion<T> qt = _q;
83  qt.Normalize();
84  this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
85  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
86  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
87  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
88  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
89  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
90  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
91  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
92  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y());
93  }
94 
96  public: virtual ~Matrix3() {}
97 
108  public: void Set(T _v00, T _v01, T _v02,
109  T _v10, T _v11, T _v12,
110  T _v20, T _v21, T _v22)
111  {
112  this->data[0][0] = _v00;
113  this->data[0][1] = _v01;
114  this->data[0][2] = _v02;
115  this->data[1][0] = _v10;
116  this->data[1][1] = _v11;
117  this->data[1][2] = _v12;
118  this->data[2][0] = _v20;
119  this->data[2][1] = _v21;
120  this->data[2][2] = _v22;
121  }
122 
127  public: void Axes(const Vector3<T> &_xAxis,
128  const Vector3<T> &_yAxis,
129  const Vector3<T> &_zAxis)
130  {
131  this->Col(0, _xAxis);
132  this->Col(1, _yAxis);
133  this->Col(2, _zAxis);
134  }
135 
139  public: void Axis(const Vector3<T> &_axis, T _angle)
140  {
141  T c = cos(_angle);
142  T s = sin(_angle);
143  T C = 1-c;
144 
145  this->data[0][0] = _axis.X()*_axis.X()*C + c;
146  this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
147  this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;
148 
149  this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
150  this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
151  this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;
152 
153  this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
154  this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
155  this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
156  }
157 
161  public: void Col(unsigned int _c, const Vector3<T> &_v)
162  {
163  if (_c >= 3)
164  throw IndexException();
165 
166  this->data[0][_c] = _v.X();
167  this->data[1][_c] = _v.Y();
168  this->data[2][_c] = _v.Z();
169  }
170 
172  public: Matrix3<T> operator-(const Matrix3<T> &_m) const
173  {
174  return Matrix3<T>(
175  this->data[0][0] - _m(0, 0),
176  this->data[0][1] - _m(0, 1),
177  this->data[0][2] - _m(0, 2),
178  this->data[1][0] - _m(1, 0),
179  this->data[1][1] - _m(1, 1),
180  this->data[1][2] - _m(1, 2),
181  this->data[2][0] - _m(2, 0),
182  this->data[2][1] - _m(2, 1),
183  this->data[2][2] - _m(2, 2));
184  }
185 
187  public: Matrix3<T> operator+(const Matrix3<T> &_m) const
188  {
189  return Matrix3<T>(
190  this->data[0][0]+_m(0, 0),
191  this->data[0][1]+_m(0, 1),
192  this->data[0][2]+_m(0, 2),
193  this->data[1][0]+_m(1, 0),
194  this->data[1][1]+_m(1, 1),
195  this->data[1][2]+_m(1, 2),
196  this->data[2][0]+_m(2, 0),
197  this->data[2][1]+_m(2, 1),
198  this->data[2][2]+_m(2, 2));
199  }
200 
202  public: Matrix3<T> operator*(const T &_s) const
203  {
204  return Matrix3<T>(
205  _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
206  _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
207  _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
208  }
209 
213  public: Matrix3<T> operator*(const Matrix3<T> &_m) const
214  {
215  return Matrix3<T>(
216  // first row
217  this->data[0][0]*_m(0, 0)+
218  this->data[0][1]*_m(1, 0)+
219  this->data[0][2]*_m(2, 0),
220 
221  this->data[0][0]*_m(0, 1)+
222  this->data[0][1]*_m(1, 1)+
223  this->data[0][2]*_m(2, 1),
224 
225  this->data[0][0]*_m(0, 2)+
226  this->data[0][1]*_m(1, 2)+
227  this->data[0][2]*_m(2, 2),
228 
229  // second row
230  this->data[1][0]*_m(0, 0)+
231  this->data[1][1]*_m(1, 0)+
232  this->data[1][2]*_m(2, 0),
233 
234  this->data[1][0]*_m(0, 1)+
235  this->data[1][1]*_m(1, 1)+
236  this->data[1][2]*_m(2, 1),
237 
238  this->data[1][0]*_m(0, 2)+
239  this->data[1][1]*_m(1, 2)+
240  this->data[1][2]*_m(2, 2),
241 
242  // third row
243  this->data[2][0]*_m(0, 0)+
244  this->data[2][1]*_m(1, 0)+
245  this->data[2][2]*_m(2, 0),
246 
247  this->data[2][0]*_m(0, 1)+
248  this->data[2][1]*_m(1, 1)+
249  this->data[2][2]*_m(2, 1),
250 
251  this->data[2][0]*_m(0, 2)+
252  this->data[2][1]*_m(1, 2)+
253  this->data[2][2]*_m(2, 2));
254  }
255 
259  public: Vector3<T> operator*(const Vector3<T> &_vec) const
260  {
261  return Vector3<T>(
262  this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
263  this->data[0][2]*_vec.Z(),
264  this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
265  this->data[1][2]*_vec.Z(),
266  this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
267  this->data[2][2]*_vec.Z());
268  }
269 
274  public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
275  {
276  return _m * _s;
277  }
278 
282  public: bool operator==(const Matrix3<T> &_m) const
283  {
284  return math::equal(this->data[0][0], _m(0, 0)) &&
285  math::equal(this->data[0][1], _m(0, 1)) &&
286  math::equal(this->data[0][2], _m(0, 2)) &&
287 
288  math::equal(this->data[1][0], _m(1, 0)) &&
289  math::equal(this->data[1][1], _m(1, 1)) &&
290  math::equal(this->data[1][2], _m(1, 2)) &&
291 
292  math::equal(this->data[2][0], _m(2, 0)) &&
293  math::equal(this->data[2][1], _m(2, 1)) &&
294  math::equal(this->data[2][2], _m(2, 2));
295  }
296 
300  public: bool operator!=(const Matrix3<T> &_m) const
301  {
302  return !(*this == _m);
303  }
304 
308  public: inline const T &operator()(size_t _row, size_t _col) const
309  {
310  if (_row >= 3 || _col >= 3)
311  throw IndexException();
312  return this->data[_row][_col];
313  }
314 
318  public: inline T &operator()(size_t _row, size_t _col)
319  {
320  if (_row >= 3 || _col >=3)
321  throw IndexException();
322  return this->data[_row][_col];
323  }
324 
327  public: Matrix3<T> Inverse() const
328  {
329  double t0 = this->data[2][2]*this->data[1][1] -
330  this->data[2][1]*this->data[1][2];
331 
332  double t1 = -(this->data[2][2]*this->data[1][0] -
333  this->data[2][0]*this->data[1][2]);
334 
335  double t2 = this->data[2][1]*this->data[1][0] -
336  this->data[2][0]*this->data[1][1];
337 
338  double invDet = 1.0 / (t0 * this->data[0][0] +
339  t1 * this->data[0][1] +
340  t2 * this->data[0][2]);
341 
342  return invDet * Matrix3<T>(
343  t0,
344  - (this->data[2][2] * this->data[0][1] -
345  this->data[2][1] * this->data[0][2]),
346  + (this->data[1][2] * this->data[0][1] -
347  this->data[1][1] * this->data[0][2]),
348  t1,
349  + (this->data[2][2] * this->data[0][0] -
350  this->data[2][0] * this->data[0][2]),
351  - (this->data[1][2] * this->data[0][0] -
352  this->data[1][0] * this->data[0][2]),
353  t2,
354  - (this->data[2][1] * this->data[0][0] -
355  this->data[2][0] * this->data[0][1]),
356  + (this->data[1][1] * this->data[0][0] -
357  this->data[1][0] * this->data[0][1]));
358  }
359 
364  public: friend std::ostream &operator<<(
365  std::ostream &_out, const ignition::math::Matrix3<T> &_m)
366  {
367  _out << precision(_m(0, 0), 6) << " "
368  << precision(_m(0, 1), 6) << " "
369  << precision(_m(0, 2), 6) << " "
370  << precision(_m(1, 0), 6) << " "
371  << precision(_m(1, 1), 6) << " "
372  << precision(_m(1, 2), 6) << " "
373  << precision(_m(2, 0), 6) << " "
374  << precision(_m(2, 1), 6) << " "
375  << precision(_m(2, 2), 6);
376 
377  return _out;
378  }
383  public: friend std::istream &operator>>(
384  std::istream &_in, ignition::math::Matrix3<T> &_m)
385  {
386  // Skip white spaces
387  _in.setf(std::ios_base::skipws);
388  T d[9];
389  _in >> d[0] >> d[1] >> d[2]
390  >> d[3] >> d[4] >> d[5]
391  >> d[6] >> d[7] >> d[8];
392 
393  _m.Set(d[0], d[1], d[2],
394  d[3], d[4], d[5],
395  d[6], d[7], d[8]);
396  return _in;
397  }
398 
400  private: T data[3][3];
401  };
402 
403  template<typename T>
404  const Matrix3<T> Matrix3<T>::Identity(
405  1, 0, 0,
406  0, 1, 0,
407  0, 0, 1);
408 
409  template<typename T>
410  const Matrix3<T> Matrix3<T>::Zero(
411  0, 0, 0,
412  0, 0, 0,
413  0, 0, 0);
414 
418  }
419 }
420 
421 #endif
const T & operator()(size_t _row, size_t _col) const
Array subscript operator.
Definition: Matrix3.hh:308
bool operator!=(const Matrix3< T > &_m) const
Inequality test operator.
Definition: Matrix3.hh:300
friend std::istream & operator>>(std::istream &_in, ignition::math::Matrix3< T > &_m)
Stream extraction operator.
Definition: Matrix3.hh:383
Matrix3< double > Matrix3d
Definition: Matrix3.hh:416
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:188
Matrix3< int > Matrix3i
Definition: Matrix3.hh:415
Matrix3(const Matrix3< T > &_m)
Copy constructor.
Definition: Matrix3.hh:48
Matrix3()
Constructor.
Definition: Matrix3.hh:41
T & operator()(size_t _row, size_t _col)
Array subscript operator.
Definition: Matrix3.hh:318
Vector3< T > operator*(const Vector3< T > &_vec) const
Multiplication operator.
Definition: Matrix3.hh:259
const T & Y() const
Get the y component.
Definition: Quaternion.hh:750
Matrix3< T > operator*(const T &_s) const
returns the element wise scalar multiplication
Definition: Matrix3.hh:202
T X() const
Get the x value.
Definition: Vector3.hh:545
const T & Z() const
Get the z component.
Definition: Quaternion.hh:757
static const Matrix3< T > Zero
Zero matrix.
Definition: Matrix3.hh:38
friend Matrix3< T > operator*(T _s, const Matrix3< T > &_m)
Matrix multiplication operator for scaling.
Definition: Matrix3.hh:274
Matrix3< T > Inverse() const
Return the inverse matrix.
Definition: Matrix3.hh:327
virtual ~Matrix3()
Desctructor.
Definition: Matrix3.hh:96
T Y() const
Get the y value.
Definition: Vector3.hh:552
void Col(unsigned int _c, const Vector3< T > &_v)
Set a column.
Definition: Matrix3.hh:161
A 3x3 matrix class.
Definition: Matrix3.hh:32
Matrix3< float > Matrix3f
Definition: Matrix3.hh:417
bool operator==(const Matrix3< T > &_m) const
Equality test operator.
Definition: Matrix3.hh:282
T Z() const
Get the z value.
Definition: Vector3.hh:559
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:30
Matrix3< T > operator*(const Matrix3< T > &_m) const
Matrix multiplication operator.
Definition: Matrix3.hh:213
Matrix3< T > operator+(const Matrix3< T > &_m) const
returns the element wise sum of two matrices
Definition: Matrix3.hh:187
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
static const Matrix3< T > Identity
Identity matrix.
Definition: Matrix3.hh:35
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:108
Matrix3< T > operator-(const Matrix3< T > &_m) const
returns the element wise difference of two matrices
Definition: Matrix3.hh:172
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Matrix3< T > &_m)
Stream insertion operator.
Definition: Matrix3.hh:364
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:127
const T & W() const
Get the w component.
Definition: Quaternion.hh:736
Matrix3(const Quaternion< T > &_q)
Construct Matrix3 from a quaternion.
Definition: Matrix3.hh:80
bool equal(const T &_a, const T &_b, const T &_epsilon=1e-6)
check if two values are equal, within a tolerance
Definition: Helpers.hh:177
void Axis(const Vector3< T > &_axis, T _angle)
Set the matrix from an axis and angle.
Definition: Matrix3.hh:139
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:206
A quaternion class.
Definition: Quaternion.hh:31
Matrix3(T _v00, T _v01, T _v02, T _v10, T _v11, T _v12, T _v20, T _v21, T _v22)
Constructor.
Definition: Matrix3.hh:63
const T & X() const
Get the x component.
Definition: Quaternion.hh:743