diff --git a/examples/PhysicsPlayground/main.cpp b/examples/PhysicsPlayground/main.cpp index 90451d989..869a9471b 100644 --- a/examples/PhysicsPlayground/main.cpp +++ b/examples/PhysicsPlayground/main.cpp @@ -31,7 +31,7 @@ int main() #endif auto& windowing = app.AddComponent(); - Nz::Window& mainWindow = windowing.CreateWindow(Nz::VideoMode(1920, 1080), "Physics playground"); + Nz::Window& mainWindow = windowing.CreateWindow(Nz::VideoMode(1280, 720), "Physics playground"); auto& fs = app.AddComponent(); { @@ -86,7 +86,8 @@ int main() boxEntity.emplace(); #if USE_JOLT - std::shared_ptr wallCollider = std::make_shared(Nz::Vector3f(BoxDims, BoxDims, 1.f)); + float thickness = 1.f; + std::shared_ptr wallCollider = std::make_shared(Nz::Vector3f(BoxDims + thickness * 2.f, BoxDims + thickness * 2.f, thickness)); #else std::shared_ptr wallCollider = std::make_shared(Nz::Vector3f(BoxDims, BoxDims, 1.f)); #endif @@ -116,11 +117,13 @@ int main() #endif #if USE_JOLT - auto& ballPhysics = boxEntity.emplace(physSystem.CreateRigidBody(boxCollider)); + Nz::JoltRigidBody3D::StaticSettings settings; + settings.geom = boxCollider; + + auto& boxBody = boxEntity.emplace(physSystem.CreateRigidBody(settings)); #else - auto& ballPhysics = boxEntity.emplace(physSystem.CreateRigidBody(boxCollider)); + auto& boxBody = boxEntity.emplace(physSystem.CreateRigidBody(boxCollider)); #endif - ballPhysics.SetMass(0.f); std::shared_ptr colliderModel; { @@ -132,14 +135,14 @@ int main() return true; }); - /*std::shared_ptr colliderMesh = Nz::Mesh::Build(boxCollider->GenerateDebugMesh()); + std::shared_ptr colliderMesh = Nz::Mesh::Build(boxCollider->GenerateDebugMesh()); std::shared_ptr colliderGraphicalMesh = Nz::GraphicalMesh::BuildFromMesh(*colliderMesh); colliderModel = std::make_shared(colliderGraphicalMesh); for (std::size_t i = 0; i < colliderModel->GetSubMeshCount(); ++i) colliderModel->SetMaterial(i, colliderMat); - boxGfx.AttachRenderable(std::move(colliderModel));*/ + boxGfx.AttachRenderable(std::move(colliderModel)); } } @@ -178,11 +181,14 @@ int main() ballNode.SetScale(radius); #if USE_JOLT - auto& ballPhysics = ballEntity.emplace(physSystem.CreateRigidBody(sphereCollider)); + Nz::JoltRigidBody3D::DynamicSettings settings; + settings.geom = sphereCollider; + settings.mass = 4.f / 3.f * Nz::Pi * Nz::IntegralPow(radius, 3); + + auto& ballPhysics = ballEntity.emplace(physSystem.CreateRigidBody(settings)); #else auto& ballPhysics = ballEntity.emplace(physSystem.CreateRigidBody(sphereCollider)); #endif - ballPhysics.SetMass(4.f / 3.f * Nz::Pi * Nz::IntegralPow(radius, 3)); //ballPhysics.DisableSleeping(); } @@ -222,12 +228,14 @@ int main() #endif #if USE_JOLT - auto& ballPhysics = ballEntity.emplace(physSystem.CreateRigidBody(boxCollider)); + Nz::JoltRigidBody3D::DynamicSettings settings; + settings.geom = boxCollider; + settings.mass = width * height * depth; + + ballEntity.emplace(physSystem.CreateRigidBody(settings)); #else - auto& ballPhysics = ballEntity.emplace(physSystem.CreateRigidBody(boxCollider)); + ballEntity.emplace(physSystem.CreateRigidBody(boxCollider)); #endif - ballPhysics.SetMass(width * height * depth); - //ballPhysics.DisableSleeping(); } // Lumière @@ -273,7 +281,43 @@ int main() NazaraSlot(Nz::WindowEventHandler, OnMouseMoved, cameraMove); NazaraSlot(Nz::WindowEventHandler, OnMouseMoved, grabbedObjectMove); #if USE_JOLT - std::optional grabConstraint; + struct GrabConstraint + { + GrabConstraint(Nz::JoltRigidBody3D& body, const Nz::Vector3f& grabPos) : + grabBody(body.GetWorld(), BodySettings(grabPos)), + grabConstraint(body, grabBody, grabPos) + { + body.WakeUp(); + body.EnableSleeping(false); + grabConstraint.SetDamping(1.f); + grabConstraint.SetFrequency(5.f); + } + + ~GrabConstraint() + { + grabConstraint.GetBodyA().EnableSleeping(true); + } + + void SetPosition(const Nz::Vector3f& pos) + { + grabBody.SetPosition(pos); + } + + static Nz::JoltRigidBody3D::DynamicSettings BodySettings(const Nz::Vector3f& pos) + { + Nz::JoltRigidBody3D::DynamicSettings settings; + settings.mass = 0.f; //< kinematic + settings.isSimulationEnabled = false; + settings.position = pos; + + return settings; + } + + Nz::JoltRigidBody3D grabBody; + Nz::JoltDistanceConstraint3D grabConstraint; + }; + + std::optional grabConstraint; #else std::optional grabConstraint; #endif @@ -328,8 +372,12 @@ int main() { if (lastHitInfo.hitBody && lastHitInfo.hitEntity != boxEntity) { +#if USE_JOLT grabConstraint.emplace(*lastHitInfo.hitBody, lastHitInfo.hitPosition); - //grabConstraint->SetImpulseClamp(30.f); +#else + grabConstraint.emplace(*lastHitInfo.hitBody, lastHitInfo.hitPosition); + grabConstraint->SetImpulseClamp(30.f); +#endif grabbedObjectMove.Connect(eventHandler.OnMouseMoved, [&, body = lastHitInfo.hitBody, distance = Nz::Vector3f::Distance(from, lastHitInfo.hitPosition)](const Nz::WindowEventHandler*, const Nz::WindowEvent::MouseMoveEvent& event) { @@ -337,7 +385,11 @@ int main() Nz::Vector3f to = cameraComponent.Unproject({ float(event.x), float(event.y), 1.f }); Nz::Vector3f newPosition = from + (to - from).Normalize() * distance; +#if USE_JOLT + grabConstraint->SetPosition(newPosition); +#else grabConstraint->SetSecondAnchor(newPosition); +#endif }); } else diff --git a/examples/Showcase/main.cpp b/examples/Showcase/main.cpp index aae753481..1786c413b 100644 --- a/examples/Showcase/main.cpp +++ b/examples/Showcase/main.cpp @@ -348,8 +348,10 @@ int main() auto floorCollider = std::make_shared(Nz::Vector3f(planeSize.x, 1.f, planeSize.y)); auto translatedFloorCollider = std::make_shared(std::move(floorCollider), Nz::Vector3f::Down() * 0.5f); - auto& planeBody = floorEntity.emplace(physSytem.CreateRigidBody(translatedFloorCollider)); - planeBody.SetMass(0.f); + Nz::JoltRigidBody3D::StaticSettings floorSettings; + floorSettings.geom = std::move(translatedFloorCollider); + + auto& planeBody = floorEntity.emplace(physSytem.CreateRigidBody(floorSettings)); std::shared_ptr boxMeshGfx = Nz::GraphicalMesh::Build(Nz::Primitive::Box(Nz::Vector3f(0.5f, 0.5f, 0.5f)), meshPrimitiveParams); diff --git a/include/Nazara/JoltPhysics3D/Enums.hpp b/include/Nazara/JoltPhysics3D/Enums.hpp index 8423197c9..884a41e91 100644 --- a/include/Nazara/JoltPhysics3D/Enums.hpp +++ b/include/Nazara/JoltPhysics3D/Enums.hpp @@ -21,6 +21,12 @@ namespace Nz Max = TranslatedRotatedDecoration }; + + enum class JoltMotionQuality + { + Discrete, + LinearCast + }; } #endif // NAZARA_JOLTPHYSICS3D_ENUMS_HPP diff --git a/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp b/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp index 04641bf0c..a52bd70c4 100644 --- a/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp +++ b/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.hpp @@ -20,7 +20,9 @@ namespace JPH { + class BodyID; class PhysicsSystem; + class Shape; } namespace Nz @@ -31,6 +33,7 @@ namespace Nz class NAZARA_JOLTPHYSICS3D_API JoltPhysWorld3D { friend JoltCharacter; + friend JoltRigidBody3D; public: struct RaycastHit; @@ -47,10 +50,13 @@ namespace Nz Time GetStepSize() const; inline bool IsBodyActive(UInt32 bodyIndex) const; + inline bool IsBodyRegistered(UInt32 bodyIndex) const; bool RaycastQuery(const Vector3f& from, const Vector3f& to, const FunctionRef(const RaycastHit& hitInfo)>& callback); bool RaycastQueryFirst(const Vector3f& from, const Vector3f& to, const FunctionRef& callback); + void RefreshBodies(); + void SetGravity(const Vector3f& gravity); void SetMaxStepCount(std::size_t maxStepCount); void SetStepSize(Time stepSize); @@ -77,13 +83,19 @@ namespace Nz struct JoltWorld; + const JPH::Shape* GetNullShape() const; + void OnPreStep(float deltatime); + void RegisterBody(const JPH::BodyID& bodyID, bool activate, bool removeFromDeactivationList); inline void RegisterCharacter(JoltCharacter* character); + + void UnregisterBody(const JPH::BodyID& bodyID, bool destroy, bool removeFromRegisterList); inline void UnregisterCharacter(JoltCharacter* character); std::size_t m_maxStepCount; std::unique_ptr m_activeBodies; + std::unique_ptr m_registeredBodies; std::unique_ptr m_world; std::vector m_characters; Vector3f m_gravity; diff --git a/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.inl b/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.inl index b92fd4950..17a4e6473 100644 --- a/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.inl +++ b/include/Nazara/JoltPhysics3D/JoltPhysWorld3D.inl @@ -14,6 +14,14 @@ namespace Nz return m_activeBodies[blockIndex] & (UInt64(1u) << localIndex); } + inline bool JoltPhysWorld3D::IsBodyRegistered(UInt32 bodyIndex) const + { + UInt32 blockIndex = bodyIndex / 64; + UInt32 localIndex = bodyIndex % 64; + + return m_registeredBodies[blockIndex] & (UInt64(1u) << localIndex); + } + inline void JoltPhysWorld3D::RegisterCharacter(JoltCharacter* character) { auto it = std::lower_bound(m_characters.begin(), m_characters.end(), character); diff --git a/include/Nazara/JoltPhysics3D/JoltRigidBody3D.hpp b/include/Nazara/JoltPhysics3D/JoltRigidBody3D.hpp index 88c20f42d..08e249192 100644 --- a/include/Nazara/JoltPhysics3D/JoltRigidBody3D.hpp +++ b/include/Nazara/JoltPhysics3D/JoltRigidBody3D.hpp @@ -19,6 +19,7 @@ namespace JPH { class Body; + class BodyCreationSettings; } namespace Nz @@ -28,8 +29,11 @@ namespace Nz class NAZARA_JOLTPHYSICS3D_API JoltRigidBody3D { public: - JoltRigidBody3D(JoltPhysWorld3D* world, const Matrix4f& mat = Matrix4f::Identity()); - JoltRigidBody3D(JoltPhysWorld3D* world, std::shared_ptr geom, const Matrix4f& mat = Matrix4f::Identity()); + struct DynamicSettings; + struct StaticSettings; + + JoltRigidBody3D(JoltPhysWorld3D& world, const DynamicSettings& settings); + JoltRigidBody3D(JoltPhysWorld3D& world, const StaticSettings& settings); JoltRigidBody3D(const JoltRigidBody3D& object) = delete; JoltRigidBody3D(JoltRigidBody3D&& object) noexcept; ~JoltRigidBody3D(); @@ -38,7 +42,9 @@ namespace Nz void AddForce(const Vector3f& force, const Vector3f& point, CoordSys coordSys = CoordSys::Global); void AddTorque(const Vector3f& torque, CoordSys coordSys = CoordSys::Global); + inline void DisableSimulation(); inline void DisableSleeping(); + void EnableSimulation(bool enable); void EnableSleeping(bool enable); void FallAsleep(); @@ -53,16 +59,16 @@ namespace Nz float GetLinearDamping() const; Vector3f GetLinearVelocity() const; float GetMass() const; - Vector3f GetMassCenter(CoordSys coordSys = CoordSys::Local) const; Matrix4f GetMatrix() const; Vector3f GetPosition() const; std::pair GetPositionAndRotation() const; Quaternionf GetRotation() const; - inline JoltPhysWorld3D* GetWorld() const; + inline JoltPhysWorld3D& GetWorld() const; - bool IsSimulationEnabled() const; + inline bool IsSimulationEnabled() const; bool IsSleeping() const; bool IsSleepingEnabled() const; + bool IsStatic() const; void SetAngularDamping(float angularDamping); void SetAngularVelocity(const Vector3f& angularVelocity); @@ -70,7 +76,6 @@ namespace Nz void SetLinearDamping(float damping); void SetLinearVelocity(const Vector3f& velocity); void SetMass(float mass, bool recomputeInertia = true); - void SetMassCenter(const Vector3f& center); void SetPosition(const Vector3f& position); void SetRotation(const Quaternionf& rotation); @@ -86,14 +91,53 @@ namespace Nz JoltRigidBody3D& operator=(const JoltRigidBody3D& object) = delete; JoltRigidBody3D& operator=(JoltRigidBody3D&& object) noexcept; + struct CommonSettings + { + std::shared_ptr geom; + Quaternionf rotation = Quaternionf::Identity(); + Vector3f position = Vector3f::Zero(); + bool initiallySleeping = false; + bool isSimulationEnabled = true; + bool isTrigger = false; + }; + + struct DynamicSettings : CommonSettings + { + // Default values from Jolt + JoltMotionQuality motionQuality = JoltMotionQuality::Discrete; + Vector3f angularVelocity = Vector3f::Zero(); + Vector3f linearVelocity = Vector3f::Zero(); + bool allowSleeping = true; + float angularDamping = 0.05f; + float friction = 0.2f; + float gravityFactor = 1.f; + float linearDamping = 0.05f; + float mass = 1.f; + float maxAngularVelocity = 0.25f * Pi * 60.f; + float maxLinearVelocity = 500.f; + float restitution = 0.f; + }; + + struct StaticSettings : CommonSettings + { + }; + protected: - void Destroy(); + void Destroy(bool worldDestruction = false); private: + void BuildSettings(const CommonSettings& settings, JPH::BodyCreationSettings& creationSettings); + void BuildSettings(const DynamicSettings& settings, JPH::BodyCreationSettings& creationSettings); + void BuildSettings(const StaticSettings& settings, JPH::BodyCreationSettings& creationSettings); + + bool ShouldActivate() const; + std::shared_ptr m_geom; JPH::Body* m_body; JoltPhysWorld3D* m_world; UInt32 m_bodyIndex; + bool m_isSimulationEnabled; + bool m_isTrigger; }; } diff --git a/include/Nazara/JoltPhysics3D/JoltRigidBody3D.inl b/include/Nazara/JoltPhysics3D/JoltRigidBody3D.inl index d53a176c4..b6067690b 100644 --- a/include/Nazara/JoltPhysics3D/JoltRigidBody3D.inl +++ b/include/Nazara/JoltPhysics3D/JoltRigidBody3D.inl @@ -6,6 +6,11 @@ namespace Nz { + inline void JoltRigidBody3D::DisableSimulation() + { + return EnableSimulation(false); + } + inline void JoltRigidBody3D::DisableSleeping() { return EnableSleeping(false); @@ -31,9 +36,14 @@ namespace Nz return m_geom; } - inline JoltPhysWorld3D* JoltRigidBody3D::GetWorld() const + inline JoltPhysWorld3D& JoltRigidBody3D::GetWorld() const { - return m_world; + return *m_world; + } + + inline bool JoltRigidBody3D::IsSimulationEnabled() const + { + return m_isSimulationEnabled; } } diff --git a/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.inl b/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.inl index 9990c308e..f9df021ec 100644 --- a/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.inl +++ b/include/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.inl @@ -9,7 +9,7 @@ namespace Nz template JoltRigidBody3DComponent JoltPhysics3DSystem::CreateRigidBody(Args&&... args) { - return JoltRigidBody3DComponent(&m_physWorld, std::forward(args)...); + return JoltRigidBody3DComponent(m_physWorld, std::forward(args)...); } inline JoltPhysWorld3D& JoltPhysics3DSystem::GetPhysWorld() diff --git a/src/Nazara/JoltPhysics3D/JoltCharacter.cpp b/src/Nazara/JoltPhysics3D/JoltCharacter.cpp index df28556cd..e8595b457 100644 --- a/src/Nazara/JoltPhysics3D/JoltCharacter.cpp +++ b/src/Nazara/JoltPhysics3D/JoltCharacter.cpp @@ -24,6 +24,7 @@ namespace Nz JPH::CharacterSettings settings; settings.mShape = shapeResult.Get(); + settings.mLayer = 1; m_character = std::make_unique(&settings, ToJolt(position), ToJolt(rotation), 0, m_physicsWorld.GetPhysicsSystem()); m_character->AddToPhysicsSystem(); @@ -108,7 +109,7 @@ namespace Nz m_character->Activate(false); } - void JoltCharacter::PreSimulate(float elapsedTime) + void JoltCharacter::PreSimulate(float /*elapsedTime*/) { } diff --git a/src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp b/src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp index 0b5e4cc3e..f12fc77db 100644 --- a/src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp +++ b/src/Nazara/JoltPhysics3D/JoltConstraint3D.cpp @@ -52,12 +52,12 @@ namespace Nz JoltPhysWorld3D& JoltConstraint3D::GetWorld() { - return *GetBodyA().GetWorld(); + return GetBodyA().GetWorld(); } const JoltPhysWorld3D& JoltConstraint3D::GetWorld() const { - return *GetBodyA().GetWorld(); + return GetBodyA().GetWorld(); } bool JoltConstraint3D::IsSingleBody() const diff --git a/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp b/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp index d82536e0b..a3e720f27 100644 --- a/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp +++ b/src/Nazara/JoltPhysics3D/JoltPhysWorld3D.cpp @@ -15,20 +15,19 @@ #include #include #include +#include #include #include #include #include #include +#include #include -#include #include namespace DitchMeAsap { using namespace JPH; - using std::cout; - using std::endl; // Layer that objects can be in, determines which other objects it can collide with // Typically you at least want to have 1 layer for moving bodies and 1 layer for static bodies, but you can have more @@ -130,7 +129,7 @@ namespace DitchMeAsap }; // An example contact listener - class MyContactListener : public ContactListener + /*class MyContactListener : public ContactListener { public: // See: ContactListener @@ -156,7 +155,7 @@ namespace DitchMeAsap { cout << "A contact was removed" << endl; } - }; + };*/ } namespace Nz @@ -227,7 +226,7 @@ namespace Nz { } - void OnBodyActivated(const JPH::BodyID& inBodyID, UInt64 inBodyUserData) override + void OnBodyActivated(const JPH::BodyID& inBodyID, UInt64 /*inBodyUserData*/) override { UInt32 bodyIndex = inBodyID.GetIndex(); UInt32 blockIndex = bodyIndex / 64; @@ -236,7 +235,7 @@ namespace Nz m_physWorld.m_activeBodies[blockIndex] |= UInt64(1u) << localIndex; } - void OnBodyDeactivated(const JPH::BodyID& inBodyID, UInt64 inBodyUserData) override + void OnBodyDeactivated(const JPH::BodyID& inBodyID, UInt64 /*inBodyUserData*/) override { UInt32 bodyIndex = inBodyID.GetIndex(); UInt32 blockIndex = bodyIndex / 64; @@ -268,8 +267,15 @@ namespace Nz struct JoltPhysWorld3D::JoltWorld { + using BodySet = tsl::ordered_set, std::equal_to, std::allocator, std::vector>; + JPH::TempAllocatorImpl tempAllocator; JPH::PhysicsSystem physicsSystem; + BodySet pendingAdditionActivate; + BodySet pendingAdditionNoActivate; + BodySet pendingDeactivations; + std::vector tempBodyIDVec; + std::unique_ptr nullShape; JoltPhysWorld3D::BodyActivationListener bodyActivationListener; JoltPhysWorld3D::StepListener stepListener; @@ -305,9 +311,14 @@ namespace Nz m_world->physicsSystem.AddStepListener(&m_world->stepListener); std::size_t blockCount = (m_world->physicsSystem.GetMaxBodies() - 1) / 64 + 1; + m_activeBodies = std::make_unique(blockCount); for (std::size_t i = 0; i < blockCount; ++i) m_activeBodies[i] = 0; + + m_registeredBodies = std::make_unique(blockCount); + for (std::size_t i = 0; i < blockCount; ++i) + m_registeredBodies[i] = 0; } JoltPhysWorld3D::~JoltPhysWorld3D() = default; @@ -377,9 +388,57 @@ namespace Nz hitInfo.hitBody = reinterpret_cast(static_cast(body.GetUserData())); hitInfo.hitNormal = FromJolt(body.GetWorldSpaceSurfaceNormal(collector.mHit.mSubShapeID2, rayCast.GetPointOnRay(collector.mHit.GetEarlyOutFraction()))); + callback(hitInfo); return true; } + void JoltPhysWorld3D::RefreshBodies() + { + // Batch add bodies (keeps the broadphase efficient) + JPH::BodyInterface& bodyInterface = m_world->physicsSystem.GetBodyInterfaceNoLock(); + auto AddBodies = [&](const JoltWorld::BodySet& bodies, JPH::EActivation activation) + { + for (const JPH::BodyID& bodyId : bodies) + { + UInt32 bodyIndex = bodyId.GetIndex(); + UInt32 blockIndex = bodyIndex / 64; + UInt32 localIndex = bodyIndex % 64; + + m_registeredBodies[blockIndex] |= UInt64(1u) << localIndex; + } + + if (bodies.size() == 1) + bodyInterface.AddBody(bodies.front(), activation); + else + { + m_world->tempBodyIDVec.resize(bodies.size()); + std::memcpy(&m_world->tempBodyIDVec[0], bodies.data(), bodies.size() * sizeof(JPH::BodyID)); + + JPH::BodyInterface::AddState addState = bodyInterface.AddBodiesPrepare(m_world->tempBodyIDVec.data(), SafeCast(m_world->tempBodyIDVec.size())); + bodyInterface.AddBodiesFinalize(m_world->tempBodyIDVec.data(), SafeCast(m_world->tempBodyIDVec.size()), addState, activation); + } + }; + + // Handle pending register/unregister bodies + if (!m_world->pendingAdditionActivate.empty()) + { + AddBodies(m_world->pendingAdditionActivate, JPH::EActivation::Activate); + m_world->pendingAdditionActivate.clear(); + } + + if (!m_world->pendingAdditionNoActivate.empty()) + { + AddBodies(m_world->pendingAdditionNoActivate, JPH::EActivation::DontActivate); + m_world->pendingAdditionNoActivate.clear(); + } + + if (!m_world->pendingDeactivations.empty()) + { + bodyInterface.DeactivateBodies(m_world->pendingDeactivations.data(), SafeCast(m_world->pendingDeactivations.size())); + m_world->pendingDeactivations.clear(); + } + } + void JoltPhysWorld3D::SetGravity(const Vector3f& gravity) { m_world->physicsSystem.SetGravity(ToJolt(gravity)); @@ -397,18 +456,12 @@ namespace Nz void JoltPhysWorld3D::Step(Time timestep) { + RefreshBodies(); + JPH::JobSystem& jobSystem = JoltPhysics3D::Instance()->GetThreadPool(); - - m_timestepAccumulator += timestep; - float stepSize = m_stepSize.AsSeconds(); - static bool firstStep = true; - if (firstStep) - { - m_world->physicsSystem.OptimizeBroadPhase(); - firstStep = false; - } + m_timestepAccumulator += timestep; std::size_t stepCount = 0; while (m_timestepAccumulator >= m_stepSize && stepCount < m_maxStepCount) @@ -423,6 +476,63 @@ namespace Nz } } + void JoltPhysWorld3D::RegisterBody(const JPH::BodyID& bodyID, bool activate, bool removeFromDeactivationList) + { + assert(removeFromDeactivationList || !m_world->pendingDeactivations.contains(bodyID)); + + auto& activationSet = (activate) ? m_world->pendingAdditionActivate : m_world->pendingAdditionNoActivate; + activationSet.insert(bodyID); + + if (removeFromDeactivationList) + { + auto& otherActivationSet = (activate) ? m_world->pendingAdditionNoActivate : m_world->pendingAdditionActivate; + otherActivationSet.erase(bodyID); + + m_world->pendingDeactivations.erase(bodyID); + } + } + + void JoltPhysWorld3D::UnregisterBody(const JPH::BodyID& bodyID, bool destroy, bool removeFromActivationList) + { + if (destroy) + { + auto& bodyInterface = m_world->physicsSystem.GetBodyInterface(); + + UInt32 bodyIndex = bodyID.GetIndex(); + if (IsBodyRegistered(bodyIndex)) + { + UInt32 blockIndex = bodyIndex / 64; + UInt32 localIndex = bodyIndex % 64; + + m_registeredBodies[blockIndex] &= ~(UInt64(1u) << localIndex); + + bodyInterface.RemoveBody(bodyID); + } + + bodyInterface.DestroyBody(bodyID); + + } + else + m_world->pendingDeactivations.insert(bodyID); + + if (removeFromActivationList) + { + m_world->pendingAdditionActivate.erase(bodyID); + m_world->pendingAdditionNoActivate.erase(bodyID); + } + } + + const JPH::Shape* JoltPhysWorld3D::GetNullShape() const + { + if (!m_world->nullShape) + { + m_world->nullShape = std::make_unique(std::numeric_limits::epsilon()); + m_world->nullShape->SetEmbedded(); + } + + return m_world->nullShape.get(); + } + void JoltPhysWorld3D::OnPreStep(float deltatime) { for (JoltCharacter* character : m_characters) diff --git a/src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp b/src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp index 3c98e8f92..e35ce7bc6 100644 --- a/src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp +++ b/src/Nazara/JoltPhysics3D/JoltRigidBody3D.cpp @@ -17,46 +17,57 @@ namespace Nz { - JoltRigidBody3D::JoltRigidBody3D(JoltPhysWorld3D* world, const Matrix4f& mat) : - JoltRigidBody3D(world, nullptr, mat) + JoltRigidBody3D::JoltRigidBody3D(JoltPhysWorld3D& world, const DynamicSettings& settings) : + m_geom(settings.geom), + m_world(&world), + m_isSimulationEnabled(false), + m_isTrigger(settings.isTrigger) { - } - - JoltRigidBody3D::JoltRigidBody3D(JoltPhysWorld3D* world, std::shared_ptr geom, const Matrix4f& mat) : - m_geom(std::move(geom)), - m_world(world) - { - NazaraAssert(m_world, "Invalid world"); - - if (!m_geom) - m_geom = std::make_shared(std::numeric_limits::epsilon()); - JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface(); - JPH::Vec3 position = ToJolt(mat.GetTranslation()); - JPH::Quat rotation = ToJolt(mat.GetRotation().Normalize()); - - JPH::BodyCreationSettings settings(m_geom->GetShapeSettings(), position, rotation, JPH::EMotionType::Dynamic, 1); - - m_body = bodyInterface.CreateBody(settings); - m_body->SetUserData(SafeCast(reinterpret_cast(this))); + JPH::BodyCreationSettings creationSettings; + BuildSettings(settings, creationSettings); + m_body = bodyInterface.CreateBody(creationSettings); JPH::BodyID bodyId = m_body->GetID(); - - bodyInterface.AddBody(bodyId, JPH::EActivation::Activate); m_bodyIndex = bodyId.GetIndex(); + + if (settings.isSimulationEnabled) + m_world->RegisterBody(bodyId, !settings.initiallySleeping, false); } - JoltRigidBody3D::JoltRigidBody3D(JoltRigidBody3D&& object) noexcept : - m_geom(std::move(object.m_geom)), - m_body(object.m_body), - m_bodyIndex(object.m_bodyIndex), - m_world(object.m_world) + JoltRigidBody3D::JoltRigidBody3D(JoltPhysWorld3D& world, const StaticSettings& settings) : + m_geom(settings.geom), + m_world(&world), + m_isSimulationEnabled(false), + m_isTrigger(settings.isTrigger) { - m_body->SetUserData(SafeCast(reinterpret_cast(this))); + JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface(); - object.m_body = nullptr; - object.m_bodyIndex = std::numeric_limits::max(); + JPH::BodyCreationSettings creationSettings; + BuildSettings(settings, creationSettings); + + m_body = bodyInterface.CreateBody(creationSettings); + JPH::BodyID bodyId = m_body->GetID(); + m_bodyIndex = bodyId.GetIndex(); + + if (settings.isSimulationEnabled) + m_world->RegisterBody(bodyId, false, false); //< static bodies cannot be activated + } + + JoltRigidBody3D::JoltRigidBody3D(JoltRigidBody3D&& body) noexcept : + m_geom(std::move(body.m_geom)), + m_body(body.m_body), + m_world(body.m_world), + m_bodyIndex(body.m_bodyIndex), + m_isSimulationEnabled(body.m_isSimulationEnabled), + m_isTrigger(body.m_isTrigger) + { + body.m_body = nullptr; + body.m_bodyIndex = std::numeric_limits::max(); + + if (m_body) + m_body->SetUserData(SafeCast(reinterpret_cast(this))); } JoltRigidBody3D::~JoltRigidBody3D() @@ -69,10 +80,8 @@ namespace Nz switch (coordSys) { case CoordSys::Global: - { m_body->AddForce(ToJolt(force)); break; - } case CoordSys::Local: m_body->AddForce(m_body->GetRotation() * ToJolt(force)); @@ -80,21 +89,17 @@ namespace Nz } } -#if 0 void JoltRigidBody3D::AddForce(const Vector3f& force, const Vector3f& point, CoordSys coordSys) { switch (coordSys) { case CoordSys::Global: - WakeUp(); - m_body->applyForce(ToJolt(force), ToJolt(point)); + m_body->AddForce(ToJolt(force), ToJolt(point)); break; case CoordSys::Local: - { - Matrix4f transformMatrix = GetMatrix(); - return AddForce(transformMatrix.Transform(force, 0.f), point, CoordSys::Global); - } + m_body->AddForce(m_body->GetRotation() * ToJolt(force), ToJolt(ToWorld(point))); + break; } } @@ -103,74 +108,83 @@ namespace Nz switch (coordSys) { case CoordSys::Global: - WakeUp(); - m_body->applyTorque(ToJolt(torque)); + m_body->AddTorque(ToJolt(torque)); break; case CoordSys::Local: - Matrix4f transformMatrix = GetMatrix(); - WakeUp(); - m_body->applyTorque(ToJolt(transformMatrix.Transform(torque, 0.f))); + m_body->AddTorque(m_body->GetRotation() * ToJolt(torque)); break; } } -#endif + + void JoltRigidBody3D::EnableSimulation(bool enable) + { + if (m_isSimulationEnabled == enable) + return; + + if (enable) + m_world->RegisterBody(m_body->GetID(), true, true); + else + m_world->UnregisterBody(m_body->GetID(), false, true); + + m_isSimulationEnabled = enable; + } + void JoltRigidBody3D::EnableSleeping(bool enable) { m_body->SetAllowSleeping(enable); } -#if 0 void JoltRigidBody3D::FallAsleep() { - if (m_body->getActivationState() != DISABLE_DEACTIVATION) - m_body->setActivationState(ISLAND_SLEEPING); + JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface(); + bodyInterface.DeactivateBody(m_body->GetID()); } Boxf JoltRigidBody3D::GetAABB() const { - btVector3 min, max; - m_body->getAabb(min, max); - - return Boxf(FromJolt(min), FromJolt(max)); + const JPH::AABox& aabb = m_body->GetWorldSpaceBounds(); + return Boxf(FromJolt(aabb.mMin), FromJolt(aabb.GetSize())); } float JoltRigidBody3D::GetAngularDamping() const { - return m_body->getAngularDamping(); + if NAZARA_UNLIKELY(IsStatic()) + return 0.f; + + return m_body->GetMotionProperties()->GetAngularDamping(); } Vector3f JoltRigidBody3D::GetAngularVelocity() const { - return FromJolt(m_body->getAngularVelocity()); + return FromJolt(m_body->GetAngularVelocity()); } float JoltRigidBody3D::GetLinearDamping() const { - return m_body->getLinearDamping(); + if NAZARA_UNLIKELY(IsStatic()) + return 0.f; + + return m_body->GetMotionProperties()->GetLinearDamping(); } Vector3f JoltRigidBody3D::GetLinearVelocity() const { - return FromJolt(m_body->getLinearVelocity()); + return FromJolt(m_body->GetLinearVelocity()); } -#endif float JoltRigidBody3D::GetMass() const { + if NAZARA_UNLIKELY(IsStatic()) + return 0.f; + return 1.f / m_body->GetMotionProperties()->GetInverseMass(); } -#if 0 - Vector3f JoltRigidBody3D::GetMassCenter(CoordSys coordSys) const - { - return FromJolt(m_body->getCenterOfMassPosition()); - } Matrix4f JoltRigidBody3D::GetMatrix() const { - return FromJolt(m_body->getWorldTransform()); + return FromJolt(m_body->GetCenterOfMassTransform()); } -#endif Vector3f JoltRigidBody3D::GetPosition() const { @@ -190,13 +204,6 @@ namespace Nz return FromJolt(m_body->GetRotation()); } -#if 0 - bool JoltRigidBody3D::IsSimulationEnabled() const - { - return m_body->isActive(); - } -#endif - bool JoltRigidBody3D::IsSleeping() const { return m_body->IsActive(); @@ -207,30 +214,42 @@ namespace Nz return m_body->GetAllowSleeping(); } -#if 0 + bool JoltRigidBody3D::IsStatic() const + { + return m_body->GetMotionType() == JPH::EMotionType::Static; + } + void JoltRigidBody3D::SetAngularDamping(float angularDamping) { - m_body->setDamping(m_body->getLinearDamping(), angularDamping); + if NAZARA_UNLIKELY(IsStatic()) + return; + + m_body->GetMotionProperties()->SetAngularDamping(angularDamping); } void JoltRigidBody3D::SetAngularVelocity(const Vector3f& angularVelocity) { - m_body->setAngularVelocity(ToJolt(angularVelocity)); + m_body->SetAngularVelocity(ToJolt(angularVelocity)); } -#endif + void JoltRigidBody3D::SetGeom(std::shared_ptr geom, bool recomputeInertia) { if (m_geom != geom) { float mass = GetMass(); - if (geom) - m_geom = std::move(geom); + const JPH::Shape* shape; + m_geom = std::move(geom); + if (m_geom) + shape = m_geom->GetShapeSettings()->Create().Get(); else - m_geom = std::make_shared(std::numeric_limits::epsilon()); + { + m_body->SetIsSensor(true); + shape = m_world->GetNullShape(); + } JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface(); - bodyInterface.SetShape(m_body->GetID(), m_geom->GetShapeSettings()->Create().Get(), false, JPH::EActivation::Activate); + bodyInterface.SetShape(m_body->GetID(), m_geom->GetShapeSettings()->Create().Get(), false, (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate); if (recomputeInertia) { JPH::MassProperties massProperties = m_body->GetShape()->GetMassProperties(); @@ -242,6 +261,9 @@ namespace Nz void JoltRigidBody3D::SetLinearDamping(float damping) { + if NAZARA_UNLIKELY(IsStatic()) + return; + m_body->GetMotionProperties()->SetLinearDamping(damping); } @@ -270,34 +292,28 @@ namespace Nz JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock(); bodyInterface.DeactivateBody(m_body->GetID()); - m_body->SetMotionType(JPH::EMotionType::Static); + m_body->SetMotionType(JPH::EMotionType::Kinematic); } } - void JoltRigidBody3D::SetMassCenter(const Vector3f& center) - { - //m_body->GetMotionProperties()->set - } - void JoltRigidBody3D::SetPosition(const Vector3f& position) { JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock(); - bodyInterface.SetPosition(m_body->GetID(), ToJolt(position), JPH::EActivation::Activate); + bodyInterface.SetPosition(m_body->GetID(), ToJolt(position), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate); } void JoltRigidBody3D::SetRotation(const Quaternionf& rotation) { - JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock(); - body_interface.SetRotation(m_body->GetID(), ToJolt(rotation), JPH::EActivation::Activate); + JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock(); + bodyInterface.SetRotation(m_body->GetID(), ToJolt(rotation), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate); } void JoltRigidBody3D::TeleportTo(const Vector3f& position, const Quaternionf& rotation) { JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterfaceNoLock(); - bodyInterface.SetPositionAndRotation(m_body->GetID(), ToJolt(position), ToJolt(rotation), JPH::EActivation::Activate); + bodyInterface.SetPositionAndRotation(m_body->GetID(), ToJolt(position), ToJolt(rotation), (ShouldActivate()) ? JPH::EActivation::Activate : JPH::EActivation::DontActivate); } -#if 0 Quaternionf JoltRigidBody3D::ToLocal(const Quaternionf& worldRotation) { return GetRotation().Conjugate() * worldRotation; @@ -305,8 +321,7 @@ namespace Nz Vector3f JoltRigidBody3D::ToLocal(const Vector3f& worldPosition) { - btTransform worldTransform = m_body->getWorldTransform(); - return GetMatrix().InverseTransform() * worldPosition; + return FromJolt(m_body->GetInverseCenterOfMassTransform() * ToJolt(worldPosition)); } Quaternionf JoltRigidBody3D::ToWorld(const Quaternionf& localRotation) @@ -316,45 +331,100 @@ namespace Nz Vector3f JoltRigidBody3D::ToWorld(const Vector3f& localPosition) { - return GetMatrix() * localPosition; + return FromJolt(m_body->GetCenterOfMassTransform() * ToJolt(localPosition)); } -#endif void JoltRigidBody3D::WakeUp() { - JPH::BodyInterface& body_interface = m_world->GetPhysicsSystem()->GetBodyInterface(); - body_interface.ActivateBody(m_body->GetID()); + JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface(); + bodyInterface.ActivateBody(m_body->GetID()); } - JoltRigidBody3D& JoltRigidBody3D::operator=(JoltRigidBody3D&& object) noexcept + JoltRigidBody3D& JoltRigidBody3D::operator=(JoltRigidBody3D&& body) noexcept { Destroy(); - m_body = object.m_body; - m_bodyIndex = object.m_bodyIndex; - m_geom = std::move(object.m_geom); - m_world = object.m_world; + m_body = body.m_body; + m_bodyIndex = body.m_bodyIndex; + m_geom = std::move(body.m_geom); + m_world = body.m_world; + m_isSimulationEnabled = body.m_isSimulationEnabled; + m_isTrigger = body.m_isTrigger; - m_body->SetUserData(SafeCast(reinterpret_cast(this))); + body.m_body = nullptr; + body.m_bodyIndex = std::numeric_limits::max(); - object.m_body = nullptr; - object.m_bodyIndex = std::numeric_limits::max(); + if (m_body) + m_body->SetUserData(SafeCast(reinterpret_cast(this))); return *this; } - void JoltRigidBody3D::Destroy() + void JoltRigidBody3D::Destroy(bool worldDestruction) { if (m_body) { - JPH::BodyID bodyId = m_body->GetID(); - JPH::BodyInterface& bodyInterface = m_world->GetPhysicsSystem()->GetBodyInterface(); - bodyInterface.RemoveBody(bodyId); - bodyInterface.DestroyBody(bodyId); + m_world->UnregisterBody(m_body->GetID(), true, !worldDestruction); m_body = nullptr; } m_geom.reset(); } + + void JoltRigidBody3D::BuildSettings(const DynamicSettings& settings, JPH::BodyCreationSettings& creationSettings) + { + BuildSettings(static_cast(settings), creationSettings); + + creationSettings.mAngularDamping = settings.angularDamping; + creationSettings.mAngularVelocity = ToJolt(settings.angularVelocity); + creationSettings.mLinearDamping = settings.linearDamping; + creationSettings.mLinearVelocity = ToJolt(settings.linearVelocity); + creationSettings.mFriction = settings.friction; + creationSettings.mGravityFactor = settings.gravityFactor; + creationSettings.mMaxAngularVelocity = settings.maxAngularVelocity; + creationSettings.mMaxLinearVelocity = settings.maxLinearVelocity; + creationSettings.mObjectLayer = 1; + creationSettings.mRestitution = settings.restitution; + creationSettings.mUserData = SafeCast(reinterpret_cast(this)); + + creationSettings.mMassPropertiesOverride = creationSettings.GetShape()->GetMassProperties(); + creationSettings.mMassPropertiesOverride.ScaleToMass(settings.mass); + + creationSettings.mMotionType = (settings.mass > 0.f) ? JPH::EMotionType::Dynamic : JPH::EMotionType::Kinematic; + + switch (settings.motionQuality) + { + case JoltMotionQuality::Discrete: creationSettings.mMotionQuality = JPH::EMotionQuality::Discrete; break; + case JoltMotionQuality::LinearCast: creationSettings.mMotionQuality = JPH::EMotionQuality::LinearCast; break; + } + } + + void JoltRigidBody3D::BuildSettings(const StaticSettings& settings, JPH::BodyCreationSettings& creationSettings) + { + BuildSettings(static_cast(settings), creationSettings); + + creationSettings.mMotionType = JPH::EMotionType::Static; + } + + void JoltRigidBody3D::BuildSettings(const CommonSettings& settings, JPH::BodyCreationSettings& creationSettings) + { + if (settings.geom) + { + creationSettings.SetShapeSettings(settings.geom->GetShapeSettings()); + creationSettings.mIsSensor = settings.isTrigger; + } + else + { + creationSettings.SetShape(m_world->GetNullShape()); + creationSettings.mIsSensor = true; //< not the best solution but enough for now + creationSettings.mPosition = ToJolt(settings.position); + creationSettings.mRotation = ToJolt(settings.rotation); + } + } + + bool JoltRigidBody3D::ShouldActivate() const + { + return m_isSimulationEnabled && m_world->IsBodyRegistered(m_bodyIndex); + } } diff --git a/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp b/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp index 20d3902e4..8ffe2d11a 100644 --- a/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp +++ b/src/Nazara/JoltPhysics3D/Systems/JoltPhysics3DSystem.cpp @@ -28,7 +28,7 @@ namespace Nz // Ensure every RigidBody3D is destroyed before world is auto rigidBodyView = m_registry.view(); for (auto [entity, rigidBodyComponent] : rigidBodyView.each()) - rigidBodyComponent.Destroy(); + rigidBodyComponent.Destroy(true); } void JoltPhysics3DSystem::Dump() diff --git a/xmake.lua b/xmake.lua index bbc803332..0563ca0bb 100644 --- a/xmake.lua +++ b/xmake.lua @@ -111,7 +111,7 @@ local modules = { }, JoltPhysics3D = { Deps = {"NazaraUtility"}, - Packages = { "joltphysics", "entt" } + Packages = { "joltphysics", "entt", "ordered_map" } }, Network = { Deps = {"NazaraCore"},