PeriDyno 1.2.1
Loading...
Searching...
No Matches
DiscreteElements.h
Go to the documentation of this file.
1#pragma once
4
5#include "STL/Pair.h"
6
7namespace dyno
8{
10 {
11 ET_BOX = 1,
12 ET_TET = 2,
15 ET_TRI = 16,
17 ET_Other = 0x80000000
18 };
19
21 {
22 public:
23 DYN_FUNC inline uint sphereIndex() { return sphereStart; }
24 DYN_FUNC inline uint boxIndex() { return boxStart; }
25 DYN_FUNC inline uint tetIndex() { return tetStart; }
26 DYN_FUNC inline uint capsuleIndex() { return capStart; }
27 DYN_FUNC inline uint triangleIndex() { return triStart; }
28
29 DYN_FUNC inline void setSphereRange(uint startIndex, uint endIndex) {
30 sphereStart = startIndex;
31 sphereEnd = endIndex;
32 }
33
34 DYN_FUNC inline void setBoxRange(uint startIndex, uint endIndex) {
35 boxStart = startIndex;
36 boxEnd = endIndex;
37 }
38
39 DYN_FUNC inline void setTetRange(uint startIndex, uint endIndex) {
40 tetStart = startIndex;
41 tetEnd = endIndex;
42 }
43
44 DYN_FUNC inline void setCapsuleRange(uint startIndex, uint endIndex) {
45 capStart = startIndex;
46 capEnd = endIndex;
47 }
48
49 DYN_FUNC inline void setTriangleRange(uint startIndex, uint endIndex) {
50 triStart = startIndex;
51 triEnd = endIndex;
52 }
53
54 DYN_FUNC inline uint checkElementOffset(ElementType eleType)
55 {
56 if (eleType == ET_SPHERE)
57 return sphereStart;
58
59 if (eleType == ET_BOX)
60 return boxStart;
61
62 if (eleType == ET_TET)
63 return tetStart;
64
65 if (eleType == ET_CAPSULE)
66 return capStart;
67
68 if (eleType == ET_TRI)
69 return triStart;
70
71 return 0;
72 }
73
74 DYN_FUNC inline ElementType checkElementType(uint id)
75 {
76 if (id >= sphereStart && id < sphereEnd)
77 return ET_SPHERE;
78
79 if (id >= boxStart && id < boxEnd)
80 return ET_BOX;
81
82 if (id >= tetStart && id < tetEnd)
83 return ET_TET;
84
85 if (id >= capStart && id < capEnd)
86 return ET_CAPSULE;
87
88 if (id >= triStart && id < triEnd)
89 return ET_TRI;
90 }
91
92 private:
103 };
104
106 {
107 public:
109
111
112
113
115
117 };
118
119 template<typename Real>
120 class Joint
121 {
122 public:
123 DYN_FUNC Joint()
124 {
125 this->bodyId1 = INVALID;
126 this->bodyId2 = INVALID;
127
128 this->bodyType1 = ET_Other;
129 this->bodyType2 = ET_Other;
130
131 this->actor1 = nullptr;
132 this->actor2 = nullptr;
133 }
134
135 CPU_FUNC Joint(PdActor* a1, PdActor* a2)
136 {
137 this->bodyId1 = a1->idx;
138 this->bodyId2 = a2->idx;
139
140 this->bodyType1 = a1->shapeType;
141 this->bodyType2 = a2->shapeType;
142
143 this->actor1 = a1;
144 this->actor2 = a2;
145 }
146
147 public:
150
153
154 //The following two pointers should only be visited from host codes.
155 PdActor* actor1 = nullptr;
156 PdActor* actor2 = nullptr;
157 };
158
159
160 template<typename Real>
161 class BallAndSocketJoint : public Joint<Real>
162 {
163 public:
165 {
166 this->bodyId1 = INVALID;
167 this->bodyId2 = INVALID;
168
169 this->bodyType1 = ET_Other;
170 this->bodyType2 = ET_Other;
171
172 this->actor1 = nullptr;
173 this->actor2 = nullptr;
174 }
175
177 {
178 this->bodyId1 = a1->idx;
179 this->bodyId2 = a2->idx;
180
181 this->bodyType1 = a1->shapeType;
182 this->bodyType2 = a2->shapeType;
183
184 this->actor1 = a1;
185 this->actor2 = a2;
186 }
187
189 {
190 Mat3f rotMat1 = this->actor1->rot.toMatrix3x3();
191 Mat3f rotMat2 = this->actor2->rot.toMatrix3x3();
192 this->r1 = rotMat1.inverse() * (anchor_point - this->actor1->center);
193 this->r2 = rotMat2.inverse() * (anchor_point - this->actor2->center);
194 }
195
196 public:
197 // anchor point in body1 local space
199 // anchor point in body2 local space
201 };
202
203 template<typename Real>
204 class SliderJoint : public Joint<Real>
205 {
206 public:
207 DYN_FUNC SliderJoint()
208 {
209 this->bodyId1 = INVALID;
210 this->bodyId2 = INVALID;
211
212 this->bodyType1 = ET_Other;
213 this->bodyType2 = ET_Other;
214
215 this->actor1 = nullptr;
216 this->actor2 = nullptr;
217 }
218
219 CPU_FUNC SliderJoint(PdActor* a1, PdActor* a2)
220 {
221 this->bodyId1 = a1->idx;
222 this->bodyId2 = a2->idx;
223
224 this->bodyType1 = a1->shapeType;
225 this->bodyType2 = a2->shapeType;
226
227 this->actor1 = a1;
228 this->actor2 = a2;
229 }
230
232 {
233 Mat3f rotMat1 = this->actor1->rot.toMatrix3x3();
234 this->r1 = rotMat1.inverse() * (anchor_point - this->actor1->center);
235 if (this->bodyId2 != INVALID)
236 {
237 Mat3f rotMat2 = this->actor2->rot.toMatrix3x3();
238 this->r2 = rotMat2.inverse() * (anchor_point - this->actor2->center);
239 this->q_init = this->actor2->rot.inverse() * this->actor1->rot;
240 }
241 else
242 {
243 this->q_init = this->actor1->rot;
244 }
245 }
246
248 {
249 Mat3f rotMat1 = this->actor1->rot.toMatrix3x3();
250 this->sliderAxis = rotMat1.transpose() * axis;
251 }
252
254 {
255 this->useMoter = true;
256 this->v_moter = v_moter;
257 }
258
260 {
261 this->d_min = d_min;
262 this->d_max = d_max;
263 this->useRange = true;
264 }
265
266
267 public:
268 bool useRange = false;
269 bool useMoter = false;
270 // motion range
274 // anchor point position in body1 and body2 local space
277 // slider axis in body1 local space
279
281 };
282
283
284 template<typename Real>
285 class HingeJoint : public Joint<Real>
286 {
287 public:
288 DYN_FUNC HingeJoint()
289 {
290 this->bodyId1 = INVALID;
291 this->bodyId2 = INVALID;
292
293 this->bodyType1 = ET_Other;
294 this->bodyType2 = ET_Other;
295
296 this->actor1 = nullptr;
297 this->actor2 = nullptr;
298 }
299
300 CPU_FUNC HingeJoint(PdActor* a1, PdActor* a2)
301 {
302 this->bodyId1 = a1->idx;
303 this->bodyId2 = a2->idx;
304
305 this->bodyType1 = a1->shapeType;
306 this->bodyType2 = a2->shapeType;
307
308 this->actor1 = a1;
309 this->actor2 = a2;
310 }
311
313 {
314 Mat3f rotMat1 = this->actor1->rot.toMatrix3x3();
315 Mat3f rotMat2 = this->actor2->rot.toMatrix3x3();
316 this->r1 = rotMat1.inverse() * (anchor_point - this->actor1->center);
317 this->r2 = rotMat2.inverse() * (anchor_point - this->actor2->center);
318 }
319
321 {
322 Mat3f rotMat1 = this->actor1->rot.toMatrix3x3();
323 Mat3f rotMat2 = this->actor2->rot.toMatrix3x3();
324 this->hingeAxisBody1 = rotMat1.inverse() * axis;
325 this->hingeAxisBody2 = rotMat2.inverse() * axis;
326 }
327
328 void setRange(Real theta_min, Real theta_max)
329 {
330 this->d_min = theta_min;
331 this->d_max = theta_max;
332 this->useRange = true;
333 }
334
336 {
337 this->v_moter = v_moter;
338 this->useMoter = true;
339 }
340
341 public:
342 // motion range
346 // anchor point position in body1 and body2 local space
349
350 // axis a in body local space
353
354 bool useMoter = false;
355 bool useRange = false;
356 };
357
358 template<typename Real>
359 class FixedJoint : public Joint<Real>
360 {
361 public:
362 DYN_FUNC FixedJoint()
363 {
364 this->bodyId1 = INVALID;
365 this->bodyId2 = INVALID;
366
367 this->bodyType1 = ET_Other;
368 this->bodyType2 = ET_Other;
369
370 this->actor1 = nullptr;
371 this->actor2 = nullptr;
372 }
373
374 CPU_FUNC FixedJoint(PdActor* a1, PdActor* a2)
375 {
376 this->bodyId1 = a1->idx;
377 this->bodyId2 = a2->idx;
378
379 this->bodyType1 = a1->shapeType;
380 this->bodyType2 = a2->shapeType;
381
382 this->actor1 = a1;
383 this->actor2 = a2;
384 }
385
386 CPU_FUNC FixedJoint(PdActor* a1)
387 {
388 this->bodyId1 = a1->idx;
389 this->bodyId2 = INVALID;
390
391 this->bodyType1 = a1->shapeType;
392 this->bodyType2 = ET_Other;
393
394 this->actor1 = a1;
395 this->actor2 = nullptr;
396 }
397
399 {
400 Mat3f rotMat1 = this->actor1->rot.toMatrix3x3();
401 this->r1 = rotMat1.inverse() * (anchor_point - this->actor1->center);
402 this->w = anchor_point;
403 if (this->bodyId2 != INVALID)
404 {
405 Mat3f rotMat2 = this->actor2->rot.toMatrix3x3();
406 this->r2 = rotMat2.inverse() * (anchor_point - this->actor2->center);
407 this->q_init = this->actor2->rot.inverse() * this->actor1->rot;
408 }
409 else
410 {
411 this->q_init = this->actor1->rot;
412 }
413 }
414
416
417 public:
418 // anchor point position in body1 and body2 local space
424 };
425
426
427
428 template<typename Real>
429 class PointJoint : public Joint<Real>
430 {
431 public:
433 {
434 this->bodyId1 = INVALID;
435 this->bodyId2 = INVALID;
436
437 this->bodyType1 = ET_Other;
438 this->bodyType2 = ET_Other;
439
440 this->actor1 = nullptr;
441 this->actor2 = nullptr;
442 }
444 {
445 this->bodyId1 = a1->idx;
446 this->bodyId2 = INVALID;
447
448 this->bodyType1 = a1->shapeType;
449 this->bodyType2 = ET_Other;
450
451 this->actor1 = a1;
452 this->actor2 = nullptr;
453 }
455 {
456 this->anchorPoint = point;
457 }
458
459 public:
461
462 };
463
464 template<typename Real>
465 class DistanceJoint : public Joint<Real>
466 {
467 public:
469 {
470 this->bodyId1 = INVALID;
471 this->bodyId2 = INVALID;
472
473 this->bodyType1 = ET_Other;
474 this->bodyType2 = ET_Other;
475
476 this->actor1 = nullptr;
477 this->actor2 = nullptr;
478 }
480 {
481 this->bodyId1 = a1->idx;
482 this->bodyId2 = a2->idx;
483
484 this->bodyType1 = a1->shapeType;
485 this->bodyType2 = a2->shapeType;
486
487 this->actor1 = a1;
488 this->actor2 = a2;
489 }
491 {
492 this->r1 = r1;
493 this->r2 = r2;
494 this->distance = distance;
495 }
496 public:
497 // anchor point position in body1 and body2 local space
501 };
502
506 template<typename TDataType>
508 {
510 public:
511 typedef typename TDataType::Real Real;
512 typedef typename TDataType::Coord Coord;
513 typedef typename TDataType::Matrix Matrix;
514
515 typedef typename ::dyno::TSphere3D<Real> Sphere3D;
516 typedef typename ::dyno::TOrientedBox3D<Real> Box3D;
517 typedef typename ::dyno::TTet3D<Real> Tet3D;
518
519 typedef typename ::dyno::BallAndSocketJoint<Real> BallAndSocketJoint;
520 typedef typename ::dyno::SliderJoint<Real> SliderJoint;
521 typedef typename ::dyno::HingeJoint<Real> HingeJoint;
522 typedef typename ::dyno::FixedJoint<Real> FixedJoint;
523 typedef typename ::dyno::PointJoint<Real> PointJoint;
524 typedef typename ::dyno::DistanceJoint<Real> DistanceJoint;
525
527 ~DiscreteElements() override;
528
529 void scale(Real s);
530
532
534
540
542
543 //Set basic shapes in local frame
550
556
562
564
567
568 void setPosition(const DArray<Coord>& pos) { mPosition.assign(pos); }
569 void setRotation(const DArray<Matrix>& rot) { mRotation.assign(rot); }
570
577
578 void setTetBodyId(DArray<int>& body_id);
580
584
586
587 void merge(CArray<std::shared_ptr<DiscreteElements<TDataType>>>& topos);
588
590 DArray<Box3D>& boxInGlobal,
591 DArray<Sphere3D>& sphereInGlobal,
592 DArray<Tet3D>& tetInGlobal,
593 DArray<Capsule3D>& capInGlobal,
594 DArray<Triangle3D>& triInGlobal);
595
601
602 protected:
603 void updateTopology() override;
604
605 protected:
611
617
624
626
629
633 };
634}
635
#define DECLARE_TCLASS(name, T1)
Definition Object.h:87
#define INVALID
Definition STLMacro.h:8
double Real
Definition Typedef.inl:23
CPU_FUNC BallAndSocketJoint(PdActor *a1, PdActor *a2)
void setAnchorPoint(Vector< Real, 3 >anchor_point)
void setRotation(const DArray< Matrix > &rot)
::dyno::FixedJoint< Real > FixedJoint
DArray< Pair< uint, uint > > & shape2RigidBodyMapping()
DArray< Box3D > mBoxesInLocal
DArray< Box3D > mBoxInGlobal
void requestCapsuleInGlobal(DArray< Capsule3D > &capInGlobal)
DArray< Matrix > & rotation()
void requestTriangleInGlobal(DArray< Triangle3D > &triInGlobal)
DArray< BallAndSocketJoint > & ballAndSocketJoints()
::dyno::TOrientedBox3D< Real > Box3D
::dyno::HingeJoint< Real > HingeJoint
void requestSphereInGlobal(DArray< Sphere3D > &sphereInGlobal)
::dyno::BallAndSocketJoint< Real > BallAndSocketJoint
TDataType::Matrix Matrix
DArray< Coord > & position()
void copyFrom(DiscreteElements< TDataType > &de)
void setTets(DArray< Tet3D > &tets)
DArray< BallAndSocketJoint > mBallAndSocketJoints
DArray< Sphere3D > mSphereInGlobal
void requestTetInGlobal(DArray< Tet3D > &tetInGlobal)
DArray< Sphere3D > mSpheresInLocal
DArray< Tet3D > & tetsInLocal()
void setTetElementId(DArray< TopologyModule::Tetrahedron > &element_id)
DArray< Capsule3D > & capsulesInLocal()
ElementOffset calculateElementOffset()
void requestDiscreteElementsInGlobal(DArray< Box3D > &boxInGlobal, DArray< Sphere3D > &sphereInGlobal, DArray< Tet3D > &tetInGlobal, DArray< Capsule3D > &capInGlobal, DArray< Triangle3D > &triInGlobal)
DArray< Tet3D > & tetsInGlobal()
DArray< TopologyModule::Tetrahedron > m_tet_element_id
DArray< Triangle3D > & trianglesInGlobal()
void setTriangles(DArray< Triangle3D > &triangles)
void setTetBodyId(DArray< int > &body_id)
DArray< Sphere3D > & spheresInLocal()
void setCapsules(DArray< Capsule3D > &capsules)
DArray< FixedJoint > & fixedJoints()
DArray< Triangle3D > & trianglesInLocal()
void merge(CArray< std::shared_ptr< DiscreteElements< TDataType > > > &topos)
DArray< Pair< uint, uint > > mShape2RigidBody
DArray< Matrix > mRotation
DArray< PointJoint > & pointJoints()
DArray< Triangle3D > mTriangleInGlobal
void setBoxes(DArray< Box3D > &boxes)
DArray< Tet3D > mTetsInLocal
DArray< Triangle3D > mTrianglesInLocal
DArray< TopologyModule::Tetrahedron > & getTetElementMapping()
void setPosition(const DArray< Coord > &pos)
DArray< PointJoint > mPointJoints
DArray< DistanceJoint > & distanceJoints()
DArray< FixedJoint > mFixedJoints
DArray< SliderJoint > mSliderJoints
void requestBoxInGlobal(DArray< Box3D > &boxInGlobal)
DArray< Capsule3D > mCapsulesInLocal
DArray< int > & getTetBodyMapping()
DArray< HingeJoint > mHingeJoints
DArray< Capsule3D > & capsulesInGlobal()
void updateTopology() override
void setTetSDF(DArray< Real > &sdf)
DArray< Tet3D > mTetInGlobal
DArray< Capsule3D > mCapsuleInGlobal
::dyno::DistanceJoint< Real > DistanceJoint
DArray< int > m_tet_body_mapping
DArray< Sphere3D > & spheresInGlobal()
::dyno::TSphere3D< Real > Sphere3D
DArray< Box3D > & boxesInGlobal()
DArray< SliderJoint > & sliderJoints()
DArray< Real > & getTetSDF()
DArray< Box3D > & boxesInLocal()
DArray< HingeJoint > & hingeJoints()
void setSpheres(DArray< Sphere3D > &spheres)
DArray< DistanceJoint > mDistanceJoints
::dyno::SliderJoint< Real > SliderJoint
::dyno::PointJoint< Real > PointJoint
::dyno::TTet3D< Real > Tet3D
void setDistanceJoint(Vector< Real, 3 > r1, Vector< Real, 3 > r2, Real distance)
Vector< Real, 3 > r1
Vector< Real, 3 > r2
DistanceJoint(PdActor *a1, PdActor *a2)
Vector< Real, 3 > w
Vector< Real, 3 > r2
void setAnchorPoint(Vector< Real, 3 >anchor_point)
void setAnchorAngle(Quat< Real > quat)
CPU_FUNC FixedJoint(PdActor *a1, PdActor *a2)
CPU_FUNC FixedJoint(PdActor *a1)
Vector< Real, 3 > r1
void setMoter(Real v_moter)
void setRange(Real theta_min, Real theta_max)
Vector< Real, 3 > hingeAxisBody2
Vector< Real, 3 > hingeAxisBody1
Vector< Real, 3 > r1
void setAxis(Vector< Real, 3 > axis)
void setAnchorPoint(Vector< Real, 3 >anchor_point)
Vector< Real, 3 > r2
CPU_FUNC HingeJoint(PdActor *a1, PdActor *a2)
CPU_FUNC Joint(PdActor *a1, PdActor *a2)
ElementType bodyType2
DYN_FUNC Joint()
ElementType bodyType1
ElementType shapeType
PointJoint(PdActor *a1)
void setAnchorPoint(Vector< Real, 3 > point)
Vector< Real, 3 > anchorPoint
void setAnchorPoint(Vector< Real, 3 >anchor_point)
CPU_FUNC SliderJoint(PdActor *a1, PdActor *a2)
Vector< Real, 3 > r2
void setRange(Real d_min, Real d_max)
Vector< Real, 3 > r1
Vector< Real, 3 > sliderAxis
void setMoter(Real v_moter)
void setAxis(Vector< Real, 3 > axis)
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
SquareMatrix< float, 3 > Mat3f
Definition Matrix3x3.h:92
Array< T, DeviceType::GPU > DArray
Definition Array.inl:89
Quat< float > Quat1f
Definition Quat.h:136
Vector< float, 3 > Vec3f
Definition Vector3D.h:93
Array< T, DeviceType::CPU > CArray
Definition Array.h:151
unsigned int uint
Definition VkProgram.h:14
DYN_FUNC void setTriangleRange(uint startIndex, uint endIndex)
DYN_FUNC void setSphereRange(uint startIndex, uint endIndex)
DYN_FUNC ElementType checkElementType(uint id)
DYN_FUNC uint boxIndex()
DYN_FUNC uint sphereIndex()
DYN_FUNC void setCapsuleRange(uint startIndex, uint endIndex)
DYN_FUNC void setTetRange(uint startIndex, uint endIndex)
DYN_FUNC uint tetIndex()
DYN_FUNC uint checkElementOffset(ElementType eleType)
DYN_FUNC uint capsuleIndex()
DYN_FUNC uint triangleIndex()
DYN_FUNC void setBoxRange(uint startIndex, uint endIndex)
vgm::Quat quat
Definition vgMath.h:633