PeriDyno 1.0.0
Loading...
Searching...
No Matches
Vehicle.cpp
Go to the documentation of this file.
1#include "Vehicle.h"
2
3#include "Module/CarDriver.h"
4
5#include "Mapping/DiscreteElementsToTriangleSet.h"
6#include "GLSurfaceVisualModule.h"
7#include "GLWireframeVisualModule.h"
8
9//Modeling
10#include "GltfFunc.h"
11
12//Rigidbody
13#include "Module/InstanceTransform.h"
14
15//Rendering
16#include "Module/GLPhotorealisticInstanceRender.h"
17
18
19namespace dyno
20{
21 //Jeep
22 IMPLEMENT_TCLASS(Jeep, TDataType)
23
24 template<typename TDataType>
25 Jeep<TDataType>::Jeep() :
26 ArticulatedBody<TDataType>()
27 {
28 auto driver = std::make_shared<CarDriver<DataType3f>>();
29 this->stateTopology()->connect(driver->inTopology());
30 this->animationPipeline()->pushModule(driver);
31 }
32
33 template<typename TDataType>
34 Jeep<TDataType>::~Jeep()
35 {
36
37 }
38
39 template<typename TDataType>
40 void Jeep<TDataType>::resetStates()
41 {
42 this->clearRigidBodySystem();
43 this->clearVechicle();
44
45 std::string filename = getAssetPath() + "Jeep/JeepGltf/jeep.gltf";
46 if (this->varFilePath()->getValue() != filename)
47 {
48 this->varFilePath()->setValue(FilePath(filename));
49 }
50
51 auto instances = this->varVehiclesTransform()->getValue();
52 uint vehicleNum = instances.size();
53 for (size_t i = 0; i < vehicleNum; i++)
54 {
55 RigidBodyInfo rigidbody;
56 rigidbody.bodyId = i;
57 rigidbody.friction = this->varFrictionCoefficient()->getValue();
58
59 auto texMesh = this->stateTextureMesh()->constDataPtr();
60
61 //wheel
62 std::vector <int> Wheel_Id = { 0,1,2,3 };
63 std::map<int, CapsuleInfo> wheels;
64 std::map<int, std::shared_ptr<PdActor>> actors;
65 //Capsule
66 for (auto it : Wheel_Id)
67 {
68 auto up = texMesh->shapes()[it]->boundingBox.v1;
69 auto down = texMesh->shapes()[it]->boundingBox.v0;
70
71 wheels[it].center = Vec3f(0.0f);
72 wheels[it].rot = Quat1f(M_PI / 2, Vec3f(0, 0, 1));
73 wheels[it].halfLength = 0.1;
74 wheels[it].radius = std::abs(up.y - down.y) / 2;
75
76 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
77 rigidbody.angle = Quat1f(instances[i].rotation());
78
79 actors[it] = this->addCapsule(wheels[it], rigidbody, 100);
80 }
81
82
83 //body
84 int body = 5;
85 int backWheel = 4;
86
87 std::vector <int> box_Id = { body,backWheel };
88 std::map<int, BoxInfo> boxs;
89 //box
90 for (auto it : box_Id)
91 {
92 auto up = texMesh->shapes()[it]->boundingBox.v1;
93 auto down = texMesh->shapes()[it]->boundingBox.v0;
94
95 boxs[it].center = Vec3f(0.0f);
96
97 boxs[it].halfLength = (up - down) / 2;
98
99 rigidbody.offset = Vec3f(0.0f, 0.0f, 0.0f);
100 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
101 rigidbody.angle = Quat1f(instances[i].rotation());
102 actors[it] = this->addBox(boxs[it], rigidbody, 100);
103 }
104
105 //bindShapetoActor
106 for (auto it : box_Id)
107 {
108 this->bind(actors[it], Pair<uint, uint>(it, i));
109 }
110 //bindShapetoActor
111 for (auto it : Wheel_Id)
112 {
113 this->bind(actors[it], Pair<uint, uint>(it, i));
114 }
115
116 rigidbody.offset = Vec3f(0);
117
118 Real wheel_velocity = 10;
119
120 //wheel to Body
121 for (auto it : Wheel_Id)
122 {
123 auto& joint = this->createHingeJoint(actors[it], actors[body]);
124 joint.setAnchorPoint(actors[it]->center);
125 joint.setMoter(wheel_velocity);
126 joint.setAxis(Quat1f(instances[i].rotation()).rotate(Vec3f(1, 0, 0)));
127 }
128
129 auto& jointBackWheel_Body = this->createFixedJoint(actors[backWheel], actors[body]);
130 jointBackWheel_Body.setAnchorPoint(actors[backWheel]->center); //set and offset
131 }
132
133 //**************************************************//
134 ArticulatedBody<TDataType>::resetStates();
135 }
136
137 DEFINE_CLASS(Jeep);
138
139 //Tank
140 IMPLEMENT_TCLASS(Tank, TDataType)
141
142 template<typename TDataType>
143 Tank<TDataType>::Tank() :
144 ArticulatedBody<TDataType>()
145 {
146 auto driver = std::make_shared<CarDriver<DataType3f>>();
147 this->stateTopology()->connect(driver->inTopology());
148 this->animationPipeline()->pushModule(driver);
149 }
150
151 template<typename TDataType>
152 Tank<TDataType>::~Tank()
153 {
154
155 }
156
157 template<typename TDataType>
158 void Tank<TDataType>::resetStates()
159 {
160 this->clearRigidBodySystem();
161 this->clearVechicle();
162
163 std::string filename = getAssetPath() + "gltf/Tank/Tank.gltf";
164 if (this->varFilePath()->getValue() != filename)
165 {
166 this->varFilePath()->setValue(FilePath(filename));
167 }
168
169 auto instances = this->varVehiclesTransform()->getValue();
170 uint vehicleNum = instances.size();
171 for (size_t i = 0; i < vehicleNum; i++)
172 {
173 RigidBodyInfo rigidbody;
174 rigidbody.bodyId = i;
175 rigidbody.friction = this->varFrictionCoefficient()->getValue();
176
177 auto texMesh = this->stateTextureMesh()->constDataPtr();
178
179 //wheel
180 std::vector <int> Wheel_Id = { 2,3,4,5,6,7,8,9,10,11,12,13,14,15 };
181 std::map<int, CapsuleInfo> wheels;
182 std::map<int, std::shared_ptr<PdActor>> actors;
183 //Capsule
184 for (auto it : Wheel_Id)
185 {
186 auto up = texMesh->shapes()[it]->boundingBox.v1;
187 auto down = texMesh->shapes()[it]->boundingBox.v0;
188
189 wheels[it].center = Vec3f(0.0f);
190 wheels[it].rot = Quat1f(M_PI / 2, Vec3f(0, 0, 1));
191 wheels[it].halfLength = 0.1;
192 wheels[it].radius = std::abs(up.y - down.y) / 2;
193
194 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
195 rigidbody.angle = Quat1f(instances[i].rotation());
196 actors[it] = this->addCapsule(wheels[it], rigidbody, 100);
197 }
198
199
200 //body
201 int body = 0;
202 int gun = 1;
203
204 std::vector <int> box_Id = { body,gun };
205 std::map<int, BoxInfo> boxs;
206 //box
207 for (auto it : box_Id)
208 {
209 auto up = texMesh->shapes()[it]->boundingBox.v1;
210 auto down = texMesh->shapes()[it]->boundingBox.v0;
211
212 boxs[it].center = Vec3f(0.0f);
213
214 boxs[it].halfLength = (up - down) / 2 * 0.5;
215
216 rigidbody.offset = Vec3f(0.0f, 0.0f, 0.0f);
217 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
218 rigidbody.angle = Quat1f(instances[i].rotation());
219 actors[it] = this->addBox(boxs[it], rigidbody, 100);
220 }
221
222 //bindShapetoActor
223 for (auto it : box_Id)
224 {
225 this->bind(actors[it], Pair<uint, uint>(it, i));
226 }
227 //bindShapetoActor
228 for (auto it : Wheel_Id)
229 {
230 this->bind(actors[it], Pair<uint, uint>(it, i));
231 }
232
233 rigidbody.offset = Vec3f(0);
234
235 Real wheel_velocity = 10;
236
237 //wheel to Body
238 for (auto it : Wheel_Id)
239 {
240 auto& joint = this->createHingeJoint(actors[it], actors[body]);
241 joint.setAnchorPoint(actors[it]->center);
242 joint.setMoter(wheel_velocity);
243 joint.setAxis(Quat1f(instances[i].rotation()).rotate(Vec3f(1, 0, 0)));
244 }
245
246 auto& jointGun_Body = this->createFixedJoint(actors[gun], actors[body]);
247 jointGun_Body.setAnchorPoint(actors[gun]->center); //set and offset
248 }
249
250 //**************************************************//
251 ArticulatedBody<TDataType>::resetStates();
252 }
253
254 DEFINE_CLASS(Tank);
255
256 //TrackedTank
257 IMPLEMENT_TCLASS(TrackedTank, TDataType)
258
259 template<typename TDataType>
260 TrackedTank<TDataType>::TrackedTank() :
261 ArticulatedBody<TDataType>()
262 {
263 auto driver = std::make_shared<CarDriver<DataType3f>>();
264 this->stateTopology()->connect(driver->inTopology());
265 this->animationPipeline()->pushModule(driver);
266
267 this->statecaterpillarTrack()->setDataPtr(std::make_shared<EdgeSet<DataType3f>>());
268 auto wireframe = std::make_shared<GLWireframeVisualModule>();
269 this->statecaterpillarTrack()->connect(wireframe->inEdgeSet());
270 this->graphicsPipeline()->pushModule(wireframe);
271
272 }
273
274 template<typename TDataType>
279
280
281 template<typename TDataType>
282 void TrackedTank<TDataType>::resetStates()
283 {
284 this->clearRigidBodySystem();
285 this->clearVechicle();
286
287 std::string filename = getAssetPath() + "gltf/Tank/TrackedTank.gltf";
288 if (this->varFilePath()->getValue() != filename)
289 {
290 this->varFilePath()->setValue(FilePath(filename));
291 }
292
293 auto instances = this->varVehiclesTransform()->getValue();
294 uint vehicleNum = instances.size();
295 for (size_t i = 0; i < vehicleNum; i++)
296 {
297 RigidBodyInfo rigidbody;
298 rigidbody.bodyId = i;
299 rigidbody.friction = this->varFrictionCoefficient()->getValue();
300
301 auto texMesh = this->stateTextureMesh()->constDataPtr();
302 std::map<int, std::shared_ptr<PdActor>> actors;
303
304#define FULLBODY
305
306#ifdef FULLBODY
307 //wheel
308 std::vector <int> Wheel_Id = { 1,2,3,4,6,7,8,10,11,12,13,15,16,17 };
309 //Capsule
310 for (auto it : Wheel_Id)
311 {
312 auto up = texMesh->shapes()[it]->boundingBox.v1;
313 auto down = texMesh->shapes()[it]->boundingBox.v0;
314
315 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
316 rigidbody.angle = Quat1f(instances[i].rotation());
317 rigidbody.motionType = BodyType::Static;
318 auto actor = this->createRigidBody(rigidbody);
319 actors[it] = actor;
320
321 CapsuleInfo capsule;
322 capsule.rot = Quat1f(M_PI / 2, Vec3f(0, 0, 1));
323 capsule.radius = (up[1] - down[1]) / 2;
324 capsule.halfLength = (up[2] - down[2]) / 2;
325
326 float r = (up[1] - down[1]) / 2;
327 this->bindCapsule(actor, capsule);
328 this->bind(actor, Pair<uint, uint>(it, i));
329
330 }
331
332
333 //Gear
334 std::vector<int> gear = {0,5,9,14};
335 for (size_t c = 0; c < 4; c++)
336 {
337 int cid = gear[c];
338 auto up = texMesh->shapes()[cid]->boundingBox.v1;
339 auto down = texMesh->shapes()[cid]->boundingBox.v0;
340 //first gear
341 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[cid]->boundingTransform.translation()) + instances[i].translation();
342 rigidbody.angle = Quat1f(instances[i].rotation());
343
344 rigidbody.motionType = BodyType::Static;
345 auto actor = this->createRigidBody(rigidbody);
346 actors[cid] = actor;
347
348 for (uint sec = 0; sec < 14; sec++)
349 {
350 CapsuleInfo capsule;
351 capsule.radius = 0.03;
352 capsule.halfLength = (up[1] - down[1]) / 2;
353 Vec3f offset = sec <=6 ? Vec3f(-0.1,0,0): Vec3f(0.1, 0, 0);
354 float theta = sec * M_PI / 7;
355
356 capsule.rot = Quat1f(sec * M_PI / 7, Vec3f(1, 0, 0));
357 capsule.center = offset;
358 this->bindCapsule(actor, capsule);
359
360 //row
361 float r = (up[1] - down[1]) / 3;
362 float theta2 = sec * M_PI / 7;
363 float y = r * sin(theta2);
364 float z = r * cos(theta2);
365
366 capsule.rot = Quat1f(M_PI / 2, Vec3f(0, 0, 1));
367 capsule.radius = 0.05f;
368 capsule.halfLength = (up[0] - down[0]) / 2;
369 capsule.center = Vec3f(0, y, z);
370 this->bindCapsule(actor, capsule);
371 }
372
373 this->bind(actor, Pair<uint, uint>(cid, i));
374
375
376 }
377
378 //Gun
379 int head = 18;
380 {
381 auto up = texMesh->shapes()[head]->boundingBox.v1;
382 auto down = texMesh->shapes()[head]->boundingBox.v0;
383
384 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[head]->boundingTransform.translation()) + instances[i].translation();
385 rigidbody.angle = Quat1f(instances[i].rotation());
386
387 rigidbody.motionType = BodyType::Static;
388 auto actor = this->createRigidBody(rigidbody);
389 actors[head] = actor;
390
391 BoxInfo box;
392 box.halfLength = Vec3f(0.8,0.4,1.2);
393 box.center = Vec3f(0,-0.2,-1);
394
395 this->bindBox(actor, box);
396
397 CapsuleInfo capsule;
398 capsule.rot = Quat1f(M_PI / 2, Vec3f(1, 0, 0));
399 capsule.radius = 0.1f;
400 capsule.halfLength = (up[2] - down[2]) / 3;
401 capsule.center = Vec3f(0.3,-0.2,0.5);
402 this->bindCapsule(actor, capsule);
403 capsule.center = Vec3f(-0.3, -0.2, 0.5);
404 this->bindCapsule(actor, capsule);
405
406 this->bind(actor, Pair<uint, uint>(head, i));
407 }
408 //Body
409 int body = 19;
410 {
411 auto up = texMesh->shapes()[body]->boundingBox.v1;
412 auto down = texMesh->shapes()[body]->boundingBox.v0;
413
414 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[body]->boundingTransform.translation()) + instances[i].translation();
415 rigidbody.angle = Quat1f(instances[i].rotation());
416
417 rigidbody.motionType = BodyType::Static;
418 auto actor = this->createRigidBody(rigidbody);
419 actors[body] = actor;
420
421 BoxInfo box;
422 box.rot = Quat1f(0, Vec3f(0, 0, 1));
423 box.halfLength = (up - down) / Vec3f(3,2,3);
424
425 this->bindBox(actor, box);
426 this->bind(actor, Pair<uint, uint>(body, i));
427 }
428#endif // FULLBODY
429
430 std::vector<int> caterpillarTrack_L;
431 std::vector<int> caterpillarTrack_R;
432
433 for (int cid = 20; cid < 160; cid++)
434 {
435 auto up = texMesh->shapes()[cid]->boundingBox.v1;
436 auto down = texMesh->shapes()[cid]->boundingBox.v0;
437
438 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[cid]->boundingTransform.translation()) + instances[i].translation();
439 rigidbody.angle = Quat1f(instances[i].rotation());
440
441 rigidbody.motionType = BodyType::Static;
442 auto actor = this->createRigidBody(rigidbody);
443 actors[cid] = actor;
444 if(cid<90)
445 caterpillarTrack_L.push_back(cid);
446 else
447 caterpillarTrack_R.push_back(cid);
448
449
450 CapsuleInfo capsule;
451 capsule.rot = Quat1f(M_PI / 2, Vec3f(0, 0, 1));
452 capsule.radius = 0.05;
453 capsule.halfLength = (up[0] - down[0]) / 2;
454
455 this->bindCapsule(actor, capsule);
456
457 this->bind(actor, Pair<uint, uint>(cid, i));
458 }
459
460#ifdef FULLBODY
461 Real wheel_velocity = 10;
462
463 //weel to Body Joint
464 for (auto it : Wheel_Id)
465 {
466 auto& joint = this->createHingeJoint(actors[it], actors[body]);
467 joint.setAnchorPoint(actors[it]->center);
468 joint.setMoter(wheel_velocity);
469 joint.setAxis(Quat1f(instances[i].rotation()).rotate(Vec3f(1, 0, 0)));
470 }
471
472 //Gear to Body Joint
473 for (auto it : gear)
474 {
475 auto& joint = this->createHingeJoint(actors[it], actors[body]);
476 joint.setAnchorPoint(actors[it]->center);
477 joint.setMoter(wheel_velocity);
478 joint.setAxis(Quat1f(instances[i].rotation()).rotate(Vec3f(1, 0, 0)));
479 }
480
481 auto& jointGun_Body = this->createFixedJoint(actors[head], actors[body]);
482 jointGun_Body.setAnchorPoint(actors[body]->center);
483
484#endif // FULLBODY
485
486 //Caterpillar Track Joint
487 auto edgeset = this->statecaterpillarTrack()->getDataPtr();
488 std::vector<TopologyModule::Edge> edges;
489 std::vector<Vec3f> points;
490 for (int k = 0; k < caterpillarTrack_L.size(); k++)
491 {
492 auto start = caterpillarTrack_L[k];
493 auto end = k != caterpillarTrack_L.size() - 1 ? caterpillarTrack_L[k + 1] : caterpillarTrack_L[0];
494
495 auto& joint = this->createHingeJoint(actors[start], actors[end]);
496 joint.setAnchorPoint((actors[start]->center + actors[end]->center) / 2);
497 joint.setAxis(Quat1f(instances[i].rotation()).rotate(Vec3f(1, 0, 0)));
498
499 points.push_back(actors[start]->center);
500
501 if (k != caterpillarTrack_L.size() - 1)
502 edges.push_back(TopologyModule::Edge(k, k + 1));
503 else
504 edges.push_back(TopologyModule::Edge(k, 0));
505
506
507 }
508 auto edgeOffset = points.size();
509 for (int k = 0; k < caterpillarTrack_R.size(); k++)
510 {
511 auto start = caterpillarTrack_R[k];
512 auto end = k != caterpillarTrack_R.size() - 1 ? caterpillarTrack_R[k + 1] : caterpillarTrack_R[0];
513
514 auto& joint = this->createHingeJoint(actors[start], actors[end]);
515 joint.setAnchorPoint((actors[start]->center + actors[end]->center) / 2);
516 joint.setAxis(Quat1f(instances[i].rotation()).rotate(Vec3f(1, 0, 0)));
517
518 points.push_back(actors[start]->center);
519
520 if (k != caterpillarTrack_R.size() - 1)
521 edges.push_back(TopologyModule::Edge(k + edgeOffset, k + 1 + edgeOffset));
522 else
523 edges.push_back(TopologyModule::Edge(k + edgeOffset, 0 + edgeOffset));
524
525 }
526 edgeset->setPoints(points);
527 edgeset->setEdges(edges);
528 edgeset->update();
529
530
531 }
532
533 //**************************************************//
534 ArticulatedBody<TDataType>::resetStates();
535
536 }
537
538 DEFINE_CLASS(TrackedTank);
539
540 //UAV
541 IMPLEMENT_TCLASS(UAV, TDataType)
542
543 template<typename TDataType>
544 UAV<TDataType>::UAV() :
545 ArticulatedBody<TDataType>()
546 {
547 auto driver = std::make_shared<CarDriver<DataType3f>>();
548 this->stateTopology()->connect(driver->inTopology());
549 this->animationPipeline()->pushModule(driver);
550 }
551
552 template<typename TDataType>
553 UAV<TDataType>::~UAV()
554 {
555
556 }
557
558 template<typename TDataType>
559 void UAV<TDataType>::resetStates()
560 {
561 this->clearRigidBodySystem();
562 this->clearVechicle();
563
564 std::string filename = getAssetPath() + "gltf/UAV/UAV.gltf";
565 if (this->varFilePath()->getValue() != filename)
566 {
567 this->varFilePath()->setValue(FilePath(filename));
568 }
569
570 auto instances = this->varVehiclesTransform()->getValue();
571 uint vehicleNum = instances.size();
572 for (size_t i = 0; i < vehicleNum; i++)
573 {
574 RigidBodyInfo rigidbody;
575 rigidbody.bodyId = i;
576 rigidbody.friction = this->varFrictionCoefficient()->getValue();
577
578 auto texMesh = this->stateTextureMesh()->constDataPtr();
579
580 //wheel
581 std::vector <int> Wheel_Id = { 0,1,2,3 };
582 std::map<int, CapsuleInfo> wheels;
583 std::map<int, std::shared_ptr<PdActor>> actors;
584 //Capsule
585 for (auto it : Wheel_Id)
586 {
587 auto up = texMesh->shapes()[it]->boundingBox.v1;
588 auto down = texMesh->shapes()[it]->boundingBox.v0;
589
590 wheels[it].center = Vec3f(0.0f);
591
592 auto rot = it == 0 || it == 3 ? Vec3f(90, 45, 0) : Vec3f(90, -45, 0);
593
594 Quat<Real> q =
595 Quat<Real>(Real(M_PI) * rot[2] / 180, Coord(0, 0, 1))
596 * Quat<Real>(Real(M_PI) * rot[1] / 180, Coord(0, 1, 0))
597 * Quat<Real>(Real(M_PI) * rot[0] / 180, Coord(1, 0, 0));
598 q.normalize();
599
600
601 wheels[it].rot = q;
602 wheels[it].halfLength = std::abs(up.x - down.x) / 1.5;
603 wheels[it].radius = std::abs(up.y - down.y) / 2;
604
605 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
606 rigidbody.angle = Quat1f(instances[i].rotation());
607
608 actors[it] = this->addCapsule(wheels[it], rigidbody, 100);
609 }
610
611
612 //body
613 int body = 4;
614
615 std::vector <int> box_Id = { body };
616 std::map<int, BoxInfo> boxs;
617 //box
618 for (auto it : box_Id)
619 {
620 auto up = texMesh->shapes()[it]->boundingBox.v1;
621 auto down = texMesh->shapes()[it]->boundingBox.v0;
622
623 boxs[it].center = Vec3f(0.0f);
624
625 boxs[it].halfLength = (up - down) / 2 ;
626
627 rigidbody.offset = Vec3f(0.0f, 0.0f, 0.0f);
628 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
629 rigidbody.angle = Quat1f(instances[i].rotation());
630
631 actors[it] = this->addBox(boxs[it], rigidbody, 100);
632 }
633
634 //bindShapetoActor
635 for (auto it : box_Id)
636 {
637 this->bind(actors[it], Pair<uint, uint>(it, i));
638 }
639 //bindShapetoActor
640 for (auto it : Wheel_Id)
641 {
642 this->bind(actors[it], Pair<uint, uint>(it, i));
643 }
644
645 rigidbody.offset = Vec3f(0);
646
647
648
649 //wheel to Body
650 for (auto it : Wheel_Id)
651 {
652 Real wheel_velocity = (it == 0 || it == 3) ? Real(20.0) : Real(-20.0);
653
654 auto& joint = this->createHingeJoint(actors[it], actors[body]);
655 joint.setAnchorPoint(actors[it]->center);
656 joint.setMoter(wheel_velocity);
657 joint.setAxis(Quat1f(instances[i].rotation()).rotate(Vec3f(0, 1, 0)));
658 }
659
660 }
661
662 //**************************************************//
663 ArticulatedBody<TDataType>::resetStates();
664 }
665
666 DEFINE_CLASS(UAV);
667
668
669 //UUV
670 IMPLEMENT_TCLASS(UUV, TDataType)
671
672 template<typename TDataType>
673 UUV<TDataType>::UUV() :
674 ArticulatedBody<TDataType>()
675 {
676 auto driver = std::make_shared<CarDriver<DataType3f>>();
677 this->stateTopology()->connect(driver->inTopology());
678 this->animationPipeline()->pushModule(driver);
679 }
680
681 template<typename TDataType>
682 UUV<TDataType>::~UUV()
683 {
684
685 }
686
687 template<typename TDataType>
688 void UUV<TDataType>::resetStates()
689 {
690 this->clearRigidBodySystem();
691 this->clearVechicle();
692
693 std::string filename = getAssetPath() + "gltf/UUV/UUV.gltf";
694 if (this->varFilePath()->getValue() != filename)
695 {
696 this->varFilePath()->setValue(FilePath(filename));
697 }
698
699 auto instances = this->varVehiclesTransform()->getValue();
700 uint vehicleNum = instances.size();
701 for (size_t i = 0; i < vehicleNum; i++)
702 {
703 RigidBodyInfo rigidbody;
704 rigidbody.bodyId = i;
705 rigidbody.friction = this->varFrictionCoefficient()->getValue();
706
707 auto texMesh = this->stateTextureMesh()->constDataPtr();
708
709 //wheel
710 std::vector <int> Wheel_Id = { 1,2,3,4 };
711 std::map<int, CapsuleInfo> wheels;
712 std::map<int, std::shared_ptr<PdActor>> actors;
713 //Capsule
714 for (auto it : Wheel_Id)
715 {
716 auto up = texMesh->shapes()[it]->boundingBox.v1;
717 auto down = texMesh->shapes()[it]->boundingBox.v0;
718
719 wheels[it].center = Vec3f(0.0f);
720
721 auto rot = it == 1 || it == 2 ? Vec3f(90, 30, 0) : Vec3f(0, 0, 30);
722
723 Quat<Real> q =
724 Quat<Real>(Real(M_PI) * rot[2] / 180, Coord(0, 0, 1))
725 * Quat<Real>(Real(M_PI) * rot[1] / 180, Coord(0, 1, 0))
726 * Quat<Real>(Real(M_PI) * rot[0] / 180, Coord(1, 0, 0));
727 q.normalize();
728
729
730 wheels[it].rot = q;
731 wheels[it].halfLength = 0.13;
732 wheels[it].radius = 0.03;
733
734 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
735 rigidbody.angle = Quat1f(instances[i].rotation());
736
737 actors[it] = this->addCapsule(wheels[it], rigidbody, 100);
738 }
739
740
741 //body
742 int body = 0;
743
744 std::vector <int> box_Id = { body };
745 std::map<int, BoxInfo> boxs;
746 //box
747 for (auto it : box_Id)
748 {
749 auto up = texMesh->shapes()[it]->boundingBox.v1;
750 auto down = texMesh->shapes()[it]->boundingBox.v0;
751
752 boxs[it].center = Vec3f(0.0f);
753
754 boxs[it].halfLength = (up - down) / 2;
755
756 rigidbody.offset = Vec3f(0.0f, 0.0f, 0.0f);
757 rigidbody.position = Quat1f(instances[i].rotation()).rotate(texMesh->shapes()[it]->boundingTransform.translation()) + instances[i].translation();
758 rigidbody.angle = Quat1f(instances[i].rotation());
759
760 actors[it] = this->addBox(boxs[it], rigidbody, 100);
761 }
762
763 //bindShapetoActor
764 for (auto it : box_Id)
765 {
766 this->bind(actors[it], Pair<uint, uint>(it, i));
767 }
768 //bindShapetoActor
769 for (auto it : Wheel_Id)
770 {
771 this->bind(actors[it], Pair<uint, uint>(it, i));
772 }
773
774 rigidbody.offset = Vec3f(0);
775
776
777
778 //wheel to Body
779 for (auto it : Wheel_Id)
780 {
781 Real wheel_velocity = 20;
782
783 auto& joint = this->createHingeJoint(actors[it], actors[body]);
784 joint.setAnchorPoint(actors[it]->center);
785 joint.setMoter(wheel_velocity);
786 Vec3f axis = it == 1 || it == 2 ? Vec3f(0, 1, 0) : Vec3f(0, 0, 1);
787 joint.setAxis(Quat1f(instances[i].rotation()).rotate(axis));
788 }
789
790 }
791
792 //**************************************************//
793 ArticulatedBody<TDataType>::resetStates();
794 }
795
796 DEFINE_CLASS(UUV);
797
798}
DEFINE_CLASS
#define DEFINE_CLASS(name)
Definition Object.h:140
IMPLEMENT_TCLASS
#define IMPLEMENT_TCLASS(name, T1)
Definition Object.h:103
Real
double Real
Definition Typedef.inl:23
M_PI
#define M_PI
Definition Typedef.inl:36
dyno::ArticulatedBody::clearVechicle
void clearVechicle()
dyno::ArticulatedBody::ArticulatedBody
ArticulatedBody()
dyno::ArticulatedBody::bind
void bind(std::shared_ptr< PdActor > actor, Pair< uint, uint > shapeId)
dyno::ArticulatedBody::resetStates
void resetStates() override
dyno::Jeep::Real
TDataType::Real Real
Definition Vehicle.h:33
dyno::Jeep::resetStates
void resetStates() override
Definition Vehicle.cpp:40
dyno::Jeep::~Jeep
~Jeep() override
Definition Vehicle.cpp:34
dyno::Node::graphicsPipeline
std::shared_ptr< GraphicsPipeline > graphicsPipeline()
Definition Node.cpp:320
dyno::Node::animationPipeline
std::shared_ptr< AnimationPipeline > animationPipeline()
Definition Node.cpp:311
dyno::Quat::normalize
DYN_FUNC Quat< Real > & normalize()
Definition Quat.inl:183
dyno::Quat::rotate
DYN_FUNC Vector< Real, 3 > rotate(const Vector< Real, 3 > &v) const
Rotate a vector by the quaternion, guarantee the quaternion is normalized before rotating the vector.
Definition Quat.inl:259
dyno::RigidBodySystem::addBox
std::shared_ptr< PdActor > addBox(const BoxInfo &box, const RigidBodyInfo &bodyDef, const Real density=Real(100))
dyno::RigidBodySystem::createHingeJoint
HingeJoint & createHingeJoint(std::shared_ptr< PdActor > actor1, std::shared_ptr< PdActor > actor2)
Definition RigidBodySystem.h:117
dyno::RigidBodySystem::clearRigidBodySystem
void clearRigidBodySystem()
dyno::RigidBodySystem::createRigidBody
std::shared_ptr< PdActor > createRigidBody(const Coord &p, const TQuat &q)
dyno::RigidBodySystem::createFixedJoint
FixedJoint & createFixedJoint(std::shared_ptr< PdActor > actor1, std::shared_ptr< PdActor > actor2)
Definition RigidBodySystem.h:127
dyno::RigidBodySystem::bindBox
void bindBox(const std::shared_ptr< PdActor > actor, const BoxInfo &box, const Real density=Real(100))
dyno::RigidBodySystem::addCapsule
std::shared_ptr< PdActor > addCapsule(const CapsuleInfo &capsule, const RigidBodyInfo &bodyDef, const Real density=Real(100))
dyno::RigidBodySystem::bindCapsule
void bindCapsule(const std::shared_ptr< PdActor > actor, const CapsuleInfo &capsule, const Real density=Real(100))
dyno::Tank::~Tank
~Tank() override
Definition Vehicle.cpp:152
dyno::Tank::resetStates
void resetStates() override
Definition Vehicle.cpp:158
dyno::Tank::Real
TDataType::Real Real
Definition Vehicle.h:48
dyno::TopologyModule::Edge
VectorND< PointType, 2 > Edge
Definition TopologyModule.h:30
dyno::TrackedTank::resetStates
void resetStates() override
Definition Vehicle.cpp:282
dyno::TrackedTank::~TrackedTank
~TrackedTank() override
Definition Vehicle.cpp:275
dyno::TrackedTank::Real
TDataType::Real Real
Definition Vehicle.h:65
dyno::UAV::~UAV
~UAV() override
Definition Vehicle.cpp:553
dyno::UAV::Real
TDataType::Real Real
Definition Vehicle.h:86
dyno::UAV::Coord
TDataType::Coord Coord
Definition Vehicle.h:87
dyno::UAV::resetStates
void resetStates() override
Definition Vehicle.cpp:559
dyno::UUV::Real
TDataType::Real Real
Definition Vehicle.h:103
dyno::UUV::resetStates
void resetStates() override
Definition Vehicle.cpp:688
dyno::UUV::~UUV
~UUV() override
Definition Vehicle.cpp:682
dyno::UUV::Coord
TDataType::Coord Coord
Definition Vehicle.h:104
dyno
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
dyno::joint
int joint
Definition GltfFunc.h:19
dyno::sin
DYN_FUNC Complex< Real > sin(const Complex< Real > &)
Definition Complex.inl:564
dyno::Quat1f
Quat< float > Quat1f
Definition Quat.h:136
dyno::cos
DYN_FUNC Complex< Real > cos(const Complex< Real > &)
Definition Complex.inl:573
dyno::Vec3f
Vector< float, 3 > Vec3f
Definition Vector3D.h:93
dyno::uint
unsigned int uint
Definition VkProgram.h:14
dyno::BoxInfo::rot
Quat< Real > rot
Definition CollisionData.h:102
dyno::BoxInfo::halfLength
Vector< Real, 3 > halfLength
Definition CollisionData.h:100
dyno::BoxInfo::center
Vector< Real, 3 > center
Definition CollisionData.h:99
dyno::CapsuleInfo::center
Vector< Real, 3 > center
Definition CollisionData.h:141
dyno::RigidBodyInfo
Definition RigidBodyShared.h:38
dyno::RigidBodyInfo::bodyId
uint bodyId
Definition RigidBodyShared.h:90
dyno::RigidBodyInfo::offset
Vector< Real, 3 > offset
An offset from the barycenter to the geometric center.
Definition RigidBodyShared.h:85
dyno::RigidBodyInfo::position
Vector< Real, 3 > position
The barycenter of the body.
Definition RigidBodyShared.h:82
dyno::RigidBodyInfo::friction
Real friction
Definition RigidBodyShared.h:94
dyno::RigidBodyInfo::motionType
BodyType motionType
Definition RigidBodyShared.h:98
dyno::RigidBodyInfo::angle
Quat< Real > angle
Definition RigidBodyShared.h:73
Generated by doxygen 1.13.2