00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef WFMATH_ROTMATRIX_FUNCS_H
00027 #define WFMATH_ROTMATRIX_FUNCS_H
00028
00029 #include <wfmath/vector.h>
00030 #include <wfmath/rotmatrix.h>
00031 #include <wfmath/error.h>
00032 #include <wfmath/const.h>
00033
00034 namespace WFMath {
00035
00036 template<const int dim>
00037 inline RotMatrix<dim>::RotMatrix(const RotMatrix<dim>& m)
00038 : m_flip(m.m_flip), m_valid(m.m_valid)
00039 {
00040 for(int i = 0; i < dim; ++i)
00041 for(int j = 0; j < dim; ++j)
00042 m_elem[i][j] = m.m_elem[i][j];
00043 }
00044
00045 template<const int dim>
00046 RotMatrix<dim>& RotMatrix<dim>::operator=(const RotMatrix<dim>& m)
00047 {
00048 for(int i = 0; i < dim; ++i)
00049 for(int j = 0; j < dim; ++j)
00050 m_elem[i][j] = m.m_elem[i][j];
00051
00052 m_flip = m.m_flip;
00053 m_valid = m.m_valid;
00054
00055 return *this;
00056 }
00057
00058 template<const int dim>
00059 bool RotMatrix<dim>::isEqualTo(const RotMatrix<dim>& m, double epsilon) const
00060 {
00061
00062
00063
00064
00065
00066
00067 assert(epsilon > 0);
00068
00069 for(int i = 0; i < dim; ++i)
00070 for(int j = 0; j < dim; ++j)
00071 if(fabs(m_elem[i][j] - m.m_elem[i][j]) > epsilon)
00072 return false;
00073
00074
00075
00076 assert("Generated values, must be equal if all components are equal" &&
00077 m_flip == m.m_flip);
00078
00079 return true;
00080 }
00081
00082 template<const int dim>
00083 bool RotMatrix<dim>::operator< (const RotMatrix<dim>& m) const
00084 {
00085 if(operator==(m))
00086 return false;
00087
00088 for(int i = 0; i < dim; ++i)
00089 for(int j = 0; j < dim; ++j)
00090 if(m_elem[i][j] != m.m_elem[i][j])
00091 return m_elem[i][j] < m.m_elem[i][j];
00092
00093 assert(false);
00094 }
00095
00096 template<const int dim>
00097 RotMatrix<dim> Prod(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00098 {
00099 RotMatrix<dim> out;
00100
00101 for(int i = 0; i < dim; ++i) {
00102 for(int j = 0; j < dim; ++j) {
00103 out.m_elem[i][j] = 0;
00104 for(int k = 0; k < dim; ++k) {
00105 out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[k][j];
00106 }
00107 }
00108 }
00109
00110 out.m_flip = (m1.m_flip != m2.m_flip);
00111 out.m_valid = m1.m_valid && m2.m_valid;
00112
00113 return out;
00114 }
00115
00116 template<const int dim>
00117 RotMatrix<dim> ProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00118 {
00119 RotMatrix<dim> out;
00120
00121 for(int i = 0; i < dim; ++i) {
00122 for(int j = 0; j < dim; ++j) {
00123 out.m_elem[i][j] = 0;
00124 for(int k = 0; k < dim; ++k) {
00125 out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[j][k];
00126 }
00127 }
00128 }
00129
00130 out.m_flip = (m1.m_flip != m2.m_flip);
00131 out.m_valid = m1.m_valid && m2.m_valid;
00132
00133 return out;
00134 }
00135
00136 template<const int dim>
00137 RotMatrix<dim> InvProd(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00138 {
00139 RotMatrix<dim> out;
00140
00141 for(int i = 0; i < dim; ++i) {
00142 for(int j = 0; j < dim; ++j) {
00143 out.m_elem[i][j] = 0;
00144 for(int k = 0; k < dim; ++k) {
00145 out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[k][j];
00146 }
00147 }
00148 }
00149
00150 out.m_flip = (m1.m_flip != m2.m_flip);
00151 out.m_valid = m1.m_valid && m2.m_valid;
00152
00153 return out;
00154 }
00155
00156 template<const int dim>
00157 RotMatrix<dim> InvProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00158 {
00159 RotMatrix<dim> out;
00160
00161 for(int i = 0; i < dim; ++i) {
00162 for(int j = 0; j < dim; ++j) {
00163 out.m_elem[i][j] = 0;
00164 for(int k = 0; k < dim; ++k) {
00165 out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[j][k];
00166 }
00167 }
00168 }
00169
00170 out.m_flip = (m1.m_flip != m2.m_flip);
00171 out.m_valid = m1.m_valid && m2.m_valid;
00172
00173 return out;
00174 }
00175
00176 template<const int dim>
00177 Vector<dim> Prod(const RotMatrix<dim>& m, const Vector<dim>& v)
00178 {
00179 Vector<dim> out;
00180
00181 for(int i = 0; i < dim; ++i) {
00182 out.m_elem[i] = 0;
00183 for(int j = 0; j < dim; ++j) {
00184 out.m_elem[i] += m.m_elem[i][j] * v.m_elem[j];
00185 }
00186 }
00187
00188 out.m_valid = m.m_valid && v.m_valid;
00189
00190 return out;
00191 }
00192
00193 template<const int dim>
00194 Vector<dim> InvProd(const RotMatrix<dim>& m, const Vector<dim>& v)
00195 {
00196 Vector<dim> out;
00197
00198 for(int i = 0; i < dim; ++i) {
00199 out.m_elem[i] = 0;
00200 for(int j = 0; j < dim; ++j) {
00201 out.m_elem[i] += m.m_elem[j][i] * v.m_elem[j];
00202 }
00203 }
00204
00205 out.m_valid = m.m_valid && v.m_valid;
00206
00207 return out;
00208 }
00209
00210 template<const int dim>
00211 inline Vector<dim> Prod(const Vector<dim>& v, const RotMatrix<dim>& m)
00212 {
00213 return InvProd(m, v);
00214 }
00215
00216 template<const int dim>
00217 inline Vector<dim> ProdInv(const Vector<dim>& v, const RotMatrix<dim>& m)
00218 {
00219 return Prod(m, v);
00220 }
00221
00222 template<const int dim>
00223 inline RotMatrix<dim> operator*(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00224 {
00225 return Prod(m1, m2);
00226 }
00227
00228 template<const int dim>
00229 inline Vector<dim> operator*(const RotMatrix<dim>& m, const Vector<dim>& v)
00230 {
00231 return Prod(m, v);
00232 }
00233
00234 template<const int dim>
00235 Vector<dim> operator*(const Vector<dim>& v, const RotMatrix<dim>& m)
00236 {
00237 return InvProd(m, v);
00238 }
00239
00240 template<const int dim>
00241 bool RotMatrix<dim>::setVals(const CoordType vals[dim][dim], double precision)
00242 {
00243
00244 CoordType scratch_vals[dim*dim];
00245
00246 for(int i = 0; i < dim; ++i)
00247 for(int j = 0; j < dim; ++j)
00248 scratch_vals[i*dim+j] = vals[i][j];
00249
00250 return _setVals(scratch_vals, precision);
00251 }
00252
00253 template<const int dim>
00254 bool RotMatrix<dim>::setVals(const CoordType vals[dim*dim], double precision)
00255 {
00256
00257 CoordType scratch_vals[dim*dim];
00258
00259 for(int i = 0; i < dim*dim; ++i)
00260 scratch_vals[i] = vals[i];
00261
00262 return _setVals(scratch_vals, precision);
00263 }
00264
00265 bool _MatrixSetValsImpl(const int size, CoordType* vals, bool& flip,
00266 CoordType* buf1, CoordType* buf2, double precision);
00267
00268 template<const int dim>
00269 bool RotMatrix<dim>::_setVals(CoordType *vals, double precision)
00270 {
00271
00272
00273 CoordType buf1[dim*dim], buf2[dim*dim];
00274 bool flip;
00275
00276 if(!_MatrixSetValsImpl(dim, vals, flip, buf1, buf2, precision))
00277 return false;
00278
00279
00280
00281 for(int i = 0; i < dim; ++i)
00282 for(int j = 0; j < dim; ++j)
00283 m_elem[i][j] = vals[i*dim+j];
00284
00285 m_flip = flip;
00286 m_valid = true;
00287
00288 return true;
00289 }
00290
00291 template<const int dim>
00292 Vector<dim> RotMatrix<dim>::row(const int i) const
00293 {
00294 Vector<dim> out;
00295
00296 for(int j = 0; j < dim; ++j)
00297 out[j] = m_elem[i][j];
00298
00299 out.setValid(m_valid);
00300
00301 return out;
00302 }
00303
00304 template<const int dim>
00305 Vector<dim> RotMatrix<dim>::column(const int i) const
00306 {
00307 Vector<dim> out;
00308
00309 for(int j = 0; j < dim; ++j)
00310 out[j] = m_elem[j][i];
00311
00312 out.setValid(m_valid);
00313
00314 return out;
00315 }
00316
00317 template<const int dim>
00318 RotMatrix<dim> RotMatrix<dim>::inverse() const
00319 {
00320 RotMatrix<dim> m;
00321
00322 for(int i = 0; i < dim; ++i)
00323 for(int j = 0; j < dim; ++j)
00324 m.m_elem[j][i] = m_elem[i][j];
00325
00326 m.m_valid = m_valid;
00327
00328 return m;
00329 }
00330
00331 template<const int dim>
00332 RotMatrix<dim>& RotMatrix<dim>::identity()
00333 {
00334 for(int i = 0; i < dim; ++i)
00335 for(int j = 0; j < dim; ++j)
00336 m_elem[i][j] = (CoordType) ((i == j) ? 1 : 0);
00337
00338 m_flip = false;
00339 m_valid = true;
00340
00341 return *this;
00342 }
00343
00344 template<const int dim>
00345 CoordType RotMatrix<dim>::trace() const
00346 {
00347 CoordType out = dim ? m_elem[0][0] : 0;
00348
00349 for(int i = 1; i < dim; ++i)
00350 out += m_elem[i][i];
00351
00352 return out;
00353 }
00354
00355 template<const int dim>
00356 RotMatrix<dim>& RotMatrix<dim>::rotation (const int i, const int j,
00357 CoordType theta)
00358 {
00359 assert(i >= 0 && i < dim && j >= 0 && j < dim && i != j);
00360
00361 CoordType ctheta = cos(theta), stheta = sin(theta);
00362
00363 for(int k = 0; k < dim; ++k) {
00364 for(int l = 0; l < dim; ++l) {
00365 if(k == l) {
00366 if(k == i || k == j)
00367 m_elem[k][l] = ctheta;
00368 else
00369 m_elem[k][l] = 1;
00370 }
00371 else {
00372 if(k == i && l == j)
00373 m_elem[k][l] = stheta;
00374 else if(k == j && l == i)
00375 m_elem[k][l] = -stheta;
00376 else
00377 m_elem[k][l] = 0;
00378 }
00379 }
00380 }
00381
00382 m_flip = false;
00383 m_valid = true;
00384
00385 return *this;
00386 }
00387
00388 template<const int dim>
00389 RotMatrix<dim>& RotMatrix<dim>::rotation (const Vector<dim>& v1,
00390 const Vector<dim>& v2,
00391 CoordType theta)
00392 {
00393 CoordType v1_sqr_mag = v1.sqrMag();
00394
00395 assert("need nonzero length vector" && v1_sqr_mag > 0);
00396
00397
00398
00399 Vector<dim> vperp = v2 - v1 * Dot(v1, v2) / v1_sqr_mag;
00400 CoordType vperp_sqr_mag = vperp.sqrMag();
00401
00402 if((vperp_sqr_mag / v1_sqr_mag) < (dim * WFMATH_EPSILON * WFMATH_EPSILON)) {
00403 assert("need nonzero length vector" && v2.sqrMag() > 0);
00404
00405 throw ColinearVectors<dim>(v1, v2);
00406 }
00407
00408
00409
00410
00411
00412
00413
00414 CoordType mag_prod = (CoordType) sqrt(v1_sqr_mag * vperp_sqr_mag);
00415 CoordType ctheta = (CoordType) cos(theta), stheta = (CoordType) sin(theta);
00416
00417 identity();
00418
00419 for(int i = 0; i < dim; ++i)
00420 for(int j = 0; j < dim; ++j)
00421 m_elem[i][j] += ((ctheta - 1) * (v1[i] * v1[j] / v1_sqr_mag
00422 + vperp[i] * vperp[j] / vperp_sqr_mag)
00423 - stheta * (vperp[i] * v1[j] - v1[i] * vperp[j]) / mag_prod);
00424
00425 m_flip = false;
00426 m_valid = true;
00427
00428 return *this;
00429 }
00430
00431 template<const int dim>
00432 RotMatrix<dim>& RotMatrix<dim>::rotation(const Vector<dim>& from,
00433 const Vector<dim>& to)
00434 {
00435
00436
00437
00438 CoordType fromSqrMag = from.sqrMag();
00439 assert(("need nonzero length vector", fromSqrMag > 0));
00440 CoordType toSqrMag = to.sqrMag();
00441 assert(("need nonzero length vector", toSqrMag > 0));
00442 CoordType dot = Dot(from, to);
00443 CoordType sqrmagprod = fromSqrMag * toSqrMag;
00444 CoordType magprod = sqrt(sqrmagprod);
00445 CoordType ctheta_plus_1 = dot / magprod + 1;
00446
00447 if(ctheta_plus_1 < WFMATH_EPSILON) {
00448
00449 throw ColinearVectors<dim>(from, to);
00450 }
00451
00452 for(int i = 0; i < dim; ++i) {
00453 for(int j = i; j < dim; ++j) {
00454 CoordType projfrom = from[i] * from[j] / fromSqrMag;
00455 CoordType projto = to[i] * to[j] / toSqrMag;
00456
00457 CoordType ijprod = from[i] * to[j], jiprod = to[i] * from[j];
00458
00459 CoordType termthree = (ijprod + jiprod) * dot / sqrmagprod;
00460
00461 CoordType combined = (projfrom + projto - termthree) / ctheta_plus_1;
00462
00463 if(i == j) {
00464 m_elem[i][i] = 1 - combined;
00465 }
00466 else {
00467 diffterm = (jiprod - ijprod) / magprod;
00468
00469 m_elem[i][j] = -diffterm - combined;
00470 m_elem[j][i] = diffterm - combined;
00471 }
00472 }
00473 }
00474
00475 m_valid = true;
00476 return *this;
00477 }
00478
00479 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
00480 template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis,
00481 CoordType theta);
00482 template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis);
00483 template<> RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
00484 const bool not_flip);
00485 #else
00486 void _NCFS_RotMatrix3_rotation (RotMatrix<3>& m, const Vector<3>& axis, CoordType theta);
00487 void _NCFS_RotMatrix3_rotation (RotMatrix<3>& m, const Vector<3>& axis);
00488 void _NCFS_RotMatrix3_fromQuaternion(RotMatrix<3>& m, const Quaternion& q,
00489 const bool not_flip, CoordType m_elem[3][3],
00490 bool& m_flip);
00491
00492 template<>
00493 inline RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis, CoordType theta)
00494 {
00495 _NCFS_RotMatrix3_rotation(*this, axis, theta);
00496 return *this;
00497 }
00498
00499 template<>
00500 inline RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis)
00501 {
00502 _NCFS_RotMatrix3_rotation(*this, axis);
00503 return *this;
00504 }
00505
00506 template<>
00507 inline RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
00508 const bool not_flip)
00509 {
00510 _NCFS_RotMatrix3_fromQuaternion(*this, q, not_flip, m_elem, m_flip);
00511 m_valid = true;
00512 return *this;
00513 }
00514
00515 #endif
00516
00517 template<const int dim>
00518 RotMatrix<dim>& RotMatrix<dim>::mirror(const int i)
00519 {
00520 assert(i >= 0 && i < dim);
00521
00522 identity();
00523 m_elem[i][i] = -1;
00524 m_flip = true;
00525 m_valid = true;
00526
00527 return *this;
00528 }
00529
00530 template<const int dim>
00531 RotMatrix<dim>& RotMatrix<dim>::mirror (const Vector<dim>& v)
00532 {
00533
00534
00535
00536
00537
00538
00539 CoordType sqr_mag = v.sqrMag();
00540
00541 assert(("need nonzero length vector", sqr_mag > 0));
00542
00543
00544 for(int i = 0; i < dim - 1; ++i)
00545 for(int j = i + 1; j < dim; ++j)
00546 m_elem[i][j] = m_elem[j][i] = -2 * v[i] * v[j] / sqr_mag;
00547
00548
00549 for(int i = 0; i < dim; ++i)
00550 m_elem[i][i] = 1 - 2 * v[i] * v[i] / sqr_mag;
00551
00552 m_flip = true;
00553 m_valid = true;
00554
00555 return *this;
00556 }
00557
00558 template<const int dim>
00559 RotMatrix<dim>& RotMatrix<dim>::mirror()
00560 {
00561 for(int i = 0; i < dim; ++i)
00562 for(int j = 0; j < dim; ++j)
00563 m_elem[i][j] = (i == j) ? -1 : 0;
00564
00565 m_flip = dim%2;
00566 m_valid = true;
00567
00568 return *this;
00569 }
00570
00571 }
00572
00573 #endif // WFMATH_ROTMATRIX_FUNCS_H