// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "SM_MathPCH.h"
#include "MgcMultipleCurve3.h"

//----------------------------------------------------------------------------
MgcMultipleCurve3::MgcMultipleCurve3 (int iSegments, float* afTime)
    :
    MgcMultipleCurve<Vector3D>(iSegments,afTime)
{
}
//----------------------------------------------------------------------------
Vector3D MgcMultipleCurve3::GetNormal (float fTime) const
{
    Vector3D kVelocity = GetFirstDerivative(fTime);
    Vector3D kAcceleration = GetSecondDerivative(fTime);
    float fVDotV = kVelocity.Dot(kVelocity);
    float fVDotA = kVelocity.Dot(kAcceleration);
    Vector3D kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
    kNormal.Normalize();
    return kNormal;
}
//----------------------------------------------------------------------------
Vector3D MgcMultipleCurve3::GetBinormal (float fTime) const
{
    Vector3D kVelocity = GetFirstDerivative(fTime);
    Vector3D kAcceleration = GetSecondDerivative(fTime);
    float fVDotV = kVelocity.Dot(kVelocity);
    float fVDotA = kVelocity.Dot(kAcceleration);
    Vector3D kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
    kNormal.Normalize();
    kVelocity.Normalize();
    Vector3D kBinormal = kVelocity.Cross(kNormal);
    return kBinormal;
}
//----------------------------------------------------------------------------
void MgcMultipleCurve3::GetFrame (float fTime, Vector3D& kPosition,
    Vector3D& kTangent, Vector3D& kNormal, Vector3D& kBinormal) const
{
    kPosition = GetPosition(fTime);
    Vector3D kVelocity = GetFirstDerivative(fTime);
    Vector3D kAcceleration = GetSecondDerivative(fTime);
    float fVDotV = kVelocity.Dot(kVelocity);
    float fVDotA = kVelocity.Dot(kAcceleration);
    kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
    kNormal.Normalize();
    kTangent = kVelocity;
    kTangent.Normalize();
    kBinormal = kTangent.Cross(kNormal);
}
//----------------------------------------------------------------------------
float MgcMultipleCurve3::GetCurvature (float fTime) const
{
    Vector3D kVelocity = GetFirstDerivative(fTime);
    float fSpeedSqr = kVelocity.SquaredLength();

    const float fTolerance = 1e-06f;
    if ( fSpeedSqr >= fTolerance )
    {
        Vector3D kAcceleration = GetSecondDerivative(fTime);
        Vector3D kCross = kVelocity.Cross(kAcceleration);
        float fNumer = kCross.Length();
        float fDenom = powf(fSpeedSqr,1.5f);
        return fNumer/fDenom;
    }
    else
    {
        // curvature is indeterminate, just return 0
        return 0.0;
    }
}
//----------------------------------------------------------------------------
float MgcMultipleCurve3::GetTorsion (float fTime) const
{
    Vector3D kVelocity = GetFirstDerivative(fTime);
    Vector3D kAcceleration = GetSecondDerivative(fTime);
    Vector3D kCross = kVelocity.Cross(kAcceleration);
    float fDenom = kCross.SquaredLength();

    const float fTolerance = 1e-06f;
    if ( fDenom >= fTolerance )
    {
        Vector3D kJerk = GetThirdDerivative(fTime);
        float fNumer = kCross.Dot(kJerk);
        return fNumer/fDenom;
    }
    else
    {
        // torsion is indeterminate, just return 0
        return 0.0;
    }
}
//----------------------------------------------------------------------------
