#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"



inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
	
	if (bodies[nodeID].m_invMass != 0.f)
	{
		float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);

		//angular velocity
		{
			b3Float4 axis;
			//add some hardcoded angular damping
			bodies[nodeID].m_angVel.x *= angularDamping;
			bodies[nodeID].m_angVel.y *= angularDamping;
			bodies[nodeID].m_angVel.z *= angularDamping;
			
			b3Float4 angvel = bodies[nodeID].m_angVel;

			float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
			
			//limit the angular motion
			if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
			{
				fAngle = BT_GPU_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 * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
			}
			
			b3Quat dorn;
			dorn.x = axis.x;
			dorn.y = axis.y;
			dorn.z = axis.z;
			dorn.w = b3Cos(fAngle * timeStep * 0.5f);
			b3Quat orn0 = bodies[nodeID].m_quat;
			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
			predictedOrn = b3QuatNormalized(predictedOrn);
			bodies[nodeID].m_quat=predictedOrn;
		}
		//linear velocity		
		bodies[nodeID].m_pos +=  bodies[nodeID].m_linVel * timeStep;
		
		//apply gravity
		bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
		
	}
	
}

inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
	float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
	
	if( (body->m_invMass != 0.f))
	{
		//angular velocity
		{
			b3Float4 axis;
			//add some hardcoded angular damping
			body->m_angVel.x *= angularDamping;
			body->m_angVel.y *= angularDamping;
			body->m_angVel.z *= angularDamping;
			
			b3Float4 angvel = body->m_angVel;
			float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
			//limit the angular motion
			if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
			{
				fAngle = BT_GPU_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 * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
			}
			b3Quat dorn;
			dorn.x = axis.x;
			dorn.y = axis.y;
			dorn.z = axis.z;
			dorn.w = b3Cos(fAngle * timeStep * 0.5f);
			b3Quat orn0 = body->m_quat;

			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
			predictedOrn = b3QuatNormalized(predictedOrn);
			body->m_quat=predictedOrn;
		}

		//apply gravity
		body->m_linVel += gravityAcceleration * timeStep;

		//linear velocity		
		body->m_pos +=  body->m_linVel * timeStep;
		
	}
	
}