text:
/*
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.
*/

/*
2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
Pros:
- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
- Servo motor functionality
- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)

Cons:
- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation.
- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
*/

/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
/// Added support for generic constraint solver through getInfo1/getInfo2 methods

/*
2007-09-09
btGeneric6DofConstraint Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/

#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
#define BT_GENERIC_6DOF_CONSTRAINT2_H

#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"

class btRigidBody;

#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintDoubleData2
#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintDoubleData2"
#else
#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintData
#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintData"
#endif //BT_USE_DOUBLE_PRECISION

enum RotateOrder
{
	RO_XYZ=0,
	RO_XZY,
	RO_YXZ,
	RO_YZX,
	RO_ZXY,
	RO_ZYX
};

class btRotationalLimitMotor2
{
public:
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
	btScalar m_loLimit;
	btScalar m_hiLimit;
	btScalar m_bounce;
	btScalar m_stopERP;
	btScalar m_stopCFM;
	btScalar m_motorERP;
	btScalar m_motorCFM;
	bool     m_enableMotor;
	btScalar m_targetVelocity;
	btScalar m_maxMotorForce;
	bool     m_servoMotor;
	btScalar m_servoTarget;
	bool     m_enableSpring;
	btScalar m_springStiffness;
	bool     m_springStiffnessLimited;
	btScalar m_springDamping;
	bool     m_springDampingLimited;
	btScalar m_equilibriumPoint;

	btScalar m_currentLimitError;
	btScalar m_currentLimitErrorHi;
	btScalar m_currentPosition;
	int      m_currentLimit;

	btRotationalLimitMotor2()
	{
		m_loLimit                = 1.0f;
		m_hiLimit                = -1.0f;
		m_bounce                 = 0.0f;
		m_stopERP                = 0.2f;
		m_stopCFM                = 0.f;
		m_motorERP               = 0.9f;
		m_motorCFM               = 0.f;
		m_enableMotor            = false;
		m_targetVelocity         = 0;
		m_maxMotorForce          = 0.1f;
		m_servoMotor             = false;
		m_servoTarget            = 0;
		m_enableSpring           = false;
		m_springStiffness        = 0;
		m_springStiffnessLimited = false;
		m_springDamping          = 0;
		m_springDampingLimited   = false;
		m_equilibriumPoint       = 0;

		m_currentLimitError   = 0;
		m_currentLimitErrorHi = 0;
		m_currentPosition     = 0;
		m_currentLimit        = 0;
	}

	btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
	{
		m_loLimit                = limot.m_loLimit;
		m_hiLimit                = limot.m_hiLimit;
		m_bounce                 = limot.m_bounce;
		m_stopERP                = limot.m_stopERP;
		m_stopCFM                = limot.m_stopCFM;
		m_motorERP               = limot.m_motorERP;
		m_motorCFM               = limot.m_motorCFM;
		m_enableMotor            = limot.m_enableMotor;
		m_targetVelocity         = limot.m_targetVelocity;
		m_maxMotorForce          = limot.m_maxMotorForce;
		m_servoMotor             = limot.m_servoMotor;
		m_servoTarget            = limot.m_servoTarget;
		m_enableSpring           = limot.m_enableSpring;
		m_springStiffness        = limot.m_springStiffness;
		m_springStiffnessLimited = limot.m_springStiffnessLimited;
		m_springDamping          = limot.m_springDamping;
		m_springDampingLimited   = limot.m_springDampingLimited;
		m_equilibriumPoint       = limot.m_equilibriumPoint;

		m_currentLimitError   = limot.m_currentLimitError;
		m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
		m_currentPosition     = limot.m_currentPosition;
		m_currentLimit        = limot.m_currentLimit;
	}

	bool isLimited()
	{
		if(m_loLimit > m_hiLimit) return false;
		return true;
	}

	void testLimitValue(btScalar test_value);
};

class btTranslationalLimitMotor2
{
public:
// upper < lower means free
// upper == lower means locked
// upper > lower means limited
	btVector3 m_lowerLimit;
	btVector3 m_upperLimit;
	btVector3 m_bounce;
	btVector3 m_stopERP;
	btVector3 m_stopCFM;
	btVector3 m_motorERP;
	btVector3 m_motorCFM;
	bool      m_enableMotor[3];
	bool      m_servoMotor[3];
	bool      m_enableSpring[3];
	btVector3 m_servoTarget;
	btVector3 m_springStiffness;
	bool      m_springStiffnessLimited[3];
	btVector3 m_springDamping;
	bool      m_springDampingLimited[3];
	btVector3 m_equilibriumPoint;
	btVector3 m_targetVelocity;
	btVector3 m_maxMotorForce;

	btVector3 m_currentLimitError;
	btVector3 m_currentLimitErrorHi;
	btVector3 m_currentLinearDiff;
	int       m_currentLimit[3];

	btTranslationalLimitMotor2()
	{
		m_lowerLimit         .setValue(0.f , 0.f , 0.f );
		m_upperLimit         .setValue(0.f , 0.f , 0.f );
		m_bounce             .setValue(0.f , 0.f , 0.f );
		m_stopERP            .setValue(0.2f, 0.2f, 0.2f);
		m_stopCFM            .setValue(0.f , 0.f , 0.f );
		m_motorERP           .setValue(0.9f, 0.9f, 0.9f);
		m_motorCFM           .setValue(0.f , 0.f , 0.f );

		m_currentLimitError  .setValue(0.f , 0.f , 0.f );
		m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
		m_currentLinearDiff  .setValue(0.f , 0.f , 0.f );

		for(int i=0; i < 3; i++)
		{
			m_enableMotor[i]            = false;
			m_servoMotor[i]             = false;
			m_enableSpring[i]           = false;
			m_servoTarget[i]            = btScalar(0.f);
			m_springStiffness[i]        = btScalar(0.f);
			m_springStiffnessLimited[i] = false;
			m_springDamping[i]          = btScalar(0.f);
			m_springDampingLimited[i]   = false;
			m_equilibriumPoint[i]       = btScalar(0.f);
			m_targetVelocity[i]         = btScalar(0.f);
			m_maxMotorForce[i]          = btScalar(0.f);

			m_currentLimit[i]     = 0;
		}
	}

	btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
	{
		m_lowerLimit          = other.m_lowerLimit;
		m_upperLimit          = other.m_upperLimit;
		m_bounce              = other.m_bounce;
		m_stopERP             = other.m_stopERP;
		m_stopCFM             = other.m_stopCFM;
		m_motorERP            = other.m_motorERP;
		m_motorCFM            = other.m_motorCFM;

		m_currentLimitError   = other.m_currentLimitError;
		m_currentLimitErrorHi = other.m_currentLimitErrorHi;
		m_currentLinearDiff   = other.m_currentLinearDiff;

		for(int i=0; i < 3; i++)
		{
			m_enableMotor[i]            = other.m_enableMotor[i];
			m_servoMotor[i]             = other.m_servoMotor[i];
			m_enableSpring[i]           = other.m_enableSpring[i];
			m_servoTarget[i]            = other.m_servoTarget[i];
			m_springStiffness[i]        = other.m_springStiffness[i];
			m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
			m_springDamping[i]          = other.m_springDamping[i];
			m_springDampingLimited[i]   = other.m_springDampingLimited[i];
			m_equilibriumPoint[i]       = other.m_equilibriumPoint[i];
			m_targetVelocity[i]         = other.m_targetVelocity[i];
			m_maxMotorForce[i]          = other.m_maxMotorForce[i];

			m_currentLimit[i]     = other.m_currentLimit[i];
		}
	}

	inline bool isLimited(int limitIndex)
	{
		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
	}

	void testLimitValue(int limitIndex, btScalar test_value);
};

enum bt6DofFlags2
{
	BT_6DOF_FLAGS_CFM_STOP2 = 1,
	BT_6DOF_FLAGS_ERP_STOP2 = 2,
	BT_6DOF_FLAGS_CFM_MOTO2 = 4,
	BT_6DOF_FLAGS_ERP_MOTO2 = 8
};
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis

ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
{
protected:

	btTransform m_frameInA;
	btTransform m_frameInB;

	btJacobianEntry m_jacLinear[3];
	btJacobianEntry m_jacAng[3];

	btTranslationalLimitMotor2 m_linearLimits;
	btRotationalLimitMotor2 m_angularLimits[3];

	RotateOrder m_rotateOrder;

protected:

	btTransform  m_calculatedTransformA;
	btTransform  m_calculatedTransformB;
	btVector3    m_calculatedAxisAngleDiff;
	btVector3    m_calculatedAxis[3];
	btVector3    m_calculatedLinearDiff;
	btScalar     m_factA;
	btScalar     m_factB;
	bool         m_hasStaticBody;
	int          m_flags;

	btGeneric6DofSpring2Constraint&	operator=(btGeneric6DofSpring2Constraint&)
	{
		btAssert(0);
		return *this;
	}

	int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);

	void calculateLinearInfo();
	void calculateAngleInfo();
	void testAngularLimitMotor(int axis_index);

	void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
	int get_limit_motor_info2(btRotationalLimitMotor2* limot,
		const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
		btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

    btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
    btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);

	virtual void buildJacobian() {}
	virtual void getInfo1 (btConstraintInfo1* info);
	virtual void getInfo2 (btConstraintInfo2* info);
	virtual int calculateSerializeBufferSize() const;
	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;

	btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; }
	btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }

	// Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
	void calculateTransforms(const btTransform& transA,const btTransform& transB);
	void calculateTransforms();

	// Gets the global transform of the offset for body A
	const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
	// Gets the global transform of the offset for body B
	const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }

	const btTransform & getFrameOffsetA() const { return m_frameInA; }
	const btTransform & getFrameOffsetB() const { return m_frameInB; }

	btTransform & getFrameOffsetA() { return m_frameInA; }
	btTransform & getFrameOffsetB() { return m_frameInB; }

	// Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
	btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }

	// Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
	btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; }

	// Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
	btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }

	void setFrames(const btTransform & frameA, const btTransform & frameB);

	void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
	void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
	void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
	void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }

	void setAngularLowerLimit(const btVector3& angularLower)
	{
		for(int i = 0; i < 3; i++)
			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
	}

	void setAngularLowerLimitReversed(const btVector3& angularLower)
	{
		for(int i = 0; i < 3; i++)
			m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
	}

	void getAngularLowerLimit(btVector3& angularLower)
	{
		for(int i = 0; i < 3; i++)
			angularLower[i] = m_angularLimits[i].m_loLimit;
	}

	void getAngularLowerLimitReversed(btVector3& angularLower)
	{
		for(int i = 0; i < 3; i++)
			angularLower[i] = -m_angularLimits[i].m_hiLimit;
	}

	void setAngularUpperLimit(const btVector3& angularUpper)
	{
		for(int i = 0; i < 3; i++)
			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
	}

	void setAngularUpperLimitReversed(const btVector3& angularUpper)
	{
		for(int i = 0; i < 3; i++)
			m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
	}

	void getAngularUpperLimit(btVector3& angularUpper)
	{
		for(int i = 0; i < 3; i++)
			angularUpper[i] = m_angularLimits[i].m_hiLimit;
	}

	void getAngularUpperLimitReversed(btVector3& angularUpper)
	{
		for(int i = 0; i < 3; i++)
			angularUpper[i] = -m_angularLimits[i].m_loLimit;
	}

	//first 3 are linear, next 3 are angular

	void setLimit(int axis, btScalar lo, btScalar hi)
	{
		if(axis<3)
		{
			m_linearLimits.m_lowerLimit[axis] = lo;
			m_linearLimits.m_upperLimit[axis] = hi;
		}
		else
		{
			lo = btNormalizeAngle(lo);
			hi = btNormalizeAngle(hi);
			m_angularLimits[axis-3].m_loLimit = lo;
			m_angularLimits[axis-3].m_hiLimit = hi;
		}
	}

	void setLimitReversed(int axis, btScalar lo, btScalar hi)
	{
		if(axis<3)
		{
			m_linearLimits.m_lowerLimit[axis] = lo;
			m_linearLimits.m_upperLimit[axis] = hi;
		}
		else
		{
			lo = btNormalizeAngle(lo);
			hi = btNormalizeAngle(hi);
			m_angularLimits[axis-3].m_hiLimit = -lo;
			m_angularLimits[axis-3].m_loLimit = -hi;
		}
	}

	bool isLimited(int limitIndex)
	{
		if(limitIndex<3)
		{
			return m_linearLimits.isLimited(limitIndex);
		}
		return m_angularLimits[limitIndex-3].isLimited();
	}

	void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
	RotateOrder getRotationOrder() { return m_rotateOrder; }

	void setAxis( const btVector3& axis1, const btVector3& axis2);

	void setBounce(int index, btScalar bounce);

	void enableMotor(int index, bool onOff);
	void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
	void setTargetVelocity(int index, btScalar velocity);
	void setServoTarget(int index, btScalar target);
	void setMaxMotorForce(int index, btScalar force);

	void enableSpring(int index, bool onOff);
	void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
	void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
	void setEquilibriumPoint(int index, btScalar val);

	//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.
	virtual void setParam(int num, btScalar value, int axis = -1);
	virtual btScalar getParam(int num, int axis = -1) const;

    static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
    static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
    static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
    static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
    static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
    static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
    static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
};

struct btGeneric6DofSpring2ConstraintData
{
	btTypedConstraintData m_typeConstraintData;
	btTransformFloatData m_rbAFrame;
	btTransformFloatData m_rbBFrame;

	btVector3FloatData m_linearUpperLimit;
	btVector3FloatData m_linearLowerLimit;
	btVector3FloatData m_linearBounce;
	btVector3FloatData m_linearStopERP;
	btVector3FloatData m_linearStopCFM;
	btVector3FloatData m_linearMotorERP;
	btVector3FloatData m_linearMotorCFM;
	btVector3FloatData m_linearTargetVelocity;
	btVector3FloatData m_linearMaxMotorForce;
	btVector3FloatData m_linearServoTarget;
	btVector3FloatData m_linearSpringStiffness;
	btVector3FloatData m_linearSpringDamping;
	btVector3FloatData m_linearEquilibriumPoint;
	char               m_linearEnableMotor[4];
	char               m_linearServoMotor[4];
	char               m_linearEnableSpring[4];
	char               m_linearSpringStiffnessLimited[4];
	char               m_linearSpringDampingLimited[4];
	char               m_padding1[4];

	btVector3FloatData m_angularUpperLimit;
	btVector3FloatData m_angularLowerLimit;
	btVector3FloatData m_angularBounce;
	btVector3FloatData m_angularStopERP;
	btVector3FloatData m_angularStopCFM;
	btVector3FloatData m_angularMotorERP;
	btVector3FloatData m_angularMotorCFM;
	btVector3FloatData m_angularTargetVelocity;
	btVector3FloatData m_angularMaxMotorForce;
	btVector3FloatData m_angularServoTarget;
	btVector3FloatData m_angularSpringStiffness;
	btVector3FloatData m_angularSpringDamping;
	btVector3FloatData m_angularEquilibriumPoint;
	char               m_angularEnableMotor[4];
	char               m_angularServoMotor[4];
	char               m_angularEnableSpring[4];
	char               m_angularSpringStiffnessLimited[4];
	char               m_angularSpringDampingLimited[4];

	int                m_rotateOrder;
};

struct btGeneric6DofSpring2ConstraintDoubleData2
{
	btTypedConstraintDoubleData m_typeConstraintData;
	btTransformDoubleData m_rbAFrame;
	btTransformDoubleData m_rbBFrame;

	btVector3DoubleData m_linearUpperLimit;
	btVector3DoubleData m_linearLowerLimit;
	btVector3DoubleData m_linearBounce;
	btVector3DoubleData m_linearStopERP;
	btVector3DoubleData m_linearStopCFM;
	btVector3DoubleData m_linearMotorERP;
	btVector3DoubleData m_linearMotorCFM;
	btVector3DoubleData m_linearTargetVelocity;
	btVector3DoubleData m_linearMaxMotorForce;
	btVector3DoubleData m_linearServoTarget;
	btVector3DoubleData m_linearSpringStiffness;
	btVector3DoubleData m_linearSpringDamping;
	btVector3DoubleData m_linearEquilibriumPoint;
	char                m_linearEnableMotor[4];
	char                m_linearServoMotor[4];
	char                m_linearEnableSpring[4];
	char                m_linearSpringStiffnessLimited[4];
	char                m_linearSpringDampingLimited[4];
	char                m_padding1[4];

	btVector3DoubleData m_angularUpperLimit;
	btVector3DoubleData m_angularLowerLimit;
	btVector3DoubleData m_angularBounce;
	btVector3DoubleData m_angularStopERP;
	btVector3DoubleData m_angularStopCFM;
	btVector3DoubleData m_angularMotorERP;
	btVector3DoubleData m_angularMotorCFM;
	btVector3DoubleData m_angularTargetVelocity;
	btVector3DoubleData m_angularMaxMotorForce;
	btVector3DoubleData m_angularServoTarget;
	btVector3DoubleData m_angularSpringStiffness;
	btVector3DoubleData m_angularSpringDamping;
	btVector3DoubleData m_angularEquilibriumPoint;
	char                m_angularEnableMotor[4];
	char                m_angularServoMotor[4];
	char                m_angularEnableSpring[4];
	char                m_angularSpringStiffnessLimited[4];
	char                m_angularSpringDampingLimited[4];

	int                 m_rotateOrder;
};

SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
{
	return sizeof(btGeneric6DofSpring2ConstraintData2);
}

SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
	btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);

	m_frameInA.serialize(dof->m_rbAFrame);
	m_frameInB.serialize(dof->m_rbBFrame);

	int i;
	for (i=0;i<3;i++)
	{
		dof->m_angularLowerLimit.m_floats[i]       = m_angularLimits[i].m_loLimit;
		dof->m_angularUpperLimit.m_floats[i]       = m_angularLimits[i].m_hiLimit;
		dof->m_angularBounce.m_floats[i]           = m_angularLimits[i].m_bounce;
		dof->m_angularStopERP.m_floats[i]          = m_angularLimits[i].m_stopERP;
		dof->m_angularStopCFM.m_floats[i]          = m_angularLimits[i].m_stopCFM;
		dof->m_angularMotorERP.m_floats[i]         = m_angularLimits[i].m_motorERP;
		dof->m_angularMotorCFM.m_floats[i]         = m_angularLimits[i].m_motorCFM;
		dof->m_angularTargetVelocity.m_floats[i]   = m_angularLimits[i].m_targetVelocity;
		dof->m_angularMaxMotorForce.m_floats[i]    = m_angularLimits[i].m_maxMotorForce;
		dof->m_angularServoTarget.m_floats[i]      = m_angularLimits[i].m_servoTarget;
		dof->m_angularSpringStiffness.m_floats[i]  = m_angularLimits[i].m_springStiffness;
		dof->m_angularSpringDamping.m_floats[i]    = m_angularLimits[i].m_springDamping;
		dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
	}
	dof->m_angularLowerLimit.m_floats[3]       = 0;
	dof->m_angularUpperLimit.m_floats[3]       = 0;
	dof->m_angularBounce.m_floats[3]           = 0;
	dof->m_angularStopERP.m_floats[3]          = 0;
	dof->m_angularStopCFM.m_floats[3]          = 0;
	dof->m_angularMotorERP.m_floats[3]         = 0;
	dof->m_angularMotorCFM.m_floats[3]         = 0;
	dof->m_angularTargetVelocity.m_floats[3]   = 0;
	dof->m_angularMaxMotorForce.m_floats[3]    = 0;
	dof->m_angularServoTarget.m_floats[3]      = 0;
	dof->m_angularSpringStiffness.m_floats[3]  = 0;
	dof->m_angularSpringDamping.m_floats[3]    = 0;
	dof->m_angularEquilibriumPoint.m_floats[3] = 0;
	for (i=0;i<4;i++)
	{
		dof->m_angularEnableMotor[i]            = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
		dof->m_angularServoMotor[i]             = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
		dof->m_angularEnableSpring[i]           = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
		dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
		dof->m_angularSpringDampingLimited[i]   = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
	}

	m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
	m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
	m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
	m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
	m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
	m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
	m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
	m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
	m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
	m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
	m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
	m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
	m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
	for (i=0;i<4;i++)
	{
		dof->m_linearEnableMotor[i]            = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
		dof->m_linearServoMotor[i]             = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
		dof->m_linearEnableSpring[i]           = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
		dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
		dof->m_linearSpringDampingLimited[i]   = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
	}

	dof->m_rotateOrder = m_rotateOrder;

	dof->m_padding1[0] = 0;
	dof->m_padding1[1] = 0;
	dof->m_padding1[2] = 0;
	dof->m_padding1[3] = 0;

	return btGeneric6DofSpring2ConstraintDataName;
}

#endif //BT_GENERIC_6DOF_CONSTRAINT_H


text:
/*
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.
*/

#ifndef BT_SOLVER_CONSTRAINT_H
#define BT_SOLVER_CONSTRAINT_H

class	btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "btJacobianEntry.h"
#include "LinearMath/btAlignedObjectArray.h"

//#define NO_FRICTION_TANGENTIALS 1
#include "btSolverBody.h"

///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
{
	BT_DECLARE_ALIGNED_ALLOCATOR();

	btVector3		m_relpos1CrossNormal;
	btVector3		m_contactNormal1;

	btVector3		m_relpos2CrossNormal;
	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always

	btVector3		m_angularComponentA;
	btVector3		m_angularComponentB;

	mutable btSimdScalar	m_appliedPushImpulse;
	mutable btSimdScalar	m_appliedImpulse;

	btScalar	m_friction;
	btScalar	m_jacDiagABInv;
	btScalar		m_rhs;
	btScalar		m_cfm;

    btScalar		m_lowerLimit;
	btScalar		m_upperLimit;
	btScalar		m_rhsPenetration;
    union
	{
		void*		m_originalContactPoint;
		btScalar	m_unusedPadding4;
		int			m_numRowsForNonContactConstraint;
	};

	int	m_overrideNumSolverIterations;
    int			m_frictionIndex;
	int m_solverBodyIdA;
	int m_solverBodyIdB;

	enum		btSolverConstraintType
	{
		BT_SOLVER_CONTACT_1D = 0,
		BT_SOLVER_FRICTION_1D
	};
};

typedef btAlignedObjectArray<btSolverConstraint>	btConstraintArray;

#endif //BT_SOLVER_CONSTRAINT_H

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


text:
/*
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.
*/

/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */

#ifndef BT_HINGECONSTRAINT_H
#define BT_HINGECONSTRAINT_H

#define _BT_USE_CENTER_LIMIT_ 1

#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"

class btRigidBody;

#ifdef BT_USE_DOUBLE_PRECISION
#define btHingeConstraintData	btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
#define btHingeConstraintDataName	"btHingeConstraintDoubleData2"
#else
#define btHingeConstraintData	btHingeConstraintFloatData
#define btHingeConstraintDataName	"btHingeConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION

enum btHingeFlags
{
	BT_HINGE_FLAGS_CFM_STOP = 1,
	BT_HINGE_FLAGS_ERP_STOP = 2,
	BT_HINGE_FLAGS_CFM_NORM = 4,
	BT_HINGE_FLAGS_ERP_NORM = 8
};

/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/// axis defines the orientation of the hinge axis
ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
#endif
	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor

	btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
	btTransform m_rbBFrame;

	btScalar	m_motorTargetVelocity;
	btScalar	m_maxMotorImpulse;

#ifdef	_BT_USE_CENTER_LIMIT_
	btAngularLimit	m_limit;
#else
	btScalar	m_lowerLimit;
	btScalar	m_upperLimit;
	btScalar	m_limitSign;
	btScalar	m_correction;

	btScalar	m_limitSoftness;
	btScalar	m_biasFactor;
	btScalar	m_relaxationFactor;

	bool		m_solveLimit;
#endif

	btScalar	m_kHinge;

	btScalar	m_accLimitImpulse;
	btScalar	m_hingeAngle;
	btScalar	m_referenceSign;

	bool		m_angularOnly;
	bool		m_enableAngularMotor;
	bool		m_useSolveConstraintObsolete;
	bool		m_useOffsetForConstraintFrame;
	bool		m_useReferenceFrameA;

	btScalar	m_accMotorImpulse;

	int			m_flags;
	btScalar	m_normalCFM;
	btScalar	m_normalERP;
	btScalar	m_stopCFM;
	btScalar	m_stopERP;

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);

	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);

	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);

	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);

	virtual void	buildJacobian();

	virtual void getInfo1 (btConstraintInfo1* info);

	void getInfo1NonVirtual(btConstraintInfo1* info);

	virtual void getInfo2 (btConstraintInfo2* info);

	void	getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);

	void	getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
	void	getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);

	void	updateRHS(btScalar	timeStep);

	const btRigidBody& getRigidBodyA() const
	{
		return m_rbA;
	}
	const btRigidBody& getRigidBodyB() const
	{
		return m_rbB;
	}

	btRigidBody& getRigidBodyA()
	{
		return m_rbA;
	}

	btRigidBody& getRigidBodyB()
	{
		return m_rbB;
	}

	btTransform& getFrameOffsetA()
	{
	return m_rbAFrame;
	}

	btTransform& getFrameOffsetB()
	{
		return m_rbBFrame;
	}

	void setFrames(const btTransform& frameA, const btTransform& frameB);

	void	setAngularOnly(bool angularOnly)
	{
		m_angularOnly = angularOnly;
	}

	void	enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
	{
		m_enableAngularMotor  = enableMotor;
		m_motorTargetVelocity = targetVelocity;
		m_maxMotorImpulse = maxMotorImpulse;
	}

	// extra motor API, including ability to set a target rotation (as opposed to angular velocity)
	// note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
	//       maintain a given angular target.
	void enableMotor(bool enableMotor) 	{ m_enableAngularMotor = enableMotor; }
	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
	void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; }
	void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
	void setMotorTarget(btScalar targetAngle, btScalar dt);

	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
	{
#ifdef	_BT_USE_CENTER_LIMIT_
		m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
#else
		m_lowerLimit = btNormalizeAngle(low);
		m_upperLimit = btNormalizeAngle(high);
		m_limitSoftness =  _softness;
		m_biasFactor = _biasFactor;
		m_relaxationFactor = _relaxationFactor;
#endif
	}

	btScalar getLimitSoftness() const
	{
#ifdef	_BT_USE_CENTER_LIMIT_
		return m_limit.getSoftness();
#else
		return m_limitSoftness;
#endif
	}

	btScalar getLimitBiasFactor() const
	{
#ifdef	_BT_USE_CENTER_LIMIT_
		return m_limit.getBiasFactor();
#else
		return m_biasFactor;
#endif
	}

	btScalar getLimitRelaxationFactor() const
	{
#ifdef	_BT_USE_CENTER_LIMIT_
		return m_limit.getRelaxationFactor();
#else
		return m_relaxationFactor;
#endif
	}

	void	setAxis(btVector3& axisInA)
	{
		btVector3 rbAxisA1, rbAxisA2;
		btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
		btVector3 pivotInA = m_rbAFrame.getOrigin();
//		m_rbAFrame.getOrigin() = pivotInA;
		m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
										rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
										rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

		btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;

		btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
		btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
		btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);

		m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));

		m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
										rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
										rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
		m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();

	}

    bool hasLimit() const {
#ifdef  _BT_USE_CENTER_LIMIT_
        return m_limit.getHalfRange() > 0;
#else
        return m_lowerLimit <= m_upperLimit;
#endif
    }

	btScalar	getLowerLimit() const
	{
#ifdef	_BT_USE_CENTER_LIMIT_
	return m_limit.getLow();
#else
	return m_lowerLimit;
#endif
	}

	btScalar	getUpperLimit() const
	{
#ifdef	_BT_USE_CENTER_LIMIT_
	return m_limit.getHigh();
#else
	return m_upperLimit;
#endif
	}

	///The getHingeAngle gives the hinge angle in range [-PI,PI]
	btScalar getHingeAngle();

	btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);

	void testLimit(const btTransform& transA,const btTransform& transB);

	const btTransform& getAFrame() const { return m_rbAFrame; };
	const btTransform& getBFrame() const { return m_rbBFrame; };

	btTransform& getAFrame() { return m_rbAFrame; };
	btTransform& getBFrame() { return m_rbBFrame; };

	inline int getSolveLimit()
	{
#ifdef	_BT_USE_CENTER_LIMIT_
	return m_limit.isLimit();
#else
	return m_solveLimit;
#endif
	}

	inline btScalar getLimitSign()
	{
#ifdef	_BT_USE_CENTER_LIMIT_
	return m_limit.getSign();
#else
		return m_limitSign;
#endif
	}

	inline bool getAngularOnly()
	{
		return m_angularOnly;
	}
	inline bool getEnableAngularMotor()
	{
		return m_enableAngularMotor;
	}
	inline btScalar getMotorTargetVelocity()
	{
		return m_motorTargetVelocity;
	}
	inline btScalar getMaxMotorImpulse()
	{
		return m_maxMotorImpulse;
	}
	// access for UseFrameOffset
	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
	// access for UseReferenceFrameA
	bool getUseReferenceFrameA() const { return m_useReferenceFrameA; }
	void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; }

	///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.
	virtual	void	setParam(int num, btScalar value, int axis = -1);
	///return the local value of parameter
	virtual	btScalar getParam(int num, int axis = -1) const;

	virtual	int getFlags() const
	{
  	    return m_flags;
	}

	virtual	int	calculateSerializeBufferSize() const;

	///fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;

};

//only for backward compatibility
#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///this structure is not used, except for loading pre-2.82 .bullet files
struct	btHingeConstraintDoubleData
{
	btTypedConstraintData	m_typeConstraintData;
	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
	btTransformDoubleData m_rbBFrame;
	int			m_useReferenceFrameA;
	int			m_angularOnly;
	int			m_enableAngularMotor;
	float	m_motorTargetVelocity;
	float	m_maxMotorImpulse;

	float	m_lowerLimit;
	float	m_upperLimit;
	float	m_limitSoftness;
	float	m_biasFactor;
	float	m_relaxationFactor;

};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION

///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
{
protected:
	btScalar	m_accumulatedAngle;
public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
	:btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
	{
		m_accumulatedAngle=getHingeAngle();
	}

	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
	:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
	{
		m_accumulatedAngle=getHingeAngle();
	}

	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
	:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
	{
		m_accumulatedAngle=getHingeAngle();
	}

	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
	:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
	{
		m_accumulatedAngle=getHingeAngle();
	}
	btScalar getAccumulatedHingeAngle();
	void	setAccumulatedHingeAngle(btScalar accAngle);
	virtual void getInfo1 (btConstraintInfo1* info);

};

struct	btHingeConstraintFloatData
{
	btTypedConstraintData	m_typeConstraintData;
	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
	btTransformFloatData m_rbBFrame;
	int			m_useReferenceFrameA;
	int			m_angularOnly;

	int			m_enableAngularMotor;
	float	m_motorTargetVelocity;
	float	m_maxMotorImpulse;

	float	m_lowerLimit;
	float	m_upperLimit;
	float	m_limitSoftness;
	float	m_biasFactor;
	float	m_relaxationFactor;

};

///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct	btHingeConstraintDoubleData2
{
	btTypedConstraintDoubleData	m_typeConstraintData;
	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
	btTransformDoubleData m_rbBFrame;
	int			m_useReferenceFrameA;
	int			m_angularOnly;
	int			m_enableAngularMotor;
	double		m_motorTargetVelocity;
	double		m_maxMotorImpulse;

	double		m_lowerLimit;
	double		m_upperLimit;
	double		m_limitSoftness;
	double		m_biasFactor;
	double		m_relaxationFactor;
	char	m_padding1[4];

};

SIMD_FORCE_INLINE	int	btHingeConstraint::calculateSerializeBufferSize() const
{
	return sizeof(btHingeConstraintData);
}

	///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
	btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
	btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);

	m_rbAFrame.serialize(hingeData->m_rbAFrame);
	m_rbBFrame.serialize(hingeData->m_rbBFrame);

	hingeData->m_angularOnly = m_angularOnly;
	hingeData->m_enableAngularMotor = m_enableAngularMotor;
	hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
	hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
	hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
#ifdef	_BT_USE_CENTER_LIMIT_
	hingeData->m_lowerLimit = float(m_limit.getLow());
	hingeData->m_upperLimit = float(m_limit.getHigh());
	hingeData->m_limitSoftness = float(m_limit.getSoftness());
	hingeData->m_biasFactor = float(m_limit.getBiasFactor());
	hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
#else
	hingeData->m_lowerLimit = float(m_lowerLimit);
	hingeData->m_upperLimit = float(m_upperLimit);
	hingeData->m_limitSoftness = float(m_limitSoftness);
	hingeData->m_biasFactor = float(m_biasFactor);
	hingeData->m_relaxationFactor = float(m_relaxationFactor);
#endif

	// Fill padding with zeros to appease msan.
#ifdef BT_USE_DOUBLE_PRECISION
	hingeData->m_padding1[0] = 0;
	hingeData->m_padding1[1] = 0;
	hingeData->m_padding1[2] = 0;
	hingeData->m_padding1[3] = 0;
#endif

	return btHingeConstraintDataName;
}

#endif //BT_HINGECONSTRAINT_H


text:
/*
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.
*/

#ifndef BT_SOLVER_BODY_H
#define BT_SOLVER_BODY_H

class	btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"

#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.h"

///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#ifdef BT_USE_SSE
#define USE_SIMD 1
#endif //

#ifdef USE_SIMD

struct	btSimdScalar
{
	SIMD_FORCE_INLINE	btSimdScalar()
	{

	}

	SIMD_FORCE_INLINE	btSimdScalar(float	fl)
	:m_vec128 (_mm_set1_ps(fl))
	{
	}

	SIMD_FORCE_INLINE	btSimdScalar(__m128 v128)
		:m_vec128(v128)
	{
	}
	union
	{
		__m128		m_vec128;
		float		m_floats[4];
		int			m_ints[4];
		btScalar	m_unusedPadding;
	};
	SIMD_FORCE_INLINE	__m128	get128()
	{
		return m_vec128;
	}

	SIMD_FORCE_INLINE	const __m128	get128() const
	{
		return m_vec128;
	}

	SIMD_FORCE_INLINE	void	set128(__m128 v128)
	{
		m_vec128 = v128;
	}

	SIMD_FORCE_INLINE	operator       __m128()
	{
		return m_vec128;
	}
	SIMD_FORCE_INLINE	operator const __m128() const
	{
		return m_vec128;
	}

	SIMD_FORCE_INLINE	operator float() const
	{
		return m_floats[0];
	}

};

///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
{
	return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
}

///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
{
	return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
}

#else
#define btSimdScalar btScalar
#endif

///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
{
	BT_DECLARE_ALIGNED_ALLOCATOR();
	btTransform		m_worldTransform;
	btVector3		m_deltaLinearVelocity;
	btVector3		m_deltaAngularVelocity;
	btVector3		m_angularFactor;
	btVector3		m_linearFactor;
	btVector3		m_invMass;
	btVector3		m_pushVelocity;
	btVector3		m_turnVelocity;
	btVector3		m_linearVelocity;
	btVector3		m_angularVelocity;
	btVector3		m_externalForceImpulse;
	btVector3		m_externalTorqueImpulse;

	btRigidBody*	m_originalBody;
	void	setWorldTransform(const btTransform& worldTransform)
	{
		m_worldTransform = worldTransform;
	}

	const btTransform& getWorldTransform() const
	{
		return m_worldTransform;
	}

	SIMD_FORCE_INLINE void	getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const
	{
		if (m_originalBody)
			velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos);
		else
			velocity.setValue(0,0,0);
	}

	SIMD_FORCE_INLINE void	getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
	{
		if (m_originalBody)
			velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
		else
			velocity.setValue(0,0,0);
	}

	SIMD_FORCE_INLINE void	getAngularVelocity(btVector3& angVel) const
	{
		if (m_originalBody)
			angVel =m_angularVelocity+m_deltaAngularVelocity;
		else
			angVel.setValue(0,0,0);
	}

	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
	{
		if (m_originalBody)
		{
			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
		}
	}

	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
	{
		if (m_originalBody)
		{
			m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
		}
	}

	const btVector3& getDeltaLinearVelocity() const
	{
		return m_deltaLinearVelocity;
	}

	const btVector3& getDeltaAngularVelocity() const
	{
		return m_deltaAngularVelocity;
	}

	const btVector3& getPushVelocity() const
	{
		return m_pushVelocity;
	}

	const btVector3& getTurnVelocity() const
	{
		return m_turnVelocity;
	}

	////////////////////////////////////////////////
	///some internal methods, don't use them

	btVector3& internalGetDeltaLinearVelocity()
	{
		return m_deltaLinearVelocity;
	}

	btVector3& internalGetDeltaAngularVelocity()
	{
		return m_deltaAngularVelocity;
	}

	const btVector3& internalGetAngularFactor() const
	{
		return m_angularFactor;
	}

	const btVector3& internalGetInvMass() const
	{
		return m_invMass;
	}

	void internalSetInvMass(const btVector3& invMass)
	{
		m_invMass = invMass;
	}

	btVector3& internalGetPushVelocity()
	{
		return m_pushVelocity;
	}

	btVector3& internalGetTurnVelocity()
	{
		return m_turnVelocity;
	}

	SIMD_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
	{
		velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
	}

	SIMD_FORCE_INLINE void	internalGetAngularVelocity(btVector3& angVel) const
	{
		angVel = m_angularVelocity+m_deltaAngularVelocity;
	}

	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
	{
		if (m_originalBody)
		{
			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
		}
	}

	void	writebackVelocity()
	{
		if (m_originalBody)
		{
			m_linearVelocity +=m_deltaLinearVelocity;
			m_angularVelocity += m_deltaAngularVelocity;

			//m_originalBody->setCompanionId(-1);
		}
	}

	void	writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
	{
        (void) timeStep;
		if (m_originalBody)
		{
			m_linearVelocity += m_deltaLinearVelocity;
			m_angularVelocity += m_deltaAngularVelocity;

			//correct the position/orientation based on push/turn recovery
			btTransform newTransform;
			if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
			{
			//	btQuaternion orn = m_worldTransform.getRotation();
				btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
				m_worldTransform = newTransform;
			}
			//m_worldTransform.setRotation(orn);
			//m_originalBody->setCompanionId(-1);
		}
	}

};

#endif //BT_SOLVER_BODY_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org

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.
*/

#ifndef BT_FIXED_CONSTRAINT_H
#define BT_FIXED_CONSTRAINT_H

#include "btGeneric6DofSpring2Constraint.h"

ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
{

public:
	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);

	virtual ~btFixedConstraint();

};

#endif //BT_FIXED_CONSTRAINT_H


text:
/*
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.
*/

#ifndef BT_SOLVE_2LINEAR_CONSTRAINT_H
#define BT_SOLVE_2LINEAR_CONSTRAINT_H

#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btVector3.h"

class btRigidBody;

/// constraint class used for lateral tyre friction.
class	btSolve2LinearConstraint
{
	btScalar	m_tau;
	btScalar	m_damping;

public:

	btSolve2LinearConstraint(btScalar tau,btScalar damping)
	{
		m_tau = tau;
		m_damping = damping;
	}
	//
	// solve unilateral constraint (equality, direct method)
	//
	void resolveUnilateralPairConstraint(
														   btRigidBody* body0,
		btRigidBody* body1,

		const btMatrix3x3& world2A,
						const btMatrix3x3& world2B,

						const btVector3& invInertiaADiag,
						const btScalar invMassA,
						const btVector3& linvelA,const btVector3& angvelA,
						const btVector3& rel_posA1,
						const btVector3& invInertiaBDiag,
						const btScalar invMassB,
						const btVector3& linvelB,const btVector3& angvelB,
						const btVector3& rel_posA2,

					  btScalar depthA, const btVector3& normalA,
					  const btVector3& rel_posB1,const btVector3& rel_posB2,
					  btScalar depthB, const btVector3& normalB,
					  btScalar& imp0,btScalar& imp1);

	//
	// solving 2x2 lcp problem (inequality, direct solution )
	//
	void resolveBilateralPairConstraint(
			btRigidBody* body0,
						btRigidBody* body1,
		const btMatrix3x3& world2A,
						const btMatrix3x3& world2B,

						const btVector3& invInertiaADiag,
						const btScalar invMassA,
						const btVector3& linvelA,const btVector3& angvelA,
						const btVector3& rel_posA1,
						const btVector3& invInertiaBDiag,
						const btScalar invMassB,
						const btVector3& linvelB,const btVector3& angvelB,
						const btVector3& rel_posA2,

					  btScalar depthA, const btVector3& normalA,
					  const btVector3& rel_posB1,const btVector3& rel_posB2,
					  btScalar depthB, const btVector3& normalB,
					  btScalar& imp0,btScalar& imp1);

/*
	void resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
						const btScalar invMassA,
						const btVector3& linvelA,const btVector3& angvelA,
						const btVector3& rel_posA1,
						const btMatrix3x3& invInertiaBWS,
						const btScalar invMassB,
						const btVector3& linvelB,const btVector3& angvelB,
						const btVector3& rel_posA2,

					  btScalar depthA, const btVector3& normalA,
					  const btVector3& rel_posB1,const btVector3& rel_posB2,
					  btScalar depthB, const btVector3& normalB,
					  btScalar& imp0,btScalar& imp1);

*/

};

#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H


text:
/*
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.
*/

#ifndef BT_CONTACT_CONSTRAINT_H
#define BT_CONTACT_CONSTRAINT_H

#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"

///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
{
protected:

	btPersistentManifold m_contactManifold;

protected:

	btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);

public:

	void	setContactManifold(btPersistentManifold* contactManifold);

	btPersistentManifold* getContactManifold()
	{
		return &m_contactManifold;
	}

	const btPersistentManifold* getContactManifold() const
	{
		return &m_contactManifold;
	}

	virtual ~btContactConstraint();

	virtual void getInfo1 (btConstraintInfo1* info);

	virtual void getInfo2 (btConstraintInfo2* info);

	///obsolete methods
	virtual void	buildJacobian();

};

///very basic collision resolution without friction
btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance);

///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
                      btRigidBody& body2, const btVector3& pos2,
                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);

#endif //BT_CONTACT_CONSTRAINT_H


text:
/*
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.
*/

#ifndef BT_POINT2POINTCONSTRAINT_H
#define BT_POINT2POINTCONSTRAINT_H

#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"

class btRigidBody;

#ifdef BT_USE_DOUBLE_PRECISION
#define btPoint2PointConstraintData2	btPoint2PointConstraintDoubleData2
#define btPoint2PointConstraintDataName	"btPoint2PointConstraintDoubleData2"
#else
#define btPoint2PointConstraintData2	btPoint2PointConstraintFloatData
#define btPoint2PointConstraintDataName	"btPoint2PointConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION

struct	btConstraintSetting
{
	btConstraintSetting()	:
		m_tau(btScalar(0.3)),
		m_damping(btScalar(1.)),
		m_impulseClamp(btScalar(0.))
	{
	}
	btScalar		m_tau;
	btScalar		m_damping;
	btScalar		m_impulseClamp;
};

enum btPoint2PointFlags
{
	BT_P2P_FLAGS_ERP = 1,
	BT_P2P_FLAGS_CFM = 2
};

/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
#endif
	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints

	btVector3	m_pivotInA;
	btVector3	m_pivotInB;

	int			m_flags;
	btScalar	m_erp;
	btScalar	m_cfm;

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	///for backwards compatibility during the transition to 'getInfo/getInfo2'
	bool		m_useSolveConstraintObsolete;

	btConstraintSetting	m_setting;

	btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);

	btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);

	virtual void	buildJacobian();

	virtual void getInfo1 (btConstraintInfo1* info);

	void getInfo1NonVirtual (btConstraintInfo1* info);

	virtual void getInfo2 (btConstraintInfo2* info);

	void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);

	void	updateRHS(btScalar	timeStep);

	void	setPivotA(const btVector3& pivotA)
	{
		m_pivotInA = pivotA;
	}

	void	setPivotB(const btVector3& pivotB)
	{
		m_pivotInB = pivotB;
	}

	const btVector3& getPivotInA() const
	{
		return m_pivotInA;
	}

	const btVector3& getPivotInB() const
	{
		return m_pivotInB;
	}

	///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.
	virtual	void	setParam(int num, btScalar value, int axis = -1);
	///return the local value of parameter
	virtual	btScalar getParam(int num, int axis = -1) const;

	virtual	int getFlags() const
	{
        	return m_flags;
    	}

	virtual	int	calculateSerializeBufferSize() const;

	///fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;

};

///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct	btPoint2PointConstraintFloatData
{
	btTypedConstraintData	m_typeConstraintData;
	btVector3FloatData	m_pivotInA;
	btVector3FloatData	m_pivotInB;
};

///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct	btPoint2PointConstraintDoubleData2
{
	btTypedConstraintDoubleData	m_typeConstraintData;
	btVector3DoubleData	m_pivotInA;
	btVector3DoubleData	m_pivotInB;
};

#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
///this structure is not used, except for loading pre-2.82 .bullet files
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct	btPoint2PointConstraintDoubleData
{
	btTypedConstraintData	m_typeConstraintData;
	btVector3DoubleData	m_pivotInA;
	btVector3DoubleData	m_pivotInB;
};
#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION

SIMD_FORCE_INLINE	int	btPoint2PointConstraint::calculateSerializeBufferSize() const
{
	return sizeof(btPoint2PointConstraintData2);

}

	///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE	const char*	btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
	btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;

	btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
	m_pivotInA.serialize(p2pData->m_pivotInA);
	m_pivotInB.serialize(p2pData->m_pivotInB);

	return btPoint2PointConstraintDataName;
}

#endif //BT_POINT2POINTCONSTRAINT_H


text:
/*
Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.

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.
*/

#ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
#define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H

#include "LinearMath/btVector3.h"
#include "btTypedConstraint.h"
#include "btGeneric6DofConstraint.h"

#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofSpringConstraintData2		btGeneric6DofSpringConstraintDoubleData2
#define btGeneric6DofSpringConstraintDataName	"btGeneric6DofSpringConstraintDoubleData2"
#else
#define btGeneric6DofSpringConstraintData2		btGeneric6DofSpringConstraintData
#define btGeneric6DofSpringConstraintDataName	"btGeneric6DofSpringConstraintData"
#endif //BT_USE_DOUBLE_PRECISION

/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF

/// DOF index used in enableSpring() and setStiffness() means:
/// 0 : translation X
/// 1 : translation Y
/// 2 : translation Z
/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] )
/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )

ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
{
protected:
	bool		m_springEnabled[6];
	btScalar	m_equilibriumPoint[6];
	btScalar	m_springStiffness[6];
	btScalar	m_springDamping[6]; // between 0 and 1 (1 == no damping)
	void init();
	void internalUpdateSprings(btConstraintInfo2* info);
public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

    btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
    btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
	void enableSpring(int index, bool onOff);
	void setStiffness(int index, btScalar stiffness);
	void setDamping(int index, btScalar damping);
	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
	void setEquilibriumPoint(int index, btScalar val);

	bool isSpringEnabled(int index) const
	{
	    return m_springEnabled[index];
	}

	btScalar getStiffness(int index) const
	{
	    return m_springStiffness[index];
	}

	btScalar getDamping(int index) const
	{
	    return m_springDamping[index];
	}

	btScalar getEquilibriumPoint(int index) const
	{
	    return m_equilibriumPoint[index];
	}

	virtual void setAxis( const btVector3& axis1, const btVector3& axis2);

	virtual void getInfo2 (btConstraintInfo2* info);

	virtual	int	calculateSerializeBufferSize() const;
	///fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;

};

struct btGeneric6DofSpringConstraintData
{
	btGeneric6DofConstraintData	m_6dofData;

	int			m_springEnabled[6];
	float		m_equilibriumPoint[6];
	float		m_springStiffness[6];
	float		m_springDamping[6];
};

struct btGeneric6DofSpringConstraintDoubleData2
{
	btGeneric6DofConstraintDoubleData2	m_6dofData;

	int			m_springEnabled[6];
	double		m_equilibriumPoint[6];
	double		m_springStiffness[6];
	double		m_springDamping[6];
};

SIMD_FORCE_INLINE	int	btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
{
	return sizeof(btGeneric6DofSpringConstraintData2);
}

	///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE	const char*	btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{
	btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
	btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);

	int i;
	for (i=0;i<6;i++)
	{
		dof->m_equilibriumPoint[i] = m_equilibriumPoint[i];
		dof->m_springDamping[i] = m_springDamping[i];
		dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0;
		dof->m_springStiffness[i] = m_springStiffness[i];
	}
	return btGeneric6DofSpringConstraintDataName;
}

#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H


text:
/*
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.
*/

/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
/// Added support for generic constraint solver through getInfo1/getInfo2 methods

/*
2007-09-09
btGeneric6DofConstraint Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/

#ifndef BT_GENERIC_6DOF_CONSTRAINT_H
#define BT_GENERIC_6DOF_CONSTRAINT_H

#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btTypedConstraint.h"

class btRigidBody;

#ifdef BT_USE_DOUBLE_PRECISION
#define btGeneric6DofConstraintData2		btGeneric6DofConstraintDoubleData2
#define btGeneric6DofConstraintDataName	"btGeneric6DofConstraintDoubleData2"
#else
#define btGeneric6DofConstraintData2		btGeneric6DofConstraintData
#define btGeneric6DofConstraintDataName	"btGeneric6DofConstraintData"
#endif //BT_USE_DOUBLE_PRECISION

//! Rotation Limit structure for generic joints
class btRotationalLimitMotor
{
public:
    //! limit_parameters
    //!@{
    btScalar m_loLimit;//!< joint limit
    btScalar m_hiLimit;//!< joint limit
    btScalar m_targetVelocity;//!< target motor velocity
    btScalar m_maxMotorForce;//!< max force on motor
    btScalar m_maxLimitForce;//!< max force on limit
    btScalar m_damping;//!< Damping.
    btScalar m_limitSoftness;//! Relaxation factor
    btScalar m_normalCFM;//!< Constraint force mixing factor
    btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
    btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
    btScalar m_bounce;//!< restitution factor
    bool m_enableMotor;

    //!@}

    //! temp_variables
    //!@{
    btScalar m_currentLimitError;//!  How much is violated this limit
    btScalar m_currentPosition;     //!  current value of angle
    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
    btScalar m_accumulatedImpulse;
    //!@}

    btRotationalLimitMotor()
    {
    	m_accumulatedImpulse = 0.f;
        m_targetVelocity = 0;
        m_maxMotorForce = 0.1f;
        m_maxLimitForce = 300.0f;
        m_loLimit = 1.0f;
        m_hiLimit = -1.0f;
		m_normalCFM = 0.f;
		m_stopERP = 0.2f;
		m_stopCFM = 0.f;
        m_bounce = 0.0f;
        m_damping = 1.0f;
        m_limitSoftness = 0.5f;
        m_currentLimit = 0;
        m_currentLimitError = 0;
        m_enableMotor = false;
    }

    btRotationalLimitMotor(const btRotationalLimitMotor & limot)
    {
        m_targetVelocity = limot.m_targetVelocity;
        m_maxMotorForce = limot.m_maxMotorForce;
        m_limitSoftness = limot.m_limitSoftness;
        m_loLimit = limot.m_loLimit;
        m_hiLimit = limot.m_hiLimit;
		m_normalCFM = limot.m_normalCFM;
		m_stopERP = limot.m_stopERP;
		m_stopCFM =	limot.m_stopCFM;
        m_bounce = limot.m_bounce;
        m_currentLimit = limot.m_currentLimit;
        m_currentLimitError = limot.m_currentLimitError;
        m_enableMotor = limot.m_enableMotor;
    }

	//! Is limited
    bool isLimited() const
    {
    	if(m_loLimit > m_hiLimit) return false;
    	return true;
    }

	//! Need apply correction
    bool needApplyTorques() const
    {
    	if(m_currentLimit == 0 && m_enableMotor == false) return false;
    	return true;
    }

	//! calculates  error
	/*!
	calculates m_currentLimit and m_currentLimitError.
	*/
	int testLimitValue(btScalar test_value);

	//! apply the correction impulses for two bodies
    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);

};

class btTranslationalLimitMotor
{
public:
	btVector3 m_lowerLimit;//!< the constraint lower limits
    btVector3 m_upperLimit;//!< the constraint upper limits
    btVector3 m_accumulatedImpulse;
    //! Linear_Limit_parameters
    //!@{
    btScalar	m_limitSoftness;//!< Softness for linear limit
    btScalar	m_damping;//!< Damping for linear limit
    btScalar	m_restitution;//! Bounce parameter for linear limit
	btVector3	m_normalCFM;//!< Constraint force mixing factor
    btVector3	m_stopERP;//!< Error tolerance factor when joint is at limit
	btVector3	m_stopCFM;//!< Constraint force mixing factor when joint is at limit
    //!@}
	bool		m_enableMotor[3];
    btVector3	m_targetVelocity;//!< target motor velocity
    btVector3	m_maxMotorForce;//!< max force on motor
    btVector3	m_currentLimitError;//!  How much is violated this limit
    btVector3	m_currentLinearDiff;//!  Current relative offset of constraint frames
    int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit

    btTranslationalLimitMotor()
    {
    	m_lowerLimit.setValue(0.f,0.f,0.f);
    	m_upperLimit.setValue(0.f,0.f,0.f);
    	m_accumulatedImpulse.setValue(0.f,0.f,0.f);
		m_normalCFM.setValue(0.f, 0.f, 0.f);
		m_stopERP.setValue(0.2f, 0.2f, 0.2f);
		m_stopCFM.setValue(0.f, 0.f, 0.f);

    	m_limitSoftness = 0.7f;
    	m_damping = btScalar(1.0f);
    	m_restitution = btScalar(0.5f);
		for(int i=0; i < 3; i++)
		{
			m_enableMotor[i] = false;
			m_targetVelocity[i] = btScalar(0.f);
			m_maxMotorForce[i] = btScalar(0.f);
		}
    }

    btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
    {
    	m_lowerLimit = other.m_lowerLimit;
    	m_upperLimit = other.m_upperLimit;
    	m_accumulatedImpulse = other.m_accumulatedImpulse;

    	m_limitSoftness = other.m_limitSoftness ;
    	m_damping = other.m_damping;
    	m_restitution = other.m_restitution;
		m_normalCFM = other.m_normalCFM;
		m_stopERP = other.m_stopERP;
		m_stopCFM = other.m_stopCFM;

		for(int i=0; i < 3; i++)
		{
			m_enableMotor[i] = other.m_enableMotor[i];
			m_targetVelocity[i] = other.m_targetVelocity[i];
			m_maxMotorForce[i] = other.m_maxMotorForce[i];
		}
    }

    //! Test limit
	/*!
    - free means upper < lower,
    - locked means upper == lower
    - limited means upper > lower
    - limitIndex: first 3 are linear, next 3 are angular
    */
    inline bool	isLimited(int limitIndex) const
    {
       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
    }
    inline bool needApplyForce(int limitIndex) const
    {
    	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
    	return true;
    }
	int testLimitValue(int limitIndex, btScalar test_value);

    btScalar solveLinearAxis(
    	btScalar timeStep,
        btScalar jacDiagABInv,
        btRigidBody& body1,const btVector3 &pointInA,
        btRigidBody& body2,const btVector3 &pointInB,
        int limit_index,
        const btVector3 & axis_normal_on_a,
		const btVector3 & anchorPos);

};

enum bt6DofFlags
{
	BT_6DOF_FLAGS_CFM_NORM = 1,
	BT_6DOF_FLAGS_CFM_STOP = 2,
	BT_6DOF_FLAGS_ERP_STOP = 4
};
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis

/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/*!
btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
currently this limit supports rotational motors

 For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
At this moment translational motors are not supported. May be in the future.

 For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
This is accessible through btGeneric6DofConstraint.getLimitMotor method,
This brings support for limit parameters and motors.

 Angulars limits have these possible ranges:

	AXIS
	MIN ANGLE
	MAX ANGLE

	X
	-PI
	PI

	Y
	-PI/2
	PI/2

	Z
	-PI
	PI

*/
ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
{
protected:

	//! relative_frames
    //!@{
	btTransform	m_frameInA;//!< the constraint space w.r.t body A
    btTransform	m_frameInB;//!< the constraint space w.r.t body B
    //!@}

    //! Jacobians
    //!@{
    btJacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
    btJacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
    //!@}

	//! Linear_Limit_parameters
    //!@{
    btTranslationalLimitMotor m_linearLimits;
    //!@}

    //! hinge_parameters
    //!@{
    btRotationalLimitMotor m_angularLimits[3];
	//!@}

protected:
    //! temporal variables
    //!@{
    btScalar m_timeStep;
    btTransform m_calculatedTransformA;
    btTransform m_calculatedTransformB;
    btVector3 m_calculatedAxisAngleDiff;
    btVector3 m_calculatedAxis[3];
    btVector3 m_calculatedLinearDiff;
	btScalar	m_factA;
	btScalar	m_factB;
	bool		m_hasStaticBody;

	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes

    bool	m_useLinearReferenceFrameA;
	bool	m_useOffsetForConstraintFrame;

	int		m_flags;

    //!@}

    btGeneric6DofConstraint&	operator=(btGeneric6DofConstraint&	other)
    {
        btAssert(0);
        (void) other;
        return *this;
    }

	int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);

	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);

    void buildLinearJacobian(
        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
        const btVector3 & pivotAInW,const btVector3 & pivotBInW);

    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);

	// tests linear limits
	void calculateLinearInfo();

	//! calcs the euler angles between the two bodies.
    void calculateAngleInfo();

public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	///for backwards compatibility during the transition to 'getInfo/getInfo2'
	bool		m_useSolveConstraintObsolete;

    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);

	//! Calcs global transform of the offsets
	/*!
	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
	\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
	*/
    void calculateTransforms(const btTransform& transA,const btTransform& transB);

	void calculateTransforms();

	//! Gets the global transform of the offset for body A
    /*!
    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
    */
    const btTransform & getCalculatedTransformA() const
    {
    	return m_calculatedTransformA;
    }

    //! Gets the global transform of the offset for body B
    /*!
    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
    */
    const btTransform & getCalculatedTransformB() const
    {
    	return m_calculatedTransformB;
    }

    const btTransform & getFrameOffsetA() const
    {
    	return m_frameInA;
    }

    const btTransform & getFrameOffsetB() const
    {
    	return m_frameInB;
    }

    btTransform & getFrameOffsetA()
    {
    	return m_frameInA;
    }

    btTransform & getFrameOffsetB()
    {
    	return m_frameInB;
    }

	//! performs Jacobian calculation, and also calculates angle differences and axis
    virtual void	buildJacobian();

	virtual void getInfo1 (btConstraintInfo1* info);

	void getInfo1NonVirtual (btConstraintInfo1* info);

	virtual void getInfo2 (btConstraintInfo2* info);

	void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);

    void	updateRHS(btScalar	timeStep);

	//! Get the rotation axis in global coordinates
	/*!
	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
	*/
    btVector3 getAxis(int axis_index) const;

    //! Get the relative Euler angle
    /*!
	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
	*/
    btScalar getAngle(int axis_index) const;

	//! Get the relative position of the constraint pivot
    /*!
	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
	*/
	btScalar getRelativePivotPosition(int axis_index) const;

	void setFrames(const btTransform & frameA, const btTransform & frameB);

	//! Test angular limit.
	/*!
	Calculates angular correction and returns true if limit needs to be corrected.
	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
	*/
    bool testAngularLimitMotor(int axis_index);

    void	setLinearLowerLimit(const btVector3& linearLower)
    {
    	m_linearLimits.m_lowerLimit = linearLower;
    }

	void	getLinearLowerLimit(btVector3& linearLower) const
	{
		linearLower = m_linearLimits.m_lowerLimit;
	}

	void	setLinearUpperLimit(const btVector3& linearUpper)
	{
		m_linearLimits.m_upperLimit = linearUpper;
	}

	void	getLinearUpperLimit(btVector3& linearUpper) const
	{
		linearUpper = m_linearLimits.m_upperLimit;
	}

    void	setAngularLowerLimit(const btVector3& angularLower)
    {
		for(int i = 0; i < 3; i++)
			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
    }

	void	getAngularLowerLimit(btVector3& angularLower) const
	{
		for(int i = 0; i < 3; i++)
			angularLower[i] = m_angularLimits[i].m_loLimit;
	}

    void	setAngularUpperLimit(const btVector3& angularUpper)
    {
		for(int i = 0; i < 3; i++)
			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
    }

	void	getAngularUpperLimit(btVector3& angularUpper) const
	{
		for(int i = 0; i < 3; i++)
			angularUpper[i] = m_angularLimits[i].m_hiLimit;
	}

	//! Retrieves the angular limit informacion
    btRotationalLimitMotor * getRotationalLimitMotor(int index)
    {
    	return &m_angularLimits[index];
    }

    //! Retrieves the  limit informacion
    btTranslationalLimitMotor * getTranslationalLimitMotor()
    {
    	return &m_linearLimits;
    }

    //first 3 are linear, next 3 are angular
    void setLimit(int axis, btScalar lo, btScalar hi)
    {
    	if(axis<3)
    	{
    		m_linearLimits.m_lowerLimit[axis] = lo;
    		m_linearLimits.m_upperLimit[axis] = hi;
    	}
    	else
    	{
			lo = btNormalizeAngle(lo);
			hi = btNormalizeAngle(hi);
    		m_angularLimits[axis-3].m_loLimit = lo;
    		m_angularLimits[axis-3].m_hiLimit = hi;
    	}
    }

	//! Test limit
	/*!
    - free means upper < lower,
    - locked means upper == lower
    - limited means upper > lower
    - limitIndex: first 3 are linear, next 3 are angular
    */
    bool	isLimited(int limitIndex) const
    {
    	if(limitIndex<3)
    	{
			return m_linearLimits.isLimited(limitIndex);

    	}
        return m_angularLimits[limitIndex-3].isLimited();
    }

	virtual void calcAnchorPos(void); // overridable

	int get_limit_motor_info2(	btRotationalLimitMotor * limot,
								const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);

	// access for UseFrameOffset
	bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }

	bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
	void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }

	///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.
	virtual	void setParam(int num, btScalar value, int axis = -1);
	///return the local value of parameter
	virtual	btScalar getParam(int num, int axis = -1) const;

	void setAxis( const btVector3& axis1, const btVector3& axis2);

    	virtual	int getFlags() const
    	{
        	return m_flags;
	}

	virtual	int	calculateSerializeBufferSize() const;

	///fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;

};

struct btGeneric6DofConstraintData
{
	btTypedConstraintData	m_typeConstraintData;
	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
	btTransformFloatData m_rbBFrame;

	btVector3FloatData	m_linearUpperLimit;
	btVector3FloatData	m_linearLowerLimit;

	btVector3FloatData	m_angularUpperLimit;
	btVector3FloatData	m_angularLowerLimit;

	int	m_useLinearReferenceFrameA;
	int m_useOffsetForConstraintFrame;
};

struct btGeneric6DofConstraintDoubleData2
{
	btTypedConstraintDoubleData	m_typeConstraintData;
	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
	btTransformDoubleData m_rbBFrame;

	btVector3DoubleData	m_linearUpperLimit;
	btVector3DoubleData	m_linearLowerLimit;

	btVector3DoubleData	m_angularUpperLimit;
	btVector3DoubleData	m_angularLowerLimit;

	int	m_useLinearReferenceFrameA;
	int m_useOffsetForConstraintFrame;
};

SIMD_FORCE_INLINE	int	btGeneric6DofConstraint::calculateSerializeBufferSize() const
{
	return sizeof(btGeneric6DofConstraintData2);
}

	///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE	const char*	btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
{

	btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);

	m_frameInA.serialize(dof->m_rbAFrame);
	m_frameInB.serialize(dof->m_rbBFrame);

	int i;
	for (i=0;i<3;i++)
	{
		dof->m_angularLowerLimit.m_floats[i] =  m_angularLimits[i].m_loLimit;
		dof->m_angularUpperLimit.m_floats[i] =  m_angularLimits[i].m_hiLimit;
		dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
		dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
	}

	dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
	dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;

	return btGeneric6DofConstraintDataName;
}

#endif //BT_GENERIC_6DOF_CONSTRAINT_H