fix(joints): break joint when member part is updated/destroyed

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,13 @@ 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;
+}
+
 static std::shared_ptr<Instance> DUMMY_INSTANCE;
 DescendantsIterator Instance::GetDescendantsStart() {
     return DescendantsIterator(GetChildren().size() > 0 ? GetChildren()[0] : DUMMY_INSTANCE);

core/src/objects/base/instance.h

@@ -91,6 +91,7 @@ public:
     std::optional<std::shared_ptr<Instance>> GetParent();
     bool IsParentLocked();
     inline const std::vector<std::shared_ptr<Instance>> GetChildren() { return children; }
+    void Destroy();
     
     DescendantsIterator GetDescendantsStart();
     DescendantsIterator GetDescendantsEnd();

core/src/objects/part.cpp

@@ -6,6 +6,7 @@
 #include "datatypes/color3.h"
 #include "datatypes/vector.h"
 #include "objects/base/member.h"
+#include "objects/snap.h"
 #include <memory>
 #include <optional>
 
@@ -172,12 +173,18 @@ 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) {
     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
@@ -206,4 +213,72 @@ Vector3 Part::GetAABB() {
     }
 
     return (min - max).Abs() / 2;
+}
+
+void Part::BreakJoints() {
+    for (std::weak_ptr<Snap> joint : primaryJoints) {
+        if (joint.expired()) continue;
+        joint.lock()->Destroy();
+    }
+
+    for (std::weak_ptr<Snap> joint : secondaryJoints) {
+        if (joint.expired()) continue;
+        joint.lock()->Destroy();
+    }
+}
+
+void Part::trackJoint(std::shared_ptr<Snap> 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<Snap> 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

@@ -9,6 +9,7 @@
 #include "objects/base/instance.h"
 #include "rendering/surface.h"
 #include <reactphysics3d/reactphysics3d.h>
+#include <vector>
 
 namespace rp = reactphysics3d;
 
@@ -23,8 +24,20 @@ struct PartConstructParams {
     bool locked = false;
 };
 
+class Snap;
+
 class Part : public Instance {
 protected:
+    // Joints where this part is Part0
+    std::vector<std::weak_ptr<Snap>> primaryJoints;
+    // Joints where this part is Part1
+    std::vector<std::weak_ptr<Snap>> secondaryJoints;
+
+    void trackJoint(std::shared_ptr<Snap>);
+    void untrackJoint(std::shared_ptr<Snap>);
+
+    friend Snap;
+
     void OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent) override;
     void onUpdated(std::string);
 public:
@@ -58,6 +71,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/snap.cpp

@@ -4,10 +4,12 @@
 #include "datatypes/ref.h"
 #include "objects/datamodel.h"
 #include "objects/jointsservice.h"
+#include "objects/part.h"
 #include "workspace.h"
 #include <memory>
 #include <reactphysics3d/constraint/FixedJoint.h>
 #include <reactphysics3d/engine/PhysicsWorld.h>
+#include "ptr_helpers.h"
 
 const InstanceType Snap::TYPE = {
     .super = &Instance::TYPE,
@@ -59,10 +61,33 @@ void Snap::OnAncestryChanged(std::optional<std::shared_ptr<Instance>>, std::opti
 }
 
 void Snap::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<Snap>());
+    }
+
+    if (part1 != oldPart1 && !oldPart1.expired()) {
+        oldPart1.lock()->untrackJoint(shared<Snap>());
+    }
+
+    // Parts differ, add 
+    if (part0 != oldPart0 && !part0.expired()) {
+        part0.lock()->trackJoint(shared<Snap>());
+    }
+
+    if (part1 != oldPart1 && !part1.expired()) {
+        part1.lock()->trackJoint(shared<Snap>());
+    }
+
     // Destroy and rebuild the joint, if applicable
 
     breakJoint();
     buildJoint();
+
+    oldPart0 = part0;
+    oldPart1 = part1;
 }
 
 void Snap::buildJoint() {

core/src/objects/snap.h

@@ -10,6 +10,9 @@ class Workspace;
 class Snap : public Instance {
     rp::FixedJoint* joint = nullptr;
 
+    std::weak_ptr<Part> oldPart0;
+    std::weak_ptr<Part> oldPart1;
+
     // The workspace the joint was created in, if it exists
     std::weak_ptr<Workspace> jointWorkspace;
 protected:

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());
+}