float TKnot::RombergIntegral(float fA, float fB)
{
float fH = fB - fA;
const int ms_iOrder = 5;
float ms_apfRom[2][ms_iOrder];
ms_apfRom[0][0] =
0.5 * fH * ((GetFirstDerivative(fA).Length()) + (GetFirstDerivative(fB).Length()));
for (int i0 = 2, iP0 = 1; i0 <= ms_iOrder; i0++, iP0 *= 2, fH *= 0.5)
{
// approximations via the trapezoid rule
float fSum = 0.0;
int i1;
for (i1 = 1; i1 <= iP0; i1++)
fSum += (GetFirstDerivative(fA + fH * (i1 - 0.5)).Length());
// Richardson extrapolation
ms_apfRom[1][0] = 0.5 * (ms_apfRom[0][0] + fH * fSum);
for (int i2 = 1, iP2 = 4; i2 < i0; i2++, iP2 *= 4)
{
ms_apfRom[1][i2] = (iP2 * ms_apfRom[1][i2 - 1] - ms_apfRom[0][i2 - 1]) / (iP2 - 1);
}
for (i1 = 0; i1 < i0; i1++)
ms_apfRom[0][i1] = ms_apfRom[1][i1];
}
return ms_apfRom[0][ms_iOrder - 1];
}
float TKnot::GetTFromS(float s)
{
// initial guess for Newton's method
int it = 0;
float fTolerance = 0.001;
float fRatio = s / RombergIntegral(0, 1);
float fOmRatio = 1.0 - fRatio;
float fTime = fOmRatio * 0 + fRatio * 1;
// for (int i = 0; i < iIterations; i++)
while (true)
{
it++;
if (it > 10)
MessageBox(0, "Too many iterations", "TSpline->GetTFromS", MB_OK);
float fDifference = RombergIntegral(0, fTime) - s;
if ((fDifference > 0 ? fDifference : -fDifference) < fTolerance)
return fTime;
fTime -= fDifference / GetFirstDerivative(fTime).Length();
}
// Newton's method failed. If this happens, increase iterations or
// tolerance or integration accuracy.
return -1;
}
Vector3<Real> Curve3<Real>::GetNormal ( Real t ) const
{
Vector3<Real> velocity = GetFirstDerivative( t );
Vector3<Real> acceleration = GetSecondDerivative( t );
Real VDotV = velocity.Dot( velocity );
Real VDotA = velocity.Dot( acceleration );
Vector3<Real> normal = VDotV * acceleration - VDotA * velocity;
normal.Normalize();
return normal;
}
Vector3 Curve<Real>::GetNormal (Real t)
{
Vector3 velocity = GetFirstDerivative(t);
Vector3 acceleration = GetSecondDerivative(t);
Real VDotV = dot(velocity,velocity);
Real VDotA = dot(velocity,acceleration);
Vector3 normal = VDotV*acceleration - VDotA*velocity;
normal.normalize();
return normal;
}
void Curve3<Real>::GetFrame ( Real t, Vector3<Real>& position,
Vector3<Real>& tangent, Vector3<Real>& normal, Vector3<Real>& binormal )
const
{
position = GetPosition( t );
Vector3<Real> velocity = GetFirstDerivative( t );
Vector3<Real> acceleration = GetSecondDerivative( t );
Real VDotV = velocity.Dot( velocity );
Real VDotA = velocity.Dot( acceleration );
normal = VDotV * acceleration - VDotA * velocity;
normal.Normalize();
tangent = velocity;
tangent.Normalize();
binormal = tangent.Cross( normal );
}
void Curve<Real>::GetFrame (Real t, Vector3& position,
Vector3& tangent, Vector3& normal, Vector3& binormal)
{
position = GetPosition(t);
Vector3 velocity = GetFirstDerivative(t);
Vector3 acceleration = GetSecondDerivative(t);
Real VDotV = dot(velocity,velocity);
Real VDotA = dot(velocity,acceleration);
normal = VDotV*acceleration - VDotA*velocity;
normal.normalize();
tangent = velocity;
tangent.normalize();
binormal = tangent.cross(normal);
}
Real Curve3<Real>::GetTorsion ( Real t ) const
{
Vector3<Real> velocity = GetFirstDerivative( t );
Vector3<Real> acceleration = GetSecondDerivative( t );
Vector3<Real> cross = velocity.Cross( acceleration );
Real denom = cross.SquaredLength();
if ( denom >= Math<Real>::ZERO_TOLERANCE )
{
Vector3<Real> jerk = GetThirdDerivative( t );
Real numer = cross.Dot( jerk );
return numer / denom;
}
else
{
// Torsion is indeterminate, just return 0.
return ( Real )0;
}
}
Real Curve3<Real>::GetCurvature ( Real t ) const
{
Vector3<Real> velocity = GetFirstDerivative( t );
Real speedSqr = velocity.SquaredLength();
if ( speedSqr >= Math<Real>::ZERO_TOLERANCE )
{
Vector3<Real> acceleration = GetSecondDerivative( t );
Vector3<Real> cross = velocity.Cross( acceleration );
Real numer = cross.Length();
Real denom = Math<Real>::Pow( speedSqr, ( Real )1.5 );
return numer / denom;
}
else
{
// Curvature is indeterminate, just return 0.
return ( Real )0;
}
}
Real Curve<Real>::GetTorsion (Real t)
{
Vector3 velocity = GetFirstDerivative(t);
Vector3 acceleration = GetSecondDerivative(t);
Vector3 _cross = velocity.cross(acceleration);
Real denom = _cross.squaredNorm();
if (denom >= REAL_ZERO_TOLERANCE)
{
Vector3 jerk = GetThirdDerivative(t);
Real numer = dot(_cross,jerk);
return numer/denom;
}
else
{
// Torsion is indeterminate, just return 0.
return (Real)0;
}
}
Real Curve<Real>::GetCurvature (Real t)
{
Vector3 velocity = GetFirstDerivative(t);
Real speedSqr = velocity.squaredNorm();
if (speedSqr >= REAL_ZERO_TOLERANCE)
{
Vector3 acceleration = GetSecondDerivative(t);
Vector3 _cross = velocity.cross(acceleration);
Real numer = _cross.norm();
Real denom = pow(speedSqr, (Real)1.5);
return numer/denom;
}
else
{
// Curvature is indeterminate, just return 0.
return (Real)0;
}
}
Vector3<Real> Curve3<Real>::GetTangent ( Real t ) const
{
Vector3<Real> velocity = GetFirstDerivative( t );
velocity.Normalize();
return velocity;
}
Real Curve3<Real>::GetSpeed ( Real t ) const
{
Vector3<Real> velocity = GetFirstDerivative( t );
Real speed = velocity.Length();
return speed;
}
Vector3 Curve<Real>::GetTangent (Real t)
{
Vector3 velocity = GetFirstDerivative(t);
velocity.normalize();
return velocity;
}
Real Curve<Real>::GetSpeed (Real t)
{
Vector3 velocity = GetFirstDerivative(t);
Real speed = velocity.norm();
return speed;
}
