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 AngleAxis(AngleAxis _aa)
{
Angle = _aa.Angle; Axis = _aa.Axis;
}
public Quat(AngleAxis _aa)
{
qs = (float)System.Math.Cos(0.5f * _aa.Angle);
qv = (float)System.Math.Sin(0.5f * _aa.Angle) * _aa.Axis;
}