aboutsummaryrefslogtreecommitdiff
path: root/tests/bullet/src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/OpenCLC/SolveCollisionsAndUpdateVelocitiesSIMDBatched.cl
blob: 5ab2a62086834e93bcb03b25be4de32003f622e3 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
MSTRINGIFY(

typedef struct 
{
	int firstObject;
	int endObject;
} CollisionObjectIndices;

typedef struct 
{
	float4 shapeTransform[4]; /* column major 4x4 matrix */
	float4 linearVelocity;
	float4 angularVelocity;

	int softBodyIdentifier;
	int collisionShapeType;
	

	// Shape information
	// Compressed from the union
	float radius;
	float halfHeight;
	int upAxis;
		
	float margin;
	float friction;

	int padding0;
	
} CollisionShapeDescription;

/* From btBroadphaseProxy.h */
__constant int CAPSULE_SHAPE_PROXYTYPE = 10;


/* Multiply column-major matrix against vector */
float4 matrixVectorMul( float4 matrix[4], float4 vector )
{
	float4 returnVector;
	float4 row0 = (float4)(matrix[0].x, matrix[1].x, matrix[2].x, matrix[3].x);
	float4 row1 = (float4)(matrix[0].y, matrix[1].y, matrix[2].y, matrix[3].y);
	float4 row2 = (float4)(matrix[0].z, matrix[1].z, matrix[2].z, matrix[3].z);
	float4 row3 = (float4)(matrix[0].w, matrix[1].w, matrix[2].w, matrix[3].w);
	returnVector.x = dot(row0, vector);
	returnVector.y = dot(row1, vector);
	returnVector.z = dot(row2, vector);
	returnVector.w = dot(row3, vector);
	return returnVector;
}

__kernel void 
SolveCollisionsAndUpdateVelocitiesKernel( 
	const int numNodes,
	const float isolverdt,
	__global int *g_vertexClothIdentifier,
	__global float4 *g_vertexPreviousPositions,
	__global float * g_perClothFriction,
	__global float * g_clothDampingFactor,
	__global CollisionObjectIndices * g_perClothCollisionObjectIndices,
	__global CollisionShapeDescription * g_collisionObjectDetails,
	__global float4 * g_vertexForces,
	__global float4 *g_vertexVelocities,
	__global float4 *g_vertexPositions,
	__local CollisionShapeDescription *localCollisionShapes)
{
	int nodeID = get_global_id(0);
	float3 forceOnVertex = (float3)(0.f, 0.f, 0.f);

	int clothIdentifier = g_vertexClothIdentifier[nodeID];
	
	// Abort if this is not a valid cloth
	if( clothIdentifier < 0 )
		return;

	float4 position = (float4)(g_vertexPositions[nodeID].xyz, 1.f);
	float4 previousPosition = (float4)(g_vertexPreviousPositions[nodeID].xyz, 1.f);
	float3 velocity;
	float clothFriction = g_perClothFriction[clothIdentifier];
	float dampingFactor = g_clothDampingFactor[clothIdentifier];
	float velocityCoefficient = (1.f - dampingFactor);		
	CollisionObjectIndices collisionObjectIndices = g_perClothCollisionObjectIndices[clothIdentifier];
	
	int numObjects = collisionObjectIndices.endObject - collisionObjectIndices.firstObject;
	if( numObjects > 0 )
	{
		/* We have some possible collisions to deal with */
		
		/* First load all of the collision objects into LDS */
		int numObjects = collisionObjectIndices.endObject - collisionObjectIndices.firstObject;
		if( get_local_id(0) < numObjects )
		{
			localCollisionShapes[get_local_id(0)] = g_collisionObjectDetails[ collisionObjectIndices.firstObject + get_local_id(0) ];
		}
	}

	/* Safe as the vertices are padded so that not more than one soft body is in a group */
	barrier(CLK_LOCAL_MEM_FENCE);

	/* Annoyingly, even though I know the flow control is not varying, the compiler will not let me skip this */
	if( numObjects > 0 )
	{
		velocity = (float3)(0, 0, 0);
		
		
		// We have some possible collisions to deal with
		for( int collision = 0; collision < numObjects; ++collision )
		{
			CollisionShapeDescription shapeDescription = localCollisionShapes[collision];
			float colliderFriction = shapeDescription.friction;
		
			if( shapeDescription.collisionShapeType == CAPSULE_SHAPE_PROXYTYPE )
			{
				/* Colliding with a capsule */

				float capsuleHalfHeight = localCollisionShapes[collision].halfHeight;
				float capsuleRadius = localCollisionShapes[collision].radius;
				float capsuleMargin = localCollisionShapes[collision].margin;
				int capsuleupAxis = localCollisionShapes[collision].upAxis;

				float4 worldTransform[4];
				worldTransform[0] = localCollisionShapes[collision].shapeTransform[0];
				worldTransform[1] = localCollisionShapes[collision].shapeTransform[1];
				worldTransform[2] = localCollisionShapes[collision].shapeTransform[2];
				worldTransform[3] = localCollisionShapes[collision].shapeTransform[3];

				// Correctly define capsule centerline vector 
				float4 c1 = (float4)(0.f, 0.f, 0.f, 1.f); 
				float4 c2 = (float4)(0.f, 0.f, 0.f, 1.f);
				c1.x = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 0 );
				c1.y = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 1 );
				c1.z = select( 0.f, -capsuleHalfHeight, capsuleupAxis == 2 );
				c2.x = -c1.x;
				c2.y = -c1.y;
				c2.z = -c1.z;

				float4 worldC1 = matrixVectorMul(worldTransform, c1);
				float4 worldC2 = matrixVectorMul(worldTransform, c2);
				float3 segment = (worldC2 - worldC1).xyz;


				/* compute distance of tangent to vertex along line segment in capsule */
				float distanceAlongSegment = -( dot( (worldC1 - position).xyz, segment ) / dot(segment, segment) );

				float4 closestPoint = (worldC1 + (float4)(segment * distanceAlongSegment, 0.f));
				float distanceFromLine = length(position - closestPoint);
				float distanceFromC1 = length(worldC1 - position);
				float distanceFromC2 = length(worldC2 - position);
					
				/* Final distance from collision, point to push from, direction to push in
				   for impulse force */
				float dist;
				float3 normalVector;
				if( distanceAlongSegment < 0 )
				{
					dist = distanceFromC1;
					normalVector = normalize(position - worldC1).xyz;
				} else if( distanceAlongSegment > 1.f ) {
					dist = distanceFromC2;
					normalVector = normalize(position - worldC2).xyz;	
				} else {
					dist = distanceFromLine;
					normalVector = normalize(position - closestPoint).xyz;
				}
						
				float3 colliderLinearVelocity = localCollisionShapes[collision].linearVelocity.xyz;
				float3 colliderAngularVelocity = localCollisionShapes[collision].angularVelocity.xyz;
				float3 velocityOfSurfacePoint = colliderLinearVelocity + cross(colliderAngularVelocity, position.xyz -  (float3)(worldTransform[0].w, worldTransform[1].w, worldTransform[2].w));

				float minDistance = capsuleRadius + capsuleMargin;
					
				/* In case of no collision, this is the value of velocity */
				velocity = (position - previousPosition).xyz * velocityCoefficient * isolverdt;
					
					
				/* Check for a collision */
				if( dist < minDistance )
				{
					/* Project back to surface along normal */
					position = position + (float4)((minDistance - dist)*normalVector*0.9f, 0.f);
					velocity = (position - previousPosition).xyz * velocityCoefficient * isolverdt;
					float3 relativeVelocity = velocity - velocityOfSurfacePoint;

					float3 p1 = normalize(cross(normalVector, segment));
					float3 p2 = normalize(cross(p1, normalVector));
					/* Full friction is sum of velocities in each direction of plane */
					float3 frictionVector = p1*dot(relativeVelocity, p1) + p2*dot(relativeVelocity, p2);

					/* Real friction is peak friction corrected by friction coefficients */
					frictionVector = frictionVector * (colliderFriction*clothFriction);

					float approachSpeed = dot(relativeVelocity, normalVector);

					if( approachSpeed <= 0.0f )
						forceOnVertex -= frictionVector;
				}
					
			}
		}
	} else {
		/* Update velocity	*/
		float3 difference = position.xyz - previou