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 "Components/BsCRigidbody.h"
4#include "Scene/BsSceneObject.h"
5#include "Components/BsCCollider.h"
6#include "Components/BsCJoint.h"
7#include "Private/RTTI/BsCRigidbodyRTTI.h"
8#include "Physics/BsPhysics.h"
9
10using namespace std::placeholders;
11
12namespace bs
13{
14 CRigidbody::CRigidbody()
15 {
16 setName("Rigidbody");
17
18 mNotifyFlags = (TransformChangedFlags)(TCF_Parent | TCF_Transform);
19 }
20
21 CRigidbody::CRigidbody(const HSceneObject& parent)
22 : Component(parent)
23 {
24 setName("Rigidbody");
25
26 mNotifyFlags = (TransformChangedFlags)(TCF_Parent | TCF_Transform);
27 }
28
29 void CRigidbody::move(const Vector3& position)
30 {
31 if (mInternal != nullptr)
32 mInternal->move(position);
33
34 mNotifyFlags = (TransformChangedFlags)0;
35 SO()->setWorldPosition(position);
36 mNotifyFlags = (TransformChangedFlags)(TCF_Parent | TCF_Transform);
37 }
38
39 void CRigidbody::rotate(const Quaternion& rotation)
40 {
41 if (mInternal != nullptr)
42 mInternal->rotate(rotation);
43
44 mNotifyFlags = (TransformChangedFlags)0;
45 SO()->setWorldRotation(rotation);
46 mNotifyFlags = (TransformChangedFlags)(TCF_Parent | TCF_Transform);
47 }
48
49 void CRigidbody::setMass(float mass)
50 {
51 mMass = mass;
52
53 if(mInternal != nullptr)
54 mInternal->setMass(mass);
55 }
56
57 void CRigidbody::setIsKinematic(bool kinematic)
58 {
59 if (mIsKinematic == kinematic)
60 return;
61
62 mIsKinematic = kinematic;
63
64 if (mInternal != nullptr)
65 {
66 mInternal->setIsKinematic(kinematic);
67
68 clearColliders();
69 updateColliders();
70 }
71 }
72
73 bool CRigidbody::isSleeping() const
74 {
75 if (mInternal != nullptr)
76 return mInternal->isSleeping();
77
78 return true;
79 }
80
81 void CRigidbody::sleep()
82 {
83 if (mInternal != nullptr)
84 return mInternal->sleep();
85 }
86
87 void CRigidbody::wakeUp()
88 {
89 if (mInternal != nullptr)
90 return mInternal->wakeUp();
91 }
92
93 void CRigidbody::setSleepThreshold(float threshold)
94 {
95 mSleepThreshold = threshold;
96
97 if (mInternal != nullptr)
98 mInternal->setSleepThreshold(threshold);
99 }
100
101 void CRigidbody::setUseGravity(bool gravity)
102 {
103 mUseGravity = gravity;
104
105 if (mInternal != nullptr)
106 mInternal->setUseGravity(gravity);
107 }
108
109 void CRigidbody::setVelocity(const Vector3& velocity)
110 {
111 if (mInternal != nullptr)
112 mInternal->setVelocity(velocity);
113 }
114
115 Vector3 CRigidbody::getVelocity() const
116 {
117 if (mInternal != nullptr)
118 return mInternal->getVelocity();
119
120 return Vector3::ZERO;
121 }
122
123 void CRigidbody::setAngularVelocity(const Vector3& velocity)
124 {
125 if (mInternal != nullptr)
126 mInternal->setAngularVelocity(velocity);
127 }
128
129 Vector3 CRigidbody::getAngularVelocity() const
130 {
131 if (mInternal != nullptr)
132 return mInternal->getAngularVelocity();
133
134 return Vector3::ZERO;
135 }
136
137 void CRigidbody::setDrag(float drag)
138 {
139 mLinearDrag = drag;
140
141 if (mInternal != nullptr)
142 mInternal->setDrag(drag);
143 }
144
145 void CRigidbody::setAngularDrag(float drag)
146 {
147 mAngularDrag = drag;
148
149 if (mInternal != nullptr)
150 mInternal->setAngularDrag(drag);
151 }
152
153 void CRigidbody::setInertiaTensor(const Vector3& tensor)
154 {
155 mInertiaTensor = tensor;
156
157 if (mInternal != nullptr)
158 mInternal->setInertiaTensor(tensor);
159 }
160
161 Vector3 CRigidbody::getInertiaTensor() const
162 {
163 if (mInternal != nullptr)
164 return mInternal->getInertiaTensor();
165
166 return Vector3::ZERO;
167 }
168
169 void CRigidbody::setMaxAngularVelocity(float maxVelocity)
170 {
171 mMaxAngularVelocity = maxVelocity;
172
173 if (mInternal != nullptr)
174 mInternal->setMaxAngularVelocity(maxVelocity);
175 }
176
177 void CRigidbody::setCenterOfMassPosition(const Vector3& position)
178 {
179 mCMassPosition = position;
180
181 if (mInternal != nullptr)
182 mInternal->setCenterOfMass(position, mCMassRotation);
183 }
184
185 void CRigidbody::setCenterOfMassRotation(const Quaternion& rotation)
186 {
187 mCMassRotation = rotation;
188
189 if (mInternal != nullptr)
190 mInternal->setCenterOfMass(mCMassPosition, rotation);
191 }
192
193 Vector3 CRigidbody::getCenterOfMassPosition() const
194 {
195 if (mInternal != nullptr)
196 return mInternal->getCenterOfMassPosition();
197
198 return Vector3::ZERO;
199 }
200
201 Quaternion CRigidbody::getCenterOfMassRotation() const
202 {
203 if (mInternal != nullptr)
204 return mInternal->getCenterOfMassRotation();
205
206 return Quaternion::IDENTITY;
207 }
208
209 void CRigidbody::setPositionSolverCount(UINT32 count)
210 {
211 mPositionSolverCount = count;
212
213 if (mInternal != nullptr)
214 mInternal->setPositionSolverCount(count);
215 }
216
217 void CRigidbody::setVelocitySolverCount(UINT32 count)
218 {
219 mVelocitySolverCount = count;
220
221 if (mInternal != nullptr)
222 mInternal->setVelocitySolverCount(count);
223 }
224
225 void CRigidbody::setCollisionReportMode(CollisionReportMode mode)
226 {
227 if (mCollisionReportMode == mode)
228 return;
229
230 mCollisionReportMode = mode;
231
232 for (auto& entry : mChildren)
233 entry->updateCollisionReportMode();
234 }
235
236 void CRigidbody::setFlags(RigidbodyFlag flags)
237 {
238 mFlags = flags;
239
240 if (mInternal != nullptr)
241 {
242 mInternal->setFlags(flags);
243 mInternal->updateMassDistribution();
244 }
245 }
246
247 void CRigidbody::addForce(const Vector3& force, ForceMode mode)
248 {
249 if (mInternal != nullptr)
250 mInternal->addForce(force, mode);
251 }
252
253 void CRigidbody::addTorque(const Vector3& torque, ForceMode mode)
254 {
255 if (mInternal != nullptr)
256 mInternal->addTorque(torque, mode);
257 }
258
259 void CRigidbody::addForceAtPoint(const Vector3& force, const Vector3& position, PointForceMode mode)
260 {
261 if (mInternal != nullptr)
262 mInternal->addForceAtPoint(force, position, mode);
263 }
264
265 Vector3 CRigidbody::getVelocityAtPoint(const Vector3& point) const
266 {
267 if (mInternal != nullptr)
268 return mInternal->getVelocityAtPoint(point);
269
270 return Vector3::ZERO;
271 }
272
273 void CRigidbody::_updateMassDistribution()
274 {
275 if (mInternal != nullptr)
276 return mInternal->updateMassDistribution();
277 }
278
279 void CRigidbody::updateColliders()
280 {
281 Stack<HSceneObject> todo;
282 todo.push(SO());
283
284 while(!todo.empty())
285 {
286 HSceneObject currentSO = todo.top();
287 todo.pop();
288
289 if(currentSO->hasComponent<CCollider>())
290 {
291 Vector<HCollider> colliders = currentSO->getComponents<CCollider>();
292
293 for (auto& entry : colliders)
294 {
295 if (!entry->isValidParent(static_object_cast<CRigidbody>(mThisHandle)))
296 continue;
297
298 Collider* collider = entry->_getInternal();
299 if (collider == nullptr)
300 continue;
301
302 entry->setRigidbody(static_object_cast<CRigidbody>(mThisHandle), true);
303 mChildren.push_back(entry);
304
305 collider->setRigidbody(mInternal.get());
306 mInternal->addCollider(collider);
307 }
308 }
309
310 UINT32 childCount = currentSO->getNumChildren();
311 for (UINT32 i = 0; i < childCount; i++)
312 {
313 HSceneObject child = currentSO->getChild(i);
314
315 if (child->hasComponent<CRigidbody>())
316 continue;
317
318 todo.push(child);
319 }
320 }
321 }
322
323 void CRigidbody::clearColliders()
324 {
325 for (auto& collider : mChildren)
326 collider->setRigidbody(HRigidbody(), true);
327
328 mChildren.clear();
329
330 if (mInternal != nullptr)
331 mInternal->removeColliders();
332 }
333
334 void CRigidbody::addCollider(const HCollider& collider)
335 {
336 if (mInternal == nullptr)
337 return;
338
339 mChildren.push_back(collider);
340 mInternal->addCollider(collider->_getInternal());
341 }
342
343 void CRigidbody::removeCollider(const HCollider& collider)
344 {
345 if (mInternal == nullptr)
346 return;
347
348 auto iterFind = std::find(mChildren.begin(), mChildren.end(), collider);
349
350 if(iterFind != mChildren.end())
351 {
352 mInternal->removeCollider(collider->_getInternal());
353 mChildren.erase(iterFind);
354 }
355 }
356
357 void CRigidbody::checkForNestedRigibody()
358 {
359 HSceneObject currentSO = SO()->getParent();
360
361 while(currentSO != nullptr)
362 {
363 if(currentSO->hasComponent<CRigidbody>())
364 {
365 LOGWRN("Nested Rigidbodies detected. This will result in inconsistent transformations. To parent one " \
366 "Rigidbody to another move its colliders to the new parent, but remove the Rigidbody component.");
367 return;
368 }
369
370 currentSO = currentSO->getParent();
371 }
372 }
373
374 void CRigidbody::processCollisionData(const CollisionDataRaw& data, CollisionData& output)
375 {
376 output.contactPoints = std::move(data.contactPoints);
377
378 if (data.colliders[0] != nullptr)
379 {
380 CCollider* other = (CCollider*)data.colliders[0]->_getOwner(PhysicsOwnerType::Component);
381 output.collider[0] = static_object_cast<CCollider>(other->getHandle());
382 }
383
384 if (data.colliders[1] != nullptr)
385 {
386 CCollider* other = (CCollider*)data.colliders[1]->_getOwner(PhysicsOwnerType::Component);
387 output.collider[1] = static_object_cast<CCollider>(other->getHandle());
388 }
389 }
390
391 void CRigidbody::destroyInternal()
392 {
393 clearColliders();
394
395 if(mInternal)
396 {
397 mInternal->_setOwner(PhysicsOwnerType::None, nullptr);
398 mInternal = nullptr;
399 }
400 }
401
402 void CRigidbody::triggerOnCollisionBegin(const CollisionDataRaw& data)
403 {
404 CollisionData hit;
405 processCollisionData(data, hit);
406
407 onCollisionBegin(hit);
408 }
409
410 void CRigidbody::triggerOnCollisionStay(const CollisionDataRaw& data)
411 {
412 CollisionData hit;
413 processCollisionData(data, hit);
414
415 onCollisionStay(hit);
416 }
417
418 void CRigidbody::triggerOnCollisionEnd(const CollisionDataRaw& data)
419 {
420 CollisionData hit;
421 processCollisionData(data, hit);
422
423 onCollisionEnd(hit);
424 }
425
426 void CRigidbody::onInitialized()
427 {
428
429 }
430
431 void CRigidbody::onDestroyed()
432 {
433 destroyInternal();
434 }
435
436 void CRigidbody::onDisabled()
437 {
438 destroyInternal();
439 }
440
441 void CRigidbody::onEnabled()
442 {
443 mInternal = Rigidbody::create(SO());
444 mInternal->_setOwner(PhysicsOwnerType::Component, this);
445
446 updateColliders();
447
448#if BS_DEBUG_MODE
449 checkForNestedRigibody();
450#endif
451
452 mInternal->onCollisionBegin.connect(std::bind(&CRigidbody::triggerOnCollisionBegin, this, _1));
453 mInternal->onCollisionStay.connect(std::bind(&CRigidbody::triggerOnCollisionStay, this, _1));
454 mInternal->onCollisionEnd.connect(std::bind(&CRigidbody::triggerOnCollisionEnd, this, _1));
455
456 const Transform& tfrm = SO()->getTransform();
457 mInternal->setTransform(tfrm.getPosition(), tfrm.getRotation());
458
459 // Note: Merge into one call to avoid many virtual function calls
460 mInternal->setPositionSolverCount(mPositionSolverCount);
461 mInternal->setVelocitySolverCount(mVelocitySolverCount);
462 mInternal->setMaxAngularVelocity(mMaxAngularVelocity);
463 mInternal->setDrag(mLinearDrag);
464 mInternal->setAngularDrag(mAngularDrag);
465 mInternal->setSleepThreshold(mSleepThreshold);
466 mInternal->setUseGravity(mUseGravity);
467 mInternal->setIsKinematic(mIsKinematic);
468 mInternal->setFlags(mFlags);
469
470 if(((UINT32)mFlags & (UINT32)RigidbodyFlag::AutoTensors) == 0)
471 {
472 mInternal->setCenterOfMass(mCMassPosition, mCMassRotation);
473 mInternal->setInertiaTensor(mInertiaTensor);
474 mInternal->setMass(mMass);
475 }
476 else
477 {
478 if (((UINT32)mFlags & (UINT32)RigidbodyFlag::AutoMass) == 0)
479 mInternal->setMass(mMass);
480
481 mInternal->updateMassDistribution();
482 }
483 }
484
485 void CRigidbody::onTransformChanged(TransformChangedFlags flags)
486 {
487 if (!SO()->getActive())
488 return;
489
490 if((flags & TCF_Parent) != 0)
491 {
492 clearColliders();
493 updateColliders();
494
495 if (((UINT32)mFlags & (UINT32)RigidbodyFlag::AutoTensors) != 0)
496 mInternal->updateMassDistribution();
497
498#if BS_DEBUG_MODE
499 checkForNestedRigibody();
500#endif
501 }
502
503 if(gPhysics()._isUpdateInProgress())
504 return;
505
506 const Transform& tfrm = SO()->getTransform();
507 mInternal->setTransform(tfrm.getPosition(), tfrm.getRotation());
508
509 if (mParentJoint != nullptr)
510 mParentJoint->notifyRigidbodyMoved(static_object_cast<CRigidbody>(mThisHandle));
511 }
512
513 RTTITypeBase* CRigidbody::getRTTIStatic()
514 {
515 return CRigidbodyRTTI::instance();
516 }
517
518 RTTITypeBase* CRigidbody::getRTTI() const
519 {
520 return CRigidbody::getRTTIStatic();
521 }
522}