feat(physics): jolt physics implementation

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;
 }

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

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(

core/src/physics/convert.h

@@ -2,12 +2,9 @@
 #include "datatypes/cframe.h"
 #include "datatypes/vector.h"
 #include <glm/ext/vector_float3.hpp>
-#include <reactphysics3d/body/Body.h>
-#include <reactphysics3d/mathematics/Quaternion.h>
-#include <reactphysics3d/mathematics/Vector3.h>
 #include <glm/ext.hpp>
 
-#define rp reactphysics3d
+#include <Jolt/Jolt.h>
 
 template <typename T, typename F>
 T convert(F vec) = delete;
@@ -15,47 +12,33 @@ T convert(F vec) = delete;
 // Vector3
 
 template <>
-inline Vector3 convert<Vector3>(rp::Vector3 vec) {
-    return Vector3(vec.x, vec.y, vec.z);
+inline Vector3 convert<Vector3>(JPH::Vec3 vec) {
+    return Vector3(vec.GetX(), vec.GetY(), vec.GetZ());
 }
 
 template <>
-inline rp::Vector3 convert<rp::Vector3>(Vector3 vec) {
-    return rp::Vector3(vec.X(), vec.Y(), vec.Z());
+inline JPH::Vec3 convert<JPH::Vec3>(Vector3 vec) {
+    return JPH::Vec3(vec.X(), vec.Y(), vec.Z());
 }
 
 template <>
-inline glm::vec3 convert<glm::vec3>(rp::Vector3 vec) {
-    return glm::vec3(vec.x, vec.y, vec.z);
+inline glm::vec3 convert<glm::vec3>(JPH::Vec3 vec) {
+    return glm::vec3(vec.GetX(), vec.GetY(), vec.GetZ());
 }
 
 template <>
-inline rp::Vector3 convert<rp::Vector3>(glm::vec3 vec) {
-    return rp::Vector3(vec.x, vec.y, vec.z);
+inline JPH::Vec3 convert<JPH::Vec3>(glm::vec3 vec) {
+    return JPH::Vec3(vec.x, vec.y, vec.z);
 }
 
 // Quaternion
 
 template <>
-inline glm::quat convert<glm::quat>(rp::Quaternion quat) {
-    return glm::quat(quat.w, quat.x, quat.y, quat.z);
+inline glm::quat convert<glm::quat>(JPH::Quat quat) {
+    return glm::quat(quat.GetW(), quat.GetX(), quat.GetY(), quat.GetZ());
 }
 
 template <>
-inline rp::Quaternion convert<rp::Quaternion>(glm::quat quat) {
-    return rp::Quaternion(quat.w, rp::Vector3(quat.x, quat.y, quat.z));
+inline JPH::Quat convert<JPH::Quat>(glm::quat quat) {
+    return JPH::Quat(quat.x, quat.y, quat.z, quat.w);
 }
-
-// CFrame
-
-template <>
-inline rp::Transform convert<rp::Transform>(CFrame frame) {
-    return rp::Transform(convert<rp::Vector3>(frame.Position()), convert<rp::Quaternion>((glm::quat)frame.RotMatrix()));
-}
-
-template <>
-inline CFrame convert<CFrame>(rp::Transform trans) {
-    return CFrame(convert<Vector3>(trans.getPosition()), convert<glm::quat>(trans.getOrientation()));
-}
-
-#undef rp

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 <reactphysics3d/constraint/FixedJoint.h>
-#include <reactphysics3d/constraint/HingeJoint.h>
-#include <reactphysics3d/body/RigidBody.h>
-#include "timeutil.h"
-#include <memory>
 #include "objects/service/workspace.h"
+#include "physics/convert.h"
+#include "timeutil.h"
 
-rp::PhysicsCommon physicsCommon;
+#include <Jolt/Jolt.h>
+#include <Jolt/Core/JobSystemThreadPool.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Core/Factory.h>
+#include <Jolt/Core/Memory.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Body/BodyInterface.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/EActivation.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/RegisterTypes.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Body/BodyFilter.h>
+#include <Jolt/Physics/Body/BodyLockInterface.h>
+#include <Jolt/Physics/Collision/NarrowPhaseQuery.h>
+#include <Jolt/Physics/Constraints/FixedConstraint.h>
+#include <memory>
 
-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 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;
-
-        auto part0 = reinterpret_cast<BasePart*>(pair.getBody1()->getUserData())->shared<BasePart>();
-        auto part1 = reinterpret_cast<BasePart*>(pair.getBody2()->getUserData())->shared<BasePart>();
-
-        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) });
-        }
-    }
+void PhysWorld::addBody(std::shared_ptr<BasePart> part) {
+    syncBodyProperties(part);
 }
 
-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 PhysWorld::removeBody(std::shared_ptr<BasePart> part) {
+    // TODO:
+}
 
-        auto part0 = reinterpret_cast<BasePart*>(pair.getBody1()->getUserData())->shared<BasePart>();
-        auto part1 = reinterpret_cast<BasePart*>(pair.getBody2()->getUserData())->shared<BasePart>();
-
-        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) });
+JPH::Shape* makeShape(std::shared_ptr<BasePart> basePart) {
+    if (std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(basePart)) {
+        switch (part->shape) {
+        case PartType::Block:
+            return new JPH::BoxShape(convert<JPH::Vec3>(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<BasePart> 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<JPH::Vec3>(part->position()), convert<JPH::Quat>((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<JPH::Vec3>(part->velocity));
+    } else {
+        std::shared_ptr<Part> part2 = std::dynamic_pointer_cast<Part>(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<JPH::Vec3>(part->position()), convert<JPH::Quat>((glm::quat)part->cframe.RotMatrix()), convert<JPH::Vec3>(part->velocity), /* Angular velocity is NYI: */ body->GetAngularVelocity());
+    }
+}
+
+void physicsInit() {
+    JPH::RegisterDefaultAllocator();
+    JPH::Factory::sInstance = new JPH::Factory();
+    JPH::RegisterTypes();
+
+    allocator = new JPH::TempAllocatorImpl(10 * 1024 * 1024);
+    jobSystem = new JPH::JobSystemThreadPool(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, std::thread::hardware_concurrency() - 1);
+}
+
+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<BasePart> part : simulatedBodies) {
-        if (!part->rigidBody.rigidBody) continue;
-
-        // Sync properties
-        const rp::Transform& transform = part->rigidBody.rigidBody->getTransform();
-        part->cframe = convert<CFrame>(transform);
-        part->velocity = convert<Vector3>(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<JointInstance> motor = joint.lock()->CastTo<JointInstance>().expect();
-            float rate = motor->part0.lock()->GetSurfaceParamB(-motor->c0.LookVector().Unit()) * 30;
-            // part->rigidBody->enableGravity(false);
-            part->rigidBody.rigidBody->setAngularVelocity(convert<rp::Vector3>(-(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<BasePart> part = ((Instance*)interface.GetUserData(bodyID))->shared<BasePart>();
+        part->cframe = CFrame(convert<Vector3>(interface.GetPosition(bodyID)), convert<glm::quat>(interface.GetRotation(bodyID)));
     }
 
     physTime = tu_clock_micros() - startTime;
 }
 
-void PhysWorld::addBody(std::shared_ptr<BasePart> part) {
-    simulatedBodies.push_back(part);
-}
-
-void PhysWorld::removeBody(std::shared_ptr<BasePart> part) {
-    auto it = std::find(simulatedBodies.begin(), simulatedBodies.end(), part);
-    simulatedBodies.erase(it);
-}
-
-void PhysWorld::syncBodyProperties(std::shared_ptr<BasePart> part) {
-    rp::Transform transform = convert<rp::Transform>(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<rp::Vector3>(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<Vector3>(raycastInfo.worldPoint))
-    , worldNormal(convert<Vector3>(raycastInfo.worldNormal))
-    // , hitFraction(raycastInfo.hitFraction)
-    , triangleIndex(raycastInfo.triangleIndex)
-    , body(dynamic_cast<rp::RigidBody*>(raycastInfo.body))
-    // , collider(raycastInfo.collider)
-{
-    if (body.rigidBody && body.rigidBody->getUserData() != nullptr)
-        hitPart = reinterpret_cast<BasePart*>(body.rigidBody->getUserData())->shared<BasePart>();
-}
-
-class NearestRayHit : public rp::RaycastCallback {
-    rp::Vector3 startPos;
-    std::optional<RaycastFilter> filter;
-
-    std::optional<RaycastResult> 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<BasePart> part = (raycastInfo.body && raycastInfo.body->getUserData() != nullptr) ? reinterpret_cast<BasePart*>(raycastInfo.body->getUserData())->shared<BasePart>() : 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<RaycastFilter> filter = std::nullopt) : startPos(startPos), filter(filter) {}
-    std::optional<const RaycastResult> getNearestHit() { return nearestHit; };
-};
-
-std::optional<const RaycastResult> PhysWorld::castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits) {
-    // std::scoped_lock lock(globalPhysicsLock);
-    rp::Ray ray(convert<rp::Vector3>(point), convert<rp::Vector3>(glm::normalize((glm::vec3)rotation)) * maxLength);
-    NearestRayHit rayHit(convert<rp::Vector3>(point), filter);
-    worldImpl->raycast(ray, &rayHit, categoryMaskBits);
-    return rayHit.getNearestHit();
-}
-
 PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr<BasePart> part0, std::shared_ptr<BasePart> 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<PhysJointGlueInfo*>(&type)) {
-        return worldImpl->createJoint(rp::FixedJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert<rp::Vector3>(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<PhysJointWeldInfo*>(&type)) {
-        return worldImpl->createJoint(rp::FixedJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert<rp::Vector3>(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<PhysJointSnapInfo*>(&type)) {
-        return worldImpl->createJoint(rp::FixedJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert<rp::Vector3>(info->anchorPoint)));
-    } else if (PhysJointHingeInfo* info = dynamic_cast<PhysJointHingeInfo*>(&type)) {
-        return worldImpl->createJoint(rp::HingeJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert<rp::Vector3>(info->anchorPoint), convert<rp::Vector3>(info->rotationAxis)));
-    } else if (PhysJointMotorInfo* info = dynamic_cast<PhysJointMotorInfo*>(&type)) {
-        auto implInfo = rp::HingeJointInfo(part0->rigidBody.rigidBody, part1->rigidBody.rigidBody, convert<rp::Vector3>(info->anchorPoint), convert<rp::Vector3>((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<BasePart> bPart) {
-    if (std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(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<rp::Vector3>(part->size * glm::vec3(0.5f)));
+class PhysRayCastBodyFilter : public JPH::BodyFilter {
+    bool ShouldCollideLocked(const JPH::Body &inBody) const override {
+        std::shared_ptr<BasePart> part = ((Instance*)inBody.GetUserData())->shared<BasePart>();
+
+        // Ignore specifically "hidden" parts from raycast
+        // TODO: Replace this with a better system... Please.
+        if (!part->rigidBody.isCollisionsEnabled()) return false;
+
+        return true;
+    }
+};
+
+std::optional<const RaycastResult> PhysWorld::castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits) {
+    if (filter != std::nullopt) { Logger::fatalError("The filter property of PhysWorld::castRay is not yet implemented"); panic(); };
+
+    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<JPH::Vec3>(point), convert<JPH::Vec3>(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<BasePart> part = ((Instance*)interface.GetUserData(hitBodyId))->shared<BasePart>();
+    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<JPH::Vec3>(part->cframe.Inverse() * hitPosition));
+    Vector3 worldNormal = part->cframe.Rotation() * convert<Vector3>(surfaceNormal);
+
+    return RaycastResult {
+        .worldPoint = hitPosition,
+        .worldNormal = worldNormal,
+        .body = lockInterface.TryGetBody(hitBodyId),
+        .hitPart = part,
+    };
 }
 
-        // 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());
+uint BroadPhaseLayerInterface::GetNumBroadPhaseLayers() const {
+    return BPLayers::NUM_LAYERS;
 }
 
-        if (rigidBody->getNbColliders() == 0)
-            rigidBody->addCollider(physShape, rp::Transform());
-
-
-        _lastShape = part->shape;
-        _lastSize = part->size;
+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();
     }
 }

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 <functional>
 #include <list>
 #include <memory>
-#include <reactphysics3d/reactphysics3d.h>
 
-namespace rp = reactphysics3d;
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
 
 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<BasePart>);
 };
 
@@ -65,11 +56,8 @@ public:
 struct RaycastResult {
     Vector3 worldPoint;
     Vector3 worldNormal;
-    int triangleIndex;
     PhysRigidBody body;
     nullable std::shared_ptr<BasePart> hitPart;
-
-    RaycastResult(const rp::RaycastInfo& raycastInfo);
 };
 
 enum FilterResult {
@@ -81,9 +69,25 @@ enum FilterResult {
 class BasePart;
 typedef std::function<FilterResult(std::shared_ptr<BasePart>)> 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<PhysWorld> {
-    rp::PhysicsWorld* worldImpl;
-    PhysicsEventListener physicsEventListener;
+    BroadPhaseLayerInterface broadPhaseLayerInterface;
+    ObjectBroadPhaseFilter objectBroadPhasefilter;
+    ObjectLayerPairFilter objectLayerPairFilter;
+    JPH::PhysicsSystem worldImpl;
     std::list<std::shared_ptr<BasePart>> simulatedBodies;
 
 public:
@@ -102,3 +106,6 @@ public:
     void syncBodyProperties(std::shared_ptr<BasePart>);
     std::optional<const RaycastResult> castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits);
 };
+
+void physicsInit();
+void physicsDeinit();

editor/main.cpp

@@ -1,6 +1,7 @@
 #include "mainwindow.h"
 
 #include "logger.h"
+#include "physics/world.h"
 #include <QApplication>
 #include <QLocale>
 #include <QTranslator>
@@ -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;
 }

editor/mainwindow.cpp

@@ -31,6 +31,7 @@
 #include <miniaudio.h>
 #include <qtoolbutton.h>
 #include <vector>
+#include <fstream>
 
 bool worldSpaceTransforms = false;