1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#include "BsPhysXRigidbody.h"
4#include "Physics/BsCollider.h"
5#include "BsFPhysXCollider.h"
6#include "Scene/BsSceneObject.h"
7#include "Physics/BsPhysics.h"
8#include "PxRigidDynamic.h"
9#include "PxScene.h"
10#include "extensions/PxRigidBodyExt.h"
11#include "BsPhysX.h"
12
13using namespace physx;
14
15namespace bs
16{
17 PxForceMode::Enum toPxForceMode(ForceMode mode)
18 {
19 switch(mode)
20 {
21 case ForceMode::Force:
22 return PxForceMode::eFORCE;
23 case ForceMode::Impulse:
24 return PxForceMode::eIMPULSE;
25 case ForceMode::Velocity:
26 return PxForceMode::eVELOCITY_CHANGE;
27 case ForceMode::Acceleration:
28 return PxForceMode::eACCELERATION;
29 }
30
31 return PxForceMode::eFORCE;
32 }
33
34 PxForceMode::Enum toPxForceMode(PointForceMode mode)
35 {
36 switch (mode)
37 {
38 case PointForceMode::Force:
39 return PxForceMode::eFORCE;
40 case PointForceMode::Impulse:
41 return PxForceMode::eIMPULSE;
42 }
43
44 return PxForceMode::eFORCE;
45 }
46
47 PhysXRigidbody::PhysXRigidbody(PxPhysics* physx, PxScene* scene, const HSceneObject& linkedSO)
48 :Rigidbody(linkedSO)
49 {
50 const Transform& tfrm = linkedSO->getTransform();
51 PxTransform pxTfrm = toPxTransform(tfrm.getPosition(), tfrm.getRotation());
52
53 mInternal = physx->createRigidDynamic(pxTfrm);
54 mInternal->userData = this;
55
56 scene->addActor(*mInternal);
57 }
58
59 PhysXRigidbody::~PhysXRigidbody()
60 {
61 mInternal->userData = nullptr;
62 mInternal->release();
63 }
64
65 void PhysXRigidbody::move(const Vector3& position)
66 {
67 if (getIsKinematic())
68 {
69 PxTransform target;
70 if (!mInternal->getKinematicTarget(target))
71 target = PxTransform(PxIdentity);
72
73 target.p = toPxVector(position);
74
75 mInternal->setKinematicTarget(target);
76 }
77 else
78 {
79 setTransform(position, getRotation());
80 }
81 }
82
83 void PhysXRigidbody::rotate(const Quaternion& rotation)
84 {
85 if (getIsKinematic())
86 {
87 PxTransform target;
88 if (!mInternal->getKinematicTarget(target))
89 target = PxTransform(PxIdentity);
90
91 target.q = toPxQuaternion(rotation);
92
93 mInternal->setKinematicTarget(target);
94 }
95 else
96 {
97 setTransform(getPosition(), rotation);
98 }
99 }
100
101 Vector3 PhysXRigidbody::getPosition() const
102 {
103 return fromPxVector(mInternal->getGlobalPose().p);
104 }
105
106 Quaternion PhysXRigidbody::getRotation() const
107 {
108 return fromPxQuaternion(mInternal->getGlobalPose().q);
109 }
110
111 void PhysXRigidbody::setTransform(const Vector3& pos, const Quaternion& rot)
112 {
113 mInternal->setGlobalPose(toPxTransform(pos, rot));
114 }
115
116 void PhysXRigidbody::setMass(float mass)
117 {
118 if(((UINT32)mFlags & (UINT32)RigidbodyFlag::AutoMass) != 0)
119 {
120 LOGWRN("Attempting to set Rigidbody mass, but it has automatic mass calculation turned on.");
121 return;
122 }
123
124 mInternal->setMass(mass);
125 }
126
127 float PhysXRigidbody::getMass() const
128 {
129 return mInternal->getMass();
130 }
131
132 void PhysXRigidbody::setIsKinematic(bool kinematic)
133 {
134 mInternal->setRigidBodyFlag(PxRigidBodyFlag::eKINEMATIC, kinematic);
135 }
136
137 bool PhysXRigidbody::getIsKinematic() const
138 {
139 return ((UINT32)mInternal->getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC) != 0;
140 }
141
142 bool PhysXRigidbody::isSleeping() const
143 {
144 return mInternal->isSleeping();
145 }
146
147 void PhysXRigidbody::sleep()
148 {
149 mInternal->putToSleep();
150 }
151
152 void PhysXRigidbody::wakeUp()
153 {
154 mInternal->wakeUp();
155 }
156
157 void PhysXRigidbody::setSleepThreshold(float threshold)
158 {
159 mInternal->setSleepThreshold(threshold);
160 }
161
162 float PhysXRigidbody::getSleepThreshold() const
163 {
164 return mInternal->getSleepThreshold();
165 }
166
167 void PhysXRigidbody::setUseGravity(bool gravity)
168 {
169 mInternal->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, !gravity);
170 }
171
172 bool PhysXRigidbody::getUseGravity() const
173 {
174 return ((UINT32)mInternal->getActorFlags() & PxActorFlag::eDISABLE_GRAVITY) == 0;
175 }
176
177 void PhysXRigidbody::setVelocity(const Vector3& velocity)
178 {
179 mInternal->setLinearVelocity(toPxVector(velocity));
180 }
181
182 Vector3 PhysXRigidbody::getVelocity() const
183 {
184 return fromPxVector(mInternal->getLinearVelocity());
185 }
186
187 void PhysXRigidbody::setAngularVelocity(const Vector3& velocity)
188 {
189 mInternal->setAngularVelocity(toPxVector(velocity));
190 }
191
192 Vector3 PhysXRigidbody::getAngularVelocity() const
193 {
194 return fromPxVector(mInternal->getAngularVelocity());
195 }
196
197 void PhysXRigidbody::setDrag(float drag)
198 {
199 mInternal->setLinearDamping(drag);
200 }
201
202 float PhysXRigidbody::getDrag() const
203 {
204 return mInternal->getLinearDamping();
205 }
206
207 void PhysXRigidbody::setAngularDrag(float drag)
208 {
209 mInternal->setAngularDamping(drag);
210 }
211
212 float PhysXRigidbody::getAngularDrag() const
213 {
214 return mInternal->getAngularDamping();
215 }
216
217 void PhysXRigidbody::setInertiaTensor(const Vector3& tensor)
218 {
219 if (((UINT32)mFlags & (UINT32)RigidbodyFlag::AutoTensors) != 0)
220 {
221 LOGWRN("Attempting to set Rigidbody inertia tensor, but it has automatic tensor calculation turned on.");
222 return;
223 }
224
225 mInternal->setMassSpaceInertiaTensor(toPxVector(tensor));
226 }
227
228 Vector3 PhysXRigidbody::getInertiaTensor() const
229 {
230 return fromPxVector(mInternal->getMassSpaceInertiaTensor());
231 }
232
233 void PhysXRigidbody::setMaxAngularVelocity(float maxVelocity)
234 {
235 mInternal->setMaxAngularVelocity(maxVelocity);
236 }
237
238 float PhysXRigidbody::getMaxAngularVelocity() const
239 {
240 return mInternal->getMaxAngularVelocity();
241 }
242
243 void PhysXRigidbody::setCenterOfMass(const Vector3& position, const Quaternion& rotation)
244 {
245 if (((UINT32)mFlags & (UINT32)RigidbodyFlag::AutoTensors) != 0)
246 {
247 LOGWRN("Attempting to set Rigidbody center of mass, but it has automatic tensor calculation turned on.");
248 return;
249 }
250
251 mInternal->setCMassLocalPose(toPxTransform(position, rotation));
252 }
253
254 Vector3 PhysXRigidbody::getCenterOfMassPosition() const
255 {
256 PxTransform cMassTfrm = mInternal->getCMassLocalPose();
257 return fromPxVector(cMassTfrm.p);
258 }
259
260 Quaternion PhysXRigidbody::getCenterOfMassRotation() const
261 {
262 PxTransform cMassTfrm = mInternal->getCMassLocalPose();
263 return fromPxQuaternion(cMassTfrm.q);
264 }
265
266 void PhysXRigidbody::setPositionSolverCount(UINT32 count)
267 {
268 mInternal->setSolverIterationCounts(std::max(1U, count), getVelocitySolverCount());
269 }
270
271 UINT32 PhysXRigidbody::getPositionSolverCount() const
272 {
273 UINT32 posCount = 1;
274 UINT32 velCount = 1;
275
276 mInternal->getSolverIterationCounts(posCount, velCount);
277 return posCount;
278 }
279
280 void PhysXRigidbody::setVelocitySolverCount(UINT32 count)
281 {
282 mInternal->setSolverIterationCounts(getPositionSolverCount(), std::max(1U, count));
283 }
284
285 UINT32 PhysXRigidbody::getVelocitySolverCount() const
286 {
287 UINT32 posCount = 1;
288 UINT32 velCount = 1;
289
290 mInternal->getSolverIterationCounts(posCount, velCount);
291 return velCount;
292 }
293
294 void PhysXRigidbody::setFlags(RigidbodyFlag flags)
295 {
296 bool ccdEnabledOld = mInternal->getRigidBodyFlags() & PxRigidBodyFlag::eENABLE_CCD;
297 bool ccdEnabledNew = ((UINT32)flags & (UINT32)RigidbodyFlag::CCD) != 0;
298
299 if(ccdEnabledOld != ccdEnabledNew)
300 {
301 mInternal->setRigidBodyFlag(PxRigidBodyFlag::eENABLE_CCD, ccdEnabledNew);
302
303 // Enable/disable CCD on shapes so the filter can handle them properly
304 UINT32 numShapes = mInternal->getNbShapes();
305 PxShape** shapes = (PxShape**)bs_stack_alloc(sizeof(PxShape*) * numShapes);
306
307 mInternal->getShapes(shapes, sizeof(PxShape*) * numShapes);
308
309 for (UINT32 i = 0; i < numShapes; i++)
310 {
311 Collider* collider = (Collider*)shapes[i]->userData;
312 collider->_getInternal()->_setCCD(ccdEnabledNew);
313 }
314 }
315
316 Rigidbody::setFlags(flags);
317 }
318
319 void PhysXRigidbody::addForce(const Vector3& force, ForceMode mode)
320 {
321 mInternal->addForce(toPxVector(force), toPxForceMode(mode));
322 }
323
324 void PhysXRigidbody::addTorque(const Vector3& force, ForceMode mode)
325 {
326 mInternal->addTorque(toPxVector(force), toPxForceMode(mode));
327 }
328
329 void PhysXRigidbody::addForceAtPoint(const Vector3& force, const Vector3& position, PointForceMode mode)
330 {
331 const PxVec3& pxForce = toPxVector(force);
332 const PxVec3& pxPos = toPxVector(position);
333
334 const PxTransform globalPose = mInternal->getGlobalPose();
335 PxVec3 centerOfMass = globalPose.transform(mInternal->getCMassLocalPose().p);
336
337 PxForceMode::Enum pxMode = toPxForceMode(mode);
338
339 PxVec3 torque = (pxPos - centerOfMass).cross(pxForce);
340 mInternal->addForce(pxForce, pxMode);
341 mInternal->addTorque(torque, pxMode);
342 }
343
344 Vector3 PhysXRigidbody::getVelocityAtPoint(const Vector3& point) const
345 {
346 const PxVec3& pxPoint = toPxVector(point);
347
348 const PxTransform globalPose = mInternal->getGlobalPose();
349 const PxVec3 centerOfMass = globalPose.transform(mInternal->getCMassLocalPose().p);
350 const PxVec3 rpoint = pxPoint - centerOfMass;
351
352 PxVec3 velocity = mInternal->getLinearVelocity();
353 velocity += mInternal->getAngularVelocity().cross(rpoint);
354
355 return fromPxVector(velocity);
356 }
357
358 void PhysXRigidbody::updateMassDistribution()
359 {
360 if (((UINT32)mFlags & (UINT32)RigidbodyFlag::AutoTensors) == 0)
361 return;
362
363 if (((UINT32)mFlags & (UINT32)RigidbodyFlag::AutoMass) == 0)
364 {
365 PxRigidBodyExt::setMassAndUpdateInertia(*mInternal, mInternal->getMass());
366 }
367 else
368 {
369 UINT32 numShapes = mInternal->getNbShapes();
370 if (numShapes == 0)
371 {
372 PxRigidBodyExt::setMassAndUpdateInertia(*mInternal, mInternal->getMass());
373 return;
374 }
375
376 PxShape** shapes = (PxShape**)bs_stack_alloc(sizeof(PxShape*) * numShapes);
377 mInternal->getShapes(shapes, numShapes);
378
379 float* masses = (float*)bs_stack_alloc(sizeof(float) * numShapes);
380 for (UINT32 i = 0; i < numShapes; i++)
381 masses[i] = ((Collider*)shapes[i]->userData)->getMass();
382
383 PxRigidBodyExt::setMassAndUpdateInertia(*mInternal, masses, numShapes);
384
385 bs_stack_free(masses);
386 bs_stack_free(shapes);
387 }
388 }
389
390 void PhysXRigidbody::addCollider(Collider* collider)
391 {
392 if (collider == nullptr)
393 return;
394
395 FPhysXCollider* physxCollider = static_cast<FPhysXCollider*>(collider->_getInternal());
396 physxCollider->_setCCD(((UINT32)mFlags & (UINT32)RigidbodyFlag::CCD) != 0);
397
398 mInternal->attachShape(*physxCollider->_getShape());
399 }
400
401 void PhysXRigidbody::removeCollider(Collider* collider)
402 {
403 if (collider == nullptr)
404 return;
405
406 FPhysXCollider* physxCollider = static_cast<FPhysXCollider*>(collider->_getInternal());
407 physxCollider->_setCCD(false);
408
409 mInternal->detachShape(*physxCollider->_getShape());
410 }
411
412 void PhysXRigidbody::removeColliders()
413 {
414 UINT32 numShapes = mInternal->getNbShapes();
415 PxShape** shapes = (PxShape**)bs_stack_alloc(sizeof(PxShape*) * numShapes);
416
417 mInternal->getShapes(shapes, sizeof(PxShape*) * numShapes);
418
419 for (UINT32 i = 0; i < numShapes; i++)
420 {
421 Collider* collider = (Collider*)shapes[i]->userData;
422 collider->_getInternal()->_setCCD(false);
423
424 mInternal->detachShape(*shapes[i]);
425 }
426
427 bs_stack_free(shapes);
428 }
429}