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 bool RotMatrix<dim>::isEqualTo(
const RotMatrix<dim>& m,
CoordType epsilon)
const 53 if (!m.m_valid || !m_valid) {
58 for(
int i = 0; i < dim; ++i)
59 for(
int j = 0; j < dim; ++j)
60 if(std::fabs(m_elem[i][j] - m.m_elem[i][j]) > epsilon)
65 assert(
"Generated values, must be equal if all components are equal" &&
76 for(
int i = 0; i < dim; ++i) {
77 for(
int j = 0; j < dim; ++j) {
79 for(
int k = 0; k < dim; ++k) {
80 out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[k][j];
85 out.m_flip = (m1.m_flip != m2.m_flip);
86 out.m_valid = m1.m_valid && m2.m_valid;
87 out.m_age = m1.m_age + m2.m_age;
88 out.checkNormalization();
98 for(
int i = 0; i < dim; ++i) {
99 for(
int j = 0; j < dim; ++j) {
100 out.m_elem[i][j] = 0;
101 for(
int k = 0; k < dim; ++k) {
102 out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[j][k];
107 out.m_flip = (m1.m_flip != m2.m_flip);
108 out.m_valid = m1.m_valid && m2.m_valid;
109 out.m_age = m1.m_age + m2.m_age;
110 out.checkNormalization();
120 for(
int i = 0; i < dim; ++i) {
121 for(
int j = 0; j < dim; ++j) {
122 out.m_elem[i][j] = 0;
123 for(
int k = 0; k < dim; ++k) {
124 out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[k][j];
129 out.m_flip = (m1.m_flip != m2.m_flip);
130 out.m_valid = m1.m_valid && m2.m_valid;
131 out.m_age = m1.m_age + m2.m_age;
132 out.checkNormalization();
142 for(
int i = 0; i < dim; ++i) {
143 for(
int j = 0; j < dim; ++j) {
144 out.m_elem[i][j] = 0;
145 for(
int k = 0; k < dim; ++k) {
146 out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[j][k];
151 out.m_flip = (m1.m_flip != m2.m_flip);
152 out.m_valid = m1.m_valid && m2.m_valid;
153 out.m_age = m1.m_age + m2.m_age;
154 out.checkNormalization();
164 for(
int i = 0; i < dim; ++i) {
166 for(
int j = 0; j < dim; ++j) {
167 out.m_elem[i] += m.m_elem[i][j] * v.m_elem[j];
171 out.m_valid = m.m_valid && v.m_valid;
181 for(
int i = 0; i < dim; ++i) {
183 for(
int j = 0; j < dim; ++j) {
184 out.m_elem[i] += m.m_elem[j][i] * v.m_elem[j];
188 out.m_valid = m.m_valid && v.m_valid;
229 for(
int i = 0; i < dim; ++i)
230 for(
int j = 0; j < dim; ++j)
231 scratch_vals[i*dim+j] = vals[i][j];
233 return _setVals(scratch_vals, precision);
242 for(
int i = 0; i < dim*dim; ++i)
243 scratch_vals[i] = vals[i];
245 return _setVals(scratch_vals, precision);
248 bool _MatrixSetValsImpl(
int size,
CoordType* vals,
bool& flip,
259 if(!_MatrixSetValsImpl(dim, vals, flip, buf1, buf2, precision))
264 for(
int i = 0; i < dim; ++i)
265 for(
int j = 0; j < dim; ++j)
266 m_elem[i][j] = vals[i*dim+j];
280 for(
int j = 0; j < dim; ++j)
281 out[j] = m_elem[i][j];
293 for(
int j = 0; j < dim; ++j)
294 out[j] = m_elem[j][i];
306 for(
int i = 0; i < dim; ++i)
307 for(
int j = 0; j < dim; ++j)
308 m.m_elem[j][i] = m_elem[i][j];
320 for(
int i = 0; i < dim; ++i)
321 for(
int j = 0; j < dim; ++j)
322 m_elem[i][j] = ((i == j) ? 1.0f : 0.0f);
336 for(
int i = 0; i < dim; ++i)
346 assert(i >= 0 && i < dim && j >= 0 && j < dim && i != j);
348 CoordType ctheta = std::cos(theta), stheta = std::sin(theta);
350 for(
int k = 0; k < dim; ++k) {
351 for(
int l = 0; l < dim; ++l) {
354 m_elem[k][l] = ctheta;
360 m_elem[k][l] = stheta;
361 else if(k == j && l == i)
362 m_elem[k][l] = -stheta;
383 assert(
"need nonzero length vector" && v1_sqr_mag > 0);
387 Vector<dim> vperp = v2 - v1 * Dot(v1, v2) / v1_sqr_mag;
391 assert(
"need nonzero length vector" && v2.
sqrMag() > 0);
402 CoordType mag_prod = std::sqrt(v1_sqr_mag * vperp_sqr_mag);
404 stheta = std::sin(theta);
408 for(
int i = 0; i < dim; ++i)
409 for(
int j = 0; j < dim; ++j)
410 m_elem[i][j] += ((ctheta - 1) * (v1[i] * v1[j] / v1_sqr_mag
411 + vperp[i] * vperp[j] / vperp_sqr_mag)
412 - stheta * (vperp[i] * v1[j] - v1[i] * vperp[j]) / mag_prod);
429 assert(
"need nonzero length vector" && fromSqrMag > 0);
431 assert(
"need nonzero length vector" && toSqrMag > 0);
433 CoordType sqrmagprod = fromSqrMag * toSqrMag;
434 CoordType magprod = std::sqrt(sqrmagprod);
435 CoordType ctheta_plus_1 = dot / magprod + 1;
440 m_elem[0][0] = m_elem[1][1] = ctheta_plus_1 - 1;
441 CoordType sin_theta = std::sqrt(2 * ctheta_plus_1);
442 bool direction = ((to[0] * from[1] - to[1] * from[0]) >= 0);
443 m_elem[0][1] = direction ? sin_theta : -sin_theta;
444 m_elem[1][0] = -m_elem[0][1];
453 for(
int i = 0; i < dim; ++i) {
454 for(
int j = i; j < dim; ++j) {
455 CoordType projfrom = from[i] * from[j] / fromSqrMag;
456 CoordType projto = to[i] * to[j] / toSqrMag;
458 CoordType ijprod = from[i] * to[j], jiprod = to[i] * from[j];
460 CoordType termthree = (ijprod + jiprod) * dot / sqrmagprod;
462 CoordType combined = (projfrom + projto - termthree) / ctheta_plus_1;
465 m_elem[i][i] = 1 - combined;
468 CoordType diffterm = (jiprod - ijprod) / magprod;
470 m_elem[i][j] = -diffterm - combined;
471 m_elem[j][i] = diffterm - combined;
487 const bool not_flip);
502 assert(
"need nonzero length vector" && sqr_mag > 0);
505 for(
int i = 0; i < dim - 1; ++i)
506 for(
int j = i + 1; j < dim; ++j)
507 m_elem[i][j] = m_elem[j][i] = -2 * v[i] * v[j] / sqr_mag;
510 for(
int i = 0; i < dim; ++i)
511 m_elem[i][i] = 1 - 2 * v[i] * v[i] / sqr_mag;
523 for(
int i = 0; i < dim; ++i)
524 for(
int j = 0; j < dim; ++j)
525 m_elem[i][j] = (i == j) ? -1 : 0;
545 for(
int i = 0; i < dim; ++i) {
546 for(
int j = 0; j < dim; ++j) {
547 buf1[j*dim + i] = m_elem[i][j];
548 buf2[j*dim + i] = ((i == j) ? 1.f : 0.f);
552 bool success = _MatrixInverseImpl(dim, buf1, buf2);
558 for(
int i = 0; i < dim; ++i) {
559 for(
int j = 0; j < dim; ++j) {
561 elem += buf2[i*dim + j];
571 #endif // WFMATH_ROTMATRIX_FUNCS_H 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.
void normalize()
normalize to remove accumulated round-off error
RotMatrix< dim > InvProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1^-1 * m2^-1
Vector< dim > column(int i) const
Get a copy of the i'th column as a Vector.
A dim dimensional rotation matrix. Technically, a member of the group O(dim).
double CoordType
Basic floating point type.
CoordType sqrMag() const
The squared magnitude of a vector.
RotMatrix< dim > InvProd(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1^-1 * m2
void setValid(bool valid=true)
make isValid() return true if you've initialized the vector by hand
RotMatrix & rotate(const RotMatrix &m)
rotate the matrix using another matrix
A dim dimensional vector.
bool setVals(const CoordType vals[dim][dim], CoordType precision=numeric_constants< CoordType >::epsilon())
Set the values of the elements of the matrix.
RotMatrix & fromQuaternion(const Quaternion &q, bool not_flip=true)
3D only: set a RotMatrix from a Quaternion
Vector< dim > row(int i) const
Get a copy of the i'th row as a Vector.
RotMatrix & mirror()
set the matrix to mirror all axes
RotMatrix & identity()
set the matrix to the identity matrix
RotMatrix< dim > operator*(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
An error thrown by certain functions when passed parallel vectors.
RotMatrix< dim > ProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2^-1
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
CoordType trace() const
Get the trace of the matrix.
RotMatrix inverse() const
Get the inverse of the matrix.