PhysicsWorld.cpp 22 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622
  1. //
  2. // Urho3D Engine
  3. // Copyright (c) 2008-2012 Lasse Öörni
  4. //
  5. // Permission is hereby granted, free of charge, to any person obtaining a copy
  6. // of this software and associated documentation files (the "Software"), to deal
  7. // in the Software without restriction, including without limitation the rights
  8. // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  9. // copies of the Software, and to permit persons to whom the Software is
  10. // furnished to do so, subject to the following conditions:
  11. //
  12. // The above copyright notice and this permission notice shall be included in
  13. // all copies or substantial portions of the Software.
  14. //
  15. // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16. // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17. // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  18. // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  19. // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  20. // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  21. // THE SOFTWARE.
  22. //
  23. #include "Precompiled.h"
  24. #include "CollisionShape.h"
  25. #include "Constraint.h"
  26. #include "Context.h"
  27. #include "DebugRenderer.h"
  28. #include "Log.h"
  29. #include "Mutex.h"
  30. #include "PhysicsEvents.h"
  31. #include "PhysicsUtils.h"
  32. #include "PhysicsWorld.h"
  33. #include "Profiler.h"
  34. #include "Ray.h"
  35. #include "RigidBody.h"
  36. #include "Scene.h"
  37. #include "SceneEvents.h"
  38. #include "Sort.h"
  39. #include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
  40. #include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
  41. #include <BulletCollision/CollisionShapes/btBoxShape.h>
  42. #include <BulletCollision/CollisionShapes/btSphereShape.h>
  43. #include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
  44. #include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
  45. static const int DEFAULT_FPS = 60;
  46. static const Vector3 DEFAULT_GRAVITY = Vector3(0.0f, -9.81f, 0.0f);
  47. static bool CompareRaycastResults(const PhysicsRaycastResult& lhs, const PhysicsRaycastResult& rhs)
  48. {
  49. return lhs.distance_ < rhs.distance_;
  50. }
  51. void InternalPreTickCallback(btDynamicsWorld *world, btScalar timeStep)
  52. {
  53. static_cast<PhysicsWorld*>(world->getWorldUserInfo())->PreStep(timeStep);
  54. }
  55. void InternalTickCallback(btDynamicsWorld *world, btScalar timeStep)
  56. {
  57. static_cast<PhysicsWorld*>(world->getWorldUserInfo())->PostStep(timeStep);
  58. }
  59. /// Callback for physics world queries.
  60. struct PhysicsQueryCallback : public btCollisionWorld::ContactResultCallback
  61. {
  62. /// Construct.
  63. PhysicsQueryCallback(PODVector<RigidBody*>& result) : result_(result)
  64. {
  65. }
  66. /// Add a contact result.
  67. virtual btScalar addSingleResult(btManifoldPoint &, const btCollisionObject *colObj0, int, int, const btCollisionObject *colObj1, int, int)
  68. {
  69. RigidBody* body = reinterpret_cast<RigidBody*>(colObj0->getUserPointer());
  70. if (body && !result_.Contains(body))
  71. result_.Push(body);
  72. body = reinterpret_cast<RigidBody*>(colObj1->getUserPointer());
  73. if (body && !result_.Contains(body))
  74. result_.Push(body);
  75. return 0.0f;
  76. }
  77. /// Found rigid bodies.
  78. PODVector<RigidBody*>& result_;
  79. };
  80. OBJECTTYPESTATIC(PhysicsWorld);
  81. PhysicsWorld::PhysicsWorld(Context* context) :
  82. Component(context),
  83. collisionConfiguration_(0),
  84. collisionDispatcher_(0),
  85. broadphase_(0),
  86. solver_(0),
  87. world_(0),
  88. fps_(DEFAULT_FPS),
  89. timeAcc_(0.0f),
  90. maxNetworkAngularVelocity_(DEFAULT_MAX_NETWORK_ANGULAR_VELOCITY),
  91. interpolation_(true),
  92. applyingTransforms_(false),
  93. debugRenderer_(0),
  94. debugMode_(btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawConstraints)
  95. {
  96. collisionConfiguration_ = new btDefaultCollisionConfiguration();
  97. collisionDispatcher_ = new btCollisionDispatcher(collisionConfiguration_);
  98. broadphase_ = new btDbvtBroadphase();
  99. solver_ = new btSequentialImpulseConstraintSolver();
  100. world_ = new btDiscreteDynamicsWorld(collisionDispatcher_, broadphase_, solver_, collisionConfiguration_);
  101. world_->setGravity(ToBtVector3(DEFAULT_GRAVITY));
  102. world_->getDispatchInfo().m_useContinuous = true;
  103. world_->setDebugDrawer(this);
  104. world_->setInternalTickCallback(InternalPreTickCallback, static_cast<void*>(this), true);
  105. world_->setInternalTickCallback(InternalTickCallback, static_cast<void*>(this), false);
  106. }
  107. PhysicsWorld::~PhysicsWorld()
  108. {
  109. if (scene_)
  110. {
  111. // Force all remaining constraints, rigid bodies and collision shapes to release themselves
  112. for (PODVector<Constraint*>::Iterator i = constraints_.Begin(); i != constraints_.End(); ++i)
  113. (*i)->ReleaseConstraint();
  114. for (PODVector<RigidBody*>::Iterator i = rigidBodies_.Begin(); i != rigidBodies_.End(); ++i)
  115. (*i)->ReleaseBody();
  116. for (PODVector<CollisionShape*>::Iterator i = collisionShapes_.Begin(); i != collisionShapes_.End(); ++i)
  117. (*i)->ReleaseShape();
  118. }
  119. // Remove any cached geometries that still remain
  120. geometryCache_.Clear();
  121. delete world_;
  122. world_ = 0;
  123. delete solver_;
  124. solver_ = 0;
  125. delete broadphase_;
  126. broadphase_ = 0;
  127. delete collisionDispatcher_;
  128. collisionDispatcher_ = 0;
  129. delete collisionConfiguration_;
  130. collisionConfiguration_ = 0;
  131. }
  132. void PhysicsWorld::RegisterObject(Context* context)
  133. {
  134. context->RegisterFactory<PhysicsWorld>();
  135. ACCESSOR_ATTRIBUTE(PhysicsWorld, VAR_VECTOR3, "Gravity", GetGravity, SetGravity, Vector3, DEFAULT_GRAVITY, AM_DEFAULT);
  136. ATTRIBUTE(PhysicsWorld, VAR_INT, "Physics FPS", fps_, DEFAULT_FPS, AM_DEFAULT);
  137. ATTRIBUTE(PhysicsWorld, VAR_FLOAT, "Net Max Angular Vel.", maxNetworkAngularVelocity_, DEFAULT_MAX_NETWORK_ANGULAR_VELOCITY, AM_DEFAULT);
  138. ATTRIBUTE(PhysicsWorld, VAR_BOOL, "Interpolation", interpolation_, true, AM_FILE);
  139. }
  140. bool PhysicsWorld::isVisible(const btVector3& aabbMin, const btVector3& aabbMax)
  141. {
  142. if (debugRenderer_)
  143. return debugRenderer_->IsInside(BoundingBox(ToVector3(aabbMin), ToVector3(aabbMax)));
  144. else
  145. return false;
  146. }
  147. void PhysicsWorld::drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
  148. {
  149. if (debugRenderer_)
  150. debugRenderer_->AddLine(ToVector3(from), ToVector3(to), Color(color.x(), color.y(), color.z()), debugDepthTest_);
  151. }
  152. void PhysicsWorld::DrawDebugGeometry(DebugRenderer* debug, bool depthTest)
  153. {
  154. if (debug)
  155. {
  156. PROFILE(PhysicsDrawDebug);
  157. debugRenderer_ = debug;
  158. debugDepthTest_ = depthTest;
  159. world_->debugDrawWorld();
  160. debugRenderer_ = 0;
  161. }
  162. }
  163. void PhysicsWorld::reportErrorWarning(const char* warningString)
  164. {
  165. LOGWARNING("Physics: " + String(warningString));
  166. }
  167. void PhysicsWorld::Update(float timeStep)
  168. {
  169. PROFILE(UpdatePhysics);
  170. float internalTimeStep = 1.0f / fps_;
  171. delayedWorldTransforms_.Clear();
  172. if (interpolation_)
  173. {
  174. int maxSubSteps = (int)(timeStep * fps_) + 1;
  175. world_->stepSimulation(timeStep, maxSubSteps, internalTimeStep);
  176. }
  177. else
  178. {
  179. timeAcc_ += timeStep;
  180. while (timeAcc_ >= internalTimeStep)
  181. {
  182. world_->stepSimulation(internalTimeStep, 0, internalTimeStep);
  183. timeAcc_ -= internalTimeStep;
  184. }
  185. }
  186. // Apply delayed (parented) world transforms now
  187. while (!delayedWorldTransforms_.Empty())
  188. {
  189. for (HashMap<RigidBody*, DelayedWorldTransform>::Iterator i = delayedWorldTransforms_.Begin();
  190. i != delayedWorldTransforms_.End(); )
  191. {
  192. HashMap<RigidBody*, DelayedWorldTransform>::Iterator current = i++;
  193. const DelayedWorldTransform& transform = current->second_;
  194. // If parent's transform has already been assigned, can proceed
  195. if (!delayedWorldTransforms_.Contains(transform.parentRigidBody_))
  196. {
  197. transform.rigidBody_->ApplyWorldTransform(transform.worldPosition_, transform.worldRotation_);
  198. delayedWorldTransforms_.Erase(current);
  199. }
  200. }
  201. }
  202. }
  203. void PhysicsWorld::UpdateCollisions()
  204. {
  205. world_->performDiscreteCollisionDetection();
  206. }
  207. void PhysicsWorld::SetFps(int fps)
  208. {
  209. fps_ = Clamp(fps, 1, 1000);
  210. }
  211. void PhysicsWorld::SetGravity(Vector3 gravity)
  212. {
  213. world_->setGravity(ToBtVector3(gravity));
  214. }
  215. void PhysicsWorld::SetInterpolation(bool enable)
  216. {
  217. interpolation_ = enable;
  218. }
  219. void PhysicsWorld::SetMaxNetworkAngularVelocity(float velocity)
  220. {
  221. maxNetworkAngularVelocity_ = Clamp(velocity, 1.0f, 32767.0f);
  222. }
  223. void PhysicsWorld::Raycast(PODVector<PhysicsRaycastResult>& result, const Ray& ray, float maxDistance, unsigned collisionMask)
  224. {
  225. PROFILE(PhysicsRaycast);
  226. btCollisionWorld::AllHitsRayResultCallback rayCallback(ToBtVector3(ray.origin_), ToBtVector3(ray.origin_ +
  227. maxDistance * ray.direction_));
  228. rayCallback.m_collisionFilterGroup = (short)0xffff;
  229. rayCallback.m_collisionFilterMask = collisionMask;
  230. world_->rayTest(rayCallback.m_rayFromWorld, rayCallback.m_rayToWorld, rayCallback);
  231. for (int i = 0; i < rayCallback.m_collisionObjects.size(); ++i)
  232. {
  233. PhysicsRaycastResult newResult;
  234. newResult.body_ = static_cast<RigidBody*>(rayCallback.m_collisionObjects[i]->getUserPointer());
  235. newResult.position_ = ToVector3(rayCallback.m_hitPointWorld[i]);
  236. newResult.normal_ = ToVector3(rayCallback.m_hitNormalWorld[i]);
  237. newResult.distance_ = (newResult.position_ - ray.origin_).Length();
  238. result.Push(newResult);
  239. }
  240. Sort(result.Begin(), result.End(), CompareRaycastResults);
  241. }
  242. void PhysicsWorld::RaycastSingle(PhysicsRaycastResult& result, const Ray& ray, float maxDistance, unsigned collisionMask)
  243. {
  244. PROFILE(PhysicsRaycastSingle);
  245. btCollisionWorld::ClosestRayResultCallback rayCallback(ToBtVector3(ray.origin_), ToBtVector3(ray.origin_ +
  246. maxDistance * ray.direction_));
  247. rayCallback.m_collisionFilterGroup = (short)0xffff;
  248. rayCallback.m_collisionFilterMask = collisionMask;
  249. world_->rayTest(rayCallback.m_rayFromWorld, rayCallback.m_rayToWorld, rayCallback);
  250. if (rayCallback.hasHit())
  251. {
  252. result.body_ = static_cast<RigidBody*>(rayCallback.m_collisionObject->getUserPointer());
  253. result.position_ = ToVector3(rayCallback.m_hitPointWorld);
  254. result.normal_ = ToVector3(rayCallback.m_hitNormalWorld);
  255. result.distance_ = (result.position_ - ray.origin_).Length();
  256. }
  257. else
  258. {
  259. result.body_ = 0;
  260. result.position_ = Vector3::ZERO;
  261. result.normal_ = Vector3::ZERO;
  262. result.distance_ = M_INFINITY;
  263. }
  264. }
  265. void PhysicsWorld::SphereCast(PhysicsRaycastResult& result, const Ray& ray, float radius, float maxDistance, unsigned collisionMask)
  266. {
  267. PROFILE(PhysicsSphereCast);
  268. btSphereShape shape(radius);
  269. btCollisionWorld::ClosestConvexResultCallback convexCallback(ToBtVector3(ray.origin_), ToBtVector3(ray.origin_ +
  270. maxDistance * ray.direction_));
  271. convexCallback.m_collisionFilterGroup = (short)0xffff;
  272. convexCallback.m_collisionFilterMask = collisionMask;
  273. world_->convexSweepTest(&shape, btTransform(btQuaternion::getIdentity(), convexCallback.m_convexFromWorld),
  274. btTransform(btQuaternion::getIdentity(), convexCallback.m_convexToWorld), convexCallback);
  275. if (convexCallback.hasHit())
  276. {
  277. result.body_ = static_cast<RigidBody*>(convexCallback.m_hitCollisionObject->getUserPointer());
  278. result.position_ = ToVector3(convexCallback.m_hitPointWorld);
  279. result.normal_ = ToVector3(convexCallback.m_hitNormalWorld);
  280. result.distance_ = (result.position_ - ray.origin_).Length();
  281. }
  282. else
  283. {
  284. result.body_ = 0;
  285. result.position_ = Vector3::ZERO;
  286. result.normal_ = Vector3::ZERO;
  287. result.distance_ = M_INFINITY;
  288. }
  289. }
  290. void PhysicsWorld::GetRigidBodies(PODVector<RigidBody*>& result, const Sphere& sphere, unsigned collisionMask)
  291. {
  292. PROFILE(PhysicsWorld_GetRigidBodies);
  293. result.Clear();
  294. btSphereShape sphereShape(sphere.radius_);
  295. btRigidBody* tempRigidBody = new btRigidBody(1.0f, 0, &sphereShape);
  296. tempRigidBody->setWorldTransform(btTransform(btQuaternion::getIdentity(), ToBtVector3(sphere.center_)));
  297. // Need to activate the temporary rigid body to get reliable results from static, sleeping objects
  298. tempRigidBody->activate();
  299. world_->addRigidBody(tempRigidBody, (short)0xffff, (short)collisionMask);
  300. PhysicsQueryCallback callback(result);
  301. world_->contactTest(tempRigidBody, callback);
  302. world_->removeRigidBody(tempRigidBody);
  303. delete tempRigidBody;
  304. }
  305. void PhysicsWorld::GetRigidBodies(PODVector<RigidBody*>& result, const BoundingBox& box, unsigned collisionMask)
  306. {
  307. PROFILE(PhysicsWorld_GetRigidBodies);
  308. result.Clear();
  309. btBoxShape boxShape(ToBtVector3(box.HalfSize()));
  310. btRigidBody* tempRigidBody = new btRigidBody(1.0f, 0, &boxShape);
  311. tempRigidBody->setWorldTransform(btTransform(btQuaternion::getIdentity(), ToBtVector3(box.Center())));
  312. tempRigidBody->activate();
  313. world_->addRigidBody(tempRigidBody, (short)0xffff, (short)collisionMask);
  314. PhysicsQueryCallback callback(result);
  315. world_->contactTest(tempRigidBody, callback);
  316. world_->removeRigidBody(tempRigidBody);
  317. delete tempRigidBody;
  318. }
  319. Vector3 PhysicsWorld::GetGravity() const
  320. {
  321. return ToVector3(world_->getGravity());
  322. }
  323. void PhysicsWorld::AddRigidBody(RigidBody* body)
  324. {
  325. rigidBodies_.Push(body);
  326. }
  327. void PhysicsWorld::RemoveRigidBody(RigidBody* body)
  328. {
  329. rigidBodies_.Erase(rigidBodies_.Find(body));
  330. }
  331. void PhysicsWorld::AddCollisionShape(CollisionShape* shape)
  332. {
  333. collisionShapes_.Push(shape);
  334. }
  335. void PhysicsWorld::RemoveCollisionShape(CollisionShape* shape)
  336. {
  337. collisionShapes_.Erase(collisionShapes_.Find(shape));
  338. }
  339. void PhysicsWorld::AddConstraint(Constraint* constraint)
  340. {
  341. constraints_.Push(constraint);
  342. }
  343. void PhysicsWorld::RemoveConstraint(Constraint* constraint)
  344. {
  345. constraints_.Erase(constraints_.Find(constraint));
  346. }
  347. void PhysicsWorld::AddDelayedWorldTransform(const DelayedWorldTransform& transform)
  348. {
  349. delayedWorldTransforms_[transform.rigidBody_] = transform;
  350. }
  351. void PhysicsWorld::DrawDebugGeometry(bool depthTest)
  352. {
  353. DebugRenderer* debug = GetComponent<DebugRenderer>();
  354. DrawDebugGeometry(debug, depthTest);
  355. }
  356. void PhysicsWorld::SetDebugRenderer(DebugRenderer* debug)
  357. {
  358. debugRenderer_ = debug;
  359. }
  360. void PhysicsWorld::SetDebugDepthTest(bool enable)
  361. {
  362. debugDepthTest_ = enable;
  363. }
  364. void PhysicsWorld::CleanupGeometryCache()
  365. {
  366. // Remove cached shapes whose only reference is the cache itself
  367. for (HashMap<String, SharedPtr<CollisionGeometryData> >::Iterator i = geometryCache_.Begin();
  368. i != geometryCache_.End();)
  369. {
  370. HashMap<String, SharedPtr<CollisionGeometryData> >::Iterator current = i++;
  371. if (current->second_.Refs() == 1)
  372. geometryCache_.Erase(current);
  373. }
  374. }
  375. void PhysicsWorld::OnNodeSet(Node* node)
  376. {
  377. // Subscribe to the scene subsystem update, which will trigger the physics simulation step
  378. if (node)
  379. {
  380. scene_ = GetScene();
  381. SubscribeToEvent(node, E_SCENESUBSYSTEMUPDATE, HANDLER(PhysicsWorld, HandleSceneSubsystemUpdate));
  382. }
  383. }
  384. void PhysicsWorld::HandleSceneSubsystemUpdate(StringHash eventType, VariantMap& eventData)
  385. {
  386. using namespace SceneSubsystemUpdate;
  387. Update(eventData[P_TIMESTEP].GetFloat());
  388. }
  389. void PhysicsWorld::PreStep(float timeStep)
  390. {
  391. // Send pre-step event
  392. using namespace PhysicsPreStep;
  393. VariantMap eventData;
  394. eventData[P_WORLD] = (void*)this;
  395. eventData[P_TIMESTEP] = timeStep;
  396. SendEvent(E_PHYSICSPRESTEP, eventData);
  397. // Start profiling block for the actual simulation step
  398. #ifdef ENABLE_PROFILING
  399. Profiler* profiler = GetSubsystem<Profiler>();
  400. if (profiler)
  401. profiler->BeginBlock("StepSimulation");
  402. #endif
  403. }
  404. void PhysicsWorld::PostStep(float timeStep)
  405. {
  406. #ifdef ENABLE_PROFILING
  407. Profiler* profiler = GetSubsystem<Profiler>();
  408. if (profiler)
  409. profiler->EndBlock();
  410. #endif
  411. SendCollisionEvents();
  412. // Send post-step event
  413. using namespace PhysicsPreStep;
  414. VariantMap eventData;
  415. eventData[P_WORLD] = (void*)this;
  416. eventData[P_TIMESTEP] = timeStep;
  417. SendEvent(E_PHYSICSPOSTSTEP, eventData);
  418. }
  419. void PhysicsWorld::SendCollisionEvents()
  420. {
  421. PROFILE(SendCollisionEvents);
  422. currentCollisions_.Clear();
  423. int numManifolds = collisionDispatcher_->getNumManifolds();
  424. if (numManifolds)
  425. {
  426. VariantMap physicsCollisionData;
  427. VariantMap nodeCollisionData;
  428. VectorBuffer contacts;
  429. physicsCollisionData[PhysicsCollision::P_WORLD] = (void*)this;
  430. for (int i = 0; i < numManifolds; ++i)
  431. {
  432. btPersistentManifold* contactManifold = collisionDispatcher_->getManifoldByIndexInternal(i);
  433. int numContacts = contactManifold->getNumContacts();
  434. // First check that there are actual contacts, as the manifold exists also when objects are close but not touching
  435. if (!numContacts)
  436. continue;
  437. btCollisionObject* objectA = static_cast<btCollisionObject*>(contactManifold->getBody0());
  438. btCollisionObject* objectB = static_cast<btCollisionObject*>(contactManifold->getBody1());
  439. RigidBody* bodyA = static_cast<RigidBody*>(objectA->getUserPointer());
  440. RigidBody* bodyB = static_cast<RigidBody*>(objectB->getUserPointer());
  441. WeakPtr<RigidBody> bodyWeakA(bodyA);
  442. WeakPtr<RigidBody> bodyWeakB(bodyB);
  443. // Skip collision event signaling if both objects are static, or if collision event mode does not match
  444. if (bodyA->GetMass() == 0.0f && bodyB->GetMass() == 0.0f)
  445. continue;
  446. if (bodyA->GetCollisionEventMode() == COLLISION_NEVER || bodyB->GetCollisionEventMode() == COLLISION_NEVER)
  447. continue;
  448. if (bodyA->GetCollisionEventMode() == COLLISION_ACTIVE && bodyB->GetCollisionEventMode() == COLLISION_ACTIVE &&
  449. !bodyA->IsActive() && !bodyB->IsActive())
  450. continue;
  451. Node* nodeA = bodyA->GetNode();
  452. Node* nodeB = bodyB->GetNode();
  453. WeakPtr<Node> nodeWeakA(nodeA);
  454. WeakPtr<Node> nodeWeakB(nodeB);
  455. Pair<RigidBody*, RigidBody*> bodyPair;
  456. if (bodyA < bodyB)
  457. bodyPair = MakePair(bodyA, bodyB);
  458. else
  459. bodyPair = MakePair(bodyB, bodyA);
  460. currentCollisions_.Insert(bodyPair);
  461. bool newCollision = !previousCollisions_.Contains(bodyPair);
  462. physicsCollisionData[PhysicsCollision::P_NODEA] = (void*)nodeA;
  463. physicsCollisionData[PhysicsCollision::P_NODEB] = (void*)nodeB;
  464. physicsCollisionData[PhysicsCollision::P_BODYA] = (void*)bodyA;
  465. physicsCollisionData[PhysicsCollision::P_BODYB] = (void*)bodyB;
  466. physicsCollisionData[PhysicsCollision::P_NEWCOLLISION] = !previousCollisions_.Contains(bodyPair);
  467. contacts.Clear();
  468. for (int j = 0; j < numContacts; ++j)
  469. {
  470. btManifoldPoint& point = contactManifold->getContactPoint(j);
  471. contacts.WriteVector3(ToVector3(point.m_positionWorldOnB));
  472. contacts.WriteVector3(ToVector3(point.m_normalWorldOnB));
  473. contacts.WriteFloat(point.m_distance1);
  474. contacts.WriteFloat(point.m_appliedImpulse);
  475. }
  476. physicsCollisionData[PhysicsCollision::P_CONTACTS] = contacts.GetBuffer();
  477. SendEvent(E_PHYSICSCOLLISION, physicsCollisionData);
  478. // Skip if either of the nodes or bodies has been removed as a response to the event
  479. if (!nodeWeakA || !nodeWeakB || !bodyWeakA || !bodyWeakB)
  480. continue;
  481. nodeCollisionData[NodeCollision::P_BODY] = (void*)bodyA;
  482. nodeCollisionData[NodeCollision::P_OTHERNODE] = (void*)nodeB;
  483. nodeCollisionData[NodeCollision::P_OTHERBODY] = (void*)bodyB;
  484. nodeCollisionData[NodeCollision::P_NEWCOLLISION] = newCollision;
  485. nodeCollisionData[NodeCollision::P_CONTACTS] = contacts.GetBuffer();
  486. SendEvent(nodeA, E_NODECOLLISION, nodeCollisionData);
  487. // Skip if either of the nodes has been removed as a response to the event
  488. if (!nodeWeakA || !nodeWeakB)
  489. continue;
  490. contacts.Clear();
  491. for (int j = 0; j < numContacts; ++j)
  492. {
  493. btManifoldPoint& point = contactManifold->getContactPoint(j);
  494. contacts.WriteVector3(ToVector3(point.m_positionWorldOnB));
  495. contacts.WriteVector3(-ToVector3(point.m_normalWorldOnB));
  496. contacts.WriteFloat(point.m_distance1);
  497. contacts.WriteFloat(point.m_appliedImpulse);
  498. }
  499. nodeCollisionData[NodeCollision::P_BODY] = (void*)bodyB;
  500. nodeCollisionData[NodeCollision::P_OTHERNODE] = (void*)nodeA;
  501. nodeCollisionData[NodeCollision::P_OTHERBODY] = (void*)bodyA;
  502. nodeCollisionData[NodeCollision::P_CONTACTS] = contacts.GetBuffer();
  503. SendEvent(nodeB, E_NODECOLLISION, nodeCollisionData);
  504. }
  505. }
  506. previousCollisions_ = currentCollisions_;
  507. }
  508. void RegisterPhysicsLibrary(Context* context)
  509. {
  510. CollisionShape::RegisterObject(context);
  511. RigidBody::RegisterObject(context);
  512. Constraint::RegisterObject(context);
  513. PhysicsWorld::RegisterObject(context);
  514. }