From 3beae80a730cb612e2d879e7e9080ef1b00f34c5 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 16 Jul 2013 19:05:07 -0700 Subject: [PATCH] prepare for GPU fixed constraint (not done yet) --- .../GpuDemos/constraints/ConstraintsDemo.cpp | 22 +++--- .../RigidBody/b3GpuGenericConstraint.h | 3 +- .../RigidBody/b3GpuRigidBodyPipeline.cpp | 16 ++++- .../RigidBody/b3GpuRigidBodyPipeline.h | 2 +- .../RigidBody/kernels/jointSolver.cl | 72 +++++++++++++++++++ .../RigidBody/kernels/jointSolver.h | 72 +++++++++++++++++++ 6 files changed, 175 insertions(+), 12 deletions(-) diff --git a/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp b/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp index 38e4344a7..1731ba8f6 100644 --- a/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp +++ b/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)); - frameInB.setOrigin(b3Vector3(0,1.1,0)); - - c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB); + frameInA.setOrigin(pivotInA); + frameInB.setOrigin(pivotInB); + b3Quaternion relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); + + //c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB); //c->setBreakingImpulseThreshold(37.1); - */ + int cid = m_data->m_rigidBodyPipeline->createFixedConstraint(pid,prevBody,pivotInA,pivotInB,relTargetAB); + + break; } @@ -277,4 +283,4 @@ void GpuConstraintsDemo::createStaticEnvironment(const ConstructionInfo& ci) int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false); } -} +} \ No newline at end of file diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h b/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h index 78386920f..3db24bc3c 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h +++ b/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 }; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp index 502cd59bc..09cb7b21c 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ b/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; } diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h index eaff485ce..1dce3b15e 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h +++ b/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); diff --git a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl index 7f7b0d422..b88180022 100644 --- a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl +++ b/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: { } diff --git a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h index e87642108..a5dc1525e 100644 --- a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h +++ b/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"