30 #include "rotmatrix_funcs.h"
31 #include "quaternion.h"
43 const Vector<3> &vref = q.vector();
45 xx = vref[0] * vref[0];
46 xy = vref[0] * vref[1];
47 xz = vref[0] * vref[2];
48 yy = vref[1] * vref[1];
49 yz = vref[1] * vref[2];
50 zz = vref[2] * vref[2];
52 Vector<3> wvec = vref * q.scalar();
54 m_elem[0][0] = 1 - 2 * (yy + zz);
55 m_elem[1][1] = 1 - 2 * (xx + zz);
56 m_elem[2][2] = 1 - 2 * (xx + yy);
58 m_elem[0][1] = 2 * (xy - wvec[2]);
59 m_elem[0][2] = 2 * (xz + wvec[1]);
61 m_elem[1][0] = 2 * (xy + wvec[2]);
62 m_elem[1][2] = 2 * (yz - wvec[0]);
64 m_elem[2][0] = 2 * (xz - wvec[1]);
65 m_elem[2][1] = 2 * (yz + wvec[0]);
70 *
this =
Prod(*
this, RotMatrix<3>().mirror(0));
80 m_valid = m_valid && q.isValid();
85 for(
auto & vec_num : m_elem) {
86 for(
int elem_num = 0; elem_num < 3; ++elem_num)
87 vec[elem_num] = vec_num[elem_num];
89 for(
int elem_num = 0; elem_num < 3; ++elem_num)
90 vec_num[elem_num] = vec[elem_num];
105 for(
int i = 0; i < 3; ++i) {
113 assert(
"Must pass a nonzero length vector as axis to avoid this" && main_comp != -1);
115 Vector<3> tmp, v1, v2;
117 int new_comp = main_comp ? main_comp - 1 : 2;
118 for(
int i = 0; i < 3; ++i)
119 tmp[i] = ((i == new_comp) ? 1.f : 0.f);
121 v1 =
Cross(axis, tmp);
122 v2 =
Cross(axis, v1);
124 return rotation(v1, v2, theta);
138 for(
int i = 0; i < 3; ++i) {
146 assert(
"Can't happen with nonzero angle" && main_comp != -1);
148 Vector<3> tmp, v1, v2;
150 int new_comp = main_comp ? main_comp - 1 : 2;
151 for(
int i = 0; i < 3; ++i)
152 tmp[i] = ((i == new_comp) ? 1.f : 0.f);
154 v1 =
Cross(axis, tmp);
155 v2 =
Cross(axis, v1);
157 return rotation(v1, v2, angle);
160 bool _MatrixSetValsImpl(
const int size, CoordType* vals,
bool& flip,
161 CoordType* buf1, CoordType* buf2, CoordType precision)
163 precision = std::fabs(precision);
174 for(
int i = 0; i < size; ++i) {
175 for(
int j = 0; j < size; ++j) {
177 for(
int k = 0; k < size; ++k) {
178 ans += vals[i*size+k] * vals[j*size+k];
183 ans = std::fabs(ans);
189 if(try_prec > precision)
192 if(try_prec <= numeric_constants<CoordType>::epsilon())
220 for(
int i = 0; i < size; ++i) {
221 for(
int j = 0; j < size; ++j) {
222 buf1[i*size+j] = vals[j*size+i];
223 buf2[i*size+j] = ((i == j) ? 1.f : 0.f);
227 bool ans = _MatrixInverseImpl(size, buf1, buf2);
233 for(
int i = 0; i < size; ++i) {
234 for(
int j = 0; j < size; ++j) {
236 elem += buf2[i*size+j];
249 for(
int i = 0; i < size; ++i)
250 for(
int j = 0; j < size; ++j)
251 buf1[i*size+j] = vals[i*size+j];
253 flip = _MatrixDeterminantImpl(size, buf1) < 0;
258 static CoordType _MatrixDeterminantImpl(
const int size, CoordType* m)
264 for(
int i = 0; i < size - 1; ++i) {
266 for(
int j = 0; j < size; ++j)
267 minval += m[j*size+i] * m[j*size+i];
268 minval /= std::numeric_limits<CoordType>::max();
269 if(minval < std::numeric_limits<CoordType>::min())
270 minval = std::numeric_limits<CoordType>::min();
271 if(m[i*size+i] * m[i*size+i] < minval) {
273 for(j = i + 1; j < size; ++j)
274 if(m[j*size+i] * m[j*size+i] >= minval)
278 m[i*size+i] = m[j*size+i];
279 for(
int k = i + 1; k < size; ++k)
280 m[i*size+k] += m[j*size+k];
282 for(
int j = i + 1; j < size; ++j) {
283 CoordType factor = m[j*size+i] / m[i*size+i];
288 for(
int k = i + 1; k < size; ++k)
289 m[j*size+k] -= m[i*size+k] * factor;
296 for(
int i = 0; i < size; ++i)
302 bool _MatrixInverseImpl(
const int size, CoordType* in, CoordType* out)
307 for(
int i = 0; i < size; ++i) {
311 for(
int j = 0; j < size; ++j)
312 minval += in[j*size+i] * in[j*size+i];
313 minval /= std::numeric_limits<CoordType>::max();
314 if(minval < std::numeric_limits<CoordType>::min())
315 minval = std::numeric_limits<CoordType>::min();
316 if(in[i*size+i] * in[i*size+i] < minval) {
318 for(j = i + 1; j < size; ++j)
319 if(in[j*size+i] * in[j*size+i] >= minval)
323 for(
int k = 0; k < size; ++k) {
324 out[i*size+k] += out[j*size+k];
325 in[i*size+k] += in[j*size+k];
333 for(
int j = 0; j < size; ++j) {
334 out[i*size+j] /= tmp;
340 for(
int j = i + 1; j < size; ++j) {
344 for(
int k = 0; k < size; ++k) {
345 out[j*size+k] -= out[i*size+k] * tmp2;
346 in[j*size+k] -= in[i*size+k] * tmp2;
355 for(
int i = size - 1; i >= 1; --i) {
356 for(
int j = i - 1; j >= 0; --j) {
359 for(
int k = 0; k < size; ++k)
360 out[j*size+k] -= out[i*size+k] * tmp;
369 RotMatrix<3>::RotMatrix(
const Quaternion& q,
370 const bool not_flip) : m_flip(false), m_valid(false),
373 fromQuaternion(q, not_flip);
376 template class RotMatrix<2>;
377 template class RotMatrix<3>;
379 static_assert(std::is_standard_layout<RotMatrix<2>>::value,
"RotMatrix should be standard layout.");
380 static_assert(std::is_trivially_copyable<RotMatrix<2>>::value,
"RotMatrix should be trivially copyable.");
382 static_assert(std::is_standard_layout<RotMatrix<3>>::value,
"RotMatrix should be standard layout.");
383 static_assert(std::is_trivially_copyable<RotMatrix<3>>::value,
"RotMatrix should be trivially copyable.");
385 template RotMatrix<2>
operator*<2>(RotMatrix<2>
const&, RotMatrix<2>
const&);
386 template RotMatrix<3>
operator*<3>(RotMatrix<3>
const&, RotMatrix<3>
const&);
388 template RotMatrix<2> ProdInv<2>(RotMatrix<2>
const&, RotMatrix<2>
const&);
389 template RotMatrix<3> ProdInv<3>(RotMatrix<3>
const&, RotMatrix<3>
const&);
391 template Vector<2>
operator*<2>(Vector<2>
const&, RotMatrix<2>
const&);
392 template Vector<3>
operator*<3>(Vector<3>
const&, RotMatrix<3>
const&);
394 template Vector<2>
operator*<2>(RotMatrix<2>
const&, Vector<2>
const&);
395 template Vector<3>
operator*<3>(RotMatrix<3>
const&, Vector<3>
const&);
397 template Vector<2> ProdInv<2>(Vector<2>
const&, RotMatrix<2>
const&);
398 template Vector<3> ProdInv<3>(Vector<3>
const&, RotMatrix<3>
const&);
400 template Vector<3> Prod<3>(Vector<3>
const&, RotMatrix<3>
const&);
401 template Vector<2> Prod<2>(Vector<2>
const&, RotMatrix<2>
const&);
403 template RotMatrix<3> Prod<3>(RotMatrix<3>
const&, RotMatrix<3>
const&);
404 template RotMatrix<2> Prod<2>(RotMatrix<2>
const&, RotMatrix<2>
const&);
RotMatrix & rotation(int i, int j, CoordType theta)
set the matrix to a rotation by the angle theta in the (i, j) plane
RotMatrix & rotate(const RotMatrix &m)
rotate the matrix using another matrix
RotMatrix & fromQuaternion(const Quaternion &q, bool not_flip=true)
3D only: set a RotMatrix from a Quaternion
Generic library namespace.
double CoordType
Basic floating point type.
CoordType Cross(const Vector< 2 > &v1, const Vector< 2 > &v2)
2D only: get the z component of the cross product of two vectors
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2