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());
35 auto cdBV = std::make_shared<CollistionDetectionTriangleSet<TDataType>>();
36 this->stateTopology()->connect(cdBV->inDiscreteElements());
37 this->inTriangleSet()->connect(cdBV->inTriangleSet());
43 auto merge = std::make_shared<ContactsUnion<TDataType>>();
44 elementQuery->outContacts()->connect(merge->inContactsA());
45 cdBV->outContacts()->connect(merge->inContactsB());
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());
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());
65 this->stateTopology()->connect(iterSolver->inDiscreteElements());
67 merge->outContacts()->connect(iterSolver->inContacts());
71 this->inTriangleSet()->tagOptional(
true);
72 this->inTriangleSet()->tagOptional(
true);
87 if (!this->inTextureMesh()->isEmpty())
89 this->stateTextureMesh()->setDataPtr(this->inTextureMesh()->constDataPtr());
92 if (!this->varVehicleConfiguration()->getValue().isValid() && !
bool(this->varVehiclesTransform()->getValue().size()) || this->stateTextureMesh()->isEmpty())
95 auto texMesh = this->stateTextureMesh()->constDataPtr();
96 const auto config = this->varVehicleConfiguration()->getValue();
98 const auto rigidInfo = config.mVehicleRigidBodyInfo;
99 const auto jointInfo = config.mVehicleJointInfo;
102 auto instances = this->varVehiclesTransform()->getValue();
103 uint vehicleNum = instances.size();
105 for (
size_t i = 0; i < rigidInfo.size(); i++)
107 if (rigidInfo[i].rigidGroup > maxGroup)
108 maxGroup = rigidInfo[i].rigidGroup;
111 for (
size_t j = 0; j < vehicleNum; j++)
115 std::vector<std::shared_ptr<PdActor>> Actors;
117 Actors.resize(rigidInfo.size());
120 for (
size_t i = 0; i < rigidInfo.size(); i++)
123 rigidbody.
bodyId = j * (maxGroup + 1) + rigidInfo[i].rigidGroup;
125 rigidbody.
offset = rigidInfo[i].Offset;
126 rigidbody.
friction = this->varFrictionCoefficient()->getValue();
128 auto type = rigidInfo[i].shapeType;
129 auto shapeId = rigidInfo[i].meshShapeId;
131 Real density = rigidInfo[i].mDensity;
133 if (shapeId > texMesh->shapes().size() - 1)
142 up = texMesh->shapes()[shapeId]->boundingBox.v1;
143 down = texMesh->shapes()[shapeId]->boundingBox.v0;
144 T = texMesh->shapes()[shapeId]->boundingTransform.translation();
163 currentBox.
halfLength = (up - down) / 2 * rigidInfo[i].mHalfLength;
168 Actors[i] = this->
addBox(currentBox, rigidbody, density);
172 printf(
"Need Tet Configuration\n");
178 currentCapsule.
halfLength = (up.y - down.y) / 2 * rigidInfo[i].capsuleLength;
179 currentCapsule.
radius = std::abs(up.y - down.y) / 2 * rigidInfo[i].radius;
183 Actors[i] = this->
addCapsule(currentCapsule, rigidbody, density);
189 currentSphere.
radius = std::abs(up.y - down.y) / 2 * rigidInfo[i].radius;
193 Actors[i] = this->
addSphere(currentSphere, rigidbody, density);
197 printf(
"Need Tri Configuration\n");
202 printf(
"Need OtherShape Configuration\n");
210 else if (shapeId == -1)
216 currentBox.
halfLength = rigidInfo[i].mHalfLength;
221 Actors[i] = this->
addBox(currentBox, rigidbody, density);
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];
236 currentCapsule.
halfLength = rigidInfo[i].capsuleLength;
237 currentCapsule.
radius = rigidInfo[i].radius;
241 Actors[i] = this->
addCapsule(currentCapsule, rigidbody, density);
247 currentSphere.
radius = rigidInfo[i].radius;
251 Actors[i] = this->
addSphere(currentSphere, rigidbody, density);
255 printf(
"Need Tri Configuration\n");
259 printf(
"Need OtherShape Configuration\n");
267 if (shapeId != -1 && Actors[i] != NULL)
274 for (
size_t i = 0; i < jointInfo.size(); i++)
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;
284 if (first == -1 || second == -1)
286 if (Actors[first] == NULL || Actors[second] == NULL)
293 joint.setAnchorPoint(Actors[first]->center + anchorOffset);
295 if (jointInfo[i].mUseMoter)
296 joint.setMoter(speed);
297 if (jointInfo[i].mUseRange)
298 joint.setRange(jointInfo[i].mMin, jointInfo[i].mMax);
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);
314 fixedJoint1.setAnchorPoint((Actors[first]->center + Actors[first]->center) / 2 + anchorOffset);
319 pointJoint.setAnchorPoint(Actors[first]->center + anchorOffset);
324 ballAndSocketJoint.setAnchorPoint((Actors[first]->center + Actors[first]->center) / 2 + anchorOffset);
void bindShape(std::shared_ptr< PdActor > actor, Pair< uint, uint > shapeId)
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))
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)
Vector< Real, 3 > offset
An offset from the barycenter to the geometric center.