public AngleAxis( Quat _q )
{
Angle = (float) System.Math.Acos( _q.qs );
float fSine = (float) System.Math.Sin( Angle );
Angle *= 2.0f;
Quat Temp = new Quat( _q );
Temp.Normalize();
if ( System.Math.Abs( fSine ) > float.Epsilon )
Axis = Temp.qv / fSine;
else
Axis.Set( 0, 0, 0 );
}
public void Set( Quat _q )
{
qs = _q.qs; qv = _q.qv;
}
public void MakeSQUADRev( AngleAxis _aa, Quat _q0, Quat _q1, Quat _t0, Quat _t1, float _t )
{
float s, v;
float fOmega = _aa.Angle * 0.5f;
float fNbRevs = 0.0f;
Quat pp, qq;
if ( fOmega < System.Math.PI - float.Epsilon )
{
MakeSQUAD( _q0, _q1, _t0, _t1, _t );
return;
}
while ( fOmega > System.Math.PI - float.Epsilon )
{
fOmega -= (float) System.Math.PI;
fNbRevs += 1.0f;
}
if ( fOmega < 0.0f )
fOmega = 0.0f;
s = _t * _aa.Angle / (float) System.Math.PI; // 2t(omega + N.PI) / PI
if ( s < 1.0f )
{
pp = _q0;
pp.MakeOrtho( _aa.Axis );
MakeSQUAD( _q0, pp, _t0, pp, s ); // In first 90 degrees
}
else if ( ( v = s + 1.0f - 2.0f * (fNbRevs + ( fOmega / (float) System.Math.PI )) ) <= 0.0f )
{ // middle part, on great circle(p,q)
while ( s >= 2.0f )
s -= 2.0f;
pp = _q0;
pp.MakeOrtho( _aa.Axis );
MakeSLERP( _q0, pp, s );
}
else
{ // in last 90 degrees
qq = _q0;
qq.MakeOrtho( _aa.Axis );
qq = -qq;
MakeSQUAD( qq, _q1, qq, _t1, v );
}
}
public void MakeSQUAD( Quat _q0, Quat _q1, Quat _t0, Quat _t1, float _t )
{
Quat Slerp0 = new Quat();
Slerp0.MakeSLERP( _q0, _q1, _t );
Quat Slerp1 = new Quat();
Slerp1.MakeSLERP( _t0, _t1, _t );
MakeSLERP( Slerp0, Slerp1, 2.0f * _t * (1.0f - _t) );
}
public void MakeSLERP( Quat _q0, Quat _q1, float _t )
{
float fCosine = _q0 | _q1, fSine = (float) System.Math.Sqrt( 1.0f - fCosine*fCosine );
if ( fSine < float.Epsilon )
{ // Clamp to lowest bound
Set( _q0 );
return;
}
float fAngle = (float) System.Math.Atan2( fSine, fCosine ), fInvSine = 1.0f / fSine;
float c0 = (float) System.Math.Sin( (1-_t) * fAngle ) * fInvSine;
float c1 = (float) System.Math.Sin( _t * fAngle ) * fInvSine;
Set( c0 * _q0 + c1 * _q1 );
}
public void MakeShortestSLERP( Quat _q0, Quat _q1, float _t )
{
Quat ShortQ0 = new Quat( _q0 ); ShortQ0.MakeClosest( _q1 ); MakeSLERP( ShortQ0, _q1, _t );
}
public Quat( Quat _q )
{
qs = _q.qs; qv = _q.qv;
}
public void MakeShortestSLERP(Quat _q0, Quat _q1, float _t)
{
Quat ShortQ0 = new Quat(_q0); ShortQ0.MakeClosest(_q1); MakeSLERP(ShortQ0, _q1, _t);
} // Same as "MakeSlerp" except it makes a SLERP between 2 quaternions on the same hemisphere.
public void LnDiff( Quat _q )
{
Set( _q / this ); LogN();
}
public static Quat operator /( Quat _Op0, Quat _Op1 )
{
Quat InvQuat = new Quat( _Op1 ); InvQuat.Invert(); return InvQuat * _Op0;
}
public void Set(Quat _q)
{
qs = _q.qs; qv = _q.qv;
}
public Quat(Quat _q)
{
qs = _q.qs; qv = _q.qv;
}
public static Quat operator/(Quat _Op0, Quat _Op1)
{
Quat InvQuat = new Quat(_Op1); InvQuat.Invert(); return(InvQuat * _Op0);
}
public void MakeSQUADRev(AngleAxis _aa, Quat _q0, Quat _q1, Quat _t0, Quat _t1, float _t)
{
float s, v;
float fOmega = _aa.Angle * 0.5f;
float fNbRevs = 0.0f;
Quat pp, qq;
if (fOmega < System.Math.PI - float.Epsilon)
{
MakeSQUAD(_q0, _q1, _t0, _t1, _t);
return;
}
while (fOmega > System.Math.PI - float.Epsilon)
{
fOmega -= (float)System.Math.PI;
fNbRevs += 1.0f;
}
if (fOmega < 0.0f)
{
fOmega = 0.0f;
}
s = _t * _aa.Angle / (float)System.Math.PI; // 2t(omega + N.PI) / PI
if (s < 1.0f)
{
pp = _q0;
pp.MakeOrtho(_aa.Axis);
MakeSQUAD(_q0, pp, _t0, pp, s); // In first 90 degrees
}
else if ((v = s + 1.0f - 2.0f * (fNbRevs + (fOmega / (float)System.Math.PI))) <= 0.0f)
{ // middle part, on great circle(p,q)
while (s >= 2.0f)
{
s -= 2.0f;
}
pp = _q0;
pp.MakeOrtho(_aa.Axis);
MakeSLERP(_q0, pp, s);
}
else
{ // in last 90 degrees
qq = _q0;
qq.MakeOrtho(_aa.Axis);
qq = -qq;
MakeSQUAD(qq, _q1, qq, _t1, v);
}
}
} // Same as "MakeSlerp" except it makes a SLERP between 2 quaternions on the same hemisphere.
public void MakeSQUAD(Quat _q0, Quat _q1, Quat _t0, Quat _t1, float _t)
{
Quat Slerp0 = new Quat();
Slerp0.MakeSLERP(_q0, _q1, _t);
Quat Slerp1 = new Quat();
Slerp1.MakeSLERP(_t0, _t1, _t);
MakeSLERP(Slerp0, Slerp1, 2.0f * _t * (1.0f - _t));
}
public Quat SLERP( Quat _q, float _t )
{
float fCosine = this | _q, fSine = (float) System.Math.Sqrt( System.Math.Abs( 1.0f - fCosine*fCosine ) );
if ( System.Math.Abs( fSine ) < float.Epsilon )
return this;
float fAngle = (float) System.Math.Atan2( fSine, fCosine ), fInvSine = 1.0f / fSine;
float c0 = (float) System.Math.Sin( (1-_t) * fAngle) * fInvSine;
float c1 = (float) System.Math.Sin( _t * fAngle) * fInvSine;
return new Quat( c0 * (this) + c1 * _q );
}
public void MakeClosest( Quat _q )
{
if ( (this | _q) < 0.0f ) Set( -this );
}
public void LnDiff(Quat _q)
{
Set(_q / this); LogN();
}
