Index: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
===================================================================
--- code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 5781)
+++ code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 8351)
@@ -13,4 +13,8 @@
 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
@@ -46,5 +50,7 @@
     btScalar m_damping;//!< Damping.
     btScalar m_limitSoftness;//! Relaxation factor
-    btScalar m_ERP;//!< Error tolerance factor when joint is at limit
+    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;
@@ -55,4 +61,5 @@
     //!@{
     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;
@@ -65,7 +72,9 @@
         m_maxMotorForce = 0.1f;
         m_maxLimitForce = 300.0f;
-        m_loLimit = -SIMD_INFINITY;
-        m_hiLimit = SIMD_INFINITY;
-        m_ERP = 0.5f;
+        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;
@@ -83,5 +92,7 @@
         m_loLimit = limot.m_loLimit;
         m_hiLimit = limot.m_hiLimit;
-        m_ERP = limot.m_ERP;
+		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;
@@ -113,5 +124,5 @@
 
 	//! apply the correction impulses for two bodies
-    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
+    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
 
 };
@@ -130,4 +141,7 @@
     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];
@@ -135,4 +149,5 @@
     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
 
@@ -142,4 +157,7 @@
     	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;
@@ -163,4 +181,8 @@
     	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++) 
 		{
@@ -193,6 +215,6 @@
     	btScalar timeStep,
         btScalar jacDiagABInv,
-        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
-        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
         int limit_index,
         const btVector3 & axis_normal_on_a,
@@ -201,4 +223,13 @@
 
 };
+
+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
@@ -216,18 +247,20 @@
 <li> Angulars limits have these possible ranges:
 <table border=1 >
-<tr
-
+<tr>
 	<td><b>AXIS</b></td>
 	<td><b>MIN ANGLE</b></td>
 	<td><b>MAX ANGLE</b></td>
+</tr><tr>
 	<td>X</td>
-		<td>-PI</td>
-		<td>PI</td>
+	<td>-PI</td>
+	<td>PI</td>
+</tr><tr>
 	<td>Y</td>
-		<td>-PI/2</td>
-		<td>PI/2</td>
+	<td>-PI/2</td>
+	<td>PI/2</td>
+</tr><tr>
 	<td>Z</td>
-		<td>-PI/2</td>
-		<td>PI/2</td>
+	<td>-PI</td>
+	<td>PI</td>
 </tr>
 </table>
@@ -273,9 +306,15 @@
     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;
+
     //!@}
 
@@ -288,7 +327,7 @@
 
 
-	int setAngularLimits(btConstraintInfo2 *info, int row_offset);
-
-	int setLinearLimits(btConstraintInfo2 *info);
+	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(
@@ -312,7 +351,6 @@
 
     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-
-    btGeneric6DofConstraint();
-
+    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
+    
 	//! Calcs global transform of the offsets
 	/*!
@@ -320,5 +358,7 @@
 	\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
 	*/
-    void calculateTransforms();
+    void calculateTransforms(const btTransform& transA,const btTransform& transB);
+
+	void calculateTransforms();
 
 	//! Gets the global transform of the offset for body A
@@ -367,7 +407,10 @@
 	virtual void getInfo1 (btConstraintInfo1* info);
 
+	void getInfo1NonVirtual (btConstraintInfo1* info);
+
 	virtual void getInfo2 (btConstraintInfo2* info);
 
-    virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+	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);
@@ -381,12 +424,19 @@
     //! Get the relative Euler angle
     /*!
-	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	\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;
+
 
 	//! Test angular limit.
 	/*!
 	Calculates angular correction and returns true if limit needs to be corrected.
-	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
     bool testAngularLimitMotor(int axis_index);
@@ -404,14 +454,12 @@
     void	setAngularLowerLimit(const btVector3& angularLower)
     {
-        m_angularLimits[0].m_loLimit = angularLower.getX();
-        m_angularLimits[1].m_loLimit = angularLower.getY();
-        m_angularLimits[2].m_loLimit = angularLower.getZ();
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
     }
 
     void	setAngularUpperLimit(const btVector3& angularUpper)
     {
-        m_angularLimits[0].m_hiLimit = angularUpper.getX();
-        m_angularLimits[1].m_hiLimit = angularUpper.getY();
-        m_angularLimits[2].m_hiLimit = angularUpper.getZ();
+		for(int i = 0; i < 3; i++)
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
     }
 
@@ -438,4 +486,6 @@
     	else
     	{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
     		m_angularLimits[axis-3].m_loLimit = lo;
     		m_angularLimits[axis-3].m_hiLimit = hi;
@@ -460,21 +510,79 @@
     }
 
-    const btRigidBody& getRigidBodyA() const
-    {
-        return m_rbA;
-    }
-    const btRigidBody& getRigidBodyB() const
-    {
-        return m_rbB;
-    }
-
 	virtual void calcAnchorPos(void); // overridable
 
 	int get_limit_motor_info2(	btRotationalLimitMotor * limot,
-								btRigidBody * body0, btRigidBody * body1,
-								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
-
-
+								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() { return m_useOffsetForConstraintFrame; }
+	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+
+	///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	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 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;
+};
+
+SIMD_FORCE_INLINE	int	btGeneric6DofConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btGeneric6DofConstraintData);
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+
+	btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
+	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+
+	m_frameInA.serializeFloat(dof->m_rbAFrame);
+	m_frameInB.serializeFloat(dof->m_rbBFrame);
+
+		
+	int i;
+	for (i=0;i<3;i++)
+	{
+		dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
+		dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
+		dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
+		dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
+	}
+	
+	dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
+	dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
+
+	return "btGeneric6DofConstraintData";
+}
+
+
+
+
+
 #endif //GENERIC_6DOF_CONSTRAINT_H
