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,
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)
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;
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)
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
Generic library namespace.
static FloatType epsilon()
This is the attempted precision of the library.
double CoordType
Basic floating point type.
RotMatrix & rotate(const RotMatrix &m)
rotate the matrix using another matrix
CoordType Cross(const Vector< 2 > &v1, const Vector< 2 > &v2)
2D only: get the z component of the cross product of two vectors
RotMatrix & fromQuaternion(const Quaternion &q, bool not_flip=true)
3D only: set a RotMatrix from a Quaternion
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2