18 #ifndef _IGNITION_MATRIX3_HH_
19 #define _IGNITION_MATRIX3_HH_
43 std::memset(this->data, 0,
sizeof(this->data[0][0])*9);
50 std::memcpy(this->data, _m.data,
sizeof(this->data[0][0])*9);
64 T _v10, T _v11, T _v12,
65 T _v20, T _v21, T _v22)
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;
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());
108 public:
void Set(T _v00, T _v01, T _v02,
109 T _v10, T _v11, T _v12,
110 T _v20, T _v21, T _v22)
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;
131 this->
Col(0, _xAxis);
132 this->
Col(1, _yAxis);
133 this->
Col(2, _zAxis);
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;
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;
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;
166 this->data[0][_c] = _v.
X();
167 this->data[1][_c] = _v.
Y();
168 this->data[2][_c] = _v.
Z();
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));
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));
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]);
217 this->data[0][0]*_m(0, 0)+
218 this->data[0][1]*_m(1, 0)+
219 this->data[0][2]*_m(2, 0),
221 this->data[0][0]*_m(0, 1)+
222 this->data[0][1]*_m(1, 1)+
223 this->data[0][2]*_m(2, 1),
225 this->data[0][0]*_m(0, 2)+
226 this->data[0][1]*_m(1, 2)+
227 this->data[0][2]*_m(2, 2),
230 this->data[1][0]*_m(0, 0)+
231 this->data[1][1]*_m(1, 0)+
232 this->data[1][2]*_m(2, 0),
234 this->data[1][0]*_m(0, 1)+
235 this->data[1][1]*_m(1, 1)+
236 this->data[1][2]*_m(2, 1),
238 this->data[1][0]*_m(0, 2)+
239 this->data[1][1]*_m(1, 2)+
240 this->data[1][2]*_m(2, 2),
243 this->data[2][0]*_m(0, 0)+
244 this->data[2][1]*_m(1, 0)+
245 this->data[2][2]*_m(2, 0),
247 this->data[2][0]*_m(0, 1)+
248 this->data[2][1]*_m(1, 1)+
249 this->data[2][2]*_m(2, 1),
251 this->data[2][0]*_m(0, 2)+
252 this->data[2][1]*_m(1, 2)+
253 this->data[2][2]*_m(2, 2));
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());
302 return !(*
this == _m);
308 public:
inline const T &
operator()(
size_t _row,
size_t _col)
const
310 if (_row >= 3 || _col >= 3)
312 return this->data[_row][_col];
320 if (_row >= 3 || _col >=3)
322 return this->data[_row][_col];
329 double t0 = this->data[2][2]*this->data[1][1] -
330 this->data[2][1]*this->data[1][2];
332 double t1 = -(this->data[2][2]*this->data[1][0] -
333 this->data[2][0]*this->data[1][2]);
335 double t2 = this->data[2][1]*this->data[1][0] -
336 this->data[2][0]*this->data[1][1];
338 double invDet = 1.0 / (t0 * this->data[0][0] +
339 t1 * this->data[0][1] +
340 t2 * this->data[0][2]);
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]),
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]),
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]));
387 _in.setf(std::ios_base::skipws);
389 _in >> d[0] >> d[1] >> d[2]
390 >> d[3] >> d[4] >> d[5]
391 >> d[6] >> d[7] >> d[8];
393 _m.
Set(d[0], d[1], d[2],
400 private: T data[3][3];
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