dali-physics/third-party/bullet3/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h

   1 //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
   2 static const char* solveConstraintRowsCL =
   3         "/*\n"
   4         "Copyright (c) 2013 Advanced Micro Devices, Inc.  \n"
   5         "This software is provided 'as-is', without any express or implied warranty.\n"
   6         "In no event will the authors be held liable for any damages arising from the use of this software.\n"
   7         "Permission is granted to anyone to use this software for any purpose, \n"
   8         "including commercial applications, and to alter it and redistribute it freely, \n"
   9         "subject to the following restrictions:\n"
  10         "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"
  11         "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
  12         "3. This notice may not be removed or altered from any source distribution.\n"
  13         "*/\n"
  14         "//Originally written by Erwin Coumans\n"
  15         "#define B3_CONSTRAINT_FLAG_ENABLED 1\n"
  16         "#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n"
  17         "#define B3_GPU_FIXED_CONSTRAINT_TYPE 4\n"
  18         "#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails\n"
  19         "#define B3_INFINITY 1e30f\n"
  20         "#define mymake_float4 (float4)\n"
  21         "__inline float dot3F4(float4 a, float4 b)\n"
  22         "{\n"
  23         "       float4 a1 = mymake_float4(a.xyz,0.f);\n"
  24         "       float4 b1 = mymake_float4(b.xyz,0.f);\n"
  25         "       return dot(a1, b1);\n"
  26         "}\n"
  27         "typedef float4 Quaternion;\n"
  28         "typedef struct\n"
  29         "{\n"
  30         "       float4 m_row[3];\n"
  31         "}Matrix3x3;\n"
  32         "__inline\n"
  33         "float4 mtMul1(Matrix3x3 a, float4 b);\n"
  34         "__inline\n"
  35         "float4 mtMul3(float4 a, Matrix3x3 b);\n"
  36         "__inline\n"
  37         "float4 mtMul1(Matrix3x3 a, float4 b)\n"
  38         "{\n"
  39         "       float4 ans;\n"
  40         "       ans.x = dot3F4( a.m_row[0], b );\n"
  41         "       ans.y = dot3F4( a.m_row[1], b );\n"
  42         "       ans.z = dot3F4( a.m_row[2], b );\n"
  43         "       ans.w = 0.f;\n"
  44         "       return ans;\n"
  45         "}\n"
  46         "__inline\n"
  47         "float4 mtMul3(float4 a, Matrix3x3 b)\n"
  48         "{\n"
  49         "       float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n"
  50         "       float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n"
  51         "       float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n"
  52         "       float4 ans;\n"
  53         "       ans.x = dot3F4( a, colx );\n"
  54         "       ans.y = dot3F4( a, coly );\n"
  55         "       ans.z = dot3F4( a, colz );\n"
  56         "       return ans;\n"
  57         "}\n"
  58         "typedef struct\n"
  59         "{\n"
  60         "       Matrix3x3 m_invInertiaWorld;\n"
  61         "       Matrix3x3 m_initInvInertia;\n"
  62         "} BodyInertia;\n"
  63         "typedef struct\n"
  64         "{\n"
  65         "       Matrix3x3 m_basis;//orientation\n"
  66         "       float4  m_origin;//transform\n"
  67         "}b3Transform;\n"
  68         "typedef struct\n"
  69         "{\n"
  70         "//     b3Transform             m_worldTransformUnused;\n"
  71         "       float4          m_deltaLinearVelocity;\n"
  72         "       float4          m_deltaAngularVelocity;\n"
  73         "       float4          m_angularFactor;\n"
  74         "       float4          m_linearFactor;\n"
  75         "       float4          m_invMass;\n"
  76         "       float4          m_pushVelocity;\n"
  77         "       float4          m_turnVelocity;\n"
  78         "       float4          m_linearVelocity;\n"
  79         "       float4          m_angularVelocity;\n"
  80         "       union \n"
  81         "       {\n"
  82         "               void*   m_originalBody;\n"
  83         "               int             m_originalBodyIndex;\n"
  84         "       };\n"
  85         "       int padding[3];\n"
  86         "} b3GpuSolverBody;\n"
  87         "typedef struct\n"
  88         "{\n"
  89         "       float4 m_pos;\n"
  90         "       Quaternion m_quat;\n"
  91         "       float4 m_linVel;\n"
  92         "       float4 m_angVel;\n"
  93         "       unsigned int m_shapeIdx;\n"
  94         "       float m_invMass;\n"
  95         "       float m_restituitionCoeff;\n"
  96         "       float m_frictionCoeff;\n"
  97         "} b3RigidBodyCL;\n"
  98         "typedef struct\n"
  99         "{\n"
 100         "       float4          m_relpos1CrossNormal;\n"
 101         "       float4          m_contactNormal;\n"
 102         "       float4          m_relpos2CrossNormal;\n"
 103         "       //float4                m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal\n"
 104         "       float4          m_angularComponentA;\n"
 105         "       float4          m_angularComponentB;\n"
 106         "       \n"
 107         "       float   m_appliedPushImpulse;\n"
 108         "       float   m_appliedImpulse;\n"
 109         "       int     m_padding1;\n"
 110         "       int     m_padding2;\n"
 111         "       float   m_friction;\n"
 112         "       float   m_jacDiagABInv;\n"
 113         "       float           m_rhs;\n"
 114         "       float           m_cfm;\n"
 115         "       \n"
 116         "    float              m_lowerLimit;\n"
 117         "       float           m_upperLimit;\n"
 118         "       float           m_rhsPenetration;\n"
 119         "       int                     m_originalConstraint;\n"
 120         "       int     m_overrideNumSolverIterations;\n"
 121         "    int                        m_frictionIndex;\n"
 122         "       int m_solverBodyIdA;\n"
 123         "       int m_solverBodyIdB;\n"
 124         "} b3SolverConstraint;\n"
 125         "typedef struct \n"
 126         "{\n"
 127         "       int m_bodyAPtrAndSignBit;\n"
 128         "       int m_bodyBPtrAndSignBit;\n"
 129         "       int m_originalConstraintIndex;\n"
 130         "       int m_batchId;\n"
 131         "} b3BatchConstraint;\n"
 132         "typedef struct \n"
 133         "{\n"
 134         "       int                             m_constraintType;\n"
 135         "       int                             m_rbA;\n"
 136         "       int                             m_rbB;\n"
 137         "       float                   m_breakingImpulseThreshold;\n"
 138         "       float4 m_pivotInA;\n"
 139         "       float4 m_pivotInB;\n"
 140         "       Quaternion m_relTargetAB;\n"
 141         "       int     m_flags;\n"
 142         "       int m_padding[3];\n"
 143         "} b3GpuGenericConstraint;\n"
 144         "/*b3Transform  getWorldTransform(b3RigidBodyCL* rb)\n"
 145         "{\n"
 146         "       b3Transform newTrans;\n"
 147         "       newTrans.setOrigin(rb->m_pos);\n"
 148         "       newTrans.setRotation(rb->m_quat);\n"
 149         "       return newTrans;\n"
 150         "}*/\n"
 151         "__inline\n"
 152         "float4 cross3(float4 a, float4 b)\n"
 153         "{\n"
 154         "       return cross(a,b);\n"
 155         "}\n"
 156         "__inline\n"
 157         "float4 fastNormalize4(float4 v)\n"
 158         "{\n"
 159         "       v = mymake_float4(v.xyz,0.f);\n"
 160         "       return fast_normalize(v);\n"
 161         "}\n"
 162         "__inline\n"
 163         "Quaternion qtMul(Quaternion a, Quaternion b);\n"
 164         "__inline\n"
 165         "Quaternion qtNormalize(Quaternion in);\n"
 166         "__inline\n"
 167         "float4 qtRotate(Quaternion q, float4 vec);\n"
 168         "__inline\n"
 169         "Quaternion qtInvert(Quaternion q);\n"
 170         "__inline\n"
 171         "Quaternion qtMul(Quaternion a, Quaternion b)\n"
 172         "{\n"
 173         "       Quaternion ans;\n"
 174         "       ans = cross3( a, b );\n"
 175         "       ans += a.w*b+b.w*a;\n"
 176         "//     ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n"
 177         "       ans.w = a.w*b.w - dot3F4(a, b);\n"
 178         "       return ans;\n"
 179         "}\n"
 180         "__inline\n"
 181         "Quaternion qtNormalize(Quaternion in)\n"
 182         "{\n"
 183         "       return fastNormalize4(in);\n"
 184         "//     in /= length( in );\n"
 185         "//     return in;\n"
 186         "}\n"
 187         "__inline\n"
 188         "float4 qtRotate(Quaternion q, float4 vec)\n"
 189         "{\n"
 190         "       Quaternion qInv = qtInvert( q );\n"
 191         "       float4 vcpy = vec;\n"
 192         "       vcpy.w = 0.f;\n"
 193         "       float4 out = qtMul(qtMul(q,vcpy),qInv);\n"
 194         "       return out;\n"
 195         "}\n"
 196         "__inline\n"
 197         "Quaternion qtInvert(Quaternion q)\n"
 198         "{\n"
 199         "       return (Quaternion)(-q.xyz, q.w);\n"
 200         "}\n"
 201         "__inline void internalApplyImpulse(__global b3GpuSolverBody* body,  float4 linearComponent, float4 angularComponent,float impulseMagnitude)\n"
 202         "{\n"
 203         "       body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor;\n"
 204         "       body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor);\n"
 205         "}\n"
 206         "void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c)\n"
 207         "{\n"
 208         "       float deltaImpulse = c->m_rhs-c->m_appliedImpulse*c->m_cfm;\n"
 209         "       float deltaVel1Dotn     =       dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity)         + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity);\n"
 210         "       float deltaVel2Dotn     =       -dot3F4(c->m_contactNormal,body2->m_deltaLinearVelocity) + dot3F4(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity);\n"
 211         "       deltaImpulse    -=      deltaVel1Dotn*c->m_jacDiagABInv;\n"
 212         "       deltaImpulse    -=      deltaVel2Dotn*c->m_jacDiagABInv;\n"
 213         "       float sum = c->m_appliedImpulse + deltaImpulse;\n"
 214         "       if (sum < c->m_lowerLimit)\n"
 215         "       {\n"
 216         "               deltaImpulse = c->m_lowerLimit-c->m_appliedImpulse;\n"
 217         "               c->m_appliedImpulse = c->m_lowerLimit;\n"
 218         "       }\n"
 219         "       else if (sum > c->m_upperLimit) \n"
 220         "       {\n"
 221         "               deltaImpulse = c->m_upperLimit-c->m_appliedImpulse;\n"
 222         "               c->m_appliedImpulse = c->m_upperLimit;\n"
 223         "       }\n"
 224         "       else\n"
 225         "       {\n"
 226         "               c->m_appliedImpulse = sum;\n"
 227         "       }\n"
 228         "       internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse);\n"
 229         "       internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse);\n"
 230         "}\n"
 231         "__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n"
 232         "                                         __global b3BatchConstraint* batchConstraints,\n"
 233         "                                               __global b3SolverConstraint* rows,\n"
 234         "                                               __global unsigned int* numConstraintRowsInfo1, \n"
 235         "                                               __global unsigned int* rowOffsets,\n"
 236         "                                               __global b3GpuGenericConstraint* constraints,\n"
 237         "                                               int batchOffset,\n"
 238         "                                               int numConstraintsInBatch\n"
 239         "                      )\n"
 240         "{\n"
 241         "       int b = get_global_id(0);\n"
 242         "       if (b>=numConstraintsInBatch)\n"
 243         "               return;\n"
 244         "       __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];\n"
 245         "       int originalConstraintIndex = c->m_originalConstraintIndex;\n"
 246         "       if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED)\n"
 247         "       {\n"
 248         "               int numConstraintRows = numConstraintRowsInfo1[originalConstraintIndex];\n"
 249         "               int rowOffset = rowOffsets[originalConstraintIndex];\n"
 250         "               for (int jj=0;jj<numConstraintRows;jj++)\n"
 251         "               {\n"
 252         "                       __global b3SolverConstraint* constraint = &rows[rowOffset+jj];\n"
 253         "                       resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);\n"
 254         "               }\n"
 255         "       }\n"
 256         "};\n"
 257         "__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies)\n"
 258         "{\n"
 259         "       int i = get_global_id(0);\n"
 260         "       if (i>=numBodies)\n"
 261         "               return;\n"
 262         "       __global b3GpuSolverBody* solverBody = &solverBodies[i];\n"
 263         "       __global b3RigidBodyCL* bodyCL = &bodiesCL[i];\n"
 264         "       solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f);\n"
 265         "       solverBody->m_deltaAngularVelocity  = (float4)(0.f,0.f,0.f,0.f);\n"
 266         "       solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n"
 267         "       solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n"
 268         "       solverBody->m_invMass = (float4)(bodyCL->m_invMass,bodyCL->m_invMass,bodyCL->m_invMass,0.f);\n"
 269         "       solverBody->m_originalBodyIndex = i;\n"
 270         "       solverBody->m_angularFactor = (float4)(1,1,1,0);\n"
 271         "       solverBody->m_linearFactor = (float4) (1,1,1,0);\n"
 272         "       solverBody->m_linearVelocity = bodyCL->m_linVel;\n"
 273         "       solverBody->m_angularVelocity = bodyCL->m_angVel;\n"
 274         "}\n"
 275         "__kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints)\n"
 276         "{\n"
 277         "       int cid = get_global_id(0);\n"
 278         "       if (cid>=numConstraints)\n"
 279         "               return;\n"
 280         "       int numRows = numConstraintRows[cid];\n"
 281         "       if (numRows)\n"
 282         "       {\n"
 283         "               for (int i=0;i<numRows;i++)\n"
 284         "               {\n"
 285         "                       int rowIndex = rowOffsets[cid]+i;\n"
 286         "                       float breakingThreshold = constraints[cid].m_breakingImpulseThreshold;\n"
 287         "                       if (fabs(rows[rowIndex].m_appliedImpulse) >= breakingThreshold)\n"
 288         "                       {\n"
 289         "                               constraints[cid].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;\n"
 290         "                       }\n"
 291         "               }\n"
 292         "       }\n"
 293         "}\n"
 294         "__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints)\n"
 295         "{\n"
 296         "       int i = get_global_id(0);\n"
 297         "       if (i>=numConstraints)\n"
 298         "               return;\n"
 299         "       __global b3GpuGenericConstraint* constraint = &constraints[i];\n"
 300         "       switch (constraint->m_constraintType)\n"
 301         "       {\n"
 302         "               case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n"
 303         "               {\n"
 304         "                       infos[i] = 3;\n"
 305         "                       break;\n"
 306         "               }\n"
 307         "               case B3_GPU_FIXED_CONSTRAINT_TYPE:\n"
 308         "               {\n"
 309         "                       infos[i] = 6;\n"
 310         "                       break;\n"
 311         "               }\n"
 312         "               default:\n"
 313         "               {\n"
 314         "               }\n"
 315         "       }\n"
 316         "}\n"
 317         "__kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, \n"
 318         "                                                                               __global b3BatchConstraint* batchConstraints, \n"
 319         "                                                                               __global b3GpuGenericConstraint* constraints,\n"
 320         "                                                                               __global b3RigidBodyCL* bodies,\n"
 321         "                                                                               int numConstraints)\n"
 322         "{\n"
 323         "       int i = get_global_id(0);\n"
 324         "       if (i>=numConstraints)\n"
 325         "               return;\n"
 326         "       int rbA = constraints[i].m_rbA;\n"
 327         "       int rbB = constraints[i].m_rbB;\n"
 328         "       batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass != 0.f ? rbA : -rbA;\n"
 329         "       batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass != 0.f ? rbB : -rbB;\n"
 330         "       batchConstraints[i].m_batchId = -1;\n"
 331         "       batchConstraints[i].m_originalConstraintIndex = i;\n"
 332         "}\n"
 333         "typedef struct\n"
 334         "{\n"
 335         "       // integrator parameters: frames per second (1/stepsize), default error\n"
 336         "       // reduction parameter (0..1).\n"
 337         "       float fps,erp;\n"
 338         "       // for the first and second body, pointers to two (linear and angular)\n"
 339         "       // n*3 jacobian sub matrices, stored by rows. these matrices will have\n"
 340         "       // been initialized to 0 on entry. if the second body is zero then the\n"
 341         "       // J2xx pointers may be 0.\n"
 342         "       union \n"
 343         "       {\n"
 344         "               __global float4* m_J1linearAxisFloat4;\n"
 345         "               __global float* m_J1linearAxis;\n"
 346         "       };\n"
 347         "       union\n"
 348         "       {\n"
 349         "               __global float4* m_J1angularAxisFloat4;\n"
 350         "               __global float* m_J1angularAxis;\n"
 351         "       };\n"
 352         "       union\n"
 353         "       {\n"
 354         "       __global float4* m_J2linearAxisFloat4;\n"
 355         "       __global float* m_J2linearAxis;\n"
 356         "       };\n"
 357         "       union\n"
 358         "       {\n"
 359         "               __global float4* m_J2angularAxisFloat4;\n"
 360         "               __global float* m_J2angularAxis;\n"
 361         "       };\n"
 362         "       // elements to jump from one row to the next in J's\n"
 363         "       int rowskip;\n"
 364         "       // right hand sides of the equation J*v = c + cfm * lambda. cfm is the\n"
 365         "       // \"constraint force mixing\" vector. c is set to zero on entry, cfm is\n"
 366         "       // set to a constant value (typically very small or zero) value on entry.\n"
 367         "       __global float* m_constraintError;\n"
 368         "       __global float* cfm;\n"
 369         "       // lo and hi limits for variables (set to -/+ infinity on entry).\n"
 370         "       __global float* m_lowerLimit;\n"
 371         "       __global float* m_upperLimit;\n"
 372         "       // findex vector for variables. see the LCP solver interface for a\n"
 373         "       // description of what this does. this is set to -1 on entry.\n"
 374         "       // note that the returned indexes are relative to the first index of\n"
 375         "       // the constraint.\n"
 376         "       __global int *findex;\n"
 377         "       // number of solver iterations\n"
 378         "       int m_numIterations;\n"
 379         "       //damping of the velocity\n"
 380         "       float   m_damping;\n"
 381         "} b3GpuConstraintInfo2;\n"
 382         "void   getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2)\n"
 383         "{\n"
 384         "       *v0 = (float4)(0.               ,-vecIn.z               ,vecIn.y,0.f);\n"
 385         "       *v1 = (float4)(vecIn.z  ,0.                     ,-vecIn.x,0.f);\n"
 386         "       *v2 = (float4)(-vecIn.y ,vecIn.x        ,0.f,0.f);\n"
 387         "}\n"
 388         "void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies)\n"
 389         "{\n"
 390         "       float4 posA = bodies[constraint->m_rbA].m_pos;\n"
 391         "       Quaternion rotA = bodies[constraint->m_rbA].m_quat;\n"
 392         "       float4 posB = bodies[constraint->m_rbB].m_pos;\n"
 393         "       Quaternion rotB = bodies[constraint->m_rbB].m_quat;\n"
 394         "               // anchor points in global coordinates with respect to body PORs.\n"
 395         "   \n"
 396         "    // set jacobian\n"
 397         "    info->m_J1linearAxis[0] = 1;\n"
 398         "       info->m_J1linearAxis[info->rowskip+1] = 1;\n"
 399         "       info->m_J1linearAxis[2*info->rowskip+2] = 1;\n"
 400         "       float4 a1 = qtRotate(rotA,constraint->m_pivotInA);\n"
 401         "       {\n"
 402         "               __global float4* angular0 = (__global float4*)(info->m_J1angularAxis);\n"
 403         "               __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip);\n"
 404         "               __global float4* angular2 = (__global float4*)(info->m_J1angularAxis+2*info->rowskip);\n"
 405         "               float4 a1neg = -a1;\n"
 406         "               getSkewSymmetricMatrix(a1neg,angular0,angular1,angular2);\n"
 407         "       }\n"
 408         "       if (info->m_J2linearAxis)\n"
 409         "       {\n"
 410         "               info->m_J2linearAxis[0] = -1;\n"
 411         "               info->m_J2linearAxis[info->rowskip+1] = -1;\n"
 412         "               info->m_J2linearAxis[2*info->rowskip+2] = -1;\n"
 413         "       }\n"
 414         "       \n"
 415         "       float4 a2 = qtRotate(rotB,constraint->m_pivotInB);\n"
 416         "   \n"
 417         "       {\n"
 418         "       //      float4 a2n = -a2;\n"
 419         "               __global float4* angular0 = (__global float4*)(info->m_J2angularAxis);\n"
 420         "               __global float4* angular1 = (__global float4*)(info->m_J2angularAxis+info->rowskip);\n"
 421         "               __global float4* angular2 = (__global float4*)(info->m_J2angularAxis+2*info->rowskip);\n"
 422         "               getSkewSymmetricMatrix(a2,angular0,angular1,angular2);\n"
 423         "       }\n"
 424         "    \n"
 425         "    // set right hand side\n"
 426         "//     float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;\n"
 427         "       float currERP = info->erp;\n"
 428         "       float k = info->fps * currERP;\n"
 429         "    int j;\n"
 430         "       float4 result = a2 + posB - a1 - posA;\n"
 431         "       float* resultPtr = &result;\n"
 432         "       for (j=0; j<3; j++)\n"
 433         "    {\n"
 434         "        info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]);\n"
 435         "    }\n"
 436         "}\n"
 437         "Quaternion nearest( Quaternion first, Quaternion qd)\n"
 438         "{\n"
 439         "       Quaternion diff,sum;\n"
 440         "       diff = first- qd;\n"
 441         "       sum = first + qd;\n"
 442         "       \n"
 443         "       if( dot(diff,diff) < dot(sum,sum) )\n"
 444         "               return qd;\n"
 445         "       return (-qd);\n"
 446         "}\n"
 447         "float b3Acos(float x) \n"
 448         "{ \n"
 449         "       if (x<-1)       \n"
 450         "               x=-1; \n"
 451         "       if (x>1)        \n"
 452         "               x=1;\n"
 453         "       return acos(x); \n"
 454         "}\n"
 455         "float getAngle(Quaternion orn)\n"
 456         "{\n"
 457         "       if (orn.w>=1.f)\n"
 458         "               orn.w=1.f;\n"
 459         "       float s = 2.f * b3Acos(orn.w);\n"
 460         "       return s;\n"
 461         "}\n"
 462         "void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle)\n"
 463         "{\n"
 464         "       Quaternion orn1 = nearest(orn0,orn1a);\n"
 465         "       \n"
 466         "       Quaternion dorn = qtMul(orn1,qtInvert(orn0));\n"
 467         "       *angle = getAngle(dorn);\n"
 468         "       *axis = (float4)(dorn.x,dorn.y,dorn.z,0.f);\n"
 469         "       \n"
 470         "       //check for axis length\n"
 471         "       float len = dot3F4(*axis,*axis);\n"
 472         "       if (len < FLT_EPSILON*FLT_EPSILON)\n"
 473         "               *axis = (float4)(1,0,0,0);\n"
 474         "       else\n"
 475         "               *axis /= sqrt(len);\n"
 476         "}\n"
 477         "void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)\n"
 478         "{\n"
 479         "       Quaternion worldOrnA = bodies[constraint->m_rbA].m_quat;\n"
 480         "       Quaternion worldOrnB = bodies[constraint->m_rbB].m_quat;\n"
 481         "       int s = info->rowskip;\n"
 482         "       int start_index = start_row * s;\n"
 483         "       // 3 rows to make body rotations equal\n"
 484         "       info->m_J1angularAxis[start_index] = 1;\n"
 485         "       info->m_J1angularAxis[start_index + s + 1] = 1;\n"
 486         "       info->m_J1angularAxis[start_index + s*2+2] = 1;\n"
 487         "       if ( info->m_J2angularAxis)\n"
 488         "       {\n"
 489         "               info->m_J2angularAxis[start_index] = -1;\n"
 490         "               info->m_J2angularAxis[start_index + s+1] = -1;\n"
 491         "               info->m_J2angularAxis[start_index + s*2+2] = -1;\n"
 492         "       }\n"
 493         "       \n"
 494         "       float currERP = info->erp;\n"
 495         "       float k = info->fps * currERP;\n"
 496         "       float4 diff;\n"
 497         "       float angle;\n"
 498         "       float4 qrelCur = qtMul(worldOrnA,qtInvert(worldOrnB));\n"
 499         "       \n"
 500         "       calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,&diff,&angle);\n"
 501         "       diff*=-angle;\n"
 502         "               \n"
 503         "       float* resultPtr = &diff;\n"
 504         "       \n"
 505         "       for (int j=0; j<3; j++)\n"
 506         "    {\n"
 507         "        info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j];\n"
 508         "    }\n"
 509         "       \n"
 510         "}\n"
 511         "__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies)\n"
 512         "{\n"
 513         "       int i = get_global_id(0);\n"
 514         "       if (i>=numBodies)\n"
 515         "               return;\n"
 516         "       if (bodies[i].m_invMass)\n"
 517         "       {\n"
 518         "//             if (length(solverBodies[i].m_deltaLinearVelocity)<MOTIONCLAMP)\n"
 519         "               {\n"
 520         "                       bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;\n"
 521         "               }\n"
 522         "//             if (length(solverBodies[i].m_deltaAngularVelocity)<MOTIONCLAMP)\n"
 523         "               {\n"
 524         "                       bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;\n"
 525         "               } \n"
 526         "       }\n"
 527         "}\n"
 528         "__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, \n"
 529         "                                                       __global unsigned int* infos, \n"
 530         "                                                       __global unsigned int* constraintRowOffsets, \n"
 531         "                                                       __global b3GpuGenericConstraint* constraints, \n"
 532         "                                                       __global b3BatchConstraint* batchConstraints, \n"
 533         "                                                       __global b3RigidBodyCL* bodies,\n"
 534         "                                                       __global BodyInertia* inertias,\n"
 535         "                                                       __global b3GpuSolverBody* solverBodies,\n"
 536         "                                                       float timeStep,\n"
 537         "                                                       float globalErp,\n"
 538         "                                                       float globalCfm,\n"
 539         "                                                       float globalDamping,\n"
 540         "                                                       int globalNumIterations,\n"
 541         "                                                       int numConstraints)\n"
 542         "{\n"
 543         "       int i = get_global_id(0);\n"
 544         "       if (i>=numConstraints)\n"
 545         "               return;\n"
 546         "               \n"
 547         "       //for now, always initialize the batch info\n"
 548         "       int info1 = infos[i];\n"
 549         "                       \n"
 550         "       __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]];\n"
 551         "       __global b3GpuGenericConstraint* constraint = &constraints[i];\n"
 552         "       __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n"
 553         "       __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n"
 554         "       int solverBodyIdA = constraint->m_rbA;\n"
 555         "       int solverBodyIdB = constraint->m_rbB;\n"
 556         "       __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n"
 557         "       __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n"
 558         "       if (rbA->m_invMass)\n"
 559         "       {\n"
 560         "               batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;\n"
 561         "       } else\n"
 562         "       {\n"
 563         "//                     if (!solverBodyIdA)\n"
 564         "//                             m_staticIdx = 0;\n"
 565         "               batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;\n"
 566         "       }\n"
 567         "       if (rbB->m_invMass)\n"
 568         "       {\n"
 569         "               batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;\n"
 570         "       } else\n"
 571         "       {\n"
 572         "//                     if (!solverBodyIdB)\n"
 573         "//                             m_staticIdx = 0;\n"
 574         "               batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;\n"
 575         "       }\n"
 576         "       if (info1)\n"
 577         "       {\n"
 578         "               int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n"
 579         "//             if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n"
 580         "       //              m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n"
 581         "               int j;\n"
 582         "               for ( j=0;j<info1;j++)\n"
 583         "               {\n"
 584         "//                     memset(&currentConstraintRow[j],0,sizeof(b3SolverConstraint));\n"
 585         "                       currentConstraintRow[j].m_angularComponentA = (float4)(0,0,0,0);\n"
 586         "                       currentConstraintRow[j].m_angularComponentB = (float4)(0,0,0,0);\n"
 587         "                       currentConstraintRow[j].m_appliedImpulse = 0.f;\n"
 588         "                       currentConstraintRow[j].m_appliedPushImpulse = 0.f;\n"
 589         "                       currentConstraintRow[j].m_cfm = 0.f;\n"
 590         "                       currentConstraintRow[j].m_contactNormal = (float4)(0,0,0,0);\n"
 591         "                       currentConstraintRow[j].m_friction = 0.f;\n"
 592         "                       currentConstraintRow[j].m_frictionIndex = 0;\n"
 593         "                       currentConstraintRow[j].m_jacDiagABInv = 0.f;\n"
 594         "                       currentConstraintRow[j].m_lowerLimit = 0.f;\n"
 595         "                       currentConstraintRow[j].m_upperLimit = 0.f;\n"
 596         "                       currentConstraintRow[j].m_originalConstraint = i;\n"
 597         "                       currentConstraintRow[j].m_overrideNumSolverIterations = 0;\n"
 598         "                       currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0);\n"
 599         "                       currentConstraintRow[j].m_relpos2CrossNormal = (float4)(0,0,0,0);\n"
 600         "                       currentConstraintRow[j].m_rhs = 0.f;\n"
 601         "                       currentConstraintRow[j].m_rhsPenetration = 0.f;\n"
 602         "                       currentConstraintRow[j].m_solverBodyIdA = 0;\n"
 603         "                       currentConstraintRow[j].m_solverBodyIdB = 0;\n"
 604         "                                                       \n"
 605         "                       currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;\n"
 606         "                       currentConstraintRow[j].m_upperLimit = B3_INFINITY;\n"
 607         "                       currentConstraintRow[j].m_appliedImpulse = 0.f;\n"
 608         "                       currentConstraintRow[j].m_appliedPushImpulse = 0.f;\n"
 609         "                       currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;\n"
 610         "                       currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;\n"
 611         "                       currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;            \n"
 612         "               }\n"
 613         "               bodyAPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n"
 614         "               bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n"
 615         "               bodyAPtr->m_pushVelocity = (float4)(0,0,0,0);\n"
 616         "               bodyAPtr->m_turnVelocity = (float4)(0,0,0,0);\n"
 617         "               bodyBPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n"
 618         "               bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n"
 619         "               bodyBPtr->m_pushVelocity = (float4)(0,0,0,0);\n"
 620         "               bodyBPtr->m_turnVelocity  = (float4)(0,0,0,0);\n"
 621         "               int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n"
 622         "               \n"
 623         "               b3GpuConstraintInfo2 info2;\n"
 624         "               info2.fps = 1.f/timeStep;\n"
 625         "               info2.erp = globalErp;\n"
 626         "               info2.m_J1linearAxisFloat4 = &currentConstraintRow->m_contactNormal;\n"
 627         "               info2.m_J1angularAxisFloat4 = &currentConstraintRow->m_relpos1CrossNormal;\n"
 628         "               info2.m_J2linearAxisFloat4 = 0;\n"
 629         "               info2.m_J2angularAxisFloat4 = &currentConstraintRow->m_relpos2CrossNormal;\n"
 630         "               info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n"
 631         "               ///the size of b3SolverConstraint needs be a multiple of float\n"
 632         "//             b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint));\n"
 633         "               info2.m_constraintError = &currentConstraintRow->m_rhs;\n"
 634         "               currentConstraintRow->m_cfm = globalCfm;\n"
 635         "               info2.m_damping = globalDamping;\n"
 636         "               info2.cfm = &currentConstraintRow->m_cfm;\n"
 637         "               info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;\n"
 638         "               info2.m_upperLimit = &currentConstraintRow->m_upperLimit;\n"
 639         "               info2.m_numIterations = globalNumIterations;\n"
 640         "               switch (constraint->m_constraintType)\n"
 641         "               {\n"
 642         "                       case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n"
 643         "                       {\n"
 644         "                               getInfo2Point2Point(constraint,&info2,bodies);\n"
 645         "                               break;\n"
 646         "                       }\n"
 647         "                       case B3_GPU_FIXED_CONSTRAINT_TYPE:\n"
 648         "                       {\n"
 649         "                               getInfo2Point2Point(constraint,&info2,bodies);\n"
 650         "                               getInfo2FixedOrientation(constraint,&info2,bodies,3);\n"
 651         "                               break;\n"
 652         "                       }\n"
 653         "                       default:\n"
 654         "                       {\n"
 655         "                       }\n"
 656         "               }\n"
 657         "               ///finalize the constraint setup\n"
 658         "               for ( j=0;j<info1;j++)\n"
 659         "               {\n"
 660         "                       __global b3SolverConstraint* solverConstraint = &currentConstraintRow[j];\n"
 661         "                       if (solverConstraint->m_upperLimit>=constraint->m_breakingImpulseThreshold)\n"
 662         "                       {\n"
 663         "                               solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold;\n"
 664         "                       }\n"
 665         "                       if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold)\n"
 666         "                       {\n"
 667         "                               solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold;\n"
 668         "                       }\n"
 669         "//                                             solverConstraint->m_originalContactPoint = constraint;\n"
 670         "                                                       \n"
 671         "                       Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld;\n"
 672         "                       {\n"
 673         "                               //float4 angularFactorA(1,1,1);\n"
 674         "                               float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal;\n"
 675         "                               solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA;\n"
 676         "                       }\n"
 677         "                                               \n"
 678         "                       Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld;\n"
 679         "                       {\n"
 680         "                               float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal;\n"
 681         "                               solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor();\n"
 682         "                       }\n"
 683         "                       {\n"
 684         "                               //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal\n"
 685         "                               //because it gets multiplied iMJlB\n"
 686         "                               float4 iMJlA = solverConstraint->m_contactNormal*rbA->m_invMass;\n"
 687         "                               float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA);\n"
 688         "                               float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal?\n"
 689         "                               float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB);\n"
 690         "                               float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal);\n"
 691         "                               sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal);\n"
 692         "                               sum += dot3F4(iMJlB,solverConstraint->m_contactNormal);\n"
 693         "                               sum += dot3F4(iMJaB,solverConstraint->m_relpos2CrossNormal);\n"
 694         "                               float fsum = fabs(sum);\n"
 695         "                               if (fsum>FLT_EPSILON)\n"
 696         "                               {\n"
 697         "                                       solverConstraint->m_jacDiagABInv = 1.f/sum;\n"
 698         "                               } else\n"
 699         "                               {\n"
 700         "                                       solverConstraint->m_jacDiagABInv = 0.f;\n"
 701         "                               }\n"
 702         "                       }\n"
 703         "                       ///fix rhs\n"
 704         "                       ///todo: add force/torque accelerators\n"
 705         "                       {\n"
 706         "                               float rel_vel;\n"
 707         "                               float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel);\n"
 708         "                               float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel);\n"
 709         "                               rel_vel = vel1Dotn+vel2Dotn;\n"
 710         "                               float restitution = 0.f;\n"
 711         "                               float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2\n"
 712         "                               float   velocityError = restitution - rel_vel * info2.m_damping;\n"
 713         "                               float   penetrationImpulse = positionalError*solverConstraint->m_jacDiagABInv;\n"
 714         "                               float   velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv;\n"
 715         "                               solverConstraint->m_rhs = penetrationImpulse+velocityImpulse;\n"
 716         "                               solverConstraint->m_appliedImpulse = 0.f;\n"
 717         "                       }\n"
 718         "               }\n"
 719         "       }\n"
 720         "}\n";