UnitQuaternion&
UnitQuaternion::FastLerp(
const UnitQuaternion& p,
const UnitQuaternion& q,
Scalar t
)
{
if (!UseFastLerp)
return Lerp(p,q,t);
Start_Timer(SlerpTime);
Set_Statistic(SlerpCount, SlerpCount+1);
Verify(quaternionFastLerpTableBuilt);
Scalar cosom,sclp,sclq;
cosom = p.x*q.x + p.y*q.y + p.z*q.z + p.w*q.w;
if ( (1.0f + cosom) > 0.01f)
{
// usual case
if ( (1.0f - cosom) > 0.00001f )
{
//usual case
//table_entry = (int)Scaled_Float_To_Bits(cosom, MinCosom, MaxCosom, 10);
float tabled_float = cosom - MinCosom;
int cos_table_entry = Truncate_Float_To_Word(((tabled_float*CosomRangeOverOne) * CosBiggestNumber));
Verify(cos_table_entry >= 0);
Verify(cos_table_entry <= QuaternionLerpTableSize);
#if 0
sclp = Sin((1.0f - t)*Omega_Table[cos_table_entry]) * SinomOverOne_Table[cos_table_entry];
sclq = Sin(t*Omega_Table[cos_table_entry]) * SinomOverOne_Table[cos_table_entry];
#else
float difference, percent, lerped_sin;
tabled_float = ((1.0f - t)*Omega_Table[cos_table_entry]) - MinSin;
int sclp_table_entry = Truncate_Float_To_Word(((tabled_float*SinRangeOverOne) * SinBiggestNumber));
if (!(sclp_table_entry < SinTableSize))
{
Max_Clamp(sclp_table_entry, SinTableSize-1);
}
Verify(sclp_table_entry >= 0 && sclp_table_entry < SinTableSize);
difference = tabled_float - (SinIncrement * sclp_table_entry);
percent = difference / SinIncrement;
int lerp_to_entry = sclp_table_entry + 1;
Max_Clamp(lerp_to_entry, SinTableSize-1);
lerped_sin = Stuff::Lerp(Sin_Table[sclp_table_entry], Sin_Table[lerp_to_entry], percent);
sclp = lerped_sin * SinomOverOne_Table[cos_table_entry];
tabled_float = (t*Omega_Table[cos_table_entry]) - MinSin;
int sclq_table_entry = Truncate_Float_To_Word(((tabled_float*SinRangeOverOne) * SinBiggestNumber));
Verify(sclq_table_entry >= 0 && sclq_table_entry < SinTableSize);
difference = tabled_float - (SinIncrement * sclq_table_entry);
percent = difference / SinIncrement;
lerp_to_entry = sclq_table_entry + 1;
Max_Clamp(lerp_to_entry, SinTableSize-1);
lerped_sin = Stuff::Lerp(Sin_Table[sclq_table_entry], Sin_Table[lerp_to_entry], percent);
sclq = lerped_sin * SinomOverOne_Table[cos_table_entry];
#endif
}
else
{
// ends very close -- just lerp
sclp = 1.0f - t;
sclq = t;
}
x = sclp*p.x + sclq*q.x;
y = sclp*p.y + sclq*q.y;
z = sclp*p.z + sclq*q.z;
w = sclp*p.w + sclq*q.w;
}
else
{
//SPEW(("jerryeds","SPECIAL CASE"));
/* p and q nearly opposite on sphere-- this is a 360 degree
rotation, but the axis of rotation is undefined, so
slerp really is undefined too. So this apparently picks
an arbitrary plane of rotation. However, I think this
code is incorrect.
*/
//really we want the shortest distance. They are almost on top of each other.
UnitQuaternion r;
r.Subtract(q, p);
Vector3D scaled_rotation;
scaled_rotation = r;
scaled_rotation *= t;
UnitQuaternion scaled_quat;
scaled_quat = scaled_rotation;
Multiply(scaled_quat, p);
Normalize();
}
Stop_Timer(SlerpTime);
return *this;
}
UnitQuaternion &UnitQuaternion::Lerp(const UnitQuaternion& p, const UnitQuaternion& q, Scalar t)
{
Start_Timer(SlerpTime);
Set_Statistic(SlerpCount, SlerpCount+1);
Scalar omega,cosom,sinom,sclp,sclq;
//UnitQuaternion qt;
//UnitQuaternion q = q_temp;
//UnitQuaternion p = p_temp;
cosom = p.x*q.x + p.y*q.y + p.z*q.z + p.w*q.w;
if ( (1.0f + cosom) > 0.01f)
{
// usual case
if ( (1.0f - cosom) > 0.00001f )
{
//usual case
omega = Arccos(cosom);
sinom = Sin(omega);
//SPEW(("jerryeds","omega:%f sinom:%f", omega, sinom));
sclp = Sin((1.0f - t)*omega) / sinom;
sclq = Sin(t*omega) / sinom;
//SPEW(("jerryeds", "* %f %f", sclp, sclq));
}
else
{
// ends very close -- just lerp
sclp = 1.0f - t;
sclq = t;
//SPEW(("jerryeds", "# %f %f", sclp, sclq));
}
x = sclp*p.x + sclq*q.x;
y = sclp*p.y + sclq*q.y;
z = sclp*p.z + sclq*q.z;
w = sclp*p.w + sclq*q.w;
//SPEW(("jerryeds", "r:<%f,%f,%f,%f>",x,y,z,w));
}
else
{
//SPEW(("jerryeds","SPECIAL CASE"));
/* p and q nearly opposite on sphere-- this is a 360 degree
rotation, but the axis of rotation is undefined, so
slerp really is undefined too. So this apparently picks
an arbitrary plane of rotation. However, I think this
code is incorrect.
*/
//really we want the shortest distance. They are almost on top of each other.
UnitQuaternion r;
r.Subtract(q, p);
Vector3D scaled_rotation;
scaled_rotation = r;
scaled_rotation *= t;
UnitQuaternion scaled_quat;
scaled_quat = scaled_rotation;
Multiply(scaled_quat, p);
}
Stop_Timer(SlerpTime);
return *this;
}