inline Quaternion<T> Squad(const Quaternion<T>& p, const Quaternion<T>& a, const Quaternion<T>& b, const Quaternion<T>& q, const T& time)
{
Quaternion<T> qa = Slerp(p,q,time,0);
Quaternion<T> qb = Slerp(a,b,time,0);
T qtime = 2 * time * (1 - time);
return Slerp(qa,qb,qtime,0);
}
HawkQuaternion HawkQuaternion::Squad (Float fT,const HawkQuaternion& rkP, const HawkQuaternion& rkA,const HawkQuaternion& rkB, const HawkQuaternion& rkQ, bool shortestPath)
{
Float fSlerpT = 2.0f*fT*(1.0f-fT);
HawkQuaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);
HawkQuaternion kSlerpQ = Slerp(fT, rkA, rkB);
return Slerp(fSlerpT, kSlerpP ,kSlerpQ);
}
void Squad (Quaternion* res, float t, Quaternion* p,
Quaternion* a, Quaternion* b, Quaternion* q)
{
Quaternion q1, q2;
Slerp(&q1, t, p, q);
Slerp(&q2, t, a, b);
Slerp(res, 2 * t * (1 - t), &q1, &q2);
}
moQuaternion<Real>& moQuaternion<Real>::Squad (Real fT, const moQuaternion& rkQ0,
const moQuaternion& rkA0, const moQuaternion& rkA1, const moQuaternion& rkQ1)
{
Real fSlerpT = ((Real)2.0)*fT*((Real)1.0-fT);
moQuaternion kSlerpP = Slerp(fT,rkQ0,rkQ1);
moQuaternion kSlerpQ = Slerp(fT,rkA0,rkA1);
return Slerp(fSlerpT,kSlerpP,kSlerpQ);
}
//-----------------------------------------------------------------------
Quaternion Quaternion::Squad (Real fT,
const Quaternion& rkP, const Quaternion& rkA,
const Quaternion& rkB, const Quaternion& rkQ, bool shortestPath)
{
Real fSlerpT = 2.0f*fT*(1.0f-fT);
Quaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);
Quaternion kSlerpQ = Slerp(fT, rkA, rkB);
return Slerp(fSlerpT, kSlerpP ,kSlerpQ);
}
Quaternion Quaternion::Squad(
float t,
const Quaternion &p,
const Quaternion &a,
const Quaternion &b,
const Quaternion &q,
bool shortestPath)
{
float slerpT = 2.0f * t * (1.0f - t);
Quaternion slerpP = Slerp(t, p, q, shortestPath);
Quaternion slerpQ = Slerp(t, a, b);
return Slerp(slerpT, slerpP ,slerpQ);
}
SkelMat4 skelBone_AvRot::GetDirtyTransform( const skelAnimStoreFrameList_c *frames )
{
SkelQuat temp1, temp2;
SkelMat4 temp;
temp1 = m_reference1->GetTransform( frames );
temp2 = m_reference2->GetTransform( frames );
// lerp values
Slerp( temp1, temp2, m_bone2weight, &m_cachedQuat );
VectorCopy( m_basePos, m_cachedValue[ 3 ] );
if( m_parent )
{
temp.Multiply( m_cachedValue, m_parent->GetTransform( frames ) );
m_cachedValue = temp;
}
m_cachedQuat.GetMat4( m_cachedValue );
m_isDirty = false;
return m_cachedValue;
}
void AnimatedTransform::Interpolate(float time, Transform *t) const {
// Handle boundary conditions for matrix interpolation
if (!actuallyAnimated || time <= startTime) {
*t = *startTransform;
return;
}
if (time >= endTime) {
*t = *endTransform;
return;
}
float dt = (time - startTime) / (endTime - startTime);
// Interpolate translation at _dt_
Vector trans = (1.f - dt) * T[0] + dt * T[1];
// Interpolate rotation at _dt_
Quaternion rotate = Slerp(dt, R[0], R[1]);
// Interpolate scale at _dt_
Matrix4x4 scale;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
scale.m[i][j] = Lerp(dt, S[0].m[i][j], S[1].m[i][j]);
// Compute interpolated matrix as product of interpolated components
*t = Translate(trans) * rotate.ToTransform() * Transform(scale);
}
bool Interpolator::LinearQuaternion(FlKeyCode keycode, FlKeyGroup* g1, FlKeyGroup* g2, double t, Quaternion &qval)
{
qval = Quaternion(0, 0, 0, 1);
if (!g1 || !g2) return false;
FlKey *key1 = SearchKey(keycode, g1);
FlKey *key2 = SearchKey(keycode, g2);
if (!key1 || !key2) return false;
if (key1->GetType()!=FLKEY_TYPE_QUATER ||
key2->GetType()!=FLKEY_TYPE_QUATER)
return false;
double t1, t2;
Quaternion q1, q2;
q1 = ((FlKeyQuaternion*)key1)->GetValue();
q2 = ((FlKeyQuaternion*)key2)->GetValue();
t1 = g1->t;
t2 = g2->t;
if (t1 == t2)
{
qval = q1;
return true;
}
double st = Smooth((t-t1)/(t2-t1), g1->type==1, g2->type==1);
qval = Slerp(q1, q2, st);
return true;
}
void QuatApp::SlerpArray( Quat *result, const Quat *source, const Quat *target, const float slerpFactor, const int numQuats )
{
for (int i = 0; i < numQuats; i++)
{
Slerp(result[i], source[i], target[i], slerpFactor);
}
}
template<> void TUNDRACORE_API Attribute<Quat>::Interpolate(IAttribute* start, IAttribute* end, float t, AttributeChange::Type change)
{
Attribute<Quat>* startQuat = dynamic_cast<Attribute<Quat>*>(start);
Attribute<Quat>* endQuat = dynamic_cast<Attribute<Quat>*>(end);
if (startQuat && endQuat)
Set(Slerp(startQuat->Get(), endQuat->Get(), t), change);
}
void InterpolateRotation(Quat startRot, Quat endRot, AngAxis startSpin, float endSpin,
PreciseTimeValue startT, PreciseTimeValue endT, PreciseTimeValue curT,
float easyIn, bool targetConstant, Quat& resRot, AngAxis& resSpin)
{
float timeDif = float(endT - startT);
AngAxis totalSpin = startSpin;
totalSpin.angle *= timeDif;
Quat rot0 = startRot + Quat(totalSpin);
AngAxis difSpin;
difSpin.angle = QangAxis(rot0, endRot, difSpin.axis);
AngAxis reverseSpin = difSpin;
reverseSpin.angle = endSpin;
AngAxis totalDifSpin = difSpin;
totalDifSpin.angle = -endSpin*timeDif;
Quat rot1 = endRot + Quat(totalDifSpin);
AngAxis partialSpin = startSpin;
partialSpin.angle *= float(curT - startT);
Quat rot00 = startRot + Quat(partialSpin);
AngAxis partialRevSpin = reverseSpin;
partialRevSpin.angle *= float(curT - endT);
Quat rot11 = endRot + Quat(partialRevSpin);
if (!targetConstant)
rot11.MakeClosest(rot00);
float timeRatio = float(endT - curT)/timeDif;
float adjRatio = timeRatio*(timeRatio*easyIn + 1.0f - easyIn);
resRot = Slerp(rot11, rot00, adjRatio);
curT.tick -= 1;
partialSpin = startSpin;
partialSpin.angle *= float(curT - startT);
Quat rot20 = startRot + Quat(partialSpin);
partialRevSpin = reverseSpin;
partialRevSpin.angle *= float(curT - endT);
Quat rot21 = endRot + Quat(partialRevSpin);
if (!targetConstant)
rot21.MakeClosest(rot20);
timeRatio = float(endT - curT)/timeDif;
float adjRatio2 = timeRatio*(timeRatio*easyIn + 1.0f - easyIn);
Quat prevRot = Slerp(rot21, rot20, adjRatio2);
resSpin.angle = QangAxis(prevRot, resRot, resSpin.axis);
if (!targetConstant)
resSpin.angle = timeRatio*startSpin.angle + (1.0f-timeRatio)*endSpin;
}
float3 MUST_USE_RESULT Quat::SlerpVector(const float3 &from, const float3 &to, float t)
{
if (t <= 0.f)
return from;
if (t >= 1.f)
return to;
///\todo The following chain can be greatly optimized.
Quat q = Quat::RotateFromTo(from, to);
q = Slerp(Quat::identity, q, t);
return q.Transform(from);
}
float3 MUST_USE_RESULT Quat::SlerpVectorAbs(const float3 &from, const float3 &to, float angleRadians)
{
if (angleRadians <= 0.f)
return from;
Quat q = Quat::RotateFromTo(from, to);
float a = q.Angle();
if (a <= angleRadians)
return to;
float t = angleRadians / a;
q = Slerp(Quat::identity, q, t);
return q.Transform(from);
}
glm::fquat GetOrient(const glm::fquat &initial, bool bSlerp) const
{
if(bSlerp)
{
return Slerp(initial, g_Orients[m_ixFinalOrient], m_currTimer.GetAlpha());
}
else
{
return Lerp(initial, g_Orients[m_ixFinalOrient], m_currTimer.GetAlpha());
}
return initial;
}
// Spherical linear interpolation of two quaternion-transforms q0 and q1, with parameter t,
// result in qt. Only the quaternion uses slerp, position and scaling use lerp
// Non-member function
void Slerp
(
const CQuatTransform& q0,
const CQuatTransform& q1,
const TFloat32 t,
CQuatTransform& qt
)
{
// Calculate lerp for position and scale
qt.pos = q0.pos*(1.0f-t) + q1.pos*t;
qt.scale = q0.scale*(1.0f-t) + q1.scale*t;
// Call slerp function for quaternion rotation
Slerp( q0.quat, q1.quat, t, qt.quat );
}
template<> void TUNDRACORE_API Attribute<Transform>::Interpolate(IAttribute* start, IAttribute* end, float t, AttributeChange::Type change)
{
Attribute<Transform>* startTrans = dynamic_cast<Attribute<Transform>*>(start);
Attribute<Transform>* endTrans = dynamic_cast<Attribute<Transform>*>(end);
if (startTrans && endTrans)
{
const Transform& startValue = startTrans->Get();
const Transform& endValue = endTrans->Get();
Transform newTrans;
newTrans.pos = Lerp(startValue.pos, endValue.pos, t);
newTrans.SetOrientation(Slerp(startValue.Orientation(), endValue.Orientation(), t));
newTrans.scale = Lerp(startValue.scale, endValue.scale, t);
Set(newTrans, change);
}
}
// function for SLERP quaternion
void Interpolator::LinearInterpolationQuaternion(Motion * pInputMotion, Motion * pOutputMotion, int N)
{
int inputLength = pInputMotion->GetNumFrames(); // frames are indexed 0, ..., inputLength-1
int startKeyframe = 0;
while (startKeyframe + N + 1 < inputLength)
{
int endKeyframe = startKeyframe + N + 1;
Posture * startPosture = pInputMotion->GetPosture(startKeyframe);
Posture * endPosture = pInputMotion->GetPosture(endKeyframe);
// copy start and end keyframe
pOutputMotion->SetPosture(startKeyframe, *startPosture);
pOutputMotion->SetPosture(endKeyframe, *endPosture);
// interpolate in between
for(int frame=1; frame<=N; frame++)
{
Posture interpolatedPosture;
double t = 1.0 * frame / (N+1);
// interpolate root position
interpolatedPosture.root_pos = startPosture->root_pos * (1-t) + endPosture->root_pos * t;
// interpolate bone rotations
for (int bone = 0; bone < MAX_BONES_IN_ASF_FILE; bone++)
{
// convert euler angles to quaternions
Quaternion<double> boneStartRot = Vector2Quaternion(startPosture->bone_rotation[bone]);
Quaternion<double> boneEndRot = Vector2Quaternion(endPosture->bone_rotation[bone]);
// do slerp for converted quaternions at t
double interpolatedAngles[3];
Quaternion2Euler(Slerp(t, boneStartRot, boneEndRot), interpolatedAngles);
interpolatedPosture.bone_rotation[bone].setValue(interpolatedAngles);
}
pOutputMotion->SetPosture(startKeyframe + frame, interpolatedPosture);
}
startKeyframe = endKeyframe;
}
for(int frame=startKeyframe+1; frame<inputLength; frame++)
pOutputMotion->SetPosture(frame, *(pInputMotion->GetPosture(frame)));
}
// Interpolate fractional arc between two wgs84 vectors.
// This models the earths ellipsoid.
geolocation Slerper::GeoSlerp(double frac)
{
// Generate normalized ECEF vector
xyz slerp = Slerp(frac);
// Scale ecef unit vector so its roughly at earth surface
double interpaltitudestep = m_altitude_delta * frac;
slerp = slerp.scale(m_x0_magnitude + interpaltitudestep);
// Convert ecef back to wgs84
geolocation slerpgeo = slerp.togeolocation();
// Set linear interpolated altitude on slerped wgs84 vector
double interpaltitude = m_altitude_delta * frac;
slerpgeo.Alt() = m_g0.Alt() + interpaltitude;
return slerpgeo;
}
void Evaluate()
{
float frame = GetFrame()->GetValue();
int count = m_keys.GetCount();
if (frame <= m_keys[0].m_frame) {
//
// Before first key
//
// , add a SetRotate(const Quaternion&) to Matrix
Vector vec;
float angle = m_keys[0].m_quat.GetRotation(vec);
GetValueInternal().SetRotate(vec, angle);
} else if (frame >= m_keys[count - 1].m_frame) {
//
// After last key
//
Vector vec;
float angle = m_keys[count - 1].m_quat.GetRotation(vec);
GetValueInternal().SetRotate(vec, angle);
} else {
//
// Between two keys
//
int index = 0;
while (frame > m_keys[index + 1].m_frame) {
index++;
}
float interpolant = (frame - m_keys[index].m_frame) / (m_keys[index + 1].m_frame - m_keys[index].m_frame);
Quaternion quat = Slerp(m_keys[index].m_quat, m_keys[index + 1].m_quat, interpolant);
Vector vec;
float angle = quat.GetRotation(vec);
GetValueInternal().SetRotate(vec, angle);
}
}
void TAnimContainer::PrepareModel()
{ // tutaj zostawi� tylko ustawienie submodelu, przeliczanie ma by� w UpdateModel()
if (pSubModel) // pozby� si� tego - sprawdza� wcze�niej
{
// nanoszenie animacji na wzorzec
if (iAnim & 1) // zmieniona pozycja wzgl�dem pocz�tkowej
pSubModel->SetRotateXYZ(vRotateAngles); // ustawia typ animacji
if (iAnim & 2) // zmieniona pozycja wzgl�dem pocz�tkowej
pSubModel->SetTranslate(vTranslation);
if (iAnim & 4) // zmieniona pozycja wzgl�dem pocz�tkowej
{
if (fAngleSpeed > 0.0f)
{
fAngleCurrent +=
fAngleSpeed * Timer::GetDeltaTime(); // aktualny parametr interpolacji
if (fAngleCurrent >= 1.0f)
{ // interpolacja zako�czona, ustawienie na pozycj� ko�cow�
qCurrent = qDesired;
fAngleSpeed = 0.0; // wy��czenie przeliczania wektora
if (evDone)
Global::AddToQuery(evDone,
NULL); // wykonanie eventu informuj�cego o zako�czeniu
}
else
{ // obliczanie pozycji po�redniej
// normalizacja jest wymagana do interpolacji w nast�pnej animacji
qCurrent = Normalize(
Slerp(qStart, qDesired, fAngleCurrent)); // interpolacja sferyczna k�ta
// qCurrent=Slerp(qStart,qDesired,fAngleCurrent); //interpolacja sferyczna k�ta
if (qCurrent.w ==
1.0) // rozpozna� brak obrotu i wy��czy� w iAnim w takim przypadku
iAnim &= ~4; // k�ty s� zerowe
}
}
mAnim->Quaternion(&qCurrent); // wype�nienie macierzy (wymaga normalizacji?)
pSubModel->mAnimMatrix = mAnim; // u�yczenie do submodelu (na czas renderowania!)
}
}
// if (!strcmp(pSubModel->pName,"?Z?�?^?[")) //jak g��wna ko��
// WriteLog(AnsiString(pMovementData->iFrame)+": "+AnsiString(iAnim)+"
// "+AnsiString(vTranslation.x)+" "+AnsiString(vTranslation.y)+" "+AnsiString(vTranslation.z));
}
// Decasteljau Quaternion formula to find value at t
Quaternion<double> Interpolator::DeCasteljauQuaternion(double t, Quaternion<double> p0, Quaternion<double> p1, Quaternion<double> p2, Quaternion<double> p3)
{
// students should implement this
Quaternion<double> result;
Quaternion<double> q0 = Slerp(t, p0, p1);
Quaternion<double> q1 = Slerp(t, p1, p2);
Quaternion<double> q2 = Slerp(t, p2, p3);
Quaternion<double> r0 = Slerp(t, q0, q1);
Quaternion<double> r1 = Slerp(t, q1, q2);
result = Slerp(t, r0, r1);
return result;
}
CQuaternion DeCasteljau(CQuaternion const& q1, CQuaternion const& q2,CQuaternion const& q3, CQuaternion const& q4,float t)
{
// simplified version
// CQuaternion pp = Slerp(Slerp(q1,q4,t),Slerp(q2,q3,t),2*t*(1-t));
CQuaternion p1,p2,p3,p12,p23,p;
p1 = Slerp(q1,q2,t);
p2 = Slerp(q2,q3,t);
p3 = Slerp(q3,q4,t);
p12 = Slerp(p1,p2,t);
p23 = Slerp(p2,p3,t);
p= Slerp(p12,p23,t);
return p;
}
void OrientConstRotation::Update(TimeValue t){
Interval iv = FOREVER;
ivalid = FOREVER;
int ct = pblock->Count(orientation_target_list);
int ctf = pblock->Count(orientation_target_weight);
float total_orient_target_weight_prev = 0.0f, total_orient_target_weight_current = 0.0f;
float orient_targ_Wt_current = 0.0f;
Quat quat_prev, quat_current, lCurRot;
INode *orient_target_prev= NULL, *orient_target_current= NULL;
Matrix3 targetTM(1);
Point3 trans, scaleP;
quat_prev.Identity();
quat_current.Identity();
lCurRot.Identity();
if (ct == 1){
pblock->GetValue(orientation_target_list, t, orient_target_prev, iv, 0);
pblock->GetValue(orientation_target_weight, t, orient_targ_Wt_current, iv, 0);
ivalid &= iv;
if (orient_target_prev == NULL){
targetTM.IdentityMatrix();
}
else {
targetTM = orient_target_prev->GetNodeTM(t, &ivalid);
}
if (IsLocal() && orient_target_prev != NULL) {
targetTM = targetTM * Inverse(orient_target_prev->GetParentTM(t));
}
AffineParts comps; // Requires header decomp.h
decomp_affine(targetTM, &comps);
quat_current = comps.q;
quat_current.Normalize();
quat_current.MakeClosest(quat_prev);
lCurRot = quat_current;
quat_prev = lCurRot;
total_orient_target_weight_prev += orient_targ_Wt_current;
// }
}
else if (ct > 1){
pblock->GetValue(orientation_target_list, t, orient_target_prev, iv, 0);
pblock->GetValue(orientation_target_weight, t, orient_targ_Wt_current, iv, 0);
// if (orient_target_prev != NULL)
// {
ivalid &= iv;
if (orient_target_prev == NULL){
targetTM.IdentityMatrix();
}
else {
targetTM = orient_target_prev->GetNodeTM(t, &ivalid);
}
if (IsLocal() && orient_target_prev != NULL) {
targetTM = targetTM * Inverse(orient_target_prev->GetParentTM(t));
}
AffineParts comps; // Requires header decomp.h
decomp_affine(targetTM, &comps);
quat_current = comps.q;
quat_current.Normalize();
quat_current.MakeClosest(quat_prev);
lCurRot = quat_current;
quat_prev = lCurRot;
total_orient_target_weight_prev += orient_targ_Wt_current;
// }
for (int i = 0; i < ct -1; i++) {
// ct = pblock->Count(orientation_target_list);
pblock->GetValue(orientation_target_list, t, orient_target_current, iv, i+1);
pblock->GetValue(orientation_target_weight, t, orient_targ_Wt_current, iv, i+1);
// if (orient_target_current != NULL){
ivalid &= iv;
if (orient_target_current == NULL){
targetTM.IdentityMatrix();
}
else {
targetTM = orient_target_current->GetNodeTM(t, &ivalid);
}
// Matrix3 targetTM = orient_target_current->GetNodeTM(t, &ivalid);
if (IsLocal() && orient_target_current != NULL) {
targetTM = targetTM * Inverse(orient_target_current->GetParentTM(t));
}
AffineParts comps; // Requires header decomp.h
decomp_affine(targetTM, &comps);
quat_current = comps.q;
quat_current.Normalize();
quat_current.MakeClosest(quat_prev);
float slerp_wt = 0.0f;
if ((total_orient_target_weight_prev + orient_targ_Wt_current) != 0.0){
slerp_wt = orient_targ_Wt_current / (total_orient_target_weight_prev + orient_targ_Wt_current);
}
lCurRot = Slerp(quat_prev, quat_current, slerp_wt);
lCurRot.Normalize();
quat_prev = lCurRot;
total_orient_target_weight_prev += orient_targ_Wt_current;
// }
} //for (int i = 0; i < ct -1; i++)
} //else if (ct > 1)
if (oldTargetNumber != ct) {
InitialOrientQuat = lCurRot;
}
curRot = lCurRot;
if (total_orient_target_weight_prev > 0.0){
if (Relative()){
if(IsLocal()){
curRot = baseRotQuatLocal * (lCurRot /InitialOrientQuat);
}
else{
curRot = baseRotQuatWorld * (lCurRot /InitialOrientQuat);
}
}
}
else {
curRot = baseRotQuatLocal;
}
curRot.MakeClosest(IdentQuat());
curRot.Normalize();
oldTargetNumber = ct;
// if (ivalid.Empty()) ivalid.SetInstant(t);
}
//-----------------------------------------------------------------------
Quaternion Quaternion::Slerp(const Quaternion& rkP,
const Quaternion& rkQ, Real fT)
{
return Slerp(fT, rkP, rkQ, false);
}
// not 'static', because used in ExportPsa()
void CAnimTrack::GetBonePosition(float Frame, float NumFrames, bool Loop, CVec3 &DstPos, CQuat &DstQuat) const
{
guard(CAnimTrack::GetBonePosition);
// fast case: 1 frame only
if (KeyTime.Num() == 1 || NumFrames == 1 || Frame == 0)
{
if (KeyPos.Num()) DstPos = KeyPos[0];
if (KeyQuat.Num()) DstQuat = KeyQuat[0];
return;
}
// data for lerping
int posX, rotX; // index of previous frame
int posY, rotY; // index of next frame
float posF, rotF; // fraction between X and Y for lerping
int NumTimeKeys = KeyTime.Num();
int NumPosKeys = KeyPos.Num();
int NumRotKeys = KeyQuat.Num();
if (NumTimeKeys)
{
// here: KeyPos and KeyQuat sizes either equals to 1 or equals to KeyTime size
assert(NumPosKeys <= 1 || NumPosKeys == NumTimeKeys);
assert(NumRotKeys == 1 || NumRotKeys == NumTimeKeys);
GetKeyParams(KeyTime, Frame, NumFrames, Loop, posX, posY, posF);
rotX = posX;
rotY = posY;
rotF = posF;
if (NumPosKeys <= 1)
{
posX = posY = 0;
posF = 0;
}
if (NumRotKeys == 1)
{
rotX = rotY = 0;
rotF = 0;
}
}
else
{
// empty KeyTime array - keys are evenly spaced on a time line
// note: KeyPos and KeyQuat sizes can be different
if (KeyPosTime.Num())
{
GetKeyParams(KeyPosTime, Frame, NumFrames, Loop, posX, posY, posF);
}
else if (NumPosKeys > 1)
{
float Position = Frame / NumFrames * NumPosKeys;
posX = appFloor(Position);
posF = Position - posX;
posY = posX + 1;
if (posY >= NumPosKeys)
{
if (!Loop)
{
posY = NumPosKeys - 1;
posF = 0;
}
else
posY = 0;
}
}
else
{
posX = posY = 0;
posF = 0;
}
if (KeyQuatTime.Num())
{
GetKeyParams(KeyQuatTime, Frame, NumFrames, Loop, rotX, rotY, rotF);
}
else if (NumRotKeys > 1)
{
float Position = Frame / NumFrames * NumRotKeys;
rotX = appFloor(Position);
rotF = Position - rotX;
rotY = rotX + 1;
if (rotY >= NumRotKeys)
{
if (!Loop)
{
rotY = NumRotKeys - 1;
rotF = 0;
}
else
rotY = 0;
}
}
else
{
rotX = rotY = 0;
rotF = 0;
}
}
// get position
if (posF > 0)
Lerp(KeyPos[posX], KeyPos[posY], posF, DstPos);
else if (NumPosKeys) // do not change DstPos when no keys
DstPos = KeyPos[posX];
// get orientation
if (rotF > 0)
Slerp(KeyQuat[rotX], KeyQuat[rotY], rotF, DstQuat);
else if (NumRotKeys) // do not change DstQuat when no keys
DstQuat = KeyQuat[rotX];
unguard;
}
SkelMat4 skelBone_HoseRot::GetDirtyTransform( SkelMat4& myParentTM, SkelMat4& targetTM )
{
SkelMat4 m;
SkelMat4 invParentTM;
SkelMat4 temp;
SkelVec3 rotaxis;
SkelVec3 targetup, targetaim;
SkelVec3 upaxis, aimaxis;
SkelVec3 tmp;
SkelQuat targetQuat;
float l, s, c;
float angle, vScale;
aimaxis = myParentTM[ 0 ];
targetaim = targetTM[ 0 ];
CrossProduct( targetaim, aimaxis, rotaxis );
s = VectorLengthSquared( rotaxis );
if( s == 0.0 ) {
rotaxis.x = 1.0;
} else if( s != 1.0 ) {
l = 1.0 / sqrt( s );
VectorScale( rotaxis, l, rotaxis );
}
s = DotProduct( aimaxis, targetaim );
if( s < 1.0 ) {
if( s > -0.999 ) {
angle = acos( s );
} else {
angle = 6.2831855f;
}
} else {
angle = 0.0;
}
vScale = angle * m_bendRatio;
if( vScale > m_bendMax ) {
vScale = m_bendMax;
}
temp[ 0 ][ 0 ] = myParentTM[ 0 ][ 0 ];
temp[ 0 ][ 1 ] = myParentTM[ 1 ][ 0 ];
temp[ 0 ][ 2 ] = myParentTM[ 2 ][ 0 ];
temp[ 1 ][ 0 ] = myParentTM[ 0 ][ 1 ];
temp[ 1 ][ 1 ] = myParentTM[ 1 ][ 1 ];
temp[ 1 ][ 2 ] = myParentTM[ 2 ][ 1 ];
temp[ 2 ][ 0 ] = myParentTM[ 0 ][ 2 ];
temp[ 2 ][ 1 ] = myParentTM[ 1 ][ 2 ];
temp[ 2 ][ 2 ] = myParentTM[ 2 ][ 2 ];
temp[ 3 ][ 0 ] = -( myParentTM[ 0 ][ 0 ] * myParentTM[ 3 ][ 0 ] + myParentTM[ 0 ][ 1 ] * myParentTM[ 3 ][ 1 ] + myParentTM[ 0 ][ 2 ] * myParentTM[ 3 ][ 2 ] );
temp[ 3 ][ 1 ] = -( myParentTM[ 1 ][ 0 ] * myParentTM[ 3 ][ 0 ] + myParentTM[ 1 ][ 1 ] * myParentTM[ 3 ][ 1 ] + myParentTM[ 1 ][ 2 ] * myParentTM[ 3 ][ 2 ] );
temp[ 3 ][ 2 ] = -( myParentTM[ 2 ][ 0 ] * myParentTM[ 3 ][ 0 ] + myParentTM[ 2 ][ 1 ] * myParentTM[ 3 ][ 1 ] + myParentTM[ 2 ][ 2 ] * myParentTM[ 3 ][ 2 ] );
m.Multiply( temp, myParentTM );
VectorCopy( rotaxis, tmp );
rotaxis[ 0 ] = tmp[ 0 ] * temp[ 0 ][ 0 ] + tmp[ 1 ] * temp[ 1 ][ 0 ] + tmp[ 2 ] * temp[ 2 ][ 0 ];
rotaxis[ 1 ] = tmp[ 0 ] * temp[ 0 ][ 1 ] + tmp[ 1 ] * temp[ 1 ][ 1 ] + tmp[ 2 ] * temp[ 2 ][ 1 ];
rotaxis[ 2 ] = tmp[ 0 ] * temp[ 0 ][ 2 ] + tmp[ 1 ] * temp[ 1 ][ 2 ] + tmp[ 2 ] * temp[ 2 ][ 2 ];
targetaim.y = cos( vScale * 0.5 );
targetup.y = rotaxis.x * targetaim.y;
targetup.z = rotaxis.y * targetaim.y;
targetaim.x = rotaxis.z * targetaim.y;
c = cos( vScale * 0.5 );
l = sqrt( 1.0 - c * c );
m_cachedQuat[ 0 ] = rotaxis[ 0 ] * l;
m_cachedQuat[ 1 ] = rotaxis[ 1 ] * l;
m_cachedQuat[ 2 ] = rotaxis[ 2 ] * l;
m_cachedQuat[ 3 ] = c;
if( m_spinRatio < 1.0 )
{
m.Multiply( targetTM, temp );
MatToQuat( m.val, targetQuat.val );
Slerp( targetQuat, m_cachedQuat, m_spinRatio, &m_cachedQuat );
}
m_cachedQuat.GetMat4( m_cachedValue );
VectorCopy( m_basePos, m_cachedValue[ 3 ] );
m.Multiply( m_cachedValue, myParentTM );
m_cachedValue = m;
m_isDirty = false;
return m_cachedValue;
}
//-------------------------------------------------------------------------------
// @ Player::Update()
//-------------------------------------------------------------------------------
// Main update loop
//-------------------------------------------------------------------------------
void
Player::Update( float dt )
{
IvMatrix33 rotateX, rotateY, rotateZ;
IvMatrix33 rotate;
float heading = 0.0f, pitch = 0.0f, roll = 0.0f;
// clear transform
if (IvGame::mGame->mEventHandler->IsKeyPressed(' '))
{
if (mRun)
{
// reset animations
mTime = 0.0f;
mRun = false;
heading = mStartHeading;
pitch = mStartPitch;
roll = mStartRoll;
mSlerpRotate = mStartRotate;
mNumPoints = 0;
mRotate.Rotation( heading, pitch, roll );
}
else
{
// start 'er up again
mRun = true;
}
}
// swap orientation reps
if (IvGame::mGame->mEventHandler->IsKeyPressed('x'))
{
mUseQuat = !mUseQuat;
mNumPoints = 0;
}
if (mRun)
{
// get the next time step
mTime += dt;
if ( mTime > 8.0f )
mTime = 8.0f;
float alpha = mTime/8.0f;
// interpolate
heading = mStartHeading*(1.0f - alpha) + mEndHeading*alpha;
pitch = mStartPitch*(1.0f - alpha) + mEndPitch*alpha;
roll = mStartRoll*(1.0f - alpha) + mEndRoll*alpha;
Slerp(mSlerpRotate, mStartRotate, mEndRotate, alpha);
// set rotation matrix based on current orientation rep
if (mUseQuat)
{
mRotate.Rotation(mSlerpRotate);
}
else
{
mRotate.Rotation( heading, pitch, roll );
}
// add new path points
if (mNumPoints < 1280 && mFrameCounter == 0)
{
mStoredPoints[0][mNumPoints] = mRotate*IvVector3(1.0f, -1.0f, 1.0f);
mStoredPoints[1][mNumPoints++] = mRotate*IvVector3(-1.0f, 0.0f, -1.0f);
}
mFrameCounter = (mFrameCounter + 1) % 10;
}
} // End of Player::Update()
// function for SLERP bezier quaternion interpolation
void Interpolator::BezierInterpolationQuaternion(Motion * pInputMotion, Motion * pOutputMotion, int N)
{
// students should implement this
int inputLength = pInputMotion->GetNumFrames(); // frames are indexed 0, ..., inputLength-1
int startKeyframe = 0;
// default and next 'a' values for bezier are stored in memory
Quaternion<double> aDef[MAX_BONES_IN_ASF_FILE];
Quaternion<double> aNext[MAX_BONES_IN_ASF_FILE];
double bezRatio = 1.0/3.0;
vector aNextRoot;
// use a0 for the first frame by backtracking from next 2 known frames
vector v1 = pInputMotion->GetPosture(0)->root_pos;
vector v2 = pInputMotion->GetPosture(N+1)->root_pos;
vector v3 = pInputMotion->GetPosture(N+1+N+1)->root_pos;
vector aDefRoot = v1*(1-bezRatio) + (v3 * (-1) + v2 * 2) * bezRatio;
for (int i = 0; i < MAX_BONES_IN_ASF_FILE; i++)
{
Quaternion<double> q1 = Vector2Quaternion(pInputMotion->GetPosture(0)->bone_rotation[i]);
Quaternion<double> q2 = Vector2Quaternion(pInputMotion->GetPosture(N+1)->bone_rotation[i]);
Quaternion<double> q3 = Vector2Quaternion(pInputMotion->GetPosture(N+1+N+1)->bone_rotation[i]);
aDef[i] = Slerp(bezRatio, q1, Slerp(2.0, q3, q2));
}
while (startKeyframe + N + 1 < inputLength)
{
int endKeyframe = startKeyframe + N + 1;
Posture * startPosture = pInputMotion->GetPosture(startKeyframe);
Posture * endPosture = pInputMotion->GetPosture(endKeyframe);
Posture * nextPosture;
if (endKeyframe + N + 1 >= inputLength) nextPosture = pInputMotion->GetPosture(inputLength-1);
else nextPosture = pInputMotion->GetPosture(endKeyframe + (N+1));
// copy start and end keyframe
pOutputMotion->SetPosture(startKeyframe, *startPosture);
pOutputMotion->SetPosture(endKeyframe, *endPosture);
// interpolate in between
for(int frame=1; frame<=N; frame++)
{
Posture interpolatedPosture;
double t = 1.0 * frame / (N+1);
// bez interpolate root position
vector rootStartPos = startPosture->root_pos;
vector rootEndPos = endPosture->root_pos;
vector rootNextPos = nextPosture->root_pos;
vector p1 = startKeyframe == 0? aDefRoot : aNextRoot; // use a0 if first frame, otherwise use a calculated in previous iteration
vector aN = rootEndPos*(1-bezRatio) + ((rootStartPos*-1+ rootEndPos*2)*0.5+ rootNextPos*0.5)*bezRatio;
vector p2 = aN*-1+rootEndPos*2; // bn
interpolatedPosture.root_pos = DeCasteljauEuler(t, rootStartPos, p1, p2, rootEndPos);
// interpolate bone rotations
for (int bone = 0; bone < MAX_BONES_IN_ASF_FILE; bone++)
{
Quaternion<double> boneStartRot = Vector2Quaternion(startPosture->bone_rotation[bone]); // qn
Quaternion<double> boneEndRot = Vector2Quaternion(endPosture->bone_rotation[bone]); // qn+1
Quaternion<double> boneNextRot = Vector2Quaternion(nextPosture->bone_rotation[bone]); // qn+2
Quaternion<double> p1 = startKeyframe == 0? aDef[bone] : aNext[bone]; // use a0 if first frame, otherwise use a calculated in previous iteration
Quaternion<double> aBar = Slerp(0.5, Slerp(2.0, boneStartRot, boneEndRot), boneNextRot);
Quaternion<double> p2 = Slerp(-bezRatio, boneEndRot, aBar);
double interpolatedAngles[3];
Quaternion2Euler(DeCasteljauQuaternion(t, boneStartRot, p1, p2, boneEndRot), interpolatedAngles);
interpolatedPosture.bone_rotation[bone].setValue(interpolatedAngles);
}
pOutputMotion->SetPosture(startKeyframe + frame, interpolatedPosture);
}
// set the aN values for the next N frames (bezier)
vector rootStartPos = startPosture->root_pos;
vector rootEndPos = endPosture->root_pos;
vector rootNextPos = nextPosture->root_pos;
aNextRoot = rootEndPos*(1-bezRatio) + ((rootStartPos*-1+ rootEndPos*2)*0.5+ rootNextPos*0.5)*bezRatio;
for (int bone = 0; bone < MAX_BONES_IN_ASF_FILE; bone++)
{
Quaternion<double> boneStartRot = Vector2Quaternion(startPosture->bone_rotation[bone]); // qn
Quaternion<double> boneEndRot = Vector2Quaternion(endPosture->bone_rotation[bone]); // qn+1
Quaternion<double> boneNextRot = Vector2Quaternion(nextPosture->bone_rotation[bone]); // qn+2
aNext[bone] = Slerp(bezRatio, boneEndRot, Slerp(0.5, Slerp(2.0, boneStartRot, boneEndRot), boneNextRot)); // an+1
}
startKeyframe = endKeyframe;
}
for(int frame=startKeyframe+1; frame<inputLength; frame++)
pOutputMotion->SetPosture(frame, *(pInputMotion->GetPosture(frame)));
}