using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace BulletX.LinerMath
{
    static class TransformUtil
    {
        public const float ANGULAR_MOTION_THRESHOLD = (float)(0.5 * BulletGlobal.SIMD_HALF_PI);

        public static void integrateTransform(btTransform curTrans,btVector3 linvel, btVector3 angvel,   float timeStep,out btTransform predictedTransform)
	    {
            predictedTransform = btTransform.Identity;
		    predictedTransform.Origin=curTrans.Origin + linvel * timeStep;
		    //Exponential map
		    //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

		    btVector3 axis;
		    float	fAngle = angvel.Length; 
		    //limit the angular motion
		    if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
		    {
			    fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
		    }

		    if ( fAngle < 0.001f )
		    {
			    // use Taylor's expansions of sync function
			    axis   = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
		    }
		    else
		    {
			    // sync(fAngle) = sin(c*fAngle)/t
			    axis   = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
		    }
		    btQuaternion dorn =new btQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*0.5f ));
		    btQuaternion orn0 = curTrans.Rotation;

		    btQuaternion predictedOrn;
            btQuaternion.Multiply(ref dorn, ref orn0, out predictedOrn);
		    predictedOrn.normalize();
		    predictedTransform.Rotation=predictedOrn;
	    }
        public static void	calculateVelocity(btTransform transform0,btTransform transform1,float timeStep,out btVector3 linVel, out btVector3 angVel)
	    {
		    linVel = (transform1.Origin - transform0.Origin) / timeStep;
		    btVector3 axis;
		    float  angle;
		    calculateDiffAxisAngle(transform0,transform1,out axis,out angle);
		    angVel = axis * angle / timeStep;
	    }
        public static void calculateDiffAxisAngle(btTransform transform0,btTransform transform1,out btVector3 axis,out float angle)
	    {
		    btMatrix3x3 dmat = transform1.Basis * transform0.Basis.inverse();
		    btQuaternion dorn;
		    dmat.getRotation(out dorn);

		    ///floating point inaccuracy can lead to w component > 1..., which breaks 
		    dorn.normalize();
    		
		    angle = dorn.getAngle();
		    axis = new btVector3(dorn.X,dorn.Y,dorn.Z);
		    axis[3] = 0f;
		    //check for axis length
		    float len = axis.Length2;
            if (len < BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON)
			    axis = new  btVector3(1f,0f,0f);
		    else
			    axis /= (float)Math.Sqrt(len);
	    }
    }
}
