PeriDyno 1.2.1
Loading...
Searching...
No Matches
ConfigurableBody.cpp
Go to the documentation of this file.
1#include "ConfigurableBody.h"
2
3#include "Module/CarDriver.h"
4
5//Collision
8
9//RigidBody
14
15//Rendering
17
18namespace dyno
19{
20 //ConfigurableVehicle
22
23 template<typename TDataType>
25 : ParametricModel<TDataType>()
26 , ArticulatedBody<TDataType>()
27 {
28 auto elementQuery = std::make_shared<NeighborElementQuery<TDataType>>();
29 elementQuery->varSelfCollision()->setValue(false);
30 this->stateTopology()->connect(elementQuery->inDiscreteElements());
31 this->stateCollisionMask()->connect(elementQuery->inCollisionMask());
32 this->stateAttribute()->connect(elementQuery->inAttribute());
33 this->animationPipeline()->pushModule(elementQuery);
34
35 auto cdBV = std::make_shared<CollistionDetectionTriangleSet<TDataType>>();
36 this->stateTopology()->connect(cdBV->inDiscreteElements());
37 this->inTriangleSet()->connect(cdBV->inTriangleSet());
38 // auto cdBV = std::make_shared<CollistionDetectionBoundingBox<TDataType>>();
39 // this->stateTopology()->connect(cdBV->inDiscreteElements());
40 this->animationPipeline()->pushModule(cdBV);
41
42
43 auto merge = std::make_shared<ContactsUnion<TDataType>>();
44 elementQuery->outContacts()->connect(merge->inContactsA());
45 cdBV->outContacts()->connect(merge->inContactsB());
46 this->animationPipeline()->pushModule(merge);
47
48 auto iterSolver = std::make_shared<TJConstraintSolver<TDataType>>();
49 this->stateTimeStep()->connect(iterSolver->inTimeStep());
50 this->varFrictionEnabled()->connect(iterSolver->varFrictionEnabled());
51 this->varGravityEnabled()->connect(iterSolver->varGravityEnabled());
52 this->varGravityValue()->connect(iterSolver->varGravityValue());
53 //this->varFrictionCoefficient()->connect(iterSolver->varFrictionCoefficient());
54 this->varFrictionCoefficient()->setValue(20.0f);
55 this->varSlop()->connect(iterSolver->varSlop());
56 this->stateMass()->connect(iterSolver->inMass());
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
65 this->stateTopology()->connect(iterSolver->inDiscreteElements());
66
67 merge->outContacts()->connect(iterSolver->inContacts());
68
69 this->animationPipeline()->pushModule(iterSolver);
70
71 this->inTriangleSet()->tagOptional(true);
72 this->inTriangleSet()->tagOptional(true);
73 }
74
75 template<typename TDataType>
80
81 template<typename TDataType>
83 {
85 this->clearVechicle();
86
87 if (!this->inTextureMesh()->isEmpty())
88 {
89 this->stateTextureMesh()->setDataPtr(this->inTextureMesh()->constDataPtr());
90 }
91
92 if (!this->varVehicleConfiguration()->getValue().isValid() && !bool(this->varVehiclesTransform()->getValue().size()) || this->stateTextureMesh()->isEmpty())
93 return;
94
95 auto texMesh = this->stateTextureMesh()->constDataPtr();
96 const auto config = this->varVehicleConfiguration()->getValue();
97
98 const auto rigidInfo = config.mVehicleRigidBodyInfo;
99 const auto jointInfo = config.mVehicleJointInfo;
100
101 // **************************** Create RigidBody **************************** //
102 auto instances = this->varVehiclesTransform()->getValue();
103 uint vehicleNum = instances.size();
104 int maxGroup = 0;
105 for (size_t i = 0; i < rigidInfo.size(); i++)
106 {
107 if (rigidInfo[i].rigidGroup > maxGroup)
108 maxGroup = rigidInfo[i].rigidGroup;
109 }
110
111 for (size_t j = 0; j < vehicleNum; j++)
112 {
113
114
115 std::vector<std::shared_ptr<PdActor>> Actors;
116
117 Actors.resize(rigidInfo.size());
118
119
120 for (size_t i = 0; i < rigidInfo.size(); i++)
121 {
122 RigidBodyInfo rigidbody;
123 rigidbody.bodyId = j * (maxGroup + 1) + rigidInfo[i].rigidGroup;
124
125 rigidbody.offset = rigidInfo[i].Offset;
126 rigidbody.friction = this->varFrictionCoefficient()->getValue();
127
128 auto type = rigidInfo[i].shapeType;
129 auto shapeId = rigidInfo[i].meshShapeId;
130 auto transform = rigidInfo[i].transform;
131 Real density = rigidInfo[i].mDensity;
132
133 if (shapeId > texMesh->shapes().size() - 1)
134 continue;
135
136 Vec3f up;
137 Vec3f down;
138 Vec3f T;
139
140 if (shapeId != -1)
141 {
142 up = texMesh->shapes()[shapeId]->boundingBox.v1;
143 down = texMesh->shapes()[shapeId]->boundingBox.v0;
144 T = texMesh->shapes()[shapeId]->boundingTransform.translation();
145 }
146 else
147 {
148
149
150 }
151
152 BoxInfo currentBox;
153 CapsuleInfo currentCapsule;
154 SphereInfo currentSphere;
155 TetInfo currentTet;
156
157 if (shapeId != -1)
158 {
159 switch (type)
160 {
161 case dyno::Box:
162 currentBox.center = Vec3f(0.0f);
163 currentBox.halfLength = (up - down) / 2 * rigidInfo[i].mHalfLength;
164 currentBox.rot = Quat1f(transform.rotation());
165
166 rigidbody.position = Quat1f(instances[j].rotation()).rotate(T + rigidInfo[i].transform.translation()) + instances[j].translation();
167 rigidbody.angle = Quat1f(instances[j].rotation());
168 Actors[i] = this->addBox(currentBox, rigidbody, density);
169 break;
170
171 case dyno::Tet:
172 printf("Need Tet Configuration\n");
173 break;
174
175 case dyno::Capsule:
176 currentCapsule.center = Vec3f(0.0f);
177 currentCapsule.rot = Quat1f(transform.rotation());
178 currentCapsule.halfLength = (up.y - down.y) / 2 * rigidInfo[i].capsuleLength;
179 currentCapsule.radius = std::abs(up.y - down.y) / 2 * rigidInfo[i].radius;
180
181 rigidbody.position = Quat1f(instances[j].rotation()).rotate(T + rigidInfo[i].transform.translation()) + instances[j].translation();
182 rigidbody.angle = Quat1f(instances[j].rotation());
183 Actors[i] = this->addCapsule(currentCapsule, rigidbody, density);
184 break;
185
186 case dyno::Sphere:
187 currentSphere.center = Vec3f(0.0f);
188 currentSphere.rot = Quat1f(transform.rotation());
189 currentSphere.radius = std::abs(up.y - down.y) / 2 * rigidInfo[i].radius;
190
191 rigidbody.position = Quat1f(instances[j].rotation()).rotate(T + rigidInfo[i].transform.translation()) + instances[j].translation();
192 rigidbody.angle = Quat1f(instances[j].rotation());
193 Actors[i] = this->addSphere(currentSphere, rigidbody, density);
194 break;
195
196 case dyno::Tri:
197 printf("Need Tri Configuration\n");
198 Actors[i] = NULL;
199 break;
200
201 case dyno::OtherShape:
202 printf("Need OtherShape Configuration\n");
203 Actors[i] = NULL;
204 break;
205
206 default:
207 break;
208 }
209 }
210 else if (shapeId == -1)
211 {
212 switch (type)
213 {
214 case dyno::Box:
215 currentBox.center = Vec3f(0.0f);
216 currentBox.halfLength = rigidInfo[i].mHalfLength;
217 currentBox.rot = Quat<Real>(rigidInfo[i].transform.rotation());
218
219 rigidbody.position = Quat1f(instances[j].rotation()).rotate(rigidInfo[i].transform.translation()) + instances[j].translation();
220 rigidbody.angle = Quat1f(instances[j].rotation());
221 Actors[i] = this->addBox(currentBox, rigidbody, density);
222 break;
223
224 case dyno::Tet:
225 printf("Need Tet Configuration\n");
226 currentTet.v[0] = rigidInfo[i].tet[0];
227 currentTet.v[1] = rigidInfo[i].tet[1];
228 currentTet.v[2] = rigidInfo[i].tet[2];
229 currentTet.v[3] = rigidInfo[i].tet[3];
230
231 break;
232
233 case dyno::Capsule:
234 currentCapsule.center = Vec3f(0.0f);
235 currentCapsule.rot = Quat<Real>(rigidInfo[i].transform.rotation());
236 currentCapsule.halfLength = rigidInfo[i].capsuleLength;
237 currentCapsule.radius = rigidInfo[i].radius;
238
239 rigidbody.position = Quat1f(instances[j].rotation()).rotate(rigidInfo[i].transform.translation()) + instances[j].translation();
240 rigidbody.angle = Quat1f(instances[j].rotation());
241 Actors[i] = this->addCapsule(currentCapsule, rigidbody, density);
242 break;
243
244 case dyno::Sphere:
245 currentSphere.center = Vec3f(0.0f);
246 currentSphere.rot = Quat<Real>(rigidInfo[i].transform.rotation());
247 currentSphere.radius = rigidInfo[i].radius;
248
249 rigidbody.position = Quat1f(instances[j].rotation()).rotate(rigidInfo[i].transform.translation()) + instances[j].translation();
250 rigidbody.angle = Quat1f(instances[j].rotation());
251 Actors[i] = this->addSphere(currentSphere, rigidbody, density);
252 break;
253
254 case dyno::Tri:
255 printf("Need Tri Configuration\n");
256 break;
257
258 case dyno::OtherShape:
259 printf("Need OtherShape Configuration\n");
260 break;
261
262 default:
263 break;
264 }
265 }
266
267 if (shapeId != -1 && Actors[i] != NULL)
268 {
270 this->bindShape(Actors[i], Pair<uint, uint>(shapeId, j));
271 }
272 }
273
274 for (size_t i = 0; i < jointInfo.size(); i++)
275 {
277 auto type = jointInfo[i].mJointType;
278 int first = jointInfo[i].mRigidBodyName_1.rigidBodyId;
279 int second = jointInfo[i].mRigidBodyName_2.rigidBodyId;
280 Real speed = jointInfo[i].mMoter;
281 auto axis = Quat1f(instances[j].rotation()).rotate(jointInfo[i].mAxis);
282 auto anchorOffset = jointInfo[i].mAnchorPoint;
283
284 if (first == -1 || second == -1)
285 continue;
286 if (Actors[first] == NULL || Actors[second] == NULL)
287 continue;
288
289
290 if (type == Hinge)
291 {
292 auto& joint = this->createHingeJoint(Actors[first], Actors[second]);
293 joint.setAnchorPoint(Actors[first]->center + anchorOffset);
294 joint.setAxis(axis);
295 if (jointInfo[i].mUseMoter)
296 joint.setMoter(speed);
297 if (jointInfo[i].mUseRange)
298 joint.setRange(jointInfo[i].mMin, jointInfo[i].mMax);
299
300 }
301 if (type == Slider)
302 {
303 auto& sliderJoint = this->createSliderJoint(Actors[first], Actors[second]);
304 sliderJoint.setAnchorPoint((Actors[first]->center + Actors[first]->center) / 2 + anchorOffset);
305 sliderJoint.setAxis(axis);
306 if (jointInfo[i].mUseMoter)
307 sliderJoint.setMoter(speed);
308 if (jointInfo[i].mUseRange)
309 sliderJoint.setRange(jointInfo[i].mMin, jointInfo[i].mMax);
310 }
311 if (type == Fixed)
312 {
313 auto& fixedJoint1 = this->createFixedJoint(Actors[first], Actors[second]);
314 fixedJoint1.setAnchorPoint((Actors[first]->center + Actors[first]->center) / 2 + anchorOffset);
315 }
316 if (type == Point)
317 {
318 auto& pointJoint = this->createPointJoint(Actors[first]);
319 pointJoint.setAnchorPoint(Actors[first]->center + anchorOffset);
320 }
321 if (type == BallAndSocket)
322 {
323 auto& ballAndSocketJoint = this->createBallAndSocketJoint(Actors[first], Actors[second]);
324 ballAndSocketJoint.setAnchorPoint((Actors[first]->center + Actors[first]->center) / 2 + anchorOffset);
325 }
326
327 //Joint(Actor)
328 }
329 }
330
331
332
333 }
334
335 template<typename TDataType>
337 {
338 this->updateConfig();
339 /***************** Reset *************/
341
343
345 }
346
348
349
350
351}
#define DEFINE_CLASS(name)
Definition Object.h:140
#define IMPLEMENT_TCLASS(name, T1)
Definition Object.h:103
void resetStates() override
void bindShape(std::shared_ptr< PdActor > actor, Pair< uint, uint > shapeId)
std::shared_ptr< AnimationPipeline > animationPipeline()
Definition Node.cpp:302
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
std::shared_ptr< PdActor > addBox(const BoxInfo &box, const RigidBodyInfo &bodyDef, const Real density=Real(100))
SliderJoint & createSliderJoint(std::shared_ptr< PdActor > actor1, std::shared_ptr< PdActor > actor2)
HingeJoint & createHingeJoint(std::shared_ptr< PdActor > actor1, std::shared_ptr< PdActor > actor2)
FixedJoint & createFixedJoint(std::shared_ptr< PdActor > actor1, std::shared_ptr< PdActor > actor2)
std::shared_ptr< PdActor > addCapsule(const CapsuleInfo &capsule, const RigidBodyInfo &bodyDef, const Real density=Real(100))
void postUpdateStates() override
PointJoint & createPointJoint(std::shared_ptr< PdActor > actor1)
std::shared_ptr< PdActor > addSphere(const SphereInfo &sphere, const RigidBodyInfo &bodyDef, const Real density=Real(100))
BallAndSocketJoint & createBallAndSocketJoint(std::shared_ptr< PdActor > actor1, std::shared_ptr< PdActor > actor2)
#define T(t)
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
int joint
Definition GltfFunc.h:19
@ Point
Definition VehicleInfo.h:37
@ BallAndSocket
Definition VehicleInfo.h:33
@ Slider
Definition VehicleInfo.h:34
@ Hinge
Definition VehicleInfo.h:35
@ Fixed
Definition VehicleInfo.h:36
@ Sphere
Definition VehicleInfo.h:26
@ OtherShape
Definition VehicleInfo.h:28
@ Capsule
Definition VehicleInfo.h:25
Quat< float > Quat1f
Definition Quat.h:136
Vector< float, 3 > Vec3f
Definition Vector3D.h:93
unsigned int uint
Definition VkProgram.h:14
Quat< Real > rot
Vector< Real, 3 > halfLength
Vector< Real, 3 > center
Vector< Real, 3 > center
Vector< Real, 3 > offset
An offset from the barycenter to the geometric center.
Vector< Real, 3 > position
The barycenter of the body.
Quat< Real > rot
Vector< Real, 3 > center
Vector< Real, 3 > v[4]