add b3FixedConstraint, with less memory footprint than b3Generic6DofConstraint, and easier to port to OpenCL.

remove the 'obsolete' part of b3Generic6DofConstraint and b3Point2PointConstraint

Demos3/GpuDemos/constraints/ConstraintsDemo.cpp

@@ -16,6 +16,9 @@
 #include "../rigidbody/GpuRigidBodyDemoInternalData.h"
 #include "../gwenUserInterface.h"
 #include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h"
+#include "Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h"
+#include "Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h"
+
 #include "OpenGLWindow/GLPrimitiveRenderer.h"
 #include "Bullet3OpenCL/Raycast/b3GpuRaycast.h"
 #include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h"
@@ -34,7 +37,7 @@ void GpuConstraintsDemo::setupScene(const ConstructionInfo& ci)
 	index+=createDynamicsObjects(ci);
 
 	m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
-
+	m_data->m_rigidBodyPipeline->setGravity(b3Vector3(4,-10,0));
 	float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0};
 	//float camPos[4]={1,12.5,1.5,0};
 	
@@ -123,9 +126,10 @@ int	GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
 		if (ci.m_useInstancedCollisionShapes)
 			colIndex = m_data->m_np->registerConvexHullShape(utilPtr);
 
+		int constraintType=0;
 		for (int i=0;i<ci.arraySizeZ;i++)
 		{
-
+			constraintType=(constraintType+1)&0x11;
 
 			for (int k=0;k<ci.arraySizeX;k++)
 			{
@@ -159,12 +163,57 @@ int	GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
 					int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false);
 
 
+					b3TypedConstraint* c = 0;
+
 					if (prevBody>=0)
 					{
-						b3Point2PointConstraint* p2p = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0));
+						switch (constraintType)
-						m_data->m_rigidBodyPipeline->addConstraint(p2p);//,false);
+						{
+						case 0:
+								c = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0));
+								break;
+						case 1:
+							{
+							b3Transform frameInA,frameInB;
+							frameInA.setIdentity();
+							frameInB.setIdentity();
+							frameInA.setOrigin(b3Vector3(0,-1.1,0));
+							frameInB.setOrigin(b3Vector3(0,1.1,0));
+
+							c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB);
+							//c->setBreakingImpulseThreshold(37.1);
+
+							break;
+							}
+						case 2:
+							{
+							b3Transform frameInA,frameInB;
+							frameInA.setIdentity();
+							frameInB.setIdentity();
+							frameInA.setOrigin(b3Vector3(0,-1.1,0));
+							frameInB.setOrigin(b3Vector3(0,1.1,0));
+
+							b3Generic6DofConstraint* dof6 = new b3Generic6DofConstraint(pid,prevBody,frameInA,frameInB,false,m_data->m_np->getBodiesCpu());
+							for (int i=0;i<6;i++)
+								dof6->setLimit(i,0,0);
+							c=dof6;
+							break;
+							}
+						default:
+							{
+
+								b3Assert(0);
+							}
+						};
+						if (c)
+						{
+							m_data->m_rigidBodyPipeline->addConstraint(c);//,false);
+						}
+
 
 					}
+
+
 					prevBody = pid;
 
 					index++;
@@ -192,7 +241,7 @@ void GpuConstraintsDemo::createStaticEnvironment(const ConstructionInfo& ci)
 	{
 		b3Vector4 scaling(400,400,400,1);
 		int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
-		b3Vector3 position(0,-400,0);
+		b3Vector3 position(0,-405,0);
 		b3Quaternion orn(0,0,0,1);
 
 		b3Vector4 color(0,0,1,1);

src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp

@@ -0,0 +1,108 @@
+
+#include "b3FixedConstraint.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
+#include "Bullet3Common/b3TransformUtil.h"
+#include <new>
+
+
+b3FixedConstraint::b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB)
+:b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE,rbA,rbB)
+{
+	m_pivotInA = frameInA.getOrigin();
+	m_pivotInB = frameInB.getOrigin();
+	m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
+
+}
+
+b3FixedConstraint::~b3FixedConstraint ()
+{
+}
+
+	
+void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
+{
+	info->m_numConstraintRows = 6;
+	info->nub = 6;
+}
+
+void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies)
+{
+	//fix the 3 linear degrees of freedom
+
+	const b3Vector3& worldPosA = bodies[m_rbA].m_pos;
+	const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat;
+	const b3Vector3& worldPosB= bodies[m_rbB].m_pos;
+	const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat;
+
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip+1] = 1;
+	info->m_J1linearAxis[2*info->rowskip+2] = 1;
+
+	b3Vector3 a1 = b3QuatRotate(worldOrnA,m_pivotInA);
+	{
+		b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
+		b3Vector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+    
+	if (info->m_J2linearAxis)
+	{
+		info->m_J2linearAxis[0] = -1;
+		info->m_J2linearAxis[info->rowskip+1] = -1;
+		info->m_J2linearAxis[2*info->rowskip+2] = -1;
+	}
+	
+	b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB);
+   
+	{
+	//	b3Vector3 a2n = -a2;
+		b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+
+    // set right hand side for the linear dofs
+	b3Scalar k = info->fps * info->erp;
+	b3Vector3 linearError = k*(a2+worldPosB-a1-worldPosA);
+    int j;
+	for (j=0; j<3; j++)
+    {
+        info->m_constraintError[j*info->rowskip] = linearError[j];
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+
+		//fix the 3 angular degrees of freedom
+
+	int start_row = 3;
+	int s = info->rowskip;
+    int start_index = start_row * s;
+
+    // 3 rows to make body rotations equal
+	info->m_J1angularAxis[start_index] = 1;
+    info->m_J1angularAxis[start_index + s + 1] = 1;
+    info->m_J1angularAxis[start_index + s*2+2] = 1;
+    if ( info->m_J2angularAxis)
+    {
+        info->m_J2angularAxis[start_index] = -1;
+        info->m_J2angularAxis[start_index + s+1] = -1;
+        info->m_J2angularAxis[start_index + s*2+2] = -1;
+    }
+
+
+    // set right hand side for the angular dofs
+
+	b3Vector3 diff;
+	b3Scalar angle;
+	b3Quaternion qrelCur = worldOrnA *worldOrnB.inverse();
+
+	b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
+	diff*=-angle;
+	for (j=0; j<3; j++)
+    {
+        info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
+    }
+
+}

src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h

@@ -0,0 +1,35 @@
+
+#ifndef B3_FIXED_CONSTRAINT_H
+#define B3_FIXED_CONSTRAINT_H
+
+#include "b3TypedConstraint.h"
+
+B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint
+{
+	b3Vector3 m_pivotInA;
+	b3Vector3 m_pivotInB;
+	b3Quaternion m_relTargetAB;
+
+public:
+	b3FixedConstraint(int  rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB);
+	
+	virtual ~b3FixedConstraint();
+
+	
+	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
+
+	virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies);
+
+	virtual	void	setParam(int num, b3Scalar value, int axis = -1)
+	{
+		b3Assert(0);
+	}
+	virtual	b3Scalar getParam(int num, int axis = -1) const
+	{
+		b3Assert(0);
+		return 0.f;
+	}
+
+};
+
+#endif //B3_FIXED_CONSTRAINT_H

src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp

@@ -42,8 +42,7 @@ b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int  rbB, const b3Trans
 , m_frameInB(frameInB),
 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
-m_flags(0),
+m_flags(0)
-m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
 {
 	calculateTransforms(bodies);
 }
@@ -275,56 +274,42 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
 
 void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
 {
-	if (m_useSolveConstraintObsolete)
+	//prepare constraint
+	calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
+	info->m_numConstraintRows = 0;
+	info->nub = 6;
+	int i;
+	//test linear limits
+	for(i = 0; i < 3; i++)
 	{
-		info->m_numConstraintRows = 0;
+		if(m_linearLimits.needApplyForce(i))
-		info->nub = 0;
-	} else
-	{
-		//prepare constraint
-		calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
-		info->m_numConstraintRows = 0;
-		info->nub = 6;
-		int i;
-		//test linear limits
-		for(i = 0; i < 3; i++)
 		{
-			if(m_linearLimits.needApplyForce(i))
+			info->m_numConstraintRows++;
-			{
+			info->nub--;
-				info->m_numConstraintRows++;
-				info->nub--;
-			}
-		}
-		//test angular limits
-		for (i=0;i<3 ;i++ )
-		{
-			if(testAngularLimitMotor(i))
-			{
-				info->m_numConstraintRows++;
-				info->nub--;
-			}
 		}
 	}
+	//test angular limits
+	for (i=0;i<3 ;i++ )
+	{
+		if(testAngularLimitMotor(i))
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+	}
+//	printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
 }
 
 void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
 {
-	if (m_useSolveConstraintObsolete)
+	//pre-allocate all 6
-	{
+	info->m_numConstraintRows = 6;
-		info->m_numConstraintRows = 0;
+	info->nub = 0;
-		info->nub = 0;
-	} else
-	{
-		//pre-allocate all 6
-		info->m_numConstraintRows = 6;
-		info->nub = 0;
-	}
 }
 
 
 void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyCL* bodies)
 {
-	b3Assert(!m_useSolveConstraintObsolete);
 
 	b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
 	b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
@@ -350,7 +335,6 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod
 void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyCL* bodies)
 {
 	
-	b3Assert(!m_useSolveConstraintObsolete);
 	//prepare constraint
 	calculateTransforms(transA,transB,bodies);
 

src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h

@@ -299,12 +299,12 @@ protected:
 protected:
     //! temporal variables
     //!@{
-    b3Scalar m_timeStep;
     b3Transform m_calculatedTransformA;
     b3Transform m_calculatedTransformB;
     b3Vector3 m_calculatedAxisAngleDiff;
     b3Vector3 m_calculatedAxis[3];
     b3Vector3 m_calculatedLinearDiff;
+    b3Scalar m_timeStep;
 	b3Scalar	m_factA;
 	b3Scalar	m_factB;
 	bool		m_hasStaticBody;
@@ -343,9 +343,6 @@ public:
 
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 	
-	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	bool		m_useSolveConstraintObsolete;
-
     b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyCL* bodies);
     
 	//! Calcs global transform of the offsets

src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp

@@ -25,8 +25,7 @@ subject to the following restrictions:
 
 b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB)
 :b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
-m_flags(0),
+m_flags(0)
-m_useSolveConstraintObsolete(false)
 {
 
 }
@@ -49,15 +48,8 @@ void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBod
 
 void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
 {
-	if (m_useSolveConstraintObsolete)
-	{
-		info->m_numConstraintRows = 0;
-		info->nub = 0;
-	} else
-	{
 		info->m_numConstraintRows = 3;
 		info->nub = 3;
-	}
 }
 
 
@@ -80,7 +72,6 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo
 
 void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
 {
-	b3Assert(!m_useSolveConstraintObsolete);
 
 	 //retrieve matrices
 
@@ -92,6 +83,8 @@ void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const
 	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
 	b3Vector3 a1 = body0_trans.getBasis()*getPivotInA();
+	b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
+
 	{
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
 		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);

src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h

@@ -68,9 +68,6 @@ public:
 
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	bool		m_useSolveConstraintObsolete;
-
 	b3ConstraintSetting	m_setting;
 
 	b3Point2PointConstraint(int  rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB);

src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h

@@ -33,6 +33,7 @@ enum b3TypedConstraintType
 	B3_CONTACT_CONSTRAINT_TYPE,
 	B3_D6_SPRING_CONSTRAINT_TYPE,
 	B3_GEAR_CONSTRAINT_TYPE,
+	B3_FIXED_CONSTRAINT_TYPE,
 	B3_MAX_CONSTRAINT_TYPE
 };