/**
this method is used to return the current heading of the character, specified as an angle measured in radians
*/
double Character::getHeadingAngle(){
//first we need to get the current heading of the character. Also, note that q and -q represent the same rotation
Quaternion q = getHeading();
if (q.s<0){
q.s = -q.s;
q.v = -q.v;
}
double currentHeading = 2 * safeACOS(q.s);
if (q.v.dotProductWith(PhysicsGlobals::up) < 0)
currentHeading = -currentHeading;
return currentHeading;
}
Quaternion2 Quaternion2::DecomposeRotation2(const VectorR3 vB) const {
//we need to compute v in A's coordinates
VectorR3 vA = this->rotate(vB);
vA.Normalize();
double temp = 0;
//compute the rotation that aligns the vector v in the two coordinate frames (A and T)
VectorR3 rotAxis = vA * vB;
rotAxis.Normalize();
double rotAngle = -safeACOS(vA.Dot(vB));
Quaternion2 TqA = getRotationQuaternion2(rotAngle, rotAxis*(-1));
return TqA * (*this);
}
/**
Assume that the current quaternion represents the relative orientation between two coordinate frames A and B.
This method decomposes the current relative rotation into a twist of frame B around the axis v passed in as a
parameter, and another more arbitrary rotation.
AqB = AqT * TqB, where T is a frame that is obtained by rotating frame B around the axis v by the angle
that is returned by this function.
In the T coordinate frame, v is the same as in B, and AqT is a rotation that aligns v from A to that
from T.
It is assumed that vB is a unit vector!! This method returns TqB, which represents a twist about
the axis vB.
*/
Quaternion Quaternion::decomposeRotation(const Vector3d vB) const{
//we need to compute v in A's coordinates
Vector3d vA = this->rotate(vB);
vA.toUnit();
double temp = 0;
//compute the rotation that aligns the vector v in the two coordinate frames (A and T)
Vector3d rotAxis = vA.crossProductWith(vB);
rotAxis.toUnit();
double rotAngle = -safeACOS(vA.dotProductWith(vB));
Quaternion TqA = Quaternion::getRotationQuaternion(rotAngle, rotAxis*(-1));
return TqA * (*this);
}
/**
Assume that the current quaternion represents the relative orientation between two coordinate frames P and C (i.e. q
rotates vectors from the child/local frame C into the parent/global frame P).
With v specified in frame C's coordinates, this method decomposes the current relative rotation, such that:
PqC = qA * qB, where qB represents a rotation about axis v.
This can be thought of us as a twist about axis v - qB - and a more general rotation, and swing
- qA - decomposition. Note that qB can be thought of as a rotation from the C frame into a tmp trame T,
and qA a rotation from T into P.
In the T coordinate frame, v is the same as in C, and qA is a rotation that aligns v from P to that
from T.
*/
void Quaternion::decomposeRotation(Quaternion* qA, Quaternion* qB, const Vector3d& vC) const{
//we need to compute v in P's coordinates
Vector3d vP;
this->fastRotate(vC, &vP);
//compute the rotation that alligns the vector v in the two coordinate frames (P and T - remember that v has the same coordinates in C and in T)
Vector3d rotAxis;
rotAxis.setToCrossProduct(vP, vC);
rotAxis.toUnit();
double rotAngle = -safeACOS(vP.dotProductWith(vC));
qA->setToRotationQuaternion(rotAngle, rotAxis);
//now qB = qAinv * PqC
qB->setToProductOf(*qA, *this, true, false);
*qB = (*qA).getComplexConjugate() * (*this);
}
/**
This method returns a quaternion that is the result of spherically interpolating between the current quaternion and the one provided as a parameter.
The value of the parameter t indicates the progress: if t = 0, the result will be *this. If it is 1, it will be other. If it is inbetween, then
the result will be a combination of the two initial quaternions.
Both quaternions that are used for the interpolation are assumed to have unit length!!!
*/
Quaternion Quaternion::sphericallyInterpolateWith(const Quaternion &other, double t) const{
//make sure that we return the same value if either of the quaternions involved is q or -q
if (this->dotProductWith(other) < 0){
Quaternion temp;
temp.s = -other.s;
temp.v = other.v * (-1);
return this->sphericallyInterpolateWith(temp, t);
}
if (t<0) t = 0;
if (t>1) t = 1;
double dotProduct = this->dotProductWith(other);
double sinTheta = sqrt(MAX(0,1-dotProduct*dotProduct));
double theta = safeACOS(dotProduct);
if (sinTheta == 0)
return (*this);
return ((*this) * sin(theta * (1-t)) + other * sin(theta * t)) * (1/sin(theta));
}
Quaternion2 Quaternion2::SlerpWith(const Quaternion2 &other, double t) const
{
//make sure that we return the same value if either of the Quaternion2s involved is q or -q
if (this->DotProductWith(other) < 0) {
Quaternion2 temp;
temp.w = -other.w;
temp.x = other.x * (-1);
temp.y = other.y * (-1);
temp.z = other.z * (-1);
return this->SlerpWith(temp, t);
}
if (t<0) t = 0;
if (t>1) t = 1;
double dotProduct = this->DotProductWith(other);
double sinTheta = sqrt(max(0,1-dotProduct*dotProduct));
double theta = safeACOS(dotProduct);
if (sinTheta == 0)
return (*this);
return ((*this) * sin(theta * (1-t)) + other * sin(theta * t)) * (1/sin(theta));
}