PeriDyno 1.0.0
Loading...
Searching...
No Matches
PJSConstraintSolver.cu
Go to the documentation of this file.
1#include "PJSConstraintSolver.h"
2#include "SharedFuncsForRigidBody.h"
3//#define USE_RELAXATION
4#define FILE_NAME "D:/Work Code/peridyno/Data/v2.txt"
5namespace dyno
6{
7 IMPLEMENT_TCLASS(PJSConstraintSolver, TDataType)
8
9 template<typename TDataType>
10 PJSConstraintSolver<TDataType>::PJSConstraintSolver()
11 :ConstraintModule()
12 {
13 this->inContacts()->tagOptional(true);
14 }
15
16 template<typename TDataType>
17 PJSConstraintSolver<TDataType>::~PJSConstraintSolver()
18 {
19 }
20
21 template<typename TDataType>
22 void PJSConstraintSolver<TDataType>::initializeRelaxation()
23 {
24 int constraint_size = 0;
25 int contact_size = this->inContacts()->size();
26
27 auto topo = this->inDiscreteElements()->constDataPtr();
28
29 int ballAndSocketJoint_size = topo->ballAndSocketJoints().size();
30 int sliderJoint_size = topo->sliderJoints().size();
31 int hingeJoint_size = topo->hingeJoints().size();
32 int fixedJoint_size = topo->fixedJoints().size();
33 int pointJoint_size = topo->pointJoints().size();
34
35 if (this->varFrictionEnabled()->getData())
36 {
37 constraint_size += 3 * contact_size;
38 }
39 else
40 {
41 constraint_size = contact_size;
42 }
43
44 if (ballAndSocketJoint_size != 0)
45 {
46 constraint_size += 3 * ballAndSocketJoint_size;
47 }
48
49 if (sliderJoint_size != 0)
50 {
51 constraint_size += 8 * sliderJoint_size;
52 }
53
54 if (hingeJoint_size != 0)
55 {
56 constraint_size += 8 * hingeJoint_size;
57 }
58
59 if (fixedJoint_size != 0)
60 {
61 constraint_size += 6 * fixedJoint_size;
62 }
63
64 if (pointJoint_size != 0)
65 {
66 constraint_size += 3 * pointJoint_size;
67 }
68
69 if (constraint_size == 0)
70 {
71 return;
72 }
73
74 mVelocityConstraints.resize(constraint_size);
75
76 if (contact_size != 0)
77 {
78 auto& contacts = this->inContacts()->getData();
79 setUpContactAndFrictionConstraints(
80 mVelocityConstraints,
81 mContactsInLocalFrame,
82 this->inCenter()->getData(),
83 this->inRotationMatrix()->getData(),
84 this->varFrictionEnabled()->getData()
85 );
86 }
87
88 if (ballAndSocketJoint_size != 0)
89 {
90 auto& joints = topo->ballAndSocketJoints();
91 int begin_index = contact_size;
92
93 if (this->varFrictionEnabled()->getData())
94 {
95 begin_index += 2 * contact_size;
96 }
97
98 setUpBallAndSocketJointConstraints(
99 mVelocityConstraints,
100 joints,
101 this->inCenter()->getData(),
102 this->inRotationMatrix()->getData(),
103 begin_index
104 );
105 }
106
107 if (sliderJoint_size != 0)
108 {
109 auto& joints = topo->sliderJoints();
110 int begin_index = contact_size;
111
112 if (this->varFrictionEnabled()->getData())
113 {
114 begin_index += 2 * contact_size;
115 }
116 begin_index += 3 * ballAndSocketJoint_size;
117 setUpSliderJointConstraints(
118 mVelocityConstraints,
119 joints,
120 this->inCenter()->getData(),
121 this->inRotationMatrix()->getData(),
122 this->inQuaternion()->getData(),
123 begin_index
124 );
125 }
126
127 if (hingeJoint_size != 0)
128 {
129 auto& joints = topo->hingeJoints();
130 int begin_index = contact_size + 3 * ballAndSocketJoint_size + 8 * sliderJoint_size;
131 if (this->varFrictionEnabled()->getData())
132 {
133 begin_index += 2 * contact_size;
134 }
135 setUpHingeJointConstraints(
136 mVelocityConstraints,
137 joints,
138 this->inCenter()->getData(),
139 this->inRotationMatrix()->getData(),
140 this->inQuaternion()->getData(),
141 begin_index
142 );
143 }
144
145 if (fixedJoint_size != 0)
146 {
147 auto& joints = topo->fixedJoints();
148 int begin_index = contact_size + 3 * ballAndSocketJoint_size + 8 * sliderJoint_size + 8 * hingeJoint_size;
149 if (this->varFrictionEnabled()->getData())
150 {
151 begin_index += 2 * contact_size;
152 }
153 setUpFixedJointConstraints(
154 mVelocityConstraints,
155 joints,
156 this->inRotationMatrix()->getData(),
157 this->inQuaternion()->getData(),
158 begin_index
159 );
160 }
161
162 if (pointJoint_size != 0)
163 {
164 auto& joints = topo->pointJoints();
165 int begin_index = contact_size + 3 * ballAndSocketJoint_size + 8 * sliderJoint_size + 8 * hingeJoint_size + 6 * fixedJoint_size;
166 if (this->varFrictionEnabled()->getData())
167 {
168 begin_index += 2 * contact_size;
169 }
170 setUpPointJointConstraints(
171 mVelocityConstraints,
172 joints,
173 this->inCenter()->getData(),
174 begin_index
175 );
176 }
177
178 auto sizeOfRigids = this->inCenter()->size();
179 mContactNumber.resize(sizeOfRigids);
180
181 mJ.resize(4 * constraint_size);
182 mB.resize(4 * constraint_size);
183 mK_1.resize(constraint_size);
184 mK_2.resize(constraint_size);
185 mK_3.resize(constraint_size);
186 mEta.resize(constraint_size);
187
188 mJ.reset();
189 mB.reset();
190 mK_1.reset();
191 mK_2.reset();
192 mK_3.reset();
193 mEta.reset();
194
195 calculateJacobianMatrix(
196 mJ,
197 mB,
198 this->inCenter()->getData(),
199 this->inInertia()->getData(),
200 this->inMass()->getData(),
201 this->inRotationMatrix()->getData(),
202 mVelocityConstraints
203 );
204
205 calculateK(
206 mVelocityConstraints,
207 mJ,
208 mB,
209 this->inCenter()->getData(),
210 this->inInertia()->getData(),
211 this->inMass()->getData(),
212 mK_1,
213 mK_2,
214 mK_3
215 );
216
217 calculateEtaVectorForRelaxation(
218 mEta,
219 mJ,
220 this->inVelocity()->getData(),
221 this->inAngularVelocity()->getData(),
222 mVelocityConstraints
223 );
224
225
226 }
227
228 template<typename TDataType>
229 void PJSConstraintSolver<TDataType>::initializeJacobian(Real dt)
230 {
231 int constraint_size = 0;
232 int contact_size = this->inContacts()->size();
233
234 auto topo = this->inDiscreteElements()->constDataPtr();
235
236 int ballAndSocketJoint_size = topo->ballAndSocketJoints().size();
237 int sliderJoint_size = topo->sliderJoints().size();
238 int hingeJoint_size = topo->hingeJoints().size();
239 int fixedJoint_size = topo->fixedJoints().size();
240 int pointJoint_size = topo->pointJoints().size();
241
242 if (this->varFrictionEnabled()->getData())
243 {
244 constraint_size += 3 * contact_size;
245 }
246 else
247 {
248 constraint_size = contact_size;
249 }
250
251 if (ballAndSocketJoint_size != 0)
252 {
253 constraint_size += 3 * ballAndSocketJoint_size;
254 }
255
256 if (sliderJoint_size != 0)
257 {
258 constraint_size += 8 * sliderJoint_size;
259 }
260
261 if (hingeJoint_size != 0)
262 {
263 constraint_size += 8 * hingeJoint_size;
264 }
265
266 if (fixedJoint_size != 0)
267 {
268 constraint_size += 6 * fixedJoint_size;
269 }
270
271 if (pointJoint_size != 0)
272 {
273 constraint_size += 3 * pointJoint_size;
274 }
275
276 if (constraint_size == 0)
277 {
278 return;
279 }
280
281 mVelocityConstraints.resize(constraint_size);
282
283 if (contact_size != 0)
284 {
285 if (mContactsInLocalFrame.size() != this->inContacts()->size()) {
286 mContactsInLocalFrame.resize(this->inContacts()->size());
287 }
288
289 setUpContactsInLocalFrame(
290 mContactsInLocalFrame,
291 this->inContacts()->getData(),
292 this->inCenter()->getData(),
293 this->inRotationMatrix()->getData()
294 );
295
296 auto& contacts = this->inContacts()->getData();
297 setUpContactAndFrictionConstraints(
298 mVelocityConstraints,
299 mContactsInLocalFrame,
300 this->inCenter()->getData(),
301 this->inRotationMatrix()->getData(),
302 this->varFrictionEnabled()->getData()
303 );
304 }
305
306 if (ballAndSocketJoint_size != 0)
307 {
308 auto& joints = topo->ballAndSocketJoints();
309 int begin_index = contact_size;
310
311 if (this->varFrictionEnabled()->getData())
312 {
313 begin_index += 2 * contact_size;
314 }
315
316 setUpBallAndSocketJointConstraints(
317 mVelocityConstraints,
318 joints,
319 this->inCenter()->getData(),
320 this->inRotationMatrix()->getData(),
321 begin_index
322 );
323 }
324
325 if (sliderJoint_size != 0)
326 {
327 auto& joints = topo->sliderJoints();
328 int begin_index = contact_size;
329
330 if (this->varFrictionEnabled()->getData())
331 {
332 begin_index += 2 * contact_size;
333 }
334 begin_index += 3 * ballAndSocketJoint_size;
335 setUpSliderJointConstraints(
336 mVelocityConstraints,
337 joints,
338 this->inCenter()->getData(),
339 this->inRotationMatrix()->getData(),
340 this->inQuaternion()->getData(),
341 begin_index
342 );
343 }
344
345 if (hingeJoint_size != 0)
346 {
347 auto& joints = topo->hingeJoints();
348 int begin_index = contact_size + 3 * ballAndSocketJoint_size + 8 * sliderJoint_size;
349 if (this->varFrictionEnabled()->getData())
350 {
351 begin_index += 2 * contact_size;
352 }
353 setUpHingeJointConstraints(
354 mVelocityConstraints,
355 joints,
356 this->inCenter()->getData(),
357 this->inRotationMatrix()->getData(),
358 this->inQuaternion()->getData(),
359 begin_index
360 );
361 }
362
363 if (fixedJoint_size != 0)
364 {
365 auto& joints = topo->fixedJoints();
366 int begin_index = contact_size + 3 * ballAndSocketJoint_size + 8 * sliderJoint_size + 8 * hingeJoint_size;
367 if (this->varFrictionEnabled()->getData())
368 {
369 begin_index += 2 * contact_size;
370 }
371 setUpFixedJointConstraints(
372 mVelocityConstraints,
373 joints,
374 this->inRotationMatrix()->getData(),
375 this->inQuaternion()->getData(),
376 begin_index
377 );;
378 }
379
380 if (pointJoint_size != 0)
381 {
382 auto& joints = topo->pointJoints();
383 int begin_index = contact_size + 3 * ballAndSocketJoint_size + 8 * sliderJoint_size + 8 * hingeJoint_size + 6 * fixedJoint_size;
384 if (this->varFrictionEnabled()->getData())
385 {
386 begin_index += 2 * contact_size;
387 }
388 setUpPointJointConstraints(
389 mVelocityConstraints,
390 joints,
391 this->inCenter()->getData(),
392 begin_index
393 );
394 }
395
396 auto sizeOfRigids = this->inCenter()->size();
397 mContactNumber.resize(sizeOfRigids);
398
399 mJ.resize(4 * constraint_size);
400 mB.resize(4 * constraint_size);
401 mK_1.resize(constraint_size);
402 mK_2.resize(constraint_size);
403 mK_3.resize(constraint_size);
404 mEta.resize(constraint_size);
405 mLambda.resize(constraint_size);
406
407
408
409
410 mJ.reset();
411 mB.reset();
412 mK_1.reset();
413 mK_2.reset();
414 mK_3.reset();
415 mEta.reset();
416 mLambda.reset();
417
418
419 mContactNumber.reset();
420
421 calculateJacobianMatrix(
422 mJ,
423 mB,
424 this->inCenter()->getData(),
425 this->inInertia()->getData(),
426 this->inMass()->getData(),
427 this->inRotationMatrix()->getData(),
428 mVelocityConstraints
429 );
430
431 mErrors.resize(constraint_size);
432 mErrors.reset();
433
434 calculateEtaVectorForPJSBaumgarte(
435 mEta,
436 mJ,
437 this->inVelocity()->getData(),
438 this->inAngularVelocity()->getData(),
439 this->inCenter()->getData(),
440 this->inQuaternion()->getData(),
441 mVelocityConstraints,
442 mErrors,
443 this->varSlop()->getValue(),
444 this->varBaumgarteRate()->getValue(),
445 1,
446 dt
447 );
448
449 calculateK(
450 mVelocityConstraints,
451 mJ,
452 mB,
453 this->inCenter()->getData(),
454 this->inInertia()->getData(),
455 this->inMass()->getData(),
456 mK_1,
457 mK_2,
458 mK_3
459 );
460
461 if (contact_size != 0)
462 {
463 calculateContactPoints(
464 this->inContacts()->getData(),
465 mContactNumber);
466 }
467 }
468
469 template<typename TDataType>
470 void PJSConstraintSolver<TDataType>::constrain()
471 {
472 uint bodyNum = this->inCenter()->size();
473
474 auto topo = this->inDiscreteElements()->constDataPtr();
475
476 mImpulseC.resize(bodyNum * 2);
477 mImpulseExt.resize(bodyNum * 2);
478 mImpulseC.reset();
479 mImpulseExt.reset();
480
481 Real dt = this->inTimeStep()->getData();
482
483 if (this->varGravityEnabled()->getValue())
484 {
485 setUpGravity(
486 mImpulseExt,
487 this->varGravityValue()->getValue(),
488 dt
489 );
490 }
491
492
493 updateVelocity(
494 this->inAttribute()->getData(),
495 this->inVelocity()->getData(),
496 this->inAngularVelocity()->getData(),
497 mImpulseExt,
498 this->varLinearDamping()->getValue(),
499 this->varAngularDamping()->getValue(),
500 dt
501 );
502
503 if (!this->inContacts()->isEmpty() || topo->totalJointSize() > 0)
504 {
505 int contact_size = this->inContacts()->size();
506 initializeJacobian(dt);
507 errors.push_back(checkOutErrors(mErrors));
508 int constraint_size = mVelocityConstraints.size();
509
510
511
512 for (int i = 0; i < this->varIterationNumberForVelocitySolver()->getValue(); i++)
513 {
514 JacobiIteration(
515 mLambda,
516 mImpulseC,
517 mJ,
518 mB,
519 mEta,
520 mVelocityConstraints,
521 mContactNumber,
522 mK_1,
523 mK_2,
524 mK_3,
525 this->inMass()->getData(),
526 this->inFrictionCoefficients()->getData(),
527 this->varFrictionCoefficient()->getData(),
528 this->varGravityValue()->getData(),
529 dt
530 );
531 }
532
533 Real norm = checkOutError(
534 mJ,
535 mImpulseC,
536 mVelocityConstraints,
537 mEta
538 );
539
540
541 errors.push_back(norm);
542
543 updateVelocity(
544 this->inAttribute()->getData(),
545 this->inVelocity()->getData(),
546 this->inAngularVelocity()->getData(),
547 mImpulseC,
548 this->varLinearDamping()->getValue(),
549 this->varAngularDamping()->getValue(),
550 dt
551 );
552
553 updateGesture(
554 this->inAttribute()->getData(),
555 this->inCenter()->getData(),
556 this->inQuaternion()->getData(),
557 this->inRotationMatrix()->getData(),
558 this->inInertia()->getData(),
559 this->inVelocity()->getData(),
560 this->inAngularVelocity()->getData(),
561 this->inInitialInertia()->getData(),
562 dt
563 );
564
565 #ifdef USE_RELAXATION
566 // Relaxation Step
567 initializeRelaxation();
568
569 for (int i = 0; i < 30; i++)
570 {
571 JacobiIteration(
572 mLambda,
573 mImpulseC,
574 mJ,
575 mB,
576 mEta,
577 mVelocityConstraints,
578 mContactNumber,
579 mK_1,
580 mK_2,
581 mK_3,
582 this->inMass()->getData(),
583 this->varFrictionCoefficient()->getData(),
584 this->varGravityValue()->getData(),
585 dt
586 );
587 }
588
589 updateVelocity(
590 this->inAttribute()->getData(),
591 this->inVelocity()->getData(),
592 this->inAngularVelocity()->getData(),
593 mImpulseC,
594 0.0f,
595 0.0f,
596 dt
597 );
598 #else
599
600 #endif
601
602 }
603 else
604 {
605 updateGesture(
606 this->inAttribute()->getData(),
607 this->inCenter()->getData(),
608 this->inQuaternion()->getData(),
609 this->inRotationMatrix()->getData(),
610 this->inInertia()->getData(),
611 this->inVelocity()->getData(),
612 this->inAngularVelocity()->getData(),
613 this->inInitialInertia()->getData(),
614 dt
615 );
616 }
617
618
619
620
621 }
622
623 DEFINE_CLASS(PJSConstraintSolver);
624}
Generated by doxygen 1.13.2