prepare for GPU fixed constraint (not done yet)

Demos3/GpuDemos/constraints/ConstraintsDemo.cpp

@@ -53,7 +53,7 @@ void GpuConstraintsDemo::setupScene(const ConstructionInfo& ci)
 
 	m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
 //	m_data->m_rigidBodyPipeline->setGravity(b3Vector3(4,-10,0));
-	float camPos[4]={ci.arraySizeX,ci.gapY*ci.arraySizeY/2,ci.arraySizeZ,0};
+	float camPos[4]={ci.arraySizeX,ci.gapY,ci.arraySizeZ,0};
 	//float camPos[4]={1,12.5,1.5,0};
 	
 	m_instancingRenderer->setCameraTargetPosition(camPos);
@@ -197,16 +197,22 @@ int	GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
 							}
 						case 1:
 							{
-								/*
+							
+								b3Vector3 pivotInA(-1.05,0,0);
+								b3Vector3 pivotInB (1.05,0,0);
+
 							b3Transform frameInA,frameInB;
 							frameInA.setIdentity();
 							frameInB.setIdentity();
-							frameInA.setOrigin(b3Vector3(0,-1.1,0));
+							frameInA.setOrigin(pivotInA);
-							frameInB.setOrigin(b3Vector3(0,1.1,0));
+							frameInB.setOrigin(pivotInB);
+							b3Quaternion relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
 							
-							c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB);
+							//c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB);
 							//c->setBreakingImpulseThreshold(37.1);
-							*/
+							int cid = m_data->m_rigidBodyPipeline->createFixedConstraint(pid,prevBody,pivotInA,pivotInB,relTargetAB);
+							
+							
 
 							break;
 							}

src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h

@@ -26,6 +26,7 @@ enum B3_CONSTRAINT_FLAGS
 enum b3GpuGenericConstraintType
 {
 	B3_GPU_POINT2POINT_CONSTRAINT_TYPE=3,
+	B3_GPU_FIXED_CONSTRAINT_TYPE=4,
 //	B3_HINGE_CONSTRAINT_TYPE,
 //	B3_CONETWIST_CONSTRAINT_TYPE,
 //	B3_D6_CONSTRAINT_TYPE,
@@ -33,7 +34,7 @@ enum b3GpuGenericConstraintType
 //	B3_CONTACT_CONSTRAINT_TYPE,
 //	B3_D6_SPRING_CONSTRAINT_TYPE,
 //	B3_GEAR_CONSTRAINT_TYPE,
-//	B3_FIXED_CONSTRAINT_TYPE,
+	
 	B3_GPU_MAX_CONSTRAINT_TYPE
 };
 

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp

@@ -186,10 +186,22 @@ int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, co
 	m_data->m_cpuConstraints.push_back(c);
 	return c.m_uid;
 }
-int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* frameOrnA, const float* frameOrnB)
+int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB)
 {
 	m_data->m_gpuSolver->recomputeBatches();
-	return 0;
+	b3GpuGenericConstraint c;
+	c.m_uid = m_data->m_constraintUid;
+	m_data->m_constraintUid++;
+	c.m_flags = B3_CONSTRAINT_FLAG_ENABLED;
+	c.m_rbA = bodyA;
+	c.m_rbB = bodyB;
+	c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]);
+	c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]);
+	c.m_relTargetAB.setValue(relTargetAB[0],relTargetAB[1],relTargetAB[2],relTargetAB[3]);
+	c.m_breakingImpulseThreshold = 1e30f;
+	c.m_constraintType = B3_GPU_FIXED_CONSTRAINT_TYPE;
+	m_data->m_cpuConstraints.push_back(c);
+	return c.m_uid;
 }
 
 

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h

@@ -56,7 +56,7 @@ public:
 	void reset();
 	
 	int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB);
-	int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* frameOrnA, const float* frameOrnB);
+	int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB);
 	void removeConstraintByUid(int uid);
 
 	void	addConstraint(class b3TypedConstraint* constraint);

src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl

@@ -15,6 +15,9 @@ subject to the following restrictions:
 
 
 #define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3
+#define B3_GPU_FIXED_CONSTRAINT_TYPE 4
+
+
 #define B3_INFINITY 1e30f
 
 #define mymake_float4 (float4)
@@ -359,6 +362,12 @@ __kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGeneric
 			batchConstraints[i].m_numConstraintRows = 3;
 			break;
 		}
+		case B3_GPU_FIXED_CONSTRAINT_TYPE:
+		{
+			infos[i] = 6;
+			batchConstraints[i].m_numConstraintRows = 6;
+			break;
+		}
 		default:
 		{
 		}
@@ -500,6 +509,60 @@ void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstr
     }
 }
 
+/*
+@todo: convert this code to OpenCL
+void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle)
+{
+	Quaternion orn1 = orn0.nearest(orn1a);
+	Quaternion dorn = orn1 * orn0.inverse();
+	angle = dorn.getAngle();
+	axis = b3Vector3(dorn.getX(),dorn.getY(),dorn.getZ());
+	axis[3] = b3Scalar(0.);
+	//check for axis length
+	b3Scalar len = axis.length2();
+	if (len < B3_EPSILON*B3_EPSILON)
+		axis = b3Vector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
+	else
+		axis /= b3Sqrt(len);
+}
+*/
+
+void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)
+{
+	Quaternion worldOrnA = bodies[constraint->m_rbA].m_quat;
+	Quaternion worldOrnB = bodies[constraint->m_rbB].m_quat;
+
+	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;
+	}
+	/*
+	@todo
+	float currERP = info->erp;
+	float k = info->fps * currERP;
+	float4 diff;
+	float angle;
+	float4 qrelCur = worldOrnA *qtInvert(worldOrnB);
+
+	calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,diff,angle);
+	diff*=-angle;
+	for (j=0; j<3; j++)
+    {
+        info->m_constraintError[(start_row+j)*info->rowskip] = k * diff[j];
+    }
+	*/
+
+}
+
 
 __kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies)
 {
@@ -651,6 +714,15 @@ __kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows,
 				getInfo2Point2Point(constraint,&info2,bodies);
 				break;
 			}
+			case B3_GPU_FIXED_CONSTRAINT_TYPE:
+			{
+				getInfo2Point2Point(constraint,&info2,bodies);
+
+				getInfo2FixedOrientation(constraint,&info2,bodies,3);
+
+				break;
+			}
+
 			default:
 			{
 			}

src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h

@@ -17,6 +17,9 @@ static const char* solveConstraintRowsCL= \
 "\n"
 "\n"
 "#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n"
+"#define B3_GPU_FIXED_CONSTRAINT_TYPE 4\n"
+"\n"
+"\n"
 "#define B3_INFINITY 1e30f\n"
 "\n"
 "#define mymake_float4 (float4)\n"
@@ -361,6 +364,12 @@ static const char* solveConstraintRowsCL= \
 "			batchConstraints[i].m_numConstraintRows = 3;\n"
 "			break;\n"
 "		}\n"
+"		case B3_GPU_FIXED_CONSTRAINT_TYPE:\n"
+"		{\n"
+"			infos[i] = 6;\n"
+"			batchConstraints[i].m_numConstraintRows = 6;\n"
+"			break;\n"
+"		}\n"
 "		default:\n"
 "		{\n"
 "		}\n"
@@ -502,6 +511,60 @@ static const char* solveConstraintRowsCL= \
 "    }\n"
 "}\n"
 "\n"
+"/*\n"
+"@todo: convert this code to OpenCL\n"
+"void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle)\n"
+"{\n"
+"	Quaternion orn1 = orn0.nearest(orn1a);\n"
+"	Quaternion dorn = orn1 * orn0.inverse();\n"
+"	angle = dorn.getAngle();\n"
+"	axis = b3Vector3(dorn.getX(),dorn.getY(),dorn.getZ());\n"
+"	axis[3] = b3Scalar(0.);\n"
+"	//check for axis length\n"
+"	b3Scalar len = axis.length2();\n"
+"	if (len < B3_EPSILON*B3_EPSILON)\n"
+"		axis = b3Vector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));\n"
+"	else\n"
+"		axis /= b3Sqrt(len);\n"
+"}\n"
+"*/\n"
+"\n"
+"void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)\n"
+"{\n"
+"	Quaternion worldOrnA = bodies[constraint->m_rbA].m_quat;\n"
+"	Quaternion worldOrnB = bodies[constraint->m_rbB].m_quat;\n"
+"\n"
+"	int s = info->rowskip;\n"
+"	int start_index = start_row * s;\n"
+"\n"
+"	// 3 rows to make body rotations equal\n"
+"	info->m_J1angularAxis[start_index] = 1;\n"
+"	info->m_J1angularAxis[start_index + s + 1] = 1;\n"
+"	info->m_J1angularAxis[start_index + s*2+2] = 1;\n"
+"	if ( info->m_J2angularAxis)\n"
+"	{\n"
+"		info->m_J2angularAxis[start_index] = -1;\n"
+"		info->m_J2angularAxis[start_index + s+1] = -1;\n"
+"		info->m_J2angularAxis[start_index + s*2+2] = -1;\n"
+"	}\n"
+"	/*\n"
+"	@todo\n"
+"	float currERP = info->erp;\n"
+"	float k = info->fps * currERP;\n"
+"	float4 diff;\n"
+"	float angle;\n"
+"	float4 qrelCur = worldOrnA *qtInvert(worldOrnB);\n"
+"\n"
+"	calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,diff,angle);\n"
+"	diff*=-angle;\n"
+"	for (j=0; j<3; j++)\n"
+"    {\n"
+"        info->m_constraintError[(start_row+j)*info->rowskip] = k * diff[j];\n"
+"    }\n"
+"	*/\n"
+"\n"
+"}\n"
+"\n"
 "\n"
 "__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies)\n"
 "{\n"
@@ -653,6 +716,15 @@ static const char* solveConstraintRowsCL= \
 "				getInfo2Point2Point(constraint,&info2,bodies);\n"
 "				break;\n"
 "			}\n"
+"			case B3_GPU_FIXED_CONSTRAINT_TYPE:\n"
+"			{\n"
+"				getInfo2Point2Point(constraint,&info2,bodies);\n"
+"\n"
+"				getInfo2FixedOrientation(constraint,&info2,bodies,3);\n"
+"\n"
+"				break;\n"
+"			}\n"
+"\n"
 "			default:\n"
 "			{\n"
 "			}\n"