change lcpp Lua preprocessor, to keep #defines and comments, remove empty lines
remove duplicate data in b3Contact4 (now in btContact4Data shared between CPU/C++ and OpenCL) OpenCL kernels use #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" Increase number of batches back to 250 (from 50), need to fix this hard coded number (see https://github.com/erwincoumans/bullet3/issues/12) Work towards GJK/EPA, in addition to SAT/clipping (early on)
This commit is contained in:
erwincoumans
@@ -2,56 +2,37 @@
|
||||
static const char* solveConstraintRowsCL= \
|
||||
"/*\n"
|
||||
"Copyright (c) 2013 Advanced Micro Devices, Inc. \n"
|
||||
"\n"
|
||||
"This software is provided 'as-is', without any express or implied warranty.\n"
|
||||
"In no event will the authors be held liable for any damages arising from the use of this software.\n"
|
||||
"Permission is granted to anyone to use this software for any purpose, \n"
|
||||
"including commercial applications, and to alter it and redistribute it freely, \n"
|
||||
"subject to the following restrictions:\n"
|
||||
"\n"
|
||||
"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
|
||||
"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
|
||||
"3. This notice may not be removed or altered from any source distribution.\n"
|
||||
"*/\n"
|
||||
"//Originally written by Erwin Coumans\n"
|
||||
"\n"
|
||||
"#define B3_CONSTRAINT_FLAG_ENABLED 1\n"
|
||||
"\n"
|
||||
"#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n"
|
||||
"#define B3_GPU_FIXED_CONSTRAINT_TYPE 4\n"
|
||||
"\n"
|
||||
"#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails\n"
|
||||
"#define B3_INFINITY 1e30f\n"
|
||||
"\n"
|
||||
"#define mymake_float4 (float4)\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline float dot3F4(float4 a, float4 b)\n"
|
||||
"{\n"
|
||||
" float4 a1 = mymake_float4(a.xyz,0.f);\n"
|
||||
" float4 b1 = mymake_float4(b.xyz,0.f);\n"
|
||||
" return dot(a1, b1);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef float4 Quaternion;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" float4 m_row[3];\n"
|
||||
"}Matrix3x3;\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 mtMul1(Matrix3x3 a, float4 b);\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 mtMul3(float4 a, Matrix3x3 b);\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 mtMul1(Matrix3x3 a, float4 b)\n"
|
||||
"{\n"
|
||||
@@ -62,36 +43,28 @@ static const char* solveConstraintRowsCL= \
|
||||
" ans.w = 0.f;\n"
|
||||
" return ans;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 mtMul3(float4 a, Matrix3x3 b)\n"
|
||||
"{\n"
|
||||
" float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n"
|
||||
" float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n"
|
||||
" float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n"
|
||||
"\n"
|
||||
" float4 ans;\n"
|
||||
" ans.x = dot3F4( a, colx );\n"
|
||||
" ans.y = dot3F4( a, coly );\n"
|
||||
" ans.z = dot3F4( a, colz );\n"
|
||||
" return ans;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" Matrix3x3 m_invInertiaWorld;\n"
|
||||
" Matrix3x3 m_initInvInertia;\n"
|
||||
"} BodyInertia;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" Matrix3x3 m_basis;//orientation\n"
|
||||
" float4 m_origin;//transform\n"
|
||||
"}b3Transform;\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
"// b3Transform m_worldTransformUnused;\n"
|
||||
@@ -104,38 +77,30 @@ static const char* solveConstraintRowsCL= \
|
||||
" float4 m_turnVelocity;\n"
|
||||
" float4 m_linearVelocity;\n"
|
||||
" float4 m_angularVelocity;\n"
|
||||
"\n"
|
||||
" union \n"
|
||||
" {\n"
|
||||
" void* m_originalBody;\n"
|
||||
" int m_originalBodyIndex;\n"
|
||||
" };\n"
|
||||
" int padding[3];\n"
|
||||
"\n"
|
||||
"} b3GpuSolverBody;\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" float4 m_pos;\n"
|
||||
" Quaternion m_quat;\n"
|
||||
" float4 m_linVel;\n"
|
||||
" float4 m_angVel;\n"
|
||||
"\n"
|
||||
" unsigned int m_shapeIdx;\n"
|
||||
" float m_invMass;\n"
|
||||
" float m_restituitionCoeff;\n"
|
||||
" float m_frictionCoeff;\n"
|
||||
"} b3RigidBodyCL;\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
"\n"
|
||||
" float4 m_relpos1CrossNormal;\n"
|
||||
" float4 m_contactNormal;\n"
|
||||
"\n"
|
||||
" float4 m_relpos2CrossNormal;\n"
|
||||
" //float4 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal\n"
|
||||
"\n"
|
||||
" float4 m_angularComponentA;\n"
|
||||
" float4 m_angularComponentB;\n"
|
||||
" \n"
|
||||
@@ -152,15 +117,11 @@ static const char* solveConstraintRowsCL= \
|
||||
" float m_upperLimit;\n"
|
||||
" float m_rhsPenetration;\n"
|
||||
" int m_originalConstraint;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" int m_overrideNumSolverIterations;\n"
|
||||
" int m_frictionIndex;\n"
|
||||
" int m_solverBodyIdA;\n"
|
||||
" int m_solverBodyIdB;\n"
|
||||
"\n"
|
||||
"} b3SolverConstraint;\n"
|
||||
"\n"
|
||||
"typedef struct \n"
|
||||
"{\n"
|
||||
" int m_bodyAPtrAndSignBit;\n"
|
||||
@@ -168,28 +129,18 @@ static const char* solveConstraintRowsCL= \
|
||||
" int m_originalConstraintIndex;\n"
|
||||
" int m_batchId;\n"
|
||||
"} b3BatchConstraint;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct \n"
|
||||
"{\n"
|
||||
" int m_constraintType;\n"
|
||||
" int m_rbA;\n"
|
||||
" int m_rbB;\n"
|
||||
" float m_breakingImpulseThreshold;\n"
|
||||
"\n"
|
||||
" float4 m_pivotInA;\n"
|
||||
" float4 m_pivotInB;\n"
|
||||
" Quaternion m_relTargetAB;\n"
|
||||
"\n"
|
||||
" int m_flags;\n"
|
||||
" int m_padding[3];\n"
|
||||
"} b3GpuGenericConstraint;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"/*b3Transform getWorldTransform(b3RigidBodyCL* rb)\n"
|
||||
"{\n"
|
||||
" b3Transform newTrans;\n"
|
||||
@@ -197,39 +148,25 @@ static const char* solveConstraintRowsCL= \
|
||||
" newTrans.setRotation(rb->m_quat);\n"
|
||||
" return newTrans;\n"
|
||||
"}*/\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 cross3(float4 a, float4 b)\n"
|
||||
"{\n"
|
||||
" return cross(a,b);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 fastNormalize4(float4 v)\n"
|
||||
"{\n"
|
||||
" v = mymake_float4(v.xyz,0.f);\n"
|
||||
" return fast_normalize(v);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Quaternion qtMul(Quaternion a, Quaternion b);\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Quaternion qtNormalize(Quaternion in);\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 qtRotate(Quaternion q, float4 vec);\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Quaternion qtInvert(Quaternion q);\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Quaternion qtMul(Quaternion a, Quaternion b)\n"
|
||||
"{\n"
|
||||
@@ -240,7 +177,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" ans.w = a.w*b.w - dot3F4(a, b);\n"
|
||||
" return ans;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Quaternion qtNormalize(Quaternion in)\n"
|
||||
"{\n"
|
||||
@@ -257,30 +193,23 @@ static const char* solveConstraintRowsCL= \
|
||||
" float4 out = qtMul(qtMul(q,vcpy),qInv);\n"
|
||||
" return out;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Quaternion qtInvert(Quaternion q)\n"
|
||||
"{\n"
|
||||
" return (Quaternion)(-q.xyz, q.w);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline void internalApplyImpulse(__global b3GpuSolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude)\n"
|
||||
"{\n"
|
||||
" body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor;\n"
|
||||
" body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c)\n"
|
||||
"{\n"
|
||||
" float deltaImpulse = c->m_rhs-c->m_appliedImpulse*c->m_cfm;\n"
|
||||
" float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity);\n"
|
||||
" float deltaVel2Dotn = -dot3F4(c->m_contactNormal,body2->m_deltaLinearVelocity) + dot3F4(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity);\n"
|
||||
"\n"
|
||||
" deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv;\n"
|
||||
" deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv;\n"
|
||||
"\n"
|
||||
" float sum = c->m_appliedImpulse + deltaImpulse;\n"
|
||||
" if (sum < c->m_lowerLimit)\n"
|
||||
" {\n"
|
||||
@@ -296,12 +225,9 @@ static const char* solveConstraintRowsCL= \
|
||||
" {\n"
|
||||
" c->m_appliedImpulse = sum;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse);\n"
|
||||
" internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse);\n"
|
||||
"\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n"
|
||||
" __global b3BatchConstraint* batchConstraints,\n"
|
||||
" __global b3SolverConstraint* rows,\n"
|
||||
@@ -315,7 +241,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" int b = get_global_id(0);\n"
|
||||
" if (b>=numConstraintsInBatch)\n"
|
||||
" return;\n"
|
||||
"\n"
|
||||
" __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];\n"
|
||||
" int originalConstraintIndex = c->m_originalConstraintIndex;\n"
|
||||
" if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED)\n"
|
||||
@@ -329,16 +254,13 @@ static const char* solveConstraintRowsCL= \
|
||||
" }\n"
|
||||
" }\n"
|
||||
"};\n"
|
||||
"\n"
|
||||
"__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies)\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" if (i>=numBodies)\n"
|
||||
" return;\n"
|
||||
"\n"
|
||||
" __global b3GpuSolverBody* solverBody = &solverBodies[i];\n"
|
||||
" __global b3RigidBodyCL* bodyCL = &bodiesCL[i];\n"
|
||||
"\n"
|
||||
" solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f);\n"
|
||||
" solverBody->m_deltaAngularVelocity = (float4)(0.f,0.f,0.f,0.f);\n"
|
||||
" solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n"
|
||||
@@ -350,7 +272,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" solverBody->m_linearVelocity = bodyCL->m_linVel;\n"
|
||||
" solverBody->m_angularVelocity = bodyCL->m_angVel;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints)\n"
|
||||
"{\n"
|
||||
" int cid = get_global_id(0);\n"
|
||||
@@ -370,17 +291,12 @@ static const char* solveConstraintRowsCL= \
|
||||
" }\n"
|
||||
" }\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints)\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" if (i>=numConstraints)\n"
|
||||
" return;\n"
|
||||
"\n"
|
||||
" __global b3GpuGenericConstraint* constraint = &constraints[i];\n"
|
||||
"\n"
|
||||
" switch (constraint->m_constraintType)\n"
|
||||
" {\n"
|
||||
" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n"
|
||||
@@ -398,7 +314,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" }\n"
|
||||
" }\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, \n"
|
||||
" __global b3BatchConstraint* batchConstraints, \n"
|
||||
" __global b3GpuGenericConstraint* constraints,\n"
|
||||
@@ -408,26 +323,18 @@ static const char* solveConstraintRowsCL= \
|
||||
" int i = get_global_id(0);\n"
|
||||
" if (i>=numConstraints)\n"
|
||||
" return;\n"
|
||||
"\n"
|
||||
" int rbA = constraints[i].m_rbA;\n"
|
||||
" int rbB = constraints[i].m_rbB;\n"
|
||||
"\n"
|
||||
" batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass? rbA : -rbA;\n"
|
||||
" batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass? rbB : -rbB;\n"
|
||||
" batchConstraints[i].m_batchId = -1;\n"
|
||||
" batchConstraints[i].m_originalConstraintIndex = i;\n"
|
||||
"\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" // integrator parameters: frames per second (1/stepsize), default error\n"
|
||||
" // reduction parameter (0..1).\n"
|
||||
" float fps,erp;\n"
|
||||
"\n"
|
||||
" // for the first and second body, pointers to two (linear and angular)\n"
|
||||
" // n*3 jacobian sub matrices, stored by rows. these matrices will have\n"
|
||||
" // been initialized to 0 on entry. if the second body is zero then the\n"
|
||||
@@ -441,7 +348,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" {\n"
|
||||
" __global float4* m_J1angularAxisFloat4;\n"
|
||||
" __global float* m_J1angularAxis;\n"
|
||||
"\n"
|
||||
" };\n"
|
||||
" union\n"
|
||||
" {\n"
|
||||
@@ -455,17 +361,14 @@ static const char* solveConstraintRowsCL= \
|
||||
" };\n"
|
||||
" // elements to jump from one row to the next in J's\n"
|
||||
" int rowskip;\n"
|
||||
"\n"
|
||||
" // right hand sides of the equation J*v = c + cfm * lambda. cfm is the\n"
|
||||
" // \"constraint force mixing\" vector. c is set to zero on entry, cfm is\n"
|
||||
" // set to a constant value (typically very small or zero) value on entry.\n"
|
||||
" __global float* m_constraintError;\n"
|
||||
" __global float* cfm;\n"
|
||||
"\n"
|
||||
" // lo and hi limits for variables (set to -/+ infinity on entry).\n"
|
||||
" __global float* m_lowerLimit;\n"
|
||||
" __global float* m_upperLimit;\n"
|
||||
"\n"
|
||||
" // findex vector for variables. see the LCP solver interface for a\n"
|
||||
" // description of what this does. this is set to -1 on entry.\n"
|
||||
" // note that the returned indexes are relative to the first index of\n"
|
||||
@@ -473,39 +376,28 @@ static const char* solveConstraintRowsCL= \
|
||||
" __global int *findex;\n"
|
||||
" // number of solver iterations\n"
|
||||
" int m_numIterations;\n"
|
||||
"\n"
|
||||
" //damping of the velocity\n"
|
||||
" float m_damping;\n"
|
||||
"} b3GpuConstraintInfo2;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"void getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2)\n"
|
||||
"{\n"
|
||||
" *v0 = (float4)(0. ,-vecIn.z ,vecIn.y,0.f);\n"
|
||||
" *v1 = (float4)(vecIn.z ,0. ,-vecIn.x,0.f);\n"
|
||||
" *v2 = (float4)(-vecIn.y ,vecIn.x ,0.f,0.f);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies)\n"
|
||||
"{\n"
|
||||
" float4 posA = bodies[constraint->m_rbA].m_pos;\n"
|
||||
" Quaternion rotA = bodies[constraint->m_rbA].m_quat;\n"
|
||||
"\n"
|
||||
" float4 posB = bodies[constraint->m_rbB].m_pos;\n"
|
||||
" Quaternion rotB = bodies[constraint->m_rbB].m_quat;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" // anchor points in global coordinates with respect to body PORs.\n"
|
||||
" \n"
|
||||
" // set jacobian\n"
|
||||
" info->m_J1linearAxis[0] = 1;\n"
|
||||
" info->m_J1linearAxis[info->rowskip+1] = 1;\n"
|
||||
" info->m_J1linearAxis[2*info->rowskip+2] = 1;\n"
|
||||
"\n"
|
||||
" float4 a1 = qtRotate(rotA,constraint->m_pivotInA);\n"
|
||||
"\n"
|
||||
" {\n"
|
||||
" __global float4* angular0 = (__global float4*)(info->m_J1angularAxis);\n"
|
||||
" __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip);\n"
|
||||
@@ -533,18 +425,15 @@ static const char* solveConstraintRowsCL= \
|
||||
" // set right hand side\n"
|
||||
"// float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;\n"
|
||||
" float currERP = info->erp;\n"
|
||||
"\n"
|
||||
" float k = info->fps * currERP;\n"
|
||||
" int j;\n"
|
||||
" float4 result = a2 + posB - a1 - posA;\n"
|
||||
" float* resultPtr = &result;\n"
|
||||
"\n"
|
||||
" for (j=0; j<3; j++)\n"
|
||||
" {\n"
|
||||
" info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]);\n"
|
||||
" }\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"Quaternion nearest( Quaternion first, Quaternion qd)\n"
|
||||
"{\n"
|
||||
" Quaternion diff,sum;\n"
|
||||
@@ -555,7 +444,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" return qd;\n"
|
||||
" return (-qd);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"float b3Acos(float x) \n"
|
||||
"{ \n"
|
||||
" if (x<-1) \n"
|
||||
@@ -564,7 +452,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" x=1;\n"
|
||||
" return acos(x); \n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"float getAngle(Quaternion orn)\n"
|
||||
"{\n"
|
||||
" if (orn.w>=1.f)\n"
|
||||
@@ -572,7 +459,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" float s = 2.f * b3Acos(orn.w);\n"
|
||||
" return s;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle)\n"
|
||||
"{\n"
|
||||
" Quaternion orn1 = nearest(orn0,orn1a);\n"
|
||||
@@ -588,17 +474,12 @@ static const char* solveConstraintRowsCL= \
|
||||
" else\n"
|
||||
" *axis /= sqrt(len);\n"
|
||||
"}\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"
|
||||
@@ -626,16 +507,12 @@ static const char* solveConstraintRowsCL= \
|
||||
" info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j];\n"
|
||||
" }\n"
|
||||
" \n"
|
||||
"\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies)\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" if (i>=numBodies)\n"
|
||||
" return;\n"
|
||||
"\n"
|
||||
" if (bodies[i].m_invMass)\n"
|
||||
" {\n"
|
||||
"// if (length(solverBodies[i].m_deltaLinearVelocity)<MOTIONCLAMP)\n"
|
||||
@@ -648,8 +525,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" } \n"
|
||||
" }\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, \n"
|
||||
" __global unsigned int* infos, \n"
|
||||
" __global unsigned int* constraintRowOffsets, \n"
|
||||
@@ -665,7 +540,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" int globalNumIterations,\n"
|
||||
" int numConstraints)\n"
|
||||
"{\n"
|
||||
"\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" if (i>=numConstraints)\n"
|
||||
" return;\n"
|
||||
@@ -675,17 +549,12 @@ static const char* solveConstraintRowsCL= \
|
||||
" \n"
|
||||
" __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]];\n"
|
||||
" __global b3GpuGenericConstraint* constraint = &constraints[i];\n"
|
||||
"\n"
|
||||
" __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n"
|
||||
" __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n"
|
||||
"\n"
|
||||
" int solverBodyIdA = constraint->m_rbA;\n"
|
||||
" int solverBodyIdB = constraint->m_rbB;\n"
|
||||
"\n"
|
||||
" __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n"
|
||||
" __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" if (rbA->m_invMass)\n"
|
||||
" {\n"
|
||||
" batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;\n"
|
||||
@@ -695,7 +564,6 @@ static const char* solveConstraintRowsCL= \
|
||||
"// m_staticIdx = 0;\n"
|
||||
" batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" if (rbB->m_invMass)\n"
|
||||
" {\n"
|
||||
" batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;\n"
|
||||
@@ -705,14 +573,11 @@ static const char* solveConstraintRowsCL= \
|
||||
"// m_staticIdx = 0;\n"
|
||||
" batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" if (info1)\n"
|
||||
" {\n"
|
||||
" int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n"
|
||||
"// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n"
|
||||
" // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" int j;\n"
|
||||
" for ( j=0;j<info1;j++)\n"
|
||||
" {\n"
|
||||
@@ -728,7 +593,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" currentConstraintRow[j].m_jacDiagABInv = 0.f;\n"
|
||||
" currentConstraintRow[j].m_lowerLimit = 0.f;\n"
|
||||
" currentConstraintRow[j].m_upperLimit = 0.f;\n"
|
||||
"\n"
|
||||
" currentConstraintRow[j].m_originalConstraint = i;\n"
|
||||
" currentConstraintRow[j].m_overrideNumSolverIterations = 0;\n"
|
||||
" currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0);\n"
|
||||
@@ -746,7 +610,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;\n"
|
||||
" currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; \n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" bodyAPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n"
|
||||
" bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n"
|
||||
" bodyAPtr->m_pushVelocity = (float4)(0,0,0,0);\n"
|
||||
@@ -755,12 +618,8 @@ static const char* solveConstraintRowsCL= \
|
||||
" bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n"
|
||||
" bodyBPtr->m_pushVelocity = (float4)(0,0,0,0);\n"
|
||||
" bodyBPtr->m_turnVelocity = (float4)(0,0,0,0);\n"
|
||||
"\n"
|
||||
" int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n"
|
||||
"\n"
|
||||
" \n"
|
||||
"\n"
|
||||
"\n"
|
||||
" b3GpuConstraintInfo2 info2;\n"
|
||||
" info2.fps = 1.f/timeStep;\n"
|
||||
" info2.erp = globalErp;\n"
|
||||
@@ -769,7 +628,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" info2.m_J2linearAxisFloat4 = 0;\n"
|
||||
" info2.m_J2angularAxisFloat4 = ¤tConstraintRow->m_relpos2CrossNormal;\n"
|
||||
" info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n"
|
||||
"\n"
|
||||
" ///the size of b3SolverConstraint needs be a multiple of float\n"
|
||||
"// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint));\n"
|
||||
" info2.m_constraintError = ¤tConstraintRow->m_rhs;\n"
|
||||
@@ -779,7 +637,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;\n"
|
||||
" info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;\n"
|
||||
" info2.m_numIterations = globalNumIterations;\n"
|
||||
"\n"
|
||||
" switch (constraint->m_constraintType)\n"
|
||||
" {\n"
|
||||
" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n"
|
||||
@@ -790,37 +647,29 @@ static const char* solveConstraintRowsCL= \
|
||||
" 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"
|
||||
" }\n"
|
||||
"\n"
|
||||
" ///finalize the constraint setup\n"
|
||||
" for ( j=0;j<info1;j++)\n"
|
||||
" {\n"
|
||||
" __global b3SolverConstraint* solverConstraint = ¤tConstraintRow[j];\n"
|
||||
"\n"
|
||||
" if (solverConstraint->m_upperLimit>=constraint->m_breakingImpulseThreshold)\n"
|
||||
" {\n"
|
||||
" solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold)\n"
|
||||
" {\n"
|
||||
" solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
"// solverConstraint->m_originalContactPoint = constraint;\n"
|
||||
" \n"
|
||||
" Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld;\n"
|
||||
" {\n"
|
||||
"\n"
|
||||
" //float4 angularFactorA(1,1,1);\n"
|
||||
" float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal;\n"
|
||||
" solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA;\n"
|
||||
@@ -828,11 +677,9 @@ static const char* solveConstraintRowsCL= \
|
||||
" \n"
|
||||
" Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld;\n"
|
||||
" {\n"
|
||||
"\n"
|
||||
" float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal;\n"
|
||||
" solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor();\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" {\n"
|
||||
" //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal\n"
|
||||
" //because it gets multiplied iMJlB\n"
|
||||
@@ -840,7 +687,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA);\n"
|
||||
" float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal?\n"
|
||||
" float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB);\n"
|
||||
"\n"
|
||||
" float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal);\n"
|
||||
" sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal);\n"
|
||||
" sum += dot3F4(iMJlB,solverConstraint->m_contactNormal);\n"
|
||||
@@ -854,17 +700,13 @@ static const char* solveConstraintRowsCL= \
|
||||
" solverConstraint->m_jacDiagABInv = 0.f;\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" ///fix rhs\n"
|
||||
" ///todo: add force/torque accelerators\n"
|
||||
" {\n"
|
||||
" float rel_vel;\n"
|
||||
" float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel);\n"
|
||||
" float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel);\n"
|
||||
"\n"
|
||||
" rel_vel = vel1Dotn+vel2Dotn;\n"
|
||||
"\n"
|
||||
" float restitution = 0.f;\n"
|
||||
" float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2\n"
|
||||
" float velocityError = restitution - rel_vel * info2.m_damping;\n"
|
||||
@@ -872,7 +714,6 @@ static const char* solveConstraintRowsCL= \
|
||||
" float velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv;\n"
|
||||
" solverConstraint->m_rhs = penetrationImpulse+velocityImpulse;\n"
|
||||
" solverConstraint->m_appliedImpulse = 0.f;\n"
|
||||
"\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
|
||||
Reference in New Issue
Block a user