/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

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.
*/
/*
2007-09-09
Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/

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

#include "Bullet3Common/b3TransformUtil.h"
#include "Bullet3Common/b3TransformUtil.h"
#include <new>



#define D6_USE_OBSOLETE_METHOD false
#define D6_USE_FRAME_OFFSET true






b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int  rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0)
{
	calculateTransforms(bodies);
}






#define GENERIC_D6_DISABLE_WARMSTARTING 1



b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
{
	int i = index%3;
	int j = index/3;
	return mat[i][j];
}



///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz);
bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
{
	//	// rot =  cy*cz          -cy*sz           sy
	//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
	//

	b3Scalar fi = btGetMatrixElem(mat,2);
	if (fi < b3Scalar(1.0f))
	{
		if (fi > b3Scalar(-1.0f))
		{
			xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
			xyz[1] = b3Asin(btGetMatrixElem(mat,2));
			xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
			return true;
		}
		else
		{
			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
			xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
			xyz[1] = -B3_HALF_PI;
			xyz[2] = b3Scalar(0.0);
			return false;
		}
	}
	else
	{
		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
		xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
		xyz[1] = B3_HALF_PI;
		xyz[2] = 0.0;
	}
	return false;
}

//////////////////////////// b3RotationalLimitMotor ////////////////////////////////////

int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
{
	if(m_loLimit>m_hiLimit)
	{
		m_currentLimit = 0;//Free from violation
		return 0;
	}
	if (test_value < m_loLimit)
	{
		m_currentLimit = 1;//low limit violation
		m_currentLimitError =  test_value - m_loLimit;
		if(m_currentLimitError>B3_PI) 
			m_currentLimitError-=B3_2_PI;
		else if(m_currentLimitError<-B3_PI) 
			m_currentLimitError+=B3_2_PI;
		return 1;
	}
	else if (test_value> m_hiLimit)
	{
		m_currentLimit = 2;//High limit violation
		m_currentLimitError = test_value - m_hiLimit;
		if(m_currentLimitError>B3_PI) 
			m_currentLimitError-=B3_2_PI;
		else if(m_currentLimitError<-B3_PI) 
			m_currentLimitError+=B3_2_PI;
		return 2;
	};

	m_currentLimit = 0;//Free from violation
	return 0;

}




//////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////




//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////


int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
{
	b3Scalar loLimit = m_lowerLimit[limitIndex];
	b3Scalar hiLimit = m_upperLimit[limitIndex];
	if(loLimit > hiLimit)
	{
		m_currentLimit[limitIndex] = 0;//Free from violation
		m_currentLimitError[limitIndex] = b3Scalar(0.f);
		return 0;
	}

	if (test_value < loLimit)
	{
		m_currentLimit[limitIndex] = 2;//low limit violation
		m_currentLimitError[limitIndex] =  test_value - loLimit;
		return 2;
	}
	else if (test_value> hiLimit)
	{
		m_currentLimit[limitIndex] = 1;//High limit violation
		m_currentLimitError[limitIndex] = test_value - hiLimit;
		return 1;
	};

	m_currentLimit[limitIndex] = 0;//Free from violation
	m_currentLimitError[limitIndex] = b3Scalar(0.f);
	return 0;
}



//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////

void b3Generic6DofConstraint::calculateAngleInfo()
{
	b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
	// in euler angle mode we do not actually constrain the angular velocity
	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
	//
	//    to get			constrain w2-w1 along		...not
	//    ------			---------------------		------
	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
	//    d(angle[1])/dt = 0	ax[1]
	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
	//
	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
	// to prove the result for angle[0], write the expression for angle[0] from
	// GetInfo1 then take the derivative. to prove this for angle[2] it is
	// easier to take the euler rate expression for d(angle[2])/dt with respect
	// to the components of w and set that to 0.
	b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
	b3Vector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);

	m_calculatedAxis[1] = axis2.cross(axis0);
	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);

	m_calculatedAxis[0].normalize();
	m_calculatedAxis[1].normalize();
	m_calculatedAxis[2].normalize();

}

static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
{
	b3Transform tr(body.m_quat,body.m_pos);
	return tr;
}

void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
{
	b3Transform transA;
	b3Transform transB;
	transA = getCenterOfMassTransform(bodies[m_rbA]);
	transB = getCenterOfMassTransform(bodies[m_rbB]);
	calculateTransforms(transA,transB,bodies);
}

void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies)
{
	m_calculatedTransformA = transA * m_frameInA;
	m_calculatedTransformB = transB * m_frameInB;
	calculateLinearInfo();
	calculateAngleInfo();
	if(m_useOffsetForConstraintFrame)
	{	//  get weight factors depending on masses
		b3Scalar miA = bodies[m_rbA].m_invMass;
		b3Scalar miB = bodies[m_rbB].m_invMass;
		m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
		b3Scalar miS = miA + miB;
		if(miS > b3Scalar(0.f))
		{
			m_factA = miB / miS;
		}
		else 
		{
			m_factA = b3Scalar(0.5f);
		}
		m_factB = b3Scalar(1.0f) - m_factA;
	}
}







bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
{
	b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
	angle = b3AdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
	m_angularLimits[axis_index].m_currentPosition = angle;
	//test limits
	m_angularLimits[axis_index].testLimitValue(angle);
	return m_angularLimits[axis_index].needApplyTorques();
}




void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
{
	//prepare constraint
	calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
	info->m_numConstraintRows = 0;
	info->nub = 6;
	int i;
	//test linear limits
	for(i = 0; i < 3; i++)
	{
		if(m_linearLimits.needApplyForce(i))
		{
			info->m_numConstraintRows++;
			info->nub--;
		}
	}
	//test angular limits
	for (i=0;i<3 ;i++ )
	{
		if(testAngularLimitMotor(i))
		{
			info->m_numConstraintRows++;
			info->nub--;
		}
	}
//	printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
}

void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
{
	//pre-allocate all 6
	info->m_numConstraintRows = 6;
	info->nub = 0;
}


void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies)
{

	b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
	b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
	const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
	const b3Vector3& linVelB = bodies[m_rbB].m_linVel;
	const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
	const b3Vector3& angVelB = bodies[m_rbB].m_angVel;

	if(m_useOffsetForConstraintFrame)
	{ // for stability better to solve angular limits first
		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
	}
	else
	{ // leave old version for compatibility
		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
	}

}


void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies)
{
	
	//prepare constraint
	calculateTransforms(transA,transB,bodies);

	int i;
	for (i=0;i<3 ;i++ )
	{
		testAngularLimitMotor(i);
	}

	if(m_useOffsetForConstraintFrame)
	{ // for stability better to solve angular limits first
		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
	}
	else
	{ // leave old version for compatibility
		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
	}
}



int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
{
//	int row = 0;
	//solve linear limits
	b3RotationalLimitMotor limot;
	for (int i=0;i<3 ;i++ )
	{
		if(m_linearLimits.needApplyForce(i))
		{ // re-use rotational motor code
			limot.m_bounce = b3Scalar(0.f);
			limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
			limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
			limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
			limot.m_damping  = m_linearLimits.m_damping;
			limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
			limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
			limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
			limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
			limot.m_maxLimitForce  = b3Scalar(0.f);
			limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
			limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
			b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
			int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
			limot.m_normalCFM	= (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
			limot.m_stopCFM		= (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
			limot.m_stopERP		= (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
			if(m_useOffsetForConstraintFrame)
			{
				int indx1 = (i + 1) % 3;
				int indx2 = (i + 2) % 3;
				int rotAllowed = 1; // rotations around orthos to current axis
				if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
				{
					rotAllowed = 0;
				}
				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
			}
			else
			{
				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
			}
		}
	}
	return row;
}



int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
{
	b3Generic6DofConstraint * d6constraint = this;
	int row = row_offset;
	//solve angular limits
	for (int i=0;i<3 ;i++ )
	{
		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
		{
			b3Vector3 axis = d6constraint->getAxis(i);
			int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
			if(!(flags & B3_6DOF_FLAGS_CFM_NORM))
			{
				m_angularLimits[i].m_normalCFM = info->cfm[0];
			}
			if(!(flags & B3_6DOF_FLAGS_CFM_STOP))
			{
				m_angularLimits[i].m_stopCFM = info->cfm[0];
			}
			if(!(flags & B3_6DOF_FLAGS_ERP_STOP))
			{
				m_angularLimits[i].m_stopERP = info->erp;
			}
			row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
												transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
		}
	}

	return row;
}




void	b3Generic6DofConstraint::updateRHS(b3Scalar	timeStep)
{
	(void)timeStep;

}


void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies)
{
	m_frameInA = frameA;
	m_frameInB = frameB;

	calculateTransforms(bodies);
}



b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
{
	return m_calculatedAxis[axis_index];
}


b3Scalar	b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
{
	return m_calculatedLinearDiff[axisIndex];
}


b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
{
	return m_calculatedAxisAngleDiff[axisIndex];
}



void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
{
	b3Scalar imA = bodies[m_rbA].m_invMass;
	b3Scalar imB = bodies[m_rbB].m_invMass;
	b3Scalar weight;
	if(imB == b3Scalar(0.0))
	{
		weight = b3Scalar(1.0);
	}
	else
	{
		weight = imA / (imA + imB);
	}
	const b3Vector3& pA = m_calculatedTransformA.getOrigin();
	const b3Vector3& pB = m_calculatedTransformB.getOrigin();
	m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight);
	return;
}



void b3Generic6DofConstraint::calculateLinearInfo()
{
	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
	for(int i = 0; i < 3; i++)
	{
		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
	}
}



int b3Generic6DofConstraint::get_limit_motor_info2(
	b3RotationalLimitMotor * limot,
	const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
	b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed)
{
    int srow = row * info->rowskip;
    bool powered = limot->m_enableMotor;
    int limit = limot->m_currentLimit;
    if (powered || limit)
    {   // if the joint is powered, or has joint limits, add in the extra row
        b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
        b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
		if (J1)
		{
			J1[srow+0] = ax1[0];
			J1[srow+1] = ax1[1];
			J1[srow+2] = ax1[2];
		}
		if (J2)
		{
			J2[srow+0] = -ax1[0];
			J2[srow+1] = -ax1[1];
			J2[srow+2] = -ax1[2];
		}
		if((!rotational))
        {
			if (m_useOffsetForConstraintFrame)
			{
				b3Vector3 tmpA, tmpB, relA, relB;
				// get vector from bodyB to frameB in WCS
				relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
				// get its projection to constraint axis
				b3Vector3 projB = ax1 * relB.dot(ax1);
				// get vector directed from bodyB to constraint axis (and orthogonal to it)
				b3Vector3 orthoB = relB - projB;
				// same for bodyA
				relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
				b3Vector3 projA = ax1 * relA.dot(ax1);
				b3Vector3 orthoA = relA - projA;
				// get desired offset between frames A and B along constraint axis
				b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
				// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
				b3Vector3 totalDist = projA + ax1 * desiredOffs - projB;
				// get offset vectors relA and relB
				relA = orthoA + totalDist * m_factA;
				relB = orthoB - totalDist * m_factB;
				tmpA = relA.cross(ax1);
				tmpB = relB.cross(ax1);
				if(m_hasStaticBody && (!rotAllowed))
				{
					tmpA *= m_factA;
					tmpB *= m_factB;
				}
				int i;
				for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
				for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
			} else
			{
				b3Vector3 ltd;	// Linear Torque Decoupling vector
				b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
				ltd = c.cross(ax1);
				info->m_J1angularAxis[srow+0] = ltd[0];
				info->m_J1angularAxis[srow+1] = ltd[1];
				info->m_J1angularAxis[srow+2] = ltd[2];

				c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
				ltd = -c.cross(ax1);
				info->m_J2angularAxis[srow+0] = ltd[0];
				info->m_J2angularAxis[srow+1] = ltd[1];
				info->m_J2angularAxis[srow+2] = ltd[2];
			}
        }
        // if we're limited low and high simultaneously, the joint motor is
        // ineffective
        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
        info->m_constraintError[srow] = b3Scalar(0.f);
        if (powered)
        {
			info->cfm[srow] = limot->m_normalCFM;
            if(!limit)
            {
				b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;

				b3Scalar mot_fact = getMotorFactor(	limot->m_currentPosition, 
													limot->m_loLimit,
													limot->m_hiLimit, 
													tag_vel, 
													info->fps * limot->m_stopERP);
				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
                info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
                info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
            }
        }
        if(limit)
        {
            b3Scalar k = info->fps * limot->m_stopERP;
			if(!rotational)
			{
				info->m_constraintError[srow] += k * limot->m_currentLimitError;
			}
			else
			{
				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
			}
			info->cfm[srow] = limot->m_stopCFM;
            if (limot->m_loLimit == limot->m_hiLimit)
            {   // limited low and high simultaneously
                info->m_lowerLimit[srow] = -B3_INFINITY;
                info->m_upperLimit[srow] = B3_INFINITY;
            }
            else
            {
                if (limit == 1)
                {
                    info->m_lowerLimit[srow] = 0;
                    info->m_upperLimit[srow] = B3_INFINITY;
                }
                else
                {
                    info->m_lowerLimit[srow] = -B3_INFINITY;
                    info->m_upperLimit[srow] = 0;
                }
                // deal with bounce
                if (limot->m_bounce > 0)
                {
                    // calculate joint velocity
                    b3Scalar vel;
                    if (rotational)
                    {
                        vel = angVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
//                        if (body1)
                            vel -= angVelB.dot(ax1);
                    }
                    else
                    {
                        vel = linVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
//                        if (body1)
                            vel -= linVelB.dot(ax1);
                    }
                    // only apply bounce if the velocity is incoming, and if the
                    // resulting c[] exceeds what we already have.
                    if (limit == 1)
                    {
                        if (vel < 0)
                        {
                            b3Scalar newc = -limot->m_bounce* vel;
                            if (newc > info->m_constraintError[srow]) 
								info->m_constraintError[srow] = newc;
                        }
                    }
                    else
                    {
                        if (vel > 0)
                        {
                            b3Scalar newc = -limot->m_bounce * vel;
                            if (newc < info->m_constraintError[srow]) 
								info->m_constraintError[srow] = newc;
                        }
                    }
                }
            }
        }
        return 1;
    }
    else return 0;
}






	///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 b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
{
	if((axis >= 0) && (axis < 3))
	{
		switch(num)
		{
			case B3_CONSTRAINT_STOP_ERP : 
				m_linearLimits.m_stopERP[axis] = value;
				m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
				break;
			case B3_CONSTRAINT_STOP_CFM : 
				m_linearLimits.m_stopCFM[axis] = value;
				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
				break;
			case B3_CONSTRAINT_CFM : 
				m_linearLimits.m_normalCFM[axis] = value;
				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
				break;
			default : 
				b3AssertConstrParams(0);
		}
	}
	else if((axis >=3) && (axis < 6))
	{
		switch(num)
		{
			case B3_CONSTRAINT_STOP_ERP : 
				m_angularLimits[axis - 3].m_stopERP = value;
				m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
				break;
			case B3_CONSTRAINT_STOP_CFM : 
				m_angularLimits[axis - 3].m_stopCFM = value;
				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
				break;
			case B3_CONSTRAINT_CFM : 
				m_angularLimits[axis - 3].m_normalCFM = value;
				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
				break;
			default : 
				b3AssertConstrParams(0);
		}
	}
	else
	{
		b3AssertConstrParams(0);
	}
}

	///return the local value of parameter
b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const 
{
	b3Scalar retVal = 0;
	if((axis >= 0) && (axis < 3))
	{
		switch(num)
		{
			case B3_CONSTRAINT_STOP_ERP : 
				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
				retVal = m_linearLimits.m_stopERP[axis];
				break;
			case B3_CONSTRAINT_STOP_CFM : 
				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
				retVal = m_linearLimits.m_stopCFM[axis];
				break;
			case B3_CONSTRAINT_CFM : 
				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
				retVal = m_linearLimits.m_normalCFM[axis];
				break;
			default : 
				b3AssertConstrParams(0);
		}
	}
	else if((axis >=3) && (axis < 6))
	{
		switch(num)
		{
			case B3_CONSTRAINT_STOP_ERP : 
				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
				retVal = m_angularLimits[axis - 3].m_stopERP;
				break;
			case B3_CONSTRAINT_STOP_CFM : 
				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
				retVal = m_angularLimits[axis - 3].m_stopCFM;
				break;
			case B3_CONSTRAINT_CFM : 
				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
				retVal = m_angularLimits[axis - 3].m_normalCFM;
				break;
			default : 
				b3AssertConstrParams(0);
		}
	}
	else
	{
		b3AssertConstrParams(0);
	}
	return retVal;
}

 

void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyData* bodies)
{
	b3Vector3 zAxis = axis1.normalized();
	b3Vector3 yAxis = axis2.normalized();
	b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
	
	b3Transform frameInW;
	frameInW.setIdentity();
	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
	                                xAxis[1], yAxis[1], zAxis[1],
	                               xAxis[2], yAxis[2], zAxis[2]);
	
	// now get constraint frame in local coordinate systems
	m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
	m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
	
	calculateTransforms(bodies);
}