//----------------------------------------------------------------------- Quaternion Quaternion::Slerp (Real fT, const Quaternion& rkP, const Quaternion& rkQ, bool shortestPath) { Real fCos = rkP.Dot(rkQ); Radian fAngle ( Math::ACos(fCos) ); if ( Math::Abs(fAngle.valueRadians()) < ms_fEpsilon ) return rkP; Real fSin = Math::Sin(fAngle); Real fInvSin = 1.0/fSin; Real fCoeff0 = Math::Sin((1.0-fT)*fAngle)*fInvSin; Real fCoeff1 = Math::Sin(fT*fAngle)*fInvSin; // Do we need to invert rotation? if (fCos < 0.0f && shortestPath) { fCoeff0 = -fCoeff0; // taking the complement requires renormalisation Quaternion t(fCoeff0*rkP + fCoeff1*rkQ); t.normalise(); return t; } else { return fCoeff0*rkP + fCoeff1*rkQ; } }
// ******************************************************************************** // // The spherical interpolation for union quaternions Quaternion OrE::Math::Slerp(const Quaternion& a, const Quaternion& b, const float t) { float fOmega = Arccos( Clamp(a.Dot(b), -1.0f, 1.0f) ); float f1 = Sin( fOmega * (1.0f-t) ); float f2 = Sin( fOmega * t ); return Quaternion( a.r*f1+b.r*f2, a.i*f1+b.i*f2, a.j*f1+b.j*f2, a.k*f1+b.k*f2 ); }
//! \verbatim //! Real Quaternion.dot( Quaternion other ) //! \endverbatim void Quaternion::jsDot( const FunctionCallbackInfo<v8::Value>& args ) { Quaternion* ptr = unwrap( args.Holder() ); Quaternion* other = Util::extractQuaternion( 0, args ); if ( !other ) return; args.GetReturnValue().Set( ptr->Dot( *other ) ); }
Quaternion<Real> Quaternion<Real>::Slerp(Real fT, const Quaternion& rkP, const Quaternion& rkQ) { Real fCos = rkP.Dot(rkQ); Real fAngle = Math<Real>::ACos(fCos); if (Math<Real>::FAbs(fAngle) < Math<Real>::EPSILON) return rkP; Real fSin = Math<Real>::Sin(fAngle); Real fInvSin = ((Real) 1.0) / fSin; Real fCoeff0 = Math<Real>::Sin((((Real) 1.0) - fT) * fAngle) * fInvSin; Real fCoeff1 = Math<Real>::Sin(fT* fAngle) * fInvSin; return fCoeff0 * rkP + fCoeff1 * rkQ; }
//----------------------------------------------------------------------- Quaternion Quaternion::nlerp(Real fT, const Quaternion& rkP, const Quaternion& rkQ, bool shortestPath) { Quaternion result; Real fCos = rkP.Dot(rkQ); if (fCos < 0.0f && shortestPath) { result = rkP + fT * ((-rkQ) - rkP); } else { result = rkP + fT * (rkQ - rkP); } result.normalise(); return result; }
/* 为什么下面是-rkQ? 因为两者角度大于180度时,需要把第二个角度减去360度,表示从反方向变换到rkQ 令本来rkQ的角度是r, 变过以后就是r-360 而四元数采取的是cos(r/2), sin(r/2)*xxx 于是r/2 变成r/2 -180 而cos(r/2-180) =-cos(r/2), sin(r/2-180) =-sin(r/2) 所以得到的是-rkQ */ Quaternion Quaternion::nlerp(float fT, const Quaternion& rkP, const Quaternion& rkQ, bool shortestPath) { Quaternion result; float fCos = rkP.Dot(rkQ); if (fCos < 0.0f && shortestPath) { result = rkP + ((-rkQ) - rkP)*fT; } else { result = rkP +(rkQ - rkP)*fT; } result.Normalize(); return result; }
//----------------------------------------------------------------------- Quaternion Quaternion::SlerpExtraSpins (Real fT, const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins) { Real fCos = rkP.Dot(rkQ); Radian fAngle ( Math::ACos(fCos) ); if ( Math::Abs(fAngle.valueRadians()) < msEpsilon ) return rkP; Real fSin = Math::Sin(fAngle); Radian fPhase ( Math::PI*iExtraSpins*fT ); Real fInvSin = 1.0f/fSin; Real fCoeff0 = Math::Sin((1.0f-fT)*fAngle - fPhase)*fInvSin; Real fCoeff1 = Math::Sin(fT*fAngle + fPhase)*fInvSin; return fCoeff0*rkP + fCoeff1*rkQ; }
//----------------------------------------------------------------------- Quaternion Quaternion::SlerpExtraSpins (float fT, const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins) { float fCos = rkP.Dot(rkQ); float fAngle ( acosf(fCos) ); if ( fabs(fAngle) < ms_fEpsilon ) return rkP; float fSin = sinf(fAngle); float fPhase (float(M_PI)*iExtraSpins*fT ); float fInvSin = 1.0f/fSin; float fCoeff0 = sinf((1.0f-fT)*fAngle - fPhase)*fInvSin; float fCoeff1 = sinf(fT*fAngle + fPhase)*fInvSin; return fCoeff0*rkP + fCoeff1*rkQ; }
Quaternion<Real> Quaternion<Real>::SlerpExtraSpins(Real fT, const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins) { Real fCos = rkP.Dot(rkQ); Real fAngle = Math<Real>::ACos(fCos); if (Math<Real>::FAbs(fAngle) < Math<Real>::EPSILON) return rkP; Real fSin = Math<Real>::Sin(fAngle); Real fPhase = Math<Real>::PI* iExtraSpins* fT; Real fInvSin = ((Real) 1.0) / fSin; Real fCoeff0 = Math<Real>::Sin((((Real) 1.0) - fT) * fAngle - fPhase) * fInvSin; Real fCoeff1 = Math<Real>::Sin(fT* fAngle + fPhase) * fInvSin; return fCoeff0 * rkP + fCoeff1 * rkQ; }
Quaternion Quaternion::Slerp(const Quaternion &first, const Quaternion &second, float interpolationCoefficient) { float dot = first.Dot(second); if(dot > s_quaternionDotTolerance) return Lerp(first, second, interpolationCoefficient); dot = Clamp(dot, -1.0f, 1.0f); float theta = acosf(dot) * interpolationCoefficient; Quaternion third(second - first * dot); third.NormalizeThis(); return first * cosf(theta) + third * sinf(theta); }
const Ellipsoid3<Real> MergeEllipsoids (const Ellipsoid3<Real>& ellipsoid0, const Ellipsoid3<Real>& ellipsoid1) { Ellipsoid3<Real> merge; // compute the average of the input centers merge.Center = ((Real)0.5)*(ellipsoid0.Center + ellipsoid1.Center); // bounding ellipsoid orientation is average of input orientations Quaternion<Real> q0(ellipsoid0.Axis), q1(ellipsoid1.Axis); if (q0.Dot(q1) < (Real)0) { q1 = -q1; } Quaternion<Real> q = q0 + q1; q = Math<Real>::InvSqrt(q.Dot(q))*q; q.ToRotationMatrix(merge.Axis); // Project the input ellipsoids onto the axes obtained by the average // of the orientations and that go through the center obtained by the // average of the centers. for (int i = 0; i < 3; ++i) { // Projection axis. Line3<Real> line(merge.Center, merge.Axis[i]); // Project ellipsoids onto the axis. Real min0, max0, min1, max1; ProjectEllipsoid(ellipsoid0, line, min0, max0); ProjectEllipsoid(ellipsoid1, line, min1, max1); // Determine the smallest interval containing the projected // intervals. Real maxIntr = (max0 >= max1 ? max0 : max1); Real minIntr = (min0 <= min1 ? min0 : min1); // Update the average center to be the center of the bounding box // defined by the projected intervals. merge.Center += line.Direction*(((Real)0.5)*(minIntr + maxIntr)); // Compute the extents of the box based on the new center. merge.Extent[i] = ((Real)0.5)*(maxIntr - minIntr); } return merge; }
Quaternion Quaternion::Slerp(float pT, Quaternion &pq) { //We calculate the angle spread between both quaternions float AngleCos = pq.Dot(*this); float Angle = qACos(AngleCos); //see the function ACos above if (Angle < MINFLOAT) { return Quaternion(*this); } //We calculate the interpolated angle and deduce the resulting quaternion float InvAngleSin = (1.0f / sin(Angle)); float Coeff0 = sin((1 - pT) * Angle) * InvAngleSin; float Coeff1 = sin(pT * Angle) * InvAngleSin; return Quaternion((*this * Coeff0) + (pq * Coeff1)); }
Quaternion Quaternion::Slerp (Real fT, const Quaternion& rkP, const Quaternion& rkQ, bool shortestPath) { Real fCos = rkP.Dot(rkQ); Quaternion rkT; // Do we need to invert rotation? if (fCos < 0.0f && shortestPath) { fCos = -fCos; rkT = -rkQ; } else { rkT = rkQ; } if (Math::Abs(fCos) < 1 - ms_fEpsilon) { // Standard case (slerp) Real fSin = Math::Sqrt(1 - Math::Sqr(fCos)); Radian fAngle = Math::ATan2(fSin, fCos); Real fInvSin = 1.0f / fSin; Real fCoeff0 = Math::Sin((1.0f - fT) * fAngle) * fInvSin; Real fCoeff1 = Math::Sin(fT * fAngle) * fInvSin; return fCoeff0 * rkP + fCoeff1 * rkT; } else { // There are two situations: // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear // interpolation safely. // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there // are an infinite number of possibilities interpolation. but we haven't // have method to fix this case, so just use linear interpolation here. Quaternion t = (1.0f - fT) * rkP + fT * rkT; // taking the complement requires renormalisation t.normalise(); return t; } }
/* 球形插值: 以恒定的速率描述两四元数之间的曲线 即得到的效果是角度的线性插值θ=(1-t)*θ1+t*θ2 结论是: q(t) = q1*sin(1-t)(θ)/sin(θ) +q2*sin(tθ)/sin(θ) 因为涉及到tan的计算,所以需要对cos的值进行一个范围的判断,防止越界 */ Quaternion Quaternion::slerp (float fT, const Quaternion& rkP, const Quaternion& rkQ, bool shortestPath) { float fCos = rkP.Dot(rkQ); Quaternion rkT; // Do we need to invert rotation? if (fCos < 0.0f && shortestPath) { fCos = -fCos; rkT = -rkQ; } else { rkT = rkQ; } if (fabs(fCos) < 1 - msEpsilon) { // Standard case (slerp) float fSin = sqrtf(1 - fCos*fCos); float fAngle = atan2f(fSin, fCos); float fInvSin = 1.0f / fSin; float fCoeff0 = sinf((1.0f - fT) * fAngle) * fInvSin; float fCoeff1 = sinf(fT * fAngle) * fInvSin; return rkP *fCoeff0 + rkT*fCoeff1; } else { // There are two situations: // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear // interpolation safely. // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there // are an infinite number of possibilities interpolation. but we haven't // have method to fix this case, so just use linear interpolation here. Quaternion t = rkP * (1.0f - fT)+ rkT * fT ; // taking the complement requires renormalisation t.Normalize(); return t; } }
//----------------------------------------------------------------------- Quaternion Quaternion::Slerp(Real fT, const Quaternion& rkP, const Quaternion& rkQ, bool shortestPath) { Real fCos = rkP.Dot(rkQ); Quaternion rkT; // 需要翻转旋转? if (fCos < 0.0f && shortestPath) { fCos = -fCos; rkT = -rkQ; } else { rkT = rkQ; } if (Math::Abs(fCos) < 1 - msEpsilon) { // 正常情况 (slerp) Real fSin = Math::Sqrt(1 - Math::Sqr(fCos)); Radian fAngle = Math::ATan2(fSin, fCos); Real fInvSin = 1.0f / fSin; Real fCoeff0 = Math::Sin((1.0f - fT) * fAngle.valueRadians()) * fInvSin; Real fCoeff1 = Math::Sin(fT * fAngle.valueRadians()) * fInvSin; return fCoeff0 * rkP + fCoeff1 * rkT; } else { // 这有两种情况 // 1. "rkP" 和 "rkQ" 是非常接近(fCos ~= +1), 所以我们能做安全的做线性插值 // 2. "rkP" 和 "rkQ" 几乎是每一个 (fCos ~= -1)的反转,这就有无数种插值的可能性。但是我们不可能有修正这个问题的方法, // 所有就在这里用线性插值 Quaternion t = (1.0f - fT) * rkP + fT * rkT; // 使这个分量重新标准化 t.normalise(); return t; } }
Box3<Real> Wml::MergeBoxes (const Box3<Real>& rkBox0, const Box3<Real>& rkBox1) { // construct a box that contains the input boxes Box3<Real> kBox; // The first guess at the box center. This value will be updated later // after the input box vertices are projected onto axes determined by an // average of box axes. kBox.Center() = ((Real)0.5)*(rkBox0.Center() + rkBox1.Center()); // A box's axes, when viewed as the columns of a matrix, form a rotation // matrix. The input box axes are converted to quaternions. The average // quaternion is computed, then normalized to unit length. The result is // the slerp of the two input quaternions with t-value of 1/2. The result // is converted back to a rotation matrix and its columns are selected as // the merged box axes. Quaternion<Real> kQ0, kQ1; kQ0.FromRotationMatrix(rkBox0.Axes()); kQ1.FromRotationMatrix(rkBox1.Axes()); if ( kQ0.Dot(kQ1) < 0.0f ) kQ1 = -kQ1; Quaternion<Real> kQ = kQ0 + kQ1; Real fInvLength = Math<Real>::InvSqrt(kQ.Dot(kQ)); kQ = fInvLength*kQ; kQ.ToRotationMatrix(kBox.Axes()); // Project the input box vertices onto the merged-box axes. Each axis // D[i] containing the current center C has a minimum projected value // pmin[i] and a maximum projected value pmax[i]. The corresponding end // points on the axes are C+pmin[i]*D[i] and C+pmax[i]*D[i]. The point C // is not necessarily the midpoint for any of the intervals. The actual // box center will be adjusted from C to a point C' that is the midpoint // of each interval, // C' = C + sum_{i=0}^2 0.5*(pmin[i]+pmax[i])*D[i] // The box extents are // e[i] = 0.5*(pmax[i]-pmin[i]) int i, j; Real fDot; Vector3<Real> akVertex[8], kDiff; Vector3<Real> kMin = Vector3<Real>::ZERO; Vector3<Real> kMax = Vector3<Real>::ZERO; rkBox0.ComputeVertices(akVertex); for (i = 0; i < 8; i++) { kDiff = akVertex[i] - kBox.Center(); for (j = 0; j < 3; j++) { fDot = kDiff.Dot(kBox.Axis(j)); if ( fDot > kMax[j] ) kMax[j] = fDot; else if ( fDot < kMin[j] ) kMin[j] = fDot; } } rkBox1.ComputeVertices(akVertex); for (i = 0; i < 8; i++) { kDiff = akVertex[i] - kBox.Center(); for (j = 0; j < 3; j++) { fDot = kDiff.Dot(kBox.Axis(j)); if ( fDot > kMax[j] ) kMax[j] = fDot; else if ( fDot < kMin[j] ) kMin[j] = fDot; } } // [kMin,kMax] is the axis-aligned box in the coordinate system of the // merged box axes. Update the current box center to be the center of // the new box. Compute the extens based on the new center. for (j = 0; j < 3; j++) { kBox.Center() += (((Real)0.5)*(kMax[j]+kMin[j]))*kBox.Axis(j); kBox.Extent(j) = ((Real)0.5)*(kMax[j]-kMin[j]); } return kBox; }
Box3<Real> MergeBoxes (const Box3<Real>& box0, const Box3<Real>& box1) { // Construct a box that contains the input boxes. Box3<Real> box; // The first guess at the box center. This value will be updated later // after the input box vertices are projected onto axes determined by an // average of box axes. box.Center = ((Real)0.5)*(box0.Center + box1.Center); // A box's axes, when viewed as the columns of a matrix, form a rotation // matrix. The input box axes are converted to quaternions. The average // quaternion is computed, then normalized to unit length. The result is // the slerp of the two input quaternions with t-value of 1/2. The result // is converted back to a rotation matrix and its columns are selected as // the merged box axes. Quaternion<Real> q0, q1; q0.FromRotationMatrix(box0.Axis); q1.FromRotationMatrix(box1.Axis); if (q0.Dot(q1) < (Real)0) { q1 = -q1; } Quaternion<Real> q = q0 + q1; Real invLength = Math<Real>::InvSqrt(q.Dot(q)); q = invLength*q; q.ToRotationMatrix(box.Axis); // Project the input box vertices onto the merged-box axes. Each axis // D[i] containing the current center C has a minimum projected value // min[i] and a maximum projected value max[i]. The corresponding end // points on the axes are C+min[i]*D[i] and C+max[i]*D[i]. The point C // is not necessarily the midpoint for any of the intervals. The actual // box center will be adjusted from C to a point C' that is the midpoint // of each interval, // C' = C + sum_{i=0}^2 0.5*(min[i]+max[i])*D[i] // The box extents are // e[i] = 0.5*(max[i]-min[i]) int i, j; Real dot; Vector3<Real> vertex[8], diff; Vector3<Real> pmin = Vector3<Real>::ZERO; Vector3<Real> pmax = Vector3<Real>::ZERO; box0.ComputeVertices(vertex); for (i = 0; i < 8; ++i) { diff = vertex[i] - box.Center; for (j = 0; j < 3; ++j) { dot = diff.Dot(box.Axis[j]); if (dot > pmax[j]) { pmax[j] = dot; } else if (dot < pmin[j]) { pmin[j] = dot; } } } box1.ComputeVertices(vertex); for (i = 0; i < 8; ++i) { diff = vertex[i] - box.Center; for (j = 0; j < 3; ++j) { dot = diff.Dot(box.Axis[j]); if (dot > pmax[j]) { pmax[j] = dot; } else if (dot < pmin[j]) { pmin[j] = dot; } } } // [min,max] is the axis-aligned box in the coordinate system of the // merged box axes. Update the current box center to be the center of // the new box. Compute the extents based on the new center. for (j = 0; j < 3; ++j) { box.Center += (((Real)0.5)*(pmax[j] + pmin[j]))*box.Axis[j]; box.Extent[j] = ((Real)0.5)*(pmax[j] - pmin[j]); } return box; }
void Animation::SetFrameOverlay( F32 frame, AnimKey &state, F32 controlFrame) const { controlFrame; U32 i; #if 0 Quaternion conq; conq.ClearData(); for (i = 1; i < keys.count; i++) { AnimKey &lastKey = keys[i - 1]; AnimKey &thisKey = keys[i]; if (controlFrame <= thisKey.frame) { // determine the current fraction to this cycle F32 dk = thisKey.frame - lastKey.frame; ASSERT( dk > 0); F32 dfdk = (controlFrame - lastKey.frame) / dk; if (lastKey.type & animQUATERNION) { // do a parametric interpolation of the quaternion using dfdk conq = (thisKey.quaternion - lastKey.quaternion) * dfdk + lastKey.quaternion; } break; } } #endif // 0; 4; 0.923880, 0.382683, 0.0, 0.0;;, // 0; 4; 0.898028, 0.439939, 0.0, 0.0;;, // 0; 4; 0.868631, 0.495459, 0.0, 0.0;;, // 0; 4; 0.770513, 0.637424, 0.0, 0.0;;, for (i = 1; i < keys.count; i++) { AnimKey &lastKey = keys[i - 1]; AnimKey &thisKey = keys[i]; if (frame <= thisKey.frame) { // LOG_DIAG( ("%f ; %f ; %f", lastKey.frame, frame, thisKey.frame) ); ASSERT( lastKey.frame < thisKey.frame); // determine the current fraction to this cycle F32 dk = thisKey.frame - lastKey.frame; ASSERT( dk > 0); F32 dfdk = (frame - lastKey.frame) / dk; if (lastKey.type & animQUATERNION) { // do a parametric interpolation of the quaternion using dfdk Quaternion q = (thisKey.quaternion - lastKey.quaternion) * dfdk + lastKey.quaternion; #if 1 if (q.Dot( q) <= 0) { LOG_WARN( ("Quat::Set q.Dot(q) <= 0: %f %f %f %f", q.s, q.v.x, q.v.y, q.v.z) ); LOG_WARN( ("lastKey.quat: %f %f %f %f", lastKey.quaternion.s, lastKey.quaternion.v.x, lastKey.quaternion.v.y, lastKey.quaternion.v.z) ); LOG_WARN( ("thisKey.quat: %f %f %f %f", thisKey.quaternion.s, thisKey.quaternion.v.x, thisKey.quaternion.v.y, thisKey.quaternion.v.z) ); LOG_WARN( ("dfdk: %f; lastKey.frame %f; thisKey.frame; frame", dfdk, lastKey.frame, thisKey.frame, frame) ); statQuatError = TRUE; } #else ASSERT( q.Dot( q) > 0); #endif // blend in the quat // // q -= conq; // q = state.quaternion * q; // q.Set( PI * 0.11f, Matrix::I.right); // q = state.quaternion * q; state.Set( q ); } break; } } }
void Animation::Set( F32 frame, const AnimKey &lastKey, const AnimKey &thisKey, AnimKey &state) { // determine the current fraction to this cycle F32 dk = thisKey.frame - lastKey.frame; ASSERT( dk > 0); F32 dfdk = (frame - lastKey.frame) / dk; // state.type = 0; if (lastKey.type & animSCALE) { // do a parametric interpolation of the quaternion using dfdk Quaternion q = (thisKey.quaternion - lastKey.quaternion) * dfdk + lastKey.quaternion; // do a parametric interpolation of the quaternion using dfdk if (q.Dot( q) <= 0) { LOG_WARN( ("Quat::Set q.Dot(q) <= 0: %f %f %f %f", q.s, q.v.x, q.v.y, q.v.z) ); LOG_WARN( ("lastKey.quat: %f %f %f %f", lastKey.quaternion.s, lastKey.quaternion.v.x, lastKey.quaternion.v.y, lastKey.quaternion.v.z) ); LOG_WARN( ("thisKey.quat: %f %f %f %f", thisKey.quaternion.s, thisKey.quaternion.v.x, thisKey.quaternion.v.y, thisKey.quaternion.v.z) ); LOG_WARN( ("dfdk: %f; lastKey.frame %f; thisKey.frame %f; frame %f", dfdk, lastKey.frame, thisKey.frame, frame) ); statQuatError = TRUE; state.Set(thisKey.quaternion); } else { // set the matrix state.Set( q); } // do a parametric interpolation of the scale using dfdk Vector v = (thisKey.scale - lastKey.scale) * dfdk + lastKey.scale; // set the matrix state.SetScale( v); // state.type |= animSCALE; } else if (lastKey.type & animQUATERNION) { Quaternion q = (thisKey.quaternion - lastKey.quaternion) * dfdk + lastKey.quaternion; // do a parametric interpolation of the quaternion using dfdk if (q.Dot( q) <= 0) { LOG_WARN( ("Quat::Set q.Dot(q) <= 0: %f %f %f %f", q.s, q.v.x, q.v.y, q.v.z) ); LOG_WARN( ("lastKey.quat: %f %f %f %f", lastKey.quaternion.s, lastKey.quaternion.v.x, lastKey.quaternion.v.y, lastKey.quaternion.v.z) ); LOG_WARN( ("thisKey.quat: %f %f %f %f", thisKey.quaternion.s, thisKey.quaternion.v.x, thisKey.quaternion.v.y, thisKey.quaternion.v.z) ); LOG_WARN( ("dfdk: %f; lastKey.frame %f; thisKey.frame %f; frame %f", dfdk, lastKey.frame, thisKey.frame, frame) ); statQuatError = TRUE; state.Set(thisKey.quaternion); } else { // set the matrix state.Set( q); } // state.type |= animQUATERNION; } if (lastKey.type & animPOSITION) { // do a parametric interpolation of the pos using dfdk Vector v = (thisKey.position - lastKey.position) * dfdk + lastKey.position; // set the matrix state.Set( v); // state.type |= animPOSITION; } }