From 4005bf1cb555901d8e080ef7851920cc010877d1 Mon Sep 17 00:00:00 2001 From: maelstrom Date: Wed, 23 Apr 2025 12:38:41 +0200 Subject: [PATCH] fix(joints): break joint when member part is updated/destroyed --- core/src/objects/base/instance.cpp | 19 ++++---- core/src/objects/base/instance.h | 1 + core/src/objects/part.cpp | 75 ++++++++++++++++++++++++++++++ core/src/objects/part.h | 16 +++++++ core/src/objects/snap.cpp | 25 ++++++++++ core/src/objects/snap.h | 3 ++ core/src/ptr_helpers.h | 14 ++++++ 7 files changed, 142 insertions(+), 11 deletions(-) create mode 100644 core/src/ptr_helpers.h diff --git a/core/src/objects/base/instance.cpp b/core/src/objects/base/instance.cpp index f2f64c5..c04f4e1 100644 --- a/core/src/objects/base/instance.cpp +++ b/core/src/objects/base/instance.cpp @@ -17,6 +17,7 @@ #include #include #include +#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 -bool operator ==(std::optional> a, std::optional> 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 -bool operator ==(std::weak_ptr a, std::weak_ptr b) { - return a.expired() && b.expired() || (!a.expired() && !b.expired() && a.lock() == b.lock()); -} - template std::weak_ptr optional_to_weak(std::optional> a) { return a ? a.value() : std::weak_ptr(); @@ -151,6 +141,13 @@ std::optional> 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 DUMMY_INSTANCE; DescendantsIterator Instance::GetDescendantsStart() { return DescendantsIterator(GetChildren().size() > 0 ? GetChildren()[0] : DUMMY_INSTANCE); diff --git a/core/src/objects/base/instance.h b/core/src/objects/base/instance.h index 3ff9f9c..bc5c85c 100644 --- a/core/src/objects/base/instance.h +++ b/core/src/objects/base/instance.h @@ -91,6 +91,7 @@ public: std::optional> GetParent(); bool IsParentLocked(); inline const std::vector> GetChildren() { return children; } + void Destroy(); DescendantsIterator GetDescendantsStart(); DescendantsIterator GetDescendantsEnd(); diff --git a/core/src/objects/part.cpp b/core/src/objects/part.cpp index 3935e2a..c6ffd5d 100644 --- a/core/src/objects/part.cpp +++ b/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 #include @@ -172,12 +173,18 @@ void Part::OnAncestryChanged(std::optional> child, std if (workspace()) workspace().value()->SyncPartPhysics(std::dynamic_pointer_cast(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(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 joint : primaryJoints) { + if (joint.expired()) continue; + joint.lock()->Destroy(); + } + + for (std::weak_ptr joint : secondaryJoints) { + if (joint.expired()) continue; + joint.lock()->Destroy(); + } +} + +void Part::trackJoint(std::shared_ptr 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 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++; + } } \ No newline at end of file diff --git a/core/src/objects/part.h b/core/src/objects/part.h index 9b52feb..68fc1f5 100644 --- a/core/src/objects/part.h +++ b/core/src/objects/part.h @@ -9,6 +9,7 @@ #include "objects/base/instance.h" #include "rendering/surface.h" #include +#include 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> primaryJoints; + // Joints where this part is Part1 + std::vector> secondaryJoints; + + void trackJoint(std::shared_ptr); + void untrackJoint(std::shared_ptr); + + friend Snap; + void OnAncestryChanged(std::optional> child, std::optional> 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(); }; \ No newline at end of file diff --git a/core/src/objects/snap.cpp b/core/src/objects/snap.cpp index a49e603..ab75b2b 100644 --- a/core/src/objects/snap.cpp +++ b/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 #include #include +#include "ptr_helpers.h" const InstanceType Snap::TYPE = { .super = &Instance::TYPE, @@ -59,10 +61,33 @@ void Snap::OnAncestryChanged(std::optional>, 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()); + } + + if (part1 != oldPart1 && !oldPart1.expired()) { + oldPart1.lock()->untrackJoint(shared()); + } + + // Parts differ, add + if (part0 != oldPart0 && !part0.expired()) { + part0.lock()->trackJoint(shared()); + } + + if (part1 != oldPart1 && !part1.expired()) { + part1.lock()->trackJoint(shared()); + } + // Destroy and rebuild the joint, if applicable breakJoint(); buildJoint(); + + oldPart0 = part0; + oldPart1 = part1; } void Snap::buildJoint() { diff --git a/core/src/objects/snap.h b/core/src/objects/snap.h index 2dd83ae..f61b10e 100644 --- a/core/src/objects/snap.h +++ b/core/src/objects/snap.h @@ -10,6 +10,9 @@ class Workspace; class Snap : public Instance { rp::FixedJoint* joint = nullptr; + std::weak_ptr oldPart0; + std::weak_ptr oldPart1; + // The workspace the joint was created in, if it exists std::weak_ptr jointWorkspace; protected: diff --git a/core/src/ptr_helpers.h b/core/src/ptr_helpers.h new file mode 100644 index 0000000..1f749a6 --- /dev/null +++ b/core/src/ptr_helpers.h @@ -0,0 +1,14 @@ +#pragma once +#include +#include + +template +bool operator ==(std::optional> a, std::optional> 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 +bool operator ==(std::weak_ptr a, std::weak_ptr b) { + return a.expired() && b.expired() || (!a.expired() && !b.expired() && a.lock() == b.lock()); +} \ No newline at end of file