added Bullet rigidbodies that behave like Sumo 'dynamic', without rotations. Done using a special hinge constraint (no translational degrees of freedom removed)

3 files changed, 56 insertions, 39 deletions

diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp
index b24ec54b75d..12e02f2837a 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp
@@ -28,7 +28,8 @@ HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector
 								 SimdVector3& axisInA,SimdVector3& axisInB)
 :TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
 m_axisInA(axisInA),
-m_axisInB(axisInB)
+m_axisInB(axisInB),
+m_angularOnly(false)
 {
 
 }
@@ -38,7 +39,8 @@ HingeConstraint::HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,Simd
 :TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
 m_axisInA(axisInA),
 //fixed axis in worldspace
-m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA)
+m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
+m_angularOnly(false)
 {
 	
 }
@@ -47,20 +49,23 @@ void	HingeConstraint::BuildJacobian()
 {
 	SimdVector3	normal(0,0,0);
 
-	for (int i=0;i<3;i++)
+	if (!m_angularOnly)
 	{
-		normal[i] = 1;
-		new (&m_jac[i]) JacobianEntry(
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
-			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
-			normal,
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbA.getInvMass(),
-			m_rbB.getInvInertiaDiagLocal(),
-			m_rbB.getInvMass());
-		normal[i] = 0;
+		for (int i=0;i<3;i++)
+		{
+			normal[i] = 1;
+			new (&m_jac[i]) JacobianEntry(
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+				normal,
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+			normal[i] = 0;
+		}
 	}
 
 	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
@@ -97,28 +102,31 @@ void	HingeConstraint::SolveConstraint(SimdScalar	timeStep)
 	SimdScalar tau = 0.3f;
 	SimdScalar damping = 1.f;
 
-	for (int i=0;i<3;i++)
-	{		
-		normal[i] = 1;
-		SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
-
-		SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-		SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-		
-		SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		SimdVector3 vel = vel1 - vel2;
-		SimdScalar rel_vel;
-		rel_vel = normal.dot(vel);
-		//positional error (zeroth order error)
-		SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-		SimdScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
-
-		SimdVector3 impulse_vector = normal * impulse;
-		m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-		m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-		
-		normal[i] = 0;
+	if (!m_angularOnly)
+	{
+		for (int i=0;i<3;i++)
+		{		
+			normal[i] = 1;
+			SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
+
+			SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+			
+			SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+			SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+			SimdVector3 vel = vel1 - vel2;
+			SimdScalar rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			SimdScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
+
+			SimdVector3 impulse_vector = normal * impulse;
+			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+			
+			normal[i] = 0;
+		}
 	}
 
 	///solve angular part
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.h
index c9d2b5adf6a..a43463511b1 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.h
@@ -36,6 +36,7 @@ class HingeConstraint : public TypedConstraint
 	SimdVector3	m_axisInA;
 	SimdVector3	m_axisInB;
 
+	bool	m_angularOnly;
 	
 public:
 
@@ -60,6 +61,11 @@ public:
 		return m_rbB;
 	}
 
+	void	setAngularOnly(bool angularOnly)
+	{
+		m_angularOnly = angularOnly;
+	}
+
 
 
 };
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
index fd4bca40e7f..5cb2859046c 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
@@ -508,7 +508,6 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 
 
 
-
 	if (!SimdFuzzyZero(timeStep))
 	{
 
@@ -1153,6 +1152,8 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 		(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : 
 	rb0->getCenterOfMassTransform().getBasis() * -axisInA;
 
+	bool hingeAngularOnly = false;
+
 	switch (type)
 	{
 	case PHY_POINT2POINT_CONSTRAINT:
@@ -1179,6 +1180,8 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 			break;
 		}
 
+	case PHY_ANGULAR_CONSTRAINT:
+			hingeAngularOnly = true;
 	case PHY_LINEHINGE_CONSTRAINT:
 		{
 			HingeConstraint* hinge = 0;
@@ -1196,7 +1199,7 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 					pivotInA,axisInA);
 
 			}
-			
+			hinge->setAngularOnly( hingeAngularOnly );
 			m_constraints.push_back(hinge);
 			hinge->SetUserConstraintId(gConstraintUid++);
 			hinge->SetUserConstraintType(type);