26 #ifndef WFMATH_ROTMATRIX_FUNCS_H
27 #define WFMATH_ROTMATRIX_FUNCS_H
29 #include <wfmath/rotmatrix.h>
31 #include <wfmath/vector.h>
32 #include <wfmath/error.h>
33 #include <wfmath/const.h>
42 inline RotMatrix<dim>::RotMatrix(
const RotMatrix<dim>& m)
43 : m_flip(m.m_flip), m_valid(m.m_valid), m_age(1)
45 for(
int i = 0; i < dim; ++i)
46 for(
int j = 0; j < dim; ++j)
47 m_elem[i][j] = m.m_elem[i][j];
51 inline RotMatrix<dim>& RotMatrix<dim>::operator=(
const RotMatrix<dim>& m)
53 for(
int i = 0; i < dim; ++i)
54 for(
int j = 0; j < dim; ++j)
55 m_elem[i][j] = m.m_elem[i][j];
65 inline bool RotMatrix<dim>::isEqualTo(
const RotMatrix<dim>& m,
CoordType epsilon)
const
75 for(
int i = 0; i < dim; ++i)
76 for(
int j = 0; j < dim; ++j)
77 if(std::fabs(m_elem[i][j] - m.m_elem[i][j]) > epsilon)
82 assert(
"Generated values, must be equal if all components are equal" &&
93 for(
int i = 0; i < dim; ++i) {
94 for(
int j = 0; j < dim; ++j) {
96 for(
int k = 0; k < dim; ++k) {
97 out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[k][j];
102 out.m_flip = (m1.m_flip != m2.m_flip);
103 out.m_valid = m1.m_valid && m2.m_valid;
104 out.m_age = m1.m_age + m2.m_age;
105 out.checkNormalization();
115 for(
int i = 0; i < dim; ++i) {
116 for(
int j = 0; j < dim; ++j) {
117 out.m_elem[i][j] = 0;
118 for(
int k = 0; k < dim; ++k) {
119 out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[j][k];
124 out.m_flip = (m1.m_flip != m2.m_flip);
125 out.m_valid = m1.m_valid && m2.m_valid;
126 out.m_age = m1.m_age + m2.m_age;
127 out.checkNormalization();
137 for(
int i = 0; i < dim; ++i) {
138 for(
int j = 0; j < dim; ++j) {
139 out.m_elem[i][j] = 0;
140 for(
int k = 0; k < dim; ++k) {
141 out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[k][j];
146 out.m_flip = (m1.m_flip != m2.m_flip);
147 out.m_valid = m1.m_valid && m2.m_valid;
148 out.m_age = m1.m_age + m2.m_age;
149 out.checkNormalization();
159 for(
int i = 0; i < dim; ++i) {
160 for(
int j = 0; j < dim; ++j) {
161 out.m_elem[i][j] = 0;
162 for(
int k = 0; k < dim; ++k) {
163 out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[j][k];
168 out.m_flip = (m1.m_flip != m2.m_flip);
169 out.m_valid = m1.m_valid && m2.m_valid;
170 out.m_age = m1.m_age + m2.m_age;
171 out.checkNormalization();
181 for(
int i = 0; i < dim; ++i) {
183 for(
int j = 0; j < dim; ++j) {
184 out.m_elem[i] += m.m_elem[i][j] * v.m_elem[j];
188 out.m_valid = m.m_valid && v.m_valid;
198 for(
int i = 0; i < dim; ++i) {
200 for(
int j = 0; j < dim; ++j) {
201 out.m_elem[i] += m.m_elem[j][i] * v.m_elem[j];
205 out.m_valid = m.m_valid && v.m_valid;
229 inline Vector<dim>
operator*(
const RotMatrix<dim>& m,
const Vector<dim>& v)
235 inline Vector<dim>
operator*(
const Vector<dim>& v,
const RotMatrix<dim>& m)
246 for(
int i = 0; i < dim; ++i)
247 for(
int j = 0; j < dim; ++j)
248 scratch_vals[i*dim+j] = vals[i][j];
250 return _setVals(scratch_vals, precision);
259 for(
int i = 0; i < dim*dim; ++i)
260 scratch_vals[i] = vals[i];
262 return _setVals(scratch_vals, precision);
265 bool _MatrixSetValsImpl(
const int size,
CoordType* vals,
bool& flip,
276 if(!_MatrixSetValsImpl(dim, vals, flip, buf1, buf2, precision))
281 for(
int i = 0; i < dim; ++i)
282 for(
int j = 0; j < dim; ++j)
283 m_elem[i][j] = vals[i*dim+j];
297 for(
int j = 0; j < dim; ++j)
298 out[j] = m_elem[i][j];
310 for(
int j = 0; j < dim; ++j)
311 out[j] = m_elem[j][i];
323 for(
int i = 0; i < dim; ++i)
324 for(
int j = 0; j < dim; ++j)
325 m.m_elem[j][i] = m_elem[i][j];
337 for(
int i = 0; i < dim; ++i)
338 for(
int j = 0; j < dim; ++j)
339 m_elem[i][j] = ((i == j) ? 1.0f : 0.0f);
353 for(
int i = 0; i < dim; ++i)
363 assert(i >= 0 && i < dim && j >= 0 && j < dim && i != j);
365 CoordType ctheta = std::cos(theta), stheta = std::sin(theta);
367 for(
int k = 0; k < dim; ++k) {
368 for(
int l = 0; l < dim; ++l) {
371 m_elem[k][l] = ctheta;
377 m_elem[k][l] = stheta;
378 else if(k == j && l == i)
379 m_elem[k][l] = -stheta;
400 assert(
"need nonzero length vector" && v1_sqr_mag > 0);
404 Vector<dim> vperp = v2 - v1 * Dot(v1, v2) / v1_sqr_mag;
407 if((vperp_sqr_mag / v1_sqr_mag) < (dim * numeric_constants<CoordType>::epsilon() * numeric_constants<CoordType>::epsilon())) {
408 assert(
"need nonzero length vector" && v2.
sqrMag() > 0);
419 CoordType mag_prod = std::sqrt(v1_sqr_mag * vperp_sqr_mag);
421 stheta = std::sin(theta);
425 for(
int i = 0; i < dim; ++i)
426 for(
int j = 0; j < dim; ++j)
427 m_elem[i][j] += ((ctheta - 1) * (v1[i] * v1[j] / v1_sqr_mag
428 + vperp[i] * vperp[j] / vperp_sqr_mag)
429 - stheta * (vperp[i] * v1[j] - v1[i] * vperp[j]) / mag_prod);
446 assert(
"need nonzero length vector" && fromSqrMag > 0);
448 assert(
"need nonzero length vector" && toSqrMag > 0);
450 CoordType sqrmagprod = fromSqrMag * toSqrMag;
451 CoordType magprod = std::sqrt(sqrmagprod);
452 CoordType ctheta_plus_1 = dot / magprod + 1;
454 if(ctheta_plus_1 < numeric_constants<CoordType>::epsilon()) {
457 m_elem[0][0] = m_elem[1][1] = ctheta_plus_1 - 1;
458 CoordType sin_theta = std::sqrt(2 * ctheta_plus_1);
459 bool direction = ((to[0] * from[1] - to[1] * from[0]) >= 0);
460 m_elem[0][1] = direction ? sin_theta : -sin_theta;
461 m_elem[1][0] = -m_elem[0][1];
470 for(
int i = 0; i < dim; ++i) {
471 for(
int j = i; j < dim; ++j) {
472 CoordType projfrom = from[i] * from[j] / fromSqrMag;
473 CoordType projto = to[i] * to[j] / toSqrMag;
475 CoordType ijprod = from[i] * to[j], jiprod = to[i] * from[j];
477 CoordType termthree = (ijprod + jiprod) * dot / sqrmagprod;
479 CoordType combined = (projfrom + projto - termthree) / ctheta_plus_1;
482 m_elem[i][i] = 1 - combined;
485 CoordType diffterm = (jiprod - ijprod) / magprod;
487 m_elem[i][j] = -diffterm - combined;
488 m_elem[j][i] = diffterm - combined;
504 const bool not_flip);
519 assert(
"need nonzero length vector" && sqr_mag > 0);
522 for(
int i = 0; i < dim - 1; ++i)
523 for(
int j = i + 1; j < dim; ++j)
524 m_elem[i][j] = m_elem[j][i] = -2 * v[i] * v[j] / sqr_mag;
527 for(
int i = 0; i < dim; ++i)
528 m_elem[i][i] = 1 - 2 * v[i] * v[i] / sqr_mag;
540 for(
int i = 0; i < dim; ++i)
541 for(
int j = 0; j < dim; ++j)
542 m_elem[i][j] = (i == j) ? -1 : 0;
562 for(
int i = 0; i < dim; ++i) {
563 for(
int j = 0; j < dim; ++j) {
564 buf1[j*dim + i] = m_elem[i][j];
565 buf2[j*dim + i] = ((i == j) ? 1.f : 0.f);
569 bool success = _MatrixInverseImpl(dim, buf1, buf2);
575 for(
int i = 0; i < dim; ++i) {
576 for(
int j = 0; j < dim; ++j) {
578 elem += buf2[i*dim + j];
588 #endif // WFMATH_ROTMATRIX_FUNCS_H