fix: buggy handles 2/2 (scaling and moving implemented)

.gitignore

@@ -13,4 +13,7 @@ CMakeLists.txt.user*
 
 # Clangd
 /compile_commands.json
-/.cache
+/.cache
+
+# Gdb
+.gdb_history

core/src/datatypes/cframe.cpp

@@ -2,11 +2,15 @@
 #include "datatypes/vector.h"
 #include "physics/util.h"
 #include <glm/ext/matrix_transform.hpp>
+#include <glm/gtc/matrix_inverse.hpp>
+#include <glm/matrix.hpp>
 #include <reactphysics3d/mathematics/Transform.h>
 #define GLM_ENABLE_EXPERIMENTAL
 #include <glm/gtx/euler_angles.hpp>
 // #include "meta.h" // IWYU pragma: keep
 
+const Data::CFrame Data::CFrame::IDENTITY(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)
     , rotation({
@@ -33,17 +37,13 @@ Data::CFrame::CFrame(const rp::Transform& transform) : Data::CFrame::CFrame(rpTo
 }
 
 glm::mat3 lookAt(Data::Vector3 position, Data::Vector3 lookAt, Data::Vector3 up) {
-    // https://github.com/sgorsten/glm/issues/29#issuecomment-743989030
+    // https://github.com/sgorsten/linalg/issues/29#issuecomment-743989030
 	Data::Vector3 f = (lookAt - position).Unit(); // Forward/Look
 	Data::Vector3 u = up.Unit(); // Up
 	Data::Vector3 s = f.Cross(u).Unit(); // Right
-	u = s.Cross(u);
+	u = s.Cross(f);
 
-	return {
-		{ s.X(), u.X(), -f.X() },
-		{ s.Y(), u.Y(), -f.Y() },
-		{ s.Z(), u.Z(), -f.Z() },
-    };
+	return { s, u, f };
 }
 
 Data::CFrame::CFrame(Data::Vector3 position, Data::Vector3 lookAt, Data::Vector3 up)
@@ -85,8 +85,21 @@ Data::CFrame Data::CFrame::FromEulerAnglesXYZ(Data::Vector3 vector) {
     return Data::CFrame(Data::Vector3::ZERO, glm::column(mat, 2), (Data::Vector3)glm::column(mat, 1)); // Getting LookAt (3rd) and Up (2nd) vectors
 }
 
+Data::CFrame Data::CFrame::Inverse() const {
+    return CFrame { -translation * glm::transpose(glm::inverse(rotation)), glm::inverse(rotation) };
+}
+
+
 // Operators
 
+Data::CFrame Data::CFrame::operator *(Data::CFrame otherFrame) const {
+    return CFrame { this->translation + this->rotation * otherFrame.translation, this->rotation * otherFrame.rotation };
+}
+
+Data::Vector3 Data::CFrame::operator *(Data::Vector3 vector) const {
+    return this->translation + this->rotation * vector;
+}
+
 Data::CFrame Data::CFrame::operator +(Data::Vector3 vector) const {
     return CFrame { this->translation + glm::vec3(vector), this->rotation };
 }

core/src/datatypes/cframe.h

@@ -7,6 +7,8 @@
 #include <glm/ext/vector_float3.hpp>
 #include <glm/fwd.hpp>
 #include <glm/gtc/matrix_access.hpp>
+#include <glm/gtc/matrix_inverse.hpp>
+#include <glm/matrix.hpp>
 #include <reactphysics3d/mathematics/Transform.h>
 #include <reactphysics3d/reactphysics3d.h>
 
@@ -28,6 +30,8 @@ namespace Data {
         CFrame(Data::Vector3 position, Data::Vector3 lookAt, Data::Vector3 up = Data::Vector3(0, 1, 0));
         ~CFrame();
 
+        static const CFrame IDENTITY;
+
         virtual const TypeInfo& GetType() const override;
         static const TypeInfo TYPE;
 
@@ -41,18 +45,21 @@ namespace Data {
         //inline static CFrame identity() { }
         inline Vector3 Position() const { return translation; }
         inline CFrame Rotation() const { return CFrame { glm::vec3(0, 0, 0), rotation }; }
+        CFrame Inverse() const;
         inline float X() const { return translation.x; }
         inline float Y() const { return translation.y; }
         inline float Z() const { return translation.z; }
 
         inline Vector3 RightVector() { return glm::column(rotation, 0); }
         inline Vector3 UpVector() { return glm::column(rotation, 1); }
-        inline Vector3 LookVector() { return glm::column(rotation, 2); }
+        inline Vector3 LookVector() { return -glm::column(rotation, 2); }
 
         Vector3 ToEulerAnglesXYZ();
         static CFrame FromEulerAnglesXYZ(Data::Vector3);
 
         // Operators
+        Data::CFrame operator *(Data::CFrame) const;
+        Data::Vector3 operator *(Data::Vector3) const;
         Data::CFrame operator +(Data::Vector3) const;
         Data::CFrame operator -(Data::Vector3) const;
     };

core/src/datatypes/vector.cpp

@@ -25,6 +25,14 @@ Data::Vector3::operator glm::vec3() const { return vector; };
 Data::Vector3::operator rp::Vector3() const { return rp::Vector3(X(), Y(), Z()); };
 
 // Operators
+Data::Vector3 Data::Vector3::operator *(float scale) const {
+    return Data::Vector3(this->X() * scale, this->Y() * scale, this->Z() * scale);
+}
+
+Data::Vector3 Data::Vector3::operator /(float scale) const {
+    return Data::Vector3(this->X() / scale, this->Y() / scale, this->Z() / scale);
+}
+
 Data::Vector3 Data::Vector3::operator +(Data::Vector3 other) const {
     return Data::Vector3(this->X() + other.X(), this->Y() + other.Y(), this->Z() + other.Z());
 }

core/src/datatypes/vector.h

@@ -40,6 +40,8 @@ namespace Data {
         float Dot(Data::Vector3) const;
     
         // Operators
+        Data::Vector3 operator *(float) const;
+        Data::Vector3 operator /(float) const;
         Data::Vector3 operator +(Data::Vector3) const;
         Data::Vector3 operator -(Data::Vector3) const;
         Data::Vector3 operator -() const;

core/src/math_helper.cpp

@@ -0,0 +1,103 @@
+#include "math_helper.h"
+
+#define CMP_EPSILON 0.00001
+
+// After a long time researching, I was able to use and adapt Godot's implementation of movable handles (godot/editor/plugins/gizmos/gizmo_3d_helper.cpp)
+// All thanks goes to them and David Eberly for his algorithm.
+
+void get_closest_points_between_segments(const glm::vec3 &p_p0, const glm::vec3 &p_p1, const glm::vec3 &p_q0, const glm::vec3 &p_q1, glm::vec3 &r_ps, glm::vec3 &r_qt) {
+    // Based on David Eberly's Computation of Distance Between Line Segments algorithm.
+
+	glm::vec3 p = p_p1 - p_p0;
+	glm::vec3 q = p_q1 - p_q0;
+	glm::vec3 r = p_p0 - p_q0;
+
+	float a = glm::dot(p, p);
+	float b = glm::dot(p, q);
+	float c = glm::dot(q, q);
+	float d = glm::dot(p, r);
+	float e = glm::dot(q, r);
+
+	float s = 0.0f;
+	float t = 0.0f;
+
+	float det = a * c - b * b;
+	if (det > CMP_EPSILON) {
+		// Non-parallel segments
+		float bte = b * e;
+		float ctd = c * d;
+
+		if (bte <= ctd) {
+			// s <= 0.0f
+			if (e <= 0.0f) {
+				// t <= 0.0f
+				s = (-d >= a ? 1 : (-d > 0.0f ? -d / a : 0.0f));
+				t = 0.0f;
+			} else if (e < c) {
+				// 0.0f < t < 1
+				s = 0.0f;
+				t = e / c;
+			} else {
+				// t >= 1
+				s = (b - d >= a ? 1 : (b - d > 0.0f ? (b - d) / a : 0.0f));
+				t = 1;
+			}
+		} else {
+			// s > 0.0f
+			s = bte - ctd;
+			if (s >= det) {
+				// s >= 1
+				if (b + e <= 0.0f) {
+					// t <= 0.0f
+					s = (-d <= 0.0f ? 0.0f : (-d < a ? -d / a : 1));
+					t = 0.0f;
+				} else if (b + e < c) {
+					// 0.0f < t < 1
+					s = 1;
+					t = (b + e) / c;
+				} else {
+					// t >= 1
+					s = (b - d <= 0.0f ? 0.0f : (b - d < a ? (b - d) / a : 1));
+					t = 1;
+				}
+			} else {
+				// 0.0f < s < 1
+				float ate = a * e;
+				float btd = b * d;
+
+				if (ate <= btd) {
+					// t <= 0.0f
+					s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
+					t = 0.0f;
+				} else {
+					// t > 0.0f
+					t = ate - btd;
+					if (t >= det) {
+						// t >= 1
+						s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
+						t = 1;
+					} else {
+						// 0.0f < t < 1
+						s /= det;
+						t /= det;
+					}
+				}
+			}
+		}
+	} else {
+		// Parallel segments
+		if (e <= 0.0f) {
+			s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
+			t = 0.0f;
+		} else if (e >= c) {
+			s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
+			t = 1;
+		} else {
+			s = 0.0f;
+			t = e / c;
+		}
+	}
+
+	r_ps = (1 - s) * p_p0 + s * p_p1;
+	r_qt = (1 - t) * p_q0 + t * p_q1;
+}

core/src/math_helper.h

@@ -0,0 +1,5 @@
+#pragma once
+#include <glm/glm.hpp>
+
+// From godot/editor/plugins/gizmos/gizmo_3d_helper.h
+void get_closest_points_between_segments(const glm::vec3 &p_p0, const glm::vec3 &p_p1, const glm::vec3 &p_q0, const glm::vec3 &p_q1, glm::vec3 &r_ps, glm::vec3 &r_qt);

core/src/objects/handles.cpp

@@ -1,4 +1,5 @@
 #include "handles.h"
+#include "common.h"
 #include "datatypes/cframe.h"
 #include "datatypes/vector.h"
 #include <optional>
@@ -36,10 +37,30 @@ Handles::Handles(): Instance(&TYPE) {
 Data::CFrame Handles::GetCFrameOfHandle(HandleFace face) {
     if (!adornee.has_value() || adornee->expired()) return Data::CFrame(glm::vec3(0,0,0), (Data::Vector3)glm::vec3(0,0,0));
 
-    // return adornee->lock()->cframe + face.normal * 5.f;
-    if (worldMode)
-        return adornee->lock()->cframe + (adornee->lock()->size * 0.5f * face.normal) + face.normal * 5.f;
-    return adornee->lock()->cframe + glm::vec3(glm::mat4(adornee->lock()->cframe.Rotation()) * glm::vec4((adornee->lock()->size * 0.5f * face.normal) + face.normal * 5.f, 0));
+    Data::CFrame localFrame = worldMode ? Data::CFrame::IDENTITY + adornee->lock()->position() : adornee->lock()->cframe;
+
+    // We don't want this to align with local * face.normal, or else we have problems.
+    glm::vec3 upAxis(0, 0, 1);
+    if (glm::abs(glm::dot(glm::vec3(localFrame.Rotation() * face.normal), upAxis)) > 0.9999f)
+        upAxis = glm::vec3(0, 1, 0);
+
+    Data::Vector3 handlePos = localFrame * ((2.f + adornee->lock()->size * 0.5f) * face.normal);
+    Data::CFrame cframe(handlePos, handlePos + localFrame.Rotation() * face.normal, upAxis);
+
+    return cframe;
+}
+
+Data::CFrame Handles::PartCFrameFromHandlePos(HandleFace face, Data::Vector3 newPos) {
+    if (!adornee.has_value() || adornee->expired()) return Data::CFrame(glm::vec3(0,0,0), (Data::Vector3)glm::vec3(0,0,0));
+
+    Data::CFrame localFrame = worldMode ? Data::CFrame::IDENTITY + adornee->lock()->position() : adornee->lock()->cframe;
+    Data::CFrame inverseFrame = localFrame.Inverse();
+
+    Data::Vector3 handlePos = localFrame * ((2.f + adornee->lock()->size * 0.5f) * face.normal);
+
+    // glm::vec3 localPos = inverseFrame * newPos;
+    glm::vec3 newPartPos = newPos - localFrame.Rotation() * ((2.f + adornee->lock()->size * 0.5f) * face.normal);
+    return adornee->lock()->cframe.Rotation() + newPartPos;
 }
 
 std::optional<HandleFace> Handles::RaycastHandle(rp3d::Ray ray) {

core/src/objects/handles.h

@@ -28,6 +28,7 @@ class Handles : public Instance {
 public:
     const static InstanceType TYPE;
 
+    bool active;
     std::optional<std::weak_ptr<Part>> adornee;
     // inline std::optional<std::weak_ptr<Part>> GetAdornee() { return adornee; }
     // inline void SetAdornee(std::optional<std::weak_ptr<Part>> newAdornee) { this->adornee = newAdornee; updateAdornee(); };
@@ -37,6 +38,7 @@ public:
     // World-space handles vs local-space handles
     bool worldMode = false;
     Data::CFrame GetCFrameOfHandle(HandleFace face);
+    Data::CFrame PartCFrameFromHandlePos(HandleFace face, Data::Vector3 newPos);
     std::optional<HandleFace> RaycastHandle(rp3d::Ray ray);
 
     static inline std::shared_ptr<Handles> New() { return std::make_shared<Handles>(); };

core/src/physics/simulation.cpp

@@ -73,7 +73,7 @@ void syncPartPhysics(std::shared_ptr<Part> part) {
 
 void physicsStep(float deltaTime) {
     // Step the simulation a few steps
-    world->update(deltaTime / 2);
+    world->update(std::min(deltaTime / 2, (1/60.f)));
 
     // Naive implementation. Parts are only considered so if they are just under Workspace
     // TODO: Add list of tracked parts in workspace based on their ancestry using inWorkspace property of Instance

core/src/rendering/defaultmeshes.h

@@ -2,5 +2,7 @@
 #include "mesh.h"
 
 extern Mesh* CUBE_MESH;
+extern Mesh* SPHERE_MESH;
+extern Mesh* ARROW_MESH;
 
 void initMeshes();

core/src/rendering/mesh.cpp

@@ -3,7 +3,7 @@
 
 #include "mesh.h"
 
-Mesh::Mesh(int vertexCount, float *vertices) {
+Mesh::Mesh(int vertexCount, float *vertices) : vertexCount(vertexCount) {
     // Generate buffers
     glGenBuffers(1, &VBO);
     glGenVertexArrays(1, &VAO);

core/src/rendering/mesh.h

@@ -4,6 +4,8 @@ class Mesh {
     unsigned int VBO, VAO;
 
 public:
+    int vertexCount;
+
     Mesh(int vertexCount, float* vertices);
     ~Mesh();
     void bind();

core/src/rendering/renderer.cpp

@@ -27,7 +27,8 @@
 
 Shader* shader = NULL;
 Shader* skyboxShader = NULL;
-Shader* handleShader;
+Shader* handleShader = NULL;
+Shader* identityShader = NULL;
 extern Camera camera;
 Skybox* skyboxTexture = NULL;
 Texture3D* studsTexture = NULL;
@@ -61,6 +62,7 @@ void renderInit(GLFWwindow* window, int width, int height) {
     shader = new Shader("assets/shaders/phong.vs", "assets/shaders/phong.fs");
     skyboxShader = new Shader("assets/shaders/skybox.vs", "assets/shaders/skybox.fs");
     handleShader = new Shader("assets/shaders/handle.vs", "assets/shaders/handle.fs");
+    identityShader = new Shader("assets/shaders/identity.vs", "assets/shaders/identity.fs");
 }
 
 void renderParts() {
@@ -72,7 +74,7 @@ void renderParts() {
     // shader->set("lightColor",  glm::vec3(1.0f, 1.0f, 1.0f));
 
     // view/projection transformations
-    glm::mat4 projection = glm::perspective(glm::radians(45.f), (float)viewportWidth / (float)viewportHeight, 0.1f, 100.0f);
+    glm::mat4 projection = glm::perspective(glm::radians(45.f), (float)viewportWidth / (float)viewportHeight, 0.1f, 1000.0f);
     glm::mat4 view = camera.getLookAt();
     shader->set("projection", projection);
     shader->set("view", view);
@@ -114,6 +116,7 @@ void renderParts() {
         if (inst->GetClass()->className != "Part") continue;
         std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(inst);
         glm::mat4 model = part->cframe;
+        if (inst->name == "camera") model = camera.getLookAt();
         model = glm::scale(model, part->size);
         shader->set("model", model);
         shader->set("material", Material {
@@ -135,7 +138,7 @@ void renderSkyBox() {
 
     skyboxShader->use();
 
-    glm::mat4 projection = glm::perspective(glm::radians(45.f), (float)viewportWidth / (float)viewportHeight, 0.1f, 100.0f);
+    glm::mat4 projection = glm::perspective(glm::radians(45.f), (float)viewportWidth / (float)viewportHeight, 0.1f, 1000.0f);
     // Remove translation component of view, making us always at (0, 0, 0)
     glm::mat4 view = glm::mat4(glm::mat3(camera.getLookAt()));
 
@@ -149,9 +152,7 @@ void renderSkyBox() {
 }
 
 void renderHandles() {
-    // if (getSelection().size() == 0) return;
-    // if (getSelection()[0].lock()->GetClass() != &Part::TYPE) return;
-    if (!editorToolHandles->adornee.has_value()) return;
+    if (!editorToolHandles->adornee.has_value() || !editorToolHandles->active) return;
 
     glDepthMask(GL_TRUE);
     
@@ -159,7 +160,7 @@ void renderHandles() {
     handleShader->use();
 
     // view/projection transformations
-    glm::mat4 projection = glm::perspective(glm::radians(45.f), (float)viewportWidth / (float)viewportHeight, 0.1f, 100.0f);
+    glm::mat4 projection = glm::perspective(glm::radians(45.f), (float)viewportWidth / (float)viewportHeight, 0.1f, 1000.0f);
     glm::mat4 view = camera.getLookAt();
     handleShader->set("projection", projection);
     handleShader->set("view", view);
@@ -186,8 +187,29 @@ void renderHandles() {
         glm::mat3 normalMatrix = glm::mat3(glm::transpose(glm::inverse(model)));
         handleShader->set("normalMatrix", normalMatrix);
 
-        CUBE_MESH->bind();
-        glDrawArrays(GL_TRIANGLES, 0, 36);
+        ARROW_MESH->bind();
+        glDrawArrays(GL_TRIANGLES, 0, ARROW_MESH->vertexCount);
+    }
+
+    // 2d square overlay
+    identityShader->use();
+    identityShader->set("aColor", glm::vec3(0.f, 1.f, 1.f));
+    
+    for (auto face : HandleFace::Faces) {
+        Data::CFrame cframe = editorToolHandles->GetCFrameOfHandle(face);
+        glm::vec4 screenPos = projection * view * glm::vec4((glm::vec3)cframe.Position(), 1.0f);
+        glm::vec3 ndcCoords = screenPos / screenPos.w;
+
+        float rad = 5;
+        float xRad = rad * 1/viewportWidth;
+        float yRad = rad * 1/viewportHeight;
+
+        glBegin(GL_QUADS);
+            glVertex3f(ndcCoords.x - xRad, ndcCoords.y - yRad, 0);
+            glVertex3f(ndcCoords.x + xRad, ndcCoords.y - yRad, 0);
+            glVertex3f(ndcCoords.x + xRad, ndcCoords.y + yRad, 0);
+            glVertex3f(ndcCoords.x - xRad, ndcCoords.y + yRad, 0);
+        glEnd();
     }
 }
 
@@ -196,8 +218,8 @@ void render(GLFWwindow* window) {
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
     renderSkyBox();
-    renderParts();
     renderHandles();
+    renderParts();
 }
 
 void setViewport(int width, int height) {

editor/editorcommon.h

@@ -1,10 +1 @@
-#pragma once
-
-enum SelectedTool {
-    SELECT,
-    MOVE,
-    SCALE,
-    ROTATE,
-};
-
-extern SelectedTool selectedTool;
+#pragma once

editor/mainglwidget.cpp

@@ -2,16 +2,22 @@
 #include <chrono>
 
 #include <QMouseEvent>
+#include <glm/common.hpp>
 #include <glm/ext/matrix_projection.hpp>
+#include <glm/ext/matrix_transform.hpp>
 #include <glm/ext/vector_float3.hpp>
 #include <glm/geometric.hpp>
+#include <glm/gtc/round.hpp>
 #include <glm/matrix.hpp>
 #include <memory>
 #include <optional>
 #include <reactphysics3d/collision/RaycastInfo.h>
 #include <vector>
 
+#include "datatypes/cframe.h"
 #include "editorcommon.h"
+#include "mainwindow.h"
+#include "objects/handles.h"
 #include "physics/util.h"
 #include "qcursor.h"
 #include "qevent.h"
@@ -25,19 +31,17 @@
 #include "rendering/shader.h"
 
 #include "mainglwidget.h"
+#include "../core/src/rendering/defaultmeshes.h"
+#include "math_helper.h"
 
 MainGLWidget::MainGLWidget(QWidget* parent): QOpenGLWidget(parent) {
     setFocusPolicy(Qt::FocusPolicy::ClickFocus);
     setMouseTracking(true);
 }
 
-Shader* identityShader;
-
 void MainGLWidget::initializeGL() {
     glewInit();
     renderInit(NULL, width(), height());
-
-    identityShader = new Shader("assets/shaders/identity.vs", "assets/shaders/identity.fs");
 }
 
 extern int vpx, vpy;
@@ -55,6 +59,8 @@ glm::vec2 firstPoint;
 glm::vec2 secondPoint;
 
 extern std::optional<std::weak_ptr<Part>> draggingObject;
+extern std::optional<HandleFace> draggingHandle;
+extern Shader* shader;
 void MainGLWidget::paintGL() {
     ::render(NULL);
 }
@@ -90,36 +96,81 @@ void MainGLWidget::handleObjectDrag(QMouseEvent* evt) {
     syncPartPhysics(draggingObject->lock());
 }
 
+inline glm::vec3 vec3fy(glm::vec4 vec) {
+    return vec / vec.w;
+}
+
 QPoint lastPoint;
 void MainGLWidget::handleHandleDrag(QMouseEvent* evt) {
     QPoint cLastPoint = lastPoint;
     lastPoint = evt->pos();
 
-    if (!isMouseDragging || !draggingHandle || !editorToolHandles->adornee) return;
+    if (!isMouseDragging || !draggingHandle || !editorToolHandles->adornee || !editorToolHandles->active) return;
 
-    // https://stackoverflow.com/a/57380616/16255372
     QPoint position = evt->pos();
-    glm::mat4 projection = glm::perspective(glm::radians(45.f), (float)width() / (float)height(), 0.1f, 100.0f);
-    glm::mat4 view = camera.getLookAt();
-    glm::mat4 worldToScreen = projection * view;
-    glm::vec4 a = worldToScreen * glm::vec4((glm::vec3)editorToolHandles->adornee->lock()->position(), 1);
-    glm::vec4 b = worldToScreen * glm::vec4((glm::vec3)editorToolHandles->adornee->lock()->position() + draggingHandle->normal, 1);
-    glm::vec2 screenDir = b / b.w - a / a.w;
 
-    QPoint mouseDelta_ = evt->pos() - cLastPoint;
-    glm::vec2 mouseDelta(mouseDelta_.x(), mouseDelta_.y() * -1.f);
-    float changeBy = glm::dot(mouseDelta, screenDir);
+    auto part = editorToolHandles->adornee->lock();
 
-    if (selectedTool == SelectedTool::MOVE)
-        editorToolHandles->adornee->lock()->cframe = editorToolHandles->adornee->lock()->cframe + draggingHandle->normal * changeBy;
-    else if (selectedTool == SelectedTool::SCALE)
-        editorToolHandles->adornee->lock()->size += glm::abs(draggingHandle->normal) * changeBy;
+    // This was actually quite a difficult problem to solve, managing to get the handle to go underneath the cursor
+
+    glm::vec3 pointDir = camera.getScreenDirection(glm::vec2(position.x(), position.y()), glm::vec2(width(), height()));
+    pointDir = glm::normalize(pointDir);
+
+    Data::CFrame handleCFrame = editorToolHandles->GetCFrameOfHandle(draggingHandle.value());
+    
+    // Current frame. Identity frame if worldMode == true, selected object's frame if worldMode == false
+    Data::CFrame frame = editorToolHandles->worldMode ? Data::CFrame::IDENTITY + part->position() : part->cframe.Rotation();
+
+    // Segment from axis stretching -4096 to +4096 rel to handle's position 
+    glm::vec3 axisSegment0 = handleCFrame.Position() + (-handleCFrame.LookVector() * 4096.0f);
+    glm::vec3 axisSegment1 = handleCFrame.Position() + (-handleCFrame.LookVector() * -4096.0f);
+
+    // Segment from camera stretching 4096 forward
+    glm::vec3 mouseSegment0 = camera.cameraPos;
+    glm::vec3 mouseSegment1 = camera.cameraPos + pointDir * 4096.0f;
+
+    // Closest point on the axis segment between the two segments
+    glm::vec3 handlePoint, rb;
+    get_closest_points_between_segments(axisSegment0, axisSegment1, mouseSegment0, mouseSegment1, handlePoint, rb);
+
+    // Find new part position
+    glm::vec3 centerPoint = editorToolHandles->PartCFrameFromHandlePos(draggingHandle.value(), handlePoint).Position();
+
+    // Apply snapping in the current frame
+    glm::vec3 diff = centerPoint - (glm::vec3)editorToolHandles->adornee->lock()->position();
+    if (snappingFactor()) diff = frame * (glm::round(glm::vec3(frame.Inverse() * diff) / snappingFactor()) * snappingFactor());
+
+    switch (mainWindow()->selectedTool) {
+        case SelectedTool::SELECT: break;
+        case SelectedTool::MOVE: {
+            // Add difference
+            editorToolHandles->adornee->lock()->cframe = editorToolHandles->adornee->lock()->cframe + diff;
+        } break;
+        
+        case SelectedTool::SCALE: {
+            // Find local difference
+            glm::vec3 localDiff = frame.Inverse() * diff;
+            // Find outwarwd difference
+            localDiff = localDiff * glm::sign(draggingHandle->normal);
+
+            // Add local difference to size
+            part->size += localDiff;
+
+            // If ctrl is not pressed, offset the part by half the size difference to keep the other bound where it was originally
+            if (!(evt->modifiers() & Qt::ControlModifier)) 
+                part->cframe = part->cframe + diff * 0.5f;
+        } break;
+
+        case SelectedTool::ROTATE: {
+            // TODO: Implement rotation
+        } break;
+    }
 
     syncPartPhysics(std::dynamic_pointer_cast<Part>(editorToolHandles->adornee->lock()));
 }
 
 std::optional<HandleFace> MainGLWidget::raycastHandle(glm::vec3 pointDir) {
-    if (!editorToolHandles->adornee.has_value()) return std::nullopt;
+    if (!editorToolHandles->adornee.has_value() || !editorToolHandles->active) return std::nullopt;
     return editorToolHandles->RaycastHandle(rp3d::Ray(glmToRp(camera.cameraPos), glmToRp(glm::normalize(pointDir)) * 50000));
 }
 
@@ -232,4 +283,17 @@ void MainGLWidget::keyPressEvent(QKeyEvent* evt) {
 void MainGLWidget::keyReleaseEvent(QKeyEvent* evt) {
     if (evt->key() == Qt::Key_W || evt->key() == Qt::Key_S) moveZ = 0;
     else if (evt->key() == Qt::Key_A || evt->key() == Qt::Key_D) moveX = 0;
+}
+
+MainWindow* MainGLWidget::mainWindow() {
+    return dynamic_cast<MainWindow*>(window());
+}
+
+float MainGLWidget::snappingFactor() {
+    switch (mainWindow()->snappingMode) {
+        case GridSnappingMode::SNAP_1_STUD: return 1;
+        case GridSnappingMode::SNAP_05_STUDS: return 0.5;
+        case GridSnappingMode::SNAP_OFF: return 0;
+    }
+    return 0;
 }

editor/mainglwidget.h

@@ -1,6 +1,7 @@
 #ifndef MAINGLWIDGET_H
 #define MAINGLWIDGET_H
 
+#include "mainwindow.h"
 #include "objects/part.h"
 #include "qevent.h"
 #include <QOpenGLWidget>
@@ -31,6 +32,9 @@ protected:
     void mouseReleaseEvent(QMouseEvent* evt) override;
     void keyPressEvent(QKeyEvent* evt) override;
     void keyReleaseEvent(QKeyEvent* evt) override;
+
+    MainWindow* mainWindow();
+    float snappingFactor();
 };
 
 #endif // MAINGLWIDGET_H

editor/mainwindow.cpp

@@ -24,8 +24,6 @@
 #include "qobject.h"
 #include "qsysinfo.h"
 
-SelectedTool selectedTool;
-
 bool simulationPlaying = false;
 
 MainWindow::MainWindow(QWidget *parent)
@@ -40,11 +38,18 @@ MainWindow::MainWindow(QWidget *parent)
 
     ConnectSelectionChangeHandler();
 
-    connect(ui->actionToolSelect, &QAction::triggered, this, [&]() { selectedTool = SelectedTool::SELECT; updateSelectedTool(); });
-    connect(ui->actionToolMove, &QAction::triggered, this, [&](bool state) { selectedTool = state ? SelectedTool::MOVE : SelectedTool::SELECT; updateSelectedTool(); });
-    connect(ui->actionToolScale, &QAction::triggered, this, [&](bool state) { selectedTool = state ? SelectedTool::SCALE : SelectedTool::SELECT; updateSelectedTool(); });
-    connect(ui->actionToolRotate, &QAction::triggered, this, [&](bool state) { selectedTool = state ? SelectedTool::ROTATE : SelectedTool::SELECT; updateSelectedTool(); });
+    connect(ui->actionToolSelect, &QAction::triggered, this, [&]() { selectedTool = SelectedTool::SELECT; updateToolbars(); });
+    connect(ui->actionToolMove, &QAction::triggered, this, [&](bool state) { selectedTool = state ? SelectedTool::MOVE : SelectedTool::SELECT; updateToolbars(); });
+    connect(ui->actionToolScale, &QAction::triggered, this, [&](bool state) { selectedTool = state ? SelectedTool::SCALE : SelectedTool::SELECT; updateToolbars(); });
+    connect(ui->actionToolRotate, &QAction::triggered, this, [&](bool state) { selectedTool = state ? SelectedTool::ROTATE : SelectedTool::SELECT; updateToolbars(); });
     ui->actionToolSelect->setChecked(true);
+    selectedTool = SelectedTool::SELECT;
+
+    connect(ui->actionGridSnap1, &QAction::triggered, this, [&]() { snappingMode = GridSnappingMode::SNAP_1_STUD; updateToolbars(); });
+    connect(ui->actionGridSnap05, &QAction::triggered, this, [&]() { snappingMode = GridSnappingMode::SNAP_05_STUDS; updateToolbars(); });
+    connect(ui->actionGridSnapOff, &QAction::triggered, this, [&]() { snappingMode = GridSnappingMode::SNAP_OFF; updateToolbars(); });
+    ui->actionGridSnap1->setChecked(true);
+    snappingMode = GridSnappingMode::SNAP_1_STUD;
 
     connect(ui->actionToggleSimulation, &QAction::triggered, this, [&]() {
         simulationPlaying = !simulationPlaying;
@@ -79,8 +84,22 @@ MainWindow::MainWindow(QWidget *parent)
         ConnectSelectionChangeHandler();
     });
     
+    // Update handles
     addSelectionListener([&](auto oldSelection, auto newSelection, bool fromExplorer) {
-        updateSelectedTool();
+        editorToolHandles->adornee = std::nullopt;
+        if (newSelection.size() == 0) return;
+        InstanceRef inst = newSelection[0].lock();
+        if (inst->GetClass() != &Part::TYPE) return;
+
+        editorToolHandles->adornee = std::dynamic_pointer_cast<Part>(inst);
+    });
+
+    // Update properties
+    addSelectionListener([&](auto oldSelection, auto newSelection, bool fromExplorer) {
+        if (newSelection.size() == 0) return;
+        if (newSelection.size() > 1)
+            ui->propertiesView->setSelected(std::nullopt);
+        ui->propertiesView->setSelected(newSelection[0].lock());
     });
 
     // ui->explorerView->Init(ui);
@@ -100,7 +119,7 @@ MainWindow::MainWindow(QWidget *parent)
 
     workspace()->AddChild(ui->mainWidget->lastPart = Part::New({
         .position = glm::vec3(0),
-        .rotation = glm::vec3(0),
+        .rotation = glm::vec3(0.5, 2, 1),
         .size = glm::vec3(4, 1.2, 2),
         .color = glm::vec3(0.639216f, 0.635294f, 0.647059f),
     }));
@@ -108,14 +127,14 @@ MainWindow::MainWindow(QWidget *parent)
 }
 
 void MainWindow::ConnectSelectionChangeHandler() {
-    connect(ui->explorerView->selectionModel(), &QItemSelectionModel::selectionChanged, this, [&](const QItemSelection &selected, const QItemSelection &deselected) {
-        if (selected.count() == 0) return;
+    // connect(ui->explorerView->selectionModel(), &QItemSelectionModel::selectionChanged, this, [&](const QItemSelection &selected, const QItemSelection &deselected) {
+    //     if (selected.count() == 0) return;
 
-        std::optional<InstanceRef> inst = selected.count() == 0 ? std::nullopt
-            : std::make_optional(((Instance*)selected.indexes()[0].internalPointer())->shared_from_this());
+    //     std::optional<InstanceRef> inst = selected.count() == 0 ? std::nullopt
+    //         : std::make_optional(((Instance*)selected.indexes()[0].internalPointer())->shared_from_this());
 
-        ui->propertiesView->setSelected(inst);
-    });
+    //     ui->propertiesView->setSelected(inst);
+    // });
 }
 
 static std::chrono::time_point lastTime = std::chrono::steady_clock::now();
@@ -134,24 +153,20 @@ void MainWindow::timerEvent(QTimerEvent* evt) {
     ui->mainWidget->updateCycle();
 }
 
-void MainWindow::updateSelectedTool() {
+void MainWindow::updateToolbars() {
     ui->actionToolSelect->setChecked(selectedTool == SelectedTool::SELECT);
     ui->actionToolMove->setChecked(selectedTool == SelectedTool::MOVE);
     ui->actionToolScale->setChecked(selectedTool == SelectedTool::SCALE);
     ui->actionToolRotate->setChecked(selectedTool == SelectedTool::ROTATE);
 
+    ui->actionGridSnap1->setChecked(snappingMode == GridSnappingMode::SNAP_1_STUD);
+    ui->actionGridSnap05->setChecked(snappingMode == GridSnappingMode::SNAP_05_STUDS);
+    ui->actionGridSnapOff->setChecked(snappingMode == GridSnappingMode::SNAP_OFF);
+
+    // editorToolHandles->worldMode = false;
     if (selectedTool == SelectedTool::MOVE) editorToolHandles->worldMode = true;
     if (selectedTool == SelectedTool::SCALE) editorToolHandles->worldMode = false;
-
-    // This code sucks. A lot
-    if (selectedTool == SelectedTool::SELECT) return;
-    if (getSelection().size() == 0) { editorToolHandles->adornee = std::nullopt; return; };
-    InstanceRef inst = getSelection()[0].lock();
-    if (inst->GetClass() != &Part::TYPE) { editorToolHandles->adornee = std::nullopt; return; };
-
-    editorToolHandles->adornee = std::dynamic_pointer_cast<Part>(inst);
-
-    // editorToolHandles->adornee = std::nullopt;
+    editorToolHandles->active = selectedTool != SelectedTool::SELECT;
 }
 
 MainWindow::~MainWindow()

editor/mainwindow.h

@@ -8,6 +8,19 @@
 #include <QMainWindow>
 #include <QLineEdit>
 
+enum SelectedTool {
+    SELECT,
+    MOVE,
+    SCALE,
+    ROTATE,
+};
+
+enum GridSnappingMode {
+    SNAP_1_STUD,
+    SNAP_05_STUDS,
+    SNAP_OFF,
+};
+
 QT_BEGIN_NAMESPACE
 namespace Ui {
 class MainWindow;
@@ -22,11 +35,14 @@ public:
     MainWindow(QWidget *parent = nullptr);
     ~MainWindow();
     
+    SelectedTool selectedTool;
+    GridSnappingMode snappingMode;
+
     Ui::MainWindow *ui;
 private:
     QBasicTimer timer;
 
-    void updateSelectedTool();
+    void updateToolbars();
     void timerEvent(QTimerEvent*) override;
     void ConnectSelectionChangeHandler();
 };

editor/mainwindow.ui

@@ -41,7 +41,7 @@
      <x>0</x>
      <y>0</y>
      <width>1027</width>
-     <height>29</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -115,6 +115,10 @@
    <addaction name="actionToolScale"/>
    <addaction name="actionToolRotate"/>
    <addaction name="separator"/>
+   <addaction name="actionGridSnap1"/>
+   <addaction name="actionGridSnap05"/>
+   <addaction name="actionGridSnapOff"/>
+   <addaction name="separator"/>
    <addaction name="actionToggleSimulation"/>
   </widget>
   <action name="actionAddPart">
@@ -257,6 +261,60 @@
     <string>F5</string>
    </property>
   </action>
+  <action name="actionGridSnap1">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset>
+     <normaloff>assets/icons/editor/snap1.png</normaloff>assets/icons/editor/snap1.png</iconset>
+   </property>
+   <property name="text">
+    <string>1-Stud Snapping</string>
+   </property>
+   <property name="toolTip">
+    <string>Set grid snapping to 1 stud</string>
+   </property>
+   <property name="menuRole">
+    <enum>QAction::MenuRole::NoRole</enum>
+   </property>
+  </action>
+  <action name="actionGridSnap05">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset>
+     <normaloff>assets/icons/editor/snap05.png</normaloff>assets/icons/editor/snap05.png</iconset>
+   </property>
+   <property name="text">
+    <string>1/2-Stud Snapping</string>
+   </property>
+   <property name="toolTip">
+    <string>Set grid snapping to 1/2 studs</string>
+   </property>
+   <property name="menuRole">
+    <enum>QAction::MenuRole::NoRole</enum>
+   </property>
+  </action>
+  <action name="actionGridSnapOff">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset>
+     <normaloff>assets/icons/editor/snapoff.png</normaloff>assets/icons/editor/snapoff.png</iconset>
+   </property>
+   <property name="text">
+    <string>No Grid Snapping</string>
+   </property>
+   <property name="toolTip">
+    <string>Turn grid snapping off</string>
+   </property>
+   <property name="menuRole">
+    <enum>QAction::MenuRole::NoRole</enum>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

editor/panes/explorerview.cpp

@@ -6,6 +6,7 @@
 #include "qabstractitemmodel.h"
 #include "qaction.h"
 #include "qnamespace.h"
+#include <qitemselectionmodel.h>
 
 ExplorerView::ExplorerView(QWidget* parent):
     QTreeView(parent),
@@ -32,7 +33,23 @@ ExplorerView::ExplorerView(QWidget* parent):
         contextMenu.exec(this->viewport()->mapToGlobal(point));
     });
 
+    connect(selectionModel(), &QItemSelectionModel::selectionChanged, this, [&](const QItemSelection &selected, const QItemSelection &deselected) {
+        std::vector<InstanceRefWeak> selectedInstances;
+        selectedInstances.reserve(selected.count()); // This doesn't reserve everything, but should enhance things anyway
+
+        for (auto range : selected) {
+            for (auto index : range.indexes()) {
+                selectedInstances.push_back(reinterpret_cast<Instance*>(index.internalPointer())->weak_from_this());
+            }
+        }
+
+        ::setSelection(selectedInstances, true);
+    });
+
     addSelectionListener([&](auto oldSelection, auto newSelection, bool fromExplorer) {
+        // It's from us, ignore it.
+        if (fromExplorer) return;
+
         this->clearSelection();
         for (InstanceRefWeak inst : newSelection) {
             if (inst.expired()) continue;

tools/genmesh.py

@@ -0,0 +1,78 @@
+#!/usr/bin/python3
+
+# Default mesh generator
+# Input from wavefront obj file
+
+# Usage: ./genmesh.py mesh.obj > mesh.cpp
+
+import sys
+
+HEADER = """
+static float CUBE_VERTICES[] = {
+    // positions                     // normals                  // texture coords
+"""
+
+FOOTER = """
+};"""
+
+file = open(sys.argv[1], "r")
+
+vert_coords = []
+vert_norms = []
+vert_uvs = []
+
+out_vertices = []
+
+min_coords: tuple[float, float, float] | None = None
+max_coords: tuple[float, float, float] | None = None
+
+def normalize(x, y, z):
+    assert min_coords
+    assert max_coords
+    return ((x-max_coords[0])/(max_coords[0]-min_coords[0])+0.5, (y-max_coords[1])/(max_coords[1]-min_coords[1])+0.5, (z-max_coords[2])/(max_coords[2]-min_coords[2])+0.5)
+
+for line in file:
+    if line.startswith('v '):
+        coords = line.split(' ')[1:]
+        coords = (float(coords[0]), float(coords[1]), float(coords[2]))
+        vert_coords.append(coords)
+
+        if not min_coords: min_coords = coords
+        if not max_coords: max_coords = coords
+
+        if coords[0] > max_coords[0]: max_coords = (coords[0], max_coords[1], max_coords[2])
+        if coords[1] > max_coords[1]: max_coords = (max_coords[0], coords[1], max_coords[2])
+        if coords[2] > max_coords[2]: max_coords = (max_coords[0], max_coords[1], coords[2])
+
+        if coords[0] < min_coords[0]: min_coords = (coords[0], min_coords[1], min_coords[2])
+        if coords[1] < min_coords[1]: min_coords = (min_coords[0], coords[1], min_coords[2])
+        if coords[2] < min_coords[2]: min_coords = (min_coords[0], min_coords[1], coords[2])
+
+    if line.startswith('vn '):
+        coords = line.split(' ')[1:]
+        vert_norms.append((float(coords[0]), float(coords[1]), float(coords[2])))
+
+    if line.startswith('vt '):
+        coords = line.split(' ')[1:]
+        vert_uvs.append((float(coords[0]), float(coords[1])))
+
+    if line.startswith('f '):
+        verts = line.split(' ')[1:]
+        for vert in verts:
+            coords, uv, normal = vert.split('/')
+            coords, uv, normal = int(coords), int(uv), int(normal)
+            coords, uv, normal = vert_coords[coords-1], vert_uvs[uv-1], vert_norms[normal-1]
+
+            coords = normalize(*coords)
+            # for coord in [*normal]:
+            #     if coord > greatest_coord: greatest_coord = coord
+            #     if coord < least_coord: least_coord = coord
+
+            out_vertices.append((coords, normal, uv))
+
+print(HEADER)
+
+for coords, normal, uv in out_vertices:
+    print(f"\t{coords[0]}, {coords[1]}, {coords[2]},\t{normal[0]}, {normal[1]}, {normal[2]},\t{uv[0]}, {uv[1]},")
+
+print(FOOTER)