// Geometric Tools, LLC // Copyright (c) 1998-2014 // Distributed under the Boost Software License, Version 1.0. // http://www.boost.org/LICENSE_1_0.txt // http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt // // File Version: 5.0.1 (2011/03/27) //---------------------------------------------------------------------------- template Quaternion::Quaternion () { // Uninitialized for performance in array construction. } //---------------------------------------------------------------------------- template Quaternion::Quaternion (Real w, Real x, Real y, Real z) { mTuple[0] = w; mTuple[1] = x; mTuple[2] = y; mTuple[3] = z; } //---------------------------------------------------------------------------- template Quaternion::Quaternion (const Quaternion& q) { mTuple[0] = q.mTuple[0]; mTuple[1] = q.mTuple[1]; mTuple[2] = q.mTuple[2]; mTuple[3] = q.mTuple[3]; } //---------------------------------------------------------------------------- template Quaternion::Quaternion (const Matrix3& rot) { FromRotationMatrix(rot); } //---------------------------------------------------------------------------- template Quaternion::Quaternion (const Vector3& axis, Real angle) { FromAxisAngle(axis, angle); } //---------------------------------------------------------------------------- template Quaternion::Quaternion (const Vector3 rotColumn[3]) { FromRotationMatrix(rotColumn); } //---------------------------------------------------------------------------- template inline Quaternion::operator const Real* () const { return mTuple; } //---------------------------------------------------------------------------- template inline Quaternion::operator Real* () { return mTuple; } //---------------------------------------------------------------------------- template inline const Real& Quaternion::operator[] (int i) const { return mTuple[i]; } //---------------------------------------------------------------------------- template inline Real& Quaternion::operator[] (int i) { return mTuple[i]; } //---------------------------------------------------------------------------- template inline Real Quaternion::W () const { return mTuple[0]; } //---------------------------------------------------------------------------- template inline Real& Quaternion::W () { return mTuple[0]; } //---------------------------------------------------------------------------- template inline Real Quaternion::X () const { return mTuple[1]; } //---------------------------------------------------------------------------- template inline Real& Quaternion::X () { return mTuple[1]; } //---------------------------------------------------------------------------- template inline Real Quaternion::Y () const { return mTuple[2]; } //---------------------------------------------------------------------------- template inline Real& Quaternion::Y () { return mTuple[2]; } //---------------------------------------------------------------------------- template inline Real Quaternion::Z () const { return mTuple[3]; } //---------------------------------------------------------------------------- template inline Real& Quaternion::Z () { return mTuple[3]; } //---------------------------------------------------------------------------- template inline Quaternion& Quaternion::operator= (const Quaternion& q) { mTuple[0] = q.mTuple[0]; mTuple[1] = q.mTuple[1]; mTuple[2] = q.mTuple[2]; mTuple[3] = q.mTuple[3]; return *this; } //---------------------------------------------------------------------------- template inline bool Quaternion::operator== (const Quaternion& q) const { return memcmp(mTuple, q.mTuple, 4*sizeof(Real)) == 0; } //---------------------------------------------------------------------------- template inline bool Quaternion::operator!= (const Quaternion& q) const { return memcmp(mTuple, q.mTuple, 4*sizeof(Real)) != 0; } //---------------------------------------------------------------------------- template inline bool Quaternion::operator< (const Quaternion& q) const { return memcmp(mTuple, q.mTuple, 4*sizeof(Real)) < 0; } //---------------------------------------------------------------------------- template inline bool Quaternion::operator<= (const Quaternion& q) const { return memcmp(mTuple, q.mTuple, 4*sizeof(Real)) <= 0; } //---------------------------------------------------------------------------- template inline bool Quaternion::operator> (const Quaternion& q) const { return memcmp(mTuple, q.mTuple, 4*sizeof(Real)) > 0; } //---------------------------------------------------------------------------- template inline bool Quaternion::operator>= (const Quaternion& q) const { return memcmp(mTuple, q.mTuple, 4*sizeof(Real)) >= 0; } //---------------------------------------------------------------------------- template inline Quaternion Quaternion::operator+ (const Quaternion& q) const { Quaternion result; for (int i = 0; i < 4; ++i) { result.mTuple[i] = mTuple[i] + q.mTuple[i]; } return result; } //---------------------------------------------------------------------------- template inline Quaternion Quaternion::operator- (const Quaternion& q) const { Quaternion result; for (int i = 0; i < 4; ++i) { result.mTuple[i] = mTuple[i] - q.mTuple[i]; } return result; } //---------------------------------------------------------------------------- template inline Quaternion Quaternion::operator* (const Quaternion& q) const { // NOTE: Multiplication is not generally commutative, so in most // cases p*q != q*p. Quaternion result; result.mTuple[0] = mTuple[0]*q.mTuple[0] - mTuple[1]*q.mTuple[1] - mTuple[2]*q.mTuple[2] - mTuple[3]*q.mTuple[3]; result.mTuple[1] = mTuple[0]*q.mTuple[1] + mTuple[1]*q.mTuple[0] + mTuple[2]*q.mTuple[3] - mTuple[3]*q.mTuple[2]; result.mTuple[2] = mTuple[0]*q.mTuple[2] + mTuple[2]*q.mTuple[0] + mTuple[3]*q.mTuple[1] - mTuple[1]*q.mTuple[3]; result.mTuple[3] = mTuple[0]*q.mTuple[3] + mTuple[3]*q.mTuple[0] + mTuple[1]*q.mTuple[2] - mTuple[2]*q.mTuple[1]; return result; } //---------------------------------------------------------------------------- template inline Quaternion Quaternion::operator* (Real scalar) const { Quaternion result; for (int i = 0; i < 4; ++i) { result.mTuple[i] = scalar*mTuple[i]; } return result; } //---------------------------------------------------------------------------- template inline Quaternion Quaternion::operator/ (Real scalar) const { Quaternion result; int i; if (scalar != (Real)0) { Real invScalar = ((Real)1)/scalar; for (i = 0; i < 4; ++i) { result.mTuple[i] = invScalar*mTuple[i]; } } else { for (i = 0; i < 4; ++i) { result.mTuple[i] = Math::MAX_REAL; } } return result; } //---------------------------------------------------------------------------- template inline Quaternion Quaternion::operator- () const { Quaternion result; for (int i = 0; i < 4; ++i) { result.mTuple[i] = -mTuple[i]; } return result; } //---------------------------------------------------------------------------- template inline Quaternion& Quaternion::operator+= (const Quaternion& q) { for (int i = 0; i < 4; ++i) { mTuple[i] += q.mTuple[i]; } return *this; } //---------------------------------------------------------------------------- template inline Quaternion& Quaternion::operator-= (const Quaternion& q) { for (int i = 0; i < 4; ++i) { mTuple[i] -= q.mTuple[i]; } return *this; } //---------------------------------------------------------------------------- template inline Quaternion& Quaternion::operator*= (Real scalar) { for (int i = 0; i < 4; ++i) { mTuple[i] *= scalar; } return *this; } //---------------------------------------------------------------------------- template inline Quaternion& Quaternion::operator/= (Real scalar) { int i; if (scalar != (Real)0) { Real invScalar = ((Real)1)/scalar; for (i = 0; i < 4; ++i) { mTuple[i] *= invScalar; } } else { for (i = 0; i < 4; ++i) { mTuple[i] = Math::MAX_REAL; } } return *this; } //---------------------------------------------------------------------------- template void Quaternion::FromRotationMatrix (const Matrix3& rot) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternion Calculus and Fast Animation". const int next[3] = { 1, 2, 0 }; Real trace = rot(0,0) + rot(1,1) + rot(2,2); Real root; if (trace > (Real)0) { // |w| > 1/2, may as well choose w > 1/2 root = Math::Sqrt(trace + (Real)1); // 2w mTuple[0] = ((Real)0.5)*root; root = ((Real)0.5)/root; // 1/(4w) mTuple[1] = (rot(2,1) - rot(1,2))*root; mTuple[2] = (rot(0,2) - rot(2,0))*root; mTuple[3] = (rot(1,0) - rot(0,1))*root; } else { // |w| <= 1/2 int i = 0; if (rot(1,1) > rot(0,0)) { i = 1; } if (rot(2,2) > rot(i,i)) { i = 2; } int j = next[i]; int k = next[j]; root = Math::Sqrt(rot(i,i) - rot(j,j) - rot(k,k) + (Real)1); Real* quat[3] = { &mTuple[1], &mTuple[2], &mTuple[3] }; *quat[i] = ((Real)0.5)*root; root = ((Real)0.5)/root; mTuple[0] = (rot(k,j) - rot(j,k))*root; *quat[j] = (rot(j,i) + rot(i,j))*root; *quat[k] = (rot(k,i) + rot(i,k))*root; } } //---------------------------------------------------------------------------- template void Quaternion::ToRotationMatrix (Matrix3& rot) const { Real twoX = ((Real)2)*mTuple[1]; Real twoY = ((Real)2)*mTuple[2]; Real twoZ = ((Real)2)*mTuple[3]; Real twoWX = twoX*mTuple[0]; Real twoWY = twoY*mTuple[0]; Real twoWZ = twoZ*mTuple[0]; Real twoXX = twoX*mTuple[1]; Real twoXY = twoY*mTuple[1]; Real twoXZ = twoZ*mTuple[1]; Real twoYY = twoY*mTuple[2]; Real twoYZ = twoZ*mTuple[2]; Real twoZZ = twoZ*mTuple[3]; rot(0,0) = (Real)1 - (twoYY + twoZZ); rot(0,1) = twoXY - twoWZ; rot(0,2) = twoXZ + twoWY; rot(1,0) = twoXY + twoWZ; rot(1,1) = (Real)1 - (twoXX + twoZZ); rot(1,2) = twoYZ - twoWX; rot(2,0) = twoXZ - twoWY; rot(2,1) = twoYZ + twoWX; rot(2,2) = (Real)1 - (twoXX + twoYY); } //---------------------------------------------------------------------------- template void Quaternion::FromRotationMatrix (const Vector3 rotColumn[3]) { Matrix3 kRot; for (int col = 0; col < 3; ++col) { kRot(0,col) = rotColumn[col][0]; kRot(1,col) = rotColumn[col][1]; kRot(2,col) = rotColumn[col][2]; } FromRotationMatrix(kRot); } //---------------------------------------------------------------------------- template void Quaternion::ToRotationMatrix (Vector3 rotColumn[3]) const { Matrix3 rot; ToRotationMatrix(rot); for (int col = 0; col < 3; ++col) { rotColumn[col][0] = rot(0,col); rotColumn[col][1] = rot(1,col); rotColumn[col][2] = rot(2,col); } } //---------------------------------------------------------------------------- template void Quaternion::FromAxisAngle (const Vector3& axis, Real angle) { // assert: axis[] is unit length // // The quaternion representing the rotation is // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k) Real halfAngle = ((Real)0.5)*angle; Real sn = Math::Sin(halfAngle); mTuple[0] = Math::Cos(halfAngle); mTuple[1] = sn*axis[0]; mTuple[2] = sn*axis[1]; mTuple[3] = sn*axis[2]; } //---------------------------------------------------------------------------- template void Quaternion::ToAxisAngle (Vector3& axis, Real& angle) const { // The quaternion representing the rotation is // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k) Real sqrLength = mTuple[1]*mTuple[1] + mTuple[2]*mTuple[2] + mTuple[3]*mTuple[3]; if (sqrLength > Math::ZERO_TOLERANCE) { angle = ((Real)2)*Math::ACos(mTuple[0]); Real invLength = Math::InvSqrt(sqrLength); axis[0] = mTuple[1]*invLength; axis[1] = mTuple[2]*invLength; axis[2] = mTuple[3]*invLength; } else { // Angle is 0 (mod 2*pi), so any axis will do. angle = (Real)0; axis[0] = (Real)1; axis[1] = (Real)0; axis[2] = (Real)0; } } //---------------------------------------------------------------------------- template inline Real Quaternion::Length () const { return Math::Sqrt(mTuple[0]*mTuple[0] + mTuple[1]*mTuple[1] + mTuple[2]*mTuple[2] + mTuple[3]*mTuple[3]); } //---------------------------------------------------------------------------- template inline Real Quaternion::SquaredLength () const { return mTuple[0]*mTuple[0] + mTuple[1]*mTuple[1] + mTuple[2]*mTuple[2] + mTuple[3]*mTuple[3]; } //---------------------------------------------------------------------------- template inline Real Quaternion::Dot (const Quaternion& q) const { return mTuple[0]*q.mTuple[0] + mTuple[1]*q.mTuple[1] + mTuple[2]*q.mTuple[2] + mTuple[3]*q.mTuple[3]; } //---------------------------------------------------------------------------- template inline Real Quaternion::Normalize (Real epsilon) { Real length = Length(); if (length > epsilon) { Real invLength = ((Real)1)/length; mTuple[0] *= invLength; mTuple[1] *= invLength; mTuple[2] *= invLength; mTuple[3] *= invLength; } else { length = (Real)0; mTuple[0] = (Real)0; mTuple[1] = (Real)0; mTuple[2] = (Real)0; mTuple[3] = (Real)0; } return length; } //---------------------------------------------------------------------------- template Quaternion Quaternion::Inverse () const { Quaternion inverse; Real norm = SquaredLength(); if (norm > (Real)0) { Real invNorm = ((Real)1)/norm; inverse.mTuple[0] = mTuple[0]*invNorm; inverse.mTuple[1] = -mTuple[1]*invNorm; inverse.mTuple[2] = -mTuple[2]*invNorm; inverse.mTuple[3] = -mTuple[3]*invNorm; } else { // Return an invalid result to flag the error. for (int i = 0; i < 4; ++i) { inverse.mTuple[i] = (Real)0; } } return inverse; } //---------------------------------------------------------------------------- template Quaternion Quaternion::Conjugate () const { return Quaternion(mTuple[0], -mTuple[1], -mTuple[2], -mTuple[3]); } //---------------------------------------------------------------------------- template Quaternion Quaternion::Exp () const { // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero, // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1. Quaternion result; Real angle = Math::Sqrt(mTuple[1]*mTuple[1] + mTuple[2]*mTuple[2] + mTuple[3]*mTuple[3]); Real sn = Math::Sin(angle); result.mTuple[0] = Math::Cos(angle); int i; if (Math::FAbs(sn) >= Math::ZERO_TOLERANCE) { Real coeff = sn/angle; for (i = 1; i < 4; ++i) { result.mTuple[i] = coeff*mTuple[i]; } } else { for (i = 1; i < 4; ++i) { result.mTuple[i] = mTuple[i]; } } return result; } //---------------------------------------------------------------------------- template Quaternion Quaternion::Log () const { // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then // log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) = // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1. Quaternion result; result.mTuple[0] = (Real)0; int i; if (Math::FAbs(mTuple[0]) < (Real)1) { Real angle = Math::ACos(mTuple[0]); Real sn = Math::Sin(angle); if (Math::FAbs(sn) >= Math::ZERO_TOLERANCE) { Real coeff = angle/sn; for (i = 1; i < 4; ++i) { result.mTuple[i] = coeff*mTuple[i]; } return result; } } for (i = 1; i < 4; ++i) { result.mTuple[i] = mTuple[i]; } return result; } //---------------------------------------------------------------------------- template Vector3 Quaternion::Rotate (const Vector3& vec) const { // Given a vector u = (x0,y0,z0) and a unit length quaternion // q = , the vector v = (x1,y1,z1) which represents the // rotation of u by q is v = q*u*q^{-1} where * indicates quaternion // multiplication and where u is treated as the quaternion <0,x0,y0,z0>. // Note that q^{-1} = , so no real work is required to // invert q. Now // // q*u*q^{-1} = q*<0,x0,y0,z0>*q^{-1} // = q*(x0*i+y0*j+z0*k)*q^{-1} // = x0*(q*i*q^{-1})+y0*(q*j*q^{-1})+z0*(q*k*q^{-1}) // // As 3-vectors, q*i*q^{-1}, q*j*q^{-1}, and 2*k*q^{-1} are the columns // of the rotation matrix computed in Quaternion::ToRotationMatrix. // The vector v is obtained as the product of that rotation matrix with // vector u. As such, the quaternion representation of a rotation // matrix requires less space than the matrix and more time to compute // the rotated vector. Typical space-time tradeoff... Matrix3 rot; ToRotationMatrix(rot); return rot*vec; } //---------------------------------------------------------------------------- template Quaternion& Quaternion::Slerp (Real t, const Quaternion& p, const Quaternion& q) { Real cs = p.Dot(q); Real angle = Math::ACos(cs); if (Math::FAbs(angle) >= Math::ZERO_TOLERANCE) { Real sn = Math::Sin(angle); Real invSn = ((Real)1)/sn; Real tAngle = t*angle; Real coeff0 = Math::Sin(angle - tAngle)*invSn; Real coeff1 = Math::Sin(tAngle)*invSn; mTuple[0] = coeff0*p.mTuple[0] + coeff1*q.mTuple[0]; mTuple[1] = coeff0*p.mTuple[1] + coeff1*q.mTuple[1]; mTuple[2] = coeff0*p.mTuple[2] + coeff1*q.mTuple[2]; mTuple[3] = coeff0*p.mTuple[3] + coeff1*q.mTuple[3]; } else { mTuple[0] = p.mTuple[0]; mTuple[1] = p.mTuple[1]; mTuple[2] = p.mTuple[2]; mTuple[3] = p.mTuple[3]; } return *this; } //---------------------------------------------------------------------------- template Quaternion& Quaternion::SlerpExtraSpins (Real t, const Quaternion& p, const Quaternion& q, int extraSpins) { Real cs = p.Dot(q); Real angle = Math::ACos(cs); if (Math::FAbs(angle) >= Math::ZERO_TOLERANCE) { Real sn = Math::Sin(angle); Real phase = Math::PI*extraSpins*t; Real invSin = ((Real)1)/sn; Real coeff0 = Math::Sin(((Real)1 - t)*angle - phase)*invSin; Real coeff1 = Math::Sin(t*angle + phase)*invSin; mTuple[0] = coeff0*p.mTuple[0] + coeff1*q.mTuple[0]; mTuple[1] = coeff0*p.mTuple[1] + coeff1*q.mTuple[1]; mTuple[2] = coeff0*p.mTuple[2] + coeff1*q.mTuple[2]; mTuple[3] = coeff0*p.mTuple[3] + coeff1*q.mTuple[3]; } else { mTuple[0] = p.mTuple[0]; mTuple[1] = p.mTuple[1]; mTuple[2] = p.mTuple[2]; mTuple[3] = p.mTuple[3]; } return *this; } //---------------------------------------------------------------------------- template Quaternion& Quaternion::Intermediate (const Quaternion& q0, const Quaternion& q1, const Quaternion& q2) { // assert: Q0, Q1, Q2 all unit-length Quaternion q1Inv = q1.Conjugate(); Quaternion p0 = q1Inv*q0; Quaternion p2 = q1Inv*q2; Quaternion arg = -((Real)0.25)*(p0.Log() + p2.Log()); Quaternion a = q1*arg.Exp(); *this = a; return *this; } //---------------------------------------------------------------------------- template Quaternion& Quaternion::Squad (Real t, const Quaternion& q0, const Quaternion& a0, const Quaternion& a1, const Quaternion& q1) { Real slerpT = ((Real)2)*t*((Real)1 - t); Quaternion slerpP = Slerp(t, q0, q1); Quaternion slerpQ = Slerp(t, a0, a1); return Slerp(slerpT, slerpP, slerpQ); } //---------------------------------------------------------------------------- template Quaternion& Quaternion::Align (const Vector3& v1, const Vector3& v2) { // If V1 and V2 are not parallel, the axis of rotation is the unit-length // vector U = Cross(V1,V2)/Length(Cross(V1,V2)). The angle of rotation, // A, is the angle between V1 and V2. The quaternion for the rotation is // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz). // // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then // compute sin(A/2) and cos(A/2), we reduce the computational costs by // computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) = // Dot(V1,B). // // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but // Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in // which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where // C = Cross(V1,B). // // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0). If V1 = -V2, // then B = 0. This can happen even if V1 is approximately -V2 using // floating point arithmetic, since Vector3::Normalize checks for // closeness to zero and returns the zero vector accordingly. The test // for exactly zero is usually not recommend for floating point // arithmetic, but the implementation of Vector3::Normalize guarantees // the comparison is robust. In this case, the A = pi and any axis // perpendicular to V1 may be used as the rotation axis. Vector3 bisector = v1 + v2; bisector.Normalize(); Real cosHalfAngle = v1.Dot(bisector); Vector3 cross; mTuple[0] = cosHalfAngle; if (cosHalfAngle != (Real)0) { cross = v1.Cross(bisector); mTuple[1] = cross.X(); mTuple[2] = cross.Y(); mTuple[3] = cross.Z(); } else { Real invLength; if (Math::FAbs(v1[0]) >= Math::FAbs(v1[1])) { // V1.x or V1.z is the largest magnitude component. invLength = Math::InvSqrt(v1[0]*v1[0] + v1[2]*v1[2]); mTuple[1] = -v1[2]*invLength; mTuple[2] = (Real)0; mTuple[3] = +v1[0]*invLength; } else { // V1.y or V1.z is the largest magnitude component. invLength = Math::InvSqrt(v1[1]*v1[1] + v1[2]*v1[2]); mTuple[1] = (Real)0; mTuple[2] = +v1[2]*invLength; mTuple[3] = -v1[1]*invLength; } } return *this; } //---------------------------------------------------------------------------- template void Quaternion::DecomposeTwistTimesSwing (const Vector3& v1, Quaternion& twist, Quaternion& swing) { Vector3 v2 = Rotate(v1); swing = Align(v1, v2); twist = (*this)*swing.Conjugate(); } //---------------------------------------------------------------------------- template void Quaternion::DecomposeSwingTimesTwist (const Vector3& v1, Quaternion& swing, Quaternion& twist) { Vector3 v2 = Rotate(v1); swing = Align(v1, v2); twist = swing.Conjugate()*(*this); } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestX () const { return GetClosest(1); } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestY () const { return GetClosest(2); } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestZ () const { return GetClosest(3); } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestXY () const { Quaternion q; Real det = mTuple[0]*mTuple[3] - mTuple[1]*mTuple[2]; if(Math::FAbs(det) < (Real)0.5 - Math::ZERO_TOLERANCE) { Real discr = (Real)1 - ((Real)4)*det*det; discr = Math::Sqrt(Math::FAbs(discr)); Real a = mTuple[0]*mTuple[1] + mTuple[2]*mTuple[3]; Real b = mTuple[0]*mTuple[0] - mTuple[1]*mTuple[1] + mTuple[2]*mTuple[2] - mTuple[3]*mTuple[3]; Real c0, s0, c1, s1, invLength; if (b >= (Real)0) { c0 = ((Real)0.5)*(discr + b); s0 = a; } else { c0 = a; s0 = ((Real)0.5)*(discr - b); } invLength = Math::InvSqrt(c0*c0 + s0*s0); c0 *= invLength; s0 *= invLength; c1 = mTuple[0]*c0 + mTuple[1]*s0; s1 = mTuple[2]*c0 + mTuple[3]*s0; invLength = Math::InvSqrt(c1*c1 + s1*s1); c1 *= invLength; s1 *= invLength; q[0] = c0*c1; q[1] = s0*c1; q[2] = c0*s1; q[3] = s0*s1; } else { Real invLength = Math::InvSqrt(Math::FAbs(det)); q[0] = mTuple[0]*invLength; q[1] = mTuple[1]*invLength; q[2] = (Real)0; q[3] = (Real)0; } return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestYX () const { Quaternion alt(mTuple[0], mTuple[1], mTuple[2], -mTuple[3]); Quaternion q = alt.GetClosestXY(); q[3] = -q[3]; return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestZX () const { Quaternion alt(mTuple[0], mTuple[1], mTuple[3], mTuple[2]); Quaternion q = alt.GetClosestXY(); Real save = q[2]; q[2] = q[3]; q[3] = save; return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestXZ () const { Quaternion alt(mTuple[0], mTuple[1], -mTuple[3], mTuple[2]); Quaternion q = alt.GetClosestXY(); Real save = q[2]; q[2] = q[3]; q[3] = -save; return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestYZ () const { Quaternion alt(mTuple[0], mTuple[2], mTuple[3], mTuple[1]); Quaternion q = alt.GetClosestXY(); Real save = q[3]; q[3] = q[2]; q[2] = q[1]; q[1] = save; return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestZY () const { Quaternion alt(mTuple[0], mTuple[2], mTuple[3], -mTuple[1]); Quaternion q = alt.GetClosestXY(); Real save = q[3]; q[3] = q[2]; q[2] = q[1]; q[1] = -save; return q; } //---------------------------------------------------------------------------- template void Quaternion::FactorXYZ (Real& cx, Real& sx, Real& cy, Real& sy, Real& cz, Real& sz) { Real a = mTuple[0]*mTuple[1] - mTuple[2]*mTuple[3]; Real b = ((Real)0.5)*( mTuple[0]*mTuple[0] - mTuple[1]*mTuple[1] - mTuple[2]*mTuple[2] + mTuple[3]*mTuple[3]); Real fLength = Math::Sqrt(a*a + b*b); if (fLength > Math::ZERO_TOLERANCE) { Real invLength = ((Real)1)/fLength; Real sigma0 = a * invLength; Real gamma0 = b * invLength; if (gamma0 >= (Real)0) { cx = Math::Sqrt(((Real)0.5)*((Real)1 + gamma0)); sx = ((Real)0.5)*sigma0/cx; } else { sx = Math::Sqrt(((Real)0.5)*((Real)1 - gamma0)); cx = ((Real)0.5)*sigma0/sx; } Real tmp0 = cx*mTuple[0] + sx*mTuple[1]; Real tmp1 = cx*mTuple[3] - sx*mTuple[2]; invLength = Math::InvSqrt(tmp0*tmp0 + tmp1*tmp1); cz = tmp0 * invLength; sz = tmp1 * invLength; if(Math::FAbs(cz) >= Math::FAbs(sz)) { invLength = ((Real)1)/cz; cy = tmp0 * invLength; sy = (cx*mTuple[2] + sx*mTuple[3]) * invLength; } else { invLength = ((Real)1)/sz; cy = tmp1 * invLength; sy = (cx*mTuple[1] - sx*mTuple[0]) * invLength; } } else { // Infinitely many solutions. Choose one of them. if(mTuple[0]*mTuple[2] + mTuple[1]*mTuple[3] > (Real)0) { // p = (p0,p1,p0,p1) cx = (Real)1; sx = (Real)0; cy = Math::INV_SQRT_2; sy = Math::INV_SQRT_2; cz = Math::SQRT_2 * mTuple[0]; sz = Math::SQRT_2 * mTuple[1]; } else { // p = (p0,p1,-p0,-p1) cx = (Real)1; sx = (Real)0; cy = Math::INV_SQRT_2; sy = -Math::INV_SQRT_2; cz = Math::SQRT_2 * mTuple[0]; sz = -Math::SQRT_2 * mTuple[1]; } } } //---------------------------------------------------------------------------- template void Quaternion::FactorXZY (Real& cx, Real& sx, Real& cz, Real& sz, Real& cy, Real& sy) { Quaternion alt(mTuple[0], mTuple[1], mTuple[3], -mTuple[2]); alt.FactorXYZ(cx, sx, cz, sz, cy, sy); sy = -sy; } //---------------------------------------------------------------------------- template void Quaternion::FactorYZX (Real& cy, Real& sy, Real& cz, Real& sz, Real& cx, Real& sx) { Quaternion alt(mTuple[0], -mTuple[2], mTuple[3], -mTuple[1]); alt.FactorXYZ(cy, sy, cz, sz, cx, sx); sx = -sx; sy = -sy; } //---------------------------------------------------------------------------- template void Quaternion::FactorYXZ (Real& cy, Real& sy, Real& cx, Real& sx, Real& cz, Real& sz) { Quaternion alt(mTuple[0], -mTuple[2], mTuple[1], mTuple[3]); alt.FactorXYZ(cy, sy, cx, sx, cz, sz); sy = -sy; } //---------------------------------------------------------------------------- template void Quaternion::FactorZXY (Real& cz, Real& sz, Real& cx, Real& sx, Real& cy, Real& sy) { Quaternion alt(mTuple[0], -mTuple[3], mTuple[1], -mTuple[2]); alt.FactorXYZ(cz, sz, cx, sx, cy, sy); sy = -sy; sz = -sz; } //---------------------------------------------------------------------------- template void Quaternion::FactorZYX (Real& cz, Real& sz, Real& cy, Real& sy, Real& cx, Real& sx) { Quaternion alt(mTuple[0], mTuple[3], -mTuple[2], mTuple[1]); alt.FactorXYZ(cz, sz, cy, sy, cx, sx); sy = -sy; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestX (const Constraints& xCon) const { return GetClosest(1, xCon); } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestY (const Constraints& yCon) const { return GetClosest(2, yCon); } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestZ (const Constraints& zCon) const { return GetClosest(3, zCon); } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestXY (const Constraints& xCon, const Constraints& yCon) const { Quaternion q, tmp; Real c0, s0, c1, s1, invLength; Real det = mTuple[0]*mTuple[3] - mTuple[1]*mTuple[2]; if (Math::FAbs(det) < (Real)0.5 - Math::ZERO_TOLERANCE) { Real discr = Math::Sqrt(Math::FAbs((Real)1 - ((Real)4)*det*det)); Real a = mTuple[0]*mTuple[1] + mTuple[2]*mTuple[3]; Real b = mTuple[0]*mTuple[0] - mTuple[1]*mTuple[1] + mTuple[2]*mTuple[2] - mTuple[3]*mTuple[3]; if (b >= (Real)0) { c0 = ((Real)0.5)*(discr + b); s0 = a; } else { c0 = a; s0 = ((Real)0.5)*(discr - b); } invLength = Math::InvSqrt(c0*c0 + s0*s0); c0 *= invLength; s0 *= invLength; c1 = mTuple[0]*c0 + mTuple[1]*s0; s1 = mTuple[2]*c0 + mTuple[3]*s0; invLength = Math::InvSqrt(c1*c1 + s1*s1); c1 *= invLength; s1 *= invLength; if (xCon.IsValid(c0, s0) && yCon.IsValid(c1, s1)) { // The maximum occurs at an interior point. q[0] = c0*c1; q[1] = s0*c1; q[2] = c0*s1; q[3] = s0*s1; } else { // The maximum occurs at a boundary point. Quaternion r(xCon.CosMinAngle, xCon.SinMinAngle, (Real)0, (Real)0); Quaternion rInv(xCon.CosMinAngle, -xCon.SinMinAngle, (Real)0, (Real)0); Quaternion prod = rInv*(*this); tmp = prod.GetClosest(2, yCon); Real dotOptAngle = prod.Dot(tmp); q = r*tmp; r = Quaternion(xCon.CosMaxAngle, xCon.SinMaxAngle, (Real)0, (Real)0); rInv = Quaternion(xCon.CosMaxAngle, -xCon.SinMaxAngle, (Real)0, (Real)0); prod = rInv*(*this); tmp = prod.GetClosest(2, yCon); Real dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = r*tmp; dotOptAngle = dotAngle; } r = Quaternion(yCon.CosMinAngle, (Real)0, yCon.SinMinAngle, (Real)0); rInv = Quaternion(yCon.CosMinAngle, (Real)0, -yCon.SinMinAngle, (Real)0); prod = (*this)*rInv; tmp = prod.GetClosest(1, xCon); dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = tmp*r; dotOptAngle = dotAngle; } r = Quaternion(yCon.CosMaxAngle, (Real)0, yCon.SinMaxAngle, (Real)0); rInv = Quaternion(yCon.CosMaxAngle, (Real)0, -yCon.SinMaxAngle, (Real)0); prod = (*this)*rInv; tmp = prod.GetClosest(1, xCon); dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = tmp*r; dotOptAngle = dotAngle; } } } else { // Infinitely many solutions, choose one that satisfies the angle // constraints. Real minAngle, maxAngle, angle; Constraints con; if (det > (Real)0) { minAngle = xCon.MinAngle - yCon.MaxAngle; maxAngle = xCon.MaxAngle - yCon.MinAngle; con.SetAngles(minAngle, maxAngle); tmp = GetClosest(1, con); angle = Math::ATan2(tmp[1], tmp[0]); if (angle < minAngle || angle > maxAngle) { angle -= (tmp[1] >= (Real)0 ? Math::PI : -Math::PI); // assert(minAngle <= angle && angle <= maxAngle); } if (angle <= xCon.MaxAngle - yCon.MaxAngle) { c1 = yCon.CosMaxAngle; s1 = yCon.SinMaxAngle; angle = yCon.MaxAngle + angle; c0 = Math::Cos(angle); s0 = Math::Sin(angle); } else { c0 = xCon.CosMaxAngle; s0 = xCon.SinMaxAngle; angle = xCon.MaxAngle - angle; c1 = Math::Cos(angle); s1 = Math::Sin(angle); } } else { minAngle = xCon.MinAngle + yCon.MinAngle; maxAngle = xCon.MaxAngle + yCon.MaxAngle; con.SetAngles(minAngle, maxAngle); tmp = GetClosest(1, con); angle = Math::ATan2(tmp[1], tmp[0]); if (angle < minAngle || angle > maxAngle) { angle -= (tmp[1] >= (Real)0 ? Math::PI : -Math::PI); // assert(minAngle <= angle && angle <= maxAngle); } if (angle >= xCon.MinAngle + yCon.MaxAngle) { c1 = yCon.CosMaxAngle; s1 = yCon.SinMaxAngle; angle = angle - yCon.MaxAngle; c0 = Math::Cos(angle); s0 = Math::Sin(angle); } else { c0 = xCon.CosMaxAngle; s0 = xCon.SinMaxAngle; angle = angle - xCon.MaxAngle; c1 = Math::Cos(angle); s1 = Math::Sin(angle); } } q[0] = c0*c1; q[1] = s0*c1; q[2] = c0*s1; q[3] = s0*s1; if (Dot(q) < (Real)0) { q = -q; } } return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestYX (const Constraints& yCon, const Constraints& xCon) const { Quaternion alt(mTuple[0], mTuple[1], mTuple[2], -mTuple[3]); Quaternion q = alt.GetClosestXY(xCon, yCon); q[3] = -q[3]; return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestZX (const Constraints& zCon, const Constraints& xCon) const { Quaternion q, tmp; Real c2, s2, c0, s0, invLength; Real det = mTuple[0]*mTuple[2] - mTuple[1]*mTuple[3]; if (Math::FAbs(det) < (Real)0.5 - Math::ZERO_TOLERANCE) { Real discr = Math::Sqrt(Math::FAbs((Real)1 - ((Real)4)*det*det)); Real a = mTuple[0]*mTuple[3] + mTuple[1]*mTuple[2]; Real b = mTuple[0]*mTuple[0] + mTuple[1]*mTuple[1] - mTuple[2]*mTuple[2] - mTuple[3]*mTuple[3]; if (b >= (Real)0) { c2 = ((Real)0.5)*(discr + b); s2 = a; } else { c2 = a; s2 = ((Real)0.5)*(discr - b); } invLength = Math::InvSqrt(c2*c2 + s2*s2); c2 *= invLength; s2 *= invLength; c0 = mTuple[0]*c2 + mTuple[3]*s2; s0 = mTuple[1]*c2 + mTuple[2]*s2; invLength = Math::InvSqrt(c0*c0 + s0*s0); c0 *= invLength; s0 *= invLength; if (zCon.IsValid(c2, s2) && xCon.IsValid(c0, s0)) { // The maximum occurs at an interior point. q[0] = c2*c0; q[1] = c2*s0; q[2] = s2*s0; q[3] = s2*c0; } else { // The maximum occurs at a boundary point. Quaternion r(zCon.CosMinAngle, (Real)0, (Real)0, zCon.SinMinAngle); Quaternion rInv(zCon.CosMinAngle, (Real)0, (Real)0, -zCon.SinMinAngle); Quaternion prod = rInv*(*this); tmp = prod.GetClosest(1, xCon); Real dotOptAngle = prod.Dot(tmp); q = r*tmp; r = Quaternion(zCon.CosMaxAngle, (Real)0, (Real)0, zCon.SinMaxAngle); rInv = Quaternion(zCon.CosMaxAngle, (Real)0, (Real)0, -zCon.SinMaxAngle); prod = rInv*(*this); tmp = prod.GetClosest(1, xCon); Real dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = r*tmp; dotOptAngle = dotAngle; } r = Quaternion(xCon.CosMinAngle, xCon.SinMinAngle, (Real)0, (Real)0); rInv = Quaternion(xCon.CosMinAngle, -xCon.SinMinAngle, (Real)0, (Real)0); prod = (*this)*rInv; tmp = prod.GetClosest(3, zCon); dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = tmp*r; dotOptAngle = dotAngle; } r = Quaternion(xCon.CosMaxAngle, xCon.SinMaxAngle, (Real)0, (Real)0); rInv = Quaternion(xCon.CosMaxAngle, -xCon.SinMaxAngle, (Real)0, (Real)0); prod = (*this)*rInv; tmp = prod.GetClosest(3, zCon); dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = tmp*r; dotOptAngle = dotAngle; } } } else { // Infinitely many solutions, choose one that satisfies the angle // constraints. Real minAngle, maxAngle, angle; Constraints con; if (det > (Real)0) { minAngle = xCon.MinAngle - zCon.MaxAngle; maxAngle = xCon.MaxAngle - zCon.MinAngle; con.SetAngles(minAngle, maxAngle); tmp = GetClosest(1, con); angle = Math::ATan2(tmp[1], tmp[0]); if (angle < minAngle || angle > maxAngle) { angle -= (tmp[1] >= (Real)0 ? Math::PI : -Math::PI); // assert(minAngle <= angle && angle <= maxAngle); } if (angle <= xCon.MaxAngle - zCon.MaxAngle) { c2 = zCon.CosMaxAngle; s2 = zCon.SinMaxAngle; angle = zCon.MaxAngle + angle; c0 = Math::Cos(angle); s0 = Math::Sin(angle); } else { c0 = xCon.CosMaxAngle; s0 = xCon.SinMaxAngle; angle = xCon.MaxAngle - angle; c2 = Math::Cos(angle); s2 = Math::Sin(angle); } } else { minAngle = xCon.MinAngle + zCon.MinAngle; maxAngle = xCon.MaxAngle + zCon.MaxAngle; con.SetAngles(minAngle, maxAngle); tmp = GetClosest(1, con); angle = Math::ATan2(tmp[1], tmp[0]); if (angle < minAngle || angle > maxAngle) { angle -= (tmp[1] >= (Real)0 ? Math::PI : -Math::PI); // assert(minAngle <= angle && angle <= maxAngle); } if (angle >= xCon.MinAngle + zCon.MaxAngle) { c2 = zCon.CosMaxAngle; s2 = zCon.SinMaxAngle; angle = angle - zCon.MaxAngle; c0 = Math::Cos(angle); s0 = Math::Sin(angle); } else { c0 = xCon.CosMaxAngle; s0 = xCon.SinMaxAngle; angle = angle - xCon.MaxAngle; c2 = Math::Cos(angle); s2 = Math::Sin(angle); } } q[0] = c2*c0; q[1] = c2*s0; q[2] = s2*s0; q[3] = s2*c0; if (Dot(q) < (Real)0) { q = -q; } } return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestXZ (const Constraints& xCon, const Constraints& zCon) const { Quaternion alt(mTuple[0], mTuple[1], -mTuple[2], mTuple[3]); Quaternion q = alt.GetClosestZX(zCon,xCon); q[2] = -q[2]; return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestZY (const Constraints& zCon, const Constraints& yCon) const { Quaternion q, tmp; Real c2, s2, c1, s1, invLength; Real det = mTuple[0]*mTuple[1] + mTuple[2]*mTuple[3]; if (Math::FAbs(det) < (Real)0.5 - Math::ZERO_TOLERANCE) { Real discr = Math::Sqrt(Math::FAbs((Real)1 - ((Real)4)*det*det)); Real a = mTuple[0]*mTuple[3] - mTuple[1]*mTuple[2]; Real b = mTuple[0]*mTuple[0] - mTuple[1]*mTuple[1] + mTuple[2]*mTuple[2] - mTuple[3]*mTuple[3]; if (b >= (Real)0) { c2 = ((Real)0.5)*(discr + b); s2 = a; } else { c2 = a; s2 = ((Real)0.5)*(discr - b); } invLength = Math::InvSqrt(c2*c2 + s2*s2); c2 *= invLength; s2 *= invLength; c1 = mTuple[0]*c2 + mTuple[3]*s2; s1 = mTuple[2]*c2 - mTuple[1]*s2; invLength = Math::InvSqrt(c1*c1 + s1*s1); c1 *= invLength; s1 *= invLength; if (zCon.IsValid(c2, s2) && yCon.IsValid(c1, s1)) { // The maximum occurs at an interior point. q[0] = c2*c1; q[1] = -s2*s1; q[2] = c2*s1; q[3] = s2*c1; } else { // The maximum occurs at a boundary point. Quaternion r(zCon.CosMinAngle, (Real)0, (Real)0, zCon.SinMinAngle); Quaternion rInv(zCon.CosMinAngle, (Real)0, (Real)0, -zCon.SinMinAngle); Quaternion prod = rInv*(*this); tmp = prod.GetClosest(2, yCon); Real dotOptAngle = prod.Dot(tmp); q = r*tmp; r = Quaternion(zCon.CosMaxAngle, (Real)0, (Real)0, zCon.SinMaxAngle); rInv = Quaternion(zCon.CosMaxAngle, (Real)0, (Real)0, -zCon.SinMaxAngle); prod = rInv*(*this); tmp = prod.GetClosest(2, yCon); Real dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = r*tmp; dotOptAngle = dotAngle; } r = Quaternion(yCon.CosMinAngle, (Real)0, yCon.SinMinAngle, (Real)0); rInv = Quaternion(yCon.CosMinAngle, (Real)0, -yCon.SinMinAngle, (Real)0); prod = (*this)*rInv; tmp = prod.GetClosest(3, zCon); dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = tmp*r; dotOptAngle = dotAngle; } r = Quaternion(yCon.CosMaxAngle, (Real)0, yCon.SinMaxAngle, (Real)0); rInv = Quaternion(yCon.CosMaxAngle, (Real)0, -yCon.SinMaxAngle, (Real)0); prod = (*this)*rInv; tmp = prod.GetClosest(3, zCon); dotAngle = prod.Dot(tmp); if (dotAngle > dotOptAngle) { q = tmp*r; dotOptAngle = dotAngle; } } } else { // Infinitely many solutions, choose one that satisfies the angle // constraints. Real minAngle, maxAngle, angle; Constraints con; if (det < (Real)0) { minAngle = yCon.MinAngle - zCon.MaxAngle; maxAngle = yCon.MaxAngle - zCon.MinAngle; con.SetAngles(minAngle, maxAngle); tmp = GetClosest(2, con); angle = Math::ATan2(tmp[2], tmp[0]); if (angle < minAngle || angle > maxAngle) { angle -= (tmp[2] >= (Real)0 ? Math::PI : -Math::PI); // assert(minAngle <= angle && angle <= maxAngle); } if (angle <= yCon.MaxAngle - zCon.MaxAngle) { c2 = zCon.CosMaxAngle; s2 = zCon.SinMaxAngle; angle = zCon.MaxAngle + angle; c1 = Math::Cos(angle); s1 = Math::Sin(angle); } else { c1 = yCon.CosMaxAngle; s1 = yCon.SinMaxAngle; angle = yCon.MaxAngle - angle; c2 = Math::Cos(angle); s2 = Math::Sin(angle); } } else { minAngle = yCon.MinAngle + zCon.MinAngle; maxAngle = yCon.MaxAngle + zCon.MaxAngle; con.SetAngles(minAngle, maxAngle); tmp = GetClosest(2, con); angle = Math::ATan2(tmp[2], tmp[0]); if (angle < minAngle || angle > maxAngle) { angle -= (tmp[2] >= (Real)0 ? Math::PI : -Math::PI); // assert(minAngle <= angle && angle <= maxAngle); } if (angle >= yCon.MinAngle + zCon.MaxAngle) { c2 = zCon.CosMaxAngle; s2 = zCon.SinMaxAngle; angle = angle - zCon.MaxAngle; c1 = Math::Cos(angle); s1 = Math::Sin(angle); } else { c1 = yCon.CosMaxAngle; s1 = yCon.SinMaxAngle; angle = angle - yCon.MaxAngle; c2 = Math::Cos(angle); s2 = Math::Sin(angle); } } q[0] = c2*c1; q[1] = -s2*s1; q[2] = c2*s1; q[3] = s2*c1; if (Dot(q) < (Real)0) { q = -q; } } return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosestYZ (const Constraints& yCon, const Constraints& zCon) const { Quaternion alt(mTuple[0], -mTuple[1], mTuple[2], mTuple[3]); Quaternion q = alt.GetClosestZY(zCon, yCon); q[1] = -q[1]; return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosest (int axis) const { // The appropriate nonzero components will be set later. Quaternion q((Real)0, (Real)0, (Real)0, (Real)0); Real p0 = mTuple[0]; Real p1 = mTuple[axis]; Real sqrLength = p0*p0 + p1*p1; if (sqrLength > Math::ZERO_TOLERANCE) { // A unique closest point. Real invLength = Math::InvSqrt(sqrLength); q[0] = p0*invLength; q[axis] = p1*invLength; } else { // Infinitely many solutions, choose the one for theta = 0. q[0] = (Real)1; q[axis] = (Real)0; } return q; } //---------------------------------------------------------------------------- template Quaternion Quaternion::GetClosest (int axis, const Constraints& con) const { Quaternion q((Real)0, (Real)0, (Real)0, (Real)0); Real p0 = mTuple[0]; Real p1 = mTuple[axis]; Real sqrLength = p0*p0 + p1*p1; if (sqrLength > Math::ZERO_TOLERANCE) { Real invLength = Math::InvSqrt(sqrLength); p0 *= invLength; p1 *= invLength; if (con.IsValid(p0, p1)) { // The maximum occurs at an interior point. q[0] = p0; q[axis] = p1; } else { // The maximum occurs at a boundary point. Real csMin = con.CosMinAngle; Real snMin = con.SinMinAngle; Real dotMinAngle = p0*csMin + p1*snMin; if (dotMinAngle < (Real)0) { csMin = -csMin; snMin = -snMin; dotMinAngle = -dotMinAngle; } Real csMax = con.CosMaxAngle; Real snMax = con.SinMaxAngle; Real dotMaxAngle = p0*csMax + p1*snMax; if (dotMaxAngle < (Real)0) { csMax = -csMax; snMax = -snMax; dotMaxAngle = -dotMaxAngle; } if (dotMaxAngle >= dotMinAngle) { q[0] = csMax; q[axis] = snMax; } else { q[0] = csMin; q[axis] = snMin; } } } else { // Infinitely many solutions, choose one that satisfies the angle // constraints. q[0] = con.CosAvrAngle; q[axis] = con.SinAvrAngle; } return q; } //---------------------------------------------------------------------------- //---------------------------------------------------------------------------- // Quaternion::Constraints //---------------------------------------------------------------------------- template Quaternion::Constraints::Constraints () { // Members are uninitialized. } //---------------------------------------------------------------------------- template Quaternion::Constraints::Constraints (Real minAngle, Real maxAngle) { SetAngles(minAngle, maxAngle); } //---------------------------------------------------------------------------- template void Quaternion::Constraints::SetAngles (Real minAngle, Real maxAngle) { MinAngle = minAngle; MaxAngle = maxAngle; CosMinAngle = Math::Cos(MinAngle); SinMinAngle = Math::Sin(MinAngle); CosMaxAngle = Math::Cos(MaxAngle); SinMaxAngle = Math::Sin(MaxAngle); DiffCosMaxMin = CosMaxAngle - CosMinAngle; DiffSinMaxMin = SinMaxAngle - SinMinAngle; Real avrAngle = ((Real)0.5)*(MinAngle + MaxAngle); CosAvrAngle = Math::Cos(avrAngle); SinAvrAngle = Math::Sin(avrAngle); } //---------------------------------------------------------------------------- template bool Quaternion::Constraints::IsValid (Real x, Real y) const { // (x,y) must be unit-length. // Test whether (x,y) satisfies the constraints. Real xm = x - CosMinAngle; Real ym = y - SinMinAngle; if (xm*DiffSinMaxMin >= ym*DiffCosMaxMin) { return true; } // Test whether (-x,-y) satisfies the constraints. Real xp = x + CosMinAngle; Real yp = y + SinMinAngle; if (xp*DiffSinMaxMin <= yp*DiffCosMaxMin) { return true; } return false; } //----------------------------------------------------------------------------