/*
Bullet Continuous Collision Detection and Physics Library
btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.

Written by: Marcus Hennix
*/


#include "btConeTwistConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btMinMax.h"
#include <new>



//#define CONETWIST_USE_OBSOLETE_SOLVER true
#define CONETWIST_USE_OBSOLETE_SOLVER false
#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)


SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
{
	btVector3 vec = axis * invInertiaWorld;
	return axis.dot(vec);
}




btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
											 const btTransform& rbAFrame,const btTransform& rbBFrame)
											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
											 m_angularOnly(false),
											 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
	init();
}

btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
											 m_angularOnly(false),
											 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
	m_rbBFrame = m_rbAFrame;
	m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
	init();	
}


void btConeTwistConstraint::init()
{
	m_angularOnly = false;
	m_solveTwistLimit = false;
	m_solveSwingLimit = false;
	m_bMotorEnabled = false;
	m_maxMotorImpulse = btScalar(-1);

	setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
	m_damping = btScalar(0.01);
	m_fixThresh = CONETWIST_DEF_FIX_THRESH;
	m_flags = 0;
	m_linCFM = btScalar(0.f);
	m_linERP = btScalar(0.7f);
	m_angCFM = btScalar(0.f);
}


void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
{
	if (m_useSolveConstraintObsolete)
	{
		info->m_numConstraintRows = 0;
		info->nub = 0;
	} 
	else
	{
		info->m_numConstraintRows = 3;
		info->nub = 3;
		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
		if(m_solveSwingLimit)
		{
			info->m_numConstraintRows++;
			info->nub--;
			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
			{
				info->m_numConstraintRows++;
				info->nub--;
			}
		}
		if(m_solveTwistLimit)
		{
			info->m_numConstraintRows++;
			info->nub--;
		}
	}
}

void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
{
	//always reserve 6 rows: object transform is not available on SPU
	info->m_numConstraintRows = 6;
	info->nub = 0;
		
}
	

void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
{
	getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
}

void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
{
	calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
	
	btAssert(!m_useSolveConstraintObsolete);
    // set jacobian
    info->m_J1linearAxis[0] = 1;
    info->m_J1linearAxis[info->rowskip+1] = 1;
    info->m_J1linearAxis[2*info->rowskip+2] = 1;
	btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
	{
		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
		btVector3 a1neg = -a1;
		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
	}
    info->m_J2linearAxis[0] = -1;
    info->m_J2linearAxis[info->rowskip+1] = -1;
    info->m_J2linearAxis[2*info->rowskip+2] = -1;
	btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
	{
		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
	}
    // set right hand side
	btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
    btScalar k = info->fps * linERP;
    int j;
	for (j=0; j<3; j++)
    {
        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
		info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
		info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
		if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
		{
			info->cfm[j*info->rowskip] = m_linCFM;
		}
    }
	int row = 3;
    int srow = row * info->rowskip;
	btVector3 ax1;
	// angular limits
	if(m_solveSwingLimit)
	{
		btScalar *J1 = info->m_J1angularAxis;
		btScalar *J2 = info->m_J2angularAxis;
		if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
		{
			btTransform trA = transA*m_rbAFrame;
			btVector3 p = trA.getBasis().getColumn(1);
			btVector3 q = trA.getBasis().getColumn(2);
			int srow1 = srow + info->rowskip;
			J1[srow+0] = p[0];
			J1[srow+1] = p[1];
			J1[srow+2] = p[2];
			J1[srow1+0] = q[0];
			J1[srow1+1] = q[1];
			J1[srow1+2] = q[2];
			J2[srow+0] = -p[0];
			J2[srow+1] = -p[1];
			J2[srow+2] = -p[2];
			J2[srow1+0] = -q[0];
			J2[srow1+1] = -q[1];
			J2[srow1+2] = -q[2];
			btScalar fact = info->fps * m_relaxationFactor;
			info->m_constraintError[srow] =   fact * m_swingAxis.dot(p);
			info->m_constraintError[srow1] =  fact * m_swingAxis.dot(q);
			info->m_lowerLimit[srow] = -SIMD_INFINITY;
			info->m_upperLimit[srow] = SIMD_INFINITY;
			info->m_lowerLimit[srow1] = -SIMD_INFINITY;
			info->m_upperLimit[srow1] = SIMD_INFINITY;
			srow = srow1 + info->rowskip;
		}
		else
		{
			ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
			J1[srow+0] = ax1[0];
			J1[srow+1] = ax1[1];
			J1[srow+2] = ax1[2];
			J2[srow+0] = -ax1[0];
			J2[srow+1] = -ax1[1];
			J2[srow+2] = -ax1[2];
			btScalar k = info->fps * m_biasFactor;

			info->m_constraintError[srow] = k * m_swingCorrection;
			if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
			{
				info->cfm[srow] = m_angCFM;
			}
			// m_swingCorrection is always positive or 0
			info->m_lowerLimit[srow] = 0;
			info->m_upperLimit[srow] = (m_bMotorEnabled && m_maxMotorImpulse >= 0.0f) ? m_maxMotorImpulse : SIMD_INFINITY;
			srow += info->rowskip;
		}
	}
	if(m_solveTwistLimit)
	{
		ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
		btScalar *J1 = info->m_J1angularAxis;
		btScalar *J2 = info->m_J2angularAxis;
		J1[srow+0] = ax1[0];
		J1[srow+1] = ax1[1];
		J1[srow+2] = ax1[2];
		J2[srow+0] = -ax1[0];
		J2[srow+1] = -ax1[1];
		J2[srow+2] = -ax1[2];
		btScalar k = info->fps * m_biasFactor;
		info->m_constraintError[srow] = k * m_twistCorrection;
		if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
		{
			info->cfm[srow] = m_angCFM;
		}
		if(m_twistSpan > 0.0f)
		{

			if(m_twistCorrection > 0.0f)
			{
				info->m_lowerLimit[srow] = 0;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			} 
			else
			{
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = 0;
			} 
		}
		else
		{
			info->m_lowerLimit[srow] = -SIMD_INFINITY;
			info->m_upperLimit[srow] = SIMD_INFINITY;
		}
		srow += info->rowskip;
	}
}
	


void	btConeTwistConstraint::buildJacobian()
{
	if (m_useSolveConstraintObsolete)
	{
		m_appliedImpulse = btScalar(0.);
		m_accTwistLimitImpulse = btScalar(0.);
		m_accSwingLimitImpulse = btScalar(0.);
		m_accMotorImpulse = btVector3(0.,0.,0.);

		if (!m_angularOnly)
		{
			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
			btVector3 relPos = pivotBInW - pivotAInW;

			btVector3 normal[3];
			if (relPos.length2() > SIMD_EPSILON)
			{
				normal[0] = relPos.normalized();
			}
			else
			{
				normal[0].setValue(btScalar(1.0),0,0);
			}

			btPlaneSpace1(normal[0], normal[1], normal[2]);

			for (int i=0;i<3;i++)
			{
				new (&m_jac[i]) btJacobianEntry(
				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
				pivotAInW - m_rbA.getCenterOfMassPosition(),
				pivotBInW - m_rbB.getCenterOfMassPosition(),
				normal[i],
				m_rbA.getInvInertiaDiagLocal(),
				m_rbA.getInvMass(),
				m_rbB.getInvInertiaDiagLocal(),
				m_rbB.getInvMass());
			}
		}

		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
	}
}



void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
{
	#ifndef __SPU__
	if (m_useSolveConstraintObsolete)
	{
		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();

		btScalar tau = btScalar(0.3);

		//linear part
		if (!m_angularOnly)
		{
			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();

			btVector3 vel1;
			bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
			btVector3 vel2;
			bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
			btVector3 vel = vel1 - vel2;

			for (int i=0;i<3;i++)
			{		
				const btVector3& normal = m_jac[i].m_linearJointAxis;
				btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();

				btScalar rel_vel;
				rel_vel = normal.dot(vel);
				//positional error (zeroth order error)
				btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
				btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
				m_appliedImpulse += impulse;
				
				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
				bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
				bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
		
			}
		}

		// apply motor
		if (m_bMotorEnabled)
		{
			// compute current and predicted transforms
			btTransform trACur = m_rbA.getCenterOfMassTransform();
			btTransform trBCur = m_rbB.getCenterOfMassTransform();
			btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
			btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
			btTransform trAPred; trAPred.setIdentity(); 
			btVector3 zerovec(0,0,0);
			btTransformUtil::integrateTransform(
				trACur, zerovec, omegaA, timeStep, trAPred);
			btTransform trBPred; trBPred.setIdentity(); 
			btTransformUtil::integrateTransform(
				trBCur, zerovec, omegaB, timeStep, trBPred);

			// compute desired transforms in world
			btTransform trPose(m_qTarget);
			btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
			btTransform trADes = trBPred * trABDes;
			btTransform trBDes = trAPred * trABDes.inverse();

			// compute desired omegas in world
			btVector3 omegaADes, omegaBDes;
			
			btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
			btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);

			// compute delta omegas
			btVector3 dOmegaA = omegaADes - omegaA;
			btVector3 dOmegaB = omegaBDes - omegaB;

			// compute weighted avg axis of dOmega (weighting based on inertias)
			btVector3 axisA, axisB;
			btScalar kAxisAInv = 0, kAxisBInv = 0;

			if (dOmegaA.length2() > SIMD_EPSILON)
			{
				axisA = dOmegaA.normalized();
				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
			}

			if (dOmegaB.length2() > SIMD_EPSILON)
			{
				axisB = dOmegaB.normalized();
				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
			}

			btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;

			static bool bDoTorque = true;
			if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
			{
				avgAxis.normalize();
				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
				btScalar kInvCombined = kAxisAInv + kAxisBInv;

				btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
									(kInvCombined * kInvCombined);

				if (m_maxMotorImpulse >= 0)
				{
					btScalar fMaxImpulse = m_maxMotorImpulse;
					if (m_bNormalizedMotorStrength)
						fMaxImpulse = fMaxImpulse/kAxisAInv;

					btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
					btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
					if (newUnclampedMag > fMaxImpulse)
					{
						newUnclampedAccImpulse.normalize();
						newUnclampedAccImpulse *= fMaxImpulse;
						impulse = newUnclampedAccImpulse - m_accMotorImpulse;
					}
					m_accMotorImpulse += impulse;
				}

				btScalar  impulseMag  = impulse.length();
				btVector3 impulseAxis =  impulse / impulseMag;

				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);

			}
		}
		else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
		{
			btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
			btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
			btVector3 relVel = angVelB - angVelA;
			if (relVel.length2() > SIMD_EPSILON)
			{
				btVector3 relVelAxis = relVel.normalized();
				btScalar m_kDamping =  btScalar(1.) /
					(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
					 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
				btVector3 impulse = m_damping * m_kDamping * relVel;

				btScalar  impulseMag  = impulse.length();
				btVector3 impulseAxis = impulse / impulseMag;
				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
			}
		}

		// joint limits
		{
			///solve angular part
			btVector3 angVelA;
			bodyA.internalGetAngularVelocity(angVelA);
			btVector3 angVelB;
			bodyB.internalGetAngularVelocity(angVelB);

			// solve swing limit
			if (m_solveSwingLimit)
			{
				btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
				btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
				if (relSwingVel > 0)
					amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
				btScalar impulseMag = amplitude * m_kSwing;

				// Clamp the accumulated impulse
				btScalar temp = m_accSwingLimitImpulse;
				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
				impulseMag = m_accSwingLimitImpulse - temp;

				btVector3 impulse = m_swingAxis * impulseMag;

				// don't let cone response affect twist
				// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
				{
					btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
					btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
					impulse = impulseNoTwistCouple;
				}

				impulseMag = impulse.length();
				btVector3 noTwistSwingAxis = impulse / impulseMag;

				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
			}


			// solve twist limit
			if (m_solveTwistLimit)
			{
				btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
				btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
				if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
					amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
				btScalar impulseMag = amplitude * m_kTwist;

				// Clamp the accumulated impulse
				btScalar temp = m_accTwistLimitImpulse;
				m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
				impulseMag = m_accTwistLimitImpulse - temp;

		//		btVector3 impulse = m_twistAxis * impulseMag;

				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
			}		
		}
	}
#else
btAssert(0);
#endif //__SPU__
}




void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
{
	(void)timeStep;

}


#ifndef __SPU__
void btConeTwistConstraint::calcAngleInfo()
{
	m_swingCorrection = btScalar(0.);
	m_twistLimitSign = btScalar(0.);
	m_solveTwistLimit = false;
	m_solveSwingLimit = false;

	btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
	btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);

	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);

	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);

	btScalar swx=btScalar(0.),swy = btScalar(0.);
	btScalar thresh = btScalar(10.);
	btScalar fact;

	// Get Frame into world space
	if (m_swingSpan1 >= btScalar(0.05f))
	{
		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
		swx = b2Axis1.dot(b1Axis1);
		swy = b2Axis1.dot(b1Axis2);
		swing1  = btAtan2Fast(swy, swx);
		fact = (swy*swy + swx*swx) * thresh * thresh;
		fact = fact / (fact + btScalar(1.0));
		swing1 *= fact; 
	}

	if (m_swingSpan2 >= btScalar(0.05f))
	{
		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
		swx = b2Axis1.dot(b1Axis1);
		swy = b2Axis1.dot(b1Axis3);
		swing2  = btAtan2Fast(swy, swx);
		fact = (swy*swy + swx*swx) * thresh * thresh;
		fact = fact / (fact + btScalar(1.0));
		swing2 *= fact; 
	}

	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
	btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;

	if (EllipseAngle > 1.0f)
	{
		m_swingCorrection = EllipseAngle-1.0f;
		m_solveSwingLimit = true;
		// Calculate necessary axis & factors
		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
		m_swingAxis.normalize();
		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
		m_swingAxis *= swingAxisSign;
	}

	// Twist limits
	if (m_twistSpan >= btScalar(0.))
	{
		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
		m_twistAngle = twist;

//		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
		if (twist <= -m_twistSpan*lockedFreeFactor)
		{
			m_twistCorrection = -(twist + m_twistSpan);
			m_solveTwistLimit = true;
			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
			m_twistAxis.normalize();
			m_twistAxis *= -1.0f;
		}
		else if (twist >  m_twistSpan*lockedFreeFactor)
		{
			m_twistCorrection = (twist - m_twistSpan);
			m_solveTwistLimit = true;
			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
			m_twistAxis.normalize();
		}
	}
}
#endif //__SPU__

static btVector3 vTwist(1,0,0); // twist axis in constraint's space



void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
{
	m_swingCorrection = btScalar(0.);
	m_twistLimitSign = btScalar(0.);
	m_solveTwistLimit = false;
	m_solveSwingLimit = false;
	// compute rotation of A wrt B (in constraint space)
	if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
	{	// it is assumed that setMotorTarget() was alredy called 
		// and motor target m_qTarget is within constraint limits
		// TODO : split rotation to pure swing and pure twist
		// compute desired transforms in world
		btTransform trPose(m_qTarget);
		btTransform trA = transA * m_rbAFrame;
		btTransform trB = transB * m_rbBFrame;
		btTransform trDeltaAB = trB * trPose * trA.inverse();
		btQuaternion qDeltaAB = trDeltaAB.getRotation();
		btVector3 swingAxis = 	btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
		btScalar swingAxisLen2 = swingAxis.length2();
		if(btFuzzyZero(swingAxisLen2))
		{
		   return;
		}
		m_swingAxis = swingAxis;
		m_swingAxis.normalize();
		m_swingCorrection = qDeltaAB.getAngle();
		if(!btFuzzyZero(m_swingCorrection))
		{
			m_solveSwingLimit = true;
		}
		return;
	}


	{
		// compute rotation of A wrt B (in constraint space)
		btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
		btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
		btQuaternion qAB = qB.inverse() * qA;
		// split rotation into cone and twist
		// (all this is done from B's perspective. Maybe I should be averaging axes...)
		btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
		btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
		btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();

		if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
		{
			btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
			computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);

			if (swingAngle > swingLimit * m_limitSoftness)
			{
				m_solveSwingLimit = true;

				// compute limit ratio: 0->1, where
				// 0 == beginning of soft limit
				// 1 == hard/real limit
				m_swingLimitRatio = 1.f;
				if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
				{
					m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
										(swingLimit - swingLimit * m_limitSoftness);
				}				

				// swing correction tries to get back to soft limit
				m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);

				// adjustment of swing axis (based on ellipse normal)
				adjustSwingAxisToUseEllipseNormal(swingAxis);

				// Calculate necessary axis & factors		
				m_swingAxis = quatRotate(qB, -swingAxis);

				m_twistAxisA.setValue(0,0,0);

				m_kSwing =  btScalar(1.) /
					(computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
					 computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
			}
		}
		else
		{
			// you haven't set any limits;
			// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
			// anyway, we have either hinge or fixed joint
			btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
			btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
			btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
			btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
			btVector3 target;
			btScalar x = ivB.dot(ivA);
			btScalar y = ivB.dot(jvA);
			btScalar z = ivB.dot(kvA);
			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
			{ // fixed. We'll need to add one more row to constraint
				if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
				{
					m_solveSwingLimit = true;
					m_swingAxis = -ivB.cross(ivA);
				}
			}
			else
			{
				if(m_swingSpan1 < m_fixThresh)
				{ // hinge around Y axis
//					if(!(btFuzzyZero(y)))
					if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
					{
						m_solveSwingLimit = true;
						if(m_swingSpan2 >= m_fixThresh)
						{
							y = btScalar(0.f);
							btScalar span2 = btAtan2(z, x);
							if(span2 > m_swingSpan2)
							{
								x = btCos(m_swingSpan2);
								z = btSin(m_swingSpan2);
							}
							else if(span2 < -m_swingSpan2)
							{
								x =  btCos(m_swingSpan2);
								z = -btSin(m_swingSpan2);
							}
						}
					}
				}
				else
				{ // hinge around Z axis
//					if(!btFuzzyZero(z))
					if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
					{
						m_solveSwingLimit = true;
						if(m_swingSpan1 >= m_fixThresh)
						{
							z = btScalar(0.f);
							btScalar span1 = btAtan2(y, x);
							if(span1 > m_swingSpan1)
							{
								x = btCos(m_swingSpan1);
								y = btSin(m_swingSpan1);
							}
							else if(span1 < -m_swingSpan1)
							{
								x =  btCos(m_swingSpan1);
								y = -btSin(m_swingSpan1);
							}
						}
					}
				}
				target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
				target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
				target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
				target.normalize();
				m_swingAxis = -ivB.cross(target);
                                m_swingCorrection = m_swingAxis.length();

                                if (!btFuzzyZero(m_swingCorrection))
                                    m_swingAxis.normalize();
			}
		}

		if (m_twistSpan >= btScalar(0.f))
		{
			btVector3 twistAxis;
			computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);

			if (m_twistAngle > m_twistSpan*m_limitSoftness)
			{
				m_solveTwistLimit = true;

				m_twistLimitRatio = 1.f;
				if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
				{
					m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
										(m_twistSpan  - m_twistSpan * m_limitSoftness);
				}

				// twist correction tries to get back to soft limit
				m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);

				m_twistAxis = quatRotate(qB, -twistAxis);

				m_kTwist = btScalar(1.) /
					(computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
					 computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
			}

			if (m_solveSwingLimit)
				m_twistAxisA = quatRotate(qA, -twistAxis);
		}
		else
		{
			m_twistAngle = btScalar(0.f);
		}
	}
}



// given a cone rotation in constraint space, (pre: twist must already be removed)
// this method computes its corresponding swing angle and axis.
// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
												 btScalar& swingAngle, // out
												 btVector3& vSwingAxis, // out
												 btScalar& swingLimit) // out
{
	swingAngle = qCone.getAngle();
	if (swingAngle > SIMD_EPSILON)
	{
		vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
		vSwingAxis.normalize();
#if 0
        // non-zero twist?! this should never happen.
       btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
#endif
        
		// Compute limit for given swing. tricky:
		// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
		// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)

		// For starters, compute the direction from center to surface of ellipse.
		// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
		// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
		btScalar xEllipse =  vSwingAxis.y();
		btScalar yEllipse = -vSwingAxis.z();

		// Now, we use the slope of the vector (using x/yEllipse) and find the length
		// of the line that intersects the ellipse:
		//  x^2   y^2
		//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
		//  a^2   b^2
		// Do the math and it should be clear.

		swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
		if (fabs(xEllipse) > SIMD_EPSILON)
		{
			btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
			btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
			norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
			btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
			swingLimit = sqrt(swingLimit2);
		}

		// test!
		/*swingLimit = m_swingSpan2;
		if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
		{
		btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
		btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
		btScalar phi = asin(sinphi);
		btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
		btScalar alpha = 3.14159f - theta - phi;
		btScalar sinalpha = sin(alpha);
		swingLimit = m_swingSpan1 * sinphi/sinalpha;
		}*/
	}
	else if (swingAngle < 0)
	{
		// this should never happen!
#if 0
        btAssert(0);
#endif
 	}
}

btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
{
	// compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
	btScalar xEllipse = btCos(fAngleInRadians);
	btScalar yEllipse = btSin(fAngleInRadians);

	// Use the slope of the vector (using x/yEllipse) and find the length
	// of the line that intersects the ellipse:
	//  x^2   y^2
	//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
	//  a^2   b^2
	// Do the math and it should be clear.

	btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
	if (fabs(xEllipse) > SIMD_EPSILON)
	{
		btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
		btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
		norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
		btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
		swingLimit = sqrt(swingLimit2);
	}

	// convert into point in constraint space:
	// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
	btVector3 vSwingAxis(0, xEllipse, -yEllipse);
	btQuaternion qSwing(vSwingAxis, swingLimit);
	btVector3 vPointInConstraintSpace(fLength,0,0);
	return quatRotate(qSwing, vPointInConstraintSpace);
}

// given a twist rotation in constraint space, (pre: cone must already be removed)
// this method computes its corresponding angle and axis.
void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
												  btScalar& twistAngle, // out
												  btVector3& vTwistAxis) // out
{
	btQuaternion qMinTwist = qTwist;
	twistAngle = qTwist.getAngle();

	if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
	{
		qMinTwist = -(qTwist);
		twistAngle = qMinTwist.getAngle();
	}
	if (twistAngle < 0)
	{
		// this should never happen
#if 0
        btAssert(0);
#endif
	}

	vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
	if (twistAngle > SIMD_EPSILON)
		vTwistAxis.normalize();
}


void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
{
	// the swing axis is computed as the "twist-free" cone rotation,
	// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
	// so, if we're outside the limits, the closest way back inside the cone isn't 
	// along the vector back to the center. better (and more stable) to use the ellipse normal.

	// convert swing axis to direction from center to surface of ellipse
	// (ie. rotate 2D vector by PI/2)
	btScalar y = -vSwingAxis.z();
	btScalar z =  vSwingAxis.y();

	// do the math...
	if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
	{
		// compute gradient/normal of ellipse surface at current "point"
		btScalar grad = y/z;
		grad *= m_swingSpan2 / m_swingSpan1;

		// adjust y/z to represent normal at point (instead of vector to point)
		if (y > 0)
			y =  fabs(grad * z);
		else
			y = -fabs(grad * z);

		// convert ellipse direction back to swing axis
		vSwingAxis.setZ(-y);
		vSwingAxis.setY( z);
		vSwingAxis.normalize();
	}
}



void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
{
	//btTransform trACur = m_rbA.getCenterOfMassTransform();
	//btTransform trBCur = m_rbB.getCenterOfMassTransform();
//	btTransform trABCur = trBCur.inverse() * trACur;
//	btQuaternion qABCur = trABCur.getRotation();
//	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
	//btQuaternion qConstraintCur = trConstraintCur.getRotation();

	btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
	setMotorTargetInConstraintSpace(qConstraint);
}


void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
{
	m_qTarget = q;

	// clamp motor target to within limits
	{
		btScalar softness = 1.f;//m_limitSoftness;

		// split into twist and cone
		btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
		btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
		btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();

		// clamp cone
		if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
		{
			btScalar swingAngle, swingLimit; btVector3 swingAxis;
			computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);

			if (fabs(swingAngle) > SIMD_EPSILON)
			{
				if (swingAngle > swingLimit*softness)
					swingAngle = swingLimit*softness;
				else if (swingAngle < -swingLimit*softness)
					swingAngle = -swingLimit*softness;
				qTargetCone = btQuaternion(swingAxis, swingAngle);
			}
		}

		// clamp twist
		if (m_twistSpan >= btScalar(0.05f))
		{
			btScalar twistAngle; btVector3 twistAxis;
			computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);

			if (fabs(twistAngle) > SIMD_EPSILON)
			{
				// eddy todo: limitSoftness used here???
				if (twistAngle > m_twistSpan*softness)
					twistAngle = m_twistSpan*softness;
				else if (twistAngle < -m_twistSpan*softness)
					twistAngle = -m_twistSpan*softness;
				qTargetTwist = btQuaternion(twistAxis, twistAngle);
			}
		}

		m_qTarget = qTargetCone * qTargetTwist;
	}
}

///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
///If no axis is provided, it uses the default axis for this constraint.
void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
{
	switch(num)
	{
		case BT_CONSTRAINT_ERP :
		case BT_CONSTRAINT_STOP_ERP :
			if((axis >= 0) && (axis < 3)) 
			{
				m_linERP = value;
				m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
			}
			else
			{
				m_biasFactor = value;
			}
			break;
		case BT_CONSTRAINT_CFM :
		case BT_CONSTRAINT_STOP_CFM :
			if((axis >= 0) && (axis < 3)) 
			{
				m_linCFM = value;
				m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
			}
			else
			{
				m_angCFM = value;
				m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
			}
			break;
		default:
			btAssertConstrParams(0);
			break;
	}
}

///return the local value of parameter
btScalar btConeTwistConstraint::getParam(int num, int axis) const 
{
	btScalar retVal = 0;
	switch(num)
	{
		case BT_CONSTRAINT_ERP :
		case BT_CONSTRAINT_STOP_ERP :
			if((axis >= 0) && (axis < 3)) 
			{
				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
				retVal = m_linERP;
			}
			else if((axis >= 3) && (axis < 6)) 
			{
				retVal = m_biasFactor;
			}
			else
			{
				btAssertConstrParams(0);
			}
			break;
		case BT_CONSTRAINT_CFM :
		case BT_CONSTRAINT_STOP_CFM :
			if((axis >= 0) && (axis < 3)) 
			{
				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
				retVal = m_linCFM;
			}
			else if((axis >= 3) && (axis < 6)) 
			{
				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
				retVal = m_angCFM;
			}
			else
			{
				btAssertConstrParams(0);
			}
			break;
		default : 
			btAssertConstrParams(0);
	}
	return retVal;
}


void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
{
	m_rbAFrame = frameA;
	m_rbBFrame = frameB;
	buildJacobian();
	//calculateTransforms();
}