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), m_age(1)
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
inline 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 m_age = m.m_age;
00055
00056
return *
this;
00057 }
00058
00059
template<const
int dim>
00060
inline bool RotMatrix<dim>::isEqualTo(
const RotMatrix<dim>& m,
double epsilon)
const
00061
{
00062
00063
00064
00065
00066
00067
00068 assert(epsilon > 0);
00069
00070
for(
int i = 0; i < dim; ++i)
00071
for(
int j = 0; j < dim; ++j)
00072
if(fabs(m_elem[i][j] - m.m_elem[i][j]) > epsilon)
00073
return false;
00074
00075
00076
00077 assert(
"Generated values, must be equal if all components are equal" &&
00078 m_flip == m.m_flip);
00079
00080
return true;
00081 }
00082
00083
template<const
int dim>
00084 inline RotMatrix<dim> Prod(
const RotMatrix<dim>& m1,
const RotMatrix<dim>& m2)
00085 {
00086
RotMatrix<dim> out;
00087
00088
for(
int i = 0; i < dim; ++i) {
00089
for(
int j = 0; j < dim; ++j) {
00090 out.
m_elem[i][j] = 0;
00091
for(
int k = 0; k < dim; ++k) {
00092 out.
m_elem[i][j] += m1.
m_elem[i][k] * m2.
m_elem[k][j];
00093 }
00094 }
00095 }
00096
00097 out.
m_flip = (m1.
m_flip != m2.
m_flip);
00098 out.
m_valid = m1.
m_valid && m2.
m_valid;
00099 out.
m_age = m1.
m_age + m2.
m_age;
00100 out.
checkNormalization();
00101
00102
return out;
00103 }
00104
00105
template<const
int dim>
00106 inline RotMatrix<dim> ProdInv(
const RotMatrix<dim>& m1,
const RotMatrix<dim>& m2)
00107 {
00108
RotMatrix<dim> out;
00109
00110
for(
int i = 0; i < dim; ++i) {
00111
for(
int j = 0; j < dim; ++j) {
00112 out.
m_elem[i][j] = 0;
00113
for(
int k = 0; k < dim; ++k) {
00114 out.
m_elem[i][j] += m1.
m_elem[i][k] * m2.
m_elem[j][k];
00115 }
00116 }
00117 }
00118
00119 out.
m_flip = (m1.
m_flip != m2.
m_flip);
00120 out.
m_valid = m1.
m_valid && m2.
m_valid;
00121 out.
m_age = m1.
m_age + m2.
m_age;
00122 out.
checkNormalization();
00123
00124
return out;
00125 }
00126
00127
template<const
int dim>
00128 inline RotMatrix<dim> InvProd(
const RotMatrix<dim>& m1,
const RotMatrix<dim>& m2)
00129 {
00130
RotMatrix<dim> out;
00131
00132
for(
int i = 0; i < dim; ++i) {
00133
for(
int j = 0; j < dim; ++j) {
00134 out.
m_elem[i][j] = 0;
00135
for(
int k = 0; k < dim; ++k) {
00136 out.
m_elem[i][j] += m1.
m_elem[k][i] * m2.
m_elem[k][j];
00137 }
00138 }
00139 }
00140
00141 out.
m_flip = (m1.
m_flip != m2.
m_flip);
00142 out.
m_valid = m1.
m_valid && m2.
m_valid;
00143 out.
m_age = m1.
m_age + m2.
m_age;
00144 out.
checkNormalization();
00145
00146
return out;
00147 }
00148
00149
template<const
int dim>
00150 inline RotMatrix<dim> InvProdInv(
const RotMatrix<dim>& m1,
const RotMatrix<dim>& m2)
00151 {
00152
RotMatrix<dim> out;
00153
00154
for(
int i = 0; i < dim; ++i) {
00155
for(
int j = 0; j < dim; ++j) {
00156 out.
m_elem[i][j] = 0;
00157
for(
int k = 0; k < dim; ++k) {
00158 out.
m_elem[i][j] += m1.
m_elem[k][i] * m2.
m_elem[j][k];
00159 }
00160 }
00161 }
00162
00163 out.
m_flip = (m1.
m_flip != m2.
m_flip);
00164 out.
m_valid = m1.
m_valid && m2.
m_valid;
00165 out.
m_age = m1.
m_age + m2.
m_age;
00166 out.
checkNormalization();
00167
00168
return out;
00169 }
00170
00171
template<const
int dim>
00172 inline Vector<dim> Prod(
const RotMatrix<dim>& m,
const Vector<dim>& v)
00173 {
00174
Vector<dim> out;
00175
00176
for(
int i = 0; i < dim; ++i) {
00177 out.
m_elem[i] = 0;
00178
for(
int j = 0; j < dim; ++j) {
00179 out.
m_elem[i] += m.
m_elem[i][j] * v.
m_elem[j];
00180 }
00181 }
00182
00183 out.
m_valid = m.
m_valid && v.
m_valid;
00184
00185
return out;
00186 }
00187
00188
template<const
int dim>
00189 inline Vector<dim> InvProd(
const RotMatrix<dim>& m,
const Vector<dim>& v)
00190 {
00191
Vector<dim> out;
00192
00193
for(
int i = 0; i < dim; ++i) {
00194 out.
m_elem[i] = 0;
00195
for(
int j = 0; j < dim; ++j) {
00196 out.
m_elem[i] += m.
m_elem[j][i] * v.
m_elem[j];
00197 }
00198 }
00199
00200 out.
m_valid = m.
m_valid && v.
m_valid;
00201
00202
return out;
00203 }
00204
00205
template<const
int dim>
00206 inline Vector<dim> Prod(
const Vector<dim>& v,
const RotMatrix<dim>& m)
00207 {
00208
return InvProd(m, v);
00209 }
00210
00211
template<const
int dim>
00212 inline Vector<dim> ProdInv(
const Vector<dim>& v,
const RotMatrix<dim>& m)
00213 {
00214
return Prod(m, v);
00215 }
00216
00217
template<const
int dim>
00218 inline RotMatrix<dim> operator*(
const RotMatrix<dim>& m1,
const RotMatrix<dim>& m2)
00219 {
00220
return Prod(m1, m2);
00221 }
00222
00223
template<const
int dim>
00224
inline Vector<dim>
operator*(
const RotMatrix<dim>& m,
const Vector<dim>& v)
00225 {
00226
return Prod(m, v);
00227 }
00228
00229
template<const
int dim>
00230
inline Vector<dim>
operator*(
const Vector<dim>& v,
const RotMatrix<dim>& m)
00231 {
00232
return InvProd(m, v);
00233 }
00234
00235
template<const
int dim>
00236 inline bool RotMatrix<dim>::setVals(
const CoordType vals[dim][dim],
double precision)
00237 {
00238
00239
CoordType scratch_vals[dim*dim];
00240
00241
for(
int i = 0; i < dim; ++i)
00242
for(
int j = 0; j < dim; ++j)
00243 scratch_vals[i*dim+j] = vals[i][j];
00244
00245
return _setVals(scratch_vals, precision);
00246 }
00247
00248
template<const
int dim>
00249 inline bool RotMatrix<dim>::setVals(
const CoordType vals[dim*dim],
double precision)
00250 {
00251
00252
CoordType scratch_vals[dim*dim];
00253
00254
for(
int i = 0; i < dim*dim; ++i)
00255 scratch_vals[i] = vals[i];
00256
00257
return _setVals(scratch_vals, precision);
00258 }
00259
00260
bool _MatrixSetValsImpl(
const int size, CoordType* vals,
bool& flip,
00261 CoordType* buf1, CoordType* buf2,
double precision);
00262
00263
template<const
int dim>
00264
inline bool RotMatrix<dim>::_setVals(CoordType *vals,
double precision)
00265 {
00266
00267
00268
CoordType buf1[dim*dim], buf2[dim*dim];
00269
bool flip;
00270
00271
if(!_MatrixSetValsImpl(dim, vals, flip, buf1, buf2, precision))
00272
return false;
00273
00274
00275
00276
for(
int i = 0; i < dim; ++i)
00277
for(
int j = 0; j < dim; ++j)
00278 m_elem[i][j] = vals[i*dim+j];
00279
00280 m_flip = flip;
00281 m_valid =
true;
00282 m_age = 1;
00283
00284
return true;
00285 }
00286
00287
template<const
int dim>
00288 inline Vector<dim> RotMatrix<dim>::row(
const int i)
const
00289
{
00290
Vector<dim> out;
00291
00292
for(
int j = 0; j < dim; ++j)
00293 out[j] = m_elem[i][j];
00294
00295 out.
setValid(m_valid);
00296
00297
return out;
00298 }
00299
00300
template<const
int dim>
00301 inline Vector<dim> RotMatrix<dim>::column(
const int i)
const
00302
{
00303
Vector<dim> out;
00304
00305
for(
int j = 0; j < dim; ++j)
00306 out[j] = m_elem[j][i];
00307
00308 out.
setValid(m_valid);
00309
00310
return out;
00311 }
00312
00313
template<const
int dim>
00314 inline RotMatrix<dim> RotMatrix<dim>::inverse()
const
00315
{
00316
RotMatrix<dim> m;
00317
00318
for(
int i = 0; i < dim; ++i)
00319
for(
int j = 0; j < dim; ++j)
00320 m.
m_elem[j][i] = m_elem[i][j];
00321
00322 m.
m_flip = m_flip;
00323 m.
m_valid = m_valid;
00324 m.
m_age = m_age + 1;
00325
00326
return m;
00327 }
00328
00329
template<const
int dim>
00330 inline RotMatrix<dim>&
RotMatrix<dim>::identity()
00331 {
00332
for(
int i = 0; i < dim; ++i)
00333
for(
int j = 0; j < dim; ++j)
00334 m_elem[i][j] = (
CoordType) ((i == j) ? 1 : 0);
00335
00336 m_flip =
false;
00337 m_valid =
true;
00338 m_age = 0;
00339
00340
return *
this;
00341 }
00342
00343
template<const
int dim>
00344 inline CoordType RotMatrix<dim>::trace()
const
00345
{
00346
CoordType out = 0;
00347
00348
for(
int i = 0; i < dim; ++i)
00349 out += m_elem[i][i];
00350
00351
return out;
00352 }
00353
00354
template<const
int dim>
00355 RotMatrix<dim>&
RotMatrix<dim>::rotation (
const int i,
const int j,
00356 CoordType theta)
00357 {
00358 assert(i >= 0 && i < dim && j >= 0 && j < dim && i != j);
00359
00360
CoordType ctheta = cos(theta), stheta = sin(theta);
00361
00362
for(
int k = 0; k < dim; ++k) {
00363
for(
int l = 0; l < dim; ++l) {
00364
if(k == l) {
00365
if(k == i || k == j)
00366 m_elem[k][l] = ctheta;
00367
else
00368 m_elem[k][l] = 1;
00369 }
00370
else {
00371
if(k == i && l == j)
00372 m_elem[k][l] = stheta;
00373
else if(k == j && l == i)
00374 m_elem[k][l] = -stheta;
00375
else
00376 m_elem[k][l] = 0;
00377 }
00378 }
00379 }
00380
00381 m_flip =
false;
00382 m_valid =
true;
00383 m_age = 1;
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 m_age = 1;
00428
00429
return *
this;
00430 }
00431
00432
template<const
int dim>
00433 RotMatrix<dim>& RotMatrix<dim>::rotation(
const Vector<dim>& from,
00434
const Vector<dim>& to)
00435 {
00436
00437
00438
00439
CoordType fromSqrMag = from.
sqrMag();
00440 assert(
"need nonzero length vector" && fromSqrMag > 0);
00441
CoordType toSqrMag = to.
sqrMag();
00442 assert(
"need nonzero length vector" && toSqrMag > 0);
00443
CoordType dot = Dot(from, to);
00444
CoordType sqrmagprod = fromSqrMag * toSqrMag;
00445
CoordType magprod = sqrt(sqrmagprod);
00446
CoordType ctheta_plus_1 = dot / magprod + 1;
00447
00448
if(ctheta_plus_1 < WFMATH_EPSILON) {
00449
00450
if(dim == 2) {
00451 m_elem[0][0] = m_elem[1][1] = ctheta_plus_1 - 1;
00452
CoordType sin_theta = sqrt(2 * ctheta_plus_1);
00453
bool direction = ((to[0] * from[1] - to[1] * from[0]) >= 0);
00454 m_elem[0][1] = direction ? sin_theta : -sin_theta;
00455 m_elem[1][0] = -m_elem[0][1];
00456 m_flip =
false;
00457 m_valid =
true;
00458 m_age = 1;
00459
return *
this;
00460 }
00461
throw ColinearVectors<dim>(from, to);
00462 }
00463
00464
for(
int i = 0; i < dim; ++i) {
00465
for(
int j = i; j < dim; ++j) {
00466
CoordType projfrom = from[i] * from[j] / fromSqrMag;
00467
CoordType projto = to[i] * to[j] / toSqrMag;
00468
00469
CoordType ijprod = from[i] * to[j], jiprod = to[i] * from[j];
00470
00471
CoordType termthree = (ijprod + jiprod) * dot / sqrmagprod;
00472
00473
CoordType combined = (projfrom + projto - termthree) / ctheta_plus_1;
00474
00475
if(i == j) {
00476 m_elem[i][i] = 1 - combined;
00477 }
00478
else {
00479
CoordType diffterm = (jiprod - ijprod) / magprod;
00480
00481 m_elem[i][j] = -diffterm - combined;
00482 m_elem[j][i] = diffterm - combined;
00483 }
00484 }
00485 }
00486
00487 m_flip =
false;
00488 m_valid =
true;
00489 m_age = 1;
00490
00491
return *
this;
00492 }
00493
00494
#ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
00495
template<>
RotMatrix<3>&
RotMatrix<3>::rotation (
const Vector<3>& axis,
00496 CoordType theta);
00497
template<>
RotMatrix<3>& RotMatrix<3>::rotation (
const Vector<3>& axis);
00498
template<>
RotMatrix<3>&
RotMatrix<3>::fromQuaternion(
const Quaternion& q,
00499
const bool not_flip);
00500
#else
00501
void _NCFS_RotMatrix3_rotation (
RotMatrix<3>& m,
const Vector<3>& axis, CoordType theta);
00502
void _NCFS_RotMatrix3_rotation (
RotMatrix<3>& m,
const Vector<3>& axis);
00503
void _NCFS_RotMatrix3_fromQuaternion(
RotMatrix<3>& m,
const Quaternion& q,
00504
const bool not_flip, CoordType m_elem[3][3],
00505
bool& m_flip);
00506
00507
template<>
00508
inline RotMatrix<3>& RotMatrix<3>::rotation (
const Vector<3>& axis, CoordType theta)
00509 {
00510 _NCFS_RotMatrix3_rotation(*
this, axis, theta);
00511
return *
this;
00512 }
00513
00514
template<>
00515
inline RotMatrix<3>& RotMatrix<3>::rotation (
const Vector<3>& axis)
00516 {
00517 _NCFS_RotMatrix3_rotation(*
this, axis);
00518
return *
this;
00519 }
00520
00521
template<>
00522
inline RotMatrix<3>& RotMatrix<3>::fromQuaternion(
const Quaternion& q,
00523
const bool not_flip)
00524 {
00525 _NCFS_RotMatrix3_fromQuaternion(*
this, q, not_flip, m_elem, m_flip);
00526 m_valid =
true;
00527
return *
this;
00528 }
00529
00530
#endif
00531
00532
template<> RotMatrix<3>& RotMatrix<3>::rotate(
const Quaternion&);
00533
00534
template<const
int dim>
00535 inline RotMatrix<dim>&
RotMatrix<dim>::mirror(
const int i)
00536 {
00537 assert(i >= 0 && i < dim);
00538
00539
identity();
00540 m_elem[i][i] = -1;
00541 m_flip =
true;
00542
00543
00544
return *
this;
00545 }
00546
00547
template<const
int dim>
00548 inline RotMatrix<dim>& RotMatrix<dim>::mirror (
const Vector<dim>& v)
00549 {
00550
00551
00552
00553
00554
00555
00556
CoordType sqr_mag = v.
sqrMag();
00557
00558 assert((
"need nonzero length vector", sqr_mag > 0));
00559
00560
00561
for(
int i = 0; i < dim - 1; ++i)
00562
for(
int j = i + 1; j < dim; ++j)
00563 m_elem[i][j] = m_elem[j][i] = -2 * v[i] * v[j] / sqr_mag;
00564
00565
00566
for(
int i = 0; i < dim; ++i)
00567 m_elem[i][i] = 1 - 2 * v[i] * v[i] / sqr_mag;
00568
00569 m_flip =
true;
00570 m_valid =
true;
00571 m_age = 1;
00572
00573
return *
this;
00574 }
00575
00576
template<const
int dim>
00577 inline RotMatrix<dim>& RotMatrix<dim>::mirror()
00578 {
00579
for(
int i = 0; i < dim; ++i)
00580
for(
int j = 0; j < dim; ++j)
00581 m_elem[i][j] = (i == j) ? -1 : 0;
00582
00583 m_flip = dim%2;
00584 m_valid =
true;
00585 m_age = 0;
00586
00587
00588
return *
this;
00589 }
00590
00591
bool _MatrixInverseImpl(
const int size, CoordType* in, CoordType* out);
00592
00593
template<const
int dim>
00594 inline void RotMatrix<dim>::normalize()
00595 {
00596
00597
00598
00599
CoordType buf1[dim*dim], buf2[dim*dim];
00600
00601
for(
int i = 0; i < dim; ++i) {
00602
for(
int j = 0; j < dim; ++j) {
00603 buf1[j*dim + i] = m_elem[i][j];
00604 buf2[j*dim + i] = (
CoordType)((i == j) ? 1 : 0);
00605 }
00606 }
00607
00608
bool success = _MatrixInverseImpl(dim, buf1, buf2);
00609 assert(success);
00610
00611
for(
int i = 0; i < dim; ++i) {
00612
for(
int j = 0; j < dim; ++j) {
00613
CoordType&
elem = m_elem[i][j];
00614 elem += buf2[i*dim + j];
00615 elem /= 2;
00616 }
00617 }
00618
00619 m_age = 1;
00620 }
00621
00622 }
00623
00624
#endif // WFMATH_ROTMATRIX_FUNCS_H