Basic support for COLLADA physics constraints (each DOF can be completely locked or free, no limits yet)

BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp

@@ -32,6 +32,10 @@ Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, con
 , m_frameInA(frameInA)
 , m_frameInB(frameInB)
 {
+	//free means upper < lower, 
+	//locked means upper == lower
+	//limited means upper > lower
+	//so start all locked
 	for (int i=0; i<6;++i)
 	{
 		m_lowerLimit[i] = 0.0f;
@@ -58,11 +62,13 @@ void Generic6DofConstraint::BuildJacobian()
 	int i;
 	//linear part
 	for (i=0;i<3;i++)
+	{
+		if (isLimited(i))
 		{
 			normal[i] = 1;
 			
 			// Create linear atom
-		new (&m_jac[i]) JacobianEntry(
+			new (&m_jacLinear[i]) JacobianEntry(
 				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
@@ -81,9 +87,12 @@ void Generic6DofConstraint::BuildJacobian()
 
 			normal[i] = 0;
 		}
+	}
 
 	// angular part
 	for (i=0;i<3;i++)
+	{
+		if (isLimited(i+3))
 		{
 			SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
 			SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
@@ -104,6 +113,7 @@ void Generic6DofConstraint::BuildJacobian()
 			m_rbA.applyTorqueImpulse( impulse_vector);
 			m_rbB.applyTorqueImpulse(-impulse_vector);
 		}
+	}
 }
 
 void	Generic6DofConstraint::SolveConstraint(SimdScalar	timeStep)
@@ -122,16 +132,18 @@ void	Generic6DofConstraint::SolveConstraint(SimdScalar	timeStep)
 
 	// linear
 	for (i=0;i<3;i++)
+	{		
+		if (isLimited(i))
 		{
 			SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
 			SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
 		
 
 			normal[i] = 1;
-		SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
+			SimdScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
 
 			//velocity error (first order error)
-		SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
+			SimdScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
 																	m_rbB.getLinearVelocity(),angvelB);
 		
 			//positional error (zeroth order error)
@@ -146,9 +158,12 @@ void	Generic6DofConstraint::SolveConstraint(SimdScalar	timeStep)
 			
 			normal[i] = 0;
 		}
+	}
 
 	// angular
 	for (i=0;i<3;i++)
+	{
+		if (isLimited(i+3))
 		{
 			SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
 			SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
@@ -176,6 +191,7 @@ void	Generic6DofConstraint::SolveConstraint(SimdScalar	timeStep)
 			m_rbA.applyTorqueImpulse( impulse_vector);
 			m_rbB.applyTorqueImpulse(-impulse_vector);
 		}
+	}
 }
 
 void	Generic6DofConstraint::UpdateRHS(SimdScalar	timeStep)

BulletDynamics/ConstraintSolver/Generic6DofConstraint.h

@@ -30,7 +30,7 @@ class RigidBody;
 /// Work in progress (is still a Hinge actually)
 class Generic6DofConstraint : public TypedConstraint
 {
-	JacobianEntry	m_jac[3];			// 3 orthogonal linear constraints
+	JacobianEntry	m_jacLinear[3];			// 3 orthogonal linear constraints
 	JacobianEntry	m_jacAng[3];		// 3 orthogonal angular constraints
 
 	SimdTransform	m_frameInA;			// the constraint space w.r.t body A
@@ -55,10 +55,48 @@ public:
 
 	SimdScalar ComputeAngle(int axis) const;
 
-	void SetAngularLimit(int axis, SimdScalar lo, SimdScalar hi)
+	void	setLinearLowerLimit(const SimdVector3& linearLower)
 	{
-		m_lowerLimit[axis + 3] = lo; 
+		m_lowerLimit[0] = linearLower.getX();
-		m_upperLimit[axis + 3] = hi; 
+		m_lowerLimit[1] = linearLower.getY();
+		m_lowerLimit[2] = linearLower.getZ();
+	}
+
+	void	setLinearUpperLimit(const SimdVector3& linearUpper)
+	{
+		m_upperLimit[0] = linearUpper.getX();
+		m_upperLimit[1] = linearUpper.getY();
+		m_upperLimit[2] = linearUpper.getZ();
+	}
+
+	void	setAngularLowerLimit(const SimdVector3& angularLower)
+	{
+		m_lowerLimit[3] = angularLower.getX();
+		m_lowerLimit[4] = angularLower.getY();
+		m_lowerLimit[5] = angularLower.getZ();
+	}
+
+	void	setAngularUpperLimit(const SimdVector3& angularUpper)
+	{
+		m_upperLimit[3] = angularUpper.getX();
+		m_upperLimit[4] = angularUpper.getY();
+		m_upperLimit[5] = angularUpper.getZ();
+	}
+
+	//first 3 are linear, next 3 are angular
+	void SetLimit(int axis, SimdScalar lo, SimdScalar hi)
+	{
+		m_lowerLimit[axis] = lo; 
+		m_upperLimit[axis] = hi; 
+	}
+
+	//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)
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
 	}
 
 	const RigidBody& GetRigidBodyA() const

Demos/ColladaDemo/ColladaDemo.cpp

@@ -87,9 +87,57 @@ extern int	gForwardAxis;
 #include "dae.h"
 #include "dom/domCOLLADA.h"
 
+
+
+
 DAE* collada = 0;
 domCOLLADA* dom = 0;
 
+
+domMatrix_Array emptyMatrixArray;
+
+SimdTransform	GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
+														domRotate_Array& rotateArray,
+														domTranslate_Array& translateArray
+														)
+
+{
+	SimdTransform	startTransform;
+	startTransform.setIdentity();
+	
+	int i;
+	//either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate'
+	for (i=0;i<matrixArray.getCount();i++)
+	{
+		domMatrixRef matrixRef = matrixArray[i];
+		domFloat4x4 fl16 = matrixRef->getValue();
+		SimdVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
+		startTransform.setOrigin(origin);
+		SimdMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
+							fl16.get(4),fl16.get(5),fl16.get(6),
+							fl16.get(8),fl16.get(9),fl16.get(10));
+		startTransform.setBasis(basis);
+	}
+
+	for (i=0;i<rotateArray.getCount();i++)
+	{
+		domRotateRef rotateRef = rotateArray[i];
+		domFloat4 fl4 = rotateRef->getValue();
+		float angleRad = SIMD_RADS_PER_DEG*fl4.get(3);
+		SimdQuaternion rotQuat(SimdVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
+		startTransform.getBasis() = startTransform.getBasis() * SimdMatrix3x3(rotQuat);
+	}
+
+	for (i=0;i<translateArray.getCount();i++)
+	{
+		domTranslateRef translateRef = translateArray[i];
+		domFloat3 fl3 = translateRef->getValue();
+		startTransform.getOrigin() += SimdVector3(fl3.get(0),fl3.get(1),fl3.get(2));
+	}
+	return startTransform;
+}
+
+
 #endif
 
 
@@ -160,7 +208,7 @@ CollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use of
 ////////////////////////////////////
 
 ///Very basic import
-void	CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startTransform,CollisionShape* shape)
+CcdPhysicsController*  CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startTransform,CollisionShape* shape)
 {
 
 	startTransforms[numObjects] = startTransform;
@@ -245,8 +293,8 @@ void	CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startT
 
 	}
 
-	numObjects++;
+	//return newly created PhysicsController
-
+	return physObjects[numObjects++];
 }
 
 
@@ -1449,38 +1497,12 @@ int main(int argc,char** argv)
 
 									//find transform of the node that this rigidbody maps to
 
-									int i;
-									//either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate'
-									for (i=0;i<node->getMatrix_array().getCount();i++)
-									{
-										domMatrixRef matrixRef = node->getMatrix_array()[i];
-										domFloat4x4 fl16 = matrixRef->getValue();
-										SimdVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
-										startTransform.setOrigin(origin);
-										SimdMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
-															fl16.get(4),fl16.get(5),fl16.get(6),
-															fl16.get(8),fl16.get(9),fl16.get(10));
-										startTransform.setBasis(basis);
-									}
 							
-
+									startTransform = GetSimdTransformFromCOLLADA_DOM(
-									
+														node->getMatrix_array(),
-
+														node->getRotate_array(),
-									for (i=0;i<node->getRotate_array().getCount();i++)
+														node->getTranslate_array()
-									{
+														);
-										domRotateRef rotateRef = node->getRotate_array()[i];
-										domFloat4 fl4 = rotateRef->getValue();
-										float angleRad = SIMD_RADS_PER_DEG*fl4.get(3);
-										SimdQuaternion rotQuat(SimdVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
-										startTransform.getBasis() = startTransform.getBasis() * SimdMatrix3x3(rotQuat);
-									}
-
-									for (i=0;i<node->getTranslate_array().getCount();i++)
-									{
-										domTranslateRef translateRef = node->getTranslate_array()[i];
-										domFloat3 fl3 = translateRef->getValue();
-										startTransform.getOrigin() += SimdVector3(fl3.get(0),fl3.get(1),fl3.get(2));
-									}
 
 									for (i=0;i<node->getScale_array().getCount();i++)
 									{
@@ -1493,7 +1515,9 @@ int main(int argc,char** argv)
 
 					
 							
-								CreatePhysicsObject(isDynamics,mass,startTransform,colShape);
+								CcdPhysicsController* ctrl = CreatePhysicsObject(isDynamics,mass,startTransform,colShape);
+								//for bodyName lookup in constraints
+								ctrl->setNewClientInfo((void*)bodyName);
 
 							}
 
@@ -1527,19 +1551,128 @@ int main(int argc,char** argv)
 									
 									if (rigidConstraintRef->getSid() && !strcmp(rigidConstraintRef->getSid(),constraintName))
 									{
-										/*
+										
 										//two bodies
-										rigidConstraintRef->getRef_attachment();
+										const domRigid_constraint::domRef_attachmentRef attachRefBody = rigidConstraintRef->getRef_attachment();
-										rigidConstraintRef->getAttachment();
+										const domRigid_constraint::domAttachmentRef attachBody1 = rigidConstraintRef->getAttachment();
+
+										daeString uri = attachRefBody->getRigid_body().getURI();
+										daeString orgUri0 = attachRefBody->getRigid_body().getOriginalURI();
+										daeString orgUri1 = attachBody1->getRigid_body().getOriginalURI();
+										CcdPhysicsController* ctrl0=0,*ctrl1=0;
+										
+										for (int i=0;i<numObjects;i++)
+										{
+											char* bodyName = (char*)physObjects[i]->getNewClientInfo();
+											if (!strcmp(bodyName,orgUri0))
+											{
+												printf("found\n");
+												ctrl0=physObjects[i];
+											}
+											if (!strcmp(bodyName,orgUri1))
+											{
+												ctrl1=physObjects[i];
+											}
+										}
+
+
+
+										const domRigid_constraint::domAttachmentRef attachOtherBody = rigidConstraintRef->getAttachment();
 
 										
 										const domRigid_constraint::domTechnique_commonRef commonRef = rigidConstraintRef->getTechnique_common();
-										domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue();
+										
-										SimdVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2));
 										domFloat3 flMin = commonRef->getLimits()->getLinear()->getMin()->getValue();
 										SimdVector3 minLinearLimit(flMin.get(0),flMin.get(1),flMin.get(2));
-										commonRef->getLimits()->getSwing_cone_and_twist();
+										
-*/
+										domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue();
+										SimdVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2));
+																			
+										domFloat3 coneMinLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMin()->getValue();
+										SimdVector3 angularMin(coneMinLimit.get(0),coneMinLimit.get(1),coneMinLimit.get(2));
+
+										domFloat3 coneMaxLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMax()->getValue();
+										SimdVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2));
+
+										{
+											int constraintId;
+
+											SimdTransform attachFrameRef0;
+											attachFrameRef0 = 
+												GetSimdTransformFromCOLLADA_DOM
+												(
+												emptyMatrixArray,
+												attachRefBody->getRotate_array(),
+												attachRefBody->getTranslate_array());
+
+											SimdTransform attachFrameOther;
+											attachFrameOther =
+												GetSimdTransformFromCOLLADA_DOM
+												(
+												emptyMatrixArray,
+												attachBody1->getRotate_array(),
+												attachBody1->getTranslate_array()
+												);
+
+
+											//convert INF / -INF into lower > upper
+
+											//currently there is a hack in the DOM to detect INF / -INF
+											//see daeMetaAttribute.cpp
+											//INF -> 999999.9
+											//-INF -> -999999.9
+											float linearCheckTreshold = 999999.0;
+											float angularCheckTreshold = 180.0;//check this
+
+
+
+											
+											//free means upper < lower, 
+											//locked means upper == lower
+											//limited means upper > lower
+											//limitIndex: first 3 are linear, next 3 are angular
+
+											SimdVector3 linearLowerLimits = minLinearLimit;
+											SimdVector3 linearUpperLimits = maxLinearLimit;
+											SimdVector3 angularLowerLimits = angularMin;
+											SimdVector3 angularUpperLimits = angularMax;
+											{
+												for (int i=0;i<3;i++)
+												{
+													if  ((linearLowerLimits[i] < -linearCheckTreshold) ||
+														(linearUpperLimits[i] > linearCheckTreshold))
+													{
+														//disable limits
+														linearLowerLimits[i] = 1;
+														linearUpperLimits[i] = 0;
+													}
+
+													if  ((angularLowerLimits[i] < -angularCheckTreshold) ||
+														(angularUpperLimits[i] > angularCheckTreshold))
+													{
+														//disable limits
+														angularLowerLimits[i] = 1;
+														angularUpperLimits[i] = 0;
+													}
+												}
+											}
+
+
+											constraintId =physicsEnvironmentPtr->createUniversalD6Constraint(
+											ctrl0,
+											ctrl1,
+											attachFrameRef0,
+											attachFrameOther,
+											linearLowerLimits,
+											linearUpperLimits,
+											angularLowerLimits,
+											angularUpperLimits
+												);
+
+
+										}
+
+
 
 									}
 								}

Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp

@@ -1327,6 +1327,62 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 
 }
 
+
+
+
+//Following the COLLADA physics specification for constraints
+int			CcdPhysicsEnvironment::createUniversalD6Constraint(
+						class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
+						SimdTransform& frameInA,
+						SimdTransform& frameInB,
+						const SimdVector3& linearMinLimits,
+						const SimdVector3& linearMaxLimits,
+						const SimdVector3& angularMinLimits,
+						const SimdVector3& angularMaxLimits
+)
+{
+
+	//we could either add some logic to recognize ball-socket and hinge, or let that up to the user
+	//perhaps some warning or hint that hinge/ball-socket is more efficient?
+	
+	Generic6DofConstraint* genericConstraint = 0;
+	CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
+	CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
+	
+	RigidBody* rb0 = ctrl0->GetRigidBody();
+	RigidBody* rb1 = ctrl1->GetRigidBody();
+
+	if (rb1)
+	{
+		
+
+		genericConstraint = new Generic6DofConstraint(
+			*rb0,*rb1,
+			frameInA,frameInB);
+		genericConstraint->setLinearLowerLimit(linearMinLimits);
+		genericConstraint->setLinearUpperLimit(linearMaxLimits);
+		genericConstraint->setAngularLowerLimit(angularMinLimits);
+		genericConstraint->setAngularUpperLimit(angularMaxLimits);
+	} else
+	{
+		// TODO: Implement single body case...
+		//No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
+
+	}
+	
+	if (genericConstraint)
+	{
+		m_constraints.push_back(genericConstraint);
+		genericConstraint->SetUserConstraintId(gConstraintUid++);
+		genericConstraint->SetUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
+		//64 bit systems can't cast pointer to int. could use size_t instead.
+		return genericConstraint->GetUserConstraintId();
+	}
+	return 0;
+}
+
+
+
 void		CcdPhysicsEnvironment::removeConstraint(int	constraintId)
 {
 	std::vector<TypedConstraint*>::iterator i;

Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h

@@ -20,6 +20,8 @@ subject to the following restrictions:
 #include <vector>
 class CcdPhysicsController;
 #include "SimdVector3.h"
+#include "SimdTransform.h"
+
 
 
 
@@ -119,6 +121,20 @@ protected:
 		virtual int			createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
 			float pivotX,float pivotY,float pivotZ,
 			float axisX,float axisY,float axisZ);
+
+
+		//Following the COLLADA physics specification for constraints
+		virtual int			createUniversalD6Constraint(
+		class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
+			SimdTransform& localAttachmentFrameRef,
+			SimdTransform& localAttachmentOther,
+			const SimdVector3& linearMinLimits,
+			const SimdVector3& linearMaxLimits,
+			const SimdVector3& angularMinLimits,
+			const SimdVector3& angularMaxLimits
+			);
+
+
 	    virtual void		removeConstraint(int	constraintid);