From 59427403be47e863332f59ce371f6b83721b7008 Mon Sep 17 00:00:00 2001 From: maelstrom Date: Wed, 16 Apr 2025 02:24:51 +0200 Subject: [PATCH] feat(joints): added snap joint --- core/src/datatypes/cframe.cpp | 4 ++- core/src/datatypes/cframe.h | 1 + core/src/objects/base/instance.cpp | 40 ++++++++++++++++++-------- core/src/objects/base/instance.h | 4 +++ core/src/objects/meta.cpp | 2 ++ core/src/objects/part.cpp | 6 ++-- core/src/objects/part.h | 2 +- core/src/objects/snap.cpp | 45 ++++++++++++++++++++++++++++++ core/src/objects/snap.h | 28 +++++++++++++++++++ core/src/objects/workspace.cpp | 2 +- core/src/objects/workspace.h | 7 ++++- editor/mainwindow.cpp | 22 ++++++++++++++- 12 files changed, 144 insertions(+), 19 deletions(-) create mode 100644 core/src/objects/snap.cpp create mode 100644 core/src/objects/snap.h diff --git a/core/src/datatypes/cframe.cpp b/core/src/datatypes/cframe.cpp index 814739e..f2eb18b 100644 --- a/core/src/datatypes/cframe.cpp +++ b/core/src/datatypes/cframe.cpp @@ -11,7 +11,9 @@ // #include "meta.h" // IWYU pragma: keep const Data::CFrame Data::CFrame::IDENTITY(glm::vec3(0, 0, 0), glm::mat3(1.f)); - const Data::CFrame Data::CFrame::YToZ(glm::vec3(0, 0, 0), glm::mat3(glm::vec3(1, 0, 0), glm::vec3(0, 0, 1), glm::vec3(0, 1, 0))); +const Data::CFrame Data::CFrame::YToZ(glm::vec3(0, 0, 0), glm::mat3(glm::vec3(1, 0, 0), glm::vec3(0, 0, 1), glm::vec3(0, 1, 0))); + +Data::CFrame::CFrame() : Data::CFrame::CFrame(glm::vec3(0, 0, 0), glm::mat3(1.f)) {} Data::CFrame::CFrame(float x, float y, float z, float R00, float R01, float R02, float R10, float R11, float R12, float R20, float R21, float R22) : translation(x, y, z) diff --git a/core/src/datatypes/cframe.h b/core/src/datatypes/cframe.h index d362085..cdd8c3d 100644 --- a/core/src/datatypes/cframe.h +++ b/core/src/datatypes/cframe.h @@ -20,6 +20,7 @@ namespace Data { // CFrame(float x, float y, float z); // CFrame(const glm::vec3&); // CFrame(const rp::Vector3&); + CFrame(); CFrame(float x, float y, float z, float R00, float R01, float R02, float R10, float R11, float R12, float R20, float R21, float R22); CFrame(const rp::Transform&); CFrame(Data::Vector3 position, glm::quat quat); diff --git a/core/src/objects/base/instance.cpp b/core/src/objects/base/instance.cpp index 5febc99..8822843 100644 --- a/core/src/objects/base/instance.cpp +++ b/core/src/objects/base/instance.cpp @@ -87,24 +87,16 @@ bool Instance::SetParent(std::optional> newParent) { this->OnParentUpdated(lastParent, newParent); - auto oldDataModel = _dataModel; - auto oldWorkspace = _workspace; - - // Update parent data model and workspace, if applicable - if (newParent) { - this->_dataModel = newParent->get()->GetClass() == &DataModel::TYPE ? std::make_optional(std::dynamic_pointer_cast(newParent.value())) : newParent.value()->dataModel(); - this->_workspace = newParent->get()->GetClass() == &Workspace::TYPE ? std::make_optional(std::dynamic_pointer_cast(newParent.value())) : newParent.value()->workspace(); - } else { - this->_dataModel = std::nullopt; - this->_workspace = std::nullopt; - } - updateAncestry(this->shared(), newParent); return true; } void Instance::updateAncestry(std::optional> updatedChild, std::optional> newParent) { + auto oldDataModel = _dataModel; + auto oldWorkspace = _workspace; + + // Update parent data model and workspace, if applicable if (GetParent()) { this->_dataModel = GetParent().value()->GetClass() == &DataModel::TYPE ? std::make_optional(std::dynamic_pointer_cast(GetParent().value())) : GetParent().value()->dataModel(); this->_workspace = GetParent().value()->GetClass() == &Workspace::TYPE ? std::make_optional(std::dynamic_pointer_cast(GetParent().value())) : GetParent().value()->workspace(); @@ -115,6 +107,16 @@ void Instance::updateAncestry(std::optional> updatedCh OnAncestryChanged(updatedChild, newParent); + // Old workspace used to exist, and workspaces differ + if (oldWorkspace.has_value() && !oldWorkspace->expired() && (!_workspace || _workspace->expired() || oldWorkspace->lock() != _workspace->lock())) { + OnWorkspaceRemoved((oldWorkspace.has_value() && !oldWorkspace->expired()) ? std::make_optional(oldWorkspace->lock()) : std::nullopt); + } + + // New workspace exists, and workspaces differ + if (_workspace.has_value() && !_workspace->expired() && (!oldWorkspace || oldWorkspace->expired() || _workspace->lock() != oldWorkspace->lock())) { + OnWorkspaceAdded((oldWorkspace.has_value() && !oldWorkspace->expired()) ? std::make_optional(oldWorkspace->lock()) : std::nullopt, _workspace->lock()); + } + // Update ancestry in descendants for (InstanceRef child : children) { child->updateAncestry(updatedChild, newParent); @@ -156,6 +158,14 @@ void Instance::OnAncestryChanged(std::optional> child, // Empty stub } +void Instance::OnWorkspaceAdded(std::optional> oldWorkspace, std::shared_ptr newWorkspace) { + // Empty stub +} + +void Instance::OnWorkspaceRemoved(std::optional> oldWorkspace) { + // Empty stub +} + // Properties result Instance::GetPropertyValue(std::string name) { @@ -198,6 +208,12 @@ result Instance::GetPropertyMeta(std::string name) } } +void Instance::UpdateProperty(std::string name) { + PropertyMeta meta = GetPropertyMeta(name).expect(); + if (!meta.updateCallback) return; // Nothing to update, exit. + meta.updateCallback.value()(name); +} + std::vector Instance::GetProperties() { if (cachedMemberList.has_value()) return cachedMemberList.value(); diff --git a/core/src/objects/base/instance.h b/core/src/objects/base/instance.h index 56be990..21ab401 100644 --- a/core/src/objects/base/instance.h +++ b/core/src/objects/base/instance.h @@ -64,6 +64,8 @@ protected: virtual void OnParentUpdated(std::optional> oldParent, std::optional> newParent); virtual void OnAncestryChanged(std::optional> child, std::optional> newParent); + virtual void OnWorkspaceAdded(std::optional> oldWorkspace, std::shared_ptr newWorkspace); + virtual void OnWorkspaceRemoved(std::optional> oldWorkspace); // The root data model this object is a descendant of std::optional> dataModel(); @@ -93,6 +95,8 @@ public: result GetPropertyValue(std::string name); fallible SetPropertyValue(std::string name, Data::Variant value); result GetPropertyMeta(std::string name); + // Manually trigger the update of a property. Useful internally when setting properties directly + void UpdateProperty(std::string name); // Returning a list of property names feels kinda janky. Is this really the way to go? std::vector GetProperties(); diff --git a/core/src/objects/meta.cpp b/core/src/objects/meta.cpp index 878e9b8..f836ad6 100644 --- a/core/src/objects/meta.cpp +++ b/core/src/objects/meta.cpp @@ -1,5 +1,6 @@ #include "meta.h" #include "objects/part.h" +#include "objects/snap.h" #include "objects/workspace.h" std::map INSTANCE_MAP = { @@ -7,4 +8,5 @@ std::map INSTANCE_MAP = { { "Part", &Part::TYPE }, { "Workspace", &Workspace::TYPE }, { "DataModel", &DataModel::TYPE }, + { "Snap", &Snap::TYPE }, }; \ No newline at end of file diff --git a/core/src/objects/part.cpp b/core/src/objects/part.cpp index d47aa39..3935e2a 100644 --- a/core/src/objects/part.cpp +++ b/core/src/objects/part.cpp @@ -61,7 +61,7 @@ const InstanceType* Part::GetClass() { Part::Part(): Part(PartConstructParams { .color = Data::Color3(0.639216f, 0.635294f, 0.647059f) }) { } -Part::Part(PartConstructParams params): Instance(&TYPE), cframe(Data::CFrame(params.position, params.rotation)), +Part::Part(PartConstructParams params): Instance(&TYPE), cframe(Data::CFrame::FromEulerAnglesXYZ((Data::Vector3)params.rotation) + params.position), size(params.size), color(params.color), anchored(params.anchored), locked(params.locked) { this->memberMap = std::make_unique(MemberMap { .super = std::move(this->memberMap), @@ -158,8 +158,10 @@ Part::Part(PartConstructParams params): Instance(&TYPE), cframe(Data::CFrame(par Part::~Part() { // This relies on physicsCommon still existing. Be very careful. - if (this->rigidBody && workspace()) + if (this->rigidBody && workspace()) { workspace().value()->DestroyRigidBody(rigidBody); + this->rigidBody = nullptr; + } } diff --git a/core/src/objects/part.h b/core/src/objects/part.h index 530eb0b..9b52feb 100644 --- a/core/src/objects/part.h +++ b/core/src/objects/part.h @@ -15,7 +15,7 @@ namespace rp = reactphysics3d; // For easy construction from C++. Maybe should be removed? struct PartConstructParams { glm::vec3 position; - glm::quat rotation = glm::identity(); + glm::vec3 rotation; glm::vec3 size; Data::Color3 color; diff --git a/core/src/objects/snap.cpp b/core/src/objects/snap.cpp new file mode 100644 index 0000000..6c90c83 --- /dev/null +++ b/core/src/objects/snap.cpp @@ -0,0 +1,45 @@ +#include "snap.h" + +#include "datatypes/vector.h" +#include "workspace.h" +#include "part.h" +#include + +const InstanceType Snap::TYPE = { + .super = &Instance::TYPE, + .className = "Snap", + .constructor = &Snap::Create, +}; + +const InstanceType* Snap::GetClass() { + return &TYPE; +} + +Snap::Snap(): Instance(&TYPE) { +} + +Snap::~Snap() { +} + +void Snap::OnWorkspaceAdded(std::optional> oldWorkspace, std::shared_ptr newWorkspace) { + if (!part0 || !part1 || part0->expired() || part1->expired()) return; + + printVec((part0->lock()->cframe * (c1.Inverse() * c0)).Rotation().ToEulerAnglesXYZ()); + printVec(part1->lock()->cframe.Rotation().ToEulerAnglesXYZ()); + + // 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; + newWorkspace->SyncPartPhysics(part1->lock()); + + rp::FixedJointInfo jointInfo(part0->lock()->rigidBody, part1->lock()->rigidBody, (c0.Inverse() * c1).Position()); + this->joint = dynamic_cast(workspace().value()->physicsWorld->createJoint(jointInfo)); +} + +void Snap::OnWorkspaceRemoved(std::optional> oldWorkspace) { + if (!this->joint || !oldWorkspace) return; + + oldWorkspace.value()->physicsWorld->destroyJoint(this->joint); + this->joint = nullptr; +} \ No newline at end of file diff --git a/core/src/objects/snap.h b/core/src/objects/snap.h new file mode 100644 index 0000000..c8e8f17 --- /dev/null +++ b/core/src/objects/snap.h @@ -0,0 +1,28 @@ +#pragma once + +#include "objects/base/instance.h" +#include +#include + +class Part; + +class Snap : public Instance { + rp::FixedJoint* joint; +protected: + void OnWorkspaceAdded(std::optional> oldWorkspace, std::shared_ptr newWorkspace) override; + void OnWorkspaceRemoved(std::optional> oldWorkspace) override; +public: + const static InstanceType TYPE; + + std::optional> part0; + std::optional> part1; + Data::CFrame c0; + Data::CFrame c1; + + Snap(); + ~Snap(); + + static inline std::shared_ptr New() { return std::make_shared(); }; + static inline std::shared_ptr Create() { return std::make_shared(); }; + virtual const InstanceType* GetClass() override; +}; \ No newline at end of file diff --git a/core/src/objects/workspace.cpp b/core/src/objects/workspace.cpp index 6e71cef..4811001 100644 --- a/core/src/objects/workspace.cpp +++ b/core/src/objects/workspace.cpp @@ -15,7 +15,7 @@ const InstanceType* Workspace::GetClass() { return &TYPE; } -rp::PhysicsCommon *physicsCommon = new rp::PhysicsCommon; +rp::PhysicsCommon* Workspace::physicsCommon = new rp::PhysicsCommon; Workspace::Workspace(): Service(&TYPE) { } diff --git a/core/src/objects/workspace.h b/core/src/objects/workspace.h index b675b84..2295588 100644 --- a/core/src/objects/workspace.h +++ b/core/src/objects/workspace.h @@ -24,11 +24,16 @@ enum FilterResult { }; class Part; +class Snap; + typedef std::function)> RaycastFilter; class Workspace : public Service { - rp::PhysicsWorld *physicsWorld = nullptr; + rp::PhysicsWorld* physicsWorld = nullptr; + static rp::PhysicsCommon* physicsCommon; + friend Part; + friend Snap; protected: void InitService() override; bool initialized = false; diff --git a/editor/mainwindow.cpp b/editor/mainwindow.cpp index 18148dd..d38c2d8 100644 --- a/editor/mainwindow.cpp +++ b/editor/mainwindow.cpp @@ -1,6 +1,7 @@ #include "mainwindow.h" #include "./ui_mainwindow.h" #include "common.h" +#include "objects/snap.h" #include #include #include @@ -123,11 +124,30 @@ MainWindow::MainWindow(QWidget *parent) gWorkspace()->AddChild(ui->mainWidget->lastPart = Part::New({ .position = glm::vec3(0), - .rotation = glm::vec3(0.5, 2, 1), + .rotation = glm::vec3(-2.6415927, 1.1415926, 2.57075), .size = glm::vec3(4, 1.2, 2), .color = glm::vec3(0.639216f, 0.635294f, 0.647059f), })); gWorkspace()->SyncPartPhysics(ui->mainWidget->lastPart); + // auto part0 = ui->mainWidget->lastPart; + + // gWorkspace()->AddChild(ui->mainWidget->lastPart = Part::New({ + // .position = glm::vec3(1.6691498, 0.82489049, -0.73040605), + // // .rotation = glm::vec3(0.5, 2, 1), + // .rotation = glm::vec3(-2.6415927, 1.1415926, -2.141639), + // .size = glm::vec3(4, 1.2, 2), + // .color = glm::vec3(0.639216f, 0.635294f, 0.647059f), + // })); + // gWorkspace()->SyncPartPhysics(ui->mainWidget->lastPart); + // auto part1 = ui->mainWidget->lastPart; + + // auto snap = Snap::New(); + // snap->part0 = part0; + // snap->part1 = part1; + // snap->c0 = part1->cframe; + // snap->c1 = part0->cframe; + + // gWorkspace()->AddChild(snap); } void MainWindow::closeEvent(QCloseEvent* evt) {