PeriDyno 1.0.0
Loading...
Searching...
No Matches
RigidBodySystem.cu
Go to the documentation of this file.
1#include "RigidBodySystem.h"
2
3#include "Primitive/Primitive3D.h"
4#include "Collision/NeighborElementQuery.h"
5#include "Collision/CollistionDetectionBoundingBox.h"
6
7#include "RigidBody/Module/TJConstraintSolver.h"
8#include "RigidBody/Module/TJSoftConstraintSolver.h"
9#include "RigidBody/Module/PJSNJSConstraintSolver.h"
10#include "RigidBody/Module/PJSConstraintSolver.h"
11#include "RigidBody/Module/PJSoftConstraintSolver.h"
12#include "RigidBody/Module/PCGConstraintSolver.h"
13#include "RigidBody/Module/CarDriver.h"
14
15//Module headers
16#include "RigidBody/Module/ContactsUnion.h"
17
18namespace dyno
19{
20 typedef typename dyno::TOrientedBox3D<Real> Box3D;
21
22 template<typename TDataType>
23 RigidBodySystem<TDataType>::RigidBodySystem()
24 : Node()
25 {
26 auto defaultTopo = std::make_shared<DiscreteElements<TDataType>>();
27 this->stateTopology()->setDataPtr(std::make_shared<DiscreteElements<TDataType>>());
28
29 auto elementQuery = std::make_shared<NeighborElementQuery<TDataType>>();
30 this->stateTopology()->connect(elementQuery->inDiscreteElements());
31 this->stateCollisionMask()->connect(elementQuery->inCollisionMask());
32 this->stateAttribute()->connect(elementQuery->inAttribute());
33 this->animationPipeline()->pushModule(elementQuery);
34 //elementQuery->varSelfCollision()->setValue(false);
35
36 auto cdBV = std::make_shared<CollistionDetectionBoundingBox<TDataType>>();
37 this->stateTopology()->connect(cdBV->inDiscreteElements());
38 this->animationPipeline()->pushModule(cdBV);
39
40 auto merge = std::make_shared<ContactsUnion<TDataType>>();
41 elementQuery->outContacts()->connect(merge->inContactsA());
42 cdBV->outContacts()->connect(merge->inContactsB());
43
44 this->animationPipeline()->pushModule(merge);
45
46 auto iterSolver = std::make_shared<TJSoftConstraintSolver<TDataType>>();
47 this->stateTimeStep()->connect(iterSolver->inTimeStep());
48 this->varFrictionEnabled()->connect(iterSolver->varFrictionEnabled());
49 this->varGravityEnabled()->connect(iterSolver->varGravityEnabled());
50 this->varGravityValue()->connect(iterSolver->varGravityValue());
51 this->varFrictionCoefficient()->connect(iterSolver->varFrictionCoefficient());
52 this->varSlop()->connect(iterSolver->varSlop());
53 this->stateMass()->connect(iterSolver->inMass());
54
55 this->stateFrictionCoefficients()->connect(iterSolver->inFrictionCoefficients());
56 this->stateAttribute()->connect(iterSolver->inAttribute());
57 this->stateCenter()->connect(iterSolver->inCenter());
58 this->stateVelocity()->connect(iterSolver->inVelocity());
59 this->stateAngularVelocity()->connect(iterSolver->inAngularVelocity());
60 this->stateRotationMatrix()->connect(iterSolver->inRotationMatrix());
61 this->stateInertia()->connect(iterSolver->inInertia());
62 this->stateQuaternion()->connect(iterSolver->inQuaternion());
63 this->stateInitialInertia()->connect(iterSolver->inInitialInertia());
64 this->stateTopology()->connect(iterSolver->inDiscreteElements());
65 merge->outContacts()->connect(iterSolver->inContacts());
66 this->animationPipeline()->pushModule(iterSolver);
67
68
69 this->setDt(0.016f);
70 }
71
72 template<typename TDataType>
73 RigidBodySystem<TDataType>::~RigidBodySystem()
74 {
75 }
76
77 template<typename Real>
78 SquareMatrix<Real, 3> ParallelAxisTheorem(Vector<Real, 3> offset, Real m)
79 {
80 SquareMatrix<Real, 3> mat;
81 mat(0, 0) = m * (offset.y * offset.y + offset.z * offset.z);
82 mat(1, 1) = m * (offset.x * offset.x + offset.z * offset.z);
83 mat(2, 2) = m * (offset.x * offset.x + offset.y * offset.y);
84
85 mat(0, 1) = m * offset.x * offset.y;
86 mat(1, 0) = m * offset.x * offset.y;
87
88 mat(0, 2) = m * offset.x * offset.z;
89 mat(2, 0) = m * offset.z * offset.x;
90
91 mat(1, 2) = m * offset.y * offset.z;
92 mat(2, 1) = m * offset.z * offset.y;
93
94 return mat;
95 }
96
97 template<typename TDataType>
98 std::shared_ptr<PdActor> RigidBodySystem<TDataType>::addBox(
99 const BoxInfo& box,
100 const RigidBodyInfo& bodyDef,
101 const Real density)
102 {
103 auto b = box;
104 auto bd = bodyDef;
105
106 float lx = 2.0f * b.halfLength[0];
107 float ly = 2.0f * b.halfLength[1];
108 float lz = 2.0f * b.halfLength[2];
109
110 bd.mass = density * lx * ly * lz;
111 //printf("Box mass : %lf\n", bd.mass);
112
113 // Calculate the inertia of box in the local frame
114 auto localInertia = 1.0f / 12.0f * bd.mass
115 * Mat3f(ly * ly + lz * lz, 0, 0,
116 0, lx * lx + lz * lz, 0,
117 0, 0, lx * lx + ly * ly);
118
119 // Transform into the rigid body frame
120 auto rotShape = box.rot.toMatrix3x3();
121 auto rotBody = bd.angle.toMatrix3x3();
122
123 bd.inertia = rotBody * (rotShape * localInertia * rotShape.transpose() + ParallelAxisTheorem(box.center, bd.mass)) * rotBody.transpose();
124
125 bd.shapeType = ET_BOX;
126
127 mHostRigidBodyStates.insert(mHostRigidBodyStates.begin() + mHostSpheres.size() + mHostBoxes.size(), bd);
128 mHostBoxes.push_back(b);
129
130 std::shared_ptr<PdActor> actor = std::make_shared<PdActor>();
131 actor->idx = mHostBoxes.size() - 1;
132 actor->shapeType = ET_BOX;
133 actor->center = bd.position;
134 actor->rot = bd.angle;
135
136 return actor;
137 }
138
139 template<typename TDataType>
140 std::shared_ptr<PdActor> RigidBodySystem<TDataType>::addSphere(
141 const SphereInfo& sphere,
142 const RigidBodyInfo& bodyDef,
143 const Real density /*= Real(1)*/)
144 {
145 auto b = sphere;
146 auto bd = bodyDef;
147
148// bd.position = b.center + bd.offset;
149
150 float r = b.radius;
151 if (bd.mass <= 0.0f) {
152 bd.mass = 4 / 3.0f*M_PI*r*r*r*density;
153 }
154 //printf("Sphere mass : %lf\n", bd.mass);
155 float I11 = r * r;
156
157 // Calculate the inertia of sphere in the local frame
158 auto localInertia = 0.4f * bd.mass
159 * Mat3f(I11, 0, 0,
160 0, I11, 0,
161 0, 0, I11);
162
163 auto rotShape = sphere.rot.toMatrix3x3();
164 auto rotBody = bd.angle.toMatrix3x3();
165
166 bd.inertia = rotBody * (rotShape * localInertia * rotShape.transpose() + ParallelAxisTheorem(sphere.center, bd.mass)) * rotBody.transpose();
167
168 bd.shapeType = ET_SPHERE;
169
170 mHostRigidBodyStates.insert(mHostRigidBodyStates.begin() + mHostSpheres.size(), bd);
171 mHostSpheres.push_back(b);
172
173 std::shared_ptr<PdActor> actor = std::make_shared<PdActor>();
174 actor->idx = mHostSpheres.size() - 1;
175 actor->shapeType = ET_SPHERE;
176 actor->center = bd.position;
177 actor->rot = bd.angle;
178
179 return actor;
180 }
181
182 template<typename TDataType>
183 Mat3f RigidBodySystem<TDataType>::pointInertia(Coord r1)
184 {
185 Real x = r1.x;
186 Real y = r1.y;
187 Real z = r1.z;
188 return Mat3f(y * y + z * z, -x * y, -x * z, -y * x, x * x + z * z, -y * z, -z * x, -z * y, x * x + y * y);
189 }
190
191 template<typename TDataType>
192 std::shared_ptr<dyno::PdActor> RigidBodySystem<TDataType>::createRigidBody(
193 const Coord& p,
194 const TQuat& q)
195 {
196 return createRigidBody(RigidBodyInfo(p, q));
197 }
198
199 template<typename TDataType>
200 std::shared_ptr<PdActor> RigidBodySystem<TDataType>::createRigidBody(const RigidBodyInfo& bodyDef)
201 {
202 auto bd = bodyDef;
203 bd.mass = 0.0f;
204 bd.shapeType = ET_COMPOUND;
205
206 mHostRigidBodyStates.push_back(bd);
207
208 std::shared_ptr<PdActor> actor = std::make_shared<PdActor>();
209 actor->idx = mHostRigidBodyStates.size() - 1;
210 actor->shapeType = ET_COMPOUND;
211 actor->center = bd.position;
212 actor->rot = bd.angle;
213
214 return actor;
215 }
216
217 template<typename TDataType>
218 void RigidBodySystem<TDataType>::bindBox(
219 const std::shared_ptr<PdActor> actor,
220 const BoxInfo& box,
221 const Real density /*= Real(100)*/)
222 {
223 auto& rigidbody = mHostRigidBodyStates[actor->idx];
224
225 float lx = 2.0f * box.halfLength[0];
226 float ly = 2.0f * box.halfLength[1];
227 float lz = 2.0f * box.halfLength[2];
228
229 Real mass = density * lx * ly * lz;
230
231 // Calculate the inertia of box in the local frame
232 auto localInertia = 1.0f / 12.0f * mass
233 * Mat3f(ly * ly + lz * lz, 0, 0,
234 0, lx * lx + lz * lz, 0,
235 0, 0, lx * lx + ly * ly);
236
237 // Transform into the rigid body frame
238 auto rotShape = box.rot.toMatrix3x3();
239 auto rotBody = rigidbody.angle.toMatrix3x3();
240
241 auto rigidbodyInertia = rotBody * (rotShape * localInertia * rotShape.transpose() + ParallelAxisTheorem(box.center, mass)) * rotBody.transpose();
242
243 rigidbody.mass += mass;
244 rigidbody.inertia += rigidbodyInertia;
245 rigidbody.shapeType = ET_COMPOUND;
246
247 mHostShape2RigidBodyMapping.insert(mHostShape2RigidBodyMapping.begin() + mHostSpheres.size() + mHostBoxes.size(), Pair<uint, uint>(mHostBoxes.size(), (uint)actor->idx));
248 mHostBoxes.push_back(box);
249 }
250
251 template<typename TDataType>
252 void RigidBodySystem<TDataType>::bindSphere(
253 const std::shared_ptr<PdActor> actor,
254 const SphereInfo& sphere,
255 const Real density /*= Real(100)*/)
256 {
257 auto& rigidbody = mHostRigidBodyStates[actor->idx];
258
259 float r = sphere.radius;
260 Real mass = 4 / 3.0f * M_PI * r * r * r * density;
261
262 float I11 = r * r;
263
264 // Calculate the inertia of sphere in the local frame
265 auto localInertia = 0.4f * mass
266 * Mat3f(I11, 0, 0,
267 0, I11, 0,
268 0, 0, I11);
269
270 auto rotShape = sphere.rot.toMatrix3x3();
271 auto rotBody = rigidbody.angle.toMatrix3x3();
272
273 auto rigidbodyInertia = rotBody * (rotShape * localInertia * rotShape.transpose() + ParallelAxisTheorem(sphere.center, mass)) * rotBody.transpose();
274
275 rigidbody.mass += mass;
276 rigidbody.inertia += rigidbodyInertia;
277 rigidbody.shapeType = ET_COMPOUND;
278
279 mHostShape2RigidBodyMapping.insert(mHostShape2RigidBodyMapping.begin() + mHostSpheres.size(), Pair<uint, uint>(mHostSpheres.size(), (uint)actor->idx));
280 mHostSpheres.push_back(sphere);
281 }
282
283 template<typename TDataType>
284 void RigidBodySystem<TDataType>::bindCapsule(
285 const std::shared_ptr<PdActor> actor,
286 const CapsuleInfo& capsule,
287 const Real density /*= Real(100)*/)
288 {
289 auto& rigidbody = mHostRigidBodyStates[actor->idx];
290
291 Real r = capsule.radius;
292 Real h = capsule.halfLength * 2;
293
294
295 Real mass_hemisphere = 2.0 / 3.0 * M_PI * r * r * r * density;
296 Real mass_cylinder = M_PI * r * r * h * density;
297
298 Real I_1_cylinder = 1.0 / 12.0 * mass_cylinder * (3 * r * r + h * h);
299 Real I_2_cylinder = 1.0 / 2.0 * mass_cylinder * r * r;
300
301
302 Real tmp = h / 2 + 3.0 / 8.0 * r;
303 Real I_1_hemisphere = mass_hemisphere * (2.0 / 5.0 * r * r + h * h / 2 + 3 * h * r / 8.0);
304 Real I_2_hemisphere = 2.0 / 5.0 * mass_hemisphere * r * r;
305
306 Real mass = mass_hemisphere * 2 + mass_cylinder;
307
308 auto localInertia = Mat3f(I_1_cylinder + 2 * I_1_hemisphere, 0, 0,
309 0, I_1_cylinder + 2 * I_1_hemisphere, 0,
310 0, 0, I_2_cylinder + 2 * I_2_hemisphere);
311
312 auto rotShape = capsule.rot.toMatrix3x3();
313 auto rotBody = rigidbody.angle.toMatrix3x3();
314
315 auto rigidbodyInertia = rotBody * (rotShape * localInertia * rotShape.transpose() + ParallelAxisTheorem(capsule.center, mass)) * rotBody.transpose();
316
317 rigidbody.mass += mass;
318 rigidbody.inertia += rigidbodyInertia;
319 rigidbody.shapeType = ET_COMPOUND;
320
321 mHostShape2RigidBodyMapping.insert(mHostShape2RigidBodyMapping.begin() + mHostSpheres.size() + mHostBoxes.size() + mHostTets.size() + mHostCapsules.size(), Pair<uint, uint>(mHostCapsules.size(), (uint)actor->idx));
322 mHostCapsules.push_back(capsule);
323 }
324
325 template<typename TDataType>
326 std::shared_ptr<PdActor> RigidBodySystem<TDataType>::addTet(
327 const TetInfo& tetInfo,
328 const RigidBodyInfo& bodyDef,
329 const Real density /*= Real(1)*/)
330 {
331 TetInfo tet = tetInfo;
332 auto bd = bodyDef;
333
334 bd.position = (tet.v[0] + tet.v[1] + tet.v[2] + tet.v[3]) / 4;
335
336 auto centroid = bd.position;
337 tet.v[0] = tet.v[0] - centroid;
338 tet.v[1] = tet.v[1] - centroid;
339 tet.v[2] = tet.v[2] - centroid;
340 tet.v[3] = tet.v[3] - centroid;
341
342 auto tmpMat = Mat3f(tet.v[1] - tet.v[0], tet.v[2] - tet.v[0], tet.v[3] - tet.v[0]);
343
344 Real detJ = abs(tmpMat.determinant());
345 Real volume = (1.0 / 6.0) * detJ;
346 Real mass = volume * density;
347 bd.mass = mass;
348
349 Real a = density * detJ * (tet.v[0].y * tet.v[0].y + tet.v[0].y * tet.v[1].y + tet.v[1].y * tet.v[1].y + tet.v[0].y * tet.v[2].y + tet.v[1].y * tet.v[2].y + tet.v[2].y * tet.v[2].y + tet.v[0].y * tet.v[3].y + tet.v[1].y * tet.v[3].y + tet.v[2].y * tet.v[3].y + tet.v[3].y * tet.v[3].y + tet.v[0].z * tet.v[0].z + tet.v[0].z * tet.v[1].z + tet.v[1].z * tet.v[1].z + tet.v[0].z * tet.v[2].z + tet.v[1].z * tet.v[2].z + tet.v[2].z * tet.v[2].z + tet.v[0].z * tet.v[3].z + tet.v[1].z * tet.v[3].z + tet.v[2].z * tet.v[3].z + tet.v[3].z * tet.v[3].z) / 60;
350 Real b = density * detJ * (tet.v[0].x * tet.v[0].x + tet.v[0].x * tet.v[1].x + tet.v[1].x * tet.v[1].x + tet.v[0].x * tet.v[2].x + tet.v[1].x * tet.v[2].x + tet.v[2].x * tet.v[2].x + tet.v[0].x * tet.v[3].x + tet.v[1].x * tet.v[3].x + tet.v[2].x * tet.v[3].x + tet.v[3].x * tet.v[3].x + tet.v[0].z * tet.v[0].z + tet.v[0].z * tet.v[1].z + tet.v[1].z * tet.v[1].z + tet.v[0].z * tet.v[2].z + tet.v[1].z * tet.v[2].z + tet.v[2].z * tet.v[2].z + tet.v[0].z * tet.v[3].z + tet.v[1].z * tet.v[3].z + tet.v[2].z * tet.v[3].z + tet.v[3].z * tet.v[3].z) / 60;
351 Real c = density * detJ * (tet.v[0].x * tet.v[0].x + tet.v[0].x * tet.v[1].x + tet.v[1].x * tet.v[1].x + tet.v[0].x * tet.v[2].x + tet.v[1].x * tet.v[2].x + tet.v[2].x * tet.v[2].x + tet.v[0].x * tet.v[3].x + tet.v[1].x * tet.v[3].x + tet.v[2].x * tet.v[3].x + tet.v[3].x * tet.v[3].x + tet.v[0].y * tet.v[0].y + tet.v[0].y * tet.v[1].y + tet.v[1].y * tet.v[1].y + tet.v[0].y * tet.v[2].y + tet.v[1].y * tet.v[2].y + tet.v[2].y * tet.v[2].y + tet.v[0].y * tet.v[3].y + tet.v[1].y * tet.v[3].y + tet.v[2].y * tet.v[3].y + tet.v[3].y * tet.v[3].y) / 60;
352 Real a_ = density * detJ * (2 * tet.v[0].y * tet.v[0].z + tet.v[1].y * tet.v[0].z + tet.v[2].y * tet.v[0].z + tet.v[3].y * tet.v[0].z + tet.v[0].y * tet.v[1].z + 2 * tet.v[1].y * tet.v[1].z + tet.v[2].y * tet.v[1].z + tet.v[3].y * tet.v[1].z + tet.v[0].y * tet.v[2].z + tet.v[1].y * tet.v[2].z + 2 * tet.v[2].y * tet.v[2].z + tet.v[3].y * tet.v[2].z + tet.v[0].y * tet.v[3].z + tet.v[1].y * tet.v[3].z + tet.v[2].y * tet.v[3].z + 2 * tet.v[3].y * tet.v[3].z) / 120;
353 Real b_ = density * detJ * (2 * tet.v[0].x * tet.v[0].z + tet.v[1].x * tet.v[0].z + tet.v[2].x * tet.v[0].z + tet.v[3].x * tet.v[0].z + tet.v[0].x * tet.v[1].z + 2 * tet.v[1].x * tet.v[1].z + tet.v[2].x * tet.v[1].z + tet.v[3].x * tet.v[1].z + tet.v[0].x * tet.v[2].z + tet.v[1].x * tet.v[2].z + 2 * tet.v[2].x * tet.v[2].z + tet.v[3].x * tet.v[2].z + tet.v[0].x * tet.v[3].z + tet.v[1].x * tet.v[3].z + tet.v[2].x * tet.v[3].z + 2 * tet.v[3].x * tet.v[3].z) / 120;
354 Real c_ = density * detJ * (2 * tet.v[0].x * tet.v[0].y + tet.v[1].x * tet.v[0].y + tet.v[2].x * tet.v[0].y + tet.v[3].x * tet.v[0].y + tet.v[0].x * tet.v[1].y + 2 * tet.v[1].x * tet.v[1].y + tet.v[2].x * tet.v[1].y + tet.v[3].x * tet.v[1].y + tet.v[0].x * tet.v[2].y + tet.v[1].x * tet.v[2].y + 2 * tet.v[2].x * tet.v[2].y + tet.v[3].x * tet.v[2].y + tet.v[0].x * tet.v[3].y + tet.v[1].x * tet.v[3].y + tet.v[2].x * tet.v[3].y + 2 * tet.v[3].x * tet.v[3].y) / 120;
355 Mat3f inertiaMatrix(a, -b_, -c_, -b_, b, -a_, -c_, -a_, c);
356
357 bd.inertia = inertiaMatrix;
358 bd.shapeType = ET_TET;
359 bd.angle = Quat<Real>();
360
361 mHostRigidBodyStates.insert(mHostRigidBodyStates.begin() + mHostSpheres.size() + mHostBoxes.size() + mHostTets.size(), bd);
362 mHostTets.push_back(tet);
363
364 std::shared_ptr<PdActor> actor = std::make_shared<PdActor>();
365 actor->idx = mHostTets.size() - 1;
366 actor->shapeType = ET_TET;
367 actor->center = bd.position;
368 actor->rot = Quat<Real>();
369
370 return actor;
371 }
372
373 template<typename TDataType>
374 std::shared_ptr<PdActor> RigidBodySystem<TDataType>::addCapsule(
375 const CapsuleInfo& capsule,
376 const RigidBodyInfo& bodyDef,
377 const Real density /*= Real(100)*/)
378 {
379 auto b = capsule;
380 auto bd = bodyDef;
381
382 Real r = b.radius;
383 Real h = b.halfLength * 2;
384
385
386 Real mass_hemisphere = 2.0 / 3.0 * M_PI * r * r * r * density;
387 Real mass_cylinder = M_PI * r * r * h * density;
388
389 Real I_1_cylinder = 1.0 / 12.0 * mass_cylinder * (3 * r * r + h * h);
390 Real I_2_cylinder = 1.0 / 2.0 * mass_cylinder * r * r;
391
392
393 Real tmp = h / 2 + 3.0 / 8.0 * r;
394 Real I_1_hemisphere = mass_hemisphere * (2.0 / 5.0 * r * r + h * h / 2 + 3 * h * r / 8.0);
395 Real I_2_hemisphere = 2.0 / 5.0 * mass_hemisphere * r * r;
396
397 bd.mass = mass_hemisphere * 2 + mass_cylinder;
398
399 auto localInertia = Mat3f(I_1_cylinder + 2 * I_1_hemisphere, 0, 0,
400 0, I_1_cylinder + 2 * I_1_hemisphere, 0,
401 0, 0, I_2_cylinder + 2 * I_2_hemisphere);
402
403 auto rotShape = capsule.rot.toMatrix3x3();
404 auto rotBody = bd.angle.toMatrix3x3();
405
406 bd.inertia = rotBody * (rotShape * localInertia * rotShape.transpose() + ParallelAxisTheorem(capsule.center, bd.mass)) * rotBody.transpose();
407
408 bd.shapeType = ET_CAPSULE;
409
410 mHostRigidBodyStates.insert(mHostRigidBodyStates.begin() + mHostSpheres.size() + mHostBoxes.size() + mHostTets.size() + mHostCapsules.size(), bd);
411 mHostCapsules.push_back(b);
412
413 std::shared_ptr<PdActor> actor = std::make_shared<PdActor>();
414 actor->idx = mHostCapsules.size() - 1;
415 actor->shapeType = ET_CAPSULE;
416 actor->center = bd.position;
417 actor->rot = bd.angle;
418
419 return actor;
420 }
421
422
423
424 template <typename Real, typename Coord, typename Matrix, typename Quat>
425 __global__ void RB_SetupInitialStates(
426 DArray<Real> mass,
427 DArray<Coord> pos,
428 DArray<Matrix> rotation,
429 DArray<Coord> velocity,
430 DArray<Coord> angularVelocity,
431 DArray<Quat> rotation_q,
432 DArray<Matrix> inertia,
433 DArray<CollisionMask> mask,
434 DArray<Attribute> atts,
435 DArray<RigidBodyInfo> states,
436 DArray<Real> fricCoeffs,
437 ElementOffset offset)
438 {
439 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
440 if (tId >= states.size())
441 return;
442
443 mass[tId] = states[tId].mass;
444 rotation[tId] = states[tId].angle.toMatrix3x3();
445 velocity[tId] = states[tId].linearVelocity;
446 angularVelocity[tId] = rotation[tId] * states[tId].angularVelocity;
447 rotation_q[tId] = states[tId].angle;
448 pos[tId] = states[tId].position;
449 inertia[tId] = states[tId].inertia;
450 mask[tId] = states[tId].collisionMask;
451 fricCoeffs[tId] = states[tId].friction;
452
453 Attribute att_i;
454 att_i.setObjectId(states[tId].bodyId);
455 if (states[tId].motionType == BodyType::Static)
456 {
457 att_i.setFixed();
458 }
459 else if (states[tId].motionType == BodyType::Kinematic)
460 {
461 att_i.setPassive();
462 }
463 else if (states[tId].motionType == BodyType::Dynamic)
464 {
465 att_i.setDynamic();
466 }
467 atts[tId] = att_i;
468 }
469
470 __global__ void SetupBoxes(
471 DArray<dyno::Box3D> box3d,
472 DArray<BoxInfo> boxInfo)
473 {
474 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
475 if (tId >= boxInfo.size()) return;
476
477 box3d[tId].center = boxInfo[tId].center;
478 box3d[tId].extent = boxInfo[tId].halfLength;
479
480 Mat3f rot = boxInfo[tId].rot.toMatrix3x3();
481
482 box3d[tId].u = rot * Vec3f(1, 0, 0);
483 box3d[tId].v = rot * Vec3f(0, 1, 0);
484 box3d[tId].w = rot * Vec3f(0, 0, 1);
485 }
486
487 __global__ void SetupSpheres(
488 DArray<Sphere3D> sphere3d,
489 DArray<SphereInfo> sphereInfo)
490 {
491 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
492 if (tId >= sphereInfo.size()) return;
493
494 sphere3d[tId].radius = sphereInfo[tId].radius;
495 sphere3d[tId].center = sphereInfo[tId].center;
496 sphere3d[tId].rotation = sphereInfo[tId].rot;
497 }
498
499 __global__ void SetupTets(
500 DArray<Tet3D> tet3d,
501 DArray<TetInfo> tetInfo)
502 {
503 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
504 if (tId >= tetInfo.size()) return;
505
506 tet3d[tId].v[0] = tetInfo[tId].v[0];
507 tet3d[tId].v[1] = tetInfo[tId].v[1];
508 tet3d[tId].v[2] = tetInfo[tId].v[2];
509 tet3d[tId].v[3] = tetInfo[tId].v[3];
510 }
511
512 __global__ void SetupCaps(
513 DArray<Capsule3D> cap3d,
514 DArray<CapsuleInfo> capInfo)
515 {
516 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
517 if (tId >= capInfo.size()) return;
518
519 float halfLenght = capInfo[tId].halfLength;
520 Mat3f rot = capInfo[tId].rot.toMatrix3x3();
521
522 cap3d[tId].center = capInfo[tId].center;
523 cap3d[tId].rotation = capInfo[tId].rot;
524 cap3d[tId].radius = capInfo[tId].radius;
525 cap3d[tId].halfLength = capInfo[tId].halfLength;
526 }
527
528 template<typename Joint>
529 __global__ void UpdateJointIndices(
530 DArray<Joint> joints,
531 ElementOffset offset)
532 {
533 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
534 if (tId >= joints.size()) return;
535
536 Joint joint = joints[tId];
537
538 joint.bodyId1 += offset.checkElementOffset(joint.bodyType1);
539 joint.bodyId2 += offset.checkElementOffset(joint.bodyType2);
540
541 joints[tId] = joint;
542 }
543
544 template<typename TDataType>
545 void RigidBodySystem<TDataType>::resetStates()
546 {
547 auto topo = TypeInfo::cast<DiscreteElements<DataType3f>>(this->stateTopology()->getDataPtr());
548
549 mDeviceBoxes.assign(mHostBoxes);
550 mDeviceSpheres.assign(mHostSpheres);
551 mDeviceTets.assign(mHostTets);
552 mDeviceCapsules.assign(mHostCapsules);
553
554 auto& boxes = topo->boxesInLocal();
555 auto& spheres = topo->spheresInLocal();
556 auto& tets = topo->tetsInLocal();
557 auto& caps = topo->capsulesInLocal();
558
559 boxes.resize(mDeviceBoxes.size());
560 spheres.resize(mDeviceSpheres.size());
561 tets.resize(mDeviceTets.size());
562 caps.resize(mDeviceCapsules.size());
563
564 //Setup the topology
565 cuExecute(mDeviceBoxes.size(),
566 SetupBoxes,
567 boxes,
568 mDeviceBoxes);
569
570 cuExecute(mDeviceSpheres.size(),
571 SetupSpheres,
572 spheres,
573 mDeviceSpheres);
574
575 cuExecute(mDeviceTets.size(),
576 SetupTets,
577 tets,
578 mDeviceTets);
579
580 cuExecute(mDeviceCapsules.size(),
581 SetupCaps,
582 caps,
583 mDeviceCapsules);
584
585 mDeviceRigidBodyStates.assign(mHostRigidBodyStates);
586
587 int sizeOfRigidBodies = mDeviceRigidBodyStates.size();// topo->totalSize();
588
589 ElementOffset eleOffset = topo->calculateElementOffset();
590
591 this->stateRotationMatrix()->resize(sizeOfRigidBodies);
592 this->stateAngularVelocity()->resize(sizeOfRigidBodies);
593 this->stateCenter()->resize(sizeOfRigidBodies);
594 this->stateVelocity()->resize(sizeOfRigidBodies);
595 this->stateMass()->resize(sizeOfRigidBodies);
596 this->stateInertia()->resize(sizeOfRigidBodies);
597 this->stateQuaternion()->resize(sizeOfRigidBodies);
598 this->stateCollisionMask()->resize(sizeOfRigidBodies);
599 this->stateAttribute()->resize(sizeOfRigidBodies);
600 this->stateFrictionCoefficients()->resize(sizeOfRigidBodies);
601
602 cuExecute(sizeOfRigidBodies,
603 RB_SetupInitialStates,
604 this->stateMass()->getData(),
605 this->stateCenter()->getData(),
606 this->stateRotationMatrix()->getData(),
607 this->stateVelocity()->getData(),
608 this->stateAngularVelocity()->getData(),
609 this->stateQuaternion()->getData(),
610 this->stateInertia()->getData(),
611 this->stateCollisionMask()->getData(),
612 this->stateAttribute()->getData(),
613 mDeviceRigidBodyStates,
614 this->stateFrictionCoefficients()->getData(),
615 eleOffset);
616
617 this->stateInitialInertia()->resize(sizeOfRigidBodies);
618 this->stateInitialInertia()->getDataPtr()->assign(this->stateInertia()->getData());
619
620 topo->ballAndSocketJoints().assign(mHostJointsBallAndSocket);
621 topo->sliderJoints().assign(mHostJointsSlider);
622 topo->hingeJoints().assign(mHostJointsHinge);
623 topo->fixedJoints().assign(mHostJointsFixed);
624 topo->pointJoints().assign(mHostJointsPoint);
625
626 uint os = eleOffset.checkElementOffset(ET_CAPSULE);
627
628 cuExecute(topo->ballAndSocketJoints().size(),
629 UpdateJointIndices,
630 topo->ballAndSocketJoints(),
631 eleOffset);
632
633 cuExecute(topo->sliderJoints().size(),
634 UpdateJointIndices,
635 topo->sliderJoints(),
636 eleOffset);
637
638 cuExecute(topo->hingeJoints().size(),
639 UpdateJointIndices,
640 topo->hingeJoints(),
641 eleOffset);
642
643 cuExecute(topo->fixedJoints().size(),
644 UpdateJointIndices,
645 topo->fixedJoints(),
646 eleOffset);
647
648 cuExecute(topo->pointJoints().size(),
649 UpdateJointIndices,
650 topo->pointJoints(),
651 eleOffset);
652
653 setupShape2RigidBodyMapping();
654
655 topo->setPosition(this->stateCenter()->constData());
656 topo->setRotation(this->stateRotationMatrix()->constData());
657 topo->update();
658 }
659
660 template<typename TDataType>
661 void RigidBodySystem<TDataType>::postUpdateStates()
662 {
663 auto discreteSet = TypeInfo::cast<DiscreteElements<DataType3f>>(this->stateTopology()->getDataPtr());
664
665 ElementOffset offset = discreteSet->calculateElementOffset();
666
667 if (this->stateCenter()->size() <= 0)
668 {
669 return;
670 }
671
672 discreteSet->setPosition(this->stateCenter()->constData());
673 discreteSet->setRotation(this->stateRotationMatrix()->constData());
674 discreteSet->update();
675 }
676
677 template<typename TDataType>
678 void RigidBodySystem<TDataType>::clearRigidBodySystem()
679 {
680 mHostRigidBodyStates.clear();
681
682 mHostSpheres.clear();
683 mHostBoxes.clear();
684 mHostTets.clear();
685 mHostCapsules.clear();
686
687 mDeviceRigidBodyStates.clear();
688
689 mDeviceSpheres.clear();
690 mDeviceBoxes.clear();
691 mDeviceTets.clear();
692 mDeviceCapsules.clear();
693
694 mHostJointsBallAndSocket.clear();
695 mHostJointsSlider.clear();
696 mHostJointsHinge.clear();
697 mHostJointsFixed.clear();
698 mHostJointsPoint.clear();
699 mHostShape2RigidBodyMapping.clear();
700
701 m_numOfSamples = 0;
702
703 m_deviceSamples.clear();
704 m_deviceNormals.clear();
705
706 samples.clear();
707 normals.clear();
708 }
709
710 __global__ void RBS_ConstructShape2RigidBodyMapping(
711 DArray<Pair<uint, uint>> mapping)
712 {
713 int pId = threadIdx.x + (blockIdx.x * blockDim.x);
714 if (pId >= mapping.size()) return;
715
716 mapping[pId] = Pair<uint, uint>(pId, pId);
717 }
718
719 __global__ void RBS_UpdateShape2RigidBodyMapping(
720 DArray<Pair<uint, uint>> mapping,
721 ElementOffset offset)
722 {
723 int pId = threadIdx.x + (blockIdx.x * blockDim.x);
724 if (pId >= mapping.size()) return;
725
726 Pair<uint, uint> pair = mapping[pId];
727
728 uint first = pair.first;
729
730 if (pId < offset.boxIndex())
731 {
732 }
733 else if (pId < offset.tetIndex())
734 {
735 first += offset.boxIndex();
736 }
737 else if (pId < offset.capsuleIndex())
738 {
739 first += offset.tetIndex();
740 }
741 else if (pId < offset.triangleIndex())
742 {
743 first += offset.capsuleIndex();
744 }
745 else
746 {
747 first += offset.triangleIndex();
748 }
749
750 mapping[pId] = Pair<uint, uint>(first, pair.second);
751 }
752
753 template<typename TDataType>
754 void RigidBodySystem<TDataType>::setupShape2RigidBodyMapping()
755 {
756 auto topo = this->stateTopology()->getDataPtr();
757 auto& mapping = topo->shape2RigidBodyMapping();
758
759 uint totalSize = topo->totalSize();
760
761 if (mHostShape2RigidBodyMapping.size() == 0)
762 {
763 mapping.resize(totalSize);
764
765 cuExecute(totalSize,
766 RBS_ConstructShape2RigidBodyMapping,
767 mapping);
768 }
769 else
770 {
771 mapping.assign(mHostShape2RigidBodyMapping);
772
773 cuExecute(totalSize,
774 RBS_UpdateShape2RigidBodyMapping,
775 mapping,
776 topo->calculateElementOffset());
777
778 return;
779 }
780 }
781
782 DEFINE_CLASS(RigidBodySystem);
783}
Generated by doxygen 1.13.2