17 #ifndef IGNITION_MATH_MATRIX3_HH_
18 #define IGNITION_MATH_MATRIX3_HH_
25 #include <ignition/math/config.hh>
31 inline namespace IGNITION_MATH_VERSION_NAMESPACE
49 std::memset(this->data, 0,
sizeof(this->data[0][0])*9);
56 std::memcpy(this->data, _m.data,
sizeof(this->data[0][0])*9);
70 T _v10, T _v11, T _v12,
71 T _v20, T _v21, T _v22)
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;
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());
114 public:
void Set(T _v00, T _v01, T _v02,
115 T _v10, T _v11, T _v12,
116 T _v20, T _v21, T _v22)
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;
137 this->
Col(0, _xAxis);
138 this->
Col(1, _yAxis);
139 this->
Col(2, _zAxis);
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;
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;
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;
173 if (_v1LengthSquared <= 0.0)
176 this->
Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
181 if (_v2LengthSquared <= 0.0)
184 this->
Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
188 const T dot = _v1.
Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
189 if (fabs(dot - 1.0) <= 1e-6)
192 this->
Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
195 else if (fabs(dot + 1.0) <= 1e-6)
198 this->
Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
204 this->
Axis(cross, acos(dot));
213 unsigned int c =
clamp(_c, 0u, 2u);
215 this->data[0][c] = _v.
X();
216 this->data[1][c] = _v.
Y();
217 this->data[2][c] = _v.
Z();
225 memcpy(this->data, _mat.data,
sizeof(this->data[0][0])*9);
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));
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));
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]);
275 this->data[0][0]*_m(0, 0)+
276 this->data[0][1]*_m(1, 0)+
277 this->data[0][2]*_m(2, 0),
279 this->data[0][0]*_m(0, 1)+
280 this->data[0][1]*_m(1, 1)+
281 this->data[0][2]*_m(2, 1),
283 this->data[0][0]*_m(0, 2)+
284 this->data[0][1]*_m(1, 2)+
285 this->data[0][2]*_m(2, 2),
288 this->data[1][0]*_m(0, 0)+
289 this->data[1][1]*_m(1, 0)+
290 this->data[1][2]*_m(2, 0),
292 this->data[1][0]*_m(0, 1)+
293 this->data[1][1]*_m(1, 1)+
294 this->data[1][2]*_m(2, 1),
296 this->data[1][0]*_m(0, 2)+
297 this->data[1][1]*_m(1, 2)+
298 this->data[1][2]*_m(2, 2),
301 this->data[2][0]*_m(0, 0)+
302 this->data[2][1]*_m(1, 0)+
303 this->data[2][2]*_m(2, 0),
305 this->data[2][0]*_m(0, 1)+
306 this->data[2][1]*_m(1, 1)+
307 this->data[2][2]*_m(2, 1),
309 this->data[2][0]*_m(0, 2)+
310 this->data[2][1]*_m(1, 2)+
311 this->data[2][2]*_m(2, 2));
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());
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());
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);
376 return this->
Equal(_m,
static_cast<T
>(1e-6));
392 return !(*
this == _m);
399 public:
inline const T &
operator()(
size_t _row,
size_t _col)
const
419 T t0 = this->data[2][2]*this->data[1][1]
420 - this->data[2][1]*this->data[1][2];
422 T t1 = -(this->data[2][2]*this->data[1][0]
423 -this->data[2][0]*this->data[1][2]);
425 T t2 = this->data[2][1]*this->data[1][0]
426 - this->data[2][0]*this->data[1][1];
428 return t0 * this->data[0][0]
429 + t1 * this->data[0][1]
430 + t2 * this->data[0][2];
437 T t0 = this->data[2][2]*this->data[1][1] -
438 this->data[2][1]*this->data[1][2];
440 T t1 = -(this->data[2][2]*this->data[1][0] -
441 this->data[2][0]*this->data[1][2]);
443 T t2 = this->data[2][1]*this->data[1][0] -
444 this->data[2][0]*this->data[1][1];
446 T invDet = 1.0 / (t0 * this->data[0][0] +
447 t1 * this->data[0][1] +
448 t2 * this->data[0][2]);
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]),
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]),
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]));
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]);
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]);
513 _in.setf(std::ios_base::skipws);
515 _in >> d[0] >> d[1] >> d[2]
516 >> d[3] >> d[4] >> d[5]
517 >> d[6] >> d[7] >> d[8];
519 _m.
Set(d[0], d[1], d[2],
526 private: T data[3][3];