src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/OpenCLC10/SolveCollisionsAndUpdateVelocitiesSIMDBatched.cl

   1 MSTRINGIFY(\r
   2 \r
   3 //#pragma OPENCL EXTENSION cl_amd_printf:enable\n\r
   4 \r
   5 float mydot3a(float4 a, float4 b)\r
   6 {\r
   7    return a.x*b.x + a.y*b.y + a.z*b.z;\r
   8 }\r
   9 \r
  10 float mylength3(float4 a)\r
  11 {\r
  12         a.w = 0;\r
  13         return length(a);\r
  14 }\r
  15 \r
  16 float4 mynormalize3(float4 a)\r
  17 {\r
  18         a.w = 0;\r
  19         return normalize(a);\r
  20 }\r
  21 \r
  22 typedef struct \r
  23 {\r
  24         int firstObject;\r
  25         int endObject;\r
  26 } CollisionObjectIndices;\r
  27 \r
  28 typedef struct \r
  29 {\r
  30         float4 shapeTransform[4]; // column major 4x4 matrix\r
  31         float4 linearVelocity;\r
  32         float4 angularVelocity;\r
  33 \r
  34         int softBodyIdentifier;\r
  35         int collisionShapeType;\r
  36         \r
  37 \r
  38         // Shape information\r
  39         // Compressed from the union\r
  40         float radius;\r
  41         float halfHeight;\r
  42         int upAxis;\r
  43                 \r
  44         float margin;\r
  45         float friction;\r
  46 \r
  47         int padding0;\r
  48         \r
  49 } CollisionShapeDescription;\r
  50 \r
  51 // From btBroadphaseProxy.h\r
  52 __constant int CAPSULE_SHAPE_PROXYTYPE = 10;\r
  53 \r
  54 // Multiply column-major matrix against vector\r
  55 float4 matrixVectorMul( float4 matrix[4], float4 vector )\r
  56 {\r
  57         float4 returnVector;\r
  58         float4 row0 = (float4)(matrix[0].x, matrix[1].x, matrix[2].x, matrix[3].x);\r
  59         float4 row1 = (float4)(matrix[0].y, matrix[1].y, matrix[2].y, matrix[3].y);\r
  60         float4 row2 = (float4)(matrix[0].z, matrix[1].z, matrix[2].z, matrix[3].z);\r
  61         float4 row3 = (float4)(matrix[0].w, matrix[1].w, matrix[2].w, matrix[3].w);\r
  62         returnVector.x = dot(row0, vector);\r
  63         returnVector.y = dot(row1, vector);\r
  64         returnVector.z = dot(row2, vector);\r
  65         returnVector.w = dot(row3, vector);\r
  66         return returnVector;\r
  67 }\r
  68 \r
  69 __kernel void \r
  70 SolveCollisionsAndUpdateVelocitiesKernel( \r
  71         const int numNodes,\r
  72         const float isolverdt,\r
  73         __global int *g_vertexClothIdentifier,\r
  74         __global float4 *g_vertexPreviousPositions,\r
  75         __global float * g_perClothFriction,\r
  76         __global float * g_clothDampingFactor,\r
  77         __global CollisionObjectIndices * g_perClothCollisionObjectIndices,\r
  78         __global CollisionShapeDescription * g_collisionObjectDetails,\r
  79         __global float4 * g_vertexForces,\r
  80         __global float4 *g_vertexVelocities,\r
  81         __global float4 *g_vertexPositions,\r
  82         __local CollisionShapeDescription *localCollisionShapes,\r
  83         __global float * g_vertexInverseMasses)\r
  84 {\r
  85         int nodeID = get_global_id(0);\r
  86         float4 forceOnVertex = (float4)(0.f, 0.f, 0.f, 0.f);\r
  87 \r
  88         int clothIdentifier = g_vertexClothIdentifier[nodeID];\r
  89 \r
  90         // Abort if this is not a valid cloth\r
  91         if( clothIdentifier < 0 )\r
  92                 return;\r
  93         \r
  94 \r
  95         float4 position = (float4)(g_vertexPositions[nodeID].xyz, 0.f);\r
  96         float4 previousPosition = (float4)(g_vertexPreviousPositions[nodeID].xyz, 0.f);\r
  97                         \r
  98         float clothFriction = g_perClothFriction[clothIdentifier];\r
  99         float dampingFactor = g_clothDampingFactor[clothIdentifier];\r
 100         float velocityCoefficient = (1.f - dampingFactor);              \r
 101         float4 difference = position - previousPosition;\r
 102         float4 velocity = difference*velocityCoefficient*isolverdt;                     \r
 103         float inverseMass = g_vertexInverseMasses[nodeID];\r
 104                 \r
 105         CollisionObjectIndices collisionObjectIndices = g_perClothCollisionObjectIndices[clothIdentifier];\r
 106         \r
 107         int numObjects = collisionObjectIndices.endObject - collisionObjectIndices.firstObject;\r
 108                 \r
 109         if( numObjects > 0 )\r
 110         {\r
 111                 // We have some possible collisions to deal with\r
 112                 \r
 113                 // First load all of the collision objects into LDS\r
 114                 int numObjects = collisionObjectIndices.endObject - collisionObjectIndices.firstObject;\r
 115                 if( get_local_id(0) < numObjects )\r
 116                 {\r
 117                         localCollisionShapes[get_local_id(0)] = g_collisionObjectDetails[ collisionObjectIndices.firstObject + get_local_id(0) ];\r
 118                 }\r
 119         }\r
 120 \r
 121         // Safe as the vertices are padded so that not more than one soft body is in a group\r
 122         barrier(CLK_LOCAL_MEM_FENCE);\r
 123 \r
 124         // Annoyingly, even though I know the flow control is not varying, the compiler will not let me skip this\r
 125         if( numObjects > 0 )\r
 126         {\r
 127                 \r
 128                 \r
 129                 // We have some possible collisions to deal with\r
 130                 for( int collision = 0; collision < numObjects; ++collision )\r
 131                 {\r
 132                         CollisionShapeDescription shapeDescription = localCollisionShapes[collision];\r
 133                         float colliderFriction = localCollisionShapes[collision].friction;\r
 134                 \r
 135                         if( localCollisionShapes[collision].collisionShapeType == CAPSULE_SHAPE_PROXYTYPE )\r
 136                         {\r
 137                                 // Colliding with a capsule\r
 138 \r
 139                                 float capsuleHalfHeight = localCollisionShapes[collision].halfHeight;\r
 140                                 float capsuleRadius = localCollisionShapes[collision].radius;\r
 141                                 float capsuleMargin = localCollisionShapes[collision].margin;\r
 142                                 int capsuleupAxis = localCollisionShapes[collision].upAxis;\r
 143 \r
 144                                 if ( capsuleHalfHeight <= 0 )\r
 145                                                 capsuleHalfHeight = 0.0001f;\r
 146                                 float4 worldTransform[4];\r
 147                                 worldTransform[0] = localCollisionShapes[collision].shapeTransform[0];\r
 148                                 worldTransform[1] = localCollisionShapes[collision].shapeTransform[1];\r
 149                                 worldTransform[2] = localCollisionShapes[collision].shapeTransform[2];\r
 150                                 worldTransform[3] = localCollisionShapes[collision].shapeTransform[3];\r
 151 \r
 152                                 // Correctly define capsule centerline vector \r
 153                                 float4 c1 = (float4)(0.f, 0.f, 0.f, 1.f); \r
 154                                 float4 c2 = (float4)(0.f, 0.f, 0.f, 1.f);\r
 155                                 c1.x = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 0 );\r
 156                                 c1.y = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 1 );\r
 157                                 c1.z = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 2 );\r
 158                                 c2.x = -c1.x;\r
 159                                 c2.y = -c1.y;\r
 160                                 c2.z = -c1.z;\r
 161 \r
 162                                 float4 worldC1 = matrixVectorMul(worldTransform, c1);\r
 163                                 float4 worldC2 = matrixVectorMul(worldTransform, c2);\r
 164                                 float4 segment = (float4)((worldC2 - worldC1).xyz, 0.f);\r
 165 \r
 166                                 float4 segmentNormalized = mynormalize3(segment);\r
 167                                 float distanceAlongSegment =mydot3a( (position - worldC1), segmentNormalized );\r
 168 \r
 169                                 float4 closestPointOnSegment = (worldC1 + (float4)(segmentNormalized * distanceAlongSegment));\r
 170                                 float distanceFromLine = mylength3(position - closestPointOnSegment);\r
 171                                 float distanceFromC1 = mylength3(worldC1 - position);\r
 172                                 float distanceFromC2 = mylength3(worldC2 - position);\r
 173         \r
 174                                 // Final distance from collision, point to push from, direction to push in\r
 175                                 // for impulse force\r
 176                                 float dist;\r
 177                                 float4 normalVector;\r
 178 \r
 179                                 if( distanceAlongSegment < 0 )\r
 180                                 {\r
 181                                         dist = distanceFromC1;\r
 182                                         normalVector = (float4)(normalize(position - worldC1).xyz, 0.f);                \r
 183                                 } else if( distanceAlongSegment > length(segment) ) {\r
 184                                         dist = distanceFromC2;\r
 185                                         normalVector = (float4)(normalize(position - worldC2).xyz, 0.f);        \r
 186                                 } else {\r
 187                                         dist = distanceFromLine;\r
 188                                         normalVector = (float4)(normalize(position - closestPointOnSegment).xyz, 0.f);\r
 189                                 }\r
 190                                                 \r
 191                                 float minDistance = capsuleRadius + capsuleMargin;\r
 192                                 float4 closestPointOnSurface = (float4)((position + (minDistance - dist) * normalVector).xyz, 0.f);\r
 193                                                                                 \r
 194                                 float4 colliderLinearVelocity = shapeDescription.linearVelocity;\r
 195                                 float4 colliderAngularVelocity = shapeDescription.angularVelocity;\r
 196                                 float4 velocityOfSurfacePoint = colliderLinearVelocity + cross(colliderAngularVelocity, closestPointOnSurface - (float4)(worldTransform[0].w, worldTransform[1].w, worldTransform[2].w, 0.f));\r
 197                                         \r
 198                                         \r
 199                                 // Check for a collision\r
 200                                 if( dist < minDistance )\r
 201                                 {\r
 202                                         // Project back to surface along normal\r
 203                                         position = closestPointOnSurface;\r
 204                                         velocity = (position - previousPosition) * velocityCoefficient * isolverdt;\r
 205                                         float4 relativeVelocity = velocity - velocityOfSurfacePoint;\r
 206 \r
 207                                         float4 p1 = mynormalize3(cross(normalVector, segment));\r
 208                                         float4 p2 = mynormalize3(cross(p1, normalVector));\r
 209                                         \r
 210                                         float4 tangentialVel = p1*mydot3a(relativeVelocity, p1) + p2*mydot3a(relativeVelocity, p2);\r
 211                                         float frictionCoef = (colliderFriction * clothFriction);\r
 212                                         if (frictionCoef>1.f)\r
 213                                                 frictionCoef = 1.f;\r
 214                                                 \r
 215                                         //only apply friction if objects are not moving apart\r
 216                                         float projVel = mydot3a(relativeVelocity,normalVector);\r
 217                                         if ( projVel >= -0.001f)\r
 218                                         {\r
 219                                                 if ( inverseMass > 0 )\r
 220                                                 {\r
 221                                                         //float4 myforceOnVertex = -tangentialVel * frictionCoef *  isolverdt * (1.0f / inverseMass);\r
 222                                                         position += (-tangentialVel * frictionCoef) / (isolverdt);\r
 223                                                 }\r
 224                                         }                                               \r
 225                                         \r
 226                                         // In case of no collision, this is the value of velocity\r
 227                                         velocity = (position - previousPosition) * velocityCoefficient * isolverdt;\r
 228 \r
 229                                 }\r
 230                         }\r
 231                 }\r
 232         }\r
 233         \r
 234         g_vertexVelocities[nodeID] = (float4)(velocity.xyz, 0.f);       \r
 235 \r
 236         // Update external force\r
 237         g_vertexForces[nodeID] = (float4)(forceOnVertex.xyz, 0.f);\r
 238 \r
 239         g_vertexPositions[nodeID] = (float4)(position.xyz, 0.f);\r
 240 }\r
 241 \r
 242 );\r