PhysicsWorld.cpp 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678
  1. // Copyright (C) 2009-present, Panagiotis Christopoulos Charitos and contributors.
  2. // All rights reserved.
  3. // Code licensed under the BSD License.
  4. // http://www.anki3d.org/LICENSE
  5. #include <AnKi/Physics2/PhysicsWorld.h>
  6. #include <AnKi/Util/System.h>
  7. #include <Jolt/Renderer/DebugRendererSimple.h>
  8. #include <Jolt/ConfigurationString.h>
  9. namespace anki {
  10. namespace v2 {
  11. class BroadphaseLayer
  12. {
  13. public:
  14. static constexpr JPH::BroadPhaseLayer kStatic{0};
  15. static constexpr JPH::BroadPhaseLayer kDynamic{1};
  16. static constexpr JPH::BroadPhaseLayer kDebris{2};
  17. static constexpr U32 kCount = 3;
  18. };
  19. static JPH::BroadPhaseLayer objectLayerToBroadphaseLayer(PhysicsLayer objectLayer)
  20. {
  21. switch(PhysicsLayer(objectLayer))
  22. {
  23. case PhysicsLayer::kStatic:
  24. return BroadphaseLayer::kStatic;
  25. case PhysicsLayer::kDebris:
  26. return BroadphaseLayer::kDebris;
  27. default:
  28. return BroadphaseLayer::kDynamic;
  29. }
  30. }
  31. class BPLayerInterfaceImpl final : public JPH::BroadPhaseLayerInterface
  32. {
  33. public:
  34. U32 GetNumBroadPhaseLayers() const override
  35. {
  36. return BroadphaseLayer::kCount;
  37. }
  38. JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer objLayer) const override
  39. {
  40. return objectLayerToBroadphaseLayer(PhysicsLayer(objLayer));
  41. }
  42. };
  43. // Class that determines if an object layer can collide with a broadphase layer
  44. class ObjectVsBroadPhaseLayerFilterImpl final : public JPH::ObjectVsBroadPhaseLayerFilter
  45. {
  46. public:
  47. Bool ShouldCollide(JPH::ObjectLayer layer1, JPH::BroadPhaseLayer layer2) const override
  48. {
  49. switch(PhysicsLayer(layer1))
  50. {
  51. case PhysicsLayer::kStatic:
  52. return layer2 != BroadphaseLayer::kStatic; // Static doesn't collide with static
  53. case PhysicsLayer::kDebris:
  54. return layer2 == BroadphaseLayer::kStatic; // Debris only collides with static
  55. default:
  56. return true;
  57. }
  58. }
  59. };
  60. /// Class that determines if two object layers can collide
  61. class ObjectLayerPairFilterImpl final : public JPH::ObjectLayerPairFilter
  62. {
  63. public:
  64. Bool ShouldCollide(JPH::ObjectLayer layer1, JPH::ObjectLayer layer2) const override
  65. {
  66. const PhysicsLayerBit layer1Bit = PhysicsLayerBit(1u << layer1);
  67. const PhysicsLayerBit layer2Bit = PhysicsLayerBit(1u << layer2);
  68. const PhysicsLayerBit layer1Mask = kPhysicsLayerCollisionTable[layer1];
  69. const PhysicsLayerBit layer2Mask = kPhysicsLayerCollisionTable[layer2];
  70. return !!(layer1Bit & layer2Mask) && !!(layer2Bit & layer1Mask);
  71. }
  72. };
  73. class MaskBroadPhaseLayerFilter final : public JPH::BroadPhaseLayerFilter
  74. {
  75. public:
  76. PhysicsLayerBit m_layerMask;
  77. Bool ShouldCollide(JPH::BroadPhaseLayer inLayer) const override
  78. {
  79. for(PhysicsLayer layer : EnumBitsIterable<PhysicsLayer, PhysicsLayerBit>(m_layerMask))
  80. {
  81. if(objectLayerToBroadphaseLayer(layer) == inLayer)
  82. {
  83. return true;
  84. }
  85. }
  86. return false;
  87. }
  88. };
  89. class MaskObjectLayerFilter final : public JPH::ObjectLayerFilter
  90. {
  91. public:
  92. PhysicsLayerBit m_layerMask;
  93. Bool ShouldCollide(JPH::ObjectLayer inLayer) const override
  94. {
  95. const PhysicsLayerBit inMask = PhysicsLayerBit(1u << inLayer);
  96. return !!(m_layerMask & inMask);
  97. }
  98. };
  99. class PhysicsWorld::MyBodyActivationListener final : public JPH::BodyActivationListener
  100. {
  101. public:
  102. void OnBodyActivated([[maybe_unused]] const JPH::BodyID& inBodyID, U64 bodyUserData) override
  103. {
  104. PhysicsObjectBase* base = numberToPtr<PhysicsObjectBase*>(bodyUserData);
  105. if(base->getType() == PhysicsObjectType::kBody)
  106. {
  107. static_cast<PhysicsBody*>(base)->m_activated = 1;
  108. }
  109. else
  110. {
  111. // Don't care
  112. }
  113. }
  114. void OnBodyDeactivated([[maybe_unused]] const JPH::BodyID& inBodyID, U64 bodyUserData) override
  115. {
  116. PhysicsObjectBase* base = numberToPtr<PhysicsObjectBase*>(bodyUserData);
  117. if(base->getType() == PhysicsObjectType::kBody)
  118. {
  119. static_cast<PhysicsBody*>(base)->m_activated = 0;
  120. }
  121. else
  122. {
  123. // Don't care
  124. }
  125. }
  126. };
  127. class PhysicsWorld::MyContactListener final : public JPH::ContactListener
  128. {
  129. public:
  130. void gatherObjects(U64 body1UserData, U64 body2UserData, PhysicsBody*& trigger, PhysicsObjectBase*& receiver)
  131. {
  132. PhysicsObjectBase* obj1 = numberToPtr<PhysicsObjectBase*>(body1UserData);
  133. PhysicsObjectBase* obj2 = numberToPtr<PhysicsObjectBase*>(body2UserData);
  134. if(obj1->getType() == PhysicsObjectType::kBody && static_cast<PhysicsBody*>(obj1)->m_triggerCallbacks)
  135. {
  136. ANKI_ASSERT(obj2->getType() == PhysicsObjectType::kBody || obj2->getType() == PhysicsObjectType::kPlayerController);
  137. trigger = static_cast<PhysicsBody*>(obj1);
  138. receiver = obj2;
  139. }
  140. else if(obj2->getType() == PhysicsObjectType::kBody && static_cast<PhysicsBody*>(obj2)->m_triggerCallbacks)
  141. {
  142. ANKI_ASSERT(obj1->getType() == PhysicsObjectType::kBody || obj1->getType() == PhysicsObjectType::kPlayerController);
  143. trigger = static_cast<PhysicsBody*>(obj2);
  144. receiver = obj1;
  145. }
  146. else
  147. {
  148. trigger = nullptr;
  149. receiver = nullptr;
  150. }
  151. }
  152. void OnContactAdded(const JPH::Body& inBody1, const JPH::Body& inBody2, [[maybe_unused]] const JPH::ContactManifold& inManifold,
  153. [[maybe_unused]] JPH::ContactSettings& ioSettings) override
  154. {
  155. // You can practically do nothing with the bodies so stash them
  156. PhysicsBody* trigger;
  157. PhysicsObjectBase* receiver;
  158. gatherObjects(inBody1.GetUserData(), inBody2.GetUserData(), trigger, receiver);
  159. if(!trigger)
  160. {
  161. return;
  162. }
  163. const Contact contact = {trigger, receiver};
  164. PhysicsWorld& world = PhysicsWorld::getSingleton();
  165. LockGuard lock(world.m_insertedContactsMtx);
  166. world.m_insertedContacts.emplaceBack(contact);
  167. }
  168. void OnContactRemoved(const JPH::SubShapeIDPair& pair) override
  169. {
  170. // You can practically do nothing with the bodies so stash them
  171. PhysicsWorld& world = PhysicsWorld::getSingleton();
  172. PhysicsBody* trigger;
  173. PhysicsObjectBase* receiver;
  174. gatherObjects(world.m_jphPhysicsSystem->GetBodyInterfaceNoLock().GetUserData(pair.GetBody1ID()),
  175. world.m_jphPhysicsSystem->GetBodyInterfaceNoLock().GetUserData(pair.GetBody2ID()), trigger, receiver);
  176. if(!trigger)
  177. {
  178. return;
  179. }
  180. const Contact contact = {trigger, receiver};
  181. LockGuard lock(world.m_deletedContactsMtx);
  182. world.m_deletedContacts.emplaceBack(contact);
  183. }
  184. };
  185. // Globals
  186. static BPLayerInterfaceImpl g_bPLayerInterfaceImpl;
  187. static ObjectVsBroadPhaseLayerFilterImpl g_objectVsBroadPhaseLayerFilterImpl;
  188. static ObjectLayerPairFilterImpl g_objectLayerPairFilterImpl;
  189. PhysicsWorld::MyBodyActivationListener PhysicsWorld::m_bodyActivationListener;
  190. PhysicsWorld::MyContactListener PhysicsWorld::m_contactListener;
  191. void PhysicsCollisionShapePtrDeleter::operator()(PhysicsCollisionShape* ptr)
  192. {
  193. ANKI_ASSERT(ptr);
  194. PhysicsWorld& world = PhysicsWorld::getSingleton();
  195. LockGuard lock(world.m_collisionShapes.m_mtx);
  196. world.m_collisionShapes.m_array.erase(ptr->m_blockArrayIndex);
  197. }
  198. void PhysicsBodyPtrDeleter::operator()(PhysicsBody* ptr)
  199. {
  200. ANKI_ASSERT(ptr);
  201. PhysicsWorld& world = PhysicsWorld::getSingleton();
  202. world.m_jphPhysicsSystem->GetBodyInterface().RemoveBody(ptr->m_jphBody->GetID());
  203. world.m_jphPhysicsSystem->GetBodyInterface().DestroyBody(ptr->m_jphBody->GetID());
  204. LockGuard lock(world.m_bodies.m_mtx);
  205. world.m_bodies.m_array.erase(ptr->m_blockArrayIndex);
  206. world.m_optimizeBroadphase = true;
  207. }
  208. void PhysicsJointPtrDeleter::operator()(PhysicsJoint* ptr)
  209. {
  210. ANKI_ASSERT(ptr);
  211. PhysicsWorld& world = PhysicsWorld::getSingleton();
  212. LockGuard lock(world.m_joints.m_mtx);
  213. world.m_joints.m_array.erase(ptr->m_blockArrayIndex);
  214. }
  215. void PhysicsPlayerControllerPtrDeleter::operator()(PhysicsPlayerController* ptr)
  216. {
  217. ANKI_ASSERT(ptr);
  218. PhysicsWorld& world = PhysicsWorld::getSingleton();
  219. LockGuard lock(world.m_characters.m_mtx);
  220. world.m_characters.m_array.erase(ptr->m_blockArrayIndex);
  221. }
  222. class PhysicsWorld::MyDebugRenderer final : public JPH::DebugRendererSimple
  223. {
  224. public:
  225. PhysicsDebugDrawerInterface* m_interface = nullptr;
  226. void DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) override
  227. {
  228. const Array<Vec3, 2> line = {toAnKi(inFrom), toAnKi(inTo)};
  229. m_interface->drawLines(line, {inColor.r, inColor.g, inColor.b, inColor.a});
  230. }
  231. void DrawTriangle(JPH::RVec3Arg inV1, JPH::RVec3Arg inV2, JPH::RVec3Arg inV3, JPH::ColorArg inColor,
  232. [[maybe_unused]] ECastShadow inCastShadow) override
  233. {
  234. Array<Vec3, 6> line = {toAnKi(inV1), toAnKi(inV2), toAnKi(inV2), toAnKi(inV3), toAnKi(inV3), toAnKi(inV1)};
  235. m_interface->drawLines(line, {inColor.r, inColor.g, inColor.b, inColor.a});
  236. }
  237. void DrawText3D([[maybe_unused]] JPH::RVec3Arg inPosition, [[maybe_unused]] const std::string_view& inString,
  238. [[maybe_unused]] JPH::ColorArg inColor, [[maybe_unused]] F32 inHeight) override
  239. {
  240. // Nothing for now
  241. }
  242. };
  243. PhysicsWorld::~PhysicsWorld()
  244. {
  245. ANKI_ASSERT(m_collisionShapes.m_array.getSize() == 0);
  246. ANKI_ASSERT(m_bodies.m_array.getSize() == 0);
  247. ANKI_ASSERT(m_joints.m_array.getSize() == 0);
  248. ANKI_ASSERT(m_characters.m_array.getSize() == 0);
  249. m_jobSystem.destroy();
  250. m_jphPhysicsSystem.destroy();
  251. JPH::UnregisterTypes();
  252. delete JPH::Factory::sInstance;
  253. JPH::Factory::sInstance = nullptr;
  254. PhysicsMemoryPool::freeSingleton();
  255. }
  256. Error PhysicsWorld::init(AllocAlignedCallback allocCb, void* allocCbData)
  257. {
  258. ANKI_PHYS_LOGI("Initializing physics. Jolt config: %s", JPH::GetConfigurationString());
  259. PhysicsMemoryPool::allocateSingleton(allocCb, allocCbData);
  260. JPH::Allocate = [](PtrSize size) -> void* {
  261. return PhysicsMemoryPool::getSingleton().allocate(size, 16);
  262. };
  263. JPH::Reallocate = []([[maybe_unused]] void* in, [[maybe_unused]] PtrSize oldSize, PtrSize newSize) -> void* {
  264. void* out;
  265. if(newSize > 0)
  266. {
  267. out = PhysicsMemoryPool::getSingleton().allocate(newSize, 16);
  268. }
  269. else
  270. {
  271. out = nullptr;
  272. }
  273. if(in && out)
  274. {
  275. ANKI_ASSERT(oldSize > 0 && newSize > 0);
  276. memcpy(out, in, min(oldSize, newSize));
  277. }
  278. PhysicsMemoryPool::getSingleton().free(in);
  279. return out;
  280. };
  281. JPH::Free = [](void* ptr) {
  282. PhysicsMemoryPool::getSingleton().free(ptr);
  283. };
  284. JPH::AlignedAllocate = [](PtrSize size, PtrSize alignment) -> void* {
  285. return PhysicsMemoryPool::getSingleton().allocate(size, alignment);
  286. };
  287. JPH::AlignedFree = [](void* ptr) {
  288. PhysicsMemoryPool::getSingleton().free(ptr);
  289. };
  290. JPH::Factory::sInstance = new JPH::Factory();
  291. JPH::RegisterTypes();
  292. m_jphPhysicsSystem.construct();
  293. constexpr U32 maxBodies = kMaxU16;
  294. constexpr U32 mutexCount = 0;
  295. constexpr U32 maxBodyPairs = kMaxU16;
  296. constexpr U32 maxConstraints = 10 * 1024;
  297. m_jphPhysicsSystem->Init(maxBodies, mutexCount, maxBodyPairs, maxConstraints, g_bPLayerInterfaceImpl, g_objectVsBroadPhaseLayerFilterImpl,
  298. g_objectLayerPairFilterImpl);
  299. m_jphPhysicsSystem->SetGravity(toJPH(Vec3(0.0f, -9.81f, 0.0f)));
  300. m_jphPhysicsSystem->SetBodyActivationListener(&m_bodyActivationListener);
  301. m_jphPhysicsSystem->SetContactListener(&m_contactListener);
  302. const U32 threadCount = min(8u, getCpuCoresCount() - 1);
  303. m_jobSystem.construct(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, threadCount);
  304. m_tempAllocator.construct(U32(10_MB));
  305. return Error::kNone;
  306. }
  307. void PhysicsWorld::update(Second dt)
  308. {
  309. // Pre-update work
  310. {
  311. for(PhysicsPlayerController& charController : m_characters.m_array)
  312. {
  313. charController.prePhysicsUpdate(dt);
  314. }
  315. if(m_optimizeBroadphase)
  316. {
  317. m_optimizeBroadphase = false;
  318. m_jphPhysicsSystem->OptimizeBroadPhase();
  319. }
  320. }
  321. constexpr I32 collisionSteps = 1;
  322. m_jphPhysicsSystem->Update(F32(dt), collisionSteps, &m_tempAllocator, &m_jobSystem);
  323. // Post-update work
  324. {
  325. for(PhysicsBody& body : m_bodies.m_array)
  326. {
  327. body.postPhysicsUpdate();
  328. }
  329. for(PhysicsPlayerController& charController : m_characters.m_array)
  330. {
  331. charController.postPhysicsUpdate();
  332. }
  333. for(Contact& contact : m_insertedContacts)
  334. {
  335. contact.m_trigger->m_triggerCallbacks->onTriggerEnter(*contact.m_trigger, *contact.m_receiver);
  336. }
  337. m_insertedContacts.destroy();
  338. for(Contact& contact : m_deletedContacts)
  339. {
  340. contact.m_trigger->m_triggerCallbacks->onTriggerExit(*contact.m_trigger, *contact.m_receiver);
  341. }
  342. m_deletedContacts.destroy();
  343. }
  344. }
  345. template<typename TJPHCollisionShape, typename... TArgs>
  346. PhysicsCollisionShapePtr PhysicsWorld::newCollisionShape(TArgs&&... args)
  347. {
  348. decltype(m_collisionShapes.m_array)::Iterator it;
  349. {
  350. LockGuard lock(m_collisionShapes.m_mtx);
  351. it = m_collisionShapes.m_array.emplace();
  352. }
  353. ClassWrapper<TJPHCollisionShape>& classw = reinterpret_cast<ClassWrapper<TJPHCollisionShape>&>(it->m_shapeBase);
  354. classw.construct(std::forward<TArgs>(args)...);
  355. classw->SetEmbedded();
  356. it->m_blockArrayIndex = it.getArrayIndex();
  357. return PhysicsCollisionShapePtr(&(*it));
  358. }
  359. PhysicsCollisionShapePtr PhysicsWorld::newSphereCollisionShape(F32 radius)
  360. {
  361. return newCollisionShape<JPH::SphereShape>(radius);
  362. }
  363. PhysicsCollisionShapePtr PhysicsWorld::newBoxCollisionShape(Vec3 extend)
  364. {
  365. return newCollisionShape<JPH::BoxShape>(toJPH(extend));
  366. }
  367. PhysicsCollisionShapePtr PhysicsWorld::newCapsuleCollisionShape(F32 height, F32 radius)
  368. {
  369. return newCollisionShape<JPH::CapsuleShape>(height / 2.0f, radius);
  370. }
  371. PhysicsCollisionShapePtr PhysicsWorld::newConvexHullShape(ConstWeakArray<Vec3> positions)
  372. {
  373. JPH::Array<JPH::Vec3> verts;
  374. verts.resize(positions.getSize());
  375. for(U32 i = 0; i < positions.getSize(); ++i)
  376. {
  377. verts[i] = toJPH(positions[i]);
  378. }
  379. JPH::ConvexHullShapeSettings settings(std::move(verts));
  380. settings.SetEmbedded();
  381. JPH::Shape::ShapeResult outResult;
  382. PhysicsCollisionShapePtr out = newCollisionShape<JPH::ConvexHullShape>(settings, outResult);
  383. ANKI_ASSERT(outResult.IsValid());
  384. return out;
  385. }
  386. PhysicsCollisionShapePtr PhysicsWorld::newStaticMeshShape(ConstWeakArray<Vec3> positions, ConstWeakArray<U32> indices)
  387. {
  388. ANKI_ASSERT(positions.getSize() && indices.getSize() && indices.getSize() % 3 == 0);
  389. JPH::VertexList verts;
  390. verts.resize(positions.getSize());
  391. for(U32 i = 0; i < positions.getSize(); ++i)
  392. {
  393. verts[i] = {positions[i].x(), positions[i].y(), positions[i].z()};
  394. }
  395. JPH::IndexedTriangleList idx;
  396. idx.resize(indices.getSize() / 3);
  397. for(U32 i = 0; i < indices.getSize(); i += 3)
  398. {
  399. const U32 material = 0;
  400. idx[i] = JPH::IndexedTriangle(indices[i], indices[i + 1], indices[i + 2], material);
  401. }
  402. JPH::MeshShapeSettings settings(std::move(verts), std::move(idx));
  403. settings.SetEmbedded();
  404. JPH::Shape::ShapeResult outResult;
  405. PhysicsCollisionShapePtr out = newCollisionShape<JPH::MeshShape>(settings, outResult);
  406. ANKI_ASSERT(outResult.IsValid());
  407. return out;
  408. }
  409. PhysicsCollisionShapePtr PhysicsWorld::newScaleCollisionObject(const Vec3& scale, PhysicsCollisionShape* baseShape)
  410. {
  411. return newCollisionShape<JPH::ScaledShape>(&baseShape->m_shapeBase, toJPH(scale));
  412. }
  413. PhysicsBodyPtr PhysicsWorld::newPhysicsBody(const PhysicsBodyInitInfo& init)
  414. {
  415. PhysicsBody* newBody;
  416. {
  417. LockGuard lock(m_bodies.m_mtx);
  418. m_optimizeBroadphase = true;
  419. auto it = m_bodies.m_array.emplace();
  420. newBody = &(*it);
  421. newBody->m_blockArrayIndex = it.getArrayIndex();
  422. }
  423. newBody->init(init);
  424. return PhysicsBodyPtr(newBody);
  425. }
  426. template<typename TJPHJoint, typename... TArgs>
  427. PhysicsJointPtr PhysicsWorld::newJoint(PhysicsBody* body1, PhysicsBody* body2, TArgs&&... args)
  428. {
  429. ANKI_ASSERT(body1 && body2);
  430. decltype(m_joints.m_array)::Iterator it;
  431. {
  432. LockGuard lock(m_joints.m_mtx);
  433. it = m_joints.m_array.emplace();
  434. }
  435. ClassWrapper<TJPHJoint>& classw = reinterpret_cast<ClassWrapper<TJPHJoint>&>(it->m_base);
  436. classw.construct(*body1->m_jphBody, *body2->m_jphBody, std::forward<TArgs>(args)...);
  437. classw->SetEmbedded();
  438. it->m_body1.reset(body1);
  439. it->m_body2.reset(body2);
  440. it->m_blockArrayIndex = it.getArrayIndex();
  441. m_jphPhysicsSystem->AddConstraint(&it->m_base); // It's thread-safe
  442. return PhysicsJointPtr(&(*it));
  443. }
  444. PhysicsJointPtr PhysicsWorld::newPointJoint(PhysicsBody* body1, PhysicsBody* body2, Bool pointsInWorldSpace, const Vec3& body1Point,
  445. const Vec3& body2Point)
  446. {
  447. JPH::PointConstraintSettings settings;
  448. settings.SetEmbedded();
  449. settings.mSpace = (pointsInWorldSpace) ? JPH::EConstraintSpace::WorldSpace : JPH::EConstraintSpace::LocalToBodyCOM;
  450. settings.mPoint1 = toJPH(body1Point);
  451. settings.mPoint2 = toJPH(body2Point);
  452. return newJoint<JPH::PointConstraint>(body1, body2, settings);
  453. }
  454. PhysicsPlayerControllerPtr PhysicsWorld::newPlayerController(const PhysicsPlayerControllerInitInfo& init)
  455. {
  456. PhysicsPlayerController* newChar;
  457. {
  458. LockGuard lock(m_characters.m_mtx);
  459. auto it = m_characters.m_array.emplace();
  460. newChar = &(*it);
  461. newChar->m_blockArrayIndex = it.getArrayIndex();
  462. }
  463. newChar->init(init);
  464. return PhysicsPlayerControllerPtr(newChar);
  465. }
  466. RayHitResult PhysicsWorld::jphToAnKi(const JPH::RRayCast& ray, const JPH::RayCastResult& hit)
  467. {
  468. RayHitResult result;
  469. const JPH::RVec3 hitPosJph = ray.GetPointOnRay(hit.mFraction);
  470. result.m_hitPosition = toAnKi(hitPosJph);
  471. const U64 userData = m_jphPhysicsSystem->GetBodyInterfaceNoLock().GetUserData(hit.mBodyID);
  472. result.m_object = numberToPtr<PhysicsObjectBase*>(userData);
  473. JPH::BodyLockRead lock(m_jphPhysicsSystem->GetBodyLockInterfaceNoLock(), hit.mBodyID);
  474. if(lock.Succeeded())
  475. {
  476. result.m_normal = toAnKi(lock.GetBody().GetWorldSpaceSurfaceNormal(hit.mSubShapeID2, hitPosJph));
  477. }
  478. return result;
  479. }
  480. Bool PhysicsWorld::castRayClosestHit(const Vec3& rayStart, const Vec3& rayEnd, PhysicsLayerBit layers, RayHitResult& result)
  481. {
  482. MaskBroadPhaseLayerFilter broadphaseFilter;
  483. broadphaseFilter.m_layerMask = layers;
  484. MaskObjectLayerFilter objectFilter;
  485. objectFilter.m_layerMask = layers;
  486. JPH::RRayCast ray;
  487. ray.mOrigin = toJPH(rayStart);
  488. ray.mDirection = toJPH(rayEnd - rayStart); // Not exactly a direction if it's not normalized but anyway
  489. JPH::RayCastResult hit;
  490. const Bool success = m_jphPhysicsSystem->GetNarrowPhaseQueryNoLock().CastRay(ray, hit, broadphaseFilter, objectFilter);
  491. if(success)
  492. {
  493. result = jphToAnKi(ray, hit);
  494. }
  495. else
  496. {
  497. result = {};
  498. }
  499. return success;
  500. }
  501. Bool PhysicsWorld::castRayAllHitsInternal(const Vec3& rayStart, const Vec3& rayEnd, PhysicsLayerBit layers,
  502. PhysicsDynamicArray<RayHitResult>& results)
  503. {
  504. MaskBroadPhaseLayerFilter broadphaseFilter;
  505. broadphaseFilter.m_layerMask = layers;
  506. MaskObjectLayerFilter objectFilter;
  507. objectFilter.m_layerMask = layers;
  508. JPH::RRayCast ray;
  509. ray.mOrigin = toJPH(rayStart);
  510. ray.mDirection = toJPH(rayEnd - rayStart); // Not exactly a direction if it's not normalized but anyway
  511. JPH::RayCastResult hit;
  512. JPH::RayCastSettings settings;
  513. class MyCastRayCollector final : public JPH::CastRayCollector
  514. {
  515. public:
  516. PhysicsDynamicArray<RayHitResult>* m_resArray;
  517. JPH::RRayCast m_ray;
  518. PhysicsWorld* m_world;
  519. void AddHit(const JPH::RayCastResult& hit) override
  520. {
  521. const RayHitResult result = m_world->jphToAnKi(m_ray, hit);
  522. m_resArray->emplaceBack(result);
  523. }
  524. } collector;
  525. collector.m_resArray = &results;
  526. collector.m_ray = ray;
  527. collector.m_world = this;
  528. m_jphPhysicsSystem->GetNarrowPhaseQueryNoLock().CastRay(ray, settings, collector, broadphaseFilter, objectFilter);
  529. return results.getSize() > 0;
  530. }
  531. void PhysicsWorld::debugDraw(PhysicsDebugDrawerInterface& interface)
  532. {
  533. MyDebugRenderer renderer;
  534. renderer.m_interface = &interface;
  535. JPH::BodyManager::DrawSettings settings;
  536. m_jphPhysicsSystem->DrawBodies(settings, &renderer);
  537. m_jphPhysicsSystem->DrawConstraints(&renderer);
  538. }
  539. } // namespace v2
  540. } // end namespace anki