From 187308be902885d3756140d893d7ccfba0cc113b Mon Sep 17 00:00:00 2001 From: maelstrom Date: Wed, 27 Aug 2025 22:55:35 +0200 Subject: [PATCH] feat(physics): jolt physics implementation --- client/src/main.cpp | 3 + core/CMakeLists.txt | 5 +- core/deps.cmake | 4 +- core/src/physics/convert.h | 45 ++-- core/src/physics/world.cpp | 427 +++++++++++++++++++------------------ core/src/physics/world.h | 61 +++--- editor/main.cpp | 4 +- editor/mainwindow.cpp | 1 + 8 files changed, 277 insertions(+), 273 deletions(-) diff --git a/client/src/main.cpp b/client/src/main.cpp index 418f8fb..368e2f9 100644 --- a/client/src/main.cpp +++ b/client/src/main.cpp @@ -3,6 +3,7 @@ #include "logger.h" #include "objects/part/part.h" #include "panic.h" +#include "physics/world.h" #include "rendering/renderer.h" #include "common.h" #include "version.h" @@ -47,6 +48,7 @@ int main() { Logger::debugf("Initialized GL context version %d.%d", GLAD_VERSION_MAJOR(version), GLAD_VERSION_MINOR(version)); } + physicsInit(); gDataModel->Init(); renderInit(1200, 900); @@ -86,6 +88,7 @@ int main() { glfwPollEvents(); } while(!glfwWindowShouldClose(window)); + physicsDeinit(); glfwTerminate(); return 0; } diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 7372394..b67fd6b 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -57,7 +57,6 @@ set(SOURCES src/rendering/font.h src/rendering/defaultmeshes.h src/rendering/texture3d.cpp - src/physics/util.h src/physics/world.h src/physics/world.cpp src/logger.cpp @@ -199,8 +198,8 @@ list(APPEND SOURCES ${CMAKE_CURRENT_BINARY_DIR}/src/version.cpp) add_library(openblocks STATIC ${SOURCES}) set_target_properties(openblocks PROPERTIES OUTPUT_NAME "openblocks") target_link_directories(openblocks PUBLIC ${LUAJIT_LIBRARY_DIRS}) -target_link_libraries(openblocks reactphysics3d pugixml::pugixml Freetype::Freetype glm::glm libluajit ${LuaJIT_LIBRARIES}) -target_include_directories(openblocks PUBLIC "src" "../include" "${CMAKE_SOURCE_DIR}/external/glad" ${ReactPhysics3D_SOURCE_DIR}/include ${LUAJIT_INCLUDE_DIRS} ${stb_SOURCE_DIR}) +target_link_libraries(openblocks Jolt pugixml::pugixml Freetype::Freetype glm::glm libluajit ${LuaJIT_LIBRARIES}) +target_include_directories(openblocks PUBLIC "src" "../include" "${CMAKE_SOURCE_DIR}/external/glad" ${LUAJIT_INCLUDE_DIRS} ${stb_SOURCE_DIR}) add_dependencies(openblocks autogen_build autogen) # Windows-specific dependencies diff --git a/core/deps.cmake b/core/deps.cmake index 46d0823..8002334 100644 --- a/core/deps.cmake +++ b/core/deps.cmake @@ -6,9 +6,7 @@ set (PREV_BIN_PATH ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}) unset (CMAKE_RUNTIME_OUTPUT_DIRECTORY) CPMAddPackage("gh:g-truc/glm#1.0.1") -CPMAddPackage(NAME reactphysics3d GITHUB_REPOSITORY "DanielChappuis/reactphysics3d" VERSION 0.10.2 PATCHES ${CMAKE_SOURCE_DIR}/patches/std_chrono.patch) -# https://github.com/StereoKit/StereoKit/blob/0be056efebcee5e58ad1438f4cf6dfdb942f6cf9/CMakeLists.txt#L205 -set_property(TARGET reactphysics3d PROPERTY POSITION_INDEPENDENT_CODE ON) +CPMAddPackage(NAME Jolt GIT_REPOSITORY "https://github.com/jrouwe/JoltPhysics" VERSION 5.3.0 SOURCE_SUBDIR "Build") CPMAddPackage("gh:zeux/pugixml@1.15") CPMAddPackage( diff --git a/core/src/physics/convert.h b/core/src/physics/convert.h index a5d471c..eb12db7 100644 --- a/core/src/physics/convert.h +++ b/core/src/physics/convert.h @@ -2,12 +2,9 @@ #include "datatypes/cframe.h" #include "datatypes/vector.h" #include -#include -#include -#include #include -#define rp reactphysics3d +#include template T convert(F vec) = delete; @@ -15,47 +12,33 @@ T convert(F vec) = delete; // Vector3 template <> -inline Vector3 convert(rp::Vector3 vec) { - return Vector3(vec.x, vec.y, vec.z); +inline Vector3 convert(JPH::Vec3 vec) { + return Vector3(vec.GetX(), vec.GetY(), vec.GetZ()); } template <> -inline rp::Vector3 convert(Vector3 vec) { - return rp::Vector3(vec.X(), vec.Y(), vec.Z()); +inline JPH::Vec3 convert(Vector3 vec) { + return JPH::Vec3(vec.X(), vec.Y(), vec.Z()); } template <> -inline glm::vec3 convert(rp::Vector3 vec) { - return glm::vec3(vec.x, vec.y, vec.z); +inline glm::vec3 convert(JPH::Vec3 vec) { + return glm::vec3(vec.GetX(), vec.GetY(), vec.GetZ()); } template <> -inline rp::Vector3 convert(glm::vec3 vec) { - return rp::Vector3(vec.x, vec.y, vec.z); +inline JPH::Vec3 convert(glm::vec3 vec) { + return JPH::Vec3(vec.x, vec.y, vec.z); } // Quaternion template <> -inline glm::quat convert(rp::Quaternion quat) { - return glm::quat(quat.w, quat.x, quat.y, quat.z); +inline glm::quat convert(JPH::Quat quat) { + return glm::quat(quat.GetW(), quat.GetX(), quat.GetY(), quat.GetZ()); } template <> -inline rp::Quaternion convert(glm::quat quat) { - return rp::Quaternion(quat.w, rp::Vector3(quat.x, quat.y, quat.z)); -} - -// CFrame - -template <> -inline rp::Transform convert(CFrame frame) { - return rp::Transform(convert(frame.Position()), convert((glm::quat)frame.RotMatrix())); -} - -template <> -inline CFrame convert(rp::Transform trans) { - return CFrame(convert(trans.getPosition()), convert(trans.getOrientation())); -} - -#undef rp \ No newline at end of file +inline JPH::Quat convert(glm::quat quat) { + return JPH::Quat(quat.x, quat.y, quat.z, quat.w); +} \ No newline at end of file diff --git a/core/src/physics/world.cpp b/core/src/physics/world.cpp index 85ea759..8d949f5 100644 --- a/core/src/physics/world.cpp +++ b/core/src/physics/world.cpp @@ -1,265 +1,276 @@ #include "world.h" -#include "datatypes/variant.h" #include "datatypes/vector.h" -#include "objects/joint/jointinstance.h" +#include "enum/part.h" +#include "logger.h" #include "objects/part/basepart.h" #include "objects/part/part.h" -#include "physics/convert.h" -#include -#include -#include -#include "timeutil.h" -#include #include "objects/service/workspace.h" +#include "physics/convert.h" +#include "timeutil.h" -rp::PhysicsCommon physicsCommon; +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -PhysWorld::PhysWorld() : physicsEventListener(this) { - worldImpl = physicsCommon.createPhysicsWorld(); +static JPH::TempAllocator* allocator; +static JPH::JobSystem* jobSystem; - worldImpl->setGravity(rp::Vector3(0, -196.2, 0)); - // world->setContactsPositionCorrectionTechnique(rp::ContactsPositionCorrectionTechnique::BAUMGARTE_CONTACTS); - // physicsWorld->setNbIterationsPositionSolver(2000); - // physicsWorld->setNbIterationsVelocitySolver(2000); - // physicsWorld->setSleepLinearVelocity(10); - // physicsWorld->setSleepAngularVelocity(5); - worldImpl->setEventListener(&physicsEventListener); +namespace Layers +{ + static constexpr JPH::ObjectLayer DYNAMIC = 0; + static constexpr JPH::ObjectLayer ANCHORED = 1; + static constexpr JPH::ObjectLayer NUM_LAYERS = 2; +}; +namespace BPLayers +{ + static constexpr JPH::BroadPhaseLayer ANCHORED(0); + static constexpr JPH::BroadPhaseLayer DYNAMIC(1); + static constexpr uint NUM_LAYERS(2); +}; + +PhysWorld::PhysWorld() { + worldImpl.Init(4096, 0, 4096, 4096, broadPhaseLayerInterface, objectBroadPhasefilter, objectLayerPairFilter); + worldImpl.SetGravity(JPH::Vec3(0, -196, 0)); + JPH::PhysicsSettings settings = worldImpl.GetPhysicsSettings(); + // settings.mPointVelocitySleepThreshold = 0.04f; // Fix parts not sleeping + // settings.mNumVelocitySteps *= 20; + // settings.mNumPositionSteps *= 20; + worldImpl.SetPhysicsSettings(settings); } PhysWorld::~PhysWorld() { - physicsCommon.destroyPhysicsWorld(worldImpl); } -PhysicsEventListener::PhysicsEventListener(PhysWorld* world) : world(world) {} +void PhysWorld::addBody(std::shared_ptr part) { + syncBodyProperties(part); +} -void PhysicsEventListener::onContact(const rp::CollisionCallback::CallbackData& data) { - for (size_t i = 0; i < data.getNbContactPairs(); i++) { - auto pair = data.getContactPair(i); - auto type = pair.getEventType(); - if (type == rp::CollisionCallback::ContactPair::EventType::ContactStay) continue; +void PhysWorld::removeBody(std::shared_ptr part) { + // TODO: +} - auto part0 = reinterpret_cast(pair.getBody1()->getUserData())->shared(); - auto part1 = reinterpret_cast(pair.getBody2()->getUserData())->shared(); - - if (type == reactphysics3d::CollisionCallback::ContactPair::EventType::ContactStart) { - part0->Touched->Fire({ (Variant)InstanceRef(part1) }); - part1->Touched->Fire({ (Variant)InstanceRef(part0) }); - } else if (type == reactphysics3d::CollisionCallback::ContactPair::EventType::ContactExit) { - part0->TouchEnded->Fire({ (Variant)InstanceRef(part1) }); - part1->TouchEnded->Fire({ (Variant)InstanceRef(part0) }); +JPH::Shape* makeShape(std::shared_ptr basePart) { + if (std::shared_ptr part = std::dynamic_pointer_cast(basePart)) { + switch (part->shape) { + case PartType::Block: + return new JPH::BoxShape(convert(part->size / 2.f), JPH::cDefaultConvexRadius); + case PartType::Ball: + return new JPH::SphereShape(glm::min(part->size.X(), part->size.Y(), part->size.Z()) / 2.f); + break; } } + return nullptr; +} + +void PhysWorld::syncBodyProperties(std::shared_ptr part) { + JPH::BodyInterface& interface = worldImpl.GetBodyInterface(); + + JPH::EMotionType motionType = part->anchored ? JPH::EMotionType::Static : JPH::EMotionType::Dynamic; + JPH::EActivation activationMode = part->anchored ? JPH::EActivation::DontActivate : JPH::EActivation::Activate; + JPH::ObjectLayer objectLayer = part->anchored ? Layers::ANCHORED : Layers::DYNAMIC; + + JPH::Body* body = part->rigidBody.bodyImpl; + + // Generate a new rigidBody + if (body == nullptr) { + JPH::Shape* shape = makeShape(part); + JPH::BodyCreationSettings settings(shape, convert(part->position()), convert((glm::quat)part->cframe.RotMatrix()), motionType, objectLayer); + settings.mRestitution = 0.5; + + body = interface.CreateBody(settings); + body->SetUserData((JPH::uint64)part.get()); + part->rigidBody.bodyImpl = body; + + interface.AddBody(body->GetID(), activationMode); + interface.SetLinearVelocity(body->GetID(), convert(part->velocity)); + } else { + std::shared_ptr part2 = std::dynamic_pointer_cast(part); + bool shouldUpdateShape = (part2 != nullptr && part->rigidBody._lastShape != part2->shape) || part->rigidBody._lastSize == part->size; + + if (shouldUpdateShape) { + // const JPH::Shape* oldShape = body->GetShape(); + JPH::Shape* newShape = makeShape(part); + + interface.SetShape(body->GetID(), newShape, true, activationMode); + // Seems like Jolt manages its memory for us, so we don't need the below + // delete oldShape; + } + + interface.SetObjectLayer(body->GetID(), objectLayer); + interface.SetMotionType(body->GetID(), motionType, activationMode); + interface.SetPositionRotationAndVelocity(body->GetID(), convert(part->position()), convert((glm::quat)part->cframe.RotMatrix()), convert(part->velocity), /* Angular velocity is NYI: */ body->GetAngularVelocity()); + } } -void PhysicsEventListener::onTrigger(const rp::OverlapCallback::CallbackData& data) { - for (size_t i = 0; i < data.getNbOverlappingPairs(); i++) { - auto pair = data.getOverlappingPair(i); - auto type = pair.getEventType(); - if (type == rp::OverlapCallback::OverlapPair::EventType::OverlapStay) continue; +void physicsInit() { + JPH::RegisterDefaultAllocator(); + JPH::Factory::sInstance = new JPH::Factory(); + JPH::RegisterTypes(); - auto part0 = reinterpret_cast(pair.getBody1()->getUserData())->shared(); - auto part1 = reinterpret_cast(pair.getBody2()->getUserData())->shared(); + allocator = new JPH::TempAllocatorImpl(10 * 1024 * 1024); + jobSystem = new JPH::JobSystemThreadPool(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, std::thread::hardware_concurrency() - 1); +} - if (type == reactphysics3d::OverlapCallback::OverlapPair::EventType::OverlapStart) { - part0->Touched->Fire({ (Variant)InstanceRef(part1) }); - part1->Touched->Fire({ (Variant)InstanceRef(part0) }); - } else if (type == reactphysics3d::OverlapCallback::OverlapPair::EventType::OverlapExit) { - part0->TouchEnded->Fire({ (Variant)InstanceRef(part1) }); - part1->TouchEnded->Fire({ (Variant)InstanceRef(part0) }); - } - } +void physicsDeinit() { + JPH::UnregisterTypes(); + delete JPH::Factory::sInstance; + JPH::Factory::sInstance = nullptr; } tu_time_t physTime; void PhysWorld::step(float deltaTime) { tu_time_t startTime = tu_clock_micros(); + // Depending on the load, it may be necessary to call this with a differing collision step count + // 5 seems to be a good number supporting the high gravity + worldImpl.Update(deltaTime, 5, allocator, jobSystem); - worldImpl->update(std::min(deltaTime / 2, (1/60.f))); - - // TODO: Add list of tracked parts in workspace based on their ancestry using inWorkspace property of Instance - for (std::shared_ptr part : simulatedBodies) { - if (!part->rigidBody.rigidBody) continue; - - // Sync properties - const rp::Transform& transform = part->rigidBody.rigidBody->getTransform(); - part->cframe = convert(transform); - part->velocity = convert(part->rigidBody.rigidBody->getLinearVelocity()); - - // part->rigidBody->enableGravity(true); - // RotateV/Motor joint - for (auto& joint : part->secondaryJoints) { - if (joint.expired() || !joint.lock()->IsA("RotateV")) continue; - - std::shared_ptr motor = joint.lock()->CastTo().expect(); - float rate = motor->part0.lock()->GetSurfaceParamB(-motor->c0.LookVector().Unit()) * 30; - // part->rigidBody->enableGravity(false); - part->rigidBody.rigidBody->setAngularVelocity(convert(-(motor->part0.lock()->cframe * motor->c0).LookVector() * rate)); - } + JPH::BodyInterface& interface = worldImpl.GetBodyInterface(); + JPH::BodyIDVector bodyIDs; + worldImpl.GetBodies(bodyIDs); + for (JPH::BodyID bodyID : bodyIDs) { + std::shared_ptr part = ((Instance*)interface.GetUserData(bodyID))->shared(); + part->cframe = CFrame(convert(interface.GetPosition(bodyID)), convert(interface.GetRotation(bodyID))); } physTime = tu_clock_micros() - startTime; } - -void PhysWorld::addBody(std::shared_ptr part) { - simulatedBodies.push_back(part); -} - -void PhysWorld::removeBody(std::shared_ptr part) { - auto it = std::find(simulatedBodies.begin(), simulatedBodies.end(), part); - simulatedBodies.erase(it); -} - -void PhysWorld::syncBodyProperties(std::shared_ptr part) { - rp::Transform transform = convert(part->cframe); - if (!part->rigidBody.rigidBody) { - part->rigidBody.rigidBody = worldImpl->createRigidBody(transform); - } else { - part->rigidBody.rigidBody->setTransform(transform); - } - - part->rigidBody.updateCollider(part); - - part->rigidBody.rigidBody->setType(part->anchored ? rp::BodyType::STATIC : rp::BodyType::DYNAMIC); - part->rigidBody.rigidBody->getCollider(0)->setCollisionCategoryBits(0b11); - - part->rigidBody.rigidBody->getCollider(0)->setIsSimulationCollider(part->canCollide); - part->rigidBody.rigidBody->getCollider(0)->setIsTrigger(!part->canCollide); - - rp::Material& material = part->rigidBody.rigidBody->getCollider(0)->getMaterial(); - material.setFrictionCoefficient(0.35); - material.setMassDensity(1.f); - - //https://github.com/DanielChappuis/reactphysics3d/issues/170#issuecomment-691514860 - part->rigidBody.rigidBody->updateMassFromColliders(); - part->rigidBody.rigidBody->updateLocalInertiaTensorFromColliders(); - - part->rigidBody.rigidBody->setLinearVelocity(convert(part->velocity)); - // part->rigidBody->setMass(density * part->size.x * part->size.y * part->size.z); - - part->rigidBody.rigidBody->setUserData(&*part); -} - -// Ray-casting - - -RaycastResult::RaycastResult(const rp::RaycastInfo& raycastInfo) - : worldPoint(convert(raycastInfo.worldPoint)) - , worldNormal(convert(raycastInfo.worldNormal)) - // , hitFraction(raycastInfo.hitFraction) - , triangleIndex(raycastInfo.triangleIndex) - , body(dynamic_cast(raycastInfo.body)) - // , collider(raycastInfo.collider) -{ - if (body.rigidBody && body.rigidBody->getUserData() != nullptr) - hitPart = reinterpret_cast(body.rigidBody->getUserData())->shared(); -} - -class NearestRayHit : public rp::RaycastCallback { - rp::Vector3 startPos; - std::optional filter; - - std::optional nearestHit; - float nearestHitDistance = -1; - - // Order is not guaranteed, so we have to figure out the nearest object using a more sophisticated algorith, - rp::decimal notifyRaycastHit(const rp::RaycastInfo& raycastInfo) override { - // If the detected object is further away than the nearest object, continue. - int distance = (raycastInfo.worldPoint - startPos).length(); - if (nearestHitDistance != -1 && distance >= nearestHitDistance) - return 1; - - if (!filter) { - nearestHit = raycastInfo; - nearestHitDistance = distance; - return 1; - } - - std::shared_ptr part = (raycastInfo.body && raycastInfo.body->getUserData() != nullptr) ? reinterpret_cast(raycastInfo.body->getUserData())->shared() : nullptr; - FilterResult result = filter.value()(part); - if (result == FilterResult::BLOCK) { - nearestHit = std::nullopt; - nearestHitDistance = distance; - return 1; - } else if (result == FilterResult::TARGET) { - nearestHit = raycastInfo; - nearestHitDistance = distance; - return 1; - } - return 1; - }; - -public: - NearestRayHit(rp::Vector3 startPos, std::optional filter = std::nullopt) : startPos(startPos), filter(filter) {} - std::optional getNearestHit() { return nearestHit; }; -}; - -std::optional PhysWorld::castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional filter, unsigned short categoryMaskBits) { - // std::scoped_lock lock(globalPhysicsLock); - rp::Ray ray(convert(point), convert(glm::normalize((glm::vec3)rotation)) * maxLength); - NearestRayHit rayHit(convert(point), filter); - worldImpl->raycast(ray, &rayHit, categoryMaskBits); - return rayHit.getNearestHit(); -} PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr part0, std::shared_ptr part1) { - // error checking - if (part0->rigidBody.rigidBody == nullptr - || part1->rigidBody.rigidBody == nullptr + if (part0->rigidBody.bodyImpl == nullptr + || part1->rigidBody.bodyImpl == nullptr || !part0->workspace() || !part1->workspace() || part0->workspace()->physicsWorld != shared_from_this() || part1->workspace()->physicsWorld != shared_from_this() ) { Logger::fatalError("Failed to create joint between two parts due to the call being invalid"); panic(); }; + JPH::TwoBodyConstraint* constraint; if (PhysJointGlueInfo* info = dynamic_cast(&type)) { - return worldImpl->createJoint(rp::FixedJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert(info->anchorPoint))); + JPH::FixedConstraintSettings settings; + settings.mAutoDetectPoint = true; // TODO: Replace this with anchor point + constraint = settings.Create(*part0->rigidBody.bodyImpl, *part1->rigidBody.bodyImpl); } else if (PhysJointWeldInfo* info = dynamic_cast(&type)) { - return worldImpl->createJoint(rp::FixedJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert(info->anchorPoint))); + JPH::FixedConstraintSettings settings; + settings.mAutoDetectPoint = true; // TODO: Replace this with anchor point + constraint = settings.Create(*part0->rigidBody.bodyImpl, *part1->rigidBody.bodyImpl); } else if (PhysJointSnapInfo* info = dynamic_cast(&type)) { - return worldImpl->createJoint(rp::FixedJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert(info->anchorPoint))); - } else if (PhysJointHingeInfo* info = dynamic_cast(&type)) { - return worldImpl->createJoint(rp::HingeJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert(info->anchorPoint), convert(info->rotationAxis))); - } else if (PhysJointMotorInfo* info = dynamic_cast(&type)) { - auto implInfo = rp::HingeJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert(info->anchorPoint), convert((info->rotationAxis))); - implInfo.isCollisionEnabled = false; - return worldImpl->createJoint(implInfo); - - // part1.lock()->rigidBody->getCollider(0)->setCollideWithMaskBits(0b10); - // part1.lock()->rigidBody->getCollider(0)->setCollisionCategoryBits(0b10); - // part0.lock()->rigidBody->getCollider(0)->setCollideWithMaskBits(0b01); - // part0.lock()->rigidBody->getCollider(0)->setCollisionCategoryBits(0b01); + JPH::FixedConstraintSettings settings; + settings.mAutoDetectPoint = true; // TODO: Replace this with anchor point + constraint = settings.Create(*part0->rigidBody.bodyImpl, *part1->rigidBody.bodyImpl); + } else { + panic(); } - panic(); // Unreachable + worldImpl.AddConstraint(constraint); + return { constraint }; } void PhysWorld::destroyJoint(PhysJoint joint) { - worldImpl->destroyJoint(joint.joint); + worldImpl.RemoveConstraint(joint.jointImpl); } -void PhysRigidBody::updateCollider(std::shared_ptr bPart) { - if (std::shared_ptr part = std::dynamic_pointer_cast(bPart)) { - rp::CollisionShape* physShape; - if (part->shape == PartType::Ball) { - physShape = physicsCommon.createSphereShape(glm::min(part->size.X(), part->size.Y(), part->size.Z()) * 0.5f); - } else if (part->shape == PartType::Block) { - physShape = physicsCommon.createBoxShape(convert(part->size * glm::vec3(0.5f))); - } +class PhysRayCastBodyFilter : public JPH::BodyFilter { + bool ShouldCollideLocked(const JPH::Body &inBody) const override { + std::shared_ptr part = ((Instance*)inBody.GetUserData())->shared(); - // Recreate the rigidbody if the shape changes - if (rigidBody->getNbColliders() > 0 && (_lastShape != part->shape || _lastSize != part->size)) { - // TODO: This causes Touched to get called twice. Fix this. - rigidBody->removeCollider(rigidBody->getCollider(0)); - rigidBody->addCollider(physShape, rp::Transform()); - } + // Ignore specifically "hidden" parts from raycast + // TODO: Replace this with a better system... Please. + if (!part->rigidBody.isCollisionsEnabled()) return false; - if (rigidBody->getNbColliders() == 0) - rigidBody->addCollider(physShape, rp::Transform()); + return true; + } +}; +std::optional PhysWorld::castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional filter, unsigned short categoryMaskBits) { + if (filter != std::nullopt) { Logger::fatalError("The filter property of PhysWorld::castRay is not yet implemented"); panic(); }; - _lastShape = part->shape; - _lastSize = part->size; + const JPH::BodyLockInterface& lockInterface = worldImpl.GetBodyLockInterfaceNoLock(); + const JPH::BodyInterface& interface = worldImpl.GetBodyInterface(); + const JPH::NarrowPhaseQuery& query = worldImpl.GetNarrowPhaseQuery(); + + // First we cast a ray to find a matching part + Vector3 end = point + rotation.Unit() * maxLength; + JPH::RRayCast ray { convert(point), convert(end) }; + JPH::RayCastResult result; + PhysRayCastBodyFilter bodyFilter; + bool hitFound = query.CastRay(ray, result, {}, {}, bodyFilter); + + // No matches found, return empty + if (!hitFound) return std::nullopt; + + // Next we cast a ray to find the hit surface and its world position and normal + JPH::BodyID hitBodyId = result.mBodyID; + std::shared_ptr part = ((Instance*)interface.GetUserData(hitBodyId))->shared(); + const JPH::Shape* shape = interface.GetShape(hitBodyId); + + // Find the hit position and hence the surface normal of the shape at that specific point + Vector3 hitPosition = point + rotation.Unit() * (maxLength * result.mFraction); + JPH::Vec3 surfaceNormal = shape->GetSurfaceNormal(result.mSubShapeID2, convert(part->cframe.Inverse() * hitPosition)); + Vector3 worldNormal = part->cframe.Rotation() * convert(surfaceNormal); + + return RaycastResult { + .worldPoint = hitPosition, + .worldNormal = worldNormal, + .body = lockInterface.TryGetBody(hitBodyId), + .hitPart = part, + }; +} + +uint BroadPhaseLayerInterface::GetNumBroadPhaseLayers() const { + return BPLayers::NUM_LAYERS; +} + +JPH::BroadPhaseLayer BroadPhaseLayerInterface::GetBroadPhaseLayer(JPH::ObjectLayer inLayer) const { + switch (inLayer) { + case Layers::DYNAMIC: return BPLayers::DYNAMIC; + case Layers::ANCHORED: return BPLayers::ANCHORED; + default: panic(); + } +} + +const char * BroadPhaseLayerInterface::GetBroadPhaseLayerName(JPH::BroadPhaseLayer inLayer) const { + using T = JPH::BroadPhaseLayer::Type; + switch ((T)inLayer) { + case (T)BPLayers::DYNAMIC: return "DYNAMIC"; + case (T)BPLayers::ANCHORED: return "ANCHORED"; + default: panic(); + } +} + +bool ObjectBroadPhaseFilter::ShouldCollide(JPH::ObjectLayer inLayer1, JPH::BroadPhaseLayer inLayer2) const { + return true; +} + +bool ObjectLayerPairFilter::ShouldCollide(JPH::ObjectLayer inLayer1, JPH::ObjectLayer inLayer2) const { + switch (inLayer1) { + case Layers::DYNAMIC: + return true; + case Layers::ANCHORED: + return inLayer2 == Layers::DYNAMIC; + default: + panic(); } } \ No newline at end of file diff --git a/core/src/physics/world.h b/core/src/physics/world.h index 5f24405..cdbdde1 100644 --- a/core/src/physics/world.h +++ b/core/src/physics/world.h @@ -2,26 +2,18 @@ #include "datatypes/vector.h" #include "enum/part.h" -#include "reactphysics3d/body/RigidBody.h" #include "utils.h" #include #include #include -#include -namespace rp = reactphysics3d; +#include +#include +#include +#include class BasePart; class PhysWorld; -class PhysicsEventListener : public rp::EventListener { - friend PhysWorld; - PhysWorld* world; - - PhysicsEventListener(PhysWorld*); - - void onContact(const rp::CollisionCallback::CallbackData&) override; - void onTrigger(const rp::OverlapCallback::CallbackData&) override; -}; struct PhysJointInfo { virtual ~PhysJointInfo() = default; protected: PhysJointInfo() = default; }; struct PhysJointGlueInfo : PhysJointInfo { Vector3 anchorPoint; inline PhysJointGlueInfo(Vector3 anchorPoint) : anchorPoint(anchorPoint) {} }; @@ -31,28 +23,27 @@ struct PhysJointHingeInfo : PhysJointInfo { Vector3 anchorPoint; Vector3 rotatio struct PhysJointMotorInfo : PhysJointInfo { Vector3 anchorPoint; Vector3 rotationAxis; inline PhysJointMotorInfo(Vector3 anchorPoint, Vector3 rotationAxis) : anchorPoint(anchorPoint), rotationAxis(rotationAxis) {} }; class PhysWorld; -class PhysJoint { - rp::Joint* joint; - inline PhysJoint(rp::Joint* joint) : joint(joint) {} - friend PhysWorld; +struct PhysJoint { public: - inline PhysJoint() {} + JPH::TwoBodyConstraint* jointImpl; }; struct RaycastResult; class PhysRigidBody { - rp::RigidBody* rigidBody = nullptr; - inline PhysRigidBody(rp::RigidBody* rigidBody) : rigidBody(rigidBody) {} + JPH::Body* bodyImpl = nullptr; + inline PhysRigidBody(JPH::Body* rigidBody) : bodyImpl(rigidBody) {} Vector3 _lastSize; PartType _lastShape; + bool collisionsEnabled = true; friend PhysWorld; friend RaycastResult; public: inline PhysRigidBody() {} - inline void setActive(bool active) { if (!rigidBody) return; rigidBody->setIsActive(active); } - inline void setCollisionsEnabled(bool enabled) { if (!rigidBody) return; rigidBody->getCollider(0)->setIsWorldQueryCollider(enabled); } + inline void setActive(bool active) { if (!bodyImpl) return; } + inline void setCollisionsEnabled(bool enabled) { collisionsEnabled = enabled; } + inline bool isCollisionsEnabled() { return collisionsEnabled; } void updateCollider(std::shared_ptr); }; @@ -65,11 +56,8 @@ public: struct RaycastResult { Vector3 worldPoint; Vector3 worldNormal; - int triangleIndex; PhysRigidBody body; nullable std::shared_ptr hitPart; - - RaycastResult(const rp::RaycastInfo& raycastInfo); }; enum FilterResult { @@ -81,9 +69,25 @@ enum FilterResult { class BasePart; typedef std::function)> RaycastFilter; +class BroadPhaseLayerInterface : public JPH::BroadPhaseLayerInterface { + uint GetNumBroadPhaseLayers() const override; + JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer inLayer) const override; + const char * GetBroadPhaseLayerName(JPH::BroadPhaseLayer inLayer) const override; +}; + +class ObjectBroadPhaseFilter : public JPH::ObjectVsBroadPhaseLayerFilter { + bool ShouldCollide(JPH::ObjectLayer inLayer1, JPH::BroadPhaseLayer inLayer2) const override; +}; + +class ObjectLayerPairFilter : public JPH::ObjectLayerPairFilter { + bool ShouldCollide(JPH::ObjectLayer inLayer1, JPH::ObjectLayer inLayer2) const override; +}; + class PhysWorld : public std::enable_shared_from_this { - rp::PhysicsWorld* worldImpl; - PhysicsEventListener physicsEventListener; + BroadPhaseLayerInterface broadPhaseLayerInterface; + ObjectBroadPhaseFilter objectBroadPhasefilter; + ObjectLayerPairFilter objectLayerPairFilter; + JPH::PhysicsSystem worldImpl; std::list> simulatedBodies; public: @@ -101,4 +105,7 @@ public: inline const std::list>& getSimulatedBodies() { return simulatedBodies; } void syncBodyProperties(std::shared_ptr); std::optional castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional filter, unsigned short categoryMaskBits); -}; \ No newline at end of file +}; + +void physicsInit(); +void physicsDeinit(); \ No newline at end of file diff --git a/editor/main.cpp b/editor/main.cpp index 1e2341b..24605dd 100644 --- a/editor/main.cpp +++ b/editor/main.cpp @@ -1,6 +1,7 @@ #include "mainwindow.h" #include "logger.h" +#include "physics/world.h" #include #include #include @@ -18,7 +19,7 @@ ma_engine miniaudio; int main(int argc, char *argv[]) { Logger::init(); - + physicsInit(); // Has to happen before Qt application initializes or we get an error in WASAPI initialization ma_result res = ma_engine_init(NULL, &miniaudio); if (res != MA_SUCCESS) { @@ -41,6 +42,7 @@ int main(int argc, char *argv[]) int result = a.exec(); ma_engine_uninit(&miniaudio); + physicsDeinit(); Logger::finish(); return result; } diff --git a/editor/mainwindow.cpp b/editor/mainwindow.cpp index d7823f9..472a664 100644 --- a/editor/mainwindow.cpp +++ b/editor/mainwindow.cpp @@ -31,6 +31,7 @@ #include #include #include +#include bool worldSpaceTransforms = false;