58 const double uZ = m[
kYX] - m[
kXY];
59 const double uY = m[
kXZ] - m[
kZX];
60 const double uX = m[
kZY] - m[
kYZ];
78 static const double pi =
M_PI;
81 const double cosdelta = (m[
kXX] + m[
kYY] + m[
kZZ] - 1.0) / 2.0;
84 }
else if (cosdelta < -1.0) {
98 static const double pi =
M_PI;
121 double psiPlusPhi, psiMinusPhi;
122 static const double pi =
M_PI;
123 static const double pi_2 =
M_PI_2;
126 (r[
kZZ] > 0.0) ? 0 :
pi;
128 double cosTheta = r[
kZZ];
129 if (cosTheta > 1) cosTheta = 1;
130 if (cosTheta < -1) cosTheta = -1;
139 }
else if (cosTheta >= 0) {
141 double s = -r[
kXY] - r[
kYX];
143 psiMinusPhi =
atan2 ( s, c );
144 }
else if (cosTheta > -1) {
146 double s = r[
kXY] - r[
kYX];
148 psiPlusPhi =
atan2 ( s, c );
154 psi = .5 * (psiPlusPhi + psiMinusPhi);
155 phi = .5 * (psiPlusPhi - psiMinusPhi);
163 w[0] = r[
kXZ]; w[1] = r[
kZX]; w[2] = r[
kYZ]; w[3] = -r[
kZY];
168 for (
int i = 1; i < 4; ++i) {
207 const double d0 = m[
kXX] + m[
kYY] + m[
kZZ];
208 const double d1 = + m[
kXX] - m[
kYY] - m[
kZZ];
209 const double d2 = - m[
kXX] + m[
kYY] - m[
kZZ];
210 const double d3 = - m[
kXX] - m[
kYY] + m[
kZZ];
215 if ( d0 >= d1 && d0 >= d2 && d0 >= d3 ) {
217 const double f = .25/q0;
218 const double q1 = f*(m[
kZY]-m[
kYZ]);
219 const double q2 = f*(m[
kXZ]-m[
kZX]);
220 const double q3 = f*(m[
kYX]-m[
kXY]);
224 }
else if ( d1 >= d2 && d1 >= d3 ) {
226 const double f = .25/q1;
227 const double q0 = f*(m[
kZY]-m[
kYZ]);
228 const double q2 = f*(m[
kXY]+m[
kYX]);
229 const double q3 = f*(m[
kXZ]+m[
kZX]);
233 }
else if ( d2 >= d3 ) {
235 const double f = .25/q2;
236 const double q0 = f*(m[
kXZ]-m[
kZX]);
237 const double q1 = f*(m[
kXY]+m[
kYX]);
238 const double q3 = f*(m[
kYZ]+m[
kZY]);
244 const double f = .25/q3;
245 const double q0 = f*(m[
kYX]-m[
kXY]);
246 const double q1 = f*(m[
kXZ]+m[
kZX]);
247 const double q2 = f*(m[
kYZ]+m[
kZY]);
265 static const double pi_2 =
M_PI_2;
273 double sinTheta = r[
kXZ];
274 if ( sinTheta < -1.0) sinTheta = -1.0;
275 if ( sinTheta > 1.0) sinTheta = 1.0;
285 double psiPlusPhi = 0;
286 double psiMinusPhi = 0;
289 if (sinTheta > - 1.0)
294 psiMinusPhi =
atan2 ( r[kZY] - r[
kYX] , r[
kYY] + r[kZX] );
296 psi = .5 * (psiPlusPhi + psiMinusPhi);
297 phi = .5 * (psiPlusPhi - psiMinusPhi);
315 w[0] = -r[
kYZ]; w[1] = -r[
kXY]; w[2] = r[
kZZ]; w[3] = r[
kXX];
320 for (
int i = 1; i < 4; ++i) {
361 const double oneMinusCosDelta = 1.0 - cosDelta;
364 const double uX = u.
X();
365 const double uY = u.
Y();
366 const double uZ = u.
Z();
370 m[
kXX] = oneMinusCosDelta * uX * uX + cosDelta;
371 m[
kXY] = oneMinusCosDelta * uX * uY - sinDelta * uZ;
372 m[
kXZ] = oneMinusCosDelta * uX * uZ + sinDelta * uY;
374 m[
kYX] = oneMinusCosDelta * uY * uX + sinDelta * uZ;
375 m[
kYY] = oneMinusCosDelta * uY * uY + cosDelta;
376 m[
kYZ] = oneMinusCosDelta * uY * uZ - sinDelta * uX;
378 m[
kZX] = oneMinusCosDelta * uZ * uX - sinDelta * uY;
379 m[
kZY] = oneMinusCosDelta * uZ * uY + sinDelta * uX;
380 m[
kZZ] = oneMinusCosDelta * uZ * uZ + cosDelta;
434 ( cPsi*cPhi-sPsi*cTheta*sPhi, cPsi*sPhi+sPsi*cTheta*cPhi, sPsi*sTheta
435 , -sPsi*cPhi-cPsi*cTheta*sPhi, -sPsi*sPhi+cPsi*cTheta*cPhi, cPsi*sTheta
436 , sTheta*sPhi, -sTheta*cPhi, cTheta
454 const Scalar plus = (from.
Phi()+from.
Psi())/2;
455 const Scalar minus = (from.
Phi()-from.
Psi())/2;
456 const Scalar sPlus =
std::sin( plus );
457 const Scalar cPlus =
std::cos( plus );
458 const Scalar sMinus =
std::sin( minus );
459 const Scalar cMinus =
std::cos( minus );
463 to.
SetComponents ( cTheta*cPlus, -sTheta*cMinus, -sTheta*sMinus, -cTheta*sPlus );
484 const double q0 = from.
U();
485 const double q1 = from.
I();
486 const double q2 = from.
J();
487 const double q3 = from.
K();
488 const double q00 = q0*q0;
489 const double q01 = q0*q1;
490 const double q02 = q0*q2;
491 const double q03 = q0*q3;
492 const double q11 = q1*q1;
493 const double q12 = q1*q2;
494 const double q13 = q1*q3;
495 const double q22 = q2*q2;
496 const double q23 = q2*q3;
497 const double q33 = q3*q3;
500 q00+q11-q22-q33 , 2*(q12-q03) , 2*(q02+q13),
501 2*(q12+q03) , q00-q11+q22-q33 , 2*(q23-q01),
502 2*(q13-q02) , 2*(q23+q01) , q00-q11-q22+q33 );
513 const double angle = 2.0 *
std::acos ( from.
U() );
515 axis (from.
I(), from.
J(), from.
K());
518 if ( u < -1 ) u = -1;
519 const double angle = 2.0 *
std::acos ( -from.
U() );
521 axis (-from.
I(), -from.
J(), -from.
K());
591 to.
SetComponents( cphi2 * cpsi2 * ctheta2 - sphi2 * spsi2 * stheta2,
592 sphi2 * cpsi2 * stheta2 + cphi2 * spsi2 * ctheta2,
593 cphi2 * cpsi2 * stheta2 - sphi2 * spsi2 * ctheta2,
594 sphi2 * cpsi2 * ctheta2 + cphi2 * spsi2 * stheta2
DisplacementVector3D< CoordSystem, Tag > & SetCoordinates(const Scalar src[])
Set internal data based on a C-style array of 3 Scalar numbers.
Scalar Phi() const
Return Phi Euler angle.
void SetComponents(IT begin, IT end)
Set the three Euler angles given a pair of pointers or iterators defining the beginning and end of an...
Rotation class representing a 3D rotation about the Z axis by the angle of rotation.
Scalar Y() const
Cartesian Y, converting if necessary from internal coordinate system.
Rotation class with the (3D) rotation represented by angles describing first a rotation of an angle p...
Scalar SinAngle() const
Sine or Cosine of the rotation angle.
AxisVector Axis() const
accesss to rotation axis
Rotation class with the (3D) rotation represented by a unit quaternion (u, i, j, k).
void GetComponents(IT begin, IT end) const
Get the axis and then the angle into data specified by an iterator begin and another to the end of th...
AxisAngle class describing rotation represented with direction axis (3D Vector) and an angle of rotat...
Scalar Angle() const
Angle of rotation.
Rotation class representing a 3D rotation about the Y axis by the angle of rotation.
void SetComponents(IT begin, IT end)
Set the axis and then the angle given a pair of pointers or iterators defining the beginning and end ...
Scalar X() const
Cartesian X, converting if necessary from internal coordinate system.
Scalar SinAngle() const
Sine or Cosine of the rotation angle.
Scalar Angle() const
access to rotation angle
void Rectify()
Re-adjust components to eliminate small deviations from the axis being a unit vector and angles out o...
VecExpr< UnaryOp< Fabs< T >, VecExpr< A, T, D >, T >, T, D > fabs(const VecExpr< A, T, D > &rhs)
Rotation class representing a 3D rotation about the X axis by the angle of rotation.
Rotation class with the (3D) rotation represented by a 3x3 orthogonal matrix.
static void correctByPi(double &psi, double &phi)
Scalar Angle() const
Angle of rotation.
void GetComponents(ForeignVector &v1, ForeignVector &v2, ForeignVector &v3) const
Get components into three vectors which will be the (orthonormal) columns of the rotation matrix...
Scalar U() const
Access to the four quaternion components: U() is the coefficient of the identity Pauli matrix...
Scalar Theta() const
Return Theta Euler angle.
void convert(R1 const &, R2 const)
Scalar Z() const
Cartesian Z, converting if necessary from internal coordinate system.
double atan2(double, double)
EulerAngles class describing rotation as three angles (Euler Angles).
Scalar SinAngle() const
Sine or Cosine of the rotation angle.
void SetComponents(const ForeignVector &v1, const ForeignVector &v2, const ForeignVector &v3)
Set components from three orthonormal vectors (which must have methods x(), y() and z()) which will b...
void Rectify()
Re-adjust components to eliminate small deviations from |Q| = 1 orthonormality.
void SetComponents(IT begin, IT end)
Set the three Euler angles given a pair of pointers or iterators defining the beginning and end of an...
void SetComponents(IT begin, IT end)
Set the four components given an iterator to the start of the desired data, and another to the end (4...
Scalar Angle() const
Angle of rotation.
Scalar Psi() const
Return Psi Euler angle.