From 5a17c1289badac94b8921c3e1641162d800fa83c Mon Sep 17 00:00:00 2001
From: maelstrom <maelstrom@maelstrom.dev>
Date: Sun, 29 Sep 2024 19:05:13 +0200
Subject: [PATCH] fysiken

---
 src/part.cpp               | 27 +++++++++++++++++++++++++
 src/part.h                 |  8 ++++++++
 src/physics/simulation.cpp | 41 +++++++++++++++++++++++++++++++++-----
 src/physics/simulation.h   |  3 +++
 src/physics/util.h         | 15 ++++++++++++++
 5 files changed, 89 insertions(+), 5 deletions(-)
 create mode 100644 src/part.cpp
 create mode 100644 src/physics/util.h

diff --git a/src/part.cpp b/src/part.cpp
new file mode 100644
index 0000000..73dbfa2
--- /dev/null
+++ b/src/part.cpp
@@ -0,0 +1,27 @@
+#include <glm/ext.hpp>
+#include <reactphysics3d/engine/PhysicsCommon.h>
+#include <reactphysics3d/engine/PhysicsWorld.h>
+#include "physics/util.h"
+
+#include "part.h"
+
+namespace rp = reactphysics3d;
+
+extern rp::PhysicsWorld* world;
+extern rp::PhysicsCommon physicsCommon;
+
+void Part::syncTransforms() {
+    glm::mat4 rotMat = glm::mat4(1.0f);
+    rotMat = glm::rotate(rotMat, rotation.x, glm::vec3(1., 0., 0.));
+    rotMat = glm::rotate(rotMat, rotation.y, glm::vec3(0., 1., 0.));
+    rotMat = glm::rotate(rotMat, rotation.z, glm::vec3(0., 0., 1.));
+    glm::quat quat(rotMat);
+
+    rp::Transform transform(glmToRp(position), glmToRp(quat));
+    rigidBody = world->createRigidBody(transform);
+
+    rp::BoxShape* shape = physicsCommon.createBoxShape(glmToRp(scale * glm::vec3(0.5)));
+
+    rigidBody->removeCollider(rigidBody->getCollider(0));
+    rigidBody->addCollider(shape, rp::Transform());
+}
\ No newline at end of file
diff --git a/src/part.h b/src/part.h
index 8da119b..f812f01 100644
--- a/src/part.h
+++ b/src/part.h
@@ -1,10 +1,18 @@
 #pragma once
 #include <glm/glm.hpp>
+#include <reactphysics3d/body/RigidBody.h>
 #include "rendering/material.h"
 
+namespace rp = reactphysics3d;
+
 struct Part {
     glm::vec3 position;
     glm::vec3 rotation;
     glm::vec3 scale;
     Material material;
+    
+    bool anchored = false;
+    rp::RigidBody* rigidBody = nullptr;
+
+    void syncTransforms();
 };
\ No newline at end of file
diff --git a/src/physics/simulation.cpp b/src/physics/simulation.cpp
index 0ae82dc..63dd71a 100644
--- a/src/physics/simulation.cpp
+++ b/src/physics/simulation.cpp
@@ -1,6 +1,17 @@
+#include <cstdio>
+#include <glm/ext/matrix_float3x3.hpp>
+#include <reactphysics3d/collision/shapes/BoxShape.h>
+#include <reactphysics3d/collision/shapes/CollisionShape.h>
+#include <reactphysics3d/components/RigidBodyComponents.h>
+#include <reactphysics3d/mathematics/Quaternion.h>
+#include <reactphysics3d/mathematics/Transform.h>
+#include <reactphysics3d/mathematics/Vector3.h>
+#include <reactphysics3d/memory/DefaultAllocator.h>
+#include <reactphysics3d/memory/MemoryAllocator.h>
 #include <reactphysics3d/reactphysics3d.h>
 #include <vector>
 #include "../part.h"
+#include "util.h"
 
 #include "simulation.h"
 
@@ -10,7 +21,6 @@ extern std::vector<Part> parts;
 
 rp::PhysicsCommon physicsCommon;
 rp::PhysicsWorld* world;
-rp::RigidBody* body;
 
 void simulationInit() {
     world = physicsCommon.createPhysicsWorld();
@@ -18,7 +28,28 @@ void simulationInit() {
     rp::Vector3 position(0, 20, 0);
     rp::Quaternion orientation = rp::Quaternion::identity();
     rp::Transform transform(position, orientation);
-    body = world->createRigidBody(transform);
+    // body = world->createRigidBody(transform);
+}
+
+void addToSimulation(Part part) {
+    if (part.rigidBody) {
+        fprintf(stderr, "Attempt to add part to simulation that already has a rigid body.\n");
+        return;
+    }
+
+    glm::mat4 rotMat = glm::mat4(1.0f);
+    rotMat = glm::rotate(rotMat, part.rotation.x, glm::vec3(1., 0., 0.));
+    rotMat = glm::rotate(rotMat, part.rotation.y, glm::vec3(0., 1., 0.));
+    rotMat = glm::rotate(rotMat, part.rotation.z, glm::vec3(0., 0., 1.));
+    glm::quat quat(rotMat);
+
+    rp::Transform transform(glmToRp(part.position), glmToRp(quat));
+    part.rigidBody = world->createRigidBody(transform);
+
+    rp::BoxShape* shape = physicsCommon.createBoxShape(rp::Vector3(.5, .5, .5));
+
+    part.rigidBody->addCollider(shape, rp::Transform());
+    part.rigidBody->setType(part.anchored ? rp::BodyType::STATIC : rp::BodyType::DYNAMIC);
 }
 
 void physicsStep(float deltaTime) {
@@ -26,9 +57,9 @@ void physicsStep(float deltaTime) {
     world->update(deltaTime);
 
     // Get the updated position of the body
-    const rp::Transform& transform = body->getTransform();
-    const rp::Vector3& position = transform.getPosition();
+    // const rp::Transform& transform = body->getTransform();
+    // const rp::Vector3& position = transform.getPosition();
 
     // Display the position of the body
-    std::cout << "Body Position: (" << position.x << ", " << position.y << ", " << position.z << ")" << std::endl;
+    // std::cout << "Body Position: (" << position.x << ", " << position.y << ", " << position.z << ")" << std::endl;
 }
\ No newline at end of file
diff --git a/src/physics/simulation.h b/src/physics/simulation.h
index 38ba9ea..6341ae3 100644
--- a/src/physics/simulation.h
+++ b/src/physics/simulation.h
@@ -1,4 +1,7 @@
 #pragma once
 
+#include "../part.h"
+
 void simulationInit();
+void addToSimulation(Part part);
 void physicsStep(float deltaTime);
\ No newline at end of file
diff --git a/src/physics/util.h b/src/physics/util.h
new file mode 100644
index 0000000..521e57a
--- /dev/null
+++ b/src/physics/util.h
@@ -0,0 +1,15 @@
+#pragma once
+#include <reactphysics3d/mathematics/Quaternion.h>
+#include <reactphysics3d/mathematics/Vector3.h>
+#include <reactphysics3d/mathematics/mathematics.h>
+#include <glm/ext.hpp>
+
+namespace rp = reactphysics3d;
+
+inline rp::Vector3 glmToRp(glm::vec3 vec) {
+    return rp::Vector3(vec.x, vec.y, vec.z);
+}
+
+inline rp::Quaternion glmToRp(glm::quat quat) {
+    return rp::Quaternion(rp::Vector3(quat.x, quat.y, quat.z), quat.w);
+}
\ No newline at end of file