fix(joint): error in continuity check leading to infinity loop then segfault

BUILD.md

@@ -22,9 +22,6 @@ The project will be built using VCPKG and MSVC
 * Qt 6.8.3 or higher, with MSVC toolchain
 * CMake
 * Git (for cloning the repo, optional)
-* ReactPhysics3D prebuilt library
-
-You will have to build and install ReactPhysics3D yourself. Check the [build instructions on ReactPhysics3D](https://www.reactphysics3d.com/documentation/index.html#building) for help
 
 To start, clone the repository:
 

CMakePresets.json

@@ -8,6 +8,14 @@
       "cacheVariables": {
         "CMAKE_TOOLCHAIN_FILE": "$env{VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake"
       }
+    },
+    {
+      "name": "vcpkg-linux",
+      "generator": "Ninja",
+      "binaryDir": "${sourceDir}/build",
+      "cacheVariables": {
+        "CMAKE_TOOLCHAIN_FILE": "$env{VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake"
+      }
     }
   ]
 }

core/src/objects/base/instance.cpp

@@ -17,6 +17,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include "ptr_helpers.h"
 
 // Static so that this variable name is "local" to this source file
 const InstanceType Instance::TYPE = {
@@ -55,17 +56,6 @@ Instance::Instance(const InstanceType* type) {
 Instance::~Instance () {
 }
 
-template <typename T>
-bool operator ==(std::optional<std::weak_ptr<T>> a, std::optional<std::weak_ptr<T>> b) {
-    return (!a.has_value() || a.value().expired()) && (!b.has_value() || b.value().expired())
-    || (a.has_value() && !a.value().expired()) && (b.has_value() && !b.value().expired()) && a.value().lock() == b.value().lock();
-}
-
-template <typename T>
-bool operator ==(std::weak_ptr<T> a, std::weak_ptr<T> b) {
-    return a.expired() && b.expired() || (!a.expired() && !b.expired() && a.lock() == b.lock());
-}
-
 template <typename T>
 std::weak_ptr<T> optional_to_weak(std::optional<std::shared_ptr<T>> a) {
     return a ? a.value() : std::weak_ptr<T>();
@@ -151,6 +141,19 @@ std::optional<std::shared_ptr<Instance>> Instance::GetParent() {
     return parent.lock();
 }
 
+void Instance::Destroy() {
+    if (parentLocked) return;
+    // TODO: Implement proper distruction stuff
+    SetParent(std::nullopt);
+    parentLocked = true;
+}
+
+bool Instance::IsA(std::string className) {
+    const InstanceType* cur = GetClass();
+    while (cur && cur->className != className) { cur = cur->super; }
+    return cur != nullptr;
+}
+
 static std::shared_ptr<Instance> DUMMY_INSTANCE;
 DescendantsIterator Instance::GetDescendantsStart() {
     return DescendantsIterator(GetChildren().size() > 0 ? GetChildren()[0] : DUMMY_INSTANCE);
@@ -385,6 +388,7 @@ std::optional<std::shared_ptr<Instance>> Instance::Clone(RefState<_RefStatePrope
             }
         } else {
             Data::Variant value = GetPropertyValue(property).expect();
+            // printf("property: %s, value: %s\n", property.c_str(), std::string(value.ToString()).c_str());
             newInstance->SetPropertyValue(property, value).expect();
         }
     }

core/src/objects/base/instance.h

@@ -41,7 +41,7 @@ struct InstanceType {
 typedef std::pair<std::shared_ptr<Instance>, std::string> _RefStatePropertyCell;
 
 class DescendantsIterator;
-class Snap;
+class JointInstance;
 
 // Base class for all instances in the data model
                  // Note: enable_shared_from_this HAS to be public or else its field will not be populated
@@ -60,7 +60,7 @@ private:
     bool ancestryContinuityCheck(std::optional<std::shared_ptr<Instance>> newParent);
     void updateAncestry(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent);
 
-    friend Snap; // This isn't ideal, but oh well
+    friend JointInstance; // This isn't ideal, but oh well
 protected:
     bool parentLocked = false;
     std::unique_ptr<MemberMap> memberMap;
@@ -91,6 +91,10 @@ public:
     std::optional<std::shared_ptr<Instance>> GetParent();
     bool IsParentLocked();
     inline const std::vector<std::shared_ptr<Instance>> GetChildren() { return children; }
+    void Destroy();
+    // Determines whether this object is an instance of, or an instance of a subclass of the sepcified type's class name
+    bool IsA(std::string className);
+    template <typename T> bool IsA() { return IsA(T::TYPE.className); }
     
     DescendantsIterator GetDescendantsStart();
     DescendantsIterator GetDescendantsEnd();
@@ -111,7 +115,7 @@ public:
     result<std::shared_ptr<T>, InstanceCastError> CastTo() {
         // TODO: Too lazy to implement a manual check
         std::shared_ptr<T> result = std::dynamic_pointer_cast<T>(shared_from_this());
-        if (result != nullptr)
+        if (result == nullptr)
             return InstanceCastError(GetClass()->className, T::TYPE.className);
         return result;
     }

core/src/objects/joint/jointinstance.cpp

@@ -0,0 +1,94 @@
+#include "jointinstance.h"
+
+#include "datatypes/cframe.h"
+#include "datatypes/ref.h"
+#include "objects/datamodel.h"
+#include "objects/jointsservice.h"
+#include "objects/part.h"
+#include "objects/workspace.h"
+#include <memory>
+#include <reactphysics3d/constraint/FixedJoint.h>
+#include <reactphysics3d/engine/PhysicsWorld.h>
+#include "ptr_helpers.h"
+
+const InstanceType JointInstance::TYPE = {
+    .super = &Instance::TYPE,
+    .className = "JointInstance",
+};
+
+const InstanceType* JointInstance::GetClass() {
+    return &TYPE;
+}
+
+JointInstance::JointInstance(const InstanceType* type): Instance(type) {
+    this->memberMap = std::make_unique<MemberMap>(MemberMap {
+        .super = std::move(this->memberMap),
+        .members = {
+            { "Part0", {
+                .backingField = &part0,
+                .type = &Data::InstanceRef::TYPE,
+                .codec = fieldCodecOf<Data::InstanceRef, std::weak_ptr<Instance>>(),
+                .updateCallback = memberFunctionOf(&JointInstance::onUpdated, this),
+            }}, { "Part1", {
+                .backingField = &part1,
+                .type = &Data::InstanceRef::TYPE,
+                .codec = fieldCodecOf<Data::InstanceRef, std::weak_ptr<Instance>>(),
+                .updateCallback = memberFunctionOf(&JointInstance::onUpdated, this),
+            }}, { "C0", {
+                .backingField = &c0,
+                .type = &Data::CFrame::TYPE,
+                .codec = fieldCodecOf<Data::CFrame>(),
+                .updateCallback = memberFunctionOf(&JointInstance::onUpdated, this),
+            }}, { "C1", {
+                .backingField = &c1,
+                .type = &Data::CFrame::TYPE,
+                .codec = fieldCodecOf<Data::CFrame>(),
+                .updateCallback = memberFunctionOf(&JointInstance::onUpdated, this),
+            }}, 
+        }
+    });
+}
+
+JointInstance::~JointInstance() {
+}
+
+void JointInstance::OnAncestryChanged(std::optional<std::shared_ptr<Instance>>, std::optional<std::shared_ptr<Instance>>) {
+    // Destroy and rebuild the joint, it's the simplest solution that actually works
+
+    breakJoint();
+    buildJoint();
+}
+
+void JointInstance::onUpdated(std::string property) {
+    // Add ourselves to the attached parts, or remove, if applicable
+
+    // Parts differ, delete 
+    if (part0 != oldPart0 && !oldPart0.expired()) {
+        oldPart0.lock()->untrackJoint(shared<JointInstance>());
+    }
+
+    if (part1 != oldPart1 && !oldPart1.expired()) {
+        oldPart1.lock()->untrackJoint(shared<JointInstance>());
+    }
+
+    // Parts differ, add 
+    if (part0 != oldPart0 && !part0.expired()) {
+        part0.lock()->trackJoint(shared<JointInstance>());
+    }
+
+    if (part1 != oldPart1 && !part1.expired()) {
+        part1.lock()->trackJoint(shared<JointInstance>());
+    }
+
+    // Destroy and rebuild the joint, if applicable
+
+    breakJoint();
+    buildJoint();
+
+    oldPart0 = part0;
+    oldPart1 = part1;
+}
+
+std::optional<std::shared_ptr<Workspace>> JointInstance::workspaceOfPart(std::shared_ptr<Part> part) {
+    return part->workspace();
+}

core/src/objects/joint/jointinstance.h

@@ -7,17 +7,19 @@
 class Part;
 class Workspace;
 
-class Snap : public Instance {
+class JointInstance : public Instance {
-    rp::FixedJoint* joint = nullptr;
+    std::weak_ptr<Part> oldPart0;
-
+    std::weak_ptr<Part> oldPart1;
+protected:
     // The workspace the joint was created in, if it exists
     std::weak_ptr<Workspace> jointWorkspace;
-protected:
+
     void OnAncestryChanged(std::optional<std::shared_ptr<Instance>>, std::optional<std::shared_ptr<Instance>>) override;
 
+    std::optional<std::shared_ptr<Workspace>> workspaceOfPart(std::shared_ptr<Part>);
     void onUpdated(std::string property);
-    void buildJoint();
+    virtual void buildJoint() = 0;
-    void breakJoint();
+    virtual void breakJoint() = 0;
 public:
     const static InstanceType TYPE;
 
@@ -26,10 +28,8 @@ public:
     Data::CFrame c0;
     Data::CFrame c1;
 
-    Snap();
+    JointInstance(const InstanceType*);
-    ~Snap();
+    ~JointInstance();
 
-    static inline std::shared_ptr<Snap> New() { return std::make_shared<Snap>(); };
-    static inline std::shared_ptr<Instance> Create() { return std::make_shared<Snap>(); };
     virtual const InstanceType* GetClass() override;
 };

core/src/objects/joint/snap.cpp

@@ -1,16 +1,17 @@
 #include "snap.h"
 
 #include "datatypes/cframe.h"
-#include "datatypes/ref.h"
 #include "objects/datamodel.h"
+#include "objects/joint/jointinstance.h"
 #include "objects/jointsservice.h"
-#include "workspace.h"
+#include "objects/part.h"
+#include "objects/workspace.h"
 #include <memory>
 #include <reactphysics3d/constraint/FixedJoint.h>
 #include <reactphysics3d/engine/PhysicsWorld.h>
 
 const InstanceType Snap::TYPE = {
-    .super = &Instance::TYPE,
+    .super = &JointInstance::TYPE,
     .className = "Snap",
     .constructor = &Snap::Create,
 };
@@ -19,60 +20,20 @@ const InstanceType* Snap::GetClass() {
     return &TYPE;
 }
 
-Snap::Snap(): Instance(&TYPE) {
+Snap::Snap(): JointInstance(&TYPE) {
-    this->memberMap = std::make_unique<MemberMap>(MemberMap {
-        .super = std::move(this->memberMap),
-        .members = {
-            { "Part0", {
-                .backingField = &part0,
-                .type = &Data::InstanceRef::TYPE,
-                .codec = fieldCodecOf<Data::InstanceRef, std::weak_ptr<Instance>>(),
-                .updateCallback = memberFunctionOf(&Snap::onUpdated, this),
-            }}, { "Part1", {
-                .backingField = &part1,
-                .type = &Data::InstanceRef::TYPE,
-                .codec = fieldCodecOf<Data::InstanceRef, std::weak_ptr<Instance>>(),
-                .updateCallback = memberFunctionOf(&Snap::onUpdated, this),
-            }}, { "C0", {
-                .backingField = &c0,
-                .type = &Data::CFrame::TYPE,
-                .codec = fieldCodecOf<Data::CFrame>(),
-                .updateCallback = memberFunctionOf(&Snap::onUpdated, this),
-            }}, { "C1", {
-                .backingField = &c1,
-                .type = &Data::CFrame::TYPE,
-                .codec = fieldCodecOf<Data::CFrame>(),
-                .updateCallback = memberFunctionOf(&Snap::onUpdated, this),
-            }}, 
-        }
-    });
 }
 
 Snap::~Snap() {
 }
 
-void Snap::OnAncestryChanged(std::optional<std::shared_ptr<Instance>>, std::optional<std::shared_ptr<Instance>>) {
-    // Destroy and rebuild the joint, it's the simplest solution that actually works
-
-    breakJoint();
-    buildJoint();
-}
-
-void Snap::onUpdated(std::string property) {
-    // Destroy and rebuild the joint, if applicable
-
-    breakJoint();
-    buildJoint();
-}
-
 void Snap::buildJoint() {
     // Only if both parts are set, are not the same part, are part of a workspace, and are part of the same workspace, we build the joint
-    if (part0.expired() || part1.expired() || part0.lock() == part1.lock() || !part0.lock()->workspace() || part0.lock()->workspace() != part1.lock()->workspace()) return;
+    if (part0.expired() || part1.expired() || part0.lock() == part1.lock() || !workspaceOfPart(part0.lock()) || workspaceOfPart(part0.lock()) != workspaceOfPart(part1.lock())) return;
 
     // Don't build the joint if we're not part of either a workspace or JointsService
     if ((!GetParent() || GetParent().value()->GetClass() != &JointsService::TYPE) && !workspace()) return;
 
-    std::shared_ptr<Workspace> workspace = part0.lock()->workspace().value();
+    std::shared_ptr<Workspace> workspace = workspaceOfPart(part0.lock()).value();
     if (!workspace->physicsWorld) return;
 
     // Update Part1's rotation and cframe prior to creating the joint as reactphysics3d locks rotation based on how it

core/src/objects/joint/snap.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "objects/base/instance.h"
+#include "objects/joint/jointinstance.h"
+#include <memory>
+
+class Snap : public JointInstance {
+    rp::FixedJoint* joint = nullptr;
+
+    virtual void buildJoint() override;
+    virtual void breakJoint() override;
+public:
+    const static InstanceType TYPE;
+
+    Snap();
+    ~Snap();
+
+    static inline std::shared_ptr<Snap> New() { return std::make_shared<Snap>(); };
+    static inline std::shared_ptr<Instance> Create() { return std::make_shared<Snap>(); };
+    virtual const InstanceType* GetClass() override;
+};

core/src/objects/joint/weld.cpp

@@ -0,0 +1,59 @@
+#include "weld.h"
+
+#include "datatypes/cframe.h"
+#include "objects/datamodel.h"
+#include "objects/joint/jointinstance.h"
+#include "objects/jointsservice.h"
+#include "objects/part.h"
+#include "objects/workspace.h"
+#include <memory>
+#include <reactphysics3d/constraint/FixedJoint.h>
+#include <reactphysics3d/engine/PhysicsWorld.h>
+
+const InstanceType Weld::TYPE = {
+    .super = &JointInstance::TYPE,
+    .className = "Weld",
+    .constructor = &Weld::Create,
+};
+
+const InstanceType* Weld::GetClass() {
+    return &TYPE;
+}
+
+Weld::Weld(): JointInstance(&TYPE) {
+}
+
+Weld::~Weld() {
+}
+
+void Weld::buildJoint() {
+    // Only if both parts are set, are not the same part, are part of a workspace, and are part of the same workspace, we build the joint
+    if (part0.expired() || part1.expired() || part0.lock() == part1.lock() || !workspaceOfPart(part0.lock()) || workspaceOfPart(part0.lock()) != workspaceOfPart(part1.lock())) return;
+
+    // Don't build the joint if we're not part of either a workspace or JointsService
+    if ((!GetParent() || GetParent().value()->GetClass() != &JointsService::TYPE) && !workspace()) return;
+
+    std::shared_ptr<Workspace> workspace = workspaceOfPart(part0.lock()).value();
+    if (!workspace->physicsWorld) return;
+
+    // Update Part1's rotation and cframe prior to creating the joint as reactphysics3d locks rotation based on how it
+    // used to be rather than specifying an anchor rotation, so whatever.
+    Data::CFrame newFrame = part0.lock()->cframe * (c1.Inverse() * c0);
+    part1.lock()->cframe = newFrame;
+    workspace->SyncPartPhysics(part1.lock());
+
+    // printf("c1.Rotation: ");
+    // printVec(c1.ToEulerAnglesXYZ());
+    rp::FixedJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (c0.Inverse() * c1).Position());
+    this->joint = dynamic_cast<rp::FixedJoint*>(workspace->physicsWorld->createJoint(jointInfo));
+    jointWorkspace = workspace;
+}
+
+// !!! REMINDER: This has to be called manually when parts are destroyed/removed from the workspace, or joints will linger
+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() || !jointWorkspace.lock()->physicsWorld) return;
+
+    jointWorkspace.lock()->physicsWorld->destroyJoint(this->joint);
+    this->joint = nullptr;
+}

core/src/objects/joint/weld.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "objects/base/instance.h"
+#include "objects/joint/jointinstance.h"
+#include <memory>
+
+class Weld : public JointInstance {
+    rp::FixedJoint* joint = nullptr;
+
+    virtual void buildJoint() override;
+    virtual void breakJoint() override;
+public:
+    const static InstanceType TYPE;
+
+    Weld();
+    ~Weld();
+
+    static inline std::shared_ptr<Weld> New() { return std::make_shared<Weld>(); };
+    static inline std::shared_ptr<Instance> Create() { return std::make_shared<Weld>(); };
+    virtual const InstanceType* GetClass() override;
+};

core/src/objects/jointsservice.h

@@ -2,12 +2,9 @@
 
 #include "objects/base/service.h"
 
-class Snap;
 class JointsService : public Service {
 private:
     std::optional<std::shared_ptr<Workspace>> jointWorkspace();
-
-    friend Snap;
 protected:
     void InitService() override;
     bool initialized = false;

core/src/objects/meta.cpp

@@ -1,7 +1,8 @@
 #include "meta.h"
+#include "objects/joint/jointinstance.h"
 #include "objects/jointsservice.h"
 #include "objects/part.h"
-#include "objects/snap.h"
+#include "objects/joint/snap.h"
 #include "objects/workspace.h"
 
 std::map<std::string, const InstanceType*> INSTANCE_MAP = {
@@ -10,5 +11,6 @@ std::map<std::string, const InstanceType*> INSTANCE_MAP = {
     { "Workspace", &Workspace::TYPE },
     { "DataModel", &DataModel::TYPE },
     { "Snap", &Snap::TYPE },
+    { "JointInstance", &JointInstance::TYPE },
     { "JointsService", &JointsService::TYPE },
 };

core/src/objects/part.cpp

@@ -6,6 +6,11 @@
 #include "datatypes/color3.h"
 #include "datatypes/vector.h"
 #include "objects/base/member.h"
+#include "objects/joint/weld.h"
+#include "objects/jointsservice.h"
+#include "objects/joint/jointinstance.h"
+#include "objects/joint/snap.h"
+#include "rendering/surface.h"
 #include <memory>
 #include <optional>
 
@@ -89,6 +94,11 @@ Part::Part(PartConstructParams params): Instance(&TYPE), cframe(Data::CFrame::Fr
                 .codec = cframeRotationCodec(),
                 .updateCallback = memberFunctionOf(&Part::onUpdated, this),
                 .flags = PropertyFlags::PROP_NOSAVE
+            }}, { "Velocity", {
+                .backingField = &velocity,
+                .type = &Vector3::TYPE,
+                .codec = fieldCodecOf<Data::Vector3>(),
+                .updateCallback = memberFunctionOf(&Part::onUpdated, this),
             }}, { "CFrame", {
                 .backingField = &cframe,
                 .type = &Data::CFrame::TYPE,
@@ -172,12 +182,22 @@ void Part::OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std
     if (workspace())
         workspace().value()->SyncPartPhysics(std::dynamic_pointer_cast<Part>(this->shared_from_this()));
 
+    // Destroy joints
+    if (!workspace()) BreakJoints();
+
     // TODO: Sleeping bodies that touch this one also need to be updated
 }
 
 void Part::onUpdated(std::string property) {
+    // Reset velocity
+    if (property != "Velocity")
+        velocity = Data::Vector3::ZERO;
+
     if (workspace())
         workspace().value()->SyncPartPhysics(std::dynamic_pointer_cast<Part>(this->shared_from_this()));
+
+    // When position/rotation/size is manually edited, break all joints, they don't apply anymore
+    BreakJoints();
 }
 
 // Expands provided extents to fit point
@@ -207,3 +227,179 @@ Vector3 Part::GetAABB() {
 
     return (min - max).Abs() / 2;
 }
+
+void Part::BreakJoints() {
+    for (std::weak_ptr<JointInstance> joint : primaryJoints) {
+        if (joint.expired()) continue;
+        joint.lock()->Destroy();
+    }
+
+    for (std::weak_ptr<JointInstance> joint : secondaryJoints) {
+        if (joint.expired()) continue;
+        joint.lock()->Destroy();
+    }
+}
+
+static Data::Vector3 FACES[6] = {
+    {1, 0, 0},
+    {0, 1, 0},
+    {0, 0, 1},
+    {-1, 0, 0},
+    {0, -1, 0},
+    {0, 0, -1},
+};
+
+SurfaceType Part::surfaceFromFace(NormalId face) {
+    switch (face) {
+        case Top: return topSurface;
+        case Bottom: return bottomSurface;
+        case Right: return rightSurface;
+        case Left: return leftSurface;
+        case Front: return frontSurface;
+        case Back: return backSurface;
+    }
+    return SurfaceSmooth; // Unreachable
+}
+
+bool Part::checkJointContinuity(std::shared_ptr<Part> otherPart) {
+    // Make sure that the two parts don't depend on one another
+
+    return checkJointContinuityUp(otherPart) && checkJointContinuityDown(otherPart);
+}
+
+bool Part::checkJointContinuityDown(std::shared_ptr<Part> otherPart) {
+    if (shared<Part>() == otherPart) return false;
+    for (auto joint : primaryJoints) {
+        if (joint.expired() || joint.lock()->part1.expired()) continue;
+        if (!joint.lock()->part1.lock()->checkJointContinuityDown(otherPart))
+            return false;
+    }
+    return true;
+}
+
+bool Part::checkJointContinuityUp(std::shared_ptr<Part> otherPart) {
+    if (shared<Part>() == otherPart) return false;
+    for (auto joint : secondaryJoints) {
+        if (joint.expired() || joint.lock()->part0.expired()) continue;
+        if (!joint.lock()->part0.lock()->checkJointContinuityUp(otherPart))
+            return false;
+    }
+    return true;
+}
+
+std::optional<std::shared_ptr<JointInstance>> makeJointFromSurfaces(SurfaceType a, SurfaceType b) {
+    if (a == SurfaceWeld || b == SurfaceWeld || a == SurfaceGlue || b == SurfaceGlue) return Weld::New();
+    if ((a == SurfaceStuds && (b == SurfaceInlets || b == SurfaceUniversal))
+    || (a == SurfaceInlets && (b == SurfaceStuds || b == SurfaceUniversal))
+    || (a == SurfaceUniversal && (b == SurfaceStuds || b == SurfaceInlets || b == SurfaceUniversal)))
+        return Snap::New();
+    return std::nullopt;
+}
+
+void Part::MakeJoints() {
+    // Algorithm: Find nearby parts
+    // Make sure parts are not dependant on each other (via primary/secondaryJoints)
+    // Find matching surfaces (surface normal dot product < -0.999)
+    // Get surface cframe of this part
+    // Transform surface center of other part to local via surface cframe of this part
+    // Make sure z of transformed center is not greater than 0.05
+
+    if (!workspace()) return;
+
+    // TEMPORARY
+    // TODO: Use more efficient algorithm to *actually* find nearby parts)
+    for (auto it = workspace().value()->GetDescendantsStart(); it != workspace().value()->GetDescendantsEnd(); it++) {
+        InstanceRef obj = *it;
+        if (obj == shared_from_this()) continue; // Skip ourselves
+        if (obj->GetClass()->className != "Part") continue; // TODO: Replace this with a .IsA call instead of comparing the class name directly
+        std::shared_ptr<Part> otherPart = obj->CastTo<Part>().expect();
+
+        for (Data::Vector3 myFace : FACES) {
+            Data::Vector3 myWorldNormal = cframe.Rotation() * myFace;
+            Data::Vector3 validUp = cframe.Rotation() * Data::Vector3(1,1,1).Unit(); // If myFace == (0, 1, 0), then (0, 1, 0) would produce NaN as up, so we fudge the up so that it works
+            Data::CFrame surfaceFrame(cframe.Position(), cframe * (myFace * size), validUp);
+
+            for (Data::Vector3 otherFace : FACES) {
+                Data::Vector3 otherWorldNormal = otherPart->cframe.Rotation() * otherFace;
+                Data::Vector3 otherSurfaceCenter = otherPart->cframe * (otherFace * otherPart->size);
+                Data::Vector3 surfacePointLocalToMyFrame = surfaceFrame.Inverse() * otherSurfaceCenter;
+
+                float dot = myWorldNormal.Dot(otherWorldNormal);
+                if (dot > -0.99) continue; // Surface is pointing opposite to ours
+                if (abs(surfacePointLocalToMyFrame.Z()) > 0.05) continue; // Surfaces are within 0.05 studs of one another
+                if (!checkJointContinuity(otherPart)) continue;
+
+                SurfaceType mySurface = surfaceFromFace(faceFromNormal(myFace));
+                SurfaceType otherSurface = surfaceFromFace(faceFromNormal(otherFace));
+
+                auto joint_ = makeJointFromSurfaces(mySurface, otherSurface);
+                if (!joint_) continue;
+                std::shared_ptr<JointInstance> joint = joint_.value();
+                joint->part0 = shared<Part>();
+                joint->part1 = otherPart->shared<Part>();
+                joint->c1 = cframe;
+                joint->c0 = otherPart->cframe;
+                dataModel().value()->GetService<JointsService>()->AddChild(joint);
+                joint->UpdateProperty("Part0");
+
+                Logger::debugf("Made joint between %s and %s!\n", name.c_str(), otherPart->name.c_str());
+            }
+        }
+    }
+}
+
+void Part::trackJoint(std::shared_ptr<JointInstance> joint) {
+    if (!joint->part0.expired() && joint->part0.lock() == shared_from_this()) {
+        for (auto it = primaryJoints.begin(); it != primaryJoints.end();) {
+            // Clean expired refs
+            if (it->expired()) {
+                primaryJoints.erase(it);
+                continue;
+            }
+
+            // If the joint is already tracked, skip
+            if (it->lock() == joint)
+                return;
+            it++;
+        }
+
+        primaryJoints.push_back(joint);
+    } else if (!joint->part1.expired() && joint->part1.lock() == shared_from_this()) {
+        for (auto it = secondaryJoints.begin(); it != secondaryJoints.end();) {
+            // Clean expired refs
+            if (it->expired()) {
+                secondaryJoints.erase(it);
+                continue;
+            }
+
+            // If the joint is already tracked, skip
+            if (it->lock() == joint)
+                return;
+            it++;
+        }
+
+        secondaryJoints.push_back(joint);
+    }
+}
+
+void Part::untrackJoint(std::shared_ptr<JointInstance> joint) {
+    for (auto it = primaryJoints.begin(); it != primaryJoints.end();) {
+        // Clean expired refs
+        if (it->expired() || it->lock() == joint) {
+            primaryJoints.erase(it);
+            continue;
+        }
+
+        it++;
+    }
+
+    for (auto it = secondaryJoints.begin(); it != secondaryJoints.end();) {
+        // Clean expired refs
+        if (it->expired() || it->lock() == joint) {
+            secondaryJoints.erase(it);
+            continue;
+        }
+
+        it++;
+    }
+}

core/src/objects/part.h

@@ -8,7 +8,9 @@
 #include "datatypes/vector.h"
 #include "objects/base/instance.h"
 #include "rendering/surface.h"
+#include <optional>
 #include <reactphysics3d/reactphysics3d.h>
+#include <vector>
 
 namespace rp = reactphysics3d;
 
@@ -23,13 +25,31 @@ struct PartConstructParams {
     bool locked = false;
 };
 
+class Snap;
+
 class Part : public Instance {
 protected:
+    // Joints where this part is Part0
+    std::vector<std::weak_ptr<JointInstance>> primaryJoints;
+    // Joints where this part is Part1
+    std::vector<std::weak_ptr<JointInstance>> secondaryJoints;
+
+    void trackJoint(std::shared_ptr<JointInstance>);
+    void untrackJoint(std::shared_ptr<JointInstance>);
+
+    SurfaceType surfaceFromFace(NormalId);
+    bool checkJointContinuity(std::shared_ptr<Part>);
+    bool checkJointContinuityUp(std::shared_ptr<Part>);
+    bool checkJointContinuityDown(std::shared_ptr<Part>);
+
+    friend JointInstance;
+
     void OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent) override;
     void onUpdated(std::string);
 public:
     const static InstanceType TYPE;
 
+    Data::Vector3 velocity;
     Data::CFrame cframe;
     glm::vec3 size;
     Data::Color3 color;
@@ -58,6 +78,9 @@ public:
 
     inline Data::Vector3 position() { return cframe.Position(); }
 
+    void MakeJoints();
+    void BreakJoints();
+
     // Calculate size of axis-aligned bounding box
     Data::Vector3 GetAABB();
 };

core/src/objects/workspace.cpp

@@ -1,7 +1,7 @@
 #include "workspace.h"
 #include "objects/base/instance.h"
 #include "objects/jointsservice.h"
-#include "objects/snap.h"
+#include "objects/joint/jointinstance.h"
 #include "physics/util.h"
 #include <reactphysics3d/engine/PhysicsCommon.h>
 
@@ -45,22 +45,23 @@ void Workspace::InitService() {
     // Sync all parts
     for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
         InstanceRef obj = *it;
-        if (obj->GetClass()->className != "Part") continue; // TODO: Replace this with a .IsA call instead of comparing the class name directly
+        if (!obj->IsA<Part>()) continue;
-        std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(obj);
+        std::shared_ptr<Part> part = obj->CastTo<Part>().expect();
         this->SyncPartPhysics(part);
+        part->MakeJoints();
     }
 
     // Activate all joints
     for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
         InstanceRef obj = *it;
-        if (obj->GetClass()->className != "Snap") continue; // TODO: Replace this with a .IsA call instead of comparing the class name directly
+        if (!obj->IsA<JointInstance>()) continue;
-        std::shared_ptr<Snap> joint = std::dynamic_pointer_cast<Snap>(obj);
+        std::shared_ptr<JointInstance> joint = obj->CastTo<JointInstance>().expect();
         joint->UpdateProperty("Part0");
     }
 
     for (auto obj : dataModel().value()->GetService<JointsService>()->GetChildren()) {
-        if (obj->GetClass()->className != "Snap") continue; // TODO: Replace this with a .IsA call instead of comparing the class name directly
+        if (!obj->IsA<JointInstance>()) continue;
-        std::shared_ptr<Snap> joint = std::dynamic_pointer_cast<Snap>(obj);
+        std::shared_ptr<JointInstance> joint = obj->CastTo<JointInstance>().expect();
         joint->UpdateProperty("Part0");
     }
 }
@@ -94,6 +95,7 @@ void Workspace::SyncPartPhysics(std::shared_ptr<Part> part) {
     part->rigidBody->updateMassFromColliders();
     part->rigidBody->updateLocalInertiaTensorFromColliders();
 
+    part->rigidBody->setLinearVelocity(part->velocity);
     // part->rigidBody->setMass(density * part->size.x * part->size.y * part->size.z);
 
     part->rigidBody->setUserData(&*part);
@@ -111,6 +113,7 @@ void Workspace::PhysicsStep(float deltaTime) {
         std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(obj);
         const rp::Transform& transform = part->rigidBody->getTransform();
         part->cframe = Data::CFrame(transform);
+        part->velocity = part->rigidBody->getLinearVelocity();
     }
 }
 

core/src/objects/workspace.h

@@ -25,6 +25,7 @@ enum FilterResult {
 
 class Part;
 class Snap;
+class Weld;
 
 typedef std::function<FilterResult(std::shared_ptr<Part>)> RaycastFilter;
 
@@ -34,6 +35,7 @@ class Workspace : public Service {
 
     friend Part;
     friend Snap;
+    friend Weld;
 protected:
     void InitService() override;
     bool initialized = false;

core/src/ptr_helpers.h

@@ -0,0 +1,14 @@
+#pragma once
+#include <optional>
+#include <memory>
+
+template <typename T>
+bool operator ==(std::optional<std::weak_ptr<T>> a, std::optional<std::weak_ptr<T>> b) {
+    return (!a.has_value() || a.value().expired()) && (!b.has_value() || b.value().expired())
+    || (a.has_value() && !a.value().expired()) && (b.has_value() && !b.value().expired()) && a.value().lock() == b.value().lock();
+}
+
+template <typename T>
+bool operator ==(std::weak_ptr<T> a, std::weak_ptr<T> b) {
+    return a.expired() && b.expired() || (!a.expired() && !b.expired() && a.lock() == b.lock());
+}

core/src/rendering/surface.cpp

@@ -1,14 +1,14 @@
 #include "surface.h"
 #include "datatypes/vector.h"
 
-static std::array<Data::Vector3, 6> FACE_NORMALS = {{
+Data::Vector3 FACE_NORMALS[6] = {
     { 1, 0, 0 },
     { 0, 1, 0 },
     { 0, 0, 1 },
     { -1, 0, 0 },
     { 0, -1, 0 },
     { 0, 0, -1 },
-}};
+};
 
 NormalId faceFromNormal(Data::Vector3 normal) {
     for (int face = 0; face < 6; face++) {

editor/mainglwidget.cpp

@@ -3,6 +3,7 @@
 #include <glm/vector_relational.hpp>
 #include <qnamespace.h>
 #include <qsoundeffect.h>
+#include <string>
 #include "mainglwidget.h"
 #include "common.h"
 #include "math_helper.h"
@@ -11,6 +12,8 @@
 #include "rendering/renderer.h"
 #include "rendering/shader.h"
 
+#define PI 3.14159
+
 static Data::CFrame XYZToZXY(glm::vec3(0, 0, 0), -glm::vec3(1, 0, 0), glm::vec3(0, 0, 1));
 
 MainGLWidget::MainGLWidget(QWidget* parent): QOpenGLWidget(parent) {
@@ -155,6 +158,7 @@ void MainGLWidget::handleObjectDrag(QMouseEvent* evt) {
 
 
     gWorkspace()->SyncPartPhysics(draggingObject.lock());
+    draggingObject.lock()->UpdateProperty("Position");
     sendPropertyUpdatedSignal(draggingObject.lock(), "Position", draggingObject.lock()->position());
 }
 
@@ -242,6 +246,8 @@ void MainGLWidget::handleLinearTransform(QMouseEvent* evt) {
         playSound("./assets/excluded/switch.wav");
 
     gWorkspace()->SyncPartPhysics(part);
+    part->UpdateProperty("Position");
+    part->UpdateProperty("Size");
     sendPropertyUpdatedSignal(part, "Position", part->position());
     sendPropertyUpdatedSignal(part, "Size", Data::Vector3(part->size));
 }
@@ -271,7 +277,7 @@ void MainGLWidget::handleRotationalTransform(QMouseEvent* evt) {
     
     // Snap the angle
     if (snappingFactor() > 0)
-        angle = roundf(angle * 4 / std::numbers::pi / snappingFactor()) / 4 * std::numbers::pi * snappingFactor();
+        angle = roundf(angle * 4 / PI / snappingFactor()) / 4 * PI * snappingFactor();
 
     // Checks if the rotation axis is facing towards, or away from the camera
     // If it pointing away from the camera, then we need to invert the angle change
@@ -285,6 +291,7 @@ void MainGLWidget::handleRotationalTransform(QMouseEvent* evt) {
     part->cframe = initialFrame * Data::CFrame::FromEulerAnglesXYZ(-angles);
 
     gWorkspace()->SyncPartPhysics(part);
+    part->UpdateProperty("Rotation");
     sendPropertyUpdatedSignal(part, "Rotation", part->cframe.ToEulerAnglesXYZ());
 }
 
@@ -439,6 +446,7 @@ void MainGLWidget::updateCycle() {
 
 }
 
+int partId = 1;
 void MainGLWidget::keyPressEvent(QKeyEvent* evt) {
     if (evt->key() == Qt::Key_W) moveZ = 1;
     else if (evt->key() == Qt::Key_S) moveZ = -1;
@@ -454,6 +462,7 @@ void MainGLWidget::keyPressEvent(QKeyEvent* evt) {
             .color = glm::vec3(1.0f, 0.5f, 0.31f),
         }));
         gWorkspace()->SyncPartPhysics(lastPart);
+        lastPart->name = "Part" + std::to_string(partId++);
     }
 
     if (evt->key() == Qt::Key_U)

editor/mainwindow.cpp

@@ -3,7 +3,7 @@
 #include "common.h"
 #include "logger.h"
 #include "objects/jointsservice.h"
-#include "objects/snap.h"
+#include "objects/joint/snap.h"
 #include <map>
 #include <memory>
 #include <qclipboard.h>
@@ -178,6 +178,8 @@ MainWindow::MainWindow(QWidget *parent)
 
     // gWorkspace()->AddChild(snap);
     gDataModel->GetService<JointsService>()->AddChild(snap);
+    snap->UpdateProperty("Part0");
+    snap->UpdateProperty("Part1");
 }
 
 void MainWindow::closeEvent(QCloseEvent* evt) {
@@ -366,18 +368,18 @@ void MainWindow::connectActionHandlers() {
 
     connect(ui->actionSave, &QAction::triggered, this, [&]() {
         std::optional<std::string> path;
-        if (!gDataModel->HasFile())
+        if (!editModeDataModel->HasFile())
-            path = openFileDialog("Openblocks Level (*.obl)", ".obl", QFileDialog::AcceptSave, QString::fromStdString("Save " + gDataModel->name));
+            path = openFileDialog("Openblocks Level (*.obl)", ".obl", QFileDialog::AcceptSave, QString::fromStdString("Save " + editModeDataModel->name));
-        if (!gDataModel->HasFile() && (!path || path == "")) return;
+        if (!editModeDataModel->HasFile() && (!path || path == "")) return;
 
-        gDataModel->SaveToFile(path);
+        editModeDataModel->SaveToFile(path);
     });
 
     connect(ui->actionSaveAs, &QAction::triggered, this, [&]() {
-        std::optional<std::string> path = openFileDialog("Openblocks Level (*.obl)", ".obl", QFileDialog::AcceptSave, QString::fromStdString("Save as " + gDataModel->name));
+        std::optional<std::string> path = openFileDialog("Openblocks Level (*.obl)", ".obl", QFileDialog::AcceptSave, QString::fromStdString("Save as " + editModeDataModel->name));
         if (!path || path == "") return;
 
-        gDataModel->SaveToFile(path);
+        editModeDataModel->SaveToFile(path);
     });
 
     connect(ui->actionOpen, &QAction::triggered, this, [&]() {
@@ -389,9 +391,16 @@ void MainWindow::connectActionHandlers() {
         
         // simulationInit();
         std::shared_ptr<DataModel> newModel = DataModel::LoadFromFile(path.value());
+        editModeDataModel = newModel;
         gDataModel = newModel;
         newModel->Init();
         ui->explorerView->updateRoot(newModel);
+
+        // Reset running state
+        runState = RUN_STOPPED;
+        ui->actionRunSimulation->setEnabled(true);
+        ui->actionPauseSimulation->setEnabled(false);
+        ui->actionStopSimulation->setEnabled(false);
     });
 
     connect(ui->actionDelete, &QAction::triggered, this, [&]() {

vcpkg-configuration.json

@@ -10,5 +10,8 @@
       "location": "https://github.com/microsoft/vcpkg-ce-catalog/archive/refs/heads/main.zip",
       "name": "microsoft"
     }
+  ],
+  "overlay-ports": [
+    "./vcpkg-overlays"
   ]
 }

vcpkg-overlays/reactphysics3d/portfile.cmake

@@ -0,0 +1,23 @@
+vcpkg_check_linkage(ONLY_STATIC_LIBRARY)
+
+vcpkg_from_github(
+    OUT_SOURCE_PATH SOURCE_PATH
+    REPO DanielChappuis/reactphysics3d
+    REF "cd958bbc0c6e84a869388cba6613f10cc645b3cb"
+    SHA512 9856c0e998473e0bfb97af9ced07952bbd4dfef79f7dc388b1ecf9b6c6406f7669333e441fe6cefdf40b32edc5a1b8e4cb35a8c15fccb64c28785aff5fd77113
+    HEAD_REF master
+    PATCHES "std_chrono.patch"
+)
+
+vcpkg_cmake_configure(
+    SOURCE_PATH "${SOURCE_PATH}"
+)
+
+vcpkg_cmake_install()
+
+vcpkg_cmake_config_fixup(PACKAGE_NAME "reactphysics3d" CONFIG_PATH "lib/cmake/ReactPhysics3D")
+
+file(REMOVE_RECURSE "${CURRENT_PACKAGES_DIR}/debug/include")
+
+# file(INSTALL "${CMAKE_CURRENT_LIST_DIR}/usage" DESTINATION "${CURRENT_PACKAGES_DIR}/share/${PORT}")
+vcpkg_install_copyright(FILE_LIST "${SOURCE_PATH}/LICENSE")

vcpkg-overlays/reactphysics3d/std_chrono.patch

@@ -0,0 +1,12 @@
+diff --git a/include/reactphysics3d/utils/DefaultLogger.h b/include/reactphysics3d/utils/DefaultLogger.h
+index 1088d1e..8360f07 100644
+--- a/include/reactphysics3d/utils/DefaultLogger.h
++++ b/include/reactphysics3d/utils/DefaultLogger.h
+@@ -37,6 +37,7 @@
+ #include <iomanip>
+ #include <mutex>
+ #include <ctime>
++#include <chrono>
+ 
+ /// ReactPhysics3D namespace
+ namespace reactphysics3d {

vcpkg-overlays/reactphysics3d/vcpkg.json

@@ -0,0 +1,18 @@
+{
+    "name": "reactphysics3d",
+    "version": "0.10.2",
+    "homepage": "https://github.com/DanielChappuis/reactphysics3d",
+    "description": "Open source C++ physics engine library in 3D",
+    "license": "zlib",
+    "dependencies": [
+      {
+        "name" : "vcpkg-cmake",
+        "host" : true
+      },
+      {
+        "name" : "vcpkg-cmake-config",
+        "host" : true
+      },
+      "fmt"
+    ]
+  }

vcpkg.json

@@ -5,6 +5,13 @@
     "glm",
     { "name": "pugixml", "version>=": "1.15" },
     "sdl2",
-    "stb"
+    "stb",
+    "reactphysics3d"
+  ],
+  "overrides": [
+    { 
+      "name": "sdl2", 
+      "version": "2.32.4"
+    }
   ]
 }