PeriDyno 1.0.0
Loading...
Searching...
No Matches
SharedFuncsForRigidBody.cu
Go to the documentation of this file.
1#include "SharedFuncsForRigidBody.h"
2
3namespace dyno
4{
5 __global__ void SF_ApplyTransform(
6 DArrayList<Transform3f> instanceTransform,
7 const DArray<Vec3f> translate,
8 const DArray<Mat3f> rotation,
9 const DArray<Pair<uint, uint>> binding,
10 const DArray<int> bindingtag)
11 {
12 int tId = threadIdx.x + blockIdx.x * blockDim.x;
13 if (tId >= rotation.size())
14 return;
15 if (bindingtag[tId] == 0)
16 return;
17
18 Pair<uint, uint> pair = binding[tId];
19
20 Mat3f rot = rotation[tId];
21
22 Transform3f ti = Transform3f(translate[tId], rot);
23
24 instanceTransform[pair.first][pair.second] = ti;
25 }
26
27 void ApplyTransform(
28 DArrayList<Transform3f>& instanceTransform,
29 const DArray<Vec3f>& translate,
30 const DArray<Mat3f>& rotation,
31 const DArray<Pair<uint, uint>>& binding,
32 const DArray<int>& bindingtag)
33 {
34 cuExecute(rotation.size(),
35 SF_ApplyTransform,
36 instanceTransform,
37 translate,
38 rotation,
39 binding,
40 bindingtag);
41
42 }
43
44 /**
45 * Update Velocity Function
46 *
47 * @param velocity velocity of rigids
48 * @param angular_velocity angular velocity of rigids
49 * @param impulse impulse exert on rigids
50 * @param linearDamping damping ratio of linear velocity
51 * @param angularDamping damping ratio of angular velocity
52 * @param dt time step
53 * This function update the velocity of rigids based on impulse
54 */
55 __global__ void SF_updateVelocity(
56 DArray<Attribute> attribute,
57 DArray<Vec3f> velocity,
58 DArray<Vec3f> angular_velocity,
59 DArray<Vec3f> impulse,
60 float linearDamping,
61 float angularDamping,
62 float dt
63 )
64 {
65 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
66 if (tId >= velocity.size())
67 return;
68
69 if (attribute[tId].isDynamic())
70 {
71 velocity[tId] += impulse[2 * tId];
72 angular_velocity[tId] += impulse[2 * tId + 1];
73 //Damping
74 /*velocity[tId] *= 1.0f / (1.0f + dt * linearDamping);
75 angular_velocity[tId] *= 1.0f / (1.0f + dt * angularDamping);*/
76 }
77 }
78
79 void updateVelocity(
80 DArray<Attribute> attribute,
81 DArray<Vec3f> velocity,
82 DArray<Vec3f> angular_velocity,
83 DArray<Vec3f> impulse,
84 float linearDamping,
85 float angularDamping,
86 float dt
87 )
88 {
89 cuExecute(velocity.size(),
90 SF_updateVelocity,
91 attribute,
92 velocity,
93 angular_velocity,
94 impulse,
95 linearDamping,
96 angularDamping,
97 dt);
98 }
99
100
101 /**
102 * Update Gesture Function
103 *
104 * @param pos position of rigids
105 * @param rotQuat quaterion of rigids
106 * @param rotMat rotation matrix of rigids
107 * @param inertia inertia matrix of rigids
108 * @param velocity velocity of rigids
109 * @param angular_velocity angular velocity of rigids
110 * @param inertia_init initial inertial matrix of rigids
111 * @param dt time step
112 * This function update the gesture of rigids based on velocity and timeStep
113 */
114 __global__ void SF_updateGesture(
115 DArray<Attribute> attribute,
116 DArray<Vec3f> pos,
117 DArray<Quat1f> rotQuat,
118 DArray<Mat3f> rotMat,
119 DArray<Mat3f> inertia,
120 DArray<Vec3f> velocity,
121 DArray<Vec3f> angular_velocity,
122 DArray<Mat3f> inertia_init,
123 float dt
124 )
125 {
126 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
127 if (tId >= pos.size())
128 return;
129
130 if (!attribute[tId].isFixed())
131 {
132 pos[tId] += velocity[tId] * dt;
133
134 rotQuat[tId] = rotQuat[tId].normalize();
135
136 rotQuat[tId] += dt * 0.5f *
137 Quat1f(angular_velocity[tId][0], angular_velocity[tId][1], angular_velocity[tId][2], 0.0)
138 * (rotQuat[tId]);
139
140 rotQuat[tId] = rotQuat[tId].normalize();
141
142 rotMat[tId] = rotQuat[tId].toMatrix3x3();
143
144 inertia[tId] = rotMat[tId] * inertia_init[tId] * rotMat[tId].inverse();
145 }
146 }
147
148 void updateGesture(
149 DArray<Attribute> attribute,
150 DArray<Vec3f> pos,
151 DArray<Quat1f> rotQuat,
152 DArray<Mat3f> rotMat,
153 DArray<Mat3f> inertia,
154 DArray<Vec3f> velocity,
155 DArray<Vec3f> angular_velocity,
156 DArray<Mat3f> inertia_init,
157 float dt
158 )
159 {
160 cuExecute(pos.size(),
161 SF_updateGesture,
162 attribute,
163 pos,
164 rotQuat,
165 rotMat,
166 inertia,
167 velocity,
168 angular_velocity,
169 inertia_init,
170 dt);
171 }
172
173 /**
174 * update the position and rotation of rigids
175 *
176 * @param pos position of rigids
177 * @param rotQuat quaterion of rigids
178 * @param rotMat rotation matrix of rigids
179 * @param inertia inertia matrix of rigids
180 * @param inertia_init initial inertia matrix of rigids
181 * @param impulse_constrain impulse to update position and rotation
182 * This function update the position of rigids use delta position
183 */
184 __global__ void SF_updatePositionAndRotation(
185 DArray<Vec3f> pos,
186 DArray<Quat1f> rotQuat,
187 DArray<Mat3f> rotMat,
188 DArray<Mat3f> inertia,
189 DArray<Mat3f> inertia_init,
190 DArray<Vec3f> impulse_constrain
191 )
192 {
193 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
194 if (tId >= pos.size())
195 return;
196 Vec3f dx = impulse_constrain[2 * tId];
197 Vec3f dq = impulse_constrain[2 * tId + 1];
198
199 pos[tId] += impulse_constrain[2 * tId];
200 rotQuat[tId] += 0.5 * Quat1f(dq.x, dq.y, dq.z, 0.0f) * rotQuat[tId];
201
202 rotQuat[tId] = rotQuat[tId].normalize();
203 rotMat[tId] = rotQuat[tId].toMatrix3x3();
204 inertia[tId] = rotMat[tId] * inertia_init[tId] * rotMat[tId].inverse();
205 }
206
207 void updatePositionAndRotation(
208 DArray<Vec3f> pos,
209 DArray<Quat1f> rotQuat,
210 DArray<Mat3f> rotMat,
211 DArray<Mat3f> inertia,
212 DArray<Mat3f> inertia_init,
213 DArray<Vec3f> impulse_constrain
214 )
215 {
216 cuExecute(pos.size(),
217 SF_updatePositionAndRotation,
218 pos,
219 rotQuat,
220 rotMat,
221 inertia,
222 inertia_init,
223 impulse_constrain);
224 }
225
226 /**
227 * calculate contact point num function
228 *
229 * @param contacts contacts
230 * @param contactCnt contact num of each rigids
231 * This function calculate the contact num of each rigids
232 */
233 template<typename ContactPair>
234 __global__ void SF_calculateContactPoints(
235 DArray<ContactPair> contacts,
236 DArray<int> contactCnt
237 )
238 {
239 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
240 if (tId >= contacts.size())
241 return;
242
243 int idx1 = contacts[tId].bodyId1;
244 int idx2 = contacts[tId].bodyId2;
245
246 atomicAdd(&contactCnt[idx1], 1);
247
248 if (idx2 != INVALID)
249 atomicAdd(&contactCnt[idx2], 1);
250 }
251
252 void calculateContactPoints(
253 DArray<TContactPair<float>> contacts,
254 DArray<int> contactCnt
255 )
256 {
257 cuExecute(contacts.size(),
258 SF_calculateContactPoints,
259 contacts,
260 contactCnt);
261 }
262
263
264 /**
265 * calculate Jacobian Matrix function
266 *
267 * @param J Jacobian Matrix
268 * @param B M^-1J Matrix
269 * @param pos postion of rigids
270 * @param inertia inertia matrix of rigids
271 * @param mass mass of rigids
272 * @param rotMat rotation Matrix of rigids
273 * @param constraints constraints data
274 * This function calculate the Jacobian Matrix of constraints
275 */
276 template<typename Coord, typename Matrix, typename Constraint>
277 __global__ void SF_calculateJacobianMatrix(
278 DArray<Coord> J,
279 DArray<Coord> B,
280 DArray<Coord> pos,
281 DArray<Matrix> inertia,
282 DArray<Real> mass,
283 DArray<Matrix> rotMat,
284 DArray<Constraint> constraints
285 )
286 {
287 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
288 if (tId >= constraints.size())
289 return;
290
291 int idx1 = constraints[tId].bodyId1;
292 int idx2 = constraints[tId].bodyId2;
293
294 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION || constraints[tId].type == ConstraintType::CN_FRICTION)
295 {
296 Coord n = constraints[tId].normal1;
297 Coord r1 = constraints[tId].pos1 - pos[idx1];
298 Coord rcn_1 = r1.cross(n);
299
300 J[4 * tId] = -n;
301 J[4 * tId + 1] = -rcn_1;
302 B[4 * tId] = -n / mass[idx1];
303 B[4 * tId + 1] = -inertia[idx1].inverse() * rcn_1;
304
305 if (idx2 != INVALID)
306 {
307 Coord r2 = constraints[tId].pos2 - pos[idx2];
308 Coord rcn_2 = r2.cross(n);
309 J[4 * tId + 2] = n;
310 J[4 * tId + 3] = rcn_2;
311 B[4 * tId + 2] = n / mass[idx2];
312 B[4 * tId + 3] = inertia[idx2].inverse() * rcn_2;
313 }
314 }
315
316 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1)
317 {
318 Coord r1 = constraints[tId].normal1;
319 Coord r2 = constraints[tId].normal2;
320
321
322 J[4 * tId] = Coord(-1, 0, 0);
323 J[4 * tId + 1] = Coord(0, -r1[2], r1[1]);
324 if (idx2 != INVALID)
325 {
326 J[4 * tId + 2] = Coord(1, 0, 0);
327 J[4 * tId + 3] = Coord(0, r2[2], -r2[1]);
328 }
329
330 B[4 * tId] = Coord(-1, 0, 0) / mass[idx1];
331 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(0, -r1[2], r1[1]);
332 if (idx2 != INVALID)
333 {
334 B[4 * tId + 2] = Coord(1, 0, 0) / mass[idx2];
335 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(0, r2[2], -r2[1]);
336 }
337 }
338
339 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_2)
340 {
341 Coord r1 = constraints[tId].normal1;
342 Coord r2 = constraints[tId].normal2;
343
344 J[4 * tId] = Coord(0, -1, 0);
345 J[4 * tId + 1] = Coord(r1[2], 0, -r1[0]);
346 if (idx2 != INVALID)
347 {
348 J[4 * tId + 2] = Coord(0, 1, 0);
349 J[4 * tId + 3] = Coord(-r2[2], 0, r2[0]);
350 }
351
352 B[4 * tId] = Coord(0, -1, 0) / mass[idx1];
353 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(r1[2], 0, -r1[0]);
354 if (idx2 != INVALID)
355 {
356 B[4 * tId + 2] = Coord(0, 1, 0) / mass[idx2];
357 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(-r2[2], 0, r2[0]);
358 }
359 }
360
361 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_3)
362 {
363 Coord r1 = constraints[tId].normal1;
364 Coord r2 = constraints[tId].normal2;
365
366 J[4 * tId] = Coord(0, 0, -1);
367 J[4 * tId + 1] = Coord(-r1[1], r1[0], 0);
368 if (idx2 != INVALID)
369 {
370 J[4 * tId + 2] = Coord(0, 0, 1);
371 J[4 * tId + 3] = Coord(r2[1], -r2[0], 0);
372 }
373
374 B[4 * tId] = Coord(0, 0, -1) / mass[idx1];
375 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(-r1[1], r1[0], 0);
376 if (idx2 != INVALID)
377 {
378 B[4 * tId + 2] = Coord(0, 0, 1) / mass[idx2];
379 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(r2[1], -r2[0], 0);
380 }
381 }
382
383 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
384 {
385 Coord r1 = constraints[tId].pos1;
386 Coord r2 = constraints[tId].pos2;
387
388 Coord n1 = constraints[tId].normal1;
389
390 J[4 * tId] = -n1;
391 J[4 * tId + 1] = -(pos[idx2] + r2 - pos[idx1]).cross(n1);
392 J[4 * tId + 2] = n1;
393 J[4 * tId + 3] = r2.cross(n1);
394
395 B[4 * tId] = -n1 / mass[idx1];
396 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
397 B[4 * tId + 2] = n1 / mass[idx2];
398 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
399 }
400
401 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_2)
402 {
403 Coord r1 = constraints[tId].pos1;
404 Coord r2 = constraints[tId].pos2;
405
406 Coord n2 = constraints[tId].normal2;
407
408 J[4 * tId] = -n2;
409 J[4 * tId + 1] = -(pos[idx2] + r2 - pos[idx1]).cross(n2);
410 J[4 * tId + 2] = n2;
411 J[4 * tId + 3] = r2.cross(n2);
412
413 B[4 * tId] = -n2 / mass[idx1];
414 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
415 B[4 * tId + 2] = n2 / mass[idx2];
416 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
417 }
418
419 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
420 {
421 J[4 * tId] = Coord(0);
422 J[4 * tId + 1] = Coord(-1, 0, 0);
423 if (idx2 != INVALID)
424 {
425 J[4 * tId + 2] = Coord(0);
426 J[4 * tId + 3] = Coord(1, 0, 0);
427 }
428
429 B[4 * tId] = Coord(0);
430 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(-1, 0, 0);
431 if (idx2 != INVALID)
432 {
433 B[4 * tId + 2] = Coord(0);
434 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(1, 0, 0);
435 }
436 }
437
438 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_2)
439 {
440 J[4 * tId] = Coord(0);
441 J[4 * tId + 1] = Coord(0, -1, 0);
442 if (idx2 != INVALID)
443 {
444 J[4 * tId + 2] = Coord(0);
445 J[4 * tId + 3] = Coord(0, 1, 0);
446 }
447
448 B[4 * tId] = Coord(0);
449 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(0, -1, 0);
450 if (idx2 != INVALID)
451 {
452 B[4 * tId + 2] = Coord(0);
453 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(0, 1, 0);
454 }
455 }
456
457 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_3)
458 {
459 J[4 * tId] = Coord(0);
460 J[4 * tId + 1] = Coord(0, 0, -1);
461 if (idx2 != INVALID)
462 {
463 J[4 * tId + 2] = Coord(0);
464 J[4 * tId + 3] = Coord(0, 0, 1);
465 }
466
467 B[4 * tId] = Coord(0);
468 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(0, 0, -1);
469 if (idx2 != INVALID)
470 {
471 B[4 * tId + 2] = Coord(0);
472 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(0, 0, 1);
473 }
474 }
475
476 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER)
477 {
478 if (constraints[tId].isValid)
479 {
480 Coord n = constraints[tId].axis;
481
482 J[4 * tId] = n;
483 J[4 * tId + 1] = Coord(0);
484 J[4 * tId + 2] = -n;
485 J[4 * tId + 3] = Coord(0);
486
487 B[4 * tId] = n / mass[idx1];
488 B[4 * tId + 1] = Coord(0);
489 B[4 * tId + 2] = -n / mass[idx2];
490 B[4 * tId + 3] = Coord(0);
491 }
492 }
493
494 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN)
495 {
496 if (constraints[tId].isValid)
497 {
498 Coord a = constraints[tId].axis;
499 Coord r1 = constraints[tId].pos1;
500 Coord r2 = constraints[tId].pos2;
501
502 J[4 * tId] = -a;
503 J[4 * tId + 1] = -(pos[idx2] + r2 - pos[idx1]).cross(a);
504 J[4 * tId + 2] = a;
505 J[4 * tId + 3] = r2.cross(a);
506
507 B[4 * tId] = -a / mass[idx1];
508 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
509 B[4 * tId + 2] = a / mass[idx2];
510 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
511 }
512 }
513
514 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX)
515 {
516 if (constraints[tId].isValid)
517 {
518 Coord a = constraints[tId].axis;
519 Coord r1 = constraints[tId].pos1;
520 Coord r2 = constraints[tId].pos2;
521
522 J[4 * tId] = a;
523 J[4 * tId + 1] = (pos[idx2] + r2 - pos[idx1]).cross(a);
524 J[4 * tId + 2] = -a;
525 J[4 * tId + 3] = -r2.cross(a);
526
527 B[4 * tId] = a / mass[idx1];
528 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
529 B[4 * tId + 2] = -a / mass[idx2];
530 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
531 }
532 }
533
534 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1)
535 {
536 Coord b2 = constraints[tId].pos1;
537 Coord a1 = constraints[tId].axis;
538
539 J[4 * tId] = Coord(0);
540 J[4 * tId + 1] = -b2.cross(a1);
541 J[4 * tId + 2] = Coord(0);
542 J[4 * tId + 3] = b2.cross(a1);
543
544 B[4 * tId] = Coord(0);
545 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
546 B[4 * tId + 2] = Coord(0);
547 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
548 }
549
550 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_2)
551 {
552 Coord c2 = constraints[tId].pos2;
553 Coord a1 = constraints[tId].axis;
554
555 J[4 * tId] = Coord(0);
556 J[4 * tId + 1] = -c2.cross(a1);
557 J[4 * tId + 2] = Coord(0);
558 J[4 * tId + 3] = c2.cross(a1);
559
560 B[4 * tId] = Coord(0);
561 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
562 B[4 * tId + 2] = Coord(0);
563 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
564 }
565
566 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN)
567 {
568 if (constraints[tId].isValid == 1)
569 {
570 Coord a = constraints[tId].axis;
571 J[4 * tId] = Coord(0);
572 J[4 * tId + 1] = -a;
573 J[4 * tId + 2] = Coord(0);
574 J[4 * tId + 3] = a;
575
576 B[4 * tId] = Coord(0);
577 B[4 * tId + 1] = inertia[idx1].inverse() * (-a);
578 B[4 * tId + 2] = Coord(0);
579 B[4 * tId + 3] = inertia[idx2].inverse() * (a);
580 }
581 }
582
583 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX)
584 {
585 if (constraints[tId].isValid == 1)
586 {
587 Coord a = constraints[tId].axis;
588 J[4 * tId] = Coord(0);
589 J[4 * tId + 1] = a;
590 J[4 * tId + 2] = Coord(0);
591 J[4 * tId + 3] = -a;
592
593 B[4 * tId] = Coord(0);
594 B[4 * tId + 1] = inertia[idx1].inverse() * (a);
595 B[4 * tId + 2] = Coord(0);
596 B[4 * tId + 3] = inertia[idx2].inverse() * (-a);
597 }
598 }
599
600 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER)
601 {
602 if (constraints[tId].isValid)
603 {
604 Coord a = constraints[tId].axis;
605 J[4 * tId] = Coord(0);
606 J[4 * tId + 1] = -a;
607 J[4 * tId + 2] = Coord(0);
608 J[4 * tId + 3] = a;
609
610 B[4 * tId] = Coord(0);
611 B[4 * tId + 1] = inertia[idx1].inverse() * (-a);
612 B[4 * tId + 2] = Coord(0);
613 B[4 * tId + 3] = inertia[idx2].inverse() * a;
614 }
615 }
616
617 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
618 {
619 J[4 * tId] = Coord(1.0, 0, 0);
620 J[4 * tId + 1] = Coord(0);
621 J[4 * tId + 2] = Coord(0);
622 J[4 * tId + 3] = Coord(0);
623
624 B[4 * tId] = Coord(1 / mass[idx1], 0, 0);
625 B[4 * tId + 1] = Coord(0);
626 B[4 * tId + 2] = Coord(0);
627 B[4 * tId + 3] = Coord(0);
628 }
629
630 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_2)
631 {
632 J[4 * tId] = Coord(0, 1.0, 0);
633 J[4 * tId + 1] = Coord(0);
634 J[4 * tId + 2] = Coord(0);
635 J[4 * tId + 3] = Coord(0);
636
637 B[4 * tId] = Coord(0, 1.0 / mass[idx1], 0);
638 B[4 * tId + 1] = Coord(0);
639 B[4 * tId + 2] = Coord(0);
640 B[4 * tId + 3] = Coord(0);
641 }
642
643 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_3)
644 {
645 J[4 * tId] = Coord(0, 0, 1.0);
646 J[4 * tId + 1] = Coord(0);
647 J[4 * tId + 2] = Coord(0);
648 J[4 * tId + 3] = Coord(0);
649
650 B[4 * tId] = Coord(0, 0, 1.0 / mass[idx1]);
651 B[4 * tId + 1] = Coord(0);
652 B[4 * tId + 2] = Coord(0);
653 B[4 * tId + 3] = Coord(0);
654 }
655 }
656
657 template<typename Coord, typename Matrix, typename Constraint>
658 __global__ void SF_calculateJacobianMatrixForNJS(
659 DArray<Coord> J,
660 DArray<Coord> B,
661 DArray<Coord> pos,
662 DArray<Matrix> inertia,
663 DArray<Real> mass,
664 DArray<Matrix> rotMat,
665 DArray<Constraint> constraints
666 )
667 {
668 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
669 if (tId >= constraints.size())
670 return;
671
672 int idx1 = constraints[tId].bodyId1;
673 int idx2 = constraints[tId].bodyId2;
674
675 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
676 {
677 Coord n = constraints[tId].normal1;
678 Coord r1 = constraints[tId].pos1 - pos[idx1];
679 Coord rcn_1 = r1.cross(n);
680
681 J[4 * tId] = -n;
682 J[4 * tId + 1] = -rcn_1;
683 B[4 * tId] = -n / mass[idx1];
684 B[4 * tId + 1] = -inertia[idx1].inverse() * rcn_1;
685
686 if (idx2 != INVALID)
687 {
688 Coord r2 = constraints[tId].pos2 - pos[idx2];
689 Coord rcn_2 = r2.cross(n);
690 J[4 * tId + 2] = n;
691 J[4 * tId + 3] = rcn_2;
692 B[4 * tId + 2] = n / mass[idx2];
693 B[4 * tId + 3] = inertia[idx2].inverse() * rcn_2;
694 }
695 }
696
697 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1)
698 {
699 Coord r1 = constraints[tId].normal1;
700 Coord r2 = constraints[tId].normal2;
701
702
703 J[4 * tId] = Coord(-1, 0, 0);
704 J[4 * tId + 1] = Coord(0, -r1[2], r1[1]);
705 if (idx2 != INVALID)
706 {
707 J[4 * tId + 2] = Coord(1, 0, 0);
708 J[4 * tId + 3] = Coord(0, r2[2], -r2[1]);
709 }
710
711 B[4 * tId] = Coord(-1, 0, 0) / mass[idx1];
712 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(0, -r1[2], r1[1]);
713 if (idx2 != INVALID)
714 {
715 B[4 * tId + 2] = Coord(1, 0, 0) / mass[idx2];
716 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(0, r2[2], -r2[1]);
717 }
718 }
719
720 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_2)
721 {
722 Coord r1 = constraints[tId].normal1;
723 Coord r2 = constraints[tId].normal2;
724
725 J[4 * tId] = Coord(0, -1, 0);
726 J[4 * tId + 1] = Coord(r1[2], 0, -r1[0]);
727 if (idx2 != INVALID)
728 {
729 J[4 * tId + 2] = Coord(0, 1, 0);
730 J[4 * tId + 3] = Coord(-r2[2], 0, r2[0]);
731 }
732
733 B[4 * tId] = Coord(0, -1, 0) / mass[idx1];
734 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(r1[2], 0, -r1[0]);
735 if (idx2 != INVALID)
736 {
737 B[4 * tId + 2] = Coord(0, 1, 0) / mass[idx2];
738 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(-r2[2], 0, r2[0]);
739 }
740 }
741
742 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_3)
743 {
744 Coord r1 = constraints[tId].normal1;
745 Coord r2 = constraints[tId].normal2;
746
747 J[4 * tId] = Coord(0, 0, -1);
748 J[4 * tId + 1] = Coord(-r1[1], r1[0], 0);
749 if (idx2 != INVALID)
750 {
751 J[4 * tId + 2] = Coord(0, 0, 1);
752 J[4 * tId + 3] = Coord(r2[1], -r2[0], 0);
753 }
754
755 B[4 * tId] = Coord(0, 0, -1) / mass[idx1];
756 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(-r1[1], r1[0], 0);
757 if (idx2 != INVALID)
758 {
759 B[4 * tId + 2] = Coord(0, 0, 1) / mass[idx2];
760 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(r2[1], -r2[0], 0);
761 }
762 }
763
764 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
765 {
766 Coord r1 = constraints[tId].pos1;
767 Coord r2 = constraints[tId].pos2;
768
769 Coord n1 = constraints[tId].normal1;
770
771 J[4 * tId] = -n1;
772 J[4 * tId + 1] = -(pos[idx2] + r2 - pos[idx1]).cross(n1);
773 J[4 * tId + 2] = n1;
774 J[4 * tId + 3] = r2.cross(n1);
775
776 B[4 * tId] = -n1 / mass[idx1];
777 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
778 B[4 * tId + 2] = n1 / mass[idx2];
779 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
780 }
781
782 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_2)
783 {
784 Coord r1 = constraints[tId].pos1;
785 Coord r2 = constraints[tId].pos2;
786
787 Coord n2 = constraints[tId].normal2;
788
789 J[4 * tId] = -n2;
790 J[4 * tId + 1] = -(pos[idx2] + r2 - pos[idx1]).cross(n2);
791 J[4 * tId + 2] = n2;
792 J[4 * tId + 3] = r2.cross(n2);
793
794 B[4 * tId] = -n2 / mass[idx1];
795 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
796 B[4 * tId + 2] = n2 / mass[idx2];
797 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
798 }
799
800 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
801 {
802 J[4 * tId] = Coord(0);
803 J[4 * tId + 1] = Coord(-1, 0, 0);
804 if (idx2 != INVALID)
805 {
806 J[4 * tId + 2] = Coord(0);
807 J[4 * tId + 3] = Coord(1, 0, 0);
808 }
809
810 B[4 * tId] = Coord(0);
811 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(-1, 0, 0);
812 if (idx2 != INVALID)
813 {
814 B[4 * tId + 2] = Coord(0);
815 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(1, 0, 0);
816 }
817 }
818
819 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_2)
820 {
821 J[4 * tId] = Coord(0);
822 J[4 * tId + 1] = Coord(0, -1, 0);
823 if (idx2 != INVALID)
824 {
825 J[4 * tId + 2] = Coord(0);
826 J[4 * tId + 3] = Coord(0, 1, 0);
827 }
828
829 B[4 * tId] = Coord(0);
830 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(0, -1, 0);
831 if (idx2 != INVALID)
832 {
833 B[4 * tId + 2] = Coord(0);
834 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(0, 1, 0);
835 }
836 }
837
838 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_3)
839 {
840 J[4 * tId] = Coord(0);
841 J[4 * tId + 1] = Coord(0, 0, -1);
842 if (idx2 != INVALID)
843 {
844 J[4 * tId + 2] = Coord(0);
845 J[4 * tId + 3] = Coord(0, 0, 1);
846 }
847
848 B[4 * tId] = Coord(0);
849 B[4 * tId + 1] = inertia[idx1].inverse() * Coord(0, 0, -1);
850 if (idx2 != INVALID)
851 {
852 B[4 * tId + 2] = Coord(0);
853 B[4 * tId + 3] = inertia[idx2].inverse() * Coord(0, 0, 1);
854 }
855 }
856
857
858 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN)
859 {
860 if (constraints[tId].isValid)
861 {
862 Coord a = constraints[tId].axis;
863 Coord r1 = constraints[tId].pos1;
864 Coord r2 = constraints[tId].pos2;
865
866 J[4 * tId] = -a;
867 J[4 * tId + 1] = -(pos[idx2] + r2 - pos[idx1]).cross(a);
868 J[4 * tId + 2] = a;
869 J[4 * tId + 3] = r2.cross(a);
870
871 B[4 * tId] = -a / mass[idx1];
872 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
873 B[4 * tId + 2] = a / mass[idx2];
874 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
875 }
876 }
877
878 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX)
879 {
880 if (constraints[tId].isValid)
881 {
882 Coord a = constraints[tId].axis;
883 Coord r1 = constraints[tId].pos1;
884 Coord r2 = constraints[tId].pos2;
885
886 J[4 * tId] = a;
887 J[4 * tId + 1] = (pos[idx2] + r2 - pos[idx1]).cross(a);
888 J[4 * tId + 2] = -a;
889 J[4 * tId + 3] = -r2.cross(a);
890
891 B[4 * tId] = a / mass[idx1];
892 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
893 B[4 * tId + 2] = -a / mass[idx2];
894 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
895 }
896 }
897
898 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1)
899 {
900 Coord b2 = constraints[tId].pos1;
901 Coord a1 = constraints[tId].axis;
902
903 J[4 * tId] = Coord(0);
904 J[4 * tId + 1] = -b2.cross(a1);
905 J[4 * tId + 2] = Coord(0);
906 J[4 * tId + 3] = b2.cross(a1);
907
908 B[4 * tId] = Coord(0);
909 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
910 B[4 * tId + 2] = Coord(0);
911 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
912 }
913
914 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_2)
915 {
916 Coord c2 = constraints[tId].pos2;
917 Coord a1 = constraints[tId].axis;
918
919 J[4 * tId] = Coord(0);
920 J[4 * tId + 1] = -c2.cross(a1);
921 J[4 * tId + 2] = Coord(0);
922 J[4 * tId + 3] = c2.cross(a1);
923
924 B[4 * tId] = Coord(0);
925 B[4 * tId + 1] = inertia[idx1].inverse() * J[4 * tId + 1];
926 B[4 * tId + 2] = Coord(0);
927 B[4 * tId + 3] = inertia[idx2].inverse() * J[4 * tId + 3];
928 }
929
930 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN)
931 {
932 if (constraints[tId].isValid == 1)
933 {
934 Coord a = constraints[tId].axis;
935 J[4 * tId] = Coord(0);
936 J[4 * tId + 1] = -a;
937 J[4 * tId + 2] = Coord(0);
938 J[4 * tId + 3] = a;
939
940 B[4 * tId] = Coord(0);
941 B[4 * tId + 1] = inertia[idx1].inverse() * (-a);
942 B[4 * tId + 2] = Coord(0);
943 B[4 * tId + 3] = inertia[idx2].inverse() * (a);
944 }
945 }
946
947 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX)
948 {
949 if (constraints[tId].isValid == 1)
950 {
951 Coord a = constraints[tId].axis;
952 J[4 * tId] = Coord(0);
953 J[4 * tId + 1] = a;
954 J[4 * tId + 2] = Coord(0);
955 J[4 * tId + 3] = -a;
956
957 B[4 * tId] = Coord(0);
958 B[4 * tId + 1] = inertia[idx1].inverse() * (a);
959 B[4 * tId + 2] = Coord(0);
960 B[4 * tId + 3] = inertia[idx2].inverse() * (-a);
961 }
962 }
963
964 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
965 {
966 J[4 * tId] = Coord(1.0, 0, 0);
967 J[4 * tId + 1] = Coord(0);
968 J[4 * tId + 2] = Coord(0);
969 J[4 * tId + 3] = Coord(0);
970
971 B[4 * tId] = Coord(1 / mass[idx1], 0, 0);
972 B[4 * tId + 1] = Coord(0);
973 B[4 * tId + 2] = Coord(0);
974 B[4 * tId + 3] = Coord(0);
975 }
976
977 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_2)
978 {
979 J[4 * tId] = Coord(0, 1.0, 0);
980 J[4 * tId + 1] = Coord(0);
981 J[4 * tId + 2] = Coord(0);
982 J[4 * tId + 3] = Coord(0);
983
984 B[4 * tId] = Coord(0, 1.0 / mass[idx1], 0);
985 B[4 * tId + 1] = Coord(0);
986 B[4 * tId + 2] = Coord(0);
987 B[4 * tId + 3] = Coord(0);
988 }
989
990 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_3)
991 {
992 J[4 * tId] = Coord(0, 0, 1.0);
993 J[4 * tId + 1] = Coord(0);
994 J[4 * tId + 2] = Coord(0);
995 J[4 * tId + 3] = Coord(0);
996
997 B[4 * tId] = Coord(0, 0, 1.0 / mass[idx1]);
998 B[4 * tId + 1] = Coord(0);
999 B[4 * tId + 2] = Coord(0);
1000 B[4 * tId + 3] = Coord(0);
1001 }
1002 }
1003
1004 void calculateJacobianMatrix(
1005 DArray<Vec3f> J,
1006 DArray<Vec3f> B,
1007 DArray<Vec3f> pos,
1008 DArray<Mat3f> inertia,
1009 DArray<float> mass,
1010 DArray<Mat3f> rotMat,
1011 DArray<TConstraintPair<float>> constraints
1012 )
1013 {
1014 cuExecute(constraints.size(),
1015 SF_calculateJacobianMatrix,
1016 J,
1017 B,
1018 pos,
1019 inertia,
1020 mass,
1021 rotMat,
1022 constraints);
1023 }
1024
1025 void calculateJacobianMatrixForNJS(
1026 DArray<Vec3f> J,
1027 DArray<Vec3f> B,
1028 DArray<Vec3f> pos,
1029 DArray<Mat3f> inertia,
1030 DArray<float> mass,
1031 DArray<Mat3f> rotMat,
1032 DArray<TConstraintPair<float>> constraints
1033 )
1034 {
1035 cuExecute(constraints.size(),
1036 SF_calculateJacobianMatrixForNJS,
1037 J,
1038 B,
1039 pos,
1040 inertia,
1041 mass,
1042 rotMat,
1043 constraints);
1044 }
1045
1046
1047 /**
1048 * calculate eta vector for PJS
1049 *
1050 * @param eta eta vector
1051 * @param J Jacobian Matrix
1052 * @param velocity linear velocity of rigids
1053 * @param angular_velocity angular velocity of rigids
1054 * @param constraints constraints data
1055 * This function calculate the diagonal Matrix of JB
1056 */
1057 template<typename Coord, typename Constraint, typename Real>
1058 __global__ void SF_calculateEtaVectorForPJS(
1059 DArray<Real> eta,
1060 DArray<Coord> J,
1061 DArray<Coord> velocity,
1062 DArray<Coord> angular_velocity,
1063 DArray<Constraint> constraints
1064 )
1065 {
1066 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
1067 if (tId >= constraints.size())
1068 return;
1069
1070 int idx1 = constraints[tId].bodyId1;
1071 int idx2 = constraints[tId].bodyId2;
1072
1073 Real eta_i = Real(0);
1074
1075 eta_i -= J[4 * tId].dot(velocity[idx1]);
1076 eta_i -= J[4 * tId + 1].dot(angular_velocity[idx1]);
1077
1078 if (idx2 != INVALID)
1079 {
1080 eta_i -= J[4 * tId + 2].dot(velocity[idx2]);
1081 eta_i -= J[4 * tId + 3].dot(angular_velocity[idx2]);
1082 }
1083
1084 eta[tId] = eta_i;
1085
1086 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER)
1087 {
1088 Real v_moter = constraints[tId].interpenetration;
1089 eta[tId] -= v_moter;
1090 }
1091 }
1092
1093 void calculateEtaVectorForPJS(
1094 DArray<float> eta,
1095 DArray<Vec3f> J,
1096 DArray<Vec3f> velocity,
1097 DArray<Vec3f> angular_velocity,
1098 DArray<TConstraintPair<float>> constraints
1099 )
1100 {
1101 cuExecute(constraints.size(),
1102 SF_calculateEtaVectorForPJS,
1103 eta,
1104 J,
1105 velocity,
1106 angular_velocity,
1107 constraints);
1108 }
1109
1110 /**
1111 * calculate eta vector for PJS Baumgarte stabilization
1112 *
1113 * @param eta eta vector
1114 * @param J Jacobian Matrix
1115 * @param velocity linear velocity of rigids
1116 * @param angular_velocity angular velocity of rigids
1117 * @param pos position of rigids
1118 * @param rotation_q quarterion of rigids
1119 * @param constraints constraints data
1120 * @param slop interpenetration slop
1121 * @param beta Baumgarte bias
1122 * @param dt time step
1123 * This function calculate the diagonal Matrix of JB
1124 */
1125 template<typename Coord, typename Constraint, typename Real, typename Quat>
1126 __global__ void SF_calculateEtaVectorForPJSBaumgarte(
1127 DArray<Real> eta,
1128 DArray<Coord> J,
1129 DArray<Coord> velocity,
1130 DArray<Coord> angular_velocity,
1131 DArray<Coord> pos,
1132 DArray<Quat> rotation_q,
1133 DArray<Constraint> constraints,
1134 DArray<Real> errors,
1135 Real slop,
1136 Real beta,
1137 uint substepping,
1138 Real dt
1139 )
1140 {
1141 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
1142 if (tId >= constraints.size())
1143 return;
1144
1145 int idx1 = constraints[tId].bodyId1;
1146 int idx2 = constraints[tId].bodyId2;
1147
1148 Real eta_i = Real(0);
1149
1150 eta_i -= J[4 * tId].dot(velocity[idx1]);
1151 eta_i -= J[4 * tId + 1].dot(angular_velocity[idx1]);
1152
1153 if (idx2 != INVALID)
1154 {
1155 eta_i -= J[4 * tId + 2].dot(velocity[idx2]);
1156 eta_i -= J[4 * tId + 3].dot(angular_velocity[idx2]);
1157 }
1158
1159 eta[tId] = eta_i;
1160
1161 Real invDt = Real(1) / dt;
1162 Real error = 0;
1163
1164 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER)
1165 {
1166 Real v_moter = constraints[tId].interpenetration;
1167 eta[tId] -= v_moter;
1168 }
1169
1170 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
1171 {
1172 error = minimum(constraints[tId].interpenetration + slop, 0.0f) / substepping;
1173 }
1174
1175 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1)
1176 {
1177 Coord r1 = constraints[tId].normal1;
1178 Coord r2 = constraints[tId].normal2;
1179 Coord pos1 = constraints[tId].pos1;
1180 Coord errorVec;
1181 if(idx2 != INVALID)
1182 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1183 else
1184 errorVec = pos1 - pos[idx1] - r1;
1185 error = errorVec[0];
1186 }
1187
1188 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_2)
1189 {
1190 Coord r1 = constraints[tId].normal1;
1191 Coord r2 = constraints[tId].normal2;
1192 Coord pos1 = constraints[tId].pos1;
1193 Coord errorVec;
1194 if (idx2 != INVALID)
1195 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1196 else
1197 errorVec = pos1 - pos[idx1] - r1;
1198 error = errorVec[1];
1199 }
1200
1201 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_3)
1202 {
1203 Coord r1 = constraints[tId].normal1;
1204 Coord r2 = constraints[tId].normal2;
1205 Coord pos1 = constraints[tId].pos1;
1206 Coord errorVec;
1207 if (idx2 != INVALID)
1208 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1209 else
1210 errorVec = pos1 - pos[idx1] - r1;
1211 error = errorVec[2];
1212 }
1213
1214 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
1215 {
1216 Coord r1 = constraints[tId].pos1;
1217 Coord r2 = constraints[tId].pos2;
1218 Coord n1 = constraints[tId].normal1;
1219
1220 error = (pos[idx2] + r2 - pos[idx1] - r1).dot(n1);
1221 }
1222
1223 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_2)
1224 {
1225 Coord r1 = constraints[tId].pos1;
1226 Coord r2 = constraints[tId].pos2;
1227 Coord n2 = constraints[tId].normal2;
1228
1229 error = (pos[idx2] + r2 - pos[idx1] - r1).dot(n2);
1230 }
1231
1232 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
1233 {
1234 Quat q1 = rotation_q[idx1];
1235 if (idx2 != INVALID)
1236 {
1237 Quat q2 = rotation_q[idx2];
1238 Quat q_init = constraints[tId].rotQuat;
1239 Quat q_error = q2 * q_init * q1.inverse();
1240
1241 error = q_error.x * 2;
1242 }
1243 else
1244 {
1245 Quat diff = rotation_q[idx1] * constraints[tId].rotQuat.inverse();
1246 error = -diff.x * 2;
1247 }
1248 }
1249
1250 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_2)
1251 {
1252 Quat q1 = rotation_q[idx1];
1253 if (idx2 != INVALID)
1254 {
1255 Quat q2 = rotation_q[idx2];
1256 Quat q_init = constraints[tId].rotQuat;
1257 Quat q_error = q2 * q_init * q1.inverse();
1258
1259 error = q_error.y * 2;
1260 }
1261 else
1262 {
1263 Quat diff = rotation_q[idx1] * constraints[tId].rotQuat.inverse();
1264 error = -diff.y * 2;
1265 }
1266 }
1267
1268 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_3)
1269 {
1270 Quat q1 = rotation_q[idx1];
1271 if (idx2 != INVALID)
1272 {
1273 Quat q2 = rotation_q[idx2];
1274 Quat q_init = constraints[tId].rotQuat;
1275 Quat q_error = q2 * q_init * q1.inverse();
1276
1277 error = q_error.z * 2;
1278 }
1279 else
1280 {
1281 Quat diff = rotation_q[idx1] * constraints[tId].rotQuat.inverse();
1282 error = -diff.z * 2;
1283 }
1284 }
1285
1286 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN)
1287 {
1288 error = constraints[tId].d_min;
1289 }
1290
1291 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX)
1292 {
1293 error = constraints[tId].d_max;
1294 }
1295
1296 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1)
1297 {
1298 Coord a1 = constraints[tId].axis;
1299 Coord b2 = constraints[tId].pos1;
1300 error = a1.dot(b2);
1301 }
1302
1303 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_2)
1304 {
1305 Coord a1 = constraints[tId].axis;
1306 Coord c2 = constraints[tId].pos2;
1307 error = a1.dot(c2);
1308 }
1309
1310 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
1311 {
1312 Coord errorVec = constraints[tId].normal1;
1313 error = errorVec[0];
1314 }
1315
1316 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_2)
1317 {
1318 Coord errorVec = constraints[tId].normal1;
1319 error = errorVec[1];
1320 }
1321
1322 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_3)
1323 {
1324 Coord errorVec = constraints[tId].normal1;
1325 error = errorVec[2];
1326 }
1327
1328 eta[tId] -= beta * invDt * error;
1329 errors[tId] = error;
1330 }
1331
1332 template<typename Coord, typename Constraint, typename Real, typename Quat>
1333 __global__ void SF_calculateEtaVectorWithERP(
1334 DArray<Real> eta,
1335 DArray<Coord> J,
1336 DArray<Coord> velocity,
1337 DArray<Coord> angular_velocity,
1338 DArray<Coord> pos,
1339 DArray<Quat> rotation_q,
1340 DArray<Constraint> constraints,
1341 DArray<Real> ERP,
1342 Real slop,
1343 Real dt
1344 )
1345 {
1346 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
1347 if (tId >= constraints.size())
1348 return;
1349
1350 if (!constraints[tId].isValid)
1351 return;
1352
1353 Real invDt = Real(1) / dt;
1354 int idx1 = constraints[tId].bodyId1;
1355 int idx2 = constraints[tId].bodyId2;
1356
1357 Real eta_i = Real(0);
1358
1359 eta_i -= J[4 * tId].dot(velocity[idx1]);
1360 eta_i -= J[4 * tId + 1].dot(angular_velocity[idx1]);
1361
1362 if (idx2 != INVALID)
1363 {
1364 eta_i -= J[4 * tId + 2].dot(velocity[idx2]);
1365 eta_i -= J[4 * tId + 3].dot(angular_velocity[idx2]);
1366 }
1367
1368 eta[tId] = eta_i;
1369
1370 Real error = 0;
1371
1372 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER)
1373 {
1374 Real v_moter = constraints[tId].interpenetration;
1375 eta[tId] -= v_moter;
1376 }
1377
1378 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
1379 {
1380 error = minimum(constraints[tId].interpenetration + slop, 0.0f);
1381 }
1382
1383 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1)
1384 {
1385 Coord r1 = constraints[tId].normal1;
1386 Coord r2 = constraints[tId].normal2;
1387 Coord pos1 = constraints[tId].pos1;
1388 Coord errorVec;
1389 if (idx2 != INVALID)
1390 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1391 else
1392 errorVec = pos1 - pos[idx1] - r1;
1393 error = errorVec[0];
1394 }
1395
1396 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_2)
1397 {
1398 Coord r1 = constraints[tId].normal1;
1399 Coord r2 = constraints[tId].normal2;
1400 Coord pos1 = constraints[tId].pos1;
1401 Coord errorVec;
1402 if (idx2 != INVALID)
1403 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1404 else
1405 errorVec = pos1 - pos[idx1] - r1;
1406 error = errorVec[1];
1407 }
1408
1409 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_3)
1410 {
1411 Coord r1 = constraints[tId].normal1;
1412 Coord r2 = constraints[tId].normal2;
1413 Coord pos1 = constraints[tId].pos1;
1414 Coord errorVec;
1415 if (idx2 != INVALID)
1416 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1417 else
1418 errorVec = pos1 - pos[idx1] - r1;
1419 error = errorVec[2];
1420 }
1421
1422 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
1423 {
1424 Coord r1 = constraints[tId].pos1;
1425 Coord r2 = constraints[tId].pos2;
1426 Coord n1 = constraints[tId].normal1;
1427
1428 error = (pos[idx2] + r2 - pos[idx1] - r1).dot(n1);
1429 }
1430
1431 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_2)
1432 {
1433 Coord r1 = constraints[tId].pos1;
1434 Coord r2 = constraints[tId].pos2;
1435 Coord n2 = constraints[tId].normal2;
1436
1437 error = (pos[idx2] + r2 - pos[idx1] - r1).dot(n2);
1438 }
1439
1440 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
1441 {
1442 Quat q1 = rotation_q[idx1];
1443 q1 = q1.normalize();
1444 if (idx2 != INVALID)
1445 {
1446 Quat q2 = rotation_q[idx2];
1447 q2 = q2.normalize();
1448
1449 Quat q_error = q2 * q1.inverse();
1450 q_error = q_error.normalize();
1451
1452 Real theta = 2.0 * acos(q_error.w);
1453 if (theta > 1e-6)
1454 {
1455 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q_error.x, q_error.y, q_error.z);
1456 error = theta * v.x;
1457 }
1458 else
1459 {
1460 error = theta;
1461 }
1462 }
1463 else
1464 {
1465 Real theta = 2.0 * acos(q1.w);
1466 if (theta > 1e-6)
1467 {
1468 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q1.x, q1.y, q1.z);
1469 error = theta * v.x;
1470 }
1471 else
1472 {
1473 error = theta;
1474 }
1475 }
1476 }
1477
1478 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_2)
1479 {
1480 Quat q1 = rotation_q[idx1];
1481 q1 = q1.normalize();
1482 if (idx2 != INVALID)
1483 {
1484 Quat q2 = rotation_q[idx2];
1485 q2 = q2.normalize();
1486 Quat q_error = q2 * q1.inverse();
1487 q_error = q_error.normalize();
1488 Real theta = 2.0 * acos(q_error.w);
1489 if (theta > 1e-6)
1490 {
1491 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q_error.x, q_error.y, q_error.z);
1492 error = theta * v.y;
1493 }
1494 else
1495 {
1496 error = theta;
1497 }
1498 }
1499 else
1500 {
1501 Real theta = 2.0 * acos(q1.w);
1502 if (theta > 1e-6)
1503 {
1504 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q1.x, q1.y, q1.z);
1505 error = theta * v.y;
1506 }
1507 else
1508 {
1509 error = theta;
1510 }
1511 }
1512 }
1513
1514 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_3)
1515 {
1516 Quat q1 = rotation_q[idx1];
1517 q1 = q1.normalize();
1518 if (idx2 != INVALID)
1519 {
1520 Quat q2 = rotation_q[idx2];
1521 q2 = q2.normalize();
1522 Quat q_error = q2 * q1.inverse();
1523 q_error = q_error.normalize();
1524 Real theta = 2.0 * acos(q_error.w);
1525 if (theta > 1e-6)
1526 {
1527 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q_error.x, q_error.y, q_error.z);
1528 error = theta * v.z;
1529 }
1530 else
1531 {
1532 error = theta;
1533 }
1534 }
1535 else
1536 {
1537 Real theta = 2.0 * acos(q1.w);
1538 if (theta > 1e-6)
1539 {
1540 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q1.x, q1.y, q1.z);
1541 error = theta * v.z;
1542 }
1543 else
1544 {
1545 error = theta;
1546 }
1547 }
1548 }
1549
1550 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN)
1551 {
1552 error = constraints[tId].d_min;
1553 }
1554
1555 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX)
1556 {
1557 error = constraints[tId].d_max;
1558 }
1559
1560 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1)
1561 {
1562 Coord a1 = constraints[tId].axis;
1563 Coord b2 = constraints[tId].pos1;
1564 error = a1.dot(b2);
1565 }
1566
1567 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_2)
1568 {
1569 Coord a1 = constraints[tId].axis;
1570 Coord c2 = constraints[tId].pos2;
1571 error = a1.dot(c2);
1572 }
1573
1574 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
1575 {
1576 Coord errorVec = constraints[tId].normal1;
1577 error = errorVec[0];
1578 }
1579
1580 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_2)
1581 {
1582 Coord errorVec = constraints[tId].normal1;
1583 error = errorVec[1];
1584 }
1585
1586 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_3)
1587 {
1588 Coord errorVec = constraints[tId].normal1;
1589 error = errorVec[2];
1590 }
1591
1592 eta[tId] -= ERP[tId] * invDt * error;
1593 }
1594
1595 void calculateEtaVectorForPJSBaumgarte(
1596 DArray<float> eta,
1597 DArray<Vec3f> J,
1598 DArray<Vec3f> velocity,
1599 DArray<Vec3f> angular_velocity,
1600 DArray<Vec3f> pos,
1601 DArray<Quat1f> rotation_q,
1602 DArray<TConstraintPair<float>> constraints,
1603 DArray<float> errors,
1604 float slop,
1605 float beta,
1606 uint substepping,
1607 float dt
1608 )
1609 {
1610 cuExecute(constraints.size(),
1611 SF_calculateEtaVectorForPJSBaumgarte,
1612 eta,
1613 J,
1614 velocity,
1615 angular_velocity,
1616 pos,
1617 rotation_q,
1618 constraints,
1619 errors,
1620 slop,
1621 beta,
1622 substepping,
1623 dt);
1624 }
1625
1626 void calculateEtaVectorWithERP(
1627 DArray<float> eta,
1628 DArray<Vec3f> J,
1629 DArray<Vec3f> velocity,
1630 DArray<Vec3f> angular_velocity,
1631 DArray<Vec3f> pos,
1632 DArray<Quat1f> rotation_q,
1633 DArray<TConstraintPair<float>> constraints,
1634 DArray<float> ERP,
1635 float slop,
1636 float dt
1637 )
1638 {
1639 cuExecute(constraints.size(),
1640 SF_calculateEtaVectorWithERP,
1641 eta,
1642 J,
1643 velocity,
1644 angular_velocity,
1645 pos,
1646 rotation_q,
1647 constraints,
1648 ERP,
1649 slop,
1650 dt);
1651 }
1652
1653
1654 template<typename Coord, typename Constraint, typename Real>
1655 __global__ void SF_calculateEtaVectorForRelaxation(
1656 DArray<Real> eta,
1657 DArray<Coord> J,
1658 DArray<Coord> velocity,
1659 DArray<Coord> angular_velocity,
1660 DArray<Constraint> constraints
1661 )
1662 {
1663 int tId = threadIdx.x + blockIdx.x * blockDim.x;
1664 if (tId >= constraints.size())
1665 return;
1666
1667 int idx1 = constraints[tId].bodyId1;
1668 int idx2 = constraints[tId].bodyId2;
1669
1670 Real eta_i = Real(0);
1671
1672 eta_i -= J[4 * tId].dot(velocity[idx1]);
1673 eta_i -= J[4 * tId + 1].dot(angular_velocity[idx1]);
1674
1675 if (idx2 != INVALID)
1676 {
1677 eta_i -= J[4 * tId + 2].dot(velocity[idx2]);
1678 eta_i -= J[4 * tId + 3].dot(angular_velocity[idx2]);
1679 }
1680
1681 eta[tId] = eta_i;
1682 }
1683
1684
1685 void calculateEtaVectorForRelaxation(
1686 DArray<float> eta,
1687 DArray<Vec3f> J,
1688 DArray<Vec3f> velocity,
1689 DArray<Vec3f> angular_velocity,
1690 DArray <TConstraintPair<float>> constraints
1691 )
1692 {
1693 cuExecute(constraints.size(),
1694 SF_calculateEtaVectorForRelaxation,
1695 eta,
1696 J,
1697 velocity,
1698 angular_velocity,
1699 constraints);
1700 }
1701
1702 template<typename Coord, typename Constraint, typename Real, typename Quat>
1703 __global__ void SF_calculateEtaVectorForPJSoft(
1704 DArray<Real> eta,
1705 DArray<Coord> J,
1706 DArray<Coord> velocity,
1707 DArray<Coord> angular_velocity,
1708 DArray<Coord> pos,
1709 DArray<Quat> rotation_q,
1710 DArray<Constraint> constraints,
1711 Real slop,
1712 Real zeta,
1713 Real hertz,
1714 Real substepping,
1715 Real dt
1716 )
1717 {
1718 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
1719 if (tId >= constraints.size())
1720 return;
1721
1722 int idx1 = constraints[tId].bodyId1;
1723 int idx2 = constraints[tId].bodyId2;
1724
1725 Real eta_i = Real(0);
1726
1727 eta_i -= J[4 * tId].dot(velocity[idx1]);
1728 eta_i -= J[4 * tId + 1].dot(angular_velocity[idx1]);
1729
1730 if (idx2 != INVALID)
1731 {
1732 eta_i -= J[4 * tId + 2].dot(velocity[idx2]);
1733 eta_i -= J[4 * tId + 3].dot(angular_velocity[idx2]);
1734 }
1735
1736 eta[tId] = eta_i;
1737
1738 Real invDt = Real(1) / dt;
1739 Real error = 0;
1740
1741 if (constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER)
1742 {
1743 Real v_moter = constraints[tId].interpenetration;
1744 eta[tId] -= v_moter;
1745 }
1746
1747 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
1748 {
1749 error = minimum(constraints[tId].interpenetration + slop, 0.0f) / substepping;
1750 }
1751
1752 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1)
1753 {
1754 Coord r1 = constraints[tId].normal1;
1755 Coord r2 = constraints[tId].normal2;
1756 Coord pos1 = constraints[tId].pos1;
1757 Coord errorVec;
1758 if (idx2 != INVALID)
1759 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1760 else
1761 errorVec = pos1 - pos[idx1] - r1;
1762 error = errorVec[0];
1763 }
1764
1765 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_2)
1766 {
1767 Coord r1 = constraints[tId].normal1;
1768 Coord r2 = constraints[tId].normal2;
1769 Coord pos1 = constraints[tId].pos1;
1770 Coord errorVec;
1771 if (idx2 != INVALID)
1772 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1773 else
1774 errorVec = pos1 - pos[idx1] - r1;
1775 error = errorVec[1];
1776 }
1777
1778 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_3)
1779 {
1780 Coord r1 = constraints[tId].normal1;
1781 Coord r2 = constraints[tId].normal2;
1782 Coord pos1 = constraints[tId].pos1;
1783 Coord errorVec;
1784 if (idx2 != INVALID)
1785 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1786 else
1787 errorVec = pos1 - pos[idx1] - r1;
1788 error = errorVec[2];
1789 }
1790
1791 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
1792 {
1793 Coord r1 = constraints[tId].pos1;
1794 Coord r2 = constraints[tId].pos2;
1795 Coord n1 = constraints[tId].normal1;
1796
1797 error = (pos[idx2] + r2 - pos[idx1] - r1).dot(n1);
1798 }
1799
1800 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_2)
1801 {
1802 Coord r1 = constraints[tId].pos1;
1803 Coord r2 = constraints[tId].pos2;
1804 Coord n2 = constraints[tId].normal2;
1805
1806 error = (pos[idx2] + r2 - pos[idx1] - r1).dot(n2);
1807 }
1808
1809 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
1810 {
1811 Quat q1 = rotation_q[idx1];
1812 if (idx2 != INVALID)
1813 {
1814 Quat q2 = rotation_q[idx2];
1815 Quat q_init = constraints[tId].rotQuat;
1816 Quat q_error = q2 * q_init * q1.inverse();
1817
1818 error = q_error.x * 2;
1819 }
1820 else
1821 {
1822 Quat diff = rotation_q[idx1] * constraints[tId].rotQuat.inverse();
1823 error = -diff.x * 2;
1824 }
1825 }
1826
1827 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_2)
1828 {
1829 Quat q1 = rotation_q[idx1];
1830 if (idx2 != INVALID)
1831 {
1832 Quat q2 = rotation_q[idx2];
1833 Quat q_init = constraints[tId].rotQuat;
1834 Quat q_error = q2 * q_init * q1.inverse();
1835
1836 error = q_error.y * 2;
1837 }
1838 else
1839 {
1840 Quat diff = rotation_q[idx1] * constraints[tId].rotQuat.inverse();
1841 error = -diff.y * 2;
1842 }
1843 }
1844
1845 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_3)
1846 {
1847 Quat q1 = rotation_q[idx1];
1848 if (idx2 != INVALID)
1849 {
1850 Quat q2 = rotation_q[idx2];
1851 Quat q_init = constraints[tId].rotQuat;
1852 Quat q_error = q2 * q_init * q1.inverse();
1853
1854 error = q_error.z * 2;
1855 }
1856 else
1857 {
1858 Quat diff = rotation_q[idx1] * constraints[tId].rotQuat.inverse();
1859 error = -diff.z * 2;
1860 }
1861 }
1862
1863 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN)
1864 {
1865 error = constraints[tId].d_min;
1866 }
1867
1868 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX)
1869 {
1870 error = constraints[tId].d_max;
1871 }
1872
1873 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1)
1874 {
1875 Coord a1 = constraints[tId].axis;
1876 Coord b2 = constraints[tId].pos1;
1877 error = a1.dot(b2);
1878 }
1879
1880 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_2)
1881 {
1882 Coord a1 = constraints[tId].axis;
1883 Coord c2 = constraints[tId].pos2;
1884 error = a1.dot(c2);
1885 }
1886
1887 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
1888 {
1889 Coord errorVec = constraints[tId].normal1;
1890 error = errorVec[0];
1891 }
1892
1893 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_2)
1894 {
1895 Coord errorVec = constraints[tId].normal1;
1896 error = errorVec[1];
1897 }
1898
1899 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_3)
1900 {
1901 Coord errorVec = constraints[tId].normal1;
1902 error = errorVec[2];
1903 }
1904
1905 Real omega = 2.0 * M_PI * hertz;
1906 Real a1 = 2.0 * zeta + omega * dt;
1907 Real biasRate = omega / a1;
1908 eta[tId] -= biasRate * error;
1909 }
1910
1911 void calculateEtaVectorForPJSoft(
1912 DArray<float> eta,
1913 DArray<Vec3f> J,
1914 DArray<Vec3f> velocity,
1915 DArray<Vec3f> angular_velocity,
1916 DArray<Vec3f> pos,
1917 DArray<Quat1f> rotation_q,
1918 DArray <TConstraintPair<float>> constraints,
1919 float slop,
1920 float zeta,
1921 float hertz,
1922 float substepping,
1923 float dt
1924 )
1925 {
1926 cuExecute(constraints.size(),
1927 SF_calculateEtaVectorForPJSoft,
1928 eta,
1929 J,
1930 velocity,
1931 angular_velocity,
1932 pos,
1933 rotation_q,
1934 constraints,
1935 slop,
1936 zeta,
1937 hertz,
1938 substepping,
1939 dt);
1940 }
1941
1942
1943 /**
1944 * calculate eta vector for NJS
1945 *
1946 * @param eta eta vector
1947 * @param J Jacobian Matrix
1948 * @param pos position of rigids
1949 * @param rotation_q quaterion of rigids
1950 * @param constraints constraints data
1951 * @param linear slop linear slop
1952 * @param angular slop angular slop
1953 * @param beta bias ratio
1954 * This function calculate the diagonal Matrix of JB
1955 */
1956 template<typename Coord, typename Constraint, typename Real, typename Quat>
1957 __global__ void SF_calculateEtaVectorForNJS(
1958 DArray<Real> eta,
1959 DArray<Coord> J,
1960 DArray<Coord> pos,
1961 DArray<Quat> rotation_q,
1962 DArray<Constraint> constraints,
1963 Real slop,
1964 Real beta
1965 )
1966 {
1967 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
1968 if (tId >= constraints.size())
1969 return;
1970
1971 int idx1 = constraints[tId].bodyId1;
1972 int idx2 = constraints[tId].bodyId2;
1973
1974 Real eta_i = Real(0);
1975
1976 Real error = 0.0f;
1977
1978 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
1979 {
1980 error = minimum(constraints[tId].interpenetration + slop, 0.0f);
1981 }
1982
1983 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1)
1984 {
1985 Coord r1 = constraints[tId].normal1;
1986 Coord r2 = constraints[tId].normal2;
1987 Coord pos1 = constraints[tId].pos1;
1988 Coord errorVec;
1989 if (idx2 != INVALID)
1990 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
1991 else
1992 errorVec = pos1 - pos[idx1] - r1;
1993 error = errorVec[0];
1994 }
1995
1996 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_2)
1997 {
1998 Coord r1 = constraints[tId].normal1;
1999 Coord r2 = constraints[tId].normal2;
2000 Coord pos1 = constraints[tId].pos1;
2001 Coord errorVec;
2002 if (idx2 != INVALID)
2003 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
2004 else
2005 errorVec = pos1 - pos[idx1] - r1;
2006 error = errorVec[1];
2007 }
2008
2009 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_3)
2010 {
2011 Coord r1 = constraints[tId].normal1;
2012 Coord r2 = constraints[tId].normal2;
2013 Coord pos1 = constraints[tId].pos1;
2014 Coord errorVec;
2015 if (idx2 != INVALID)
2016 errorVec = pos[idx2] + r2 - pos[idx1] - r1;
2017 else
2018 errorVec = pos1 - pos[idx1] - r1;
2019 error = errorVec[2];
2020 }
2021
2022 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
2023 {
2024 Coord r1 = constraints[tId].pos1;
2025 Coord r2 = constraints[tId].pos2;
2026 Coord n1 = constraints[tId].normal1;
2027
2028 error = (pos[idx2] + r2 - pos[idx1] - r1).dot(n1);
2029 }
2030
2031 if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_2)
2032 {
2033 Coord r1 = constraints[tId].pos1;
2034 Coord r2 = constraints[tId].pos2;
2035 Coord n2 = constraints[tId].normal2;
2036
2037 error = (pos[idx2] + r2 - pos[idx1] - r1).dot(n2);
2038 }
2039
2040 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
2041 {
2042 Quat q1 = rotation_q[idx1];
2043 q1 = q1.normalize();
2044 if (idx2 != INVALID)
2045 {
2046 Quat q2 = rotation_q[idx2];
2047 q2 = q2.normalize();
2048
2049 Quat q_error = q2 * q1.inverse();
2050 q_error = q_error.normalize();
2051
2052 Real theta = 2.0 * acos(q_error.w);
2053 if (theta > 1e-6)
2054 {
2055 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q_error.x, q_error.y, q_error.z);
2056 error = theta * v.x;
2057 }
2058 else
2059 {
2060 error = theta;
2061 }
2062 }
2063 else
2064 {
2065 Real theta = 2.0 * acos(q1.w);
2066 if (theta > 1e-6)
2067 {
2068 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q1.x, q1.y, q1.z);
2069 error = theta * v.x;
2070 }
2071 else
2072 {
2073 error = theta;
2074 }
2075 }
2076 }
2077
2078 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_2)
2079 {
2080 Quat q1 = rotation_q[idx1];
2081 q1 = q1.normalize();
2082 if (idx2 != INVALID)
2083 {
2084 Quat q2 = rotation_q[idx2];
2085 q2 = q2.normalize();
2086 Quat q_error = q2 * q1.inverse();
2087 q_error = q_error.normalize();
2088 Real theta = 2.0 * acos(q_error.w);
2089 if (theta > 1e-6)
2090 {
2091 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q_error.x, q_error.y, q_error.z);
2092 error = theta * v.y;
2093 }
2094 else
2095 {
2096 error = theta;
2097 }
2098 }
2099 else
2100 {
2101 Real theta = 2.0 * acos(q1.w);
2102 if (theta > 1e-6)
2103 {
2104 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q1.x, q1.y, q1.z);
2105 error = theta * v.y;
2106 }
2107 else
2108 {
2109 error = theta;
2110 }
2111 }
2112 }
2113
2114 if (constraints[tId].type == ConstraintType::CN_BAN_ROT_3)
2115 {
2116 Quat q1 = rotation_q[idx1];
2117 q1 = q1.normalize();
2118 if (idx2 != INVALID)
2119 {
2120 Quat q2 = rotation_q[idx2];
2121 q2 = q2.normalize();
2122 Quat q_error = q2 * q1.inverse();
2123 q_error = q_error.normalize();
2124 Real theta = 2.0 * acos(q_error.w);
2125 if (theta > 1e-6)
2126 {
2127 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q_error.x, q_error.y, q_error.z);
2128 error = theta * v.z;
2129 }
2130 else
2131 {
2132 error = theta;
2133 }
2134 }
2135 else
2136 {
2137 Real theta = 2.0 * acos(q1.w);
2138 if (theta > 1e-6)
2139 {
2140 Vec3f v = (1 / sin(theta / 2.0)) * Vec3f(q1.x, q1.y, q1.z);
2141 error = theta * v.z;
2142 }
2143 else
2144 {
2145 error = theta;
2146 }
2147 }
2148 }
2149
2150 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN)
2151 {
2152 error = constraints[tId].d_min;
2153 }
2154
2155 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX)
2156 {
2157 error = constraints[tId].d_max;
2158 }
2159
2160 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1)
2161 {
2162 Coord a1 = constraints[tId].axis;
2163 Coord b2 = constraints[tId].pos1;
2164 error = a1.dot(b2);
2165 }
2166
2167 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_2)
2168 {
2169 Coord a1 = constraints[tId].axis;
2170 Coord c2 = constraints[tId].pos2;
2171 error = a1.dot(c2);
2172 }
2173
2174 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
2175 {
2176 Coord errorVec = constraints[tId].normal1;
2177 error = errorVec[0];
2178 }
2179
2180 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_2)
2181 {
2182 Coord errorVec = constraints[tId].normal1;
2183 error = errorVec[1];
2184 }
2185
2186 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_3)
2187 {
2188 Coord errorVec = constraints[tId].normal1;
2189 error = errorVec[2];
2190 }
2191
2192 eta[tId] = eta_i - beta * error;
2193 }
2194
2195 void calculateEtaVectorForNJS(
2196 DArray<float> eta,
2197 DArray<Vec3f> J,
2198 DArray<Vec3f> pos,
2199 DArray<Quat1f> rotation_q,
2200 DArray <TConstraintPair<float>> constraints,
2201 float slop,
2202 float beta
2203 )
2204 {
2205 cuExecute(constraints.size(),
2206 SF_calculateEtaVectorForNJS,
2207 eta,
2208 J,
2209 pos,
2210 rotation_q,
2211 constraints,
2212 slop,
2213 beta);
2214 }
2215
2216 /**
2217 * Store the contacts in local coordinates.
2218 *
2219 * @param contactsInLocalFrame contacts in local coordinates
2220 * @param contactsInGlobalFrame contacts in global coordinates
2221 * @param pos position of rigids
2222 * @param rotMat rotation matrix of rigids
2223 * This function store the contacts in local coordinates.
2224 */
2225 template<typename Contact, typename Coord, typename Matrix>
2226 __global__ void SF_setUpContactsInLocalFrame(
2227 DArray<Contact> contactsInLocalFrame,
2228 DArray<Contact> contactsInGlobalFrame,
2229 DArray<Coord> pos,
2230 DArray<Matrix> rotMat
2231 )
2232 {
2233 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
2234 if (tId >= contactsInGlobalFrame.size())
2235 return;
2236
2237 Contact globalC = contactsInGlobalFrame[tId];
2238 int idx1 = globalC.bodyId1;
2239 int idx2 = globalC.bodyId2;
2240
2241 Contact localC;
2242 localC.bodyId1 = idx1;
2243 localC.bodyId2 = idx2;
2244
2245 localC.interpenetration = -globalC.interpenetration;
2246 localC.contactType = globalC.contactType;
2247
2248 Coord c1 = pos[idx1];
2249 Matrix rot1 = rotMat[idx1];
2250
2251 if (idx2 != INVALID)
2252 {
2253 Coord c2 = pos[idx2];
2254 Matrix rot2 = rotMat[idx2];
2255 localC.pos1 = rot1.transpose() * (globalC.pos1 - c1);
2256 localC.normal1 = -globalC.normal1;
2257 localC.pos2 = rot2.transpose() * (globalC.pos1 - c2);
2258 localC.normal2 = globalC.normal1;
2259 }
2260 else
2261 {
2262 localC.pos1 = rot1.transpose() * (globalC.pos1 - c1);
2263 localC.normal1 = - globalC.normal1;
2264 localC.pos2 = globalC.pos1;
2265 localC.normal2 = globalC.normal1;
2266 }
2267 contactsInLocalFrame[tId] = localC;
2268 }
2269
2270 void setUpContactsInLocalFrame(
2271 DArray<TContactPair<float>> contactsInLocalFrame,
2272 DArray<TContactPair<float>> contactsInGlobalFrame,
2273 DArray<Vec3f> pos,
2274 DArray<Mat3f> rotMat
2275 )
2276 {
2277 cuExecute(contactsInGlobalFrame.size(),
2278 SF_setUpContactsInLocalFrame,
2279 contactsInLocalFrame,
2280 contactsInGlobalFrame,
2281 pos,
2282 rotMat);
2283 }
2284
2285 /**
2286 * Set up the contact and friction constraints
2287 *
2288 * @param constraints constraints data
2289 * @param contactsInLocalFrame contacts in local coordinates
2290 * @param pos position of rigids
2291 * @param rotMat rotation matrix of rigids
2292 * @param hasFriction friction choice
2293 * This function set up the contact and friction constraints
2294 */
2295 template<typename Coord, typename Matrix, typename Contact, typename Constraint>
2296 __global__ void SF_setUpContactAndFrictionConstraints(
2297 DArray<Constraint> constraints,
2298 DArray<Contact> contactsInLocalFrame,
2299 DArray<Coord> pos,
2300 DArray<Matrix> rotMat,
2301 bool hasFriction
2302 )
2303 {
2304 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
2305 if (tId >= contactsInLocalFrame.size())
2306 return;
2307
2308 int contact_size = contactsInLocalFrame.size();
2309
2310 int idx1 = contactsInLocalFrame[tId].bodyId1;
2311 int idx2 = contactsInLocalFrame[tId].bodyId2;
2312
2313 Coord c1 = pos[idx1];
2314 Matrix rot1 = rotMat[idx1];
2315
2316 constraints[tId].bodyId1 = idx1;
2317 constraints[tId].bodyId2 = idx2;
2318 constraints[tId].pos1 = rot1 * contactsInLocalFrame[tId].pos1 + c1;
2319 constraints[tId].normal1 = contactsInLocalFrame[tId].normal1;
2320
2321 if (idx2 != INVALID)
2322 {
2323 Coord c2 = pos[idx2];
2324 Matrix rot2 = rotMat[idx2];
2325 constraints[tId].pos2 = rot2 * contactsInLocalFrame[tId].pos2 + c2;
2326 constraints[tId].normal2 = contactsInLocalFrame[tId].normal2;
2327 }
2328 else
2329 {
2330 constraints[tId].pos2 = contactsInLocalFrame[tId].pos2;
2331 constraints[tId].normal2 = contactsInLocalFrame[tId].normal2;
2332 }
2333
2334 constraints[tId].interpenetration = minimum(contactsInLocalFrame[tId].interpenetration + (constraints[tId].pos2 - constraints[tId].pos1).dot(contactsInLocalFrame[tId].normal1), 0.0f);
2335 constraints[tId].type = ConstraintType::CN_NONPENETRATION;
2336 constraints[tId].isValid = true;
2337
2338 if (hasFriction)
2339 {
2340 Coord n = constraints[tId].normal1;
2341 n = n.normalize();
2342
2343 Coord u1, u2;
2344
2345 if (abs(n[1]) > EPSILON || abs(n[2]) > EPSILON)
2346 {
2347 u1 = Vector<Real, 3>(0, n[2], -n[1]);
2348 u1 = u1.normalize();
2349 }
2350 else if (abs(n[0]) > EPSILON)
2351 {
2352 u1 = Vector<Real, 3>(n[2], 0, -n[0]);
2353 u1 = u1.normalize();
2354 }
2355
2356 u2 = u1.cross(n);
2357 u2 = u2.normalize();
2358
2359 constraints[tId * 2 + contact_size].bodyId1 = idx1;
2360 constraints[tId * 2 + contact_size].bodyId2 = idx2;
2361 constraints[tId * 2 + contact_size].pos1 = constraints[tId].pos1;
2362 constraints[tId * 2 + contact_size].pos2 = constraints[tId].pos2;
2363 constraints[tId * 2 + contact_size].normal1 = u1;
2364 constraints[tId * 2 + contact_size].normal2 = -u1;
2365 constraints[tId * 2 + contact_size].type = ConstraintType::CN_FRICTION;
2366 constraints[tId * 2 + contact_size].isValid = true;
2367
2368 constraints[tId * 2 + 1 + contact_size].bodyId1 = idx1;
2369 constraints[tId * 2 + 1 + contact_size].bodyId2 = idx2;
2370 constraints[tId * 2 + 1 + contact_size].pos1 = constraints[tId].pos1;
2371 constraints[tId * 2 + 1 + contact_size].pos2 = constraints[tId].pos2;
2372 constraints[tId * 2 + 1 + contact_size].normal1 = u2;
2373 constraints[tId * 2 + 1 + contact_size].normal2 = -u2;
2374 constraints[tId * 2 + 1 + contact_size].type = ConstraintType::CN_FRICTION;
2375 constraints[tId * 2 + 1 + contact_size].isValid = true;
2376 }
2377 }
2378
2379 void setUpContactAndFrictionConstraints(
2380 DArray<TConstraintPair<float>> constraints,
2381 DArray<TContactPair<float>> contactsInLocalFrame,
2382 DArray<Vec3f> pos,
2383 DArray<Mat3f> rotMat,
2384 bool hasFriction
2385 )
2386 {
2387 cuExecute(constraints.size(),
2388 SF_setUpContactAndFrictionConstraints,
2389 constraints,
2390 contactsInLocalFrame,
2391 pos,
2392 rotMat,
2393 hasFriction);
2394 }
2395
2396 /**
2397 * Set up the contact constraints
2398 *
2399 * @param constraints constraints data
2400 * @param contactsInLocalFrame contacts in local coordinates
2401 * @param pos position of rigids
2402 * @param rotMat rotation matrix of rigids
2403 * This function set up the contact constraints
2404 */
2405 template<typename Coord, typename Matrix, typename Contact, typename Constraint>
2406 __global__ void SF_setUpContactConstraints(
2407 DArray<Constraint> constraints,
2408 DArray<Contact> contactsInLocalFrame,
2409 DArray<Coord> pos,
2410 DArray<Matrix> rotMat
2411 )
2412 {
2413 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
2414 if (tId >= contactsInLocalFrame.size())
2415 return;
2416
2417 int contact_size = contactsInLocalFrame.size();
2418
2419 int idx1 = contactsInLocalFrame[tId].bodyId1;
2420 int idx2 = contactsInLocalFrame[tId].bodyId2;
2421
2422 Coord c1 = pos[idx1];
2423 Matrix rot1 = rotMat[idx1];
2424
2425 constraints[tId].bodyId1 = idx1;
2426 constraints[tId].bodyId2 = idx2;
2427 constraints[tId].pos1 = rot1 * contactsInLocalFrame[tId].pos1 + c1;
2428 constraints[tId].normal1 = contactsInLocalFrame[tId].normal1;
2429
2430 if (idx2 != INVALID)
2431 {
2432 Coord c2 = pos[idx2];
2433 Matrix rot2 = rotMat[idx2];
2434
2435 constraints[tId].pos2 = rot2 * contactsInLocalFrame[tId].pos2 + c2;
2436 constraints[tId].normal2 = contactsInLocalFrame[tId].normal2;
2437 constraints[tId].interpenetration = (constraints[tId].pos2 - constraints[tId].pos1).dot(constraints[tId].normal1) + contactsInLocalFrame[tId].interpenetration;
2438 }
2439 else
2440 {
2441 Real dist = (contactsInLocalFrame[tId].pos2 - constraints[tId].pos1).dot(constraints[tId].normal1) + contactsInLocalFrame[tId].interpenetration;
2442 constraints[tId].interpenetration = dist;
2443 }
2444
2445 constraints[tId].type = ConstraintType::CN_NONPENETRATION;
2446 }
2447
2448 void setUpContactConstraints(
2449 DArray<TConstraintPair<float>> constraints,
2450 DArray<TContactPair<float>> contactsInLocalFrame,
2451 DArray<Vec3f> pos,
2452 DArray<Mat3f> rotMat
2453 )
2454 {
2455 cuExecute(constraints.size(),
2456 SF_setUpContactConstraints,
2457 constraints,
2458 contactsInLocalFrame,
2459 pos,
2460 rotMat);
2461 }
2462
2463 /**
2464 * Set up the ball and socket constraints
2465 *
2466 * @param constraints constraints data
2467 * @param joints joints data
2468 * @param pos position of rigids
2469 * @param rotMat rotation matrix of rigids
2470 * @param begin_index begin index of ball and socket joints constraints in array
2471 * This function set up the ball and socket joint constraints
2472 */
2473 template<typename Joint, typename Constraint, typename Coord, typename Matrix>
2474 __global__ void SF_setUpBallAndSocketJointConstraints(
2475 DArray<Constraint> constraints,
2476 DArray<Joint> joints,
2477 DArray<Coord> pos,
2478 DArray<Matrix> rotMat,
2479 int begin_index
2480 )
2481 {
2482 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
2483
2484 if (tId >= joints.size())
2485 return;
2486
2487 int idx1 = joints[tId].bodyId1;
2488 int idx2 = joints[tId].bodyId2;
2489
2490 Coord r1 = rotMat[idx1] * joints[tId].r1;
2491 Coord r2 = rotMat[idx2] * joints[tId].r2;
2492
2493 int baseIndex = 3 * tId + begin_index;
2494
2495 constraints[baseIndex].bodyId1 = idx1;
2496 constraints[baseIndex].bodyId2 = idx2;
2497 constraints[baseIndex].normal1 = r1;
2498 constraints[baseIndex].normal2 = r2;
2499 constraints[baseIndex].type = ConstraintType::CN_ANCHOR_EQUAL_1;
2500 constraints[baseIndex].isValid = true;
2501
2502 constraints[baseIndex + 1].bodyId1 = idx1;
2503 constraints[baseIndex + 1].bodyId2 = idx2;
2504 constraints[baseIndex + 1].normal1 = r1;
2505 constraints[baseIndex + 1].normal2 = r2;
2506 constraints[baseIndex + 1].type = ConstraintType::CN_ANCHOR_EQUAL_2;
2507 constraints[baseIndex + 1].isValid = true;
2508
2509 constraints[baseIndex + 2].bodyId1 = idx1;
2510 constraints[baseIndex + 2].bodyId2 = idx2;
2511 constraints[baseIndex + 2].normal1 = r1;
2512 constraints[baseIndex + 2].normal2 = r2;
2513 constraints[baseIndex + 2].type = ConstraintType::CN_ANCHOR_EQUAL_3;
2514 constraints[baseIndex + 2].isValid = true;
2515 }
2516
2517 void setUpBallAndSocketJointConstraints(
2518 DArray<TConstraintPair<float>> constraints,
2519 DArray<BallAndSocketJoint<float>> joints,
2520 DArray<Vec3f> pos,
2521 DArray<Mat3f> rotMat,
2522 int begin_index
2523 )
2524 {
2525 cuExecute(constraints.size(),
2526 SF_setUpBallAndSocketJointConstraints,
2527 constraints,
2528 joints,
2529 pos,
2530 rotMat,
2531 begin_index);
2532 }
2533
2534 /**
2535 * Set up the slider constraints
2536 *
2537 * @param constraints constraints data
2538 * @param joints joints data
2539 * @param pos position of rigids
2540 * @param rotMat rotation matrix of rigids
2541 * @param begin_index begin index of slider constraints in array
2542 * This function set up the slider joint constraints
2543 */
2544 template<typename Joint, typename Constraint, typename Coord, typename Matrix, typename Quat>
2545 __global__ void SF_setUpSliderJointConstraints(
2546 DArray<Constraint> constraints,
2547 DArray<Joint> joints,
2548 DArray<Coord> pos,
2549 DArray<Matrix> rotMat,
2550 DArray<Quat> rotQuat,
2551 int begin_index
2552 )
2553 {
2554 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
2555
2556 if (tId >= joints.size())
2557 return;
2558
2559 int constraint_size = 8;
2560
2561 int idx1 = joints[tId].bodyId1;
2562 int idx2 = joints[tId].bodyId2;
2563
2564 Coord r1 = rotMat[idx1] * joints[tId].r1;
2565 Coord r2 = rotMat[idx2] * joints[tId].r2;
2566
2567 Coord n = rotMat[idx1] * joints[tId].sliderAxis;
2568 n = n.normalize();
2569 Coord n1, n2;
2570 if (abs(n[1]) > EPSILON || abs(n[2]) > EPSILON)
2571 {
2572 n1 = Coord(0, n[2], -n[1]);
2573 n1 = n1.normalize();
2574 }
2575 else if (abs(n[0]) > EPSILON)
2576 {
2577 n1 = Coord(n[2], 0, -n[0]);
2578 n1 = n1.normalize();
2579 }
2580 n2 = n1.cross(n);
2581 n2 = n2.normalize();
2582
2583 int baseIndex = constraint_size * tId + begin_index;
2584
2585 for (int i = 0; i < 5; i++)
2586 {
2587 constraints[baseIndex + i].isValid = true;
2588 }
2589
2590
2591 bool useRange = joints[tId].useRange;
2592 Real C_min = 0.0;
2593 Real C_max = 0.0;
2594 if (joints[tId].useRange)
2595 {
2596 Coord u = pos[idx2] + r2 - pos[idx1] - r1;
2597 C_min = u.dot(n) - joints[tId].d_min;
2598 C_max = joints[tId].d_max - u.dot(n);
2599 if (C_min < 0)
2600 constraints[baseIndex + 5].isValid = true;
2601 else
2602 constraints[baseIndex + 5].isValid = false;
2603 if (C_max < 0)
2604 constraints[baseIndex + 6].isValid = true;
2605 else
2606 constraints[baseIndex + 6].isValid = false;
2607 }
2608 else
2609 {
2610 constraints[baseIndex + 5].isValid = false;
2611 constraints[baseIndex + 6].isValid = false;
2612 }
2613
2614 Real v_moter = 0.0;
2615 bool useMoter = joints[tId].useMoter;
2616 if (useMoter)
2617 {
2618 v_moter = joints[tId].v_moter;
2619 constraints[baseIndex + 7].isValid = true;
2620 }
2621 else
2622 {
2623 constraints[baseIndex + 7].isValid = false;
2624 }
2625
2626 for (int i = 0; i < constraint_size; i++)
2627 {
2628 auto& constraint = constraints[baseIndex + i];
2629 constraint.bodyId1 = idx1;
2630 constraint.bodyId2 = idx2;
2631 constraint.pos1 = r1;
2632 constraint.pos2 = r2;
2633 constraint.normal1 = n1;
2634 constraint.normal2 = n2;
2635 constraint.axis = n;
2636 constraint.interpenetration = v_moter;
2637 constraint.d_min = C_min;
2638 constraint.d_max = C_max;
2639 constraint.rotQuat = joints[tId].q_init;
2640 }
2641
2642 constraints[baseIndex].type = ConstraintType::CN_ANCHOR_TRANS_1;
2643 constraints[baseIndex + 1].type = ConstraintType::CN_ANCHOR_TRANS_2;
2644 constraints[baseIndex + 2].type = ConstraintType::CN_BAN_ROT_1;
2645 constraints[baseIndex + 3].type = ConstraintType::CN_BAN_ROT_2;
2646 constraints[baseIndex + 4].type = ConstraintType::CN_BAN_ROT_3;
2647 constraints[baseIndex + 5].type = ConstraintType::CN_JOINT_SLIDER_MIN;
2648 constraints[baseIndex + 6].type = ConstraintType::CN_JOINT_SLIDER_MAX;
2649 constraints[baseIndex + 7].type = ConstraintType::CN_JOINT_SLIDER_MOTER;
2650 }
2651
2652 void setUpSliderJointConstraints(
2653 DArray<TConstraintPair<float>> constraints,
2654 DArray<SliderJoint<float>> joints,
2655 DArray<Vec3f> pos,
2656 DArray<Mat3f> rotMat,
2657 DArray<Quat1f> rotQuat,
2658 int begin_index
2659 )
2660 {
2661 cuExecute(constraints.size(),
2662 SF_setUpSliderJointConstraints,
2663 constraints,
2664 joints,
2665 pos,
2666 rotMat,
2667 rotQuat,
2668 begin_index);
2669 }
2670
2671 /**
2672 * Set up the hinge constraints
2673 *
2674 * @param constraints constraints data
2675 * @param joints joints data
2676 * @param pos position of rigids
2677 * @param rotMat rotation matrix of rigids
2678 * @oaran rotation_q quaterion of rigids
2679 * @param begin_index begin index of hinge constraints in array
2680 * This function set up the hinge joint constraints
2681 */
2682 template<typename Joint, typename Constraint, typename Coord, typename Matrix, typename Quat>
2683 __global__ void SF_setUpHingeJointConstraints(
2684 DArray<Constraint> constraints,
2685 DArray<Joint> joints,
2686 DArray<Coord> pos,
2687 DArray<Matrix> rotMat,
2688 DArray<Quat> rotation_q,
2689 int begin_index
2690 )
2691 {
2692 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
2693 if (tId >= joints.size())
2694 return;
2695
2696 int constraint_size = 8;
2697
2698 int idx1 = joints[tId].bodyId1;
2699 int idx2 = joints[tId].bodyId2;
2700
2701 Matrix rotMat1 = rotMat[idx1];
2702 Matrix rotMat2 = rotMat[idx2];
2703
2704
2705 Coord r1 = rotMat1 * joints[tId].r1;
2706 Coord r2 = rotMat2 * joints[tId].r2;
2707
2708 Coord a1 = rotMat1 * joints[tId].hingeAxisBody1;
2709 Coord a2 = rotMat2 * joints[tId].hingeAxisBody2;
2710
2711
2712
2713
2714 // two vector orthogonal to the a2
2715 Coord b2, c2;
2716 if (abs(a2[1]) > EPSILON || abs(a2[2]) > EPSILON)
2717 {
2718 b2 = Coord(0, a2[2], -a2[1]);
2719 b2 = b2.normalize();
2720 }
2721 else if (abs(a2[0]) > EPSILON)
2722 {
2723 b2 = Coord(a2[2], 0, -a2[0]);
2724 b2 = b2.normalize();
2725 }
2726 c2 = b2.cross(a2);
2727 c2 = c2.normalize();
2728
2729 Real C_min = 0.0;
2730 Real C_max = 0.0;
2731 int baseIndex = tId * constraint_size + begin_index;
2732
2733 if (joints[tId].useRange)
2734 {
2735 Real theta = rotation_q[idx2].angle(rotation_q[idx1]);
2736
2737 Quat q_rot = rotation_q[idx2] * rotation_q[idx1].inverse();
2738
2739 if (a1.dot(Coord(q_rot.x, q_rot.y, q_rot.z)) < 0)
2740 {
2741 theta = -theta;
2742 }
2743
2744
2745 C_min = theta - joints[tId].d_min;
2746 C_max = joints[tId].d_max - theta;
2747
2748 if (C_min < 0)
2749 {
2750 constraints[baseIndex + 5].isValid = true;
2751 }
2752 else
2753 constraints[baseIndex + 5].isValid = false;
2754
2755 if (C_max < 0)
2756 {
2757 constraints[baseIndex + 6].isValid = true;
2758 }
2759 else
2760 constraints[baseIndex + 6].isValid = false;
2761 }
2762
2763 else
2764 {
2765 constraints[baseIndex + 5].isValid = false;
2766 constraints[baseIndex + 6].isValid = false;
2767 }
2768
2769 Real v_moter = 0.0;
2770 if (joints[tId].useMoter)
2771 {
2772 v_moter = joints[tId].v_moter;
2773 constraints[baseIndex + 7].isValid = true;
2774 }
2775 else
2776 constraints[baseIndex + 7].isValid = false;
2777
2778 for (int i = 0; i < constraint_size; i++)
2779 {
2780 constraints[baseIndex + i].bodyId1 = idx1;
2781 constraints[baseIndex + i].bodyId2 = idx2;
2782 constraints[baseIndex + i].axis = a1;
2783 constraints[baseIndex + i].normal1 = r1;
2784 constraints[baseIndex + i].normal2 = r2;
2785 constraints[baseIndex + i].pos1 = b2;
2786 constraints[baseIndex + i].pos2 = c2;
2787 constraints[baseIndex + i].d_min = C_min > 0 ? 0 : C_min;
2788 constraints[baseIndex + i].d_max = C_max > 0 ? 0 : C_max;
2789 constraints[baseIndex + i].interpenetration = v_moter;
2790 }
2791
2792 for (int i = 0; i < 5; i++)
2793 {
2794 constraints[baseIndex + i].isValid = true;
2795 }
2796
2797 constraints[baseIndex].type = ConstraintType::CN_ANCHOR_EQUAL_1;
2798 constraints[baseIndex + 1].type = ConstraintType::CN_ANCHOR_EQUAL_2;
2799 constraints[baseIndex + 2].type = ConstraintType::CN_ANCHOR_EQUAL_3;
2800 constraints[baseIndex + 3].type = ConstraintType::CN_ALLOW_ROT1D_1;
2801 constraints[baseIndex + 4].type = ConstraintType::CN_ALLOW_ROT1D_2;
2802 constraints[baseIndex + 5].type = ConstraintType::CN_JOINT_HINGE_MIN;
2803 constraints[baseIndex + 6].type = ConstraintType::CN_JOINT_HINGE_MAX;
2804 constraints[baseIndex + 7].type = ConstraintType::CN_JOINT_HINGE_MOTER;
2805 }
2806
2807 void setUpHingeJointConstraints(
2808 DArray<TConstraintPair<float>> constraints,
2809 DArray<HingeJoint<float>> joints,
2810 DArray<Vec3f> pos,
2811 DArray<Mat3f> rotMat,
2812 DArray<Quat1f> rotation_q,
2813 int begin_index
2814 )
2815 {
2816 cuExecute(constraints.size(),
2817 SF_setUpHingeJointConstraints,
2818 constraints,
2819 joints,
2820 pos,
2821 rotMat,
2822 rotation_q,
2823 begin_index);
2824 }
2825
2826 /**
2827 * Set up the fixed joint constraints
2828 *
2829 * @param constraints constraints data
2830 * @param joints joints data
2831 * @param rotMat rotation matrix of rigids
2832 * @param begin_index begin index of fixed constraints in array
2833 * This function set up the fixed joint constraints
2834 */
2835 template<typename Joint, typename Constraint, typename Matrix, typename Quat>
2836 __global__ void SF_setUpFixedJointConstraints(
2837 DArray<Constraint> constraints,
2838 DArray<Joint> joints,
2839 DArray<Matrix> rotMat,
2840 DArray<Quat> rotQuat,
2841 int begin_index
2842 )
2843 {
2844 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
2845
2846 if (tId >= joints.size())
2847 return;
2848
2849 int idx1 = joints[tId].bodyId1;
2850 int idx2 = joints[tId].bodyId2;
2851 Vector<Real, 3> r1 = rotMat[idx1] * joints[tId].r1;
2852 Vector<Real, 3> r2;
2853 if (idx2 != INVALID)
2854 {
2855 r2 = rotMat[idx2] * joints[tId].r2;
2856 }
2857
2858 int baseIndex = 6 * tId + begin_index;
2859 for (int i = 0; i < 6; i++)
2860 {
2861 constraints[baseIndex + i].bodyId1 = idx1;
2862 constraints[baseIndex + i].bodyId2 = idx2;
2863 constraints[baseIndex + i].normal1 = r1;
2864 constraints[baseIndex + i].normal2 = r2;
2865 constraints[baseIndex + i].pos1 = joints[tId].w;
2866 constraints[baseIndex + i].rotQuat = joints[tId].q_init;
2867 constraints[baseIndex + i].isValid = true;
2868 }
2869
2870 constraints[baseIndex].type = ConstraintType::CN_ANCHOR_EQUAL_1;
2871 constraints[baseIndex + 1].type = ConstraintType::CN_ANCHOR_EQUAL_2;
2872 constraints[baseIndex + 2].type = ConstraintType::CN_ANCHOR_EQUAL_3;
2873 constraints[baseIndex + 3].type = ConstraintType::CN_BAN_ROT_1;
2874 constraints[baseIndex + 4].type = ConstraintType::CN_BAN_ROT_2;
2875 constraints[baseIndex + 5].type = ConstraintType::CN_BAN_ROT_3;
2876 }
2877
2878 void setUpFixedJointConstraints(
2879 DArray<TConstraintPair<float>> &constraints,
2880 DArray<FixedJoint<float>> &joints,
2881 DArray<Mat3f> &rotMat,
2882 DArray<Quat1f> &rotQuat,
2883 int begin_index
2884 )
2885 {
2886 cuExecute(constraints.size(),
2887 SF_setUpFixedJointConstraints,
2888 constraints,
2889 joints,
2890 rotMat,
2891 rotQuat,
2892 begin_index);
2893 }
2894
2895 /**
2896 * Set up the point joint constraints
2897 *
2898 * @param constraints constraints data
2899 * @param joints joints data
2900 * @param begin_index begin index of fixed constraints in array
2901 * This function set up the point joint constraints
2902 */
2903 template<typename Joint, typename Constraint, typename Coord>
2904 __global__ void SF_setUpPointJointConstraints(
2905 DArray<Constraint> constraints,
2906 DArray<Joint> joints,
2907 DArray<Coord> pos,
2908 int begin_index
2909 )
2910 {
2911 int tId = threadIdx.x + blockDim.x * blockIdx.x;
2912 if (tId >= joints.size())
2913 return;
2914
2915 int idx1 = joints[tId].bodyId1;
2916 int idx2 = joints[tId].bodyId2;
2917
2918 int baseIndex = 3 * tId + begin_index;
2919
2920 for (int i = 0; i < 3; i++)
2921 {
2922 constraints[baseIndex + i].bodyId1 = idx1;
2923 constraints[baseIndex + i].bodyId2 = idx2;
2924 constraints[baseIndex + i].normal1 = pos[idx1] - joints[tId].anchorPoint;
2925 constraints[baseIndex + i].isValid = true;
2926 }
2927
2928 constraints[baseIndex].type = ConstraintType::CN_JOINT_NO_MOVE_1;
2929 constraints[baseIndex + 1].type = ConstraintType::CN_JOINT_NO_MOVE_2;
2930 constraints[baseIndex + 2].type = ConstraintType::CN_JOINT_NO_MOVE_3;
2931 }
2932
2933 void setUpPointJointConstraints(
2934 DArray<TConstraintPair<float>> constraints,
2935 DArray<PointJoint<float>> joints,
2936 DArray<Vec3f> pos,
2937 int begin_index
2938 )
2939 {
2940 cuExecute(constraints.size(),
2941 SF_setUpPointJointConstraints,
2942 constraints,
2943 joints,
2944 pos,
2945 begin_index);
2946 }
2947
2948
2949 template<typename Coord, typename Constraint, typename Matrix>
2950 __global__ void SF_calculateK(
2951 DArray<Constraint> constraints,
2952 DArray<Coord> J,
2953 DArray<Coord> B,
2954 DArray<Coord> pos,
2955 DArray<Matrix> inertia,
2956 DArray<Real> mass,
2957 DArray<Real> K_1,
2958 DArray<Mat2f> K_2,
2959 DArray<Matrix> K_3
2960 )
2961 {
2962 int tId = threadIdx.x + blockIdx.x * blockDim.x;
2963 if (tId >= constraints.size())
2964 return;
2965
2966 int idx1 = constraints[tId].bodyId1;
2967 int idx2 = constraints[tId].bodyId2;
2968
2969 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1)
2970 {
2971 Matrix E(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
2972 Coord r1 = constraints[tId].normal1;
2973 Matrix r1x(0.0f, -r1[2], r1[1], r1[2], 0, -r1[0], -r1[1], r1[0], 0);
2974 if (idx2 != INVALID)
2975 {
2976 Coord r2 = constraints[tId].normal2;
2977 Matrix r2x(0.0f, -r2[2], r2[1], r2[2], 0, -r2[0], -r2[1], r2[0], 0);
2978 Matrix K = (1 / mass[idx1]) * E + (r1x * inertia[idx1].inverse()) * r1x.transpose() + (1 / mass[idx2]) * E + (r2x * inertia[idx2].inverse()) * r2x.transpose();
2979 K_3[tId] = K.inverse();
2980 }
2981 else
2982 {
2983 Matrix K = (1 / mass[idx1]) * E + (r1x * inertia[idx1].inverse()) * r1x.transpose();
2984 K_3[tId] = K.inverse();
2985 }
2986
2987 }
2988
2989 else if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1)
2990 {
2991 Coord a1 = constraints[tId].axis;
2992 Coord b2 = constraints[tId].pos1;
2993 Coord c2 = constraints[tId].pos2;
2994 Coord b2_c_a1 = b2.cross(a1);
2995 Coord c2_c_a1 = c2.cross(a1);
2996 Real a = b2_c_a1.dot(inertia[idx1].inverse() * b2_c_a1) + b2_c_a1.dot(inertia[idx2].inverse() * b2_c_a1);
2997 Real b = b2_c_a1.dot(inertia[idx1].inverse() * c2_c_a1) + b2_c_a1.dot(inertia[idx2].inverse() * c2_c_a1);
2998 Real c = c2_c_a1.dot(inertia[idx1].inverse() * b2_c_a1) + c2_c_a1.dot(inertia[idx2].inverse() * b2_c_a1);
2999 Real d = c2_c_a1.dot(inertia[idx1].inverse() * c2_c_a1) + c2_c_a1.dot(inertia[idx2].inverse() * c2_c_a1);
3000 Mat2f K(a, b, c, d);
3001 K_2[tId] = K.inverse();
3002 }
3003
3004 else if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
3005 {
3006 Matrix K = (1 / mass[idx1]) * Matrix(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
3007 K_3[tId] = K.inverse();
3008 }
3009
3010 else if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
3011 {
3012 Coord r1 = constraints[tId].pos1;
3013 Coord r2 = constraints[tId].pos2;
3014 Coord n1 = constraints[tId].normal1;
3015 Coord n2 = constraints[tId].normal2;
3016 Coord u = pos[idx2] + r2 - pos[idx1] - r1;
3017 Coord r1u_c_n1 = (r1 + u).cross(n1);
3018 Coord r1u_c_n2 = (r1 + u).cross(n2);
3019 Coord r2_c_n1 = r2.cross(n1);
3020 Coord r2_c_n2 = r2.cross(n2);
3021 Real a = 1 / mass[idx1] + 1 / mass[idx2] + r1u_c_n1.dot(inertia[idx1].inverse() * r1u_c_n1) + r2_c_n1.dot(inertia[idx2].inverse() * r2_c_n1);
3022 Real b = r1u_c_n1.dot(inertia[idx1].inverse() * r1u_c_n2) + r2_c_n1.dot(inertia[idx2].inverse() * r2_c_n2);
3023 Real c = r1u_c_n2.dot(inertia[idx1].inverse() * r1u_c_n1) + r2_c_n2.dot(inertia[idx2].inverse() * r2_c_n1);
3024 Real d = 1 / mass[idx1] + 1 / mass[idx2] + r1u_c_n2.dot(inertia[idx1].inverse() * r1u_c_n2) + r2_c_n2.dot(inertia[idx2].inverse() * r2_c_n2);
3025 Mat2f K(a, b, c, d);
3026 K_2[tId] = K.inverse();
3027
3028 }
3029
3030 else if (constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
3031 {
3032 if (idx2 != INVALID)
3033 {
3034 Matrix K = inertia[idx1].inverse() + inertia[idx2].inverse();
3035 K_3[tId] = K.inverse();
3036 }
3037 else
3038 {
3039 Matrix K = inertia[idx1].inverse();
3040 K_3[tId] = K.inverse();
3041 }
3042 }
3043
3044 else
3045 {
3046 if (constraints[tId].isValid)
3047 {
3048 Real K = J[4 * tId].dot(B[4 * tId]) + J[4 * tId + 1].dot(B[4 * tId + 1]) + J[4 * tId + 2].dot(B[4 * tId + 2]) + J[4 * tId + 3].dot(B[4 * tId + 3]);
3049 K_1[tId] = 1 / K;
3050 }
3051 }
3052 }
3053
3054 template<typename Coord, typename Constraint, typename Matrix>
3055 __global__ void SF_calculateKWithCFM(
3056 DArray<Constraint> constraints,
3057 DArray<Coord> J,
3058 DArray<Coord> B,
3059 DArray<Coord> pos,
3060 DArray<Matrix> inertia,
3061 DArray<Real> mass,
3062 DArray<Real> K_1,
3063 DArray<Mat2f> K_2,
3064 DArray<Matrix> K_3,
3065 DArray<float> CFM
3066 )
3067 {
3068 int tId = threadIdx.x + blockIdx.x * blockDim.x;
3069 if (tId >= constraints.size())
3070 return;
3071
3072 int idx1 = constraints[tId].bodyId1;
3073 int idx2 = constraints[tId].bodyId2;
3074
3075 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1)
3076 {
3077 Matrix E(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
3078 Coord r1 = constraints[tId].normal1;
3079 Matrix r1x(0.0f, -r1[2], r1[1], r1[2], 0, -r1[0], -r1[1], r1[0], 0);
3080 Matrix CFM_3(CFM[tId], 0, 0, 0, CFM[tId], 0, 0, 0, CFM[tId]);
3081
3082 if (idx2 != INVALID)
3083 {
3084 Coord r2 = constraints[tId].normal2;
3085 Matrix r2x(0.0f, -r2[2], r2[1], r2[2], 0, -r2[0], -r2[1], r2[0], 0);
3086 Matrix K = (1 / mass[idx1]) * E + (r1x * inertia[idx1].inverse()) * r1x.transpose() + (1 / mass[idx2]) * E + (r2x * inertia[idx2].inverse()) * r2x.transpose();
3087 K_3[tId] = (K + CFM_3).inverse();
3088 }
3089 else
3090 {
3091 Matrix K = (1 / mass[idx1]) * E + (r1x * inertia[idx1].inverse()) * r1x.transpose();
3092 K_3[tId] = (K + CFM_3).inverse();
3093 }
3094
3095 }
3096
3097 else if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1)
3098 {
3099 Coord a1 = constraints[tId].axis;
3100 Coord b2 = constraints[tId].pos1;
3101 Coord c2 = constraints[tId].pos2;
3102 Coord b2_c_a1 = b2.cross(a1);
3103 Coord c2_c_a1 = c2.cross(a1);
3104 Real a = b2_c_a1.dot(inertia[idx1].inverse() * b2_c_a1) + b2_c_a1.dot(inertia[idx2].inverse() * b2_c_a1);
3105 Real b = b2_c_a1.dot(inertia[idx1].inverse() * c2_c_a1) + b2_c_a1.dot(inertia[idx2].inverse() * c2_c_a1);
3106 Real c = c2_c_a1.dot(inertia[idx1].inverse() * b2_c_a1) + c2_c_a1.dot(inertia[idx2].inverse() * b2_c_a1);
3107 Real d = c2_c_a1.dot(inertia[idx1].inverse() * c2_c_a1) + c2_c_a1.dot(inertia[idx2].inverse() * c2_c_a1);
3108 Mat2f K(a, b, c, d);
3109 Mat2f CFM_2(CFM[tId], 0, 0, CFM[tId]);
3110 K_2[tId] = (K + CFM_2).inverse();
3111 }
3112
3113 else if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
3114 {
3115 Matrix K = (1 / mass[idx1]) * Matrix(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
3116 Matrix CFM_3(CFM[tId], 0, 0, 0, CFM[tId], 0, 0, 0, CFM[tId]);
3117 K_3[tId] = (K + CFM_3).inverse();
3118 }
3119
3120 else if (constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
3121 {
3122 Coord r1 = constraints[tId].pos1;
3123 Coord r2 = constraints[tId].pos2;
3124 Coord n1 = constraints[tId].normal1;
3125 Coord n2 = constraints[tId].normal2;
3126 Coord u = pos[idx2] + r2 - pos[idx1] - r1;
3127 Coord r1u_c_n1 = (r1 + u).cross(n1);
3128 Coord r1u_c_n2 = (r1 + u).cross(n2);
3129 Coord r2_c_n1 = r2.cross(n1);
3130 Coord r2_c_n2 = r2.cross(n2);
3131 Real a = 1 / mass[idx1] + 1 / mass[idx2] + r1u_c_n1.dot(inertia[idx1].inverse() * r1u_c_n1) + r2_c_n1.dot(inertia[idx2].inverse() * r2_c_n1);
3132 Real b = r1u_c_n1.dot(inertia[idx1].inverse() * r1u_c_n2) + r2_c_n1.dot(inertia[idx2].inverse() * r2_c_n2);
3133 Real c = r1u_c_n2.dot(inertia[idx1].inverse() * r1u_c_n1) + r2_c_n2.dot(inertia[idx2].inverse() * r2_c_n1);
3134 Real d = 1 / mass[idx1] + 1 / mass[idx2] + r1u_c_n2.dot(inertia[idx1].inverse() * r1u_c_n2) + r2_c_n2.dot(inertia[idx2].inverse() * r2_c_n2);
3135 Mat2f K(a, b, c, d);
3136 Mat2f CFM_2(CFM[tId], 0, 0, CFM[tId]);
3137 K_2[tId] = (K + CFM_2).inverse();
3138 }
3139
3140 else if (constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
3141 {
3142 Matrix CFM_3(CFM[tId], 0, 0, 0, CFM[tId], 0, 0, 0, CFM[tId]);
3143 if (idx2 != INVALID)
3144 {
3145 Matrix K = inertia[idx1].inverse() + inertia[idx2].inverse();
3146 K_3[tId] = (K + CFM_3).inverse();
3147 }
3148 else
3149 {
3150 Matrix K = inertia[idx1].inverse();
3151 K_3[tId] = (K + CFM_3).inverse();
3152 }
3153 }
3154
3155 else
3156 {
3157 if (constraints[tId].isValid)
3158 {
3159 Real K = J[4 * tId].dot(B[4 * tId]) + J[4 * tId + 1].dot(B[4 * tId + 1]) + J[4 * tId + 2].dot(B[4 * tId + 2]) + J[4 * tId + 3].dot(B[4 * tId + 3]);
3160 K_1[tId] = 1 / (K + CFM[tId]);
3161 }
3162 }
3163 }
3164
3165
3166 void calculateK(
3167 DArray<TConstraintPair<float>> constraints,
3168 DArray<Vec3f> J,
3169 DArray<Vec3f> B,
3170 DArray<Vec3f> pos,
3171 DArray<Mat3f> inertia,
3172 DArray<float> mass,
3173 DArray<float> K_1,
3174 DArray<Mat2f> K_2,
3175 DArray<Mat3f> K_3
3176 )
3177 {
3178 cuExecute(constraints.size(),
3179 SF_calculateK,
3180 constraints,
3181 J,
3182 B,
3183 pos,
3184 inertia,
3185 mass,
3186 K_1,
3187 K_2,
3188 K_3);
3189 }
3190
3191 void calculateKWithCFM(
3192 DArray<TConstraintPair<float>> constraints,
3193 DArray<Vec3f> J,
3194 DArray<Vec3f> B,
3195 DArray<Vec3f> pos,
3196 DArray<Mat3f> inertia,
3197 DArray<float> mass,
3198 DArray<float> K_1,
3199 DArray<Mat2f> K_2,
3200 DArray<Mat3f> K_3,
3201 DArray<float> CFM
3202 )
3203 {
3204 cuExecute(constraints.size(),
3205 SF_calculateKWithCFM,
3206 constraints,
3207 J,
3208 B,
3209 pos,
3210 inertia,
3211 mass,
3212 K_1,
3213 K_2,
3214 K_3,
3215 CFM);
3216 }
3217
3218 /**
3219 * take one Jacobi Iteration
3220 * @param lambda
3221 * @param impulse
3222 * @param J
3223 * @param B
3224 * @param eta
3225 * @param constraints
3226 * @param nbq
3227 * @param K_1
3228 * @param K_2
3229 * @param K_3
3230 * @param mass
3231 * @param mu
3232 * @param g
3233 * @param dt
3234 * This function take one Jacobi Iteration to calculate constrain impulse
3235 */
3236 template<typename Real, typename Coord, typename Constraint, typename Matrix3, typename Matrix2>
3237 __global__ void SF_JacobiIteration(
3238 DArray<Real> lambda,
3239 DArray<Coord> impulse,
3240 DArray<Coord> J,
3241 DArray<Coord> B,
3242 DArray<Real> eta,
3243 DArray<Constraint> constraints,
3244 DArray<int> nbq,
3245 DArray<Real> K_1,
3246 DArray<Matrix2> K_2,
3247 DArray<Matrix3> K_3,
3248 DArray<Real> mass,
3249 DArray<Real> fricCoeffs,
3250 Real mu,
3251 Real g,
3252 Real dt
3253 )
3254 {
3255 int tId = threadIdx.x + blockIdx.x * blockDim.x;
3256 if (tId >= constraints.size())
3257 return;
3258
3259
3260 int idx1 = constraints[tId].bodyId1;
3261 int idx2 = constraints[tId].bodyId2;
3262
3263 int stepInverse = 0;
3264 if (constraints[tId].type == ConstraintType::CN_FRICTION || constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3265 {
3266 if (idx2 != INVALID)
3267 {
3268 stepInverse = nbq[idx1] + nbq[idx2];
3269 }
3270 else
3271 {
3272 stepInverse = nbq[idx1];
3273 }
3274 }
3275 else
3276 {
3277 stepInverse = 5;
3278 }
3279
3280 Real omega = Real(1) / stepInverse;
3281
3282 if (constraints[tId].type == ConstraintType::CN_FRICTION || constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3283 {
3284 Real tmp = eta[tId];
3285 tmp -= J[4 * tId].dot(impulse[idx1 * 2]);
3286 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]);
3287 if (idx2 != INVALID)
3288 {
3289 tmp -= J[4 * tId + 2].dot(impulse[idx2 * 2]);
3290 tmp -= J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
3291 }
3292 Real delta_lambda = 0;
3293 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3294 {
3295 Real lambda_new = maximum(0.0f, lambda[tId] + (tmp * K_1[tId] * omega));
3296 delta_lambda = lambda_new - lambda[tId];
3297 }
3298 if (constraints[tId].type == ConstraintType::CN_FRICTION)
3299 {
3300 Real mass_avl = mass[idx1];
3301 Real mu_i = fricCoeffs[idx1];
3302 if (idx2 != INVALID)
3303 {
3304 mass_avl = (mass_avl + mass[idx2]) / 2;
3305 mu_i = (mu_i + fricCoeffs[idx2]) / 2;
3306 }
3307 Real lambda_new = minimum(maximum(lambda[tId] + (tmp * K_1[tId] * omega), -mu_i * mass_avl * g * dt), mu_i * mass_avl * g * dt);
3308 delta_lambda = lambda_new - lambda[tId];
3309 }
3310
3311 lambda[tId] += delta_lambda;
3312
3313 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
3314 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
3315 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
3316
3317 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
3318 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
3319 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
3320
3321 if (idx2 != INVALID)
3322 {
3323 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
3324 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
3325 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
3326
3327 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
3328 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
3329 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
3330 }
3331 }
3332
3333 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1 || constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
3334 {
3335 Coord tmp(eta[tId], eta[tId + 1], eta[tId + 2]);
3336 if (idx2 != INVALID)
3337 {
3338 for (int i = 0; i < 3; i++)
3339 {
3340 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]) + J[4 * (tId + i) + 2].dot(impulse[idx2 * 2]);
3341 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * (tId + i) + 3].dot(impulse[idx2 * 2 + 1]);
3342 }
3343 }
3344 else
3345 {
3346 for (int i = 0; i < 3; i++)
3347 {
3348 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]);
3349 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]);
3350 }
3351 }
3352
3353 Coord delta_lambda = omega * (K_3[tId] * tmp);
3354
3355 for (int i = 0; i < 3; i++)
3356 {
3357 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3358 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3359 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3360
3361 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3362 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3363 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3364
3365 if (idx2 != INVALID)
3366 {
3367 atomicAdd(&impulse[idx2 * 2][0], B[4 * (tId + i) + 2][0] * delta_lambda[i]);
3368 atomicAdd(&impulse[idx2 * 2][1], B[4 * (tId + i) + 2][1] * delta_lambda[i]);
3369 atomicAdd(&impulse[idx2 * 2][2], B[4 * (tId + i) + 2][2] * delta_lambda[i]);
3370
3371 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * (tId + i) + 3][0] * delta_lambda[i]);
3372 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * (tId + i) + 3][1] * delta_lambda[i]);
3373 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * (tId + i) + 3][2] * delta_lambda[i]);
3374 }
3375 }
3376 }
3377
3378 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1 || constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
3379 {
3380 Vec2f tmp(eta[tId], eta[tId + 1]);
3381
3382 for (int i = 0; i < 2; i++)
3383 {
3384 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]) + J[4 * (tId + i) + 2].dot(impulse[idx2 * 2]);
3385 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * (tId + i) + 3].dot(impulse[idx2 * 2 + 1]);
3386 }
3387
3388 Vec2f delta_lambda = omega * (K_2[tId] * tmp);
3389
3390 for (int i = 0; i < 2; i++)
3391 {
3392 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3393 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3394 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3395
3396 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3397 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3398 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3399
3400 atomicAdd(&impulse[idx2 * 2][0], B[4 * (tId + i) + 2][0] * delta_lambda[i]);
3401 atomicAdd(&impulse[idx2 * 2][1], B[4 * (tId + i) + 2][1] * delta_lambda[i]);
3402 atomicAdd(&impulse[idx2 * 2][2], B[4 * (tId + i) + 2][2] * delta_lambda[i]);
3403
3404 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * (tId + i) + 3][0] * delta_lambda[i]);
3405 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * (tId + i) + 3][1] * delta_lambda[i]);
3406 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * (tId + i) + 3][2] * delta_lambda[i]);
3407 }
3408 }
3409
3410 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER)
3411 {
3412 Real tmp = eta[tId];
3413 tmp -= J[4 * tId].dot(impulse[idx1 * 2]) + J[4 * tId + 2].dot(impulse[idx2 * 2]);
3414 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
3415 if (K_1[tId] > 0)
3416 {
3417 Real delta_lambda = tmp * K_1[tId] * omega;
3418 lambda[tId] += delta_lambda;
3419 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
3420 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
3421 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
3422
3423 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
3424 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
3425 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
3426
3427 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
3428 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
3429 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
3430
3431 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
3432 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
3433 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
3434 }
3435 }
3436
3437 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
3438 {
3439 Coord tmp(eta[tId], eta[tId + 1], eta[tId + 2]);
3440 for (int i = 0; i < 3; i++)
3441 {
3442 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]);
3443 }
3444
3445 Coord delta_lambda = omega * (K_3[tId] * tmp);
3446 for (int i = 0; i < 3; i++)
3447 {
3448 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3449 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3450 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3451
3452 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3453 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3454 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3455 }
3456 }
3457 }
3458
3459 template<typename Real, typename Coord, typename Constraint, typename Matrix3, typename Matrix2>
3460 __global__ void SF_JacobiIterationForCFM(
3461 DArray<Real> lambda,
3462 DArray<Coord> impulse,
3463 DArray<Coord> J,
3464 DArray<Coord> B,
3465 DArray<Real> eta,
3466 DArray<Constraint> constraints,
3467 DArray<int> nbq,
3468 DArray<Real> K_1,
3469 DArray<Matrix2> K_2,
3470 DArray<Matrix3> K_3,
3471 DArray<Real> mass,
3472 DArray<Real> CFM,
3473 Real mu,
3474 Real g,
3475 Real dt
3476 )
3477 {
3478 int tId = threadIdx.x + blockIdx.x * blockDim.x;
3479 if (tId >= constraints.size())
3480 return;
3481
3482
3483 int idx1 = constraints[tId].bodyId1;
3484 int idx2 = constraints[tId].bodyId2;
3485
3486
3487
3488
3489 int stepInverse = 0;
3490 if (constraints[tId].type == ConstraintType::CN_FRICTION || constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3491 {
3492 if (idx2 != INVALID)
3493 {
3494 stepInverse = nbq[idx1] + nbq[idx2];
3495 }
3496 else
3497 {
3498 stepInverse = nbq[idx1];
3499 }
3500 }
3501 else
3502 {
3503 stepInverse = 5;
3504 }
3505
3506 Real omega = Real(1) / stepInverse;
3507
3508 if (constraints[tId].type == ConstraintType::CN_FRICTION || constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3509 {
3510 Real tmp = eta[tId];
3511 tmp -= J[4 * tId].dot(impulse[idx1 * 2]);
3512 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]);
3513 if (idx2 != INVALID)
3514 {
3515 tmp -= J[4 * tId + 2].dot(impulse[idx2 * 2]);
3516 tmp -= J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
3517 }
3518
3519 tmp -= lambda[tId] * CFM[tId];
3520
3521 Real delta_lambda = 0;
3522 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3523 {
3524 Real lambda_new = maximum(0.0f, lambda[tId] + (tmp * K_1[tId] * omega));
3525 delta_lambda = lambda_new - lambda[tId];
3526 }
3527
3528
3529 lambda[tId] += delta_lambda;
3530
3531 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
3532 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
3533 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
3534
3535 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
3536 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
3537 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
3538
3539 if (idx2 != INVALID)
3540 {
3541 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
3542 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
3543 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
3544
3545 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
3546 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
3547 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
3548 }
3549 }
3550
3551 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1 || constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
3552 {
3553 Coord tmp(eta[tId], eta[tId + 1], eta[tId + 2]);
3554 if (idx2 != INVALID)
3555 {
3556 for (int i = 0; i < 3; i++)
3557 {
3558 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]) + J[4 * (tId + i) + 2].dot(impulse[idx2 * 2]);
3559 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * (tId + i) + 3].dot(impulse[idx2 * 2 + 1]);
3560 tmp[i] -= lambda[tId + i] * CFM[tId + i];
3561 }
3562 }
3563 else
3564 {
3565 for (int i = 0; i < 3; i++)
3566 {
3567 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]);
3568 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]);
3569 tmp[i] -= lambda[tId + i] * CFM[tId + i];
3570 }
3571 }
3572
3573 Coord delta_lambda = omega * (K_3[tId] * tmp);
3574
3575 for (int i = 0; i < 3; i++)
3576 {
3577 lambda[tId + i] += delta_lambda[i];
3578 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3579 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3580 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3581
3582 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3583 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3584 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3585
3586 if (idx2 != INVALID)
3587 {
3588 atomicAdd(&impulse[idx2 * 2][0], B[4 * (tId + i) + 2][0] * delta_lambda[i]);
3589 atomicAdd(&impulse[idx2 * 2][1], B[4 * (tId + i) + 2][1] * delta_lambda[i]);
3590 atomicAdd(&impulse[idx2 * 2][2], B[4 * (tId + i) + 2][2] * delta_lambda[i]);
3591
3592 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * (tId + i) + 3][0] * delta_lambda[i]);
3593 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * (tId + i) + 3][1] * delta_lambda[i]);
3594 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * (tId + i) + 3][2] * delta_lambda[i]);
3595 }
3596 }
3597 }
3598
3599 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1 || constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
3600 {
3601 Vec2f tmp(eta[tId], eta[tId + 1]);
3602
3603 for (int i = 0; i < 2; i++)
3604 {
3605 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]) + J[4 * (tId + i) + 2].dot(impulse[idx2 * 2]);
3606 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * (tId + i) + 3].dot(impulse[idx2 * 2 + 1]);
3607 tmp[i] -= lambda[tId + i] * CFM[tId + i];
3608 }
3609
3610 Vec2f delta_lambda = omega * (K_2[tId] * tmp);
3611
3612 for (int i = 0; i < 2; i++)
3613 {
3614 lambda[tId + i] += delta_lambda[i];
3615 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3616 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3617 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3618
3619 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3620 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3621 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3622
3623 atomicAdd(&impulse[idx2 * 2][0], B[4 * (tId + i) + 2][0] * delta_lambda[i]);
3624 atomicAdd(&impulse[idx2 * 2][1], B[4 * (tId + i) + 2][1] * delta_lambda[i]);
3625 atomicAdd(&impulse[idx2 * 2][2], B[4 * (tId + i) + 2][2] * delta_lambda[i]);
3626
3627 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * (tId + i) + 3][0] * delta_lambda[i]);
3628 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * (tId + i) + 3][1] * delta_lambda[i]);
3629 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * (tId + i) + 3][2] * delta_lambda[i]);
3630 }
3631 }
3632
3633 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER)
3634 {
3635 Real tmp = eta[tId];
3636 tmp -= J[4 * tId].dot(impulse[idx1 * 2]) + J[4 * tId + 2].dot(impulse[idx2 * 2]);
3637 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
3638 tmp -= lambda[tId] * CFM[tId];
3639 if (K_1[tId] > 0)
3640 {
3641 Real delta_lambda = tmp * (K_1[tId] * omega);
3642 lambda[tId] += delta_lambda;
3643 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
3644 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
3645 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
3646
3647 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
3648 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
3649 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
3650
3651 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
3652 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
3653 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
3654
3655 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
3656 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
3657 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
3658 }
3659 }
3660
3661 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
3662 {
3663 Coord tmp(eta[tId], eta[tId + 1], eta[tId + 2]);
3664 for (int i = 0; i < 3; i++)
3665 {
3666 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]);
3667 tmp[i] -= lambda[tId + i] * CFM[tId + i];
3668 }
3669
3670 Coord delta_lambda = omega * (K_3[tId] * tmp);
3671 for (int i = 0; i < 3; i++)
3672 {
3673 lambda[tId + i] += delta_lambda[i];
3674 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3675 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3676 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3677
3678 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3679 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3680 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3681 }
3682 }
3683 }
3684
3685
3686 template<typename Real, typename Coord, typename Constraint, typename Matrix3, typename Matrix2>
3687 __global__ void SF_JacobiIterationForNJS(
3688 DArray<Real> lambda,
3689 DArray<Coord> impulse,
3690 DArray<Coord> J,
3691 DArray<Coord> B,
3692 DArray<Real> eta,
3693 DArray<Constraint> constraints,
3694 DArray<int> nbq,
3695 DArray<Real> K_1,
3696 DArray<Matrix2> K_2,
3697 DArray<Matrix3> K_3
3698 )
3699 {
3700 int tId = threadIdx.x + blockIdx.x * blockDim.x;
3701 if (tId >= constraints.size())
3702 return;
3703
3704
3705 int idx1 = constraints[tId].bodyId1;
3706 int idx2 = constraints[tId].bodyId2;
3707
3708 int stepInverse = 0;
3709 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3710 {
3711 if (idx2 != INVALID)
3712 {
3713 stepInverse = nbq[idx1] > nbq[idx2] ? nbq[idx1] : nbq[idx2];
3714 }
3715 else
3716 {
3717 stepInverse = nbq[idx1];
3718 }
3719 }
3720 else
3721 {
3722 stepInverse = 3;
3723 }
3724
3725 Real omega = Real(1) / stepInverse;
3726
3727 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3728 {
3729 Real tmp = eta[tId];
3730 tmp -= J[4 * tId].dot(impulse[idx1 * 2]);
3731 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]);
3732 if (idx2 != INVALID)
3733 {
3734 tmp -= J[4 * tId + 2].dot(impulse[idx2 * 2]);
3735 tmp -= J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
3736 }
3737 Real delta_lambda = 0;
3738 Real lambda_new = maximum(0.0f, lambda[tId] + (tmp / (K_1[tId] * stepInverse)));
3739 delta_lambda = lambda_new - lambda[tId];
3740
3741
3742 lambda[tId] += delta_lambda;
3743
3744 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
3745 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
3746 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
3747
3748 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
3749 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
3750 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
3751
3752 if (idx2 != INVALID)
3753 {
3754 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
3755 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
3756 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
3757
3758 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
3759 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
3760 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
3761 }
3762 }
3763
3764 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1 || constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
3765 {
3766 Coord tmp(eta[tId], eta[tId + 1], eta[tId + 2]);
3767 if (idx2 != INVALID)
3768 {
3769 for (int i = 0; i < 3; i++)
3770 {
3771 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]) + J[4 * (tId + i) + 2].dot(impulse[idx2 * 2]);
3772 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * (tId + i) + 3].dot(impulse[idx2 * 2 + 1]);
3773 }
3774 }
3775 else
3776 {
3777 for (int i = 0; i < 3; i++)
3778 {
3779 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]);
3780 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]);
3781 }
3782 }
3783
3784 Coord delta_lambda = omega * (K_3[tId].inverse() * tmp);
3785
3786 for (int i = 0; i < 3; i++)
3787 {
3788 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3789 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3790 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3791
3792 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3793 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3794 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3795
3796 if (idx2 != INVALID)
3797 {
3798 atomicAdd(&impulse[idx2 * 2][0], B[4 * (tId + i) + 2][0] * delta_lambda[i]);
3799 atomicAdd(&impulse[idx2 * 2][1], B[4 * (tId + i) + 2][1] * delta_lambda[i]);
3800 atomicAdd(&impulse[idx2 * 2][2], B[4 * (tId + i) + 2][2] * delta_lambda[i]);
3801
3802 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * (tId + i) + 3][0] * delta_lambda[i]);
3803 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * (tId + i) + 3][1] * delta_lambda[i]);
3804 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * (tId + i) + 3][2] * delta_lambda[i]);
3805 }
3806 }
3807 }
3808
3809 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1 || constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
3810 {
3811 Vec2f tmp(eta[tId], eta[tId + 1]);
3812
3813 for (int i = 0; i < 2; i++)
3814 {
3815 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]) + J[4 * (tId + i) + 2].dot(impulse[idx2 * 2]);
3816 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * (tId + i) + 3].dot(impulse[idx2 * 2 + 1]);
3817 }
3818
3819 Vec2f delta_lambda = omega * (K_2[tId].inverse() * tmp);
3820
3821 for (int i = 0; i < 2; i++)
3822 {
3823 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3824 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3825 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3826
3827 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3828 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3829 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3830
3831 atomicAdd(&impulse[idx2 * 2][0], B[4 * (tId + i) + 2][0] * delta_lambda[i]);
3832 atomicAdd(&impulse[idx2 * 2][1], B[4 * (tId + i) + 2][1] * delta_lambda[i]);
3833 atomicAdd(&impulse[idx2 * 2][2], B[4 * (tId + i) + 2][2] * delta_lambda[i]);
3834
3835 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * (tId + i) + 3][0] * delta_lambda[i]);
3836 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * (tId + i) + 3][1] * delta_lambda[i]);
3837 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * (tId + i) + 3][2] * delta_lambda[i]);
3838 }
3839 }
3840
3841 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX)
3842 {
3843 Real tmp = eta[tId];
3844 tmp -= J[4 * tId].dot(impulse[idx1 * 2]) + J[4 * tId + 2].dot(impulse[idx2 * 2]);
3845 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
3846 if (K_1[tId] > 0)
3847 {
3848 Real delta_lambda = tmp / (K_1[tId] * stepInverse);
3849 lambda[tId] += delta_lambda;
3850 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
3851 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
3852 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
3853
3854 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
3855 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
3856 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
3857
3858 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
3859 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
3860 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
3861
3862 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
3863 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
3864 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
3865 }
3866 }
3867
3868 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
3869 {
3870 Coord tmp(eta[tId], eta[tId + 1], eta[tId + 2]);
3871 for (int i = 0; i < 3; i++)
3872 {
3873 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]);
3874 }
3875
3876 Coord delta_lambda = omega * (K_3[tId].inverse() * tmp);
3877 for (int i = 0; i < 3; i++)
3878 {
3879 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
3880 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
3881 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
3882
3883 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
3884 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
3885 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
3886 }
3887 }
3888 }
3889
3890 template<typename Real, typename Coord, typename Constraint, typename Matrix3, typename Matrix2>
3891 __global__ void SF_JacobiIterationForSoft(
3892 DArray<Real> lambda,
3893 DArray<Coord> impulse,
3894 DArray<Coord> J,
3895 DArray<Coord> B,
3896 DArray<Real> eta,
3897 DArray<Constraint> constraints,
3898 DArray<int> nbq,
3899 DArray<Real> K_1,
3900 DArray<Matrix2> K_2,
3901 DArray<Matrix3> K_3,
3902 DArray<Real> mass,
3903 DArray<Real> mu,
3904 Real g,
3905 Real dt,
3906 Real zeta,
3907 Real hertz
3908 )
3909 {
3910 int tId = threadIdx.x + blockIdx.x * blockDim.x;
3911 if (tId >= constraints.size())
3912 return;
3913
3914
3915 int idx1 = constraints[tId].bodyId1;
3916 int idx2 = constraints[tId].bodyId2;
3917
3918 Real ome = 2.0f * M_PI * hertz;
3919 Real a1 = 2.0 * zeta + ome * dt;
3920 Real a2 = dt * ome * a1;
3921 Real a3 = 1.0f / (1.0f + a2);
3922 Real massCoeff = a2 * a3;
3923 Real impulseCoeff = a3;
3924
3925 int stepInverse = 0;
3926 if (constraints[tId].type == ConstraintType::CN_FRICTION || constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3927 {
3928 if (idx2 != INVALID)
3929 {
3930 stepInverse = nbq[idx1] > nbq[idx2] ? nbq[idx1] : nbq[idx2];
3931 }
3932 else
3933 {
3934 stepInverse = nbq[idx1];
3935 }
3936 }
3937 else
3938 {
3939 stepInverse = 5;
3940 }
3941
3942 Real omega = Real(1) / stepInverse;
3943
3944 if (constraints[tId].type == ConstraintType::CN_FRICTION || constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3945 {
3946 Real tmp = eta[tId];
3947 tmp -= J[4 * tId].dot(impulse[idx1 * 2]);
3948 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]);
3949 if (idx2 != INVALID)
3950 {
3951 tmp -= J[4 * tId + 2].dot(impulse[idx2 * 2]);
3952 tmp -= J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
3953 }
3954 Real delta_lambda = 0;
3955 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
3956 {
3957 Real lambda_new = maximum(0.0f, lambda[tId] + omega * (massCoeff * tmp * K_1[tId] - impulseCoeff * lambda[tId]));
3958 delta_lambda = lambda_new - lambda[tId];
3959 }
3960 if (constraints[tId].type == ConstraintType::CN_FRICTION)
3961 {
3962 Real mass_avl = mass[idx1];
3963 Real mu_i = mu[idx1];
3964 if (idx2 != INVALID)
3965 {
3966 mass_avl = (mass_avl + mass[idx2]) / 2;
3967 mu_i = (mu_i + mu[idx2]) / 2;
3968 }
3969 Real lambda_new = minimum(maximum(lambda[tId] + omega * (massCoeff * tmp * K_1[tId] - impulseCoeff * lambda[tId]), -mu_i * mass_avl * g * dt), mu_i * mass_avl * g * dt);
3970 delta_lambda = lambda_new - lambda[tId];
3971
3972 }
3973
3974 lambda[tId] += delta_lambda;
3975
3976 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
3977 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
3978 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
3979
3980 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
3981 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
3982 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
3983
3984 if (idx2 != INVALID)
3985 {
3986 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
3987 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
3988 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
3989
3990 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
3991 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
3992 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
3993 }
3994 }
3995 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1 || constraints[tId].type == ConstraintType::CN_BAN_ROT_1)
3996 {
3997 Coord tmp(eta[tId], eta[tId + 1], eta[tId + 2]);
3998 if (idx2 != INVALID)
3999 {
4000 for (int i = 0; i < 3; i++)
4001 {
4002 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]) + J[4 * (tId + i) + 2].dot(impulse[idx2 * 2]);
4003 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * (tId + i) + 3].dot(impulse[idx2 * 2 + 1]);
4004 }
4005 }
4006 else
4007 {
4008 for (int i = 0; i < 3; i++)
4009 {
4010 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]);
4011 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]);
4012 }
4013 }
4014 Coord tmp_lambda(lambda[tId], lambda[tId + 1], lambda[tId + 2]);
4015 Coord delta_lambda = omega * (massCoeff * K_3[tId] * tmp - impulseCoeff * tmp_lambda);
4016
4017 for (int i = 0; i < 3; i++)
4018 {
4019 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
4020 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
4021 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
4022
4023 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
4024 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
4025 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
4026
4027 if (idx2 != INVALID)
4028 {
4029 atomicAdd(&impulse[idx2 * 2][0], B[4 * (tId + i) + 2][0] * delta_lambda[i]);
4030 atomicAdd(&impulse[idx2 * 2][1], B[4 * (tId + i) + 2][1] * delta_lambda[i]);
4031 atomicAdd(&impulse[idx2 * 2][2], B[4 * (tId + i) + 2][2] * delta_lambda[i]);
4032
4033 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * (tId + i) + 3][0] * delta_lambda[i]);
4034 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * (tId + i) + 3][1] * delta_lambda[i]);
4035 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * (tId + i) + 3][2] * delta_lambda[i]);
4036 }
4037 }
4038 }
4039
4040 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1 || constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
4041 {
4042 Vec2f tmp(eta[tId], eta[tId + 1]);
4043
4044 for (int i = 0; i < 2; i++)
4045 {
4046 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]) + J[4 * (tId + i) + 2].dot(impulse[idx2 * 2]);
4047 tmp[i] -= J[4 * (tId + i) + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * (tId + i) + 3].dot(impulse[idx2 * 2 + 1]);
4048 }
4049 Vec2f oldLambda(lambda[tId], lambda[tId + 1]);
4050 Vec2f delta_lambda = omega * (massCoeff * K_2[tId] * tmp - impulseCoeff * oldLambda);
4051
4052
4053 for (int i = 0; i < 2; i++)
4054 {
4055 lambda[tId + i] += delta_lambda[i];
4056 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
4057 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
4058 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
4059
4060 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
4061 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
4062 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
4063
4064 atomicAdd(&impulse[idx2 * 2][0], B[4 * (tId + i) + 2][0] * delta_lambda[i]);
4065 atomicAdd(&impulse[idx2 * 2][1], B[4 * (tId + i) + 2][1] * delta_lambda[i]);
4066 atomicAdd(&impulse[idx2 * 2][2], B[4 * (tId + i) + 2][2] * delta_lambda[i]);
4067
4068 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * (tId + i) + 3][0] * delta_lambda[i]);
4069 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * (tId + i) + 3][1] * delta_lambda[i]);
4070 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * (tId + i) + 3][2] * delta_lambda[i]);
4071 }
4072 }
4073
4074 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER)
4075 {
4076 Real tmp = eta[tId];
4077 tmp -= J[4 * tId].dot(impulse[idx1 * 2]) + J[4 * tId + 2].dot(impulse[idx2 * 2]);
4078 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]) + J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
4079 if (K_1[tId] > 0)
4080 {
4081 Real delta_lambda = omega * (massCoeff * K_1[tId] * tmp - impulseCoeff * lambda[tId]);
4082 lambda[tId] += delta_lambda;
4083 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
4084 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
4085 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
4086
4087 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
4088 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
4089 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
4090
4091 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
4092 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
4093 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
4094
4095 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
4096 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
4097 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
4098 }
4099 }
4100
4101 if (constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
4102 {
4103 Coord tmp(eta[tId], eta[tId + 1], eta[tId + 2]);
4104 for (int i = 0; i < 3; i++)
4105 {
4106 tmp[i] -= J[4 * (tId + i)].dot(impulse[idx1 * 2]);
4107 }
4108 Coord oldLambda(lambda[tId], lambda[tId + 1], lambda[tId + 2]);
4109 Coord delta_lambda = omega * (massCoeff * K_3[tId] * tmp - impulseCoeff * oldLambda);
4110 for (int i = 0; i < 3; i++)
4111 {
4112 lambda[tId + i] += delta_lambda[i];
4113 atomicAdd(&impulse[idx1 * 2][0], B[4 * (tId + i)][0] * delta_lambda[i]);
4114 atomicAdd(&impulse[idx1 * 2][1], B[4 * (tId + i)][1] * delta_lambda[i]);
4115 atomicAdd(&impulse[idx1 * 2][2], B[4 * (tId + i)][2] * delta_lambda[i]);
4116
4117 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * (tId + i) + 1][0] * delta_lambda[i]);
4118 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * (tId + i) + 1][1] * delta_lambda[i]);
4119 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * (tId + i) + 1][2] * delta_lambda[i]);
4120 }
4121 }
4122 }
4123
4124 template<typename Real, typename Coord, typename Constraint>
4125 __global__ void SF_JacobiIterationStrict(
4126 DArray<Real> lambda,
4127 DArray<Coord> impulse,
4128 DArray<Coord> J,
4129 DArray<Coord> B,
4130 DArray<Real> eta,
4131 DArray<Constraint> constraints,
4132 DArray<int> nbq,
4133 DArray<Real> d,
4134 DArray<Real> mass,
4135 Real mu,
4136 Real g,
4137 Real dt
4138 )
4139 {
4140 int tId = threadIdx.x + blockIdx.x * blockDim.x;
4141 if (tId >= constraints.size())
4142 return;
4143
4144 int idx1 = constraints[tId].bodyId1;
4145 int idx2 = constraints[tId].bodyId2;
4146
4147 Real tmp = eta[tId];
4148
4149 tmp -= J[4 * tId].dot(impulse[idx1 * 2]);
4150 tmp -= J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]);
4151
4152 if (idx2 != INVALID)
4153 {
4154 tmp -= J[4 * tId + 2].dot(impulse[idx2 * 2]);
4155 tmp -= J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
4156 }
4157
4158 int stepInverse = 0;
4159 if (constraints[tId].type == ConstraintType::CN_FRICTION || constraints[tId].type == ConstraintType::CN_NONPENETRATION)
4160 {
4161 if (idx2 != INVALID)
4162 {
4163 stepInverse = nbq[idx1] + nbq[idx2];
4164 }
4165 else
4166 {
4167 stepInverse = nbq[idx1];
4168 }
4169 }
4170 else
4171 {
4172 stepInverse = 5;
4173 }
4174
4175 Real omega = Real(1) / stepInverse;
4176
4177 if (d[tId] > EPSILON)
4178 {
4179 Real delta_lambda = (tmp / d[tId]) * omega;
4180 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
4181 {
4182 Real lambda_new = maximum(0.0f, lambda[tId] + (tmp / (d[tId] * stepInverse)));
4183 delta_lambda = lambda_new - lambda[tId];
4184 }
4185 if (constraints[tId].type == ConstraintType::CN_FRICTION)
4186 {
4187 Real mass_avl = mass[idx1];
4188 Real lambda_new = minimum(maximum(lambda[tId] + (tmp / (d[tId] * stepInverse)), -mu * mass_avl * g * dt), mu * mass_avl * g * dt);
4189 delta_lambda = lambda_new - lambda[tId];
4190 }
4191
4192 lambda[tId] += delta_lambda;
4193
4194 atomicAdd(&impulse[idx1 * 2][0], B[4 * tId][0] * delta_lambda);
4195 atomicAdd(&impulse[idx1 * 2][1], B[4 * tId][1] * delta_lambda);
4196 atomicAdd(&impulse[idx1 * 2][2], B[4 * tId][2] * delta_lambda);
4197
4198 atomicAdd(&impulse[idx1 * 2 + 1][0], B[4 * tId + 1][0] * delta_lambda);
4199 atomicAdd(&impulse[idx1 * 2 + 1][1], B[4 * tId + 1][1] * delta_lambda);
4200 atomicAdd(&impulse[idx1 * 2 + 1][2], B[4 * tId + 1][2] * delta_lambda);
4201
4202 if (idx2 != INVALID)
4203 {
4204 atomicAdd(&impulse[idx2 * 2][0], B[4 * tId + 2][0] * delta_lambda);
4205 atomicAdd(&impulse[idx2 * 2][1], B[4 * tId + 2][1] * delta_lambda);
4206 atomicAdd(&impulse[idx2 * 2][2], B[4 * tId + 2][2] * delta_lambda);
4207
4208 atomicAdd(&impulse[idx2 * 2 + 1][0], B[4 * tId + 3][0] * delta_lambda);
4209 atomicAdd(&impulse[idx2 * 2 + 1][1], B[4 * tId + 3][1] * delta_lambda);
4210 atomicAdd(&impulse[idx2 * 2 + 1][2], B[4 * tId + 3][2] * delta_lambda);
4211 }
4212 }
4213
4214 }
4215
4216 void JacobiIteration(
4217 DArray<float> lambda,
4218 DArray<Vec3f> impulse,
4219 DArray<Vec3f> J,
4220 DArray<Vec3f> B,
4221 DArray<float> eta,
4222 DArray<TConstraintPair<float>> constraints,
4223 DArray<int> nbq,
4224 DArray<float> K_1,
4225 DArray<Mat2f> K_2,
4226 DArray<Mat3f> K_3,
4227 DArray<float> mass,
4228 DArray<float> fricCoeffs,
4229 float mu,
4230 float g,
4231 float dt
4232 )
4233 {
4234 cuExecute(constraints.size(),
4235 SF_JacobiIteration,
4236 lambda,
4237 impulse,
4238 J,
4239 B,
4240 eta,
4241 constraints,
4242 nbq,
4243 K_1,
4244 K_2,
4245 K_3,
4246 mass,
4247 fricCoeffs,
4248 mu,
4249 g,
4250 dt);
4251 }
4252
4253 void JacobiIterationForCFM(
4254 DArray<float> lambda,
4255 DArray<Vec3f> impulse,
4256 DArray<Vec3f> J,
4257 DArray<Vec3f> B,
4258 DArray<float> eta,
4259 DArray<TConstraintPair<float>> constraints,
4260 DArray<int> nbq,
4261 DArray<float> K_1,
4262 DArray<Mat2f> K_2,
4263 DArray<Mat3f> K_3,
4264 DArray<float> mass,
4265 DArray<float> CFM,
4266 float mu,
4267 float g,
4268 float dt
4269 )
4270 {
4271 cuExecute(constraints.size(),
4272 SF_JacobiIterationForCFM,
4273 lambda,
4274 impulse,
4275 J,
4276 B,
4277 eta,
4278 constraints,
4279 nbq,
4280 K_1,
4281 K_2,
4282 K_3,
4283 mass,
4284 CFM,
4285 mu,
4286 g,
4287 dt);
4288 }
4289
4290 void JacobiIterationStrict(
4291 DArray<float> lambda,
4292 DArray<Vec3f> impulse,
4293 DArray<Vec3f> J,
4294 DArray<Vec3f> B,
4295 DArray<float> eta,
4296 DArray<TConstraintPair<float>> constraints,
4297 DArray<int> nbq,
4298 DArray<float> d,
4299 DArray<float> mass,
4300 float mu,
4301 float g,
4302 float dt
4303 )
4304 {
4305 cuExecute(constraints.size(),
4306 SF_JacobiIterationStrict,
4307 lambda,
4308 impulse,
4309 J,
4310 B,
4311 eta,
4312 constraints,
4313 nbq,
4314 d,
4315 mass,
4316 mu,
4317 g,
4318 dt);
4319 }
4320
4321
4322 void JacobiIterationForSoft(
4323 DArray<float> lambda,
4324 DArray<Vec3f> impulse,
4325 DArray<Vec3f> J,
4326 DArray<Vec3f> B,
4327 DArray<float> eta,
4328 DArray<TConstraintPair<float>> constraints,
4329 DArray<int> nbq,
4330 DArray<float> K_1,
4331 DArray<Mat2f> K_2,
4332 DArray<Mat3f> K_3,
4333 DArray<float> mass,
4334 DArray<float> mu,
4335 float g,
4336 float dt,
4337 float zeta,
4338 float hertz
4339 )
4340 {
4341 cuExecute(constraints.size(),
4342 SF_JacobiIterationForSoft,
4343 lambda,
4344 impulse,
4345 J,
4346 B,
4347 eta,
4348 constraints,
4349 nbq,
4350 K_1,
4351 K_2,
4352 K_3,
4353 mass,
4354 mu,
4355 g,
4356 dt,
4357 zeta,
4358 hertz);
4359 }
4360
4361 void JacobiIterationForNJS(
4362 DArray<float> lambda,
4363 DArray<Vec3f> impulse,
4364 DArray<Vec3f> J,
4365 DArray<Vec3f> B,
4366 DArray<float> eta,
4367 DArray<TConstraintPair<float>> constraints,
4368 DArray<int> nbq,
4369 DArray<float> K_1,
4370 DArray<Mat2f> K_2,
4371 DArray<Mat3f> K_3
4372 )
4373 {
4374 cuExecute(constraints.size(),
4375 SF_JacobiIterationForNJS,
4376 lambda,
4377 impulse,
4378 J,
4379 B,
4380 eta,
4381 constraints,
4382 nbq,
4383 K_1,
4384 K_2,
4385 K_3);
4386 }
4387
4388 /**
4389 * Set up Gravity
4390 * @param impulse_ext
4391 * @param g
4392 * @param dt
4393 * This function set up gravity
4394 */
4395 template<typename Coord>
4396 __global__ void SF_setUpGravity(
4397 DArray<Coord> impulse_ext,
4398 Real g,
4399 Real dt
4400 )
4401 {
4402 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
4403 if (tId >= impulse_ext.size() / 2)
4404 return;
4405
4406 impulse_ext[2 * tId] = Coord(0, -g, 0) * dt;
4407 impulse_ext[2 * tId + 1] = Coord(0);
4408 }
4409
4410 void setUpGravity(
4411 DArray<Vec3f> impulse_ext,
4412 float g,
4413 float dt
4414 )
4415 {
4416 cuExecute(impulse_ext.size() / 2,
4417 SF_setUpGravity,
4418 impulse_ext,
4419 g,
4420 dt);
4421 }
4422
4423 template<typename Coord, typename Real>
4424 __global__ void SF_calculateDiagnals(
4425 DArray<Real> D,
4426 DArray<Coord> J,
4427 DArray<Coord> B
4428 )
4429 {
4430 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
4431 if (tId >= D.size())
4432 return;
4433
4434 Real d = J[4 * tId].dot(B[4 * tId]) + J[4 * tId + 1].dot(B[4 * tId + 1]) + J[4 * tId + 2].dot(B[4 * tId + 2]) + J[4 * tId + 3].dot(B[4 * tId + 3]);
4435
4436 D[tId] = d;
4437 }
4438
4439 void calculateDiagnals(
4440 DArray<float> d,
4441 DArray<Vec3f> J,
4442 DArray<Vec3f> B
4443 )
4444 {
4445 cuExecute(d.size(),
4446 SF_calculateDiagnals,
4447 d,
4448 J,
4449 B);
4450 }
4451
4452 template<typename Coord, typename Real>
4453 __global__ void SF_preConditionJ(
4454 DArray<Coord> J,
4455 DArray<Real> d,
4456 DArray<Real> eta
4457 )
4458 {
4459 int tId = threadIdx.x + blockIdx.x * blockDim.x;
4460 if (tId >= d.size())
4461 return;
4462
4463 if (d[tId] > EPSILON)
4464 {
4465 Real d_inv = 1 / d[tId];
4466 J[4 * tId] = d_inv * J[4 * tId];
4467 J[4 * tId + 1] = d_inv * J[4 * tId + 1];
4468 J[4 * tId + 2] = d_inv * J[4 * tId + 2];
4469 J[4 * tId + 3] = d_inv * J[4 * tId + 3];
4470
4471 eta[tId] = d_inv * eta[tId];
4472 }
4473 }
4474
4475 void preConditionJ(
4476 DArray<Vec3f> J,
4477 DArray<float> d,
4478 DArray<float> eta
4479 )
4480 {
4481 cuExecute(d.size(),
4482 SF_preConditionJ,
4483 J,
4484 d,
4485 eta);
4486 }
4487
4488 template<typename Coord, typename Real, typename Constraint>
4489 __global__ void SF_checkOutError(
4490 DArray<Coord> J,
4491 DArray<Coord> mImpulse,
4492 DArray<Constraint> constraints,
4493 DArray<Real> eta,
4494 DArray<Real> error
4495 )
4496 {
4497 int tId = threadIdx.x + blockIdx.x * blockDim.x;
4498 if (tId >= constraints.size())
4499 return;
4500
4501 int idx1 = constraints[tId].bodyId1;
4502 int idx2 = constraints[tId].bodyId2;
4503
4504 Real tmp = 0;
4505 tmp += J[4 * tId].dot(mImpulse[idx1 * 2]) + J[4 * tId + 1].dot(mImpulse[idx1 * 2 + 1]);
4506 if (idx2 != INVALID)
4507 tmp += J[4 * tId + 2].dot(mImpulse[idx2 * 2]) + J[4 * tId + 3].dot(mImpulse[idx2 * 2 + 1]);
4508
4509 Real e = tmp - eta[tId];
4510 error[tId] = e * e;
4511 }
4512
4513
4514
4515 Real checkOutError(
4516 DArray<Vec3f> J,
4517 DArray<Vec3f> mImpulse,
4518 DArray<TConstraintPair<float>> constraints,
4519 DArray<float> eta
4520 )
4521 {
4522 DArray<float> error;
4523 error.resize(eta.size());
4524 error.reset();
4525
4526 cuExecute(eta.size(),
4527 SF_checkOutError,
4528 J,
4529 mImpulse,
4530 constraints,
4531 eta,
4532 error);
4533
4534 CArray<float> errorHost;
4535 errorHost.assign(error);
4536
4537 Real tmp = 0.0f;
4538 int num = errorHost.size();
4539 for (int i = 0; i < num; i++)
4540 {
4541 tmp += errorHost[i];
4542 }
4543 error.clear();
4544 errorHost.clear();
4545 return sqrt(tmp);
4546 }
4547
4548 bool saveVectorToFile(
4549 const std::vector<float>& vec,
4550 const std::string& filename
4551 )
4552 {
4553 std::ofstream file(filename);
4554 if (!file.is_open()) {
4555 std::cerr << "Failed to open file." << std::endl;
4556 return false;
4557 }
4558
4559 for (float f : vec)
4560 {
4561 file << f << " ";
4562 }
4563
4564 file.close();
4565 return true;
4566 }
4567
4568
4569 bool saveMatrixToFile(
4570 DArray<float> &Matrix,
4571 int n,
4572 const std::string& filename
4573 )
4574 {
4575 CArray<float> matrix;
4576 matrix.assign(Matrix);
4577 std::ofstream file(filename);
4578 if (!file.is_open()) {
4579 std::cerr << "Failed to open file." << std::endl;
4580 return false;
4581 }
4582
4583 for (int i = 0; i < n; i++)
4584 {
4585 for (int j = 0; j < n; j++)
4586 {
4587 file << matrix[i * n + j] << " ";
4588 }
4589 file << "\n";
4590 }
4591
4592 file.close();
4593 return true;
4594 }
4595
4596 bool saveVectorToFile(
4597 DArray<float>& vec,
4598 const std::string& filename
4599 )
4600 {
4601 CArray<float> v;
4602 v.assign(vec);
4603 std::ofstream file(filename);
4604 if (!file.is_open()) {
4605 std::cerr << "Failed to open file." << std::endl;
4606 return false;
4607 }
4608 printf("%d\n", v.size());
4609 for (int i = 0; i < v.size(); i++)
4610 {
4611 file << v[i] << " ";
4612 }
4613
4614 file.close();
4615 return true;
4616 }
4617
4618
4619 double checkOutErrors(
4620 DArray<float> errors
4621 )
4622 {
4623 CArray<float> merrors;
4624 merrors.assign(errors);
4625 double tmp = 0.0;
4626 for (int i = 0; i < merrors.size(); i++)
4627 {
4628 tmp += merrors[i] * merrors[i];
4629 }
4630
4631 return sqrt(tmp);
4632 }
4633
4634 template<typename Coord, typename Real, typename Constraint>
4635 __global__ void SF_calculateMatrixA(
4636 DArray<Coord> J,
4637 DArray<Coord> B,
4638 DArray<Real> A,
4639 DArray<Constraint> constraints,
4640 Real k
4641 )
4642 {
4643 int n = constraints.size();
4644 int tId = threadIdx.x + blockDim.x * blockIdx.x;
4645
4646 int i = tId / n;
4647 int j = tId % n;
4648
4649 if (i >= constraints.size() || j >= constraints.size())
4650 return;
4651
4652 if (i > j)
4653 return;
4654
4655
4656 int row_idx1 = constraints[i].bodyId1;
4657 int row_idx2 = constraints[i].bodyId2;
4658
4659 int col_idx1 = constraints[j].bodyId1;
4660 int col_idx2 = constraints[j].bodyId2;
4661
4662
4663
4664 Real tmp = 0.0f;
4665
4666 if (row_idx1 == col_idx1)
4667 tmp += J[4 * i].dot(B[4 * j]) + J[4 * i + 1].dot(B[4 * j + 1]);
4668
4669 if (row_idx1 == col_idx2)
4670 tmp += J[4 * i].dot(B[4 * j + 2]) + J[4 * i + 1].dot(B[4 * j + 3]);
4671
4672 if (row_idx2 == col_idx1)
4673 tmp += J[4 * i + 2].dot(B[4 * j]) + J[4 * i + 3].dot(B[4 * j + 1]);
4674
4675 if (row_idx2 == col_idx2)
4676 tmp += J[4 * i + 2].dot(B[4 * j + 2]) + J[4 * i + 3].dot(B[4 * j + 3]);
4677
4678 if (i == j && constraints[tId].isValid == true)
4679 tmp += k;
4680
4681 A[i * n + j] = tmp;
4682 A[j * n + i] = tmp;
4683
4684 }
4685
4686
4687 void calculateMatrixA(
4688 DArray<Vec3f> &J,
4689 DArray<Vec3f> &B,
4690 DArray<float> &A,
4691 DArray<TConstraintPair<float>> &constraints,
4692 float k
4693 )
4694 {
4695 int n = constraints.size();
4696
4697 cuExecute(n * n,
4698 SF_calculateMatrixA,
4699 J,
4700 B,
4701 A,
4702 constraints,
4703 k);
4704 }
4705
4706 template<typename Real, typename Constraint>
4707 __global__ void SF_vectorSub(
4708 DArray<Real> ans,
4709 DArray<Real> subtranhend,
4710 DArray<Real> minuend,
4711 DArray<Constraint> constraints
4712 )
4713 {
4714 int tId = threadIdx.x + blockIdx.x * blockDim.x;
4715 if (!constraints[tId].isValid)
4716 return;
4717 if (tId >= ans.size())
4718 return;
4719 ans[tId] = minuend[tId] - subtranhend[tId];
4720
4721
4722 }
4723
4724 void vectorSub(
4725 DArray<float> &ans,
4726 DArray<float> &subtranhend,
4727 DArray<float> &minuend,
4728 DArray<TConstraintPair<float>> &constraints
4729 )
4730 {
4731 cuExecute(ans.size(),
4732 SF_vectorSub,
4733 ans,
4734 subtranhend,
4735 minuend,
4736 constraints);
4737 }
4738
4739
4740 template<typename Real, typename Constraint>
4741 __global__ void SF_vectorAdd(
4742 DArray<Real> ans,
4743 DArray<Real> v1,
4744 DArray<Real> v2,
4745 DArray<Constraint> constraints
4746 )
4747 {
4748 int tId = threadIdx.x + blockIdx.x * blockDim.x;
4749 if (!constraints[tId].isValid)
4750 return;
4751 if (tId >= ans.size())
4752 return;
4753 ans[tId] = v1[tId] + v2[tId];
4754 }
4755
4756 void vectorAdd(
4757 DArray<float> &ans,
4758 DArray<float> &v1,
4759 DArray<float> &v2,
4760 DArray<TConstraintPair<float>> &constraints
4761 )
4762 {
4763 cuExecute(ans.size(),
4764 SF_vectorAdd,
4765 ans,
4766 v1,
4767 v2,
4768 constraints);
4769 }
4770
4771 template<typename Real, typename Coord, typename Constraint>
4772 __global__ void SF_matrixMultiplyVecBuildImpulse(
4773 DArray<Coord> B,
4774 DArray<Real> lambda,
4775 DArray<Coord> impulse,
4776 DArray<Constraint> constraints
4777 )
4778 {
4779 int tId = threadIdx.x + blockIdx.x * blockDim.x;
4780 if (tId >= lambda.size())
4781 return;
4782 if (!constraints[tId].isValid)
4783 return;
4784
4785 int idx1 = constraints[tId].bodyId1;
4786 int idx2 = constraints[tId].bodyId2;
4787
4788 Real lambda_i = lambda[tId];
4789
4790 atomicAdd(&impulse[2 * idx1][0], B[4 * tId][0] * lambda_i);
4791 atomicAdd(&impulse[2 * idx1][1], B[4 * tId][1] * lambda_i);
4792 atomicAdd(&impulse[2 * idx1][2], B[4 * tId][2] * lambda_i);
4793 atomicAdd(&impulse[2 * idx1 + 1][0], B[4 * tId + 1][0] * lambda_i);
4794 atomicAdd(&impulse[2 * idx1 + 1][1], B[4 * tId + 1][1] * lambda_i);
4795 atomicAdd(&impulse[2 * idx1 + 1][2], B[4 * tId + 1][2] * lambda_i);
4796
4797 if (idx2 != INVALID)
4798 {
4799 atomicAdd(&impulse[2 * idx2][0], B[4 * tId + 2][0] * lambda_i);
4800 atomicAdd(&impulse[2 * idx2][1], B[4 * tId + 2][1] * lambda_i);
4801 atomicAdd(&impulse[2 * idx2][2], B[4 * tId + 2][2] * lambda_i);
4802 atomicAdd(&impulse[2 * idx2 + 1][0], B[4 * tId + 3][0] * lambda_i);
4803 atomicAdd(&impulse[2 * idx2 + 1][1], B[4 * tId + 3][1] * lambda_i);
4804 atomicAdd(&impulse[2 * idx2 + 1][2], B[4 * tId + 3][2] * lambda_i);
4805 }
4806 }
4807
4808 template<typename Real, typename Coord, typename Constraint>
4809 __global__ void SF_matrixMultiplyVecUseImpulse(
4810 DArray<Coord> J,
4811 DArray<Coord> impulse,
4812 DArray<Constraint> constraints,
4813 DArray<Real> ans
4814 )
4815 {
4816 int tId = threadIdx.x + blockIdx.x * blockDim.x;
4817 if (tId >= ans.size())
4818 return;
4819 if (!constraints[tId].isValid)
4820 return;
4821
4822 Real tmp = 0.0;
4823
4824 int idx1 = constraints[tId].bodyId1;
4825 int idx2 = constraints[tId].bodyId2;
4826
4827 tmp += J[4 * tId].dot(impulse[idx1 * 2]) + J[4 * tId + 1].dot(impulse[idx1 * 2 + 1]);
4828
4829 if (idx2 != INVALID)
4830 {
4831 tmp += J[4 * tId + 2].dot(impulse[idx2 * 2]) + J[4 * tId + 3].dot(impulse[idx2 * 2 + 1]);
4832 }
4833
4834 ans[tId] = tmp;
4835
4836 }
4837
4838
4839 void matrixMultiplyVec(
4840 DArray<Vec3f> &J,
4841 DArray<Vec3f> &B,
4842 DArray<float> &lambda,
4843 DArray<float> &ans,
4844 DArray<TConstraintPair<float>> &constraints,
4845 int bodyNum
4846 )
4847 {
4848 DArray<Vec3f> impulse;
4849 impulse.resize(2 * bodyNum);
4850 impulse.reset();
4851
4852 cuExecute(constraints.size(),
4853 SF_matrixMultiplyVecBuildImpulse,
4854 B,
4855 lambda,
4856 impulse,
4857 constraints);
4858
4859 cuExecute(constraints.size(),
4860 SF_matrixMultiplyVecUseImpulse,
4861 J,
4862 impulse,
4863 constraints,
4864 ans);
4865 impulse.clear();
4866 }
4867
4868
4869 template<typename Real>
4870 __global__ void SF_vectorInnerProduct(
4871 DArray<Real> v1,
4872 DArray<Real> v2,
4873 DArray<Real> result
4874 )
4875 {
4876 int index = threadIdx.x + blockIdx.x * blockDim.x;
4877
4878 int N = v1.size();
4879
4880 if (index >= N)
4881 return;
4882
4883 result[index] = v1[index] * v2[index];
4884 }
4885
4886
4887
4888
4889 float vectorNorm(
4890 DArray<float> &a,
4891 DArray<float> &b
4892 )
4893 {
4894 DArray<float> c;
4895 c.resize(a.size());
4896 c.reset();
4897
4898 cuExecute(a.size(),
4899 SF_vectorInnerProduct,
4900 a,
4901 b,
4902 c);
4903
4904 Reduction<float> reduction;
4905 return reduction.accumulate(c.begin(), c.size());
4906 }
4907
4908 template<typename Real, typename Constraint>
4909 __global__ void SF_vectorMultiplyScale(
4910 DArray<Real> ans,
4911 DArray<Real> initialVec,
4912 DArray<Constraint> constraints,
4913 Real scale
4914 )
4915 {
4916 int tId = threadIdx.x + blockIdx.x * blockDim.x;
4917 if (!constraints[tId].isValid)
4918 return;
4919 if (tId >= ans.size())
4920 return;
4921 ans[tId] = initialVec[tId] * scale;
4922 }
4923
4924 void vectorMultiplyScale(
4925 DArray<float> &ans,
4926 DArray<float> &initialVec,
4927 float scale,
4928 DArray<TConstraintPair<float>>& constraints
4929 )
4930 {
4931 cuExecute(ans.size(),
4932 SF_vectorMultiplyScale,
4933 ans,
4934 initialVec,
4935 constraints,
4936 scale);
4937 }
4938
4939 template<typename Real, typename Constraint>
4940 __global__ void SF_vectorClampSupport(
4941 DArray<Real> v,
4942 DArray<Constraint> constraints
4943 )
4944 {
4945 int tId = threadIdx.x + blockDim.x * blockIdx.x;
4946 if (tId >= v.size())
4947 return;
4948
4949 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION)
4950 {
4951 if (v[tId] < 0)
4952 v[tId] = 0;
4953 }
4954 }
4955
4956 void vectorClampSupport(
4957 DArray<float> v,
4958 DArray<TConstraintPair<float>> constraints
4959 )
4960 {
4961 cuExecute(v.size(),
4962 SF_vectorClampSupport,
4963 v,
4964 constraints);
4965 }
4966 template<typename Real, typename Constraint>
4967 __global__ void SF_vectorClampFriction(
4968 DArray<Real> v,
4969 DArray<Constraint> constraints,
4970 Real mu,
4971 int contact_size
4972 )
4973 {
4974 int tId = threadIdx.x + blockDim.x * blockIdx.x;
4975 if (tId >= v.size())
4976 return;
4977 if (constraints[tId].type == ConstraintType::CN_FRICTION)
4978 {
4979 Real support = abs(v[tId % contact_size]);
4980 v[tId] = minimum(maximum(v[tId], -mu * support), mu * support);
4981 }
4982 }
4983
4984
4985 void vectorClampFriction(
4986 DArray<float> v,
4987 DArray<TConstraintPair<float>> constraints,
4988 int contact_size,
4989 float mu
4990 )
4991 {
4992 cuExecute(v.size(),
4993 SF_vectorClampFriction,
4994 v,
4995 constraints,
4996 mu,
4997 contact_size);
4998 }
4999
5000
5001 void calculateImpulseByLambda(
5002 DArray<float> lambda,
5003 DArray<TConstraintPair<float>> constraints,
5004 DArray<Vec3f> impulse,
5005 DArray<Vec3f> B
5006 )
5007 {
5008 cuExecute(lambda.size(),
5009 SF_matrixMultiplyVecBuildImpulse,
5010 B,
5011 lambda,
5012 impulse,
5013 constraints);
5014 }
5015
5016 template<typename Real, typename Constraint>
5017 __global__ void SF_vectorMultiplyVector(
5018 DArray<Real> v1,
5019 DArray<Real> v2,
5020 DArray<Real> ans,
5021 DArray<Constraint> constraints
5022 )
5023 {
5024 int tId = threadIdx.x + blockIdx.x * blockDim.x;
5025 if (!constraints[tId].isValid)
5026 return;
5027
5028 if (tId >= v1.size())
5029 return;
5030
5031 ans[tId] = v1[tId] * v2[tId];
5032 }
5033
5034 void vectorMultiplyVector(
5035 DArray<float>& v1,
5036 DArray<float>& v2,
5037 DArray<float>& ans,
5038 DArray<TConstraintPair<float>>& constraints
5039 )
5040 {
5041 cuExecute(v1.size(),
5042 SF_vectorMultiplyVector,
5043 v1,
5044 v2,
5045 ans,
5046 constraints);
5047 }
5048
5049 template<typename Real, typename Matrix2x2, typename Matrix3x3, typename Constraint>
5050 __global__ void SF_preconditionedResidual(
5051 DArray<Real> residual,
5052 DArray<Real> ans,
5053 DArray<Real> k_1,
5054 DArray<Matrix2x2> k_2,
5055 DArray<Matrix3x3> k_3,
5056 DArray<Constraint> constraints
5057 )
5058 {
5059 int tId = threadIdx.x + blockIdx.x * blockDim.x;
5060 if (tId >= residual.size())
5061 return;
5062
5063 if (!constraints[tId].isValid)
5064 {
5065 return;
5066 }
5067
5068 if (constraints[tId].type == ConstraintType::CN_ANCHOR_EQUAL_1 || constraints[tId].type == ConstraintType::CN_BAN_ROT_1 || constraints[tId].type == ConstraintType::CN_JOINT_NO_MOVE_1)
5069 {
5070 Vec3f tmp(residual[tId], residual[tId + 1], residual[tId + 2]);
5071 Vec3f delta = k_3[tId] * tmp;
5072 for (int i = 0; i < 3; i++)
5073 {
5074 ans[tId + i] = delta[i];
5075 }
5076 }
5077
5078 if (constraints[tId].type == ConstraintType::CN_ALLOW_ROT1D_1 || constraints[tId].type == ConstraintType::CN_ANCHOR_TRANS_1)
5079 {
5080 Vec2f tmp(residual[tId], residual[tId + 1]);
5081 Vec2f delta = k_2[tId] * tmp;
5082 for (int i = 0; i < 2; i++)
5083 {
5084 ans[tId + i] = delta[i];
5085 }
5086 }
5087
5088 if (constraints[tId].type == ConstraintType::CN_NONPENETRATION || constraints[tId].type == ConstraintType::CN_FRICTION)
5089 {
5090 ans[tId] = residual[tId] * k_1[tId];
5091 }
5092
5093 if (constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MIN || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MAX || constraints[tId].type == ConstraintType::CN_JOINT_HINGE_MOTER || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MIN || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MAX || constraints[tId].type == ConstraintType::CN_JOINT_SLIDER_MOTER)
5094 {
5095 if (constraints[tId].isValid)
5096 {
5097 ans[tId] = residual[tId] * k_1[tId];
5098 }
5099 }
5100 }
5101
5102 void preconditionedResidual(
5103 DArray<float> &residual,
5104 DArray<float> &ans,
5105 DArray<float> &k_1,
5106 DArray<Mat2f> &k_2,
5107 DArray<Mat3f> &k_3,
5108 DArray<TConstraintPair<float>> &constraints
5109 )
5110 {
5111 cuExecute(residual.size(),
5112 SF_preconditionedResidual,
5113 residual,
5114 ans,
5115 k_1,
5116 k_2,
5117 k_3,
5118 constraints);
5119 }
5120
5121 template<typename Coord, typename Constraint, typename Real>
5122 __global__ void SF_buildCFMAndERP(
5123 DArray<Coord> J,
5124 DArray<Coord> B,
5125 DArray<Constraint> constraints,
5126 DArray<Real> CFM,
5127 DArray<Real> ERP,
5128 Real hertz,
5129 Real zeta,
5130 Real dt
5131 )
5132 {
5133 int tId = threadIdx.x + blockIdx.x * blockDim.x;
5134
5135 if (tId >= constraints.size())
5136 return;
5137
5138 if (!constraints[tId].isValid)
5139 return;
5140
5141
5142 Real d = 0.0;
5143 int idx2 = constraints[tId].bodyId2;
5144 if (idx2 != INVALID)
5145 {
5146 d += J[4 * tId].dot(B[4 * tId]) + J[4 * tId + 1].dot(B[4 * tId + 1]) + J[4 * tId + 2].dot(B[4 * tId + 2]) + J[4 * tId + 3].dot(B[4 * tId + 3]);
5147 }
5148 else
5149 {
5150 d += J[4 * tId].dot(B[4 * tId]) + J[4 * tId + 1].dot(B[4 * tId + 1]);
5151 }
5152
5153 Real m_eff = 1 / d;
5154 Real omega = 2 * M_PI * hertz;
5155 Real k = m_eff * omega * omega;
5156 Real c = 2 * m_eff * zeta * omega;
5157
5158 CFM[tId] = 1 / (c * dt + dt * dt * k);
5159 ERP[tId] = dt * k / (c + dt * k);
5160
5161 //printf("%d : CFM(%lf), ERP(%lf)\n", tId, CFM[tId], ERP[tId]);
5162
5163 }
5164
5165
5166
5167 void buildCFMAndERP(
5168 DArray<Vec3f> J,
5169 DArray<Vec3f> B,
5170 DArray<TConstraintPair<float>> constraints,
5171 DArray<float> CFM,
5172 DArray<float> ERP,
5173 float hertz,
5174 float zeta,
5175 float dt
5176 )
5177 {
5178 cuExecute(constraints.size(),
5179 SF_buildCFMAndERP,
5180 J,
5181 B,
5182 constraints,
5183 CFM,
5184 ERP,
5185 hertz,
5186 zeta,
5187 dt);
5188 }
5189
5190 template<typename Real, typename Coord, typename Constraint>
5191 __global__ void SF_calculateLinearSystemLHSImpulse(
5192 DArray<Coord> B,
5193 DArray<Coord> impulse,
5194 DArray<Real> lambda,
5195 DArray<Constraint> constraints
5196 )
5197 {
5198 int tId = threadIdx.x + blockIdx.x * blockDim.x;
5199
5200 if (tId >= constraints.size())
5201 return;
5202
5203 if (!constraints[tId].isValid)
5204 return;
5205
5206
5207 int idx1 = constraints[tId].bodyId1;
5208 int idx2 = constraints[tId].bodyId2;
5209
5210 Real lambda_i = lambda[tId];
5211
5212 atomicAdd(&impulse[2 * idx1][0], B[4 * tId][0] * lambda_i);
5213 atomicAdd(&impulse[2 * idx1][1], B[4 * tId][1] * lambda_i);
5214 atomicAdd(&impulse[2 * idx1][2], B[4 * tId][2] * lambda_i);
5215 atomicAdd(&impulse[2 * idx1 + 1][0], B[4 * tId + 1][0] * lambda_i);
5216 atomicAdd(&impulse[2 * idx1 + 1][1], B[4 * tId + 1][1] * lambda_i);
5217 atomicAdd(&impulse[2 * idx1 + 1][2], B[4 * tId + 1][2] * lambda_i);
5218
5219 if (idx2 != INVALID)
5220 {
5221 atomicAdd(&impulse[2 * idx2][0], B[4 * tId + 2][0] * lambda_i);
5222 atomicAdd(&impulse[2 * idx2][1], B[4 * tId + 2][1] * lambda_i);
5223 atomicAdd(&impulse[2 * idx2][2], B[4 * tId + 2][2] * lambda_i);
5224 atomicAdd(&impulse[2 * idx2 + 1][0], B[4 * tId + 3][0] * lambda_i);
5225 atomicAdd(&impulse[2 * idx2 + 1][1], B[4 * tId + 3][1] * lambda_i);
5226 atomicAdd(&impulse[2 * idx2 + 1][2], B[4 * tId + 3][2] * lambda_i);
5227 }
5228 }
5229
5230 template<typename Real, typename Coord, typename Constraint>
5231 __global__ void SF_calculateLinearSystemLHSResult(
5232 DArray<Coord> impulse,
5233 DArray<Coord> J,
5234 DArray<Real> CFM,
5235 DArray<Real> ans,
5236 DArray<Real> lambda,
5237 DArray<Constraint> constraints
5238 )
5239 {
5240 int tId = threadIdx.x + blockIdx.x * blockDim.x;
5241
5242 if (tId >= constraints.size())
5243 return;
5244
5245 if (!constraints[tId].isValid)
5246 return;
5247
5248 int idx1 = constraints[tId].bodyId1;
5249 int idx2 = constraints[tId].bodyId2;
5250
5251 Real tmp = 0.0;
5252
5253 tmp += J[4 * tId].dot(impulse[2 * idx1]) + J[4 * tId + 1].dot(impulse[2 * idx1 + 1]);
5254
5255 if (idx2 != INVALID)
5256 tmp += J[4 * tId + 2].dot(impulse[2 * idx2]) + J[4 * tId + 3].dot(impulse[2 * idx2 + 1]);
5257
5258
5259 tmp += CFM[tId] * lambda[tId];
5260
5261
5262 ans[tId] = tmp;
5263 }
5264
5265
5266
5267 void calculateLinearSystemLHS(
5268 DArray<Vec3f>& J,
5269 DArray<Vec3f>& B,
5270 DArray<Vec3f>& impulse,
5271 DArray<float>& lambda,
5272 DArray<float>& ans,
5273 DArray<float>& CFM,
5274 DArray<TConstraintPair<float>>& constraints
5275 )
5276 {
5277 int n = constraints.size();
5278
5279 cuExecute(n,
5280 SF_calculateLinearSystemLHSImpulse,
5281 B,
5282 impulse,
5283 lambda,
5284 constraints);
5285
5286 cuExecute(n,
5287 SF_calculateLinearSystemLHSResult,
5288 impulse,
5289 J,
5290 CFM,
5291 ans,
5292 lambda,
5293 constraints);
5294 }
5295
5296}
Generated by doxygen 1.13.2