feat(physics): basic parallelism

core/src/objects/joint/rotate.cpp

@@ -28,7 +28,7 @@ void Rotate::buildJoint() {
     workspace->SyncPartPhysics(part1.lock());
     // Do NOT use Abs() in this scenario. For some reason that breaks it
     rp::HingeJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (part0.lock()->cframe * c0).Position(), -(part0.lock()->cframe * c0).LookVector().Unit());
-    this->joint = dynamic_cast<rp::HingeJoint*>(workspace->physicsWorld->createJoint(jointInfo));
+    this->joint = dynamic_cast<rp::HingeJoint*>(workspace->CreateJoint(jointInfo));
     jointWorkspace = workspace;
 
     // part1.lock()->rigidBody->getCollider(0)->setCollideWithMaskBits(0b10);
@@ -42,6 +42,6 @@ void Rotate::breakJoint() {
     // If the joint doesn't exist, or its workspace expired (not our problem anymore), then no need to do anything
     if (!this->joint || jointWorkspace.expired()) return;
 
-    jointWorkspace.lock()->physicsWorld->destroyJoint(this->joint);
+    jointWorkspace.lock()->DestroyJoint(this->joint);
     this->joint = nullptr;
 }

core/src/objects/joint/rotatev.cpp

@@ -34,7 +34,7 @@ void RotateV::buildJoint() {
     
     jointInfo.isCollisionEnabled = false;
 
-    this->joint = dynamic_cast<rp::HingeJoint*>(workspace->physicsWorld->createJoint(jointInfo));
+    this->joint = dynamic_cast<rp::HingeJoint*>(workspace->CreateJoint(jointInfo));
     jointWorkspace = workspace;
 
 
@@ -48,6 +48,6 @@ void RotateV::breakJoint() {
     // If the joint doesn't exist, or its workspace expired (not our problem anymore), then no need to do anything
     if (!this->joint || jointWorkspace.expired()) return;
 
-    jointWorkspace.lock()->physicsWorld->destroyJoint(this->joint);
+    jointWorkspace.lock()->DestroyJoint(this->joint);
     this->joint = nullptr;
 }

core/src/objects/joint/snap.cpp

@@ -32,7 +32,7 @@ void Snap::buildJoint() {
     workspace->SyncPartPhysics(part1.lock());
 
     rp::FixedJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (c0.Inverse() * c1).Position());
-    this->joint = dynamic_cast<rp::FixedJoint*>(workspace->physicsWorld->createJoint(jointInfo));
+    this->joint = dynamic_cast<rp::FixedJoint*>(workspace->CreateJoint(jointInfo));
     jointWorkspace = workspace;
 }
 
@@ -41,6 +41,6 @@ void Snap::breakJoint() {
     // If the joint doesn't exist, or its workspace expired (not our problem anymore), then no need to do anything
     if (!this->joint || jointWorkspace.expired()) return;
 
-    jointWorkspace.lock()->physicsWorld->destroyJoint(this->joint);
+    jointWorkspace.lock()->DestroyJoint(this->joint);
     this->joint = nullptr;
 }

core/src/objects/joint/weld.cpp

@@ -32,7 +32,7 @@ void Weld::buildJoint() {
     workspace->SyncPartPhysics(part1.lock());
 
     rp::FixedJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (c0.Inverse() * c1).Position());
-    this->joint = dynamic_cast<rp::FixedJoint*>(workspace->physicsWorld->createJoint(jointInfo));
+    this->joint = dynamic_cast<rp::FixedJoint*>(workspace->CreateJoint(jointInfo));
     jointWorkspace = workspace;
 }
 
@@ -41,6 +41,6 @@ void Weld::breakJoint() {
     // If the joint doesn't exist, or its workspace expired (not our problem anymore), then no need to do anything
     if (!this->joint || jointWorkspace.expired()) return;
 
-    jointWorkspace.lock()->physicsWorld->destroyJoint(this->joint);
+    jointWorkspace.lock()->DestroyJoint(this->joint);
     this->joint = nullptr;
 }

core/src/objects/part.cpp

@@ -49,6 +49,15 @@ void Part::OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std
     // TODO: Sleeping bodies that touch this one also need to be updated
 }
 
+void Part::OnWorkspaceAdded(std::optional<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) {
+    newWorkspace->AddBody(shared<Part>());
+}
+
+void Part::OnWorkspaceRemoved(std::shared_ptr<Workspace> oldWorkspace) {
+    if (simulationTicket->get() != nullptr)
+        oldWorkspace->RemoveBody(shared<Part>());
+}
+
 void Part::onUpdated(std::string property) {
     // Reset velocity
     if (property != "Velocity")

core/src/objects/part.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <list>
 #include <memory>
 #include <glm/glm.hpp>
 #include <glm/ext.hpp>
@@ -9,6 +10,7 @@
 #include "datatypes/vector.h"
 #include "objects/base/instance.h"
 #include "enum/surface.h"
+#include <mutex>
 #include <optional>
 #include <reactphysics3d/reactphysics3d.h>
 #include <vector>
@@ -30,6 +32,12 @@ struct PartConstructParams {
 
 class Workspace;
 
+#ifndef __SIMULATION_TICKET
+#define __SIMULATION_TICKET
+class Part;
+typedef std::list<std::shared_ptr<Part>>::iterator SimulationTicket;
+#endif
+
 class DEF_INST_(explorer_icon="part") Part : public PVInstance {
     AUTOGEN_PREAMBLE
 protected:
@@ -50,6 +58,8 @@ protected:
     friend JointInstance;
     friend Workspace;
 
+    virtual void OnWorkspaceAdded(std::optional<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) override;
+    virtual void OnWorkspaceRemoved(std::shared_ptr<Workspace> oldWorkspace) override;
     void OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent) override;
     void onUpdated(std::string);
 public:
@@ -99,6 +109,12 @@ public:
     DEF_SIGNAL SignalSource TouchEnded;
 
     rp::RigidBody* rigidBody = nullptr;
+    SimulationTicket simulationTicket;
+    enum {
+        PART_SYNCED,
+        PART_QUEUED_ADD,
+        PART_QUEUED_REMOVE,
+    } queueState = PART_SYNCED;
     
     inline SurfaceType GetSurfaceFromFace(NormalId face) { return surfaceFromFace(face); }
     float GetSurfaceParamA(Vector3 face);

core/src/objects/service/workspace.cpp

@@ -2,7 +2,9 @@
 #include "datatypes/variant.h"
 #include "datatypes/ref.h"
 #include "datatypes/vector.h"
+#include "logger.h"
 #include "objects/base/instance.h"
+#include "objects/part.h"
 #include "objects/service/jointsservice.h"
 #include "objects/joint/jointinstance.h"
 #include "objects/datamodel.h"
@@ -102,6 +104,10 @@ void Workspace::InitService() {
 }
 
 void Workspace::SyncPartPhysics(std::shared_ptr<Part> part) {
+    printf("SyncPartPhysics-lck\n");
+    std::scoped_lock lock(globalPhysicsLock);
+    printf("SyncPartPhysics-post\n");
+
     rp::Transform transform = part->cframe;
     if (!part->rigidBody) {
         part->rigidBody = physicsWorld->createRigidBody(transform);
@@ -145,14 +151,26 @@ void Workspace::SyncPartPhysics(std::shared_ptr<Part> part) {
 tu_time_t physTime;
 void Workspace::PhysicsStep(float deltaTime) {
     tu_time_t startTime = tu_clock_micros();
-    // Step the simulation a few steps
+
+    std::scoped_lock lock(globalPhysicsLock);
     physicsWorld->update(std::min(deltaTime / 2, (1/60.f)));
 
+    // Update queued objects
+    queueLock.lock();
+    for (QueueItem item : bodyQueue) {
+        if (item.action == QueueItem::QUEUEITEM_ADD) {
+            simulatedBodies.push_back(item.part);
+            item.part->simulationTicket = --simulatedBodies.end();
+        } else if (item.part->simulationTicket->get() != nullptr) {
+            simulatedBodies.erase(item.part->simulationTicket);
+            item.part->simulationTicket = {};
+        }
+    }
+    queueLock.unlock();
+
     // TODO: Add list of tracked parts in workspace based on their ancestry using inWorkspace property of Instance
-    for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
-        std::shared_ptr<Instance> obj = *it;
-        if (!obj->IsA<Part>()) continue;
-        std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(obj);
+    for (std::shared_ptr<Part> part : simulatedBodies) {
+        if (!part->rigidBody) continue;
 
         // Sync properties
         const rp::Transform& transform = part->rigidBody->getTransform();
@@ -233,6 +251,9 @@ public:
 };
 
 std::optional<const RaycastResult> Workspace::CastRayNearest(glm::vec3 point, glm::vec3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits) {
+    printf("Raycast-lck\n");
+    std::scoped_lock lock(globalPhysicsLock);
+    printf("Raycast-post\n");
     rp::Ray ray(glmToRp(point), glmToRp(glm::normalize(rotation)) * maxLength);
     NearestRayHit rayHit(glmToRp(point), filter);
     physicsWorld->raycast(ray, &rayHit, categoryMaskBits);
@@ -240,5 +261,30 @@ std::optional<const RaycastResult> Workspace::CastRayNearest(glm::vec3 point, gl
 }
 
 void Workspace::DestroyRigidBody(rp::RigidBody* rigidBody) {
+    std::scoped_lock lock(globalPhysicsLock);
     physicsWorld->destroyRigidBody(rigidBody);
 }
+
+void Workspace::DestroyJoint(rp::Joint* joint) {
+    std::scoped_lock lock(globalPhysicsLock);
+    physicsWorld->destroyJoint(joint);
+}
+
+rp::Joint* Workspace::CreateJoint(const rp::JointInfo& jointInfo) {
+    std::scoped_lock lock(globalPhysicsLock);
+    rp::Joint* joint = physicsWorld->createJoint(jointInfo);
+
+    return joint;
+}
+
+void Workspace::AddBody(std::shared_ptr<Part> part) {
+    queueLock.lock();
+    bodyQueue.push_back({part, QueueItem::QUEUEITEM_ADD});
+    queueLock.unlock();
+}
+
+void Workspace::RemoveBody(std::shared_ptr<Part> part) {
+    queueLock.lock();
+    bodyQueue.push_back({part, QueueItem::QUEUEITEM_REMOVE});
+    queueLock.unlock();
+}

core/src/objects/service/workspace.h

@@ -4,7 +4,9 @@
 #include "objects/base/service.h"
 #include "utils.h"
 #include <glm/ext/vector_float3.hpp>
+#include <list>
 #include <memory>
+#include <mutex>
 #include <reactphysics3d/body/RigidBody.h>
 #include <reactphysics3d/engine/EventListener.h>
 #include <reactphysics3d/engine/PhysicsCommon.h>
@@ -35,8 +37,21 @@ class Weld;
 class Rotate;
 class RotateV;
 
+#ifndef __SIMULATION_TICKET
+#define __SIMULATION_TICKET
+typedef std::list<std::shared_ptr<Part>>::iterator SimulationTicket;
+#endif
+
 typedef std::function<FilterResult(std::shared_ptr<Part>)> RaycastFilter;
 
+struct QueueItem {
+    std::shared_ptr<Part> part;
+    enum {
+        QUEUEITEM_ADD,
+        QUEUEITEM_REMOVE,
+    } action;
+};
+
 class Workspace;
 class PhysicsEventListener : public rp::EventListener {
     friend Workspace;
@@ -51,15 +66,11 @@ class PhysicsEventListener : public rp::EventListener {
 class DEF_INST_SERVICE_(explorer_icon="workspace") Workspace : public Service {
     AUTOGEN_PREAMBLE
     
-    rp::PhysicsWorld* notnull physicsWorld;
+    std::list<std::shared_ptr<Part>> simulatedBodies;
+    std::list<QueueItem> bodyQueue;
+    rp::PhysicsWorld* physicsWorld;
     static rp::PhysicsCommon* physicsCommon;
     PhysicsEventListener physicsEventListener;
-
-    friend Part;
-    friend Snap;
-    friend Weld;
-    friend Rotate;
-    friend RotateV;
 protected:
     void InitService() override;
     bool initialized = false;
@@ -68,14 +79,22 @@ public:
     Workspace();
     ~Workspace();
 
+    std::mutex globalPhysicsLock;
+    std::recursive_mutex queueLock;
+
     DEF_PROP float fallenPartsDestroyHeight = -500;
 
     // static inline std::shared_ptr<Workspace> New() { return std::make_shared<Workspace>(); };
     static inline std::shared_ptr<Instance> Create() { return std::make_shared<Workspace>(); };
 
+    void AddBody(std::shared_ptr<Part> part);
+    void RemoveBody(std::shared_ptr<Part> part);
     void SyncPartPhysics(std::shared_ptr<Part> part);
     void DestroyRigidBody(rp::RigidBody* rigidBody);
 
+    rp::Joint* CreateJoint(const rp::JointInfo& jointInfo);
+    void DestroyJoint(rp::Joint* joint);
+
     void PhysicsStep(float deltaTime);
     std::optional<const RaycastResult> CastRayNearest(glm::vec3 point, glm::vec3 rotation, float maxLength, std::optional<RaycastFilter> filter = std::nullopt, unsigned short categoryMaskBits = 0xFFFF);
 };

editor/mainwindow.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1027</width>
+    <width>1050</width>
     <height>750</height>
    </rect>
   </property>

editor/placedocument.cpp

@@ -3,21 +3,54 @@
 #include "datatypes/variant.h"
 #include "mainglwidget.h"
 #include "mainwindow.h"
-#include "objects/joint/snap.h"
-#include "objects/script.h"
 #include "objects/service/script/scriptcontext.h"
-#include "enum/surface.h"
-#include <cstdio>
 #include <memory>
+#include <mutex>
 #include <qboxlayout.h>
+#include <qcoreevent.h>
 #include <qdebug.h>
 #include <qevent.h>
+#include <qglobal.h>
 #include <qmargins.h>
 #include <qmdisubwindow.h>
 #include <qlayout.h>
 #include <qmimedata.h>
+#include <qmutex.h>
+#include <qwaitcondition.h>
+#include <thread>
 #include "../ui_mainwindow.h"
 #include "objects/service/selection.h"
+#include "timeutil.h"
+
+class PlaceDocumentPhysicsWorker {
+public:
+    std::mutex sync;
+    std::thread thread;
+    std::condition_variable runningCond;
+    bool running = false;
+    bool quit = false;
+
+    PlaceDocumentPhysicsWorker() : thread(&PlaceDocumentPhysicsWorker::doWork, this) {}
+private:
+    tu_time_t lastTime = tu_clock_micros();
+    void doWork() {
+        do {
+            tu_time_t deltaTime = tu_clock_micros() - lastTime;
+            lastTime = tu_clock_micros();
+
+            // First frame is always empty
+            if (deltaTime > 100) {
+                gWorkspace()->PhysicsStep(float(deltaTime)/1'000'000);
+            }
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(33 - deltaTime/1000));
+
+            std::unique_lock lock(sync);
+            runningCond.wait(lock, [&]{ return running || quit; });
+            lock.unlock();
+        } while (!quit);
+    }
+};
 
 PlaceDocument::PlaceDocument(QWidget* parent):
     QMdiSubWindow(parent) {
@@ -28,15 +61,29 @@ PlaceDocument::PlaceDocument(QWidget* parent):
 
     _runState = RUN_STOPPED;
     updateSelectionListeners(gDataModel->GetService<Selection>());
+
+    worker = new PlaceDocumentPhysicsWorker();
 }
 
 PlaceDocument::~PlaceDocument() {
+    worker->quit = true;
+    worker->runningCond.notify_all();
+    worker->thread.join();
+}
+
+void PlaceDocument::updatePhysicsWorker() {
+    {
+        std::lock_guard lock(worker->sync);
+        worker->running = _runState == RUN_RUNNING;
+    }
+    worker->runningCond.notify_all();
 }
 
 void PlaceDocument::setRunState(RunState newState) {
     if (newState == RUN_RUNNING && _runState != RUN_RUNNING) {
         if (_runState == RUN_PAUSED) {
             _runState = RUN_RUNNING;
+            updatePhysicsWorker();
             return;
         }
 
@@ -55,6 +102,8 @@ void PlaceDocument::setRunState(RunState newState) {
         gDataModel = editModeDataModel;
         updateSelectionListeners(gDataModel->GetService<Selection>());
     }
+
+    updatePhysicsWorker();
 }
 
 void PlaceDocument::updateSelectionListeners(std::shared_ptr<Selection> selection) {
@@ -81,18 +130,12 @@ void PlaceDocument::closeEvent(QCloseEvent *closeEvent) {
 }
 
 std::shared_ptr<Part> shit;
-static std::chrono::time_point lastTime = std::chrono::steady_clock::now();
 void PlaceDocument::timerEvent(QTimerEvent* evt) {
     if (evt->timerId() != timer.timerId()) {
         QWidget::timerEvent(evt);
         return;
     }
 
-    float deltaTime = std::chrono::duration_cast<std::chrono::duration<float>>(std::chrono::steady_clock::now() - lastTime).count();
-    lastTime = std::chrono::steady_clock::now();
-
-    if (_runState == RUN_RUNNING)
-        gWorkspace()->PhysicsStep(deltaTime);
     placeWidget->repaint();
     placeWidget->updateCycle();
     gDataModel->GetService<ScriptContext>()->RunSleepingThreads();

editor/placedocument.h

@@ -2,11 +2,18 @@
 
 #include "datatypes/signal.h"
 #include "mainglwidget.h"
+#include <condition_variable>
+#include <mutex>
 #include <qevent.h>
 #include <qmdisubwindow.h>
+#include <qthread.h>
+#include <qmutex.h>
+#include <qwaitcondition.h>
 #include <QBasicTimer>
+#include <type_traits>
 
 class Selection;
+class PlaceDocumentPhysicsWorker;
 
 enum RunState {
     RUN_STOPPED,
@@ -18,10 +25,14 @@ class PlaceDocument : public QMdiSubWindow {
     QBasicTimer timer;
     RunState _runState;
 
+    PlaceDocumentPhysicsWorker* worker;
+
     std::weak_ptr<SignalConnection> selectionConnection;
 
     void timerEvent(QTimerEvent*) override;
     void updateSelectionListeners(std::shared_ptr<Selection>);
+
+    void updatePhysicsWorker();
 public:
     MainGLWidget* placeWidget;
     PlaceDocument(QWidget* parent = nullptr);