src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/MiniCL/MiniCLTaskWrap.cpp

   1 /*\r
   2 Bullet Continuous Collision Detection and Physics Library\r
   3 Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com\r
   4 \r
   5 This software is provided 'as-is', without any express or implied warranty.\r
   6 In no event will the authors be held liable for any damages arising from the use of this software.\r
   7 Permission is granted to anyone to use this software for any purpose, \r
   8 including commercial applications, and to alter it and redistribute it freely, \r
   9 subject to the following restrictions:\r
  10 \r
  11 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.\r
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\r
  13 3. This notice may not be removed or altered from any source distribution.\r
  14 */\r
  15 \r
  16 #include <MiniCL/cl_MiniCL_Defs.h>\r
  17 \r
  18 #define MSTRINGIFY(A) A\r
  19 #include "../OpenCLC10/ApplyForces.cl"\r
  20 #include "../OpenCLC10/Integrate.cl"\r
  21 #include "../OpenCLC10/PrepareLinks.cl"\r
  22 #include "../OpenCLC10/SolvePositions.cl"\r
  23 #include "../OpenCLC10/UpdateNodes.cl"\r
  24 #include "../OpenCLC10/UpdateNormals.cl"\r
  25 #include "../OpenCLC10/UpdatePositions.cl"\r
  26 #include "../OpenCLC10/UpdatePositionsFromVelocities.cl"\r
  27 #include "../OpenCLC10/VSolveLinks.cl"\r
  28 #include "../OpenCLC10/UpdateFixedVertexPositions.cl"\r
  29 //#include "../OpenCLC10/SolveCollisionsAndUpdateVelocities.cl"\r
  30 \r
  31 \r
  32 MINICL_REGISTER(PrepareLinksKernel)\r
  33 MINICL_REGISTER(VSolveLinksKernel)\r
  34 MINICL_REGISTER(UpdatePositionsFromVelocitiesKernel)\r
  35 MINICL_REGISTER(SolvePositionsFromLinksKernel)\r
  36 MINICL_REGISTER(updateVelocitiesFromPositionsWithVelocitiesKernel)\r
  37 MINICL_REGISTER(updateVelocitiesFromPositionsWithoutVelocitiesKernel)\r
  38 MINICL_REGISTER(IntegrateKernel)\r
  39 MINICL_REGISTER(ApplyForcesKernel)\r
  40 MINICL_REGISTER(ResetNormalsAndAreasKernel)\r
  41 MINICL_REGISTER(NormalizeNormalsAndAreasKernel)\r
  42 MINICL_REGISTER(UpdateSoftBodiesKernel)\r
  43 MINICL_REGISTER(UpdateFixedVertexPositions)\r
  44 \r
  45 float mydot3a(float4 a, float4 b)\r
  46 {\r
  47    return a.x*b.x + a.y*b.y + a.z*b.z;\r
  48 }\r
  49 \r
  50 \r
  51 typedef struct \r
  52 {\r
  53         int firstObject;\r
  54         int endObject;\r
  55 } CollisionObjectIndices;\r
  56 \r
  57 typedef struct \r
  58 {\r
  59         float4 shapeTransform[4]; // column major 4x4 matrix\r
  60         float4 linearVelocity;\r
  61         float4 angularVelocity;\r
  62 \r
  63         int softBodyIdentifier;\r
  64         int collisionShapeType;\r
  65         \r
  66 \r
  67         // Shape information\r
  68         // Compressed from the union\r
  69         float radius;\r
  70         float halfHeight;\r
  71         int upAxis;\r
  72                 \r
  73         float margin;\r
  74         float friction;\r
  75 \r
  76         int padding0;\r
  77         \r
  78 } CollisionShapeDescription;\r
  79 \r
  80 // From btBroadphaseProxy.h\r
  81 __constant int CAPSULE_SHAPE_PROXYTYPE = 10;\r
  82 \r
  83 // Multiply column-major matrix against vector\r
  84 float4 matrixVectorMul( float4 matrix[4], float4 vector )\r
  85 {\r
  86         float4 returnVector;\r
  87         float4 row0 = float4(matrix[0].x, matrix[1].x, matrix[2].x, matrix[3].x);\r
  88         float4 row1 = float4(matrix[0].y, matrix[1].y, matrix[2].y, matrix[3].y);\r
  89         float4 row2 = float4(matrix[0].z, matrix[1].z, matrix[2].z, matrix[3].z);\r
  90         float4 row3 = float4(matrix[0].w, matrix[1].w, matrix[2].w, matrix[3].w);\r
  91         returnVector.x = dot(row0, vector);\r
  92         returnVector.y = dot(row1, vector);\r
  93         returnVector.z = dot(row2, vector);\r
  94         returnVector.w = dot(row3, vector);\r
  95         return returnVector;\r
  96 }\r
  97 \r
  98 __kernel void \r
  99 SolveCollisionsAndUpdateVelocitiesKernel( \r
 100         const int numNodes,\r
 101         const float isolverdt,\r
 102         __global int *g_vertexClothIdentifier,\r
 103         __global float4 *g_vertexPreviousPositions,\r
 104         __global float * g_perClothFriction,\r
 105         __global float * g_clothDampingFactor,\r
 106         __global CollisionObjectIndices * g_perClothCollisionObjectIndices,\r
 107         __global CollisionShapeDescription * g_collisionObjectDetails,\r
 108         __global float4 * g_vertexForces,\r
 109         __global float4 *g_vertexVelocities,\r
 110         __global float4 *g_vertexPositions GUID_ARG)\r
 111 {\r
 112         int nodeID = get_global_id(0);\r
 113         float4 forceOnVertex = (float4)(0.f, 0.f, 0.f, 0.f);\r
 114         \r
 115         if( get_global_id(0) < numNodes )\r
 116         {       \r
 117                 int clothIdentifier = g_vertexClothIdentifier[nodeID];\r
 118                 \r
 119                 // Abort if this is not a valid cloth\r
 120                 if( clothIdentifier < 0 )\r
 121                         return;\r
 122 \r
 123 \r
 124                 float4 position (g_vertexPositions[nodeID].xyz, 1.f);\r
 125                 float4 previousPosition (g_vertexPreviousPositions[nodeID].xyz, 1.f);\r
 126                         \r
 127                 float clothFriction = g_perClothFriction[clothIdentifier];\r
 128                 float dampingFactor = g_clothDampingFactor[clothIdentifier];\r
 129                 float velocityCoefficient = (1.f - dampingFactor);              \r
 130                 float4 difference = position - previousPosition;\r
 131                 float4 velocity = difference*velocityCoefficient*isolverdt;\r
 132                 \r
 133                 CollisionObjectIndices collisionObjectIndices = g_perClothCollisionObjectIndices[clothIdentifier];\r
 134         \r
 135                 int numObjects = collisionObjectIndices.endObject - collisionObjectIndices.firstObject;\r
 136                 \r
 137                 if( numObjects > 0 )\r
 138                 {\r
 139                         // We have some possible collisions to deal with\r
 140                         for( int collision = collisionObjectIndices.firstObject; collision < collisionObjectIndices.endObject; ++collision )\r
 141                         {\r
 142                                 CollisionShapeDescription shapeDescription = g_collisionObjectDetails[collision];\r
 143                                 float colliderFriction = shapeDescription.friction;\r
 144 \r
 145                                 if( shapeDescription.collisionShapeType == CAPSULE_SHAPE_PROXYTYPE )\r
 146                                 {\r
 147                                         // Colliding with a capsule\r
 148 \r
 149                                         float capsuleHalfHeight = shapeDescription.halfHeight;\r
 150                                         float capsuleRadius = shapeDescription.radius;\r
 151                                         float capsuleMargin = shapeDescription.margin;\r
 152                                         int capsuleupAxis = shapeDescription.upAxis;\r
 153 \r
 154                                         // Four columns of worldTransform matrix\r
 155                                         float4 worldTransform[4];\r
 156                                         worldTransform[0] = shapeDescription.shapeTransform[0];\r
 157                                         worldTransform[1] = shapeDescription.shapeTransform[1];\r
 158                                         worldTransform[2] = shapeDescription.shapeTransform[2];\r
 159                                         worldTransform[3] = shapeDescription.shapeTransform[3];\r
 160 \r
 161                                         // Correctly define capsule centerline vector \r
 162                                         float4 c1 (0.f, 0.f, 0.f, 1.f); \r
 163                                         float4 c2 (0.f, 0.f, 0.f, 1.f);\r
 164                                         c1.x = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 0 );\r
 165                                         c1.y = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 1 );\r
 166                                         c1.z = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 2 );\r
 167                                         c2.x = -c1.x;\r
 168                                         c2.y = -c1.y;\r
 169                                         c2.z = -c1.z;\r
 170 \r
 171 \r
 172                                         float4 worldC1 = matrixVectorMul(worldTransform, c1);\r
 173                                         float4 worldC2 = matrixVectorMul(worldTransform, c2);\r
 174                                         float4 segment = (worldC2 - worldC1);\r
 175 \r
 176                                         // compute distance of tangent to vertex along line segment in capsule\r
 177                                         float distanceAlongSegment = -( mydot3a( (worldC1 - position), segment ) / mydot3a(segment, segment) );\r
 178 \r
 179                                         float4 closestPoint = (worldC1 + (segment * distanceAlongSegment));\r
 180                                         float distanceFromLine = length(position - closestPoint);\r
 181                                         float distanceFromC1 = length(worldC1 - position);\r
 182                                         float distanceFromC2 = length(worldC2 - position);\r
 183                                         \r
 184                                         // Final distance from collision, point to push from, direction to push in\r
 185                                         // for impulse force\r
 186                                         float dist;\r
 187                                         float4 normalVector;\r
 188                                         if( distanceAlongSegment < 0 )\r
 189                                         {\r
 190                                                 dist = distanceFromC1;\r
 191                                                 normalVector = float4(normalize(position - worldC1).xyz, 0.f);\r
 192                                         } else if( distanceAlongSegment > 1.f ) {\r
 193                                                 dist = distanceFromC2;\r
 194                                                 normalVector = float4(normalize(position - worldC2).xyz, 0.f);  \r
 195                                         } else {\r
 196                                                 dist = distanceFromLine;\r
 197                                                 normalVector = float4(normalize(position - closestPoint).xyz, 0.f);\r
 198                                         }\r
 199                                                 \r
 200                                         float4 colliderLinearVelocity = shapeDescription.linearVelocity;\r
 201                                         float4 colliderAngularVelocity = shapeDescription.angularVelocity;\r
 202                                         float4 velocityOfSurfacePoint = colliderLinearVelocity + cross(colliderAngularVelocity, position - float4(worldTransform[0].w, worldTransform[1].w, worldTransform[2].w, 0.f));\r
 203 \r
 204                                         float minDistance = capsuleRadius + capsuleMargin;\r
 205                                         \r
 206                                         // In case of no collision, this is the value of velocity\r
 207                                         velocity = (position - previousPosition) * velocityCoefficient * isolverdt;\r
 208                                         \r
 209                                         \r
 210                                         // Check for a collision\r
 211                                         if( dist < minDistance )\r
 212                                         {\r
 213                                                 // Project back to surface along normal\r
 214                                                 position = position + float4(normalVector*(minDistance - dist)*0.9f);\r
 215                                                 velocity = (position - previousPosition) * velocityCoefficient * isolverdt;\r
 216                                                 float4 relativeVelocity = velocity - velocityOfSurfacePoint;\r
 217 \r
 218                                                 float4 p1 = normalize(cross(normalVector, segment));\r
 219                                                 float4 p2 = normalize(cross(p1, normalVector));\r
 220                                                 // Full friction is sum of velocities in each direction of plane\r
 221                                                 float4 frictionVector = p1*mydot3a(relativeVelocity, p1) + p2*mydot3a(relativeVelocity, p2);\r
 222 \r
 223                                                 // Real friction is peak friction corrected by friction coefficients\r
 224                                                 frictionVector = frictionVector * (colliderFriction*clothFriction);\r
 225 \r
 226                                                 float approachSpeed = dot(relativeVelocity, normalVector);\r
 227 \r
 228                                                 if( approachSpeed <= 0.0f )\r
 229                                                         forceOnVertex -= frictionVector;\r
 230                                         }\r
 231                                 }\r
 232                         }\r
 233                 }\r
 234 \r
 235                 g_vertexVelocities[nodeID] = float4(velocity.xyz, 0.f); \r
 236 \r
 237                 // Update external force\r
 238                 g_vertexForces[nodeID] = float4(forceOnVertex.xyz, 0.f);\r
 239 \r
 240                 g_vertexPositions[nodeID] = float4(position.xyz, 0.f);\r
 241         }\r
 242 }\r
 243 \r
 244 \r
 245 MINICL_REGISTER(SolveCollisionsAndUpdateVelocitiesKernel);\r
 246 \r
 247 \r
 248 \r
 249 \r