Engine/lib/bullet/src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/MiniCL/MiniCLTaskWrap.cpp

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