wip(joints): hinge joint

core/src/datatypes/cframe.cpp

@@ -67,6 +67,10 @@ const Data::String Data::CFrame::ToString() const {
     return std::to_string(X()) + ", " + std::to_string(Y()) + ", " + std::to_string(Z());
 }
 
+Data::CFrame Data::CFrame::pointToward(Vector3 position, Vector3 toward) {
+    return Data::CFrame(position, position + toward, (abs(glm::dot((glm::vec3)toward, glm::vec3(0, 1, 0))) > 0.999) ? glm::vec3(0, 0, 1) : glm::vec3(0, 1, 0));
+}
+
 Data::CFrame::operator glm::mat4() const {
     // Always make sure to translate the position first, then rotate. Matrices work backwards
     return glm::translate(glm::mat4(1.0f), this->translation) * glm::mat4(this->rotation);

core/src/datatypes/cframe.h

@@ -27,6 +27,9 @@ namespace Data {
         CFrame(Vector3 position, Vector3 lookAt, Vector3 up = Vector3(0, 1, 0));
         ~CFrame();
 
+        // Same as CFrame(position, position + toward), but makes sure that up and toward are not linearly dependant
+        static CFrame pointToward(Vector3 position, Vector3 toward);
+
         static const CFrame IDENTITY;
         static const CFrame YToZ;
 

core/src/objects/joint/jointinstance.h

@@ -4,6 +4,7 @@
 #include "../annotation.h"
 #include <memory>
 #include <optional>
+#include <reactphysics3d/engine/PhysicsWorld.h>
 
 //this is necessary ebcause we use std::weak_ptr<Part> without including it in this file
 #ifdef __AUTOGEN_EXTRA_INCLUDES__

core/src/objects/joint/rotate.cpp

@@ -0,0 +1,47 @@
+#include "rotate.h"
+#include "objects/jointsservice.h"
+#include "objects/part.h"
+#include "objects/workspace.h"
+#include <reactphysics3d/constraint/HingeJoint.h>
+
+Rotate::Rotate(): JointInstance(&TYPE) {
+}
+
+Rotate::~Rotate() {
+}
+
+void Rotate::buildJoint() {
+    // Only if both parts are set, are not the same part, are part of a workspace, and are part of the same workspace, we build the joint
+    if (part0.expired() || part1.expired() || part0.lock() == part1.lock() || !workspaceOfPart(part0.lock()) || workspaceOfPart(part0.lock()) != workspaceOfPart(part1.lock())) return;
+
+    // Don't build the joint if we're not part of either a workspace or JointsService
+    if ((!GetParent() || GetParent().value()->GetClass() != &JointsService::TYPE) && !workspace()) return;
+
+    std::shared_ptr<Workspace> workspace = workspaceOfPart(part0.lock()).value();
+    if (!workspace->physicsWorld) return;
+
+    // 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.
+    CFrame newFrame = part0.lock()->cframe * (c1.Inverse() * c0);
+    part1.lock()->cframe = newFrame;
+    workspace->SyncPartPhysics(part1.lock());
+
+    rp::HingeJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (c0.Inverse() * c1).Position(), (part0.lock()->cframe * c0).LookVector().Unit().Abs());
+    this->joint = dynamic_cast<rp::HingeJoint*>(workspace->physicsWorld->createJoint(jointInfo));
+    jointWorkspace = workspace;
+
+    part1.lock()->rigidBody->getCollider(0)->setCollideWithMaskBits(0b10);
+    part1.lock()->rigidBody->getCollider(0)->setCollisionCategoryBits(0b10);
+    part0.lock()->rigidBody->getCollider(0)->setCollideWithMaskBits(0b01);
+    part0.lock()->rigidBody->getCollider(0)->setCollisionCategoryBits(0b01);
+    printf("Bits set\n");
+}
+
+// !!! REMINDER: This has to be called manually when parts are destroyed/removed from the workspace, or joints will linger
+void Rotate::breakJoint() {
+    // If the joint doesn't exist, or its workspace expired (not our problem anymore), then no need to do anything
+    if (!this->joint || jointWorkspace.expired() || !jointWorkspace.lock()->physicsWorld) return;
+
+    jointWorkspace.lock()->physicsWorld->destroyJoint(this->joint);
+    this->joint = nullptr;
+}

core/src/objects/joint/rotate.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "objects/annotation.h"
+#include "objects/base/instance.h"
+#include "objects/joint/jointinstance.h"
+#include <memory>
+
+class INSTANCE Rotate : public JointInstance {
+    AUTOGEN_PREAMBLE
+
+    rp::HingeJoint* joint = nullptr;
+
+    virtual void buildJoint() override;
+    virtual void breakJoint() override;
+public:
+    Rotate();
+    ~Rotate();
+
+    static inline std::shared_ptr<Rotate> New() { return std::make_shared<Rotate>(); };
+    static inline std::shared_ptr<Instance> Create() { return std::make_shared<Rotate>(); };
+};

core/src/objects/joint/snap.cpp

@@ -28,7 +28,7 @@ void Snap::buildJoint() {
 
     // 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.
-    CFrame newFrame = part0.lock()->cframe * (c1.Inverse() * c0);
+    CFrame newFrame = part0.lock()->cframe * (c0 * c1.Inverse());
     part1.lock()->cframe = newFrame;
     workspace->SyncPartPhysics(part1.lock());
 

core/src/objects/joint/weld.cpp

@@ -28,7 +28,7 @@ void Weld::buildJoint() {
 
     // 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.
-    CFrame newFrame = part0.lock()->cframe * (c1.Inverse() * c0);
+    CFrame newFrame = part0.lock()->cframe * (c0 * c1.Inverse());
     part1.lock()->cframe = newFrame;
     workspace->SyncPartPhysics(part1.lock());
 

core/src/objects/part.cpp

@@ -6,10 +6,12 @@
 #include "datatypes/color3.h"
 #include "datatypes/vector.h"
 #include "objects/base/member.h"
+#include "objects/joint/rotate.h"
 #include "objects/joint/weld.h"
 #include "objects/jointsservice.h"
 #include "objects/joint/jointinstance.h"
 #include "objects/joint/snap.h"
+#include "rendering/renderer.h"
 #include "rendering/surface.h"
 #include <memory>
 #include <optional>
@@ -185,6 +187,8 @@ std::optional<std::shared_ptr<JointInstance>> makeJointFromSurfaces(SurfaceType
     || (a == SurfaceInlets && (b == SurfaceStuds || b == SurfaceUniversal))
     || (a == SurfaceUniversal && (b == SurfaceStuds || b == SurfaceInlets || b == SurfaceUniversal)))
         return Snap::New();
+    if (a == SurfaceHinge)
+        return Rotate::New();
     return std::nullopt;
 }
 
@@ -210,6 +214,7 @@ void Part::MakeJoints() {
             Vector3 myWorldNormal = cframe.Rotation() * myFace;
             Vector3 validUp = cframe.Rotation() * Vector3(1,1,1).Unit(); // If myFace == (0, 1, 0), then (0, 1, 0) would produce NaN as up, so we fudge the up so that it works
             CFrame surfaceFrame(cframe.Position(), cframe * (myFace * size), validUp);
+            Vector3 mySurfaceCenter = cframe * (myFace * size);
 
             for (Vector3 otherFace : FACES) {
                 Vector3 otherWorldNormal = otherPart->cframe.Rotation() * otherFace;
@@ -224,13 +229,29 @@ void Part::MakeJoints() {
                 SurfaceType mySurface = surfaceFromFace(faceFromNormal(myFace));
                 SurfaceType otherSurface = surfaceFromFace(faceFromNormal(otherFace));
 
+                // Create contacts
+                // Contact always occurs at the center of Part0's surface (even if that point does not overlap both surfaces)
+                // Contact 0 is Part0's contact relative to Part0. It should point *opposite* the direction of its surface normal
+                // Contact 1 is Part1's contact relative to Part1. It should point directly toward the direction of its surface normal
+
+                // My additional notes:
+                // Contact == Part0.CFrame * C0 == Part1.CFrame * C1
+                // C1 == Part1.CFrame:Inverse() * Part0.CFrame * C0
+                // Part1.CFrame == Part0.CFrame * C0 * C1:Inverse()
+                // C0 == Part0.CFrame:Inverse() * Contact
+
+                CFrame contactPoint = CFrame::pointToward(mySurfaceCenter, -myWorldNormal);
+                CFrame contact0 = cframe.Inverse() * contactPoint;
+                CFrame contact1 = otherPart->cframe.Inverse() * contactPoint;
+                addDebugRenderCFrame(contactPoint);
+
                 auto joint_ = makeJointFromSurfaces(mySurface, otherSurface);
                 if (!joint_) continue;
                 std::shared_ptr<JointInstance> joint = joint_.value();
                 joint->part0 = shared<Part>();
                 joint->part1 = otherPart->shared<Part>();
-                joint->c1 = cframe;
+                joint->c0 = contact0;
-                joint->c0 = otherPart->cframe;
+                joint->c1 = contact1;
                 dataModel().value()->GetService<JointsService>()->AddChild(joint);
                 joint->UpdateProperty("Part0");
 

core/src/objects/workspace.cpp

@@ -86,6 +86,7 @@ void Workspace::SyncPartPhysics(std::shared_ptr<Part> part) {
     part->rigidBody->setLinearVelocity(part->velocity);
     // part->rigidBody->setMass(density * part->size.x * part->size.y * part->size.z);
 
+    printf("Bits unset\n");
     part->rigidBody->setUserData(&*part);
 }
 

core/src/objects/workspace.h

@@ -27,6 +27,7 @@ enum FilterResult {
 class Part;
 class Snap;
 class Weld;
+class Rotate;
 
 typedef std::function<FilterResult(std::shared_ptr<Part>)> RaycastFilter;
 
@@ -39,6 +40,7 @@ class INSTANCE_SERVICE(explorer_icon="workspace") Workspace : public Service {
     friend Part;
     friend Snap;
     friend Weld;
+    friend Rotate;
 protected:
     void InitService() override;
     bool initialized = false;

core/src/rendering/renderer.cpp

@@ -11,6 +11,7 @@
 #include <glm/gtc/quaternion.hpp>
 #include <glm/trigonometric.hpp>
 #include <memory>
+#include <vector>
 
 #include "datatypes/cframe.h"
 #include "objects/handles.h"
@@ -453,12 +454,60 @@ void renderRotationArcs() {
     }
 }
 
+std::vector<CFrame> DEBUG_CFRAMES;
+
+void renderDebugCFrames() {
+    glDepthMask(GL_TRUE);
+    glCullFace(GL_BACK);
+    glFrontFace(GL_CCW); // This is right... Probably.....
+    
+    // Use shader
+    handleShader->use();
+
+    // view/projection transformations
+    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);
+    handleShader->set("sunLight", DirLight {
+        .direction = glm::vec3(-0.2f, -1.0f, -0.3f),
+        .ambient = glm::vec3(0.2f, 0.2f, 0.2f),
+        .diffuse = glm::vec3(0.5f, 0.5f, 0.5f),
+        .specular = glm::vec3(1.0f, 1.0f, 1.0f),
+    });
+    handleShader->set("numPointLights", 0);
+
+    // Pass in the camera position
+    handleShader->set("viewPos", camera.cameraPos);
+
+    for (CFrame frame : DEBUG_CFRAMES) {
+        glm::mat4 model = frame;
+        model = glm::scale(model, glm::vec3(0.5, 0.5, 1.5));
+        handleShader->set("model", model);
+        handleShader->set("material", Material {
+            .diffuse = glm::vec3(0.0f, 0.0f, 1.0f),
+            .specular = glm::vec3(0.5f, 0.5f, 0.5f),
+            .shininess = 16.0f,
+        });
+        glm::mat3 normalMatrix = glm::mat3(glm::transpose(glm::inverse(model)));
+        handleShader->set("normalMatrix", normalMatrix);
+
+        ARROW_MESH->bind();
+        glDrawArrays(GL_TRIANGLES, 0, ARROW_MESH->vertexCount);
+    }
+}
+
+void addDebugRenderCFrame(CFrame frame) {
+    DEBUG_CFRAMES.push_back(frame);
+}
+
 void render(GLFWwindow* window) {
     glClearColor(0.1f, 0.1f, 0.1f, 1.0f);
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
     renderSkyBox();
     renderHandles();
+    renderDebugCFrames();
     renderParts();
     renderOutlines();
     renderRotationArcs();

core/src/rendering/renderer.h

@@ -3,6 +3,9 @@
 
 extern bool wireframeRendering;
 
+namespace Data { class CFrame; };
+
 void renderInit(GLFWwindow* window, int width, int height);
 void render(GLFWwindow* window);
-void setViewport(int width, int height);
+void setViewport(int width, int height);
+void addDebugRenderCFrame(Data::CFrame);

core/src/rendering/surface.h

@@ -16,6 +16,7 @@ enum SurfaceType {
     SurfaceStuds = 3,
     SurfaceInlets = 4,
     SurfaceUniversal = 5,
+    SurfaceHinge = 6,
 };
 
 namespace Data { class Vector3; } using Data::Vector3;

editor/mainglwidget.cpp

@@ -5,6 +5,7 @@
 #include <qsoundeffect.h>
 #include <string>
 #include "mainglwidget.h"
+#include "logger.h"
 #include "mainwindow.h"
 #include "common.h"
 #include "math_helper.h"
@@ -475,6 +476,9 @@ void MainGLWidget::keyPressEvent(QKeyEvent* evt) {
 
     if (evt->key() == Qt::Key_C && getSelection().size() > 0 && !getSelection()[0].expired())
         getSelection()[0].lock()->Clone().value()->SetParent(gWorkspace());
+
+    if (evt->key() == Qt::Key_H && getSelection().size() > 0 && !getSelection()[0].expired())
+        Logger::infof("Object at: 0x%x\n", getSelection()[0].lock().get());
 }
 
 void MainGLWidget::keyReleaseEvent(QKeyEvent* evt) {

editor/mainwindow.cpp

@@ -146,8 +146,8 @@ MainWindow::MainWindow(QWidget *parent)
 
     ui->mdiArea->setTabsClosable(true);
 
-    auto script = Script::New();
+    // auto script = Script::New();
-    gWorkspace()->AddChild(script);
+    // gWorkspace()->AddChild(script);
     // ui->mdiArea->addSubWindow(new ScriptDocument(script));
 }
 

editor/panes/propertiesview.cpp

@@ -348,7 +348,7 @@ void PropertiesView::propertyChanged(QTreeWidgetItem *item, int column) {
     PropertyMeta meta = inst->GetPropertyMeta(propertyName).expect();
 
     if (meta.type == &Data::Bool::TYPE) {
-        inst->SetPropertyValue(propertyName, Data::Bool(item->checkState(1))).expect();
+        inst->SetPropertyValue(propertyName, Data::Bool(item->checkState(1) == Qt::Checked)).expect();
     }
 }
 

editor/placedocument.cpp

@@ -2,6 +2,7 @@
 #include "common.h"
 #include "mainglwidget.h"
 #include "objects/joint/snap.h"
+#include "rendering/surface.h"
 #include <cstdio>
 #include <memory>
 #include <qboxlayout.h>
@@ -49,8 +50,10 @@ void PlaceDocument::closeEvent(QCloseEvent *closeEvent) {
     closeEvent->ignore();
 }
 
+std::shared_ptr<Part> shit;
 static std::chrono::time_point lastTime = std::chrono::steady_clock::now();
 void PlaceDocument::timerEvent(QTimerEvent* evt) {
+    // printf("Is anchored: %d\n", shit->anchored);
     if (evt->timerId() != timer.timerId()) {
         QWidget::timerEvent(evt);
         return;
@@ -65,10 +68,11 @@ void PlaceDocument::timerEvent(QTimerEvent* evt) {
     placeWidget->updateCycle();
 }
 
-std::shared_ptr<Part> lastPart;
+
 void PlaceDocument::init() {
     timer.start(33, this);
 
+    std::shared_ptr<Part> lastPart;
     // Baseplate
     gWorkspace()->AddChild(lastPart = Part::New({
         .position = glm::vec3(0, -5, 0),
@@ -100,13 +104,26 @@ void PlaceDocument::init() {
     gWorkspace()->SyncPartPhysics(lastPart);
     auto part1 = lastPart;
 
-    auto snap = Snap::New();
+    printf("How many times is this called\n");
-    snap->part0 = part0;
+    lastPart = Part::New();
-    snap->part1 = part1;
+    shit = part1;
-    snap->c0 = part1->cframe;
-    snap->c1 = part0->cframe;
 
-    gWorkspace()->AddChild(snap);
+    part0->anchored = true;
-    snap->UpdateProperty("Part0");
+    part0->UpdateProperty("Anchored");
-    snap->UpdateProperty("Part1");
+
+    // auto snap = Snap::New();
+    // snap->part0 = part0;
+    // snap->part1 = part1;
+    // snap->c0 = part1->cframe;
+    // snap->c1 = part0->cframe;
+
+    // gWorkspace()->AddChild(snap);
+    // snap->UpdateProperty("Part0");
+    // snap->UpdateProperty("Part1");
+
+    // part0->backSurface = SurfaceWeld;
+    // part1->frontSurface = SurfaceWeld;
+
+    part0->backSurface = SurfaceHinge;
+    // part1->frontSurface = SurfaceHinge;
 }