fix(editor): dragging objects starts from the initial drag point

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)

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);

core/src/datatypes/vector.cpp

@@ -3,6 +3,7 @@
 #include <string>
 #include "meta.h" // IWYU pragma: keep
 
+Data::Vector3::Vector3() : vector(glm::vec3(0, 0, 0)) {};
 Data::Vector3::Vector3(const glm::vec3& src) : vector(src) {};
 Data::Vector3::Vector3(const rp::Vector3& src) : vector(glm::vec3(src.x, src.y, src.z)) {};
 Data::Vector3::Vector3(float x, const float y, float z) : vector(glm::vec3(x, y, z)) {};

core/src/datatypes/vector.h

@@ -12,6 +12,7 @@ namespace Data {
         glm::vec3 vector;
     
     public:
+        Vector3();
         Vector3(float x, float y, float z);
         Vector3(const glm::vec3&);
         Vector3(const rp::Vector3&);

core/src/objects/base/instance.cpp

@@ -87,24 +87,16 @@ bool Instance::SetParent(std::optional<std::shared_ptr<Instance>> 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<DataModel>(newParent.value())) : newParent.value()->dataModel();
-        this->_workspace = newParent->get()->GetClass() == &Workspace::TYPE ? std::make_optional(std::dynamic_pointer_cast<Workspace>(newParent.value())) : newParent.value()->workspace();
-    } else {
-        this->_dataModel = std::nullopt;
-        this->_workspace = std::nullopt;
-    }
-
     updateAncestry(this->shared<Instance>(), newParent);
 
     return true;
 }
 
 void Instance::updateAncestry(std::optional<std::shared_ptr<Instance>> updatedChild, std::optional<std::shared_ptr<Instance>> 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<DataModel>(GetParent().value())) : GetParent().value()->dataModel();
         this->_workspace = GetParent().value()->GetClass() == &Workspace::TYPE ? std::make_optional(std::dynamic_pointer_cast<Workspace>(GetParent().value())) : GetParent().value()->workspace();
@@ -115,6 +107,16 @@ void Instance::updateAncestry(std::optional<std::shared_ptr<Instance>> 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<std::shared_ptr<Instance>> child,
     // Empty stub
 }
 
+void Instance::OnWorkspaceAdded(std::optional<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) {
+    // Empty stub
+}
+
+void Instance::OnWorkspaceRemoved(std::optional<std::shared_ptr<Workspace>> oldWorkspace) {
+    // Empty stub
+}
+
 // Properties
 
 result<Data::Variant, MemberNotFound> Instance::GetPropertyValue(std::string name) {
@@ -198,6 +208,12 @@ result<PropertyMeta, MemberNotFound> 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<std::string> Instance::GetProperties() {
     if (cachedMemberList.has_value()) return cachedMemberList.value();
 

core/src/objects/base/instance.h

@@ -64,6 +64,8 @@ protected:
 
     virtual void OnParentUpdated(std::optional<std::shared_ptr<Instance>> oldParent, std::optional<std::shared_ptr<Instance>> newParent);
     virtual void OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent);
+    virtual void OnWorkspaceAdded(std::optional<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> newWorkspace);
+    virtual void OnWorkspaceRemoved(std::optional<std::shared_ptr<Workspace>> oldWorkspace);
 
     // The root data model this object is a descendant of
     std::optional<std::shared_ptr<DataModel>> dataModel();
@@ -93,6 +95,8 @@ public:
     result<Data::Variant, MemberNotFound> GetPropertyValue(std::string name);
     fallible<MemberNotFound, AssignToReadOnlyMember> SetPropertyValue(std::string name, Data::Variant value);
     result<PropertyMeta, MemberNotFound> 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<std::string> GetProperties();
 

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<std::string, const InstanceType*> INSTANCE_MAP = {
@@ -7,4 +8,5 @@ std::map<std::string, const InstanceType*> INSTANCE_MAP = {
     { "Part", &Part::TYPE },
     { "Workspace", &Workspace::TYPE },
     { "DataModel", &DataModel::TYPE },
+    { "Snap", &Snap::TYPE },
 };

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

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::quat>();
+    glm::vec3 rotation;
     glm::vec3 size;
     Data::Color3 color;
     

core/src/objects/snap.cpp

@@ -0,0 +1,45 @@
+#include "snap.h"
+
+#include "datatypes/vector.h"
+#include "workspace.h"
+#include "part.h"
+#include <reactphysics3d/constraint/FixedJoint.h>
+
+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<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> 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<rp::FixedJoint*>(workspace().value()->physicsWorld->createJoint(jointInfo));
+}
+
+void Snap::OnWorkspaceRemoved(std::optional<std::shared_ptr<Workspace>> oldWorkspace) {
+    if (!this->joint || !oldWorkspace) return;
+
+    oldWorkspace.value()->physicsWorld->destroyJoint(this->joint);
+    this->joint = nullptr;
+}

core/src/objects/snap.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "objects/base/instance.h"
+#include <memory>
+#include <optional>
+
+class Part;
+
+class Snap : public Instance {
+    rp::FixedJoint* joint;
+protected:
+    void OnWorkspaceAdded(std::optional<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) override;
+    void OnWorkspaceRemoved(std::optional<std::shared_ptr<Workspace>> oldWorkspace) override;
+public:
+    const static InstanceType TYPE;
+
+    std::optional<std::weak_ptr<Part>> part0;
+    std::optional<std::weak_ptr<Part>> part1;
+    Data::CFrame c0;
+    Data::CFrame c1;
+
+    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/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) {
 }

core/src/objects/workspace.h

@@ -24,11 +24,16 @@ enum FilterResult {
 };
 
 class Part;
+class Snap;
+
 typedef std::function<FilterResult(std::shared_ptr<Part>)> 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;

editor/mainglwidget.cpp

@@ -107,6 +107,9 @@ Data::CFrame snapCFrame(Data::CFrame frame) {
 bool isMouseDragging = false;
 std::optional<std::weak_ptr<Part>> draggingObject;
 std::optional<HandleFace> draggingHandle;
+Data::Vector3 initialHitPos;
+Data::Vector3 initialHitNormal;
+Data::CFrame initialFrame;
 void MainGLWidget::handleObjectDrag(QMouseEvent* evt) {
     if (!isMouseDragging || !draggingObject || mainWindow()->selectedTool >= TOOL_SMOOTH) return;
 
@@ -119,10 +122,13 @@ void MainGLWidget::handleObjectDrag(QMouseEvent* evt) {
     
     if (!rayHit) return;
 
-    glm::vec3 partSize = partFromBody(rayHit->body)->size;
-    Data::Vector3 vec = rayHit->worldPoint;
     Data::CFrame targetFrame = partFromBody(rayHit->body)->cframe;
     Data::Vector3 surfaceNormal = targetFrame.Inverse().Rotation() * rayHit->worldNormal;
+    Data::Vector3 inverseSurfaceNormal = Data::Vector3::ONE - surfaceNormal.Abs();
+    glm::vec3 partSize = partFromBody(rayHit->body)->size;
+    Data::Vector3 tFormedHitPos = targetFrame * ((targetFrame.Inverse() * initialHitPos) * inverseSurfaceNormal);
+    Data::Vector3 tFormedInitialPos = targetFrame * ((targetFrame.Inverse() * initialFrame.Position()) * inverseSurfaceNormal);
+    Data::Vector3 vec = rayHit->worldPoint + (tFormedInitialPos - tFormedHitPos);
     // The part being dragged's frame local to the hit target's frame, but without its position component
     // To find a world vector local to the new frame, use newFrame, not localFrame, as localFrame is localFrame is local to targetFrame in itself
     Data::CFrame localFrame = (targetFrame.Inverse() * (draggingObject->lock()->cframe.Rotation() + vec));
@@ -132,7 +138,6 @@ void MainGLWidget::handleObjectDrag(QMouseEvent* evt) {
 
     // Snap to studs
     Data::Vector3 draggingPartSize = draggingObject->lock()->size;
-    Data::Vector3 inverseSurfaceNormal = Data::Vector3::ONE - surfaceNormal.Abs();
     glm::vec3 inverseNormalPartSize = (Data::Vector3)(partSize - glm::vec3(localFrame.Rotation() * draggingPartSize)) * inverseSurfaceNormal / 2.f;
     if (snappingFactor() > 0)
         localFrame = localFrame.Rotation() + glm::round(glm::vec3(localFrame.Position() * inverseSurfaceNormal - inverseNormalPartSize) / snappingFactor()) * snappingFactor() + inverseNormalPartSize
@@ -146,6 +151,7 @@ void MainGLWidget::handleObjectDrag(QMouseEvent* evt) {
 
     draggingObject->lock()->cframe = newFrame + unsinkOffset;
 
+
     gWorkspace()->SyncPartPhysics(draggingObject->lock());
     sendPropertyUpdatedSignal(draggingObject->lock(), "Position", draggingObject->lock()->position());
 }
@@ -231,7 +237,6 @@ void MainGLWidget::handleLinearTransform(QMouseEvent* evt) {
 
 // Also implemented based on Godot: [c7ea8614](godot/editor/plugins/canvas_item_editor_plugin.cpp#L1490)
 glm::vec2 startPoint;
-Data::CFrame initialFrame = Data::CFrame::IDENTITY;
 void MainGLWidget::handleRotationalTransform(QMouseEvent* evt) {
     if (!isMouseDragging || !draggingHandle || !editorToolHandles->adornee || !editorToolHandles->active) return;
 
@@ -348,6 +353,9 @@ void MainGLWidget::mousePressEvent(QMouseEvent* evt) {
         if (!rayHit || !partFromBody(rayHit->body)) return;
         std::shared_ptr<Part> part = partFromBody(rayHit->body);
         if (part->locked) return;
+        initialFrame = part->cframe;
+        initialHitPos = rayHit->worldPoint;
+        initialHitNormal = rayHit->worldNormal;
         
         // Handle surface tool
         if (mainWindow()->selectedTool >= TOOL_SMOOTH) {

editor/mainwindow.cpp

@@ -1,6 +1,7 @@
 #include "mainwindow.h"
 #include "./ui_mainwindow.h"
 #include "common.h"
+#include "objects/snap.h"
 #include <memory>
 #include <qclipboard.h>
 #include <qmessagebox.h>
@@ -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) {