fix(editor): qcursorconstraints for windows

.clangd

@@ -1,2 +1,3 @@
 CompileFlags:
   Add: [-std=c++20]
+  Remove: [-mno-direct-extern-access]

.vscode/launch.json

@@ -4,6 +4,13 @@
     // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
     "version": "0.2.0",
     "configurations": [
+        {
+            "type": "gdb",
+            "request": "launch",
+            "name": "Debug (gdb)",
+            "program": "${workspaceFolder}/build/bin/editor",
+            "cwd": "${workspaceFolder}",
+        },
         {
             "type": "lldb",
             "request": "launch",

assets/shaders/phong.fs

@@ -54,11 +54,14 @@ uniform int numPointLights;
 uniform DirLight sunLight;
 uniform Material material;
 uniform sampler2DArray studs;
+uniform samplerCube skybox;
 uniform float transparency;
+uniform float reflectance;
 uniform vec3 texScale;
 
-// Functions
 
+// Functions
+vec3 calculateReflection();
 vec3 calculateDirectionalLight(DirLight light);
 vec3 calculatePointLight(PointLight light);
 mat3 lookAlong(vec3 pos, vec3 forward, vec3 up);
@@ -68,6 +71,7 @@ mat3 lookAlong(vec3 pos, vec3 forward, vec3 up);
 void main() {
     vec3 result = vec3(0.0);
 
+    result += calculateReflection();
     result += calculateDirectionalLight(sunLight);
     
     for (int i = 0; i < numPointLights; i++) {
@@ -95,6 +99,25 @@ mat3 lookAlong(vec3 pos, vec3 forward, vec3 up) {
 	return mat3(s, u, f);
 }
 
+
+vec3 sampleSkybox()
+{
+    vec3 norm = normalize(vNormal);
+    vec3 viewDir = normalize(viewPos - vPos);
+    vec3 reflectDir = reflect(-viewDir, norm);
+
+    return textureLod(skybox,reflectDir, 5.0 * (1.0-material.shininess)).rgb;
+}
+
+vec3 calculateReflection() {
+    vec3 norm = normalize(vNormal);
+    vec3 viewDir = normalize(viewPos - vPos);
+    vec3 reflectDir = reflect(viewDir, norm);
+    float fresnel = (pow(1.0-max(dot(viewDir, norm), 0.0), 5.0));
+    vec3 result = sampleSkybox() * mix(/* TEMPORARY: will be replaced by setting */ 0 * /* /TEMPORARY */ fresnel * material.specular, vec3(1.0), reflectance);
+    return result;
+}
+
 vec3 calculateDirectionalLight(DirLight light) {
     // Calculate diffuse
     vec3 norm = normalize(vNormal);
@@ -105,10 +128,13 @@ vec3 calculateDirectionalLight(DirLight light) {
     vec3 viewDir = normalize(viewPos - vPos);
     vec3 reflectDir = reflect(-lightDir, norm);
     float spec = pow(max(dot(viewDir, reflectDir), 0.0), material.shininess);
+    // float fresnel = (pow(1.0-max(dot(viewDir, norm), 0.0), 5.0));
     
-    vec3 ambient = light.ambient * material.diffuse;
-    vec3 diffuse = light.diffuse * diff * material.diffuse;
+    
+    vec3 ambient = light.ambient * (material.diffuse * (1.0-reflectance));
+    vec3 diffuse = light.diffuse * diff * (material.diffuse * (1.0-reflectance));
     vec3 specular = light.specular * spec * material.specular;
+    // specular += sampleSkybox() * fresnel * material.specular;
     
     return (ambient + diffuse + specular);
 }

client/src/main.cpp

@@ -3,6 +3,7 @@
 #include "logger.h"
 #include "objects/part/part.h"
 #include "panic.h"
+#include "physics/world.h"
 #include "rendering/renderer.h"
 #include "common.h"
 #include "version.h"
@@ -47,6 +48,7 @@ int main() {
         Logger::debugf("Initialized GL context version %d.%d", GLAD_VERSION_MAJOR(version), GLAD_VERSION_MINOR(version));
     }
 
+    physicsInit();
     gDataModel->Init();
     renderInit(1200, 900);
 
@@ -86,6 +88,7 @@ int main() {
         glfwPollEvents();
     } while(!glfwWindowShouldClose(window));
 
+    physicsDeinit();
     glfwTerminate();
     return 0;
 }

core/CMakeLists.txt

@@ -57,6 +57,8 @@ set(SOURCES
     src/rendering/font.h
     src/rendering/defaultmeshes.h
     src/rendering/texture3d.cpp
+    src/physics/world.h
+    src/physics/world.cpp
     src/logger.cpp
     src/handles.h
     src/timeutil.h
@@ -126,7 +128,6 @@ set(SOURCES
     src/panic.h
     src/lua/instancelib.cpp
     src/timeutil.cpp
-    src/physics/util.h
     src/luaapis.h
     src/math_helper.h
 )
@@ -197,8 +198,8 @@ list(APPEND SOURCES ${CMAKE_CURRENT_BINARY_DIR}/src/version.cpp)
 add_library(openblocks STATIC ${SOURCES})
 set_target_properties(openblocks PROPERTIES OUTPUT_NAME "openblocks")
 target_link_directories(openblocks PUBLIC ${LUAJIT_LIBRARY_DIRS})
-target_link_libraries(openblocks reactphysics3d pugixml::pugixml Freetype::Freetype glm::glm libluajit ${LuaJIT_LIBRARIES})
-target_include_directories(openblocks PUBLIC "src" "../include" "${CMAKE_SOURCE_DIR}/external/glad" ${ReactPhysics3D_SOURCE_DIR}/include ${LUAJIT_INCLUDE_DIRS} ${stb_SOURCE_DIR})
+target_link_libraries(openblocks Jolt pugixml::pugixml Freetype::Freetype glm::glm libluajit ${LuaJIT_LIBRARIES})
+target_include_directories(openblocks PUBLIC "src" "../include" "${CMAKE_SOURCE_DIR}/external/glad" ${LUAJIT_INCLUDE_DIRS} ${stb_SOURCE_DIR})
 add_dependencies(openblocks autogen_build autogen)
 
 # Windows-specific dependencies

core/deps.cmake

@@ -6,9 +6,7 @@ set (PREV_BIN_PATH ${CMAKE_RUNTIME_OUTPUT_DIRECTORY})
 unset (CMAKE_RUNTIME_OUTPUT_DIRECTORY)
 
 CPMAddPackage("gh:g-truc/glm#1.0.1")
-CPMAddPackage(NAME reactphysics3d GITHUB_REPOSITORY "DanielChappuis/reactphysics3d" VERSION 0.10.2 PATCHES ${CMAKE_SOURCE_DIR}/patches/std_chrono.patch)
-# https://github.com/StereoKit/StereoKit/blob/0be056efebcee5e58ad1438f4cf6dfdb942f6cf9/CMakeLists.txt#L205
-set_property(TARGET reactphysics3d PROPERTY POSITION_INDEPENDENT_CODE ON)
+CPMAddPackage(NAME Jolt GIT_REPOSITORY "https://github.com/jrouwe/JoltPhysics" VERSION 5.3.0 SOURCE_SUBDIR "Build")
 CPMAddPackage("gh:zeux/pugixml@1.15")
 
 CPMAddPackage(

core/src/common.h

@@ -8,9 +8,8 @@
 #include <memory>
 
 class Instance;
-// typedef std::function<void(std::shared_ptr<Instance> element, std::optional<std::shared_ptr<Instance>> newParent)> HierarchyUpdateHandler;
-typedef std::function<void(std::shared_ptr<Instance> object, std::optional<std::shared_ptr<Instance>> oldParent, std::optional<std::shared_ptr<Instance>> newParent)> HierarchyPreUpdateHandler;
-typedef std::function<void(std::shared_ptr<Instance> object, std::optional<std::shared_ptr<Instance>> oldParent, std::optional<std::shared_ptr<Instance>> newParent)> HierarchyPostUpdateHandler;
+typedef std::function<void(std::shared_ptr<Instance> object, nullable std::shared_ptr<Instance> oldParent, nullable std::shared_ptr<Instance> newParent)> HierarchyPreUpdateHandler;
+typedef std::function<void(std::shared_ptr<Instance> object, nullable std::shared_ptr<Instance> oldParent, nullable std::shared_ptr<Instance> newParent)> HierarchyPostUpdateHandler;
 typedef std::function<void(std::shared_ptr<Instance> instance, std::string property, Variant newValue)> PropertyUpdateHandler;
 
 // TEMPORARY COMMON DATA FOR VARIOUS INTERNAL COMPONENTS

core/src/datatypes/cframe.cpp

@@ -1,12 +1,10 @@
 #include "cframe.h"
 #include "datatypes/vector.h"
 #include "error/data.h"
-#include "physics/util.h"
 #include <glm/ext/matrix_transform.hpp>
 #include <glm/gtc/matrix_inverse.hpp>
 #include <glm/gtc/quaternion.hpp>
 #include <glm/matrix.hpp>
-#include <reactphysics3d/mathematics/Transform.h>
 #include "datatypes/variant.h"
 #include <pugixml.hpp>
 #define GLM_ENABLE_EXPERIMENTAL
@@ -40,9 +38,6 @@ CFrame::CFrame(Vector3 position, glm::quat quat)
     , rotation(quat) {
 }
 
-CFrame::CFrame(const rp::Transform& transform) : CFrame::CFrame(rpToGlm(transform.getPosition()), rpToGlm(transform.getOrientation())) {
-}
-
 glm::mat3 lookAt(Vector3 position, Vector3 lookAt, Vector3 up) {
     // https://github.com/sgorsten/linalg/issues/29#issuecomment-743989030
 	Vector3 f = (lookAt - position).Unit(); // Forward/Look
@@ -77,10 +72,6 @@ CFrame::operator glm::mat4() const {
     return glm::translate(glm::mat4(1.0f), this->translation) * glm::mat4(this->rotation);
 }
 
-CFrame::operator rp::Transform() const {
-    return rp::Transform(glmToRp(translation), glmToRp(rotation));
-}
-
 Vector3 CFrame::ToEulerAnglesXYZ() {
     float x;
     float y;

core/src/datatypes/cframe.h

@@ -8,8 +8,6 @@
 #include <glm/gtc/matrix_access.hpp>
 #include <glm/matrix.hpp>
 
-namespace reactphysics3d { class Transform; };
-
 class DEF_DATA_(name="CoordinateFrame") CFrame {
     AUTOGEN_PREAMBLE_DATA
 
@@ -24,7 +22,6 @@ public:
     DEF_DATA_CTOR CFrame();
     DEF_DATA_CTOR 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);
     DEF_DATA_CTOR CFrame(Vector3 , Vector3 lookAt, Vector3 up = Vector3(0, 1, 0));
-    CFrame(const reactphysics3d::Transform&);
     CFrame(Vector3 position, glm::quat quat);
     virtual ~CFrame();
 
@@ -46,11 +43,11 @@ public:
     static void PushLuaLibrary(lua_State*);
 
     operator glm::mat4() const;
-    operator reactphysics3d::Transform() const;
 
     //inline static CFrame identity() { }
     DEF_DATA_PROP inline Vector3 Position() const { return translation; }
     DEF_DATA_PROP inline CFrame Rotation() const { return CFrame { glm::vec3(0, 0, 0), rotation }; }
+    inline glm::mat3 RotMatrix() const { return rotation; }
     DEF_DATA_METHOD CFrame Inverse() const;
     DEF_DATA_PROP inline float X() const { return translation.x; }
     DEF_DATA_PROP inline float Y() const { return translation.y; }

core/src/datatypes/primitives.cpp

@@ -68,7 +68,7 @@ void Bool_PushLuaValue(Variant self, lua_State* L) {
 result<Variant, LuaCastError> Bool_FromLuaValue(lua_State* L, int idx) {
     if (!lua_isboolean(L, idx))
         return LuaCastError(lua_typename(L, idx), "boolean");
-    return Variant(lua_toboolean(L, idx));
+    return Variant((bool)lua_toboolean(L, idx));
 }
 
 const TypeDesc BOOL_TYPE {

core/src/datatypes/ref.cpp

@@ -151,9 +151,9 @@ static int inst_index(lua_State* L) {
     }
 
     // Look for child
-    std::optional<std::shared_ptr<Instance>> child = inst->FindFirstChild(key);
+    nullable std::shared_ptr<Instance> child = inst->FindFirstChild(key);
     if (child) {
-        InstanceRef(child.value()).PushLuaValue(L);
+        InstanceRef(child).PushLuaValue(L);
         return 1;
     }
 
@@ -173,7 +173,7 @@ static int inst_newindex(lua_State* L) {
     if (meta->flags & PROP_READONLY)
         return luaL_error(L, "'%s' of %s is read-only", key.c_str(), inst->GetClass()->className.c_str());
     if (key == "Parent" && inst->IsParentLocked())
-        return luaL_error(L, "Cannot set property Parent (%s) of %s, parent is locked", inst->GetParent() ? inst->GetParent().value()->name.c_str() : "NULL", inst->GetClass()->className.c_str());
+        return luaL_error(L, "Cannot set property Parent (%s) of %s, parent is locked", inst->GetParent() ? inst->GetParent()->name.c_str() : "NULL", inst->GetClass()->className.c_str());
 
     // TODO: Make this work for enums, this is not a solution!!
     result<Variant, LuaCastError> value = meta->type.descriptor->fromLuaValue(L, -1);

core/src/datatypes/vector.cpp

@@ -3,7 +3,6 @@
 #include <cstdlib>
 #include <glm/ext/quaternion_geometric.hpp>
 #include <iomanip>
-#include <reactphysics3d/mathematics/Vector3.h>
 #include <string>
 #include <pugixml.hpp>
 #include "datatypes/base.h"
@@ -11,11 +10,8 @@
 #include "error/data.h"
 #include <sstream>
 
-namespace rp = reactphysics3d;
-
 Vector3::Vector3() : vector(glm::vec3(0, 0, 0)) {};
 Vector3::Vector3(const glm::vec3& src) : vector(src) {};
-Vector3::Vector3(const rp::Vector3& src) : vector(glm::vec3(src.x, src.y, src.z)) {};
 Vector3::Vector3(float x, const float y, float z) : vector(glm::vec3(x, y, z)) {};
 
 Vector3::~Vector3() = default;
@@ -31,7 +27,6 @@ const std::string Vector3::ToString() const {
 }
 
 Vector3::operator glm::vec3() const { return vector; };
-Vector3::operator rp::Vector3() const { return rp::Vector3(X(), Y(), Z()); };
 
 // Operators
 

core/src/datatypes/vector.h

@@ -5,9 +5,6 @@
 #include "error/data.h"
 #include <glm/ext/vector_float3.hpp>
 #include <glm/geometric.hpp>
-#include <reactphysics3d/mathematics/Vector3.h>
-
-// namespace reactphysics3d { class Vector3; };
 
 class DEF_DATA Vector3 {
     AUTOGEN_PREAMBLE_DATA
@@ -18,7 +15,6 @@ public:
     DEF_DATA_CTOR Vector3(float x, float y, float z);
     inline Vector3(float value) : Vector3(value, value, value) {}
     Vector3(const glm::vec3&);
-    Vector3(const reactphysics3d::Vector3&);
     virtual ~Vector3();
 
     DEF_DATA_PROP static Vector3 ZERO;
@@ -33,7 +29,6 @@ public:
     static void PushLuaLibrary(lua_State*);
 
     operator glm::vec3() const;
-    operator reactphysics3d::Vector3() const;
 
     DEF_DATA_PROP inline float X() const { return vector.x; }
     DEF_DATA_PROP inline float Y() const { return vector.y; }

core/src/enum/part.h

@@ -7,6 +7,7 @@
 enum class DEF_ENUM PartType {
     Ball = 0,
     Block = 1,
+    Cylinder = 2,
 };
 
 namespace EnumType {

core/src/handles.cpp

@@ -8,10 +8,6 @@
 #include <glm/ext/scalar_common.hpp>
 #include <memory>
 #include <optional>
-#include <reactphysics3d/collision/RaycastInfo.h>
-#include <reactphysics3d/engine/PhysicsCommon.h>
-#include <reactphysics3d/engine/PhysicsWorld.h>
-#include <reactphysics3d/mathematics/Transform.h>
 
 HandleFace HandleFace::XPos(0, glm::vec3(1,0,0));
 HandleFace HandleFace::XNeg(1, glm::vec3(-1,0,0));
@@ -23,10 +19,6 @@ std::array<HandleFace, 6> HandleFace::Faces { HandleFace::XPos, HandleFace::XNeg
 
 static CFrame XYZToZXY(glm::vec3(0, 0, 0), -glm::vec3(1, 0, 0), glm::vec3(0, 0, 1));
 
-// Shitty solution
-static rp3d::PhysicsCommon common;
-static rp3d::PhysicsWorld* world = common.createPhysicsWorld();
-
 std::shared_ptr<BasePart> getHandleAdornee() {
     std::shared_ptr<Selection> selection = gDataModel->GetService<Selection>();
     for (std::weak_ptr<Instance> inst : selection->Get()) {
@@ -52,24 +44,25 @@ CFrame partCFrameFromHandlePos(HandleFace face, Vector3 newPos) {
     return adornee->cframe.Rotation() + newPartPos;
 }
 
-std::optional<HandleFace> raycastHandle(rp3d::Ray ray) {
+std::optional<HandleFace> raycastHandle(Vector3 rayStart, Vector3 rayEnd) {
+    std::optional<HandleFace> closestFace = {};
+    float closestDistance = -1;
+
     for (HandleFace face : HandleFace::Faces) {
         CFrame cframe = getHandleCFrame(face);
-        // Implement manual detection via boxes instead of... this shit
-        // This code also hardly works, and is not good at all... Hooo nope.
-        rp3d::RigidBody* body = world->createRigidBody(CFrame::IDENTITY + cframe.Position());
-        body->addCollider(common.createBoxShape((cframe.Rotation() * Vector3(handleSize(face) / 2.f)).Abs()), rp3d::Transform::identity());
 
-        rp3d::RaycastInfo info;
-        if (body->raycast(ray, info)) {
-            world->destroyRigidBody(body);
-            return face;
-        }
+        Vector3 halfSize = (cframe.Rotation() * Vector3(handleSize(face) / 2.f)).Abs();
+        Vector3 minB = cframe.Position() - halfSize, maxB = cframe.Position() + halfSize;
 
-        world->destroyRigidBody(body);
+        glm::vec3 hitPoint;
+        bool hit = HitBoundingBox(minB, maxB, rayStart, (rayEnd - rayStart).Unit(), hitPoint);
+        float distance = ((Vector3)hitPoint - rayStart).Magnitude();
+
+        if (hit && (closestDistance == -1 || distance < closestDistance))
+            closestFace = face, closestDistance = distance;
     }
 
-    return std::nullopt;
+    return closestFace;
 }
 
 Vector3 handleSize(HandleFace face) {

core/src/handles.h

@@ -40,7 +40,7 @@ std::shared_ptr<BasePart> getHandleAdornee();
 CFrame getHandleCFrame(HandleFace face);
 CFrame partCFrameFromHandlePos(HandleFace face, Vector3 newPos);
 Vector3 handleSize(HandleFace face);
-std::optional<HandleFace> raycastHandle(rp3d::Ray ray);
+std::optional<HandleFace> raycastHandle(Vector3 rayStart, Vector3 rayEnd);
 
 // Gets the cframe of the handle local to the center of the selected objects
 CFrame getLocalHandleCFrame(HandleFace face);

core/src/math_helper.cpp

@@ -2,9 +2,65 @@
 
 #define CMP_EPSILON 0.00001
 
+
+void expandAABB(glm::vec3& min, glm::vec3& max, glm::vec3 point) {
+	min = glm::vec3(glm::min(min.x, point.x), glm::min(min.y, point.y), glm::min(min.z, point.z));
+	max = glm::vec3(glm::max(max.x, point.x), glm::max(max.y, point.y), glm::max(max.z, point.z));
+}
+
+void computeAABBFromPoints(glm::vec3& min, glm::vec3& max, glm::vec3* points, int count) {
+	if (count == 0) return;
+
+	min = points[0];
+	max = points[0];
+
+	for (int i = 0; i < count; i++) {
+		min = glm::vec3(glm::min(min.x, points[i].x), glm::min(min.y, points[i].y), glm::min(min.z, points[i].z));
+		max = glm::vec3(glm::max(max.x, points[i].x), glm::max(max.y, points[i].y), glm::max(max.z, points[i].z));
+	}
+}
+
+void getAABBCoords(glm::vec3 &pos, glm::vec3 &size, glm::vec3 min, glm::vec3 max) {
+	pos = (max + min) / 2.f;
+	size = (max - min);
+}
+
+
+// ==================== THIRD-PARTY SOURCE CODE ==================== //
+
 // 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.
 
+/**************************************************************************/
+/*  geometry_3d.cpp                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
 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.
 
@@ -102,24 +158,83 @@ void get_closest_points_between_segments(const glm::vec3 &p_p0, const glm::vec3
 	r_qt = (1 - t) * p_q0 + t * p_q1;
 }
 
-void expandAABB(glm::vec3& min, glm::vec3& max, glm::vec3 point) {
-	min = glm::vec3(glm::min(min.x, point.x), glm::min(min.y, point.y), glm::min(min.z, point.z));
-	max = glm::vec3(glm::max(max.x, point.x), glm::max(max.y, point.y), glm::max(max.z, point.z));
-}
+// https://github.com/erich666/GraphicsGems/tree/master?tab=License-1-ov-file#readme
+// Note: The code below predates open source
 
-void computeAABBFromPoints(glm::vec3& min, glm::vec3& max, glm::vec3* points, int count) {
-	if (count == 0) return;
+/*
+ * EULA: The Graphics Gems code is copyright-protected. In other words, you cannot claim the text of the code as your own
+ * and resell it. Using the code is permitted in any program, product, or library, non-commercial or commercial. Giving
+ * credit is not required, though is a nice gesture. The code comes as-is, and if there are any flaws or problems with
+ * any Gems code, nobody involved with Gems - authors, editors, publishers, or webmasters - are to be held responsible.
+ * Basically, don't be a jerk, and remember that anything free comes with no guarantee.
+ */
 
-	min = points[0];
-	max = points[0];
+/* 
+Fast Ray-Box Intersection
+by Andrew Woo
+from "Graphics Gems", Academic Press, 1990
+*/
 
-	for (int i = 0; i < count; i++) {
-		min = glm::vec3(glm::min(min.x, points[i].x), glm::min(min.y, points[i].y), glm::min(min.z, points[i].z));
-		max = glm::vec3(glm::max(max.x, points[i].x), glm::max(max.y, points[i].y), glm::max(max.z, points[i].z));
+#define RIGHT	0
+#define LEFT	1
+#define MIDDLE	2
+
+bool HitBoundingBox(
+	glm::vec3 minB, glm::vec3 maxB,             /*box */
+	glm::vec3 origin, glm::vec3 dir,            /*ray */
+	glm::vec3 &coord                            /* hit point */
+) {
+	bool inside = true;
+	glm::vec3 quadrant;
+	int i;
+	int whichPlane;
+	glm::vec3 maxT;
+	glm::vec3 candidatePlane;
+
+	/* Find candidate planes; this loop can be avoided if
+   	rays cast all from the eye(assume perpsective view) */
+	for (i = 0; i < 3; i++)
+		if(origin[i] < minB[i]) {
+			quadrant[i] = LEFT;
+			candidatePlane[i] = minB[i];
+			inside = false;
+		}else if (origin[i] > maxB[i]) {
+			quadrant[i] = RIGHT;
+			candidatePlane[i] = maxB[i];
+			inside = false;
+		}else	{
+			quadrant[i] = MIDDLE;
+		}
+
+	/* Ray origin inside bounding box */
+	if (inside)	{
+		coord = origin;
+		return (true);
 	}
-}
 
-void getAABBCoords(glm::vec3 &pos, glm::vec3 &size, glm::vec3 min, glm::vec3 max) {
-	pos = (max + min) / 2.f;
-	size = (max - min);
+
+	/* Calculate T distances to candidate planes */
+	for (i = 0; i < 3; i++)
+		if (quadrant[i] != MIDDLE && dir[i] !=0.)
+			maxT[i] = (candidatePlane[i]-origin[i]) / dir[i];
+		else
+			maxT[i] = -1.;
+
+	/* Get largest of the maxT's for final choice of intersection */
+	whichPlane = 0;
+	for (i = 1; i < 3; i++)
+		if (maxT[whichPlane] < maxT[i])
+			whichPlane = i;
+
+	/* Check final candidate actually inside box */
+	if (maxT[whichPlane] < 0.) return (false);
+	for (i = 0; i < 3; i++)
+		if (whichPlane != i) {
+			coord[i] = origin[i] + maxT[whichPlane] * dir[i];
+			if (coord[i] < minB[i] || coord[i] > maxB[i])
+				return (false);
+		} else {
+			coord[i] = candidatePlane[i];
+		}
+	return true;				/* ray hits box */
 }

core/src/math_helper.h

@@ -1,9 +1,20 @@
 #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);
-
 void expandAABB(glm::vec3& min, glm::vec3& max, glm::vec3 point);
 void computeAABBFromPoints(glm::vec3& min, glm::vec3& max, glm::vec3* points, int count);
 void getAABBCoords(glm::vec3& pos, glm::vec3& size, glm::vec3 min, glm::vec3 max);
+
+// 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);
+
+/* 
+Fast Ray-Box Intersection
+by Andrew Woo
+from "Graphics Gems", Academic Press, 1990
+*/
+bool HitBoundingBox(
+	glm::vec3 minB, glm::vec3 maxB,             /*box */
+	glm::vec3 origin, glm::vec3 dir,            /*ray */
+	glm::vec3 &coord                            /* hit point */
+);

core/src/objects/base/instance.cpp

@@ -44,21 +44,16 @@ Instance::Instance(const InstanceType* type) {
 Instance::~Instance () {
 }
 
-template <typename T>
-std::weak_ptr<T> optional_to_weak(std::optional<std::shared_ptr<T>> a) {
-    return a ? a.value() : std::weak_ptr<T>();
-}
-
 // TODO: Test this
-bool Instance::ancestryContinuityCheck(std::optional<std::shared_ptr<Instance>> newParent) {
-    for (std::optional<std::shared_ptr<Instance>> currentParent = newParent; currentParent.has_value(); currentParent = currentParent.value()->GetParent()) {
-        if (currentParent.value() == this->shared_from_this())
+bool Instance::ancestryContinuityCheck(nullable std::shared_ptr<Instance> newParent) {
+    for (std::shared_ptr<Instance> currentParent = newParent; currentParent != nullptr; currentParent = currentParent->GetParent()) {
+        if (currentParent == this->shared_from_this())
             return false;
     }
     return true;
 }
 
-bool Instance::SetParent(std::optional<std::shared_ptr<Instance>> newParent) {
+bool Instance::SetParent(nullable std::shared_ptr<Instance> newParent) {
     if (this->parentLocked || !ancestryContinuityCheck(newParent))
         return false;
 
@@ -70,10 +65,10 @@ bool Instance::SetParent(std::optional<std::shared_ptr<Instance>> newParent) {
         oldParent->children.erase(std::find(oldParent->children.begin(), oldParent->children.end(), this->shared_from_this()));
     }
     // Add ourselves to the new parent
-    if (newParent.has_value()) {
-        newParent.value()->children.push_back(this->shared_from_this());
+    if (newParent != nullptr) {
+        newParent->children.push_back(this->shared_from_this());
     }
-    this->parent = optional_to_weak(newParent);
+    this->parent = newParent;
     // TODO: Add code for sending signals for parent updates
     // TODO: Yeahhh maybe this isn't the best way of doing this?
     if (hierarchyPostUpdateHandler.has_value()) hierarchyPostUpdateHandler.value()(this->shared_from_this(), lastParent, newParent);
@@ -85,21 +80,21 @@ bool Instance::SetParent(std::optional<std::shared_ptr<Instance>> newParent) {
     return true;
 }
 
-void Instance::updateAncestry(std::optional<std::shared_ptr<Instance>> updatedChild, std::optional<std::shared_ptr<Instance>> newParent) {
+void Instance::updateAncestry(nullable std::shared_ptr<Instance> updatedChild, nullable 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::dynamic_pointer_cast<DataModel>(GetParent().value()) : GetParent().value()->_dataModel;
-        this->_workspace = GetParent().value()->GetClass() == &Workspace::TYPE ? std::dynamic_pointer_cast<Workspace>(GetParent().value()) : GetParent().value()->_workspace;
+    if (GetParent() != nullptr) {
+        this->_dataModel = GetParent()->GetClass() == &DataModel::TYPE ? std::dynamic_pointer_cast<DataModel>(GetParent()) : GetParent()->_dataModel;
+        this->_workspace = GetParent()->GetClass() == &Workspace::TYPE ? std::dynamic_pointer_cast<Workspace>(GetParent()) : GetParent()->_workspace;
     } else {
         this->_dataModel = {};
         this->_workspace = {};
     }
 
     OnAncestryChanged(updatedChild, newParent);
-    AncestryChanged->Fire({updatedChild.has_value() ? InstanceRef(updatedChild.value()) : InstanceRef(), newParent.has_value() ? InstanceRef(newParent.value()) : InstanceRef()});
+    AncestryChanged->Fire({updatedChild != nullptr ? InstanceRef(updatedChild) : InstanceRef(), newParent != nullptr ? InstanceRef(newParent) : InstanceRef()});
 
     // Old workspace used to exist, and workspaces differ
     if (!oldWorkspace.expired() && oldWorkspace != _workspace) {
@@ -108,7 +103,7 @@ void Instance::updateAncestry(std::optional<std::shared_ptr<Instance>> updatedCh
 
     // New workspace exists, and workspaces differ
     if (!_workspace.expired() && (_workspace != oldWorkspace)) {
-        OnWorkspaceAdded(!oldWorkspace.expired() ? std::make_optional(oldWorkspace.lock()) : std::nullopt, _workspace.lock());
+        OnWorkspaceAdded(oldWorkspace.expired() ? nullptr : oldWorkspace.lock(), _workspace.lock());
     }
 
     // Update ancestry in descendants
@@ -117,23 +112,22 @@ void Instance::updateAncestry(std::optional<std::shared_ptr<Instance>> updatedCh
     }
 }
 
-std::optional<std::shared_ptr<DataModel>> Instance::dataModel() {
-    return (_dataModel.expired()) ? std::nullopt : std::make_optional(_dataModel.lock());
+nullable std::shared_ptr<DataModel> Instance::dataModel() {
+    return _dataModel.expired() ? nullptr : _dataModel.lock();
 }
 
-std::optional<std::shared_ptr<Workspace>> Instance::workspace() {
-    return (_workspace.expired()) ? std::nullopt : std::make_optional(_workspace.lock());
+nullable std::shared_ptr<Workspace> Instance::workspace() {
+    return _workspace.expired() ? nullptr : _workspace.lock();
 }
 
-std::optional<std::shared_ptr<Instance>> Instance::GetParent() {
-    if (parent.expired()) return std::nullopt;
-    return parent.lock();
+nullable std::shared_ptr<Instance> Instance::GetParent() {
+    return parent.expired() ? nullptr : parent.lock();
 }
 
 void Instance::Destroy() {
     if (parentLocked) return;
     // TODO: Implement proper distruction stuff
-    SetParent(std::nullopt);
+    SetParent(nullptr);
     parentLocked = true;
 }
 
@@ -143,12 +137,12 @@ bool Instance::IsA(std::string className) {
     return cur != nullptr;
 }
 
-std::optional<std::shared_ptr<Instance>> Instance::FindFirstChild(std::string name) {
+nullable std::shared_ptr<Instance> Instance::FindFirstChild(std::string name) {
     for (auto child : children) {
         if (child->name == name)
             return child;
     }
-    return std::nullopt;
+    return nullptr;
 }
 
 static std::shared_ptr<Instance> DUMMY_INSTANCE;
@@ -164,15 +158,15 @@ bool Instance::IsParentLocked() {
     return this->parentLocked;
 }
 
-void Instance::OnParentUpdated(std::optional<std::shared_ptr<Instance>> oldParent, std::optional<std::shared_ptr<Instance>> newParent) {
+void Instance::OnParentUpdated(nullable std::shared_ptr<Instance> oldParent, nullable std::shared_ptr<Instance> newParent) {
     // Empty stub
 }
 
-void Instance::OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent) {
+void Instance::OnAncestryChanged(nullable std::shared_ptr<Instance> child, nullable std::shared_ptr<Instance> newParent) {
     // Empty stub
 }
 
-void Instance::OnWorkspaceAdded(std::optional<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) {
+void Instance::OnWorkspaceAdded(nullable std::shared_ptr<Workspace> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) {
     // Empty stub
 }
 
@@ -230,7 +224,7 @@ fallible<MemberNotFound, AssignToReadOnlyMember> Instance::InternalSetPropertyVa
         this->name = (std::string)value.get<std::string>();
     } else if (name == "Parent") {
         std::weak_ptr<Instance> ref = value.get<InstanceRef>();
-        SetParent(ref.expired() ? std::nullopt : std::make_optional(ref.lock()));
+        SetParent(ref.expired() ? nullptr : ref.lock());
     } else if (name == "ClassName") {
         return AssignToReadOnlyMember(GetClass()->className, name);
     } else {
@@ -430,9 +424,9 @@ DescendantsIterator::self_type DescendantsIterator::operator++(int _) {
     }
 
     // If we've hit the end of this item's children, move one up
-    while (current->GetParent() && current->GetParent().value()->GetChildren().size() <= size_t(siblingIndex.back() + 1)) {
+    while (current->GetParent() != nullptr && current->GetParent()->GetChildren().size() <= size_t(siblingIndex.back() + 1)) {
         siblingIndex.pop_back();
-        current = current->GetParent().value();
+        current = current->GetParent();
 
         // But not if one up is null or the root element
         if (!current->GetParent() || current == root) {
@@ -443,12 +437,12 @@ DescendantsIterator::self_type DescendantsIterator::operator++(int _) {
 
     // Now move to the next sibling
     siblingIndex.back()++;
-    current = current->GetParent().value()->GetChildren()[siblingIndex.back()];
+    current = current->GetParent()->GetChildren()[siblingIndex.back()];
 
     return *this;
 }
 
-std::optional<std::shared_ptr<Instance>> Instance::Clone(RefStateClone state) {
+nullable std::shared_ptr<Instance> Instance::Clone(RefStateClone state) {
     if (state == nullptr) state = std::make_shared<__RefStateClone>();
     std::shared_ptr<Instance> newInstance = GetClass()->constructor();
 
@@ -494,9 +488,9 @@ std::optional<std::shared_ptr<Instance>> Instance::Clone(RefStateClone state) {
 
     // Clone children
     for (std::shared_ptr<Instance> child : GetChildren()) {
-        std::optional<std::shared_ptr<Instance>> clonedChild = child->Clone(state);
+        nullable std::shared_ptr<Instance> clonedChild = child->Clone(state);
         if (clonedChild)
-            newInstance->AddChild(clonedChild.value());
+            newInstance->AddChild(clonedChild);
     }
 
     return newInstance;
@@ -521,12 +515,12 @@ std::vector<std::pair<std::string, std::shared_ptr<Instance>>> Instance::GetRefe
 
 std::string Instance::GetFullName() {
     std::string currentName = name;
-    std::optional<std::shared_ptr<Instance>> currentParent = GetParent();
+    nullable std::shared_ptr<Instance> currentParent = GetParent();
 
-    while (currentParent.has_value() && !currentParent.value()->IsA("DataModel")) {
-        currentName = currentParent.value()->name + "." + currentName;
+    while (currentParent && !currentParent->IsA("DataModel")) {
+        currentName = currentParent->name + "." + currentName;
 
-        currentParent = currentParent.value()->GetParent();
+        currentParent = currentParent->GetParent();
     }
 
     return currentName;

core/src/objects/base/instance.h

@@ -16,6 +16,7 @@
 #include "error/result.h"
 #include "member.h"
 #include "objects/base/refstate.h"
+#include "utils.h"
 
 class Instance;
 typedef std::shared_ptr<Instance>(*InstanceConstructor)();
@@ -59,8 +60,8 @@ private:
     std::weak_ptr<DataModel> _dataModel;
     std::weak_ptr<Workspace> _workspace;
 
-    bool ancestryContinuityCheck(std::optional<std::shared_ptr<Instance>> newParent);
-    void updateAncestry(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent);
+    bool ancestryContinuityCheck(nullable std::shared_ptr<Instance> newParent);
+    void updateAncestry(nullable std::shared_ptr<Instance> child, nullable std::shared_ptr<Instance> newParent);
 
     friend JointInstance; // This isn't ideal, but oh well
 protected:
@@ -75,17 +76,17 @@ protected:
     virtual void InternalUpdateProperty(std::string name);
     virtual std::vector<std::string> InternalGetProperties();
 
-    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 OnParentUpdated(nullable std::shared_ptr<Instance> oldParent, nullable std::shared_ptr<Instance> newParent);
+    virtual void OnAncestryChanged(nullable std::shared_ptr<Instance> child, nullable std::shared_ptr<Instance> newParent);
+    virtual void OnWorkspaceAdded(nullable std::shared_ptr<Workspace> oldWorkspace, std::shared_ptr<Workspace> newWorkspace);
     virtual void OnWorkspaceRemoved(std::shared_ptr<Workspace> oldWorkspace);
 
     // The root data model this object is a descendant of
-    std::optional<std::shared_ptr<DataModel>> dataModel();
+    nullable std::shared_ptr<DataModel> dataModel();
     // The root workspace this object is a descendant of
     // NOTE: This value is not necessarily present if dataModel is present
     // Objects under services other than workspace will NOT have this field set
-    std::optional<std::shared_ptr<Workspace>> workspace();
+    nullable std::shared_ptr<Workspace> workspace();
 public:
     const static InstanceType TYPE;
     std::string name;
@@ -96,8 +97,8 @@ public:
     // Instance is abstract, so it should not implement GetClass directly
     virtual const InstanceType* GetClass() = 0;
     static void PushLuaLibrary(lua_State*); // Defined in lua/instancelib.cpp
-    bool SetParent(std::optional<std::shared_ptr<Instance>> newParent);
-    std::optional<std::shared_ptr<Instance>> GetParent();
+    bool SetParent(nullable std::shared_ptr<Instance> newParent);
+    nullable std::shared_ptr<Instance> GetParent();
     bool IsParentLocked();
     inline const std::vector<std::shared_ptr<Instance>> GetChildren() { return children; }
     void Destroy();
@@ -111,7 +112,7 @@ public:
     DescendantsIterator GetDescendantsEnd();
     // Utility functions
     inline void AddChild(std::shared_ptr<Instance> object) { object->SetParent(this->shared_from_this()); }
-    std::optional<std::shared_ptr<Instance>> FindFirstChild(std::string);
+    nullable std::shared_ptr<Instance> FindFirstChild(std::string);
     std::string GetFullName();
 
     // Properties
@@ -136,7 +137,7 @@ public:
     // Serialization
     void Serialize(pugi::xml_node parent, RefStateSerialize state = {});
     static result<std::shared_ptr<Instance>, NoSuchInstance> Deserialize(pugi::xml_node node, RefStateDeserialize state = {});
-    std::optional<std::shared_ptr<Instance>> Clone(RefStateClone state = {});
+    nullable std::shared_ptr<Instance> Clone(RefStateClone state = {});
 };
 
 // https://gist.github.com/jeetsukumaran/307264
@@ -158,7 +159,7 @@ public:
 
     self_type operator++(int _);
 private:
-    std::optional<std::shared_ptr<Instance>> root;
+    nullable std::shared_ptr<Instance> root;
     std::shared_ptr<Instance> current;
     std::vector<int> siblingIndex;
 };

core/src/objects/base/service.cpp

@@ -7,9 +7,9 @@
 Service::Service(const InstanceType* type) : Instance(type) {}
 
 // Fail if parented to non-datamodel, otherwise lock parent
-void Service::OnParentUpdated(std::optional<std::shared_ptr<Instance>> oldParent, std::optional<std::shared_ptr<Instance>> newParent) {
-    if (!newParent || newParent.value()->GetClass() != &DataModel::TYPE) {
-        Logger::fatalErrorf("Service %s was parented to object of type %s", GetClass()->className.c_str(), newParent ? newParent.value()->GetClass()->className.c_str() : "NULL");
+void Service::OnParentUpdated(nullable std::shared_ptr<Instance> oldParent, nullable std::shared_ptr<Instance> newParent) {
+    if (!newParent || newParent->GetClass() != &DataModel::TYPE) {
+        Logger::fatalErrorf("Service %s was parented to object of type %s", GetClass()->className.c_str(), newParent ? newParent->GetClass()->className.c_str() : "NULL");
         panic();
     }
 

core/src/objects/base/service.h

@@ -13,7 +13,7 @@ protected:
     virtual void InitService();
     virtual void OnRun();
 
-    void OnParentUpdated(std::optional<std::shared_ptr<Instance>> oldParent, std::optional<std::shared_ptr<Instance>> newParent) override;
+    void OnParentUpdated(nullable std::shared_ptr<Instance> oldParent, nullable std::shared_ptr<Instance> newParent) override;
 
     friend class DataModel;
 };

core/src/objects/datamodel.cpp

@@ -20,6 +20,12 @@ DataModel::DataModel()
     this->name = "Place";
 }
 
+DataModel::~DataModel() {
+#ifndef NDEBUG
+    printf("Datamodel successfully destroyed\n");
+#endif
+}
+
 void DataModel::Init(bool runMode) {
     // Create default services
     GetService<Workspace>();
@@ -107,14 +113,14 @@ result<std::shared_ptr<Service>, NoSuchService> DataModel::GetService(std::strin
     return std::dynamic_pointer_cast<Service>(services[className]);
 }
 
-result<std::optional<std::shared_ptr<Service>>, NoSuchService> DataModel::FindService(std::string className) {
+result<nullable std::shared_ptr<Service>, NoSuchService> DataModel::FindService(std::string className) {
     if (!INSTANCE_MAP[className] || (INSTANCE_MAP[className]->flags ^ (INSTANCE_NOTCREATABLE | INSTANCE_SERVICE)) != 0) {
         return NoSuchService(className);
     }
 
     if (services.count(className) != 0)
-        return std::make_optional(std::dynamic_pointer_cast<Service>(services[className]));
-    return (std::optional<std::shared_ptr<Service>>)std::nullopt;
+        return std::dynamic_pointer_cast<Service>(services[className]);
+    return nullptr;
 }
 
 std::shared_ptr<DataModel> DataModel::CloneModel() {
@@ -166,11 +172,11 @@ std::shared_ptr<DataModel> DataModel::CloneModel() {
         if (!result)
             continue;
 
-        newModel->AddChild(result.value());
+        newModel->AddChild(result);
 
         // Special case: Ignore instances parented to DataModel which are not services
         if (child->GetClass()->flags & INSTANCE_SERVICE) {
-            newModel->services[child->GetClass()->className] = std::dynamic_pointer_cast<Service>(result.value());            
+            newModel->services[child->GetClass()->className] = std::dynamic_pointer_cast<Service>(result);
         }
     }
 

core/src/objects/datamodel.h

@@ -24,12 +24,13 @@ public:
     std::optional<std::string> currentFile;
 
     DataModel();
+    ~DataModel();
     void Init(bool runMode = false);
 
     static inline std::shared_ptr<DataModel> New() { return std::make_shared<DataModel>(); };
 
     result<std::shared_ptr<Service>, NoSuchService> GetService(std::string className);
-    result<std::optional<std::shared_ptr<Service>>, NoSuchService> FindService(std::string className);
+    result<nullable std::shared_ptr<Service>, NoSuchService> FindService(std::string className);
 
     template <typename T>
     std::shared_ptr<T> GetService() {
@@ -38,10 +39,10 @@ public:
     }
 
     template <typename T>
-    std::optional<std::shared_ptr<T>> FindService() {
+    nullable std::shared_ptr<T> FindService() {
         auto result = FindService(T::TYPE.className).expect("FindService<T>() was called with a non-service instance type");
-        if (!result) return std::nullopt;
-        return std::dynamic_pointer_cast<T>(result.value());
+        if (!result) return nullptr;
+        return std::dynamic_pointer_cast<T>(result);
     }
 
     // Saving/loading

core/src/objects/joint/jointinstance.cpp

@@ -1,15 +1,11 @@
 #include "jointinstance.h"
 
-#include "datatypes/cframe.h"
 #include "datatypes/ref.h"
 #include "objects/datamodel.h"
 #include "objects/service/jointsservice.h"
-#include "objects/part/part.h"
+#include "objects/part/basepart.h"
 #include "objects/service/workspace.h"
 #include <memory>
-#include <reactphysics3d/constraint/FixedJoint.h>
-#include <reactphysics3d/engine/PhysicsWorld.h>
-#include "ptr_helpers.h"
 
 JointInstance::JointInstance(const InstanceType* type): Instance(type) {
 }
@@ -17,43 +13,49 @@ JointInstance::JointInstance(const InstanceType* type): Instance(type) {
 JointInstance::~JointInstance() {
 }
 
-void JointInstance::OnAncestryChanged(std::optional<std::shared_ptr<Instance>>, std::optional<std::shared_ptr<Instance>>) {
-    // Destroy and rebuild the joint, it's the simplest solution that actually works
-
-    breakJoint();
-    buildJoint();
+void JointInstance::OnAncestryChanged(nullable std::shared_ptr<Instance>, nullable std::shared_ptr<Instance>) {
+    Update();
 }
 
-void JointInstance::onUpdated(std::string property) {
-    // Add ourselves to the attached parts, or remove, if applicable
+void JointInstance::OnPartParamsUpdated() {
+}
 
-    // Parts differ, delete 
-    if (part0 != oldPart0 && !oldPart0.expired()) {
-        oldPart0.lock()->untrackJoint(shared<JointInstance>());
+void JointInstance::Update() {
+    // To keep it simple compared to our previous algorithm, this one is pretty barebones:
+    // 1. Every time we update, (whether our parent changed, or a property), destroy the current joints
+    // 2. If the new configuration is valid, rebuild our joints
+
+    if (!jointWorkspace.expired()) {
+        jointWorkspace.lock()->DestroyJoint(joint);
+        if (!oldPart0.expired())
+            oldPart0.lock()->untrackJoint(shared<JointInstance>());
+        if (!oldPart1.expired())
+            oldPart1.lock()->untrackJoint(shared<JointInstance>());
     }
 
-    if (part1 != oldPart1 && !oldPart1.expired()) {
-        oldPart1.lock()->untrackJoint(shared<JointInstance>());
-    }
-
-    // Parts differ, add 
-    if (part0 != oldPart0 && !part0.expired()) {
-        part0.lock()->trackJoint(shared<JointInstance>());
-    }
-
-    if (part1 != oldPart1 && !part1.expired()) {
-        part1.lock()->trackJoint(shared<JointInstance>());
-    }
-
-    // Destroy and rebuild the joint, if applicable
-
-    breakJoint();
-    buildJoint();
-
     oldPart0 = part0;
     oldPart1 = part1;
+
+    // Don't build the joint if we're not part of either a workspace or JointsService
+    if ((!GetParent() || GetParent()->GetClass() != &JointsService::TYPE) && !workspace()) return;
+
+    // If either part is invalid or they are part of separate worlds, fail
+    if (part0.expired()
+        || part1.expired()
+        || !workspaceOfPart(part0.lock())
+        || !workspaceOfPart(part1.lock())
+        || workspaceOfPart(part0.lock()) != workspaceOfPart(part1.lock())
+    ) return;
+
+    // TODO: Add joint continuity check here
+
+    // Finally, build the joint
+    buildJoint();
+
+    part0.lock()->trackJoint(shared<JointInstance>());
+    part1.lock()->trackJoint(shared<JointInstance>());
 }
 
-std::optional<std::shared_ptr<Workspace>> JointInstance::workspaceOfPart(std::shared_ptr<BasePart> part) {
+nullable std::shared_ptr<Workspace> JointInstance::workspaceOfPart(std::shared_ptr<BasePart> part) {
     return part->workspace();
 }

core/src/objects/joint/jointinstance.h

@@ -3,14 +3,16 @@
 #include "objects/base/instance.h"
 #include "../annotation.h"
 #include <memory>
-#include <optional>
 #include "datatypes/cframe.h"
+#include "physics/world.h"
 
 //this is necessary ebcause we use std::weak_ptr<Part> without including it in this file
 #ifdef __AUTOGEN_EXTRA_INCLUDES__
 #include "objects/part/part.h"
 #endif
 
+#define DEF_PROP_PHYS DEF_PROP_(on_update=onUpdated)
+
 class BasePart;
 class Workspace;
 
@@ -22,20 +24,25 @@ class DEF_INST_ABSTRACT JointInstance : public Instance {
 protected:
     // The workspace the joint was created in, if it exists
     std::weak_ptr<Workspace> jointWorkspace;
+    PhysJoint joint;
 
-    void OnAncestryChanged(std::optional<std::shared_ptr<Instance>>, std::optional<std::shared_ptr<Instance>>) override;
+    void OnAncestryChanged(nullable std::shared_ptr<Instance>, nullable std::shared_ptr<Instance>) override;
+
+    nullable std::shared_ptr<Workspace> workspaceOfPart(std::shared_ptr<BasePart>);
+    inline void onUpdated(std::string property) { Update(); };
 
-    std::optional<std::shared_ptr<Workspace>> workspaceOfPart(std::shared_ptr<BasePart>);
-    void onUpdated(std::string property);
     virtual void buildJoint() = 0;
-    virtual void breakJoint() = 0;
 public:
+    void Update();
+    virtual void OnPartParamsUpdated();
 
-    DEF_PROP_(on_update=onUpdated) std::weak_ptr<BasePart> part0;
-    DEF_PROP_(on_update=onUpdated) std::weak_ptr<BasePart> part1;
-    DEF_PROP_(on_update=onUpdated) CFrame c0;
-    DEF_PROP_(on_update=onUpdated) CFrame c1;
+    DEF_PROP_PHYS std::weak_ptr<BasePart> part0;
+    DEF_PROP_PHYS std::weak_ptr<BasePart> part1;
+    DEF_PROP_PHYS CFrame c0;
+    DEF_PROP_PHYS CFrame c1;
 
     JointInstance(const InstanceType*);
     ~JointInstance();
 };
+
+#undef DEF_PROP_PHYS

core/src/objects/joint/rotate.cpp

@@ -3,7 +3,6 @@
 #include "objects/part/part.h"
 #include "objects/service/workspace.h"
 #include "rendering/renderer.h"
-#include <reactphysics3d/constraint/HingeJoint.h>
 
 Rotate::Rotate(): JointInstance(&TYPE) {
 }
@@ -13,35 +12,15 @@ Rotate::~Rotate() {
 static CFrame XYZToZXY(glm::vec3(0, 0, 0), -glm::vec3(1, 0, 0), glm::vec3(0, 0, 1));
 
 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();
+    std::shared_ptr<Workspace> workspace = workspaceOfPart(part0.lock());
 
     // 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 * (c0 * c1.Inverse());
     part1.lock()->cframe = newFrame;
-    workspace->SyncPartPhysics(part1.lock());
+
     // Do NOT use Abs() in this scenario. For some reason that breaks it
-    rp::HingeJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (part0.lock()->cframe * c0).Position(), -(part0.lock()->cframe * c0).LookVector().Unit());
-    this->joint = dynamic_cast<rp::HingeJoint*>(workspace->CreateJoint(jointInfo));
+    PhysRotatingJointInfo jointInfo(c0, c1);
+    this->joint = workspace->CreateJoint(jointInfo, part0.lock(), part1.lock());
     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);
-}
-
-// !!! 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()) return;
-
-    jointWorkspace.lock()->DestroyJoint(this->joint);
-    this->joint = nullptr;
 }

core/src/objects/joint/rotate.h

@@ -5,15 +5,10 @@
 #include "objects/joint/jointinstance.h"
 #include <memory>
 
-namespace reactphysics3d { class HingeJoint; }
-
 class DEF_INST Rotate : public JointInstance {
     AUTOGEN_PREAMBLE
 
-    reactphysics3d::HingeJoint* joint = nullptr;
-
     virtual void buildJoint() override;
-    virtual void breakJoint() override;
 public:
     Rotate();
     ~Rotate();

core/src/objects/joint/rotatev.cpp

@@ -3,7 +3,6 @@
 #include "objects/part/part.h"
 #include "objects/service/workspace.h"
 #include "rendering/renderer.h"
-#include <reactphysics3d/constraint/HingeJoint.h>
 
 #define PI 3.14159
 
@@ -15,39 +14,21 @@ RotateV::~RotateV() {
 static CFrame XYZToZXY(glm::vec3(0, 0, 0), -glm::vec3(1, 0, 0), glm::vec3(0, 0, 1));
 
 void RotateV::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();
-
+    std::shared_ptr<Workspace> workspace = workspaceOfPart(part0.lock());
 
     // 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 * (c0 * c1.Inverse());
     part1.lock()->cframe = newFrame;
-    workspace->SyncPartPhysics(part1.lock());
     // Do NOT use Abs() in this scenario. For some reason that breaks it
-    rp::HingeJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (part0.lock()->cframe * c0).Position(), -(part0.lock()->cframe * c0).LookVector().Unit());
+    float vel = part0.lock()->GetSurfaceParamB(-c0.LookVector().Unit()) * 6.28;
+    PhysRotatingJointInfo jointInfo(c0, c1, vel);
     
-    jointInfo.isCollisionEnabled = false;
-
-    this->joint = dynamic_cast<rp::HingeJoint*>(workspace->CreateJoint(jointInfo));
+    this->joint = workspace->CreateJoint(jointInfo, part0.lock(), part1.lock());
     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);
 }
 
-void RotateV::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()) return;
-
-    jointWorkspace.lock()->DestroyJoint(this->joint);
-    this->joint = nullptr;
+void RotateV::OnPartParamsUpdated() {
+    float vel = part0.lock()->GetSurfaceParamB(-c0.LookVector().Unit()) * 6.28;
+    this->joint.setAngularVelocity(vel);
 }

core/src/objects/joint/rotatev.h

@@ -4,19 +4,17 @@
 #include "objects/base/instance.h"
 #include "objects/joint/jointinstance.h"
 #include <memory>
-namespace reactphysics3d { class HingeJoint; }
 
 class DEF_INST RotateV : public JointInstance {
     AUTOGEN_PREAMBLE
 
-    reactphysics3d::HingeJoint* joint = nullptr;
-
     virtual void buildJoint() override;
-    virtual void breakJoint() override;
 public:
     RotateV();
     ~RotateV();
 
+    void OnPartParamsUpdated() override;
+
     static inline std::shared_ptr<RotateV> New() { return std::make_shared<RotateV>(); };
     static inline std::shared_ptr<Instance> Create() { return std::make_shared<RotateV>(); };
 };

core/src/objects/joint/snap.cpp

@@ -6,9 +6,8 @@
 #include "objects/service/jointsservice.h"
 #include "objects/part/part.h"
 #include "objects/service/workspace.h"
+#include "physics/world.h"
 #include <memory>
-#include <reactphysics3d/constraint/FixedJoint.h>
-#include <reactphysics3d/engine/PhysicsWorld.h>
 
 Snap::Snap(): JointInstance(&TYPE) {
 }
@@ -17,30 +16,14 @@ Snap::~Snap() {
 }
 
 void Snap::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();
+    std::shared_ptr<Workspace> workspace = workspaceOfPart(part0.lock());
 
     // 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 * (c0 * c1.Inverse());
     part1.lock()->cframe = newFrame;
-    workspace->SyncPartPhysics(part1.lock());
 
-    rp::FixedJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (c0.Inverse() * c1).Position());
-    this->joint = dynamic_cast<rp::FixedJoint*>(workspace->CreateJoint(jointInfo));
+    PhysFixedJointInfo jointInfo(c0, c1);
+    this->joint = workspace->CreateJoint(jointInfo, part0.lock(), part1.lock());
     jointWorkspace = workspace;
 }
-
-// !!! REMINDER: This has to be called manually when parts are destroyed/removed from the workspace, or joints will linger
-void Snap::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()) return;
-
-    jointWorkspace.lock()->DestroyJoint(this->joint);
-    this->joint = nullptr;
-}

core/src/objects/joint/snap.h

@@ -5,15 +5,10 @@
 #include "objects/joint/jointinstance.h"
 #include <memory>
 
-namespace reactphysics3d { class FixedJoint; }
-
 class DEF_INST Snap : public JointInstance {
     AUTOGEN_PREAMBLE
 
-    reactphysics3d::FixedJoint* joint = nullptr;
-
     virtual void buildJoint() override;
-    virtual void breakJoint() override;
 public:
     Snap();
     ~Snap();

core/src/objects/joint/weld.cpp

@@ -6,9 +6,8 @@
 #include "objects/service/jointsservice.h"
 #include "objects/part/part.h"
 #include "objects/service/workspace.h"
+#include "physics/world.h"
 #include <memory>
-#include <reactphysics3d/constraint/FixedJoint.h>
-#include <reactphysics3d/engine/PhysicsWorld.h>
 
 Weld::Weld(): JointInstance(&TYPE) {
 }
@@ -17,30 +16,14 @@ Weld::~Weld() {
 }
 
 void Weld::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();
+    std::shared_ptr<Workspace> workspace = workspaceOfPart(part0.lock());
 
     // 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 * (c0 * c1.Inverse());
     part1.lock()->cframe = newFrame;
-    workspace->SyncPartPhysics(part1.lock());
 
-    rp::FixedJointInfo jointInfo(part0.lock()->rigidBody, part1.lock()->rigidBody, (c0.Inverse() * c1).Position());
-    this->joint = dynamic_cast<rp::FixedJoint*>(workspace->CreateJoint(jointInfo));
+    PhysFixedJointInfo jointInfo(c0, c1);
+    this->joint = workspace->CreateJoint(jointInfo, part0.lock(), part1.lock());
     jointWorkspace = workspace;
 }
-
-// !!! REMINDER: This has to be called manually when parts are destroyed/removed from the workspace, or joints will linger
-void Weld::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()) return;
-
-    jointWorkspace.lock()->DestroyJoint(this->joint);
-    this->joint = nullptr;
-}

core/src/objects/joint/weld.h

@@ -5,15 +5,10 @@
 #include "objects/joint/jointinstance.h"
 #include <memory>
 
-namespace reactphysics3d { class FixedJoint; }
-
 class DEF_INST Weld : public JointInstance {
     AUTOGEN_PREAMBLE
 
-    reactphysics3d::FixedJoint* joint = nullptr;
-
     virtual void buildJoint() override;
-    virtual void breakJoint() override;
 public:
     Weld();
     ~Weld();

core/src/objects/part/basepart.cpp

@@ -14,7 +14,6 @@
 #include "objects/joint/snap.h"
 #include "rendering/renderer.h"
 #include "enum/surface.h"
-#include <cstdio>
 #include <glm/common.hpp>
 #include <memory>
 #include <optional>
@@ -27,20 +26,17 @@ BasePart::BasePart(const InstanceType* type, PartConstructParams params): PVInst
 }
 
 BasePart::~BasePart() {
-    // This relies on physicsCommon still existing. Be very careful.
-    if (this->rigidBody && workspace()) {
-        workspace().value()->DestroyRigidBody(rigidBody);
-        this->rigidBody = nullptr;
+    if (workspace() != nullptr) {
+        workspace()->RemoveBody(shared<BasePart>());
     }
 }
 
 
-void BasePart::OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent) {
-    if (this->rigidBody)
-        this->rigidBody->setIsActive(workspace().has_value());
+void BasePart::OnAncestryChanged(nullable std::shared_ptr<Instance> child, nullable std::shared_ptr<Instance> newParent) {
+    this->rigidBody.setActive(workspace() != nullptr);
 
-    if (workspace())
-        workspace().value()->SyncPartPhysics(std::dynamic_pointer_cast<BasePart>(this->shared_from_this()));
+    if (workspace() != nullptr)
+        workspace()->SyncPartPhysics(std::dynamic_pointer_cast<BasePart>(this->shared_from_this()));
 
     // Destroy joints
     if (!workspace()) BreakJoints();
@@ -48,26 +44,41 @@ void BasePart::OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child,
     // TODO: Sleeping bodies that touch this one also need to be updated
 }
 
-void BasePart::OnWorkspaceAdded(std::optional<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) {
+void BasePart::OnWorkspaceAdded(nullable std::shared_ptr<Workspace> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) {
     newWorkspace->AddBody(shared<BasePart>());
 }
 
 void BasePart::OnWorkspaceRemoved(std::shared_ptr<Workspace> oldWorkspace) {
-    if (simulationTicket.has_value())
-        oldWorkspace->RemoveBody(shared<BasePart>());
+    BreakJoints();
+    oldWorkspace->RemoveBody(shared<BasePart>());
 }
 
 void BasePart::onUpdated(std::string property) {
     bool reset = property == "Position" || property == "Rotation" || property == "CFrame" || property == "Size" || property == "Shape";
 
-    if (workspace())
-        workspace().value()->SyncPartPhysics(std::dynamic_pointer_cast<BasePart>(this->shared_from_this()));
+    // Sanitize size
+    // TODO: Replace this with a validator instead
+    if (property == "Size") {
+        size = glm::max((glm::vec3)size, glm::vec3(0.1f, 0.1f, 0.1f));
+    }
+    
+    if (workspace() != nullptr)
+        workspace()->SyncPartPhysics(std::dynamic_pointer_cast<BasePart>(this->shared_from_this()));
 
     // When position/rotation/size is manually edited, break all joints, they don't apply anymore
     if (reset)
         BreakJoints();
 }
 
+void BasePart::onParamUpdated(std::string property) {
+    // Send signal to joints to update themselves
+    for (std::weak_ptr<JointInstance> joint : primaryJoints) {
+        if (joint.expired()) continue;
+
+        joint.lock()->OnPartParamsUpdated();
+    }
+}
+
 // Expands provided extents to fit point
 static void expandMaxExtents(Vector3* min, Vector3* max, Vector3 point) {
     *min = Vector3(glm::min(min->X(), point.X()), glm::min(min->Y(), point.Y()), glm::min(min->Z(), point.Z()));
@@ -206,7 +217,7 @@ bool BasePart::checkSurfacesTouching(CFrame surfaceFrame, Vector3 size, Vector3
     return horizOverlap && vertOverlap;
 }
 
-std::optional<std::shared_ptr<JointInstance>> makeJointFromSurfaces(SurfaceType a, SurfaceType b) {
+nullable std::shared_ptr<JointInstance> makeJointFromSurfaces(SurfaceType a, SurfaceType b) {
     if (a == SurfaceType::Weld || b == SurfaceType::Weld || a == SurfaceType::Glue || b == SurfaceType::Glue) return Weld::New();
     if ((a == SurfaceType::Studs && (b == SurfaceType::Inlet || b == SurfaceType::Universal))
     || (a == SurfaceType::Inlet && (b == SurfaceType::Studs || b == SurfaceType::Universal))
@@ -216,7 +227,7 @@ std::optional<std::shared_ptr<JointInstance>> makeJointFromSurfaces(SurfaceType
         return Rotate::New();
     if (a == SurfaceType::Motor)
         return RotateV::New();
-    return std::nullopt;
+    return nullptr;
 }
 
 void BasePart::MakeJoints() {
@@ -231,7 +242,7 @@ void BasePart::MakeJoints() {
 
     // TEMPORARY
     // TODO: Use more efficient algorithm to *actually* find nearby parts)
-    for (auto it = workspace().value()->GetDescendantsStart(); it != workspace().value()->GetDescendantsEnd(); it++) {
+    for (auto it = workspace()->GetDescendantsStart(); it != workspace()->GetDescendantsEnd(); it++) {
         std::shared_ptr<Instance> obj = *it;
         if (obj == shared_from_this()) continue; // Skip ourselves
         if (!obj->IsA<BasePart>()) continue;
@@ -278,15 +289,17 @@ void BasePart::MakeJoints() {
 
                 auto joint_ = makeJointFromSurfaces(mySurface, otherSurface);
                 if (!joint_) continue;
-                std::shared_ptr<JointInstance> joint = joint_.value();
+                std::shared_ptr<JointInstance> joint = joint_;
                 joint->part0 = shared<BasePart>();
                 joint->part1 = otherPart->shared<BasePart>();
                 joint->c0 = contact0;
                 joint->c1 = contact1;
-                dataModel().value()->GetService<JointsService>()->AddChild(joint);
+                // // If both parts touch directly, this can cause friction in Rotate and RotateV joints, so we leave a little extra space
+                // if (joint->IsA("Rotate") || joint->IsA("RotateV"))
+                //     joint->c1 = joint->c1 + joint->c1.LookVector() * 0.02f,
+                //     joint->c0 = joint->c0 - joint->c0.LookVector() * 0.02f;
+                dataModel()->GetService<JointsService>()->AddChild(joint);
                 joint->UpdateProperty("Part0");
-
-                Logger::debugf("Made joint between %s and %s!\n", name.c_str(), otherPart->name.c_str());
             }
         }
     }

core/src/objects/part/basepart.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <list>
 #include <memory>
 #include <glm/glm.hpp>
 #include <glm/ext.hpp>
@@ -10,16 +9,14 @@
 #include "datatypes/vector.h"
 #include "objects/base/instance.h"
 #include "enum/surface.h"
-#include <mutex>
-#include <optional>
-#include <reactphysics3d/body/RigidBody.h>
-#include <reactphysics3d/engine/PhysicsCommon.h>
-#include <reactphysics3d/reactphysics3d.h>
 #include <vector>
 #include "objects/annotation.h"
 #include "objects/pvinstance.h"
+#include "physics/world.h"
 
-namespace rp = reactphysics3d;
+// Common macro for part properties
+#define DEF_PROP_PHYS DEF_PROP_(on_update=onUpdated)
+#define DEF_PROP_PHYSPARAM DEF_PROP_(on_update=onParamUpdated)
 
 // For easy construction from C++. Maybe should be removed?
 struct PartConstructParams {
@@ -32,13 +29,7 @@ struct PartConstructParams {
     bool locked = false;
 };
 
-class Workspace;
-
-#ifndef __SIMULATION_TICKET
-#define __SIMULATION_TICKET
-class BasePart;
-typedef std::list<std::shared_ptr<BasePart>>::iterator SimulationTicket;
-#endif
+class PhysWorld;
 
 class DEF_INST_ABSTRACT_(explorer_icon="part") BasePart : public PVInstance {
     AUTOGEN_PREAMBLE
@@ -58,35 +49,35 @@ protected:
     bool checkSurfacesTouching(CFrame surfaceFrame, Vector3 size, Vector3 myFace, Vector3 otherFace, std::shared_ptr<BasePart> otherPart); 
 
     friend JointInstance;
-    friend Workspace;
+    friend PhysWorld;
 
-    virtual void OnWorkspaceAdded(std::optional<std::shared_ptr<Workspace>> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) override;
+    virtual void OnWorkspaceAdded(nullable std::shared_ptr<Workspace> oldWorkspace, std::shared_ptr<Workspace> newWorkspace) override;
     virtual void OnWorkspaceRemoved(std::shared_ptr<Workspace> oldWorkspace) override;
-    void OnAncestryChanged(std::optional<std::shared_ptr<Instance>> child, std::optional<std::shared_ptr<Instance>> newParent) override;
+    void OnAncestryChanged(nullable std::shared_ptr<Instance> child, nullable std::shared_ptr<Instance> newParent) override;
     void onUpdated(std::string);
-
-    virtual void updateCollider(rp::PhysicsCommon* common) = 0;
+    void onParamUpdated(std::string);
 
     BasePart(const InstanceType*);
     BasePart(const InstanceType*, PartConstructParams params);
 public:
     DEF_PROP_CATEGORY(DATA)
-    DEF_PROP_(on_update=onUpdated) Vector3 velocity;
+    DEF_PROP_PHYS Vector3 velocity;
     [[ def_prop(name="CFrame", on_update=onUpdated), cframe_position_prop(name="Position"), cframe_rotation_prop(name="Rotation") ]]
     CFrame cframe;
 
     DEF_PROP_CATEGORY(PART)
     // Special compatibility changes for this property were made in
     // Instance::Serialize
-    DEF_PROP_(on_update=onUpdated) Vector3 size;
+    DEF_PROP_PHYS Vector3 size;
 
     DEF_PROP_CATEGORY(APPEARANCE)
     DEF_PROP Color3 color;
     DEF_PROP float transparency = 0.f;
+    DEF_PROP float reflectance = 0.f;
     
     DEF_PROP_CATEGORY(BEHAVIOR)
-    DEF_PROP_(on_update=onUpdated) bool anchored = false;
-    DEF_PROP_(on_update=onUpdated) bool canCollide = true;
+    DEF_PROP_PHYS bool anchored = false;
+    DEF_PROP_PHYS bool canCollide = true;
     DEF_PROP bool locked = false;
 
     DEF_PROP_CATEGORY(SURFACE)
@@ -98,26 +89,24 @@ public:
     DEF_PROP SurfaceType backSurface = SurfaceType::Smooth;
 
     DEF_PROP_CATEGORY(SURFACE_INPUT)
-    DEF_PROP float topParamA = -0.5;
-    DEF_PROP float bottomParamA = -0.5;
-    DEF_PROP float leftParamA = -0.5;
-    DEF_PROP float rightParamA = -0.5;
-    DEF_PROP float frontParamA = -0.5;
-    DEF_PROP float backParamA = -0.5;
+    DEF_PROP_PHYSPARAM float topParamA = -0.5;
+    DEF_PROP_PHYSPARAM float bottomParamA = -0.5;
+    DEF_PROP_PHYSPARAM float leftParamA = -0.5;
+    DEF_PROP_PHYSPARAM float rightParamA = -0.5;
+    DEF_PROP_PHYSPARAM float frontParamA = -0.5;
+    DEF_PROP_PHYSPARAM float backParamA = -0.5;
 
-    DEF_PROP float topParamB = 0.5;
-    DEF_PROP float bottomParamB = 0.5;
-    DEF_PROP float leftParamB = 0.5;
-    DEF_PROP float rightParamB = 0.5;
-    DEF_PROP float frontParamB = 0.5;
-    DEF_PROP float backParamB = 0.5;
+    DEF_PROP_PHYSPARAM float topParamB = 0.5;
+    DEF_PROP_PHYSPARAM float bottomParamB = 0.5;
+    DEF_PROP_PHYSPARAM float leftParamB = 0.5;
+    DEF_PROP_PHYSPARAM float rightParamB = 0.5;
+    DEF_PROP_PHYSPARAM float frontParamB = 0.5;
+    DEF_PROP_PHYSPARAM float backParamB = 0.5;
 
     DEF_SIGNAL SignalSource Touched;
     DEF_SIGNAL SignalSource TouchEnded;
 
-    rp::RigidBody* rigidBody = nullptr;
-    std::optional<SimulationTicket> simulationTicket;
-    bool rigidBodyDirty = true;
+    PhysRigidBody rigidBody;
     
     inline SurfaceType GetSurfaceFromFace(NormalId face) { return surfaceFromFace(face); }
     float GetSurfaceParamA(Vector3 face);
@@ -134,3 +123,6 @@ public:
     // Calculate size of axis-aligned bounding box
     Vector3 GetAABB();
 };
+
+#undef DEF_PROP_PHYS
+#undef DEF_PROP_PHYSPARAM

core/src/objects/part/part.cpp

@@ -1,42 +1,22 @@
 #include "part.h"
 #include "enum/part.h"
-#include "physics/util.h"
 #include <glm/common.hpp>
 
 Part::Part(): BasePart(&TYPE) {
-    _lastShape = shape;
-    _lastSize = size;
 }
 
 Part::Part(PartConstructParams params): BasePart(&TYPE, params) {
-    _lastShape = shape;
-    _lastSize = size;
-}
-
-void Part::updateCollider(rp::PhysicsCommon* common) {
-    rp::CollisionShape* physShape;
-    if (shape == PartType::Ball) {
-        physShape = common->createSphereShape(glm::min(size.X(), size.Y(), size.Z()) * 0.5f);
-    } else if (shape == PartType::Block) {
-        physShape = common->createBoxShape(glmToRp(size * glm::vec3(0.5f)));
-    }
-
-    // Recreate the rigidbody if the shape changes
-    if (rigidBody->getNbColliders() > 0 && (_lastShape != shape || _lastSize != size)) {
-        // TODO: This causes Touched to get called twice. Fix this.
-        rigidBody->removeCollider(rigidBody->getCollider(0));
-        rigidBody->addCollider(physShape, rp::Transform());
-    }
-
-    if (rigidBody->getNbColliders() == 0)
-        rigidBody->addCollider(physShape, rp::Transform());
-
-
-    _lastShape = shape;
-    _lastSize = size;
 }
 
 Vector3 Part::GetEffectiveSize() {
-    return shape == PartType::Ball ? (Vector3)glm::vec3(glm::min(size.X(), size.Y(), size.Z())) : size;
-
+    float diameter;
+    switch (shape) {
+    case PartType::Ball:
+        return (Vector3)glm::vec3(glm::min(size.X(), size.Y(), size.Z()));
+    case PartType::Cylinder:
+        diameter = glm::min(size.Y(), size.Z());
+        return Vector3(size.X(), diameter, diameter);
+    default:
+        return size;
+    }
 }

core/src/objects/part/part.h

@@ -6,11 +6,7 @@
 
 class DEF_INST Part : public BasePart {
     AUTOGEN_PREAMBLE
-
-    PartType _lastShape;
-    Vector3 _lastSize;
 protected:
-    void updateCollider(rp::PhysicsCommon* common) override;
 
 public:
     Part();

core/src/objects/part/wedgepart.cpp

@@ -1,92 +1,6 @@
 #include "wedgepart.h"
-#include "physics/util.h"
-#include <reactphysics3d/collision/ConvexMesh.h>
-#include <reactphysics3d/collision/shapes/ConvexMeshShape.h>
-
-rp::ConvexMesh* wedgePhysMesh;
 
 WedgePart::WedgePart(): BasePart(&TYPE) {
 }
 
-WedgePart::WedgePart(PartConstructParams params): BasePart(&TYPE, params) {                      
-    
-}
-
-void WedgePart::updateCollider(rp::PhysicsCommon* common) {
-    Logger::fatalError("Wedges are currently disabled! Please do not use them or your editor may crash\n");
-    rp::ConvexMeshShape* shape = common->createConvexMeshShape(wedgePhysMesh, glmToRp(size * glm::vec3(0.5f)));
-
-    // Recreate the rigidbody if the shape changes
-    if (rigidBody->getNbColliders() > 0
-        && dynamic_cast<rp::ConvexMeshShape*>(rigidBody->getCollider(0)->getCollisionShape())->getScale() != shape->getScale()) {
-        // TODO: This causes Touched to get called twice. Fix this.
-        rigidBody->removeCollider(rigidBody->getCollider(0));
-        rigidBody->addCollider(shape, rp::Transform());
-    }
-
-    if (rigidBody->getNbColliders() == 0)
-        rigidBody->addCollider(shape, rp::Transform());
-}
-
-void WedgePart::createWedgeShape(rp::PhysicsCommon* common) {
-    // https://www.reactphysics3d.com/documentation/index.html#creatingbody
-    float vertices[] = {
-        //     X   Y   Z
-        /*0*/ -1,  1,  1, // 0
-        /*1*/ -1, -1,  1, // |
-        /*2*/ -1, -1, -1, // 1---2
-
-        /*3*/  1,  1,  1,
-        /*4*/  1, -1,  1,
-        /*5*/  1, -1, -1,
-    };
-
-    //     -x        +x
-    //  +z 1----------4
-    //     |  bottom  |
-    //  -z 2----------5
-
-    //     -x        +x
-    //  +y 0----------3
-    //     |  front   |
-    //  -y 1----------4
-
-    //      -x        +x
-    //  +yz 0----------3
-    //      |  slope   |
-    //  -yz 2----------5
-
-    int indices[] = {
-        // Base
-        1, 2, 5, 4,
-
-        // Back-face
-        0, 1, 4, 3,
-        // 4, 1, 0, 3,
-
-        // Slope
-        0, 2, 5, 3,
-        // 3, 5, 2, 0,
-
-        // Sides
-        0, 1, 2,
-        3, 4, 5,
-    };
-    
-    // Description of the six faces of the convex mesh
-    rp::PolygonVertexArray::PolygonFace* polygonFaces = new rp::PolygonVertexArray::PolygonFace[5];
-    polygonFaces[0] = { 4, 0 }; // Bottom
-    polygonFaces[1] = { 4, 4 }; // Front
-    polygonFaces[2] = { 4, 8 }; // Slope
-    polygonFaces[3] = { 3, 12 }; // Side
-    polygonFaces[4] = { 3, 15 }; // Side
-    
-    // Create the polygon vertex array
-    rp::PolygonVertexArray polygonVertexArray(6, vertices, 3 * sizeof(float), indices, sizeof(int), 5, polygonFaces,
-    rp::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
-    rp::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
-    
-    // Create the convex mesh
-    std::vector<rp3d::Message> messages;
-    // wedgePhysMesh = common->createConvexMesh(polygonVertexArray, messages);
-}
+WedgePart::WedgePart(PartConstructParams params): BasePart(&TYPE, params) {}

core/src/objects/part/wedgepart.h

@@ -3,14 +3,9 @@
 #include "basepart.h"
 #include "objects/annotation.h"
 
-class DEF_INST_(hidden) WedgePart : public BasePart {
+class DEF_INST WedgePart : public BasePart {
     AUTOGEN_PREAMBLE
 
-protected:
-    void updateCollider(rp::PhysicsCommon* common) override;
-    static void createWedgeShape(rp::PhysicsCommon* common);
-
-    friend Workspace;
 public:
     WedgePart();
     WedgePart(PartConstructParams params);

core/src/objects/script.cpp

@@ -23,7 +23,7 @@ Script::~Script() {
 }
 
 void Script::Run() {
-    std::shared_ptr<ScriptContext> scriptContext = dataModel().value()->GetService<ScriptContext>();
+    std::shared_ptr<ScriptContext> scriptContext = dataModel()->GetService<ScriptContext>();
 
     lua_State* L = scriptContext->state;
     int top = lua_gettop(L);

core/src/objects/service/jointsservice.cpp

@@ -18,8 +18,8 @@ void JointsService::InitService() {
     }
 }
 
-std::optional<std::shared_ptr<Workspace>> JointsService::jointWorkspace() {
-    if (!dataModel()) return std::nullopt;
+nullable std::shared_ptr<Workspace> JointsService::jointWorkspace() {
+    if (!dataModel()) return nullptr;
 
-    return dataModel().value()->FindService<Workspace>();
+    return dataModel()->FindService<Workspace>();
 }

core/src/objects/service/jointsservice.h

@@ -3,10 +3,10 @@
 #include "objects/annotation.h"
 #include "objects/base/service.h"
 
-class DEF_INST_SERVICE JointsService : public Service {
+class DEF_INST_SERVICE_(hidden) JointsService : public Service {
     AUTOGEN_PREAMBLE
 private:
-    std::optional<std::shared_ptr<Workspace>> jointWorkspace();
+    nullable std::shared_ptr<Workspace> jointWorkspace();
 protected:
     void InitService() override;
     bool initialized = false;

core/src/objects/service/script/scriptcontext.cpp

@@ -7,6 +7,7 @@
 #include "objects/datamodel.h"
 #include "objects/service/workspace.h"
 #include "timeutil.h"
+#include <chrono>
 #include <ctime>
 #include <string>
 #include "luaapis.h" // IWYU pragma: keep
@@ -15,6 +16,7 @@ const char* WRAPPER_SRC = "local func, errhandler = ... return function(...) loc
 
 int g_wait(lua_State*);
 int g_delay(lua_State*);
+int g_tick(lua_State*);
 static int g_print(lua_State*);
 static int g_require(lua_State*);
 static const luaL_Reg luaglobals [] = {
@@ -59,10 +61,10 @@ void ScriptContext::InitService() {
     // Add other globals
     lua_getglobal(state, "_G");
 
-    InstanceRef(dataModel().value()).PushLuaValue(state);
+    InstanceRef(dataModel()).PushLuaValue(state);
     lua_setfield(state, -2, "game");
 
-    InstanceRef(dataModel().value()->GetService<Workspace>()).PushLuaValue(state);
+    InstanceRef(dataModel()->GetService<Workspace>()).PushLuaValue(state);
     lua_setfield(state, -2, "workspace");
 
     lua_pushlightuserdata(state, this);
@@ -73,6 +75,9 @@ void ScriptContext::InitService() {
     lua_pushcclosure(state, g_delay, 1);
     lua_setfield(state, -2, "delay");
 
+    lua_pushcclosure(state, g_tick, 0);
+    lua_setfield(state, -2, "tick");
+
     lua_pop(state, 1); // _G
 
     // Add wrapper function
@@ -243,3 +248,13 @@ int g_delay(lua_State* L) {
 
     return 0;
 }
+
+int g_tick(lua_State* L) {
+    std::chrono::time_point now_local = std::chrono::current_zone()->to_local(std::chrono::system_clock::now());
+    std::chrono::microseconds us = std::chrono::duration_cast<std::chrono::microseconds>(now_local.time_since_epoch());
+    uint64_t _10millis = us.count() / 100;
+    double secs = (double)_10millis / 10000;
+
+    lua_pushnumber(L, secs);
+    return 1;
+}

core/src/objects/service/script/scriptcontext.h

@@ -15,7 +15,7 @@ struct SleepingThread {
 
 class Script;
 
-class DEF_INST_SERVICE ScriptContext : public Service {
+class DEF_INST_SERVICE_(hidden) ScriptContext : public Service {
     AUTOGEN_PREAMBLE
 
     std::vector<SleepingThread> sleepingThreads;

core/src/objects/service/script/serverscriptservice.cpp

@@ -14,7 +14,7 @@ void ServerScriptService::InitService() {
 }
 
 void ServerScriptService::OnRun() {
-    auto workspace = dataModel().value()->GetService<Workspace>();
+    auto workspace = dataModel()->GetService<Workspace>();
     for (auto it = workspace->GetDescendantsStart(); it != workspace->GetDescendantsEnd(); it++) {
         if (!it->IsA<Script>()) continue;
         it->CastTo<Script>().expect()->Run();

core/src/objects/service/script/serverscriptservice.h

@@ -5,7 +5,7 @@
 
 // Container class for server scripts
 // Also handles/manages running server scripts on run
-class DEF_INST_SERVICE_(explorer_icon="server-scripts") ServerScriptService : public Service {
+class DEF_INST_SERVICE_(explorer_icon="server-scripts", hidden) ServerScriptService : public Service {
     AUTOGEN_PREAMBLE
 protected:
     void InitService() override;

core/src/objects/service/selection.h

@@ -6,7 +6,7 @@
 #include <memory>
 #include <vector>
 
-class DEF_INST_SERVICE Selection : public Service {
+class DEF_INST_SERVICE_(hidden) Selection : public Service {
     AUTOGEN_PREAMBLE
 private:
     std::vector<std::shared_ptr<Instance>> selection;

core/src/objects/service/workspace.cpp

@@ -9,82 +9,20 @@
 #include "objects/service/jointsservice.h"
 #include "objects/joint/jointinstance.h"
 #include "objects/datamodel.h"
-#include "physics/util.h"
 #include "timeutil.h"
 #include <memory>
-#include <reactphysics3d/collision/CollisionCallback.h>
-#include <reactphysics3d/collision/OverlapCallback.h>
-#include <reactphysics3d/engine/PhysicsCommon.h>
 
-rp::PhysicsCommon* Workspace::physicsCommon = new rp::PhysicsCommon;
-
-Workspace::Workspace(): Service(&TYPE), physicsEventListener(this) {
-    physicsWorld = physicsCommon->createPhysicsWorld();
+Workspace::Workspace(): Service(&TYPE), physicsWorld(std::make_shared<PhysWorld>()) {
 }
 
-Workspace::~Workspace() {
-    if (physicsCommon)
-        physicsCommon->destroyPhysicsWorld(physicsWorld);
-}
-
-PhysicsEventListener::PhysicsEventListener(Workspace* parent) : workspace(parent) {}
-
-void PhysicsEventListener::onContact(const rp::CollisionCallback::CallbackData& data) {
-    workspace->contactQueueLock.lock();
-    for (size_t i = 0; i < data.getNbContactPairs(); i++) {
-        auto pair = data.getContactPair(i);
-        auto type = pair.getEventType();
-        if (type == rp::CollisionCallback::ContactPair::EventType::ContactStay) continue;
-
-        if (type == reactphysics3d::CollisionCallback::ContactPair::EventType::ContactStay)
-            continue;
-
-        ContactItem contact;
-        contact.part0 = reinterpret_cast<BasePart*>(pair.getBody1()->getUserData())->shared<BasePart>();
-        contact.part1 = reinterpret_cast<BasePart*>(pair.getBody2()->getUserData())->shared<BasePart>();
-        contact.action = type == reactphysics3d::CollisionCallback::ContactPair::EventType::ContactStart ? ContactItem::CONTACTITEM_TOUCHED : ContactItem::CONTACTITEM_TOUCHENDED;
-
-        workspace->contactQueue.push(contact);
-    }
-    workspace->contactQueueLock.unlock();
-}
-
-void PhysicsEventListener::onTrigger(const rp::OverlapCallback::CallbackData& data) {
-    workspace->contactQueueLock.lock();
-    for (size_t i = 0; i < data.getNbOverlappingPairs(); i++) {
-        auto pair = data.getOverlappingPair(i);
-        auto type = pair.getEventType();
-        if (type == rp::OverlapCallback::OverlapPair::EventType::OverlapStay) continue;
-
-        auto part0 = reinterpret_cast<BasePart*>(pair.getBody1()->getUserData())->shared<BasePart>();
-        auto part1 = reinterpret_cast<BasePart*>(pair.getBody2()->getUserData())->shared<BasePart>();
-
-        if (type == reactphysics3d::OverlapCallback::OverlapPair::EventType::OverlapStart) {
-            part0->Touched->Fire({ (Variant)InstanceRef(part1) });
-            part1->Touched->Fire({ (Variant)InstanceRef(part0) });
-        } else if (type == reactphysics3d::OverlapCallback::OverlapPair::EventType::OverlapExit) {
-            part0->TouchEnded->Fire({ (Variant)InstanceRef(part1) });
-            part1->TouchEnded->Fire({ (Variant)InstanceRef(part0) });
-        }
-    }
-    workspace->contactQueueLock.unlock();
-}
+Workspace::~Workspace() =  default;
 
 void Workspace::InitService() {
     if (initialized) return;
     initialized = true;
 
-    physicsWorld->setGravity(rp::Vector3(0, -196.2, 0));
-    // world->setContactsPositionCorrectionTechnique(rp3d::ContactsPositionCorrectionTechnique::BAUMGARTE_CONTACTS);
-    physicsWorld->setNbIterationsPositionSolver(2000);
-    physicsWorld->setNbIterationsVelocitySolver(2000);
-    // physicsWorld->setSleepLinearVelocity(10);
-    // physicsWorld->setSleepAngularVelocity(5);
-
-    physicsWorld->setEventListener(&physicsEventListener);
-
     // Create meshes
-    WedgePart::createWedgeShape(physicsCommon);
+    // WedgePart::createWedgeShape(physicsCommon);
 }
 
 void Workspace::OnRun() {
@@ -103,211 +41,29 @@ void Workspace::OnRun() {
         joint->UpdateProperty("Part0");
     }
 
-    for (auto obj : dataModel().value()->GetService<JointsService>()->GetChildren()) {
+    for (auto obj : dataModel()->GetService<JointsService>()->GetChildren()) {
         if (!obj->IsA<JointInstance>()) continue;
         std::shared_ptr<JointInstance> joint = obj->CastTo<JointInstance>().expect();
         joint->UpdateProperty("Part0");
     }
 }
 
-void Workspace::updatePartPhysics(std::shared_ptr<BasePart> part) {
-    rp::Transform transform = part->cframe;
-    if (!part->rigidBody) {
-        part->rigidBody = physicsWorld->createRigidBody(transform);
-    } else {
-        part->rigidBody->setTransform(transform);
-    }
-
-    part->updateCollider(physicsCommon);
-
-    part->rigidBody->setType(part->anchored ? rp::BodyType::STATIC : rp::BodyType::DYNAMIC);
-    part->rigidBody->getCollider(0)->setCollisionCategoryBits(0b11);
-
-    part->rigidBody->getCollider(0)->setIsSimulationCollider(part->canCollide);
-    part->rigidBody->getCollider(0)->setIsTrigger(!part->canCollide);
-
-    rp::Material& material = part->rigidBody->getCollider(0)->getMaterial();
-    material.setFrictionCoefficient(0.35);
-    material.setMassDensity(1.f);
-
-    //https://github.com/DanielChappuis/reactphysics3d/issues/170#issuecomment-691514860
-    part->rigidBody->updateMassFromColliders();
-    part->rigidBody->updateLocalInertiaTensorFromColliders();
-
-    part->rigidBody->setLinearVelocity(part->velocity);
-    // part->rigidBody->setMass(density * part->size.x * part->size.y * part->size.z);
-
-    part->rigidBody->setUserData(&*part);
-}
-
-void Workspace::ProcessContactEvents() {
-    contactQueueLock.lock();
-    while (!contactQueue.empty()) {
-        ContactItem& contact = contactQueue.front();
-        contactQueue.pop();
-        if (contact.action == ContactItem::CONTACTITEM_TOUCHED) {
-            contact.part0->Touched->Fire({ (Variant)InstanceRef(contact.part1) });
-            contact.part1->Touched->Fire({ (Variant)InstanceRef(contact.part0) });
-        } else if (contact.action == ContactItem::CONTACTITEM_TOUCHENDED) {
-            contact.part0->TouchEnded->Fire({ (Variant)InstanceRef(contact.part1) });
-            contact.part1->TouchEnded->Fire({ (Variant)InstanceRef(contact.part0) });
-        }
-    }
-    contactQueueLock.unlock();
-}
-
 void Workspace::SyncPartPhysics(std::shared_ptr<BasePart> part) {
-    if (globalPhysicsLock.try_lock()) {
-        updatePartPhysics(part);
-        globalPhysicsLock.unlock();
-    } else {
-        part->rigidBodyDirty = true;
-    }
+    physicsWorld->syncBodyProperties(part);
 }
 
-tu_time_t physTime;
 void Workspace::PhysicsStep(float deltaTime) {
-    tu_time_t startTime = tu_clock_micros();
-
-    std::scoped_lock lock(globalPhysicsLock);
-    physicsWorld->update(std::min(deltaTime / 2, (1/60.f)));
-
-    // Update queued objects
-    queueLock.lock();
-    for (QueueItem item : bodyQueue) {
-        if (item.action == QueueItem::QUEUEITEM_ADD) {
-            simulatedBodies.push_back(item.part);
-            item.part->simulationTicket = --simulatedBodies.end();
-        } else if (item.part->simulationTicket.has_value()) {
-            simulatedBodies.erase(item.part->simulationTicket.value());
-            item.part->simulationTicket = std::nullopt;
-        }
-    }
-    queueLock.unlock();
-
-    // TODO: Add list of tracked parts in workspace based on their ancestry using inWorkspace property of Instance
-    for (std::shared_ptr<BasePart> part : simulatedBodies) {
-        // If the part's body is dirty, update it now instead
-        if (part->rigidBodyDirty) {
-            updatePartPhysics(part);
-            part->rigidBodyDirty = false;
-            continue;
-        }
-
-        if (!part->rigidBody) continue;
-
-        // Sync properties
-        const rp::Transform& transform = part->rigidBody->getTransform();
-        part->cframe = CFrame(transform);
-        part->velocity = part->rigidBody->getLinearVelocity();
-
-        // part->rigidBody->enableGravity(true);
-        // RotateV/Motor joint
-        for (auto& joint : part->secondaryJoints) {
-            if (joint.expired() || !joint.lock()->IsA("RotateV")) continue;
-                        
-            std::shared_ptr<JointInstance> motor = joint.lock()->CastTo<JointInstance>().expect();
-            float rate = motor->part0.lock()->GetSurfaceParamB(-motor->c0.LookVector().Unit()) * 30;
-            // part->rigidBody->enableGravity(false);
-            part->rigidBody->setAngularVelocity(-(motor->part0.lock()->cframe * motor->c0).LookVector() * rate);
-        }
+    physicsWorld->step(deltaTime);
 
+    for (std::shared_ptr<BasePart> part : physicsWorld->getSimulatedBodies()) {
         // Destroy fallen parts
         if (part->cframe.Position().Y() < this->fallenPartsDestroyHeight) {
             auto parent = part->GetParent();
             part->Destroy();
 
             // If the parent of the part is a Model, destroy it too
-            if (parent.has_value() && parent.value()->IsA("Model"))
-                parent.value()->Destroy();
+            if (parent != nullptr && parent->IsA("Model"))
+                parent->Destroy();
         }
     }
-
-    physTime = tu_clock_micros() - startTime;
-}
-
-
-RaycastResult::RaycastResult(const rp::RaycastInfo& raycastInfo)
-    : worldPoint(raycastInfo.worldPoint)
-    , worldNormal(raycastInfo.worldNormal)
-    , hitFraction(raycastInfo.hitFraction)
-    , triangleIndex(raycastInfo.triangleIndex)
-    , body(raycastInfo.body)
-    , collider(raycastInfo.collider) {}
-
-class NearestRayHit : public rp::RaycastCallback {
-    rp::Vector3 startPos;
-    std::optional<RaycastFilter> filter;
-
-    std::optional<RaycastResult> nearestHit;
-    float nearestHitDistance = -1;
-
-    // Order is not guaranteed, so we have to figure out the nearest object using a more sophisticated algorith,
-    rp::decimal notifyRaycastHit(const rp::RaycastInfo& raycastInfo) override {
-        // If the detected object is further away than the nearest object, continue.
-        int distance = (raycastInfo.worldPoint - startPos).length();
-        if (nearestHitDistance != -1 && distance >= nearestHitDistance)
-            return 1;
-
-        if (!filter) {
-            nearestHit = raycastInfo;
-            nearestHitDistance = distance;
-            return 1;
-        }
-
-        std::shared_ptr<BasePart> part = partFromBody(raycastInfo.body);
-        FilterResult result = filter.value()(part);
-        if (result == FilterResult::BLOCK) {
-            nearestHit = std::nullopt;
-            nearestHitDistance = distance;
-            return 1;
-        } else if (result == FilterResult::TARGET) {
-            nearestHit = raycastInfo;
-            nearestHitDistance = distance;
-            return 1;
-        }
-        return 1;
-    };
-
-public:
-    NearestRayHit(rp::Vector3 startPos, std::optional<RaycastFilter> filter = std::nullopt) : startPos(startPos), filter(filter) {}
-    std::optional<const RaycastResult> getNearestHit() { return nearestHit; };
-};
-
-std::optional<const RaycastResult> Workspace::CastRayNearest(glm::vec3 point, glm::vec3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits) {
-    // std::scoped_lock lock(globalPhysicsLock);
-    rp::Ray ray(glmToRp(point), glmToRp(glm::normalize(rotation)) * maxLength);
-    NearestRayHit rayHit(glmToRp(point), filter);
-    physicsWorld->raycast(ray, &rayHit, categoryMaskBits);
-    return rayHit.getNearestHit();
-}
-
-void Workspace::DestroyRigidBody(rp::RigidBody* rigidBody) {
-    std::scoped_lock lock(globalPhysicsLock);
-    physicsWorld->destroyRigidBody(rigidBody);
-}
-
-void Workspace::DestroyJoint(rp::Joint* joint) {
-    std::scoped_lock lock(globalPhysicsLock);
-    physicsWorld->destroyJoint(joint);
-}
-
-rp::Joint* Workspace::CreateJoint(const rp::JointInfo& jointInfo) {
-    std::scoped_lock lock(globalPhysicsLock);
-    rp::Joint* joint = physicsWorld->createJoint(jointInfo);
-
-    return joint;
-}
-
-void Workspace::AddBody(std::shared_ptr<BasePart> part) {
-    queueLock.lock();
-    bodyQueue.push_back({part, QueueItem::QUEUEITEM_ADD});
-    part->rigidBodyDirty = true;
-    queueLock.unlock();
-}
-
-void Workspace::RemoveBody(std::shared_ptr<BasePart> part) {
-    queueLock.lock();
-    bodyQueue.push_back({part, QueueItem::QUEUEITEM_REMOVE});
-    queueLock.unlock();
 }

core/src/objects/service/workspace.h

@@ -2,35 +2,13 @@
 
 #include "objects/annotation.h"
 #include "objects/base/service.h"
+#include "physics/world.h"
 #include "utils.h"
 #include <glm/ext/vector_float3.hpp>
 #include <list>
 #include <memory>
 #include <mutex>
 #include <queue>
-#include <reactphysics3d/body/RigidBody.h>
-#include <reactphysics3d/engine/EventListener.h>
-#include <reactphysics3d/engine/PhysicsCommon.h>
-#include <reactphysics3d/engine/PhysicsWorld.h>
-
-namespace rp = reactphysics3d;
-
-struct RaycastResult {
-    rp::Vector3 worldPoint;
-    rp::Vector3 worldNormal;
-    rp::decimal hitFraction;
-    int triangleIndex;
-    rp::Body* body;
-    rp::Collider* collider;
-
-    RaycastResult(const rp::RaycastInfo& raycastInfo);
-};
-
-enum FilterResult {
-    TARGET, // The object is captured
-    BLOCK, // The object blocks any objects behind it, but is not captured
-    PASS, // The object is transparent, ignore it
-};
 
 class BasePart;
 class Snap;
@@ -38,21 +16,6 @@ class Weld;
 class Rotate;
 class RotateV;
 
-#ifndef __SIMULATION_TICKET
-#define __SIMULATION_TICKET
-typedef std::list<std::shared_ptr<BasePart>>::iterator SimulationTicket;
-#endif
-
-typedef std::function<FilterResult(std::shared_ptr<BasePart>)> RaycastFilter;
-
-struct QueueItem {
-    std::shared_ptr<BasePart> part;
-    enum {
-        QUEUEITEM_ADD,
-        QUEUEITEM_REMOVE,
-    } action;
-};
-
 struct ContactItem {
     std::shared_ptr<BasePart> part0;
     std::shared_ptr<BasePart> part1;
@@ -62,31 +25,14 @@ struct ContactItem {
     } action;
 };
 
-class Workspace;
-class PhysicsEventListener : public rp::EventListener {
-    friend Workspace;
-    Workspace* workspace;
-
-    PhysicsEventListener(Workspace*);
-
-    void onContact(const rp::CollisionCallback::CallbackData&) override;
-    void onTrigger(const rp::OverlapCallback::CallbackData&) override;
-};
-
 class DEF_INST_SERVICE_(explorer_icon="workspace") Workspace : public Service {
     AUTOGEN_PREAMBLE
     
-    friend PhysicsEventListener;
-    
-    std::list<std::shared_ptr<BasePart>> simulatedBodies;
-    std::list<QueueItem> bodyQueue;
     std::queue<ContactItem> contactQueue;
     std::mutex contactQueueLock;
-    rp::PhysicsWorld* physicsWorld;
-    static rp::PhysicsCommon* physicsCommon;
-    PhysicsEventListener physicsEventListener;
 
-    void updatePartPhysics(std::shared_ptr<BasePart> part);
+    std::shared_ptr<PhysWorld> physicsWorld;
+    friend PhysWorld;
 protected:
     void InitService() override;
     void OnRun() override;
@@ -96,7 +42,6 @@ public:
     Workspace();
     ~Workspace();
 
-    std::mutex globalPhysicsLock;
     std::recursive_mutex queueLock;
 
     DEF_PROP float fallenPartsDestroyHeight = -500;
@@ -104,15 +49,13 @@ public:
     // static inline std::shared_ptr<Workspace> New() { return std::make_shared<Workspace>(); };
     static inline std::shared_ptr<Instance> Create() { return std::make_shared<Workspace>(); };
 
-    void AddBody(std::shared_ptr<BasePart> part);
-    void RemoveBody(std::shared_ptr<BasePart> part);
-    void DestroyRigidBody(rp::RigidBody* rigidBody);
+    inline void AddBody(std::shared_ptr<BasePart> part) { physicsWorld->addBody(part); }
+    inline void RemoveBody(std::shared_ptr<BasePart> part) { physicsWorld->removeBody(part); }
     void SyncPartPhysics(std::shared_ptr<BasePart> part);
 
-    rp::Joint* CreateJoint(const rp::JointInfo& jointInfo);
-    void DestroyJoint(rp::Joint* joint);
+    inline PhysJoint CreateJoint(PhysJointInfo& info, std::shared_ptr<BasePart> part0, std::shared_ptr<BasePart> part1) { return physicsWorld->createJoint(info, part0, part1); }
+    inline void DestroyJoint(PhysJoint joint) { physicsWorld->destroyJoint(joint); }
 
-    void ProcessContactEvents();
     void PhysicsStep(float deltaTime);
-    std::optional<const RaycastResult> CastRayNearest(glm::vec3 point, glm::vec3 rotation, float maxLength, std::optional<RaycastFilter> filter = std::nullopt, unsigned short categoryMaskBits = 0xFFFF);
+    inline std::optional<const RaycastResult> CastRayNearest(glm::vec3 point, glm::vec3 rotation, float maxLength, std::optional<RaycastFilter> filter = std::nullopt, unsigned short categoryMaskBits = 0xFFFF) { return physicsWorld->castRay(point, rotation, maxLength, filter, categoryMaskBits); }
 };

core/src/panic.cpp

@@ -1,13 +1,8 @@
 #include "panic.h"
 
-#include <cstdlib>
 #include "logger.h"
 #include "platform.h"
 
-#ifdef _NDEBUG
-#define NDEBUG
-#endif
-
 bool trySafeAbort = false;
 void panic() {
     // We've already been here, safe aborting has failed.

core/src/partassembly.cpp

@@ -61,7 +61,7 @@ PartAssembly PartAssembly::FromSelection(std::shared_ptr<Selection> selection) {
 
 void PartAssembly::SetCollisionsEnabled(bool enabled) {
     for (auto part : parts) {
-        part->rigidBody->getCollider(0)->setIsWorldQueryCollider(enabled);
+        part->rigidBody.setCollisionsEnabled(enabled);
     }
 }
 

core/src/physics/convert.h

@@ -0,0 +1,44 @@
+#pragma once
+#include "datatypes/cframe.h"
+#include "datatypes/vector.h"
+#include <glm/ext/vector_float3.hpp>
+#include <glm/ext.hpp>
+
+#include <Jolt/Jolt.h>
+
+template <typename T, typename F>
+T convert(F vec) = delete;
+
+// Vector3
+
+template <>
+inline Vector3 convert<Vector3>(JPH::Vec3 vec) {
+    return Vector3(vec.GetX(), vec.GetY(), vec.GetZ());
+}
+
+template <>
+inline JPH::Vec3 convert<JPH::Vec3>(Vector3 vec) {
+    return JPH::Vec3(vec.X(), vec.Y(), vec.Z());
+}
+
+template <>
+inline glm::vec3 convert<glm::vec3>(JPH::Vec3 vec) {
+    return glm::vec3(vec.GetX(), vec.GetY(), vec.GetZ());
+}
+
+template <>
+inline JPH::Vec3 convert<JPH::Vec3>(glm::vec3 vec) {
+    return JPH::Vec3(vec.x, vec.y, vec.z);
+}
+
+// Quaternion
+
+template <>
+inline glm::quat convert<glm::quat>(JPH::Quat quat) {
+    return glm::quat(quat.GetW(), quat.GetX(), quat.GetY(), quat.GetZ());
+}
+
+template <>
+inline JPH::Quat convert<JPH::Quat>(glm::quat quat) {
+    return JPH::Quat(quat.x, quat.y, quat.z, quat.w);
+}

core/src/physics/util.h

@@ -1,39 +0,0 @@
-#pragma once
-#include <glm/ext/vector_float3.hpp>
-#include <memory>
-#include <reactphysics3d/body/Body.h>
-#include <reactphysics3d/mathematics/Matrix3x3.h>
-#include <reactphysics3d/mathematics/Quaternion.h>
-#include <reactphysics3d/mathematics/Vector3.h>
-#include <reactphysics3d/mathematics/mathematics.h>
-#include <glm/ext.hpp>
-#include "objects/part/part.h"
-
-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(quat.w, rp::Vector3(quat.x, quat.y, quat.z));
-}
-
-// inline rp::Matrix3x3 glmToRp(glm::mat3 mat) {
-//     return (rp::Quaternion)glmToRp((glm::quat)mat);
-// }
-
-inline glm::vec3 rpToGlm(rp::Vector3 vec) {
-    return glm::vec3(vec.x, vec.y, vec.z);
-}
-
-inline glm::quat rpToGlm(rp::Quaternion quat) {
-    return glm::quat(quat.w, quat.x, quat.y, quat.z);
-}
-
-// Make this std::optional
-inline std::shared_ptr<BasePart> partFromBody(rp::Body* body) {
-    BasePart* raw = reinterpret_cast<BasePart*>(body->getUserData());
-    std::shared_ptr<BasePart> shared = std::dynamic_pointer_cast<BasePart>(raw->shared_from_this());    
-    return shared;
-}

core/src/physics/world.cpp

@@ -0,0 +1,346 @@
+#include "world.h"
+#include "datatypes/vector.h"
+#include "enum/part.h"
+#include "logger.h"
+#include "objects/part/basepart.h"
+#include "objects/part/part.h"
+#include "objects/part/wedgepart.h"
+#include "objects/service/workspace.h"
+#include "physics/convert.h"
+#include "timeutil.h"
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Core/JobSystemThreadPool.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Core/Factory.h>
+#include <Jolt/Core/Memory.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Body/BodyInterface.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/EActivation.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/RegisterTypes.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Body/BodyFilter.h>
+#include <Jolt/Physics/Body/BodyLockInterface.h>
+#include <Jolt/Physics/Collision/NarrowPhaseQuery.h>
+#include <Jolt/Physics/Constraints/FixedConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <memory>
+
+static JPH::TempAllocator* allocator;
+static JPH::JobSystem* jobSystem;
+
+namespace Layers
+{
+	static constexpr JPH::ObjectLayer DYNAMIC = 0;
+	static constexpr JPH::ObjectLayer ANCHORED = 1;
+	static constexpr JPH::ObjectLayer NOCOLLIDE = 2;
+	// static constexpr JPH::ObjectLayer NUM_LAYERS = 3;
+};
+
+namespace BPLayers
+{
+	static constexpr JPH::BroadPhaseLayer ANCHORED(0);
+	static constexpr JPH::BroadPhaseLayer DYNAMIC(1);
+	static constexpr JPH::BroadPhaseLayer NOCOLLIDE(2);
+	static constexpr uint NUM_LAYERS(3);
+};
+
+static JPH::Ref<JPH::Shape> wedgeShape;
+
+void physicsInit() {
+    JPH::RegisterDefaultAllocator();
+    JPH::Factory::sInstance = new JPH::Factory();
+    JPH::RegisterTypes();
+
+    allocator = new JPH::TempAllocatorImpl(10 * 1024 * 1024);
+    jobSystem = new JPH::JobSystemThreadPool(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers, std::thread::hardware_concurrency() - 1);
+
+    // Create special shapes
+    JPH::Array<JPH::Vec3> wedgeVerts;
+    wedgeVerts.push_back({-1, -1, -1});
+    wedgeVerts.push_back({ 1, -1, -1});
+    wedgeVerts.push_back({-1, -1,  1});
+    wedgeVerts.push_back({ 1, -1,  1});
+    wedgeVerts.push_back({ 1,  1,  1});
+    wedgeVerts.push_back({-1,  1,  1});
+    // // Invisible bevel to avoid phasing
+    // wedgeVerts.push_back({1, 1, 0.9});
+    // wedgeVerts.push_back({0, 1, 0.9});
+
+    wedgeShape = JPH::ConvexHullShapeSettings(wedgeVerts).Create().Get();	
+}
+
+void physicsDeinit() {
+    JPH::UnregisterTypes();
+    delete JPH::Factory::sInstance;
+    JPH::Factory::sInstance = nullptr;
+}
+
+PhysWorld::PhysWorld() {
+    worldImpl.Init(4096, 0, 4096, 4096, broadPhaseLayerInterface, objectBroadPhasefilter, objectLayerPairFilter);
+    worldImpl.SetGravity(JPH::Vec3(0, -196, 0));
+	JPH::PhysicsSettings settings = worldImpl.GetPhysicsSettings();
+	// settings.mPointVelocitySleepThreshold = 0.04f; // Fix parts not sleeping
+    // settings.mNumVelocitySteps *= 20;
+    // settings.mNumPositionSteps *= 20;
+	worldImpl.SetPhysicsSettings(settings);
+}
+
+PhysWorld::~PhysWorld() {
+}
+
+void PhysWorld::addBody(std::shared_ptr<BasePart> part) {
+    syncBodyProperties(part);
+}
+
+void PhysWorld::removeBody(std::shared_ptr<BasePart> part) {
+    JPH::BodyInterface& interface = worldImpl.GetBodyInterface();
+
+    // https://jrouwe.github.io/JoltPhysics/index.html#sleeping-bodies
+    // Wake sleeping bodies in its area before removing it
+    Vector3 aabbSize = part->GetAABB();
+    interface.ActivateBodiesInAABox(JPH::AABox(convert<JPH::Vec3>(part->position() - aabbSize), convert<JPH::Vec3>(part->position() + aabbSize)), {}, {});
+
+    interface.RemoveBody(part->rigidBody.bodyImpl->GetID());
+    interface.DestroyBody(part->rigidBody.bodyImpl->GetID());
+    part->rigidBody.bodyImpl = nullptr;
+}
+
+JPH::Shape* makeShape(std::shared_ptr<BasePart> basePart) {
+    if (std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(basePart)) {
+        switch (part->shape) {
+        case PartType::Block:
+            return new JPH::BoxShape(convert<JPH::Vec3>(part->size / 2.f), JPH::cDefaultConvexRadius);
+        case PartType::Ball:
+            return new JPH::SphereShape(glm::min(part->size.X(), part->size.Y(), part->size.Z()) / 2.f);
+        case PartType::Cylinder:
+            return new JPH::RotatedTranslatedShape(JPH::Vec3(), JPH::Quat::sEulerAngles(JPH::Vec3(0, 0, JPH::JPH_PI * 0.5)), new JPH::CylinderShape(part->size.X() / 2.f, glm::min(part->size.Z(), part->size.Y()) / 2.f));
+        }
+    } else if (std::shared_ptr<WedgePart> part = std::dynamic_pointer_cast<WedgePart>(basePart)) {
+        return new JPH::ScaledShape(wedgeShape, convert<JPH::Vec3>(part->size / 2.f));
+    }
+    return nullptr;
+}
+
+void PhysWorld::syncBodyProperties(std::shared_ptr<BasePart> part) {
+    JPH::BodyInterface& interface = worldImpl.GetBodyInterface();
+
+    JPH::EMotionType motionType = part->anchored ? JPH::EMotionType::Static : JPH::EMotionType::Dynamic;
+    JPH::EActivation activationMode = part->anchored ? JPH::EActivation::DontActivate : JPH::EActivation::Activate;
+    JPH::ObjectLayer objectLayer = !part->canCollide ? Layers::NOCOLLIDE : (part->anchored ? Layers::ANCHORED : Layers::DYNAMIC);
+
+    JPH::Body* body = part->rigidBody.bodyImpl;
+
+    // Generate a new rigidBody
+    if (body == nullptr) {
+        JPH::Shape* shape = makeShape(part);
+        JPH::BodyCreationSettings settings(shape, convert<JPH::Vec3>(part->position()), convert<JPH::Quat>((glm::quat)part->cframe.RotMatrix()), motionType, objectLayer);
+        settings.mAllowDynamicOrKinematic = true;
+        settings.mRestitution = 0.5;
+
+        body = interface.CreateBody(settings);
+        body->SetUserData((JPH::uint64)part.get());
+        part->rigidBody.bodyImpl = body;
+
+        interface.AddBody(body->GetID(), activationMode);
+        interface.SetLinearVelocity(body->GetID(), convert<JPH::Vec3>(part->velocity));
+    } else {
+        std::shared_ptr<Part> part2 = std::dynamic_pointer_cast<Part>(part);
+        bool shouldUpdateShape = (part2 != nullptr && part->rigidBody._lastShape != part2->shape) || part->rigidBody._lastSize == part->size;
+
+        if (shouldUpdateShape) {
+            // const JPH::Shape* oldShape = body->GetShape();
+            JPH::Shape* newShape = makeShape(part);
+
+            interface.SetShape(body->GetID(), newShape, true, activationMode);
+            // Seems like Jolt manages its memory for us, so we don't need the below
+            // delete oldShape;
+        }
+
+        interface.SetObjectLayer(body->GetID(), objectLayer);        
+        interface.SetMotionType(body->GetID(), motionType, activationMode);
+        interface.SetPositionRotationAndVelocity(body->GetID(), convert<JPH::Vec3>(part->position()), convert<JPH::Quat>((glm::quat)part->cframe.RotMatrix()), convert<JPH::Vec3>(part->velocity), /* Angular velocity is NYI: */ body->GetAngularVelocity());
+    }
+
+    part->rigidBody._lastSize = part->size;
+    if (std::shared_ptr<Part> part2 = std::dynamic_pointer_cast<Part>(part)) part->rigidBody._lastShape = part2->shape;
+}
+
+tu_time_t physTime;
+void PhysWorld::step(float deltaTime) {
+    tu_time_t startTime = tu_clock_micros();
+    // Depending on the load, it may be necessary to call this with a differing collision step count
+    // 5 seems to be a good number supporting the high gravity
+    worldImpl.Update(deltaTime, 5, allocator, jobSystem);
+
+    JPH::BodyInterface& interface = worldImpl.GetBodyInterface();
+    JPH::BodyIDVector bodyIDs;
+    worldImpl.GetBodies(bodyIDs);
+    for (JPH::BodyID bodyID : bodyIDs) {
+        std::shared_ptr<BasePart> part = ((Instance*)interface.GetUserData(bodyID))->shared<BasePart>();
+        part->cframe = CFrame(convert<Vector3>(interface.GetPosition(bodyID)), convert<glm::quat>(interface.GetRotation(bodyID)));
+    }
+
+    physTime = tu_clock_micros() - startTime;
+}
+
+PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr<BasePart> part0, std::shared_ptr<BasePart> part1) {
+    if (part0->rigidBody.bodyImpl == nullptr
+        || part1->rigidBody.bodyImpl == nullptr
+        || !part0->workspace()
+        || !part1->workspace()
+        || part0->workspace()->physicsWorld != shared_from_this()
+        || part1->workspace()->physicsWorld != shared_from_this()
+    ) { Logger::fatalError("Failed to create joint between two parts due to the call being invalid"); panic(); };
+
+    JPH::TwoBodyConstraint* constraint;
+    if (PhysFixedJointInfo* info = dynamic_cast<PhysFixedJointInfo*>(&type)) {
+        JPH::FixedConstraintSettings settings;
+        settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+        settings.mPoint1 = convert<JPH::Vec3>(info->c0.Position());
+        settings.mAxisX1 = convert<JPH::Vec3>(info->c0.RightVector());
+        settings.mAxisY1 = convert<JPH::Vec3>(info->c0.UpVector());
+        settings.mPoint2 = convert<JPH::Vec3>(info->c1.Position());
+        settings.mAxisX2 = convert<JPH::Vec3>(info->c1.RightVector());
+        settings.mAxisY2 = convert<JPH::Vec3>(info->c1.UpVector());
+        constraint = settings.Create(*part0->rigidBody.bodyImpl, *part1->rigidBody.bodyImpl);
+    } else if (PhysRotatingJointInfo* info = dynamic_cast<PhysRotatingJointInfo*>(&type)) {
+        JPH::HingeConstraintSettings settings;
+        settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+        settings.mPoint1 = convert<JPH::Vec3>(info->c0.Position());
+        settings.mNormalAxis1 = convert<JPH::Vec3>(info->c0.RightVector());
+        settings.mHingeAxis1 = convert<JPH::Vec3>(info->c0.LookVector());
+        settings.mPoint2 = convert<JPH::Vec3>(info->c1.Position());
+        settings.mNormalAxis2 = convert<JPH::Vec3>(info->c1.RightVector());
+        settings.mHingeAxis2 = convert<JPH::Vec3>(info->c1.LookVector());
+        // settings.mMotorSettings = JPH::MotorSettings(1.0f, 1.0f);
+        constraint = settings.Create(*part0->rigidBody.bodyImpl, *part1->rigidBody.bodyImpl);
+        if (info->motorized) {
+            static_cast<JPH::HingeConstraint*>(constraint)->SetMotorState(JPH::EMotorState::Velocity);
+            static_cast<JPH::HingeConstraint*>(constraint)->SetTargetAngularVelocity(-info->initialVelocity);
+        }
+    } else {
+        panic();
+    }
+
+    worldImpl.AddConstraint(constraint);
+    return { constraint };
+}
+
+// WATCH OUT! This should only be called for HingeConstraints.
+// Can't use dynamic_cast because TwoBodyConstraint is not virtual
+void PhysJoint::setAngularVelocity(float velocity) {
+    JPH::HingeConstraint* constraint = static_cast<JPH::HingeConstraint*>(jointImpl);
+    if (!constraint) return;
+    constraint->SetTargetAngularVelocity(-velocity);
+}
+
+void PhysWorld::destroyJoint(PhysJoint joint) {
+    worldImpl.RemoveConstraint(joint.jointImpl);
+}
+
+class PhysRayCastBodyFilter : public JPH::BodyFilter {
+    bool ShouldCollideLocked(const JPH::Body &inBody) const override {
+        std::shared_ptr<BasePart> part = ((Instance*)inBody.GetUserData())->shared<BasePart>();
+
+        // Ignore specifically "hidden" parts from raycast
+        // TODO: Replace this with a better system... Please.
+        if (!part->rigidBody.isCollisionsEnabled()) return false;
+
+        return true;
+    }
+};
+
+std::optional<const RaycastResult> PhysWorld::castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits) {
+    if (filter != std::nullopt) { Logger::fatalError("The filter property of PhysWorld::castRay is not yet implemented"); panic(); };
+
+    const JPH::BodyLockInterface& lockInterface = worldImpl.GetBodyLockInterfaceNoLock();
+    const JPH::BodyInterface& interface = worldImpl.GetBodyInterface();
+    const JPH::NarrowPhaseQuery& query = worldImpl.GetNarrowPhaseQuery();
+
+    // First we cast a ray to find a matching part
+    Vector3 end = point + rotation.Unit() * maxLength;
+    JPH::RRayCast ray { convert<JPH::Vec3>(point), convert<JPH::Vec3>(end) };
+    JPH::RayCastResult result;
+    PhysRayCastBodyFilter bodyFilter;
+    bool hitFound = query.CastRay(ray, result, {}, {}, bodyFilter);
+    
+    // No matches found, return empty
+    if (!hitFound) return std::nullopt;
+
+    // Next we cast a ray to find the hit surface and its world position and normal
+    JPH::BodyID hitBodyId = result.mBodyID;
+    std::shared_ptr<BasePart> part = ((Instance*)interface.GetUserData(hitBodyId))->shared<BasePart>();
+    const JPH::Shape* shape = interface.GetShape(hitBodyId);
+
+    // Find the hit position and hence the surface normal of the shape at that specific point
+    Vector3 hitPosition = point + rotation.Unit() * (maxLength * result.mFraction);
+    JPH::Vec3 surfaceNormal = shape->GetSurfaceNormal(result.mSubShapeID2, convert<JPH::Vec3>(part->cframe.Inverse() * hitPosition));
+    Vector3 worldNormal = part->cframe.Rotation() * convert<Vector3>(surfaceNormal);
+
+    return RaycastResult {
+        .worldPoint = hitPosition,
+        .worldNormal = worldNormal,
+        .body = lockInterface.TryGetBody(hitBodyId),
+        .hitPart = part,
+    };
+}
+
+uint BroadPhaseLayerInterface::GetNumBroadPhaseLayers() const {
+    return BPLayers::NUM_LAYERS;
+}
+
+JPH::BroadPhaseLayer BroadPhaseLayerInterface::GetBroadPhaseLayer(JPH::ObjectLayer inLayer) const {
+    switch (inLayer) {
+        case Layers::DYNAMIC: return BPLayers::DYNAMIC;
+        case Layers::ANCHORED: return BPLayers::ANCHORED;
+        case Layers::NOCOLLIDE: return BPLayers::NOCOLLIDE;
+        default: panic();
+    }
+}
+
+const char * BroadPhaseLayerInterface::GetBroadPhaseLayerName(JPH::BroadPhaseLayer inLayer) const {
+    using T = JPH::BroadPhaseLayer::Type;
+    switch ((T)inLayer) {
+        case (T)BPLayers::DYNAMIC: return "DYNAMIC";
+        case (T)BPLayers::ANCHORED: return "ANCHORED";
+        case (T)BPLayers::NOCOLLIDE: return "NOCOLLIDE";
+        default: panic();
+    }
+}
+
+bool ObjectBroadPhaseFilter::ShouldCollide(JPH::ObjectLayer inLayer1, JPH::BroadPhaseLayer inLayer2) const {
+    using T = JPH::BroadPhaseLayer::Type;
+    switch ((T)inLayer2) {
+    case (T)BPLayers::DYNAMIC: return true;
+    case (T)BPLayers::ANCHORED: return true;
+    case (T)BPLayers::NOCOLLIDE: return false;
+    default: panic();
+    }
+}
+
+bool ObjectLayerPairFilter::ShouldCollide(JPH::ObjectLayer inLayer1, JPH::ObjectLayer inLayer2) const {
+    switch (inLayer1) {
+    case Layers::DYNAMIC:
+        return true;
+    case Layers::ANCHORED:
+        return inLayer2 == Layers::DYNAMIC;
+    case Layers::NOCOLLIDE:
+        return false;
+    default:
+        panic();
+    }
+}

core/src/physics/world.h

@@ -0,0 +1,126 @@
+#pragma once
+
+#include "datatypes/cframe.h"
+#include "datatypes/vector.h"
+#include "enum/part.h"
+#include "utils.h"
+#include <functional>
+#include <list>
+#include <memory>
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+
+class BasePart;
+class PhysWorld;
+
+struct PhysJointInfo { virtual ~PhysJointInfo() = default; protected: PhysJointInfo() = default; };
+
+struct PhysFixedJointInfo : PhysJointInfo {
+    CFrame c0;
+    CFrame c1;
+    
+    inline PhysFixedJointInfo(CFrame c0, CFrame c1) : c0(c0), c1(c1) {}
+};
+
+struct PhysRotatingJointInfo : PhysJointInfo {
+    CFrame c0;
+    CFrame c1;
+    bool motorized;
+    float initialVelocity;
+    
+    inline PhysRotatingJointInfo(CFrame c0, CFrame c1) : c0(c0), c1(c1), motorized(false), initialVelocity(0.f){}
+    inline PhysRotatingJointInfo(CFrame c0, CFrame c1, float initialVelocity) : c0(c0), c1(c1), motorized(true), initialVelocity(initialVelocity) {}
+};
+
+class PhysWorld;
+struct PhysJoint {
+public:
+    JPH::TwoBodyConstraint* jointImpl;
+
+    void setAngularVelocity(float velocity);
+};
+
+struct RaycastResult;
+class PhysRigidBody {
+    JPH::Body* bodyImpl = nullptr;
+    inline PhysRigidBody(JPH::Body* rigidBody) : bodyImpl(rigidBody) {}
+    Vector3 _lastSize;
+    PartType _lastShape;
+    bool collisionsEnabled = true;
+
+    friend PhysWorld;
+    friend RaycastResult;
+public:
+    inline PhysRigidBody() {}
+
+    inline void setActive(bool active) { if (!bodyImpl) return; }
+    inline void setCollisionsEnabled(bool enabled) { collisionsEnabled = enabled; }
+    inline bool isCollisionsEnabled() { return collisionsEnabled; }
+    void updateCollider(std::shared_ptr<BasePart>);
+};
+
+// // Provides internal implementation-specific values from the raycast result
+// struct RaycastResultInternal {
+//     rp::decimal hitFraction;
+//     rp::Collider* collider;
+// };
+
+struct RaycastResult {
+    Vector3 worldPoint;
+    Vector3 worldNormal;
+    PhysRigidBody body;
+    nullable std::shared_ptr<BasePart> hitPart;
+};
+
+enum FilterResult {
+    TARGET, // The object is captured
+    BLOCK, // The object blocks any objects behind it, but is not captured
+    PASS, // The object is transparent, ignore it
+};
+
+class BasePart;
+typedef std::function<FilterResult(std::shared_ptr<BasePart>)> RaycastFilter;
+
+class BroadPhaseLayerInterface : public JPH::BroadPhaseLayerInterface {
+    uint GetNumBroadPhaseLayers() const override;
+    JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer inLayer) const override;
+    const char * GetBroadPhaseLayerName(JPH::BroadPhaseLayer inLayer) const override;
+};
+
+class ObjectBroadPhaseFilter : public JPH::ObjectVsBroadPhaseLayerFilter {
+    bool ShouldCollide(JPH::ObjectLayer inLayer1, JPH::BroadPhaseLayer inLayer2) const override;
+};
+
+class ObjectLayerPairFilter : public JPH::ObjectLayerPairFilter {
+    bool ShouldCollide(JPH::ObjectLayer inLayer1, JPH::ObjectLayer inLayer2) const override;
+};
+
+class PhysWorld : public std::enable_shared_from_this<PhysWorld> {
+    BroadPhaseLayerInterface broadPhaseLayerInterface;
+    ObjectBroadPhaseFilter objectBroadPhasefilter;
+    ObjectLayerPairFilter objectLayerPairFilter;
+    JPH::PhysicsSystem worldImpl;
+    std::list<std::shared_ptr<BasePart>> simulatedBodies;
+
+public:
+    PhysWorld();
+    ~PhysWorld();
+
+    void step(float deltaTime);
+    
+    void addBody(std::shared_ptr<BasePart>);
+    void removeBody(std::shared_ptr<BasePart>);
+
+    PhysJoint createJoint(PhysJointInfo& type, std::shared_ptr<BasePart> part0, std::shared_ptr<BasePart> part1);
+    void destroyJoint(PhysJoint joint);
+
+    inline const std::list<std::shared_ptr<BasePart>>& getSimulatedBodies() { return simulatedBodies; }
+    void syncBodyProperties(std::shared_ptr<BasePart>);
+    std::optional<const RaycastResult> castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits);
+};
+
+void physicsInit();
+void physicsDeinit();

core/src/rendering/defaultmeshes.cpp

@@ -2968,7 +2968,7 @@ static float OUTLINE_VERTICES[] = {
 
 };
 
-static float CYLINDER_VERTICES[] = {
+static float CYLINDER_CHEAP_VERTICES[] = {
     // positions                     // normals                  // texture coords
 
         0.0, -0.5, 0.5, 0.2588, -0.9659, -0.0,  1.0, 0.5,
@@ -3106,11 +3106,391 @@ static float CYLINDER_VERTICES[] = {
 
 };
 
+
+static float CYLINDER_VERTICES[] = {
+    // positions                     // normals                  // texture coords
+
+	0.5, 0.0, -0.5,	-0.0, -0.0, -1.0,	1.0, 0.5,
+	0.5, 0.09754499999999999, -0.4903925,	-0.0, 0.1951, -0.9808,	0.96875, 0.5,
+	-0.5, 0.0, -0.5,	-0.0, -0.0, -1.0,	1.0, 1.0,
+	0.5, 0.09754499999999999, -0.4903925,	-0.0, 0.1951, -0.9808,	0.96875, 0.5,
+	0.5, 0.1913415, -0.46194,	-0.0, 0.3827, -0.9239,	0.9375, 0.5,
+	-0.5, 0.09754499999999999, -0.4903925,	-0.0, 0.1951, -0.9808,	0.96875, 1.0,
+	0.5, 0.1913415, -0.46194,	-0.0, 0.3827, -0.9239,	0.9375, 0.5,
+	0.5, 0.277785, -0.41573499999999997,	-0.0, 0.5556, -0.8315,	0.90625, 0.5,
+	-0.5, 0.1913415, -0.46194,	-0.0, 0.3827, -0.9239,	0.9375, 1.0,
+	0.5, 0.277785, -0.41573499999999997,	-0.0, 0.5556, -0.8315,	0.90625, 0.5,
+	0.5, 0.3535535, -0.3535535000000001,	-0.0, 0.7071, -0.7071,	0.875, 0.5,
+	-0.5, 0.277785, -0.41573499999999997,	-0.0, 0.5556, -0.8315,	0.90625, 1.0,
+	0.5, 0.3535535, -0.3535535000000001,	-0.0, 0.7071, -0.7071,	0.875, 0.5,
+	0.5, 0.415735, -0.27778499999999995,	-0.0, 0.8315, -0.5556,	0.84375, 0.5,
+	-0.5, 0.3535535, -0.3535535000000001,	-0.0, 0.7071, -0.7071,	0.875, 1.0,
+	0.5, 0.415735, -0.27778499999999995,	-0.0, 0.8315, -0.5556,	0.84375, 0.5,
+	0.5, 0.46194, -0.19134150000000005,	-0.0, 0.9239, -0.3827,	0.8125, 0.5,
+	-0.5, 0.415735, -0.27778499999999995,	-0.0, 0.8315, -0.5556,	0.84375, 1.0,
+	0.5, 0.46194, -0.19134150000000005,	-0.0, 0.9239, -0.3827,	0.8125, 0.5,
+	0.5, 0.4903925, -0.09754499999999999,	-0.0, 0.9808, -0.1951,	0.78125, 0.5,
+	-0.5, 0.4619395, -0.19134150000000005,	-0.0, 0.9239, -0.3827,	0.8125, 1.0,
+	0.5, 0.4903925, -0.09754499999999999,	-0.0, 0.9808, -0.1951,	0.78125, 0.5,
+	0.5, 0.5, 0.0,	-0.0, 1.0, -0.0,	0.75, 0.5,
+	-0.5, 0.4903925, -0.09754499999999999,	-0.0, 0.9808, -0.1951,	0.78125, 1.0,
+	0.5, 0.5, 0.0,	-0.0, 1.0, -0.0,	0.75, 0.5,
+	0.5, 0.4903925, 0.09754499999999999,	-0.0, 0.9808, 0.1951,	0.71875, 0.5,
+	-0.5, 0.5, 0.0,	-0.0, 1.0, -0.0,	0.75, 1.0,
+	0.5, 0.4903925, 0.09754499999999999,	-0.0, 0.9808, 0.1951,	0.71875, 0.5,
+	0.5, 0.46194, 0.1913415,	-0.0, 0.9239, 0.3827,	0.6875, 0.5,
+	-0.5, 0.4903925, 0.09754499999999999,	-0.0, 0.9808, 0.1951,	0.71875, 1.0,
+	0.5, 0.46194, 0.1913415,	-0.0, 0.9239, 0.3827,	0.6875, 0.5,
+	0.5, 0.415735, 0.277785,	-0.0, 0.8315, 0.5556,	0.65625, 0.5,
+	-0.5, 0.4619395, 0.1913415,	-0.0, 0.9239, 0.3827,	0.6875, 1.0,
+	0.5, 0.415735, 0.277785,	-0.0, 0.8315, 0.5556,	0.65625, 0.5,
+	0.5, 0.3535535, 0.3535535,	-0.0, 0.7071, 0.7071,	0.625, 0.5,
+	-0.5, 0.415735, 0.277785,	-0.0, 0.8315, 0.5556,	0.65625, 1.0,
+	0.5, 0.3535535, 0.3535535,	-0.0, 0.7071, 0.7071,	0.625, 0.5,
+	0.5, 0.277785, 0.415735,	-0.0, 0.5556, 0.8315,	0.59375, 0.5,
+	-0.5, 0.3535535, 0.3535535,	-0.0, 0.7071, 0.7071,	0.625, 1.0,
+	0.5, 0.277785, 0.415735,	-0.0, 0.5556, 0.8315,	0.59375, 0.5,
+	0.5, 0.1913415, 0.46194,	-0.0, 0.3827, 0.9239,	0.5625, 0.5,
+	-0.5, 0.277785, 0.415735,	-0.0, 0.5556, 0.8315,	0.59375, 1.0,
+	0.5, 0.1913415, 0.46194,	-0.0, 0.3827, 0.9239,	0.5625, 0.5,
+	0.5, 0.09754499999999999, 0.4903925,	-0.0, 0.1951, 0.9808,	0.53125, 0.5,
+	-0.5, 0.1913415, 0.46194,	-0.0, 0.3827, 0.9239,	0.5625, 1.0,
+	0.5, 0.09754499999999999, 0.4903925,	-0.0, 0.1951, 0.9808,	0.53125, 0.5,
+	0.5, 0.0, 0.5,	-0.0, -0.0, 1.0,	0.5, 0.5,
+	-0.5, 0.09754499999999999, 0.4903925,	-0.0, 0.1951, 0.9808,	0.53125, 1.0,
+	0.5, 0.0, 0.5,	-0.0, -0.0, 1.0,	0.5, 0.5,
+	0.5, -0.09754499999999999, 0.4903925,	-0.0, -0.1951, 0.9808,	0.46875, 0.5,
+	-0.5, 0.0, 0.5,	-0.0, -0.0, 1.0,	0.5, 1.0,
+	0.5, -0.09754499999999999, 0.4903925,	-0.0, -0.1951, 0.9808,	0.46875, 0.5,
+	0.5, -0.19134150000000005, 0.46194,	-0.0, -0.3827, 0.9239,	0.4375, 0.5,
+	-0.5, -0.09754499999999999, 0.4903925,	-0.0, -0.1951, 0.9808,	0.46875, 1.0,
+	0.5, -0.19134150000000005, 0.46194,	-0.0, -0.3827, 0.9239,	0.4375, 0.5,
+	0.5, -0.27778499999999995, 0.415735,	-0.0, -0.5556, 0.8315,	0.40625, 0.5,
+	-0.5, -0.19134150000000005, 0.46194,	-0.0, -0.3827, 0.9239,	0.4375, 1.0,
+	0.5, -0.27778499999999995, 0.415735,	-0.0, -0.5556, 0.8315,	0.40625, 0.5,
+	0.5, -0.3535535000000001, 0.3535535,	-0.0, -0.7071, 0.7071,	0.375, 0.5,
+	-0.5, -0.27778499999999995, 0.415735,	-0.0, -0.5556, 0.8315,	0.40625, 1.0,
+	0.5, -0.3535535000000001, 0.3535535,	-0.0, -0.7071, 0.7071,	0.375, 0.5,
+	0.5, -0.41573499999999997, 0.277785,	-0.0, -0.8315, 0.5556,	0.34375, 0.5,
+	-0.5, -0.3535535000000001, 0.3535535,	-0.0, -0.7071, 0.7071,	0.375, 1.0,
+	0.5, -0.41573499999999997, 0.277785,	-0.0, -0.8315, 0.5556,	0.34375, 0.5,
+	0.5, -0.46193949999999995, 0.1913415,	-0.0, -0.9239, 0.3827,	0.3125, 0.5,
+	-0.5, -0.41573499999999997, 0.277785,	-0.0, -0.8315, 0.5556,	0.34375, 1.0,
+	0.5, -0.46193949999999995, 0.1913415,	-0.0, -0.9239, 0.3827,	0.3125, 0.5,
+	0.5, -0.4903925, 0.09754499999999999,	-0.0, -0.9808, 0.1951,	0.28125, 0.5,
+	-0.5, -0.46194, 0.1913415,	-0.0, -0.9239, 0.3827,	0.3125, 1.0,
+	0.5, -0.4903925, 0.09754499999999999,	-0.0, -0.9808, 0.1951,	0.28125, 0.5,
+	0.5, -0.5, 0.0,	-0.0, -1.0, -0.0,	0.25, 0.5,
+	-0.5, -0.4903925, 0.09754499999999999,	-0.0, -0.9808, 0.1951,	0.28125, 1.0,
+	0.5, -0.5, 0.0,	-0.0, -1.0, -0.0,	0.25, 0.5,
+	0.5, -0.4903925, -0.09754499999999999,	-0.0, -0.9808, -0.1951,	0.21875, 0.5,
+	-0.5, -0.5, 0.0,	-0.0, -1.0, -0.0,	0.25, 1.0,
+	0.5, -0.4903925, -0.09754499999999999,	-0.0, -0.9808, -0.1951,	0.21875, 0.5,
+	0.5, -0.46193949999999995, -0.19134150000000005,	-0.0, -0.9239, -0.3827,	0.1875, 0.5,
+	-0.5, -0.4903925, -0.09754499999999999,	-0.0, -0.9808, -0.1951,	0.21875, 1.0,
+	0.5, -0.46193949999999995, -0.19134150000000005,	-0.0, -0.9239, -0.3827,	0.1875, 0.5,
+	0.5, -0.41573499999999997, -0.27778499999999995,	-0.0, -0.8315, -0.5556,	0.15625, 0.5,
+	-0.5, -0.46194, -0.19134150000000005,	-0.0, -0.9239, -0.3827,	0.1875, 1.0,
+	0.5, -0.41573499999999997, -0.27778499999999995,	-0.0, -0.8315, -0.5556,	0.15625, 0.5,
+	0.5, -0.3535535000000001, -0.3535535000000001,	-0.0, -0.7071, -0.7071,	0.125, 0.5,
+	-0.5, -0.41573499999999997, -0.27778499999999995,	-0.0, -0.8315, -0.5556,	0.15625, 1.0,
+	0.5, -0.3535535000000001, -0.3535535000000001,	-0.0, -0.7071, -0.7071,	0.125, 0.5,
+	0.5, -0.27778499999999995, -0.41573499999999997,	-0.0, -0.5556, -0.8315,	0.09375, 0.5,
+	-0.5, -0.3535535000000001, -0.3535535000000001,	-0.0, -0.7071, -0.7071,	0.125, 1.0,
+	0.5, -0.27778499999999995, -0.41573499999999997,	-0.0, -0.5556, -0.8315,	0.09375, 0.5,
+	0.5, -0.19134150000000005, -0.46194,	-0.0, -0.3827, -0.9239,	0.0625, 0.5,
+	-0.5, -0.27778499999999995, -0.41573499999999997,	-0.0, -0.5556, -0.8315,	0.09375, 1.0,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	-0.5, 0.4619395, 0.1913415,	-1.0, -0.0, -0.0,	0.471731, 0.158156,
+	-0.5, -0.19134150000000005, 0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.028269,
+	0.5, -0.19134150000000005, -0.46194,	-0.0, -0.3827, -0.9239,	0.0625, 0.5,
+	0.5, -0.09754499999999999, -0.4903925,	-0.0, -0.1951, -0.9808,	0.03125, 0.5,
+	-0.5, -0.19134150000000005, -0.46194,	-0.0, -0.3827, -0.9239,	0.0625, 1.0,
+	0.5, -0.09754499999999999, -0.4903925,	-0.0, -0.1951, -0.9808,	0.03125, 0.5,
+	0.5, 0.0, -0.5,	-0.0, -0.0, -1.0,	0.0, 0.5,
+	-0.5, -0.09754499999999999, -0.4903925,	-0.0, -0.1951, -0.9808,	0.03125, 1.0,
+	0.5, -0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.485388,
+	0.5, -0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.203178,
+	0.5, 0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.014612,
+	0.5, 0.09754499999999999, -0.4903925,	-0.0, 0.1951, -0.9808,	0.96875, 0.5,
+	-0.5, 0.09754499999999999, -0.4903925,	-0.0, 0.1951, -0.9808,	0.96875, 1.0,
+	-0.5, 0.0, -0.5,	-0.0, -0.0, -1.0,	1.0, 1.0,
+	0.5, 0.1913415, -0.46194,	-0.0, 0.3827, -0.9239,	0.9375, 0.5,
+	-0.5, 0.1913415, -0.46194,	-0.0, 0.3827, -0.9239,	0.9375, 1.0,
+	-0.5, 0.09754499999999999, -0.4903925,	-0.0, 0.1951, -0.9808,	0.96875, 1.0,
+	0.5, 0.277785, -0.41573499999999997,	-0.0, 0.5556, -0.8315,	0.90625, 0.5,
+	-0.5, 0.277785, -0.41573499999999997,	-0.0, 0.5556, -0.8315,	0.90625, 1.0,
+	-0.5, 0.1913415, -0.46194,	-0.0, 0.3827, -0.9239,	0.9375, 1.0,
+	0.5, 0.3535535, -0.3535535000000001,	-0.0, 0.7071, -0.7071,	0.875, 0.5,
+	-0.5, 0.3535535, -0.3535535000000001,	-0.0, 0.7071, -0.7071,	0.875, 1.0,
+	-0.5, 0.277785, -0.41573499999999997,	-0.0, 0.5556, -0.8315,	0.90625, 1.0,
+	0.5, 0.415735, -0.27778499999999995,	-0.0, 0.8315, -0.5556,	0.84375, 0.5,
+	-0.5, 0.415735, -0.27778499999999995,	-0.0, 0.8315, -0.5556,	0.84375, 1.0,
+	-0.5, 0.3535535, -0.3535535000000001,	-0.0, 0.7071, -0.7071,	0.875, 1.0,
+	0.5, 0.46194, -0.19134150000000005,	-0.0, 0.9239, -0.3827,	0.8125, 0.5,
+	-0.5, 0.4619395, -0.19134150000000005,	-0.0, 0.9239, -0.3827,	0.8125, 1.0,
+	-0.5, 0.415735, -0.27778499999999995,	-0.0, 0.8315, -0.5556,	0.84375, 1.0,
+	0.5, 0.4903925, -0.09754499999999999,	-0.0, 0.9808, -0.1951,	0.78125, 0.5,
+	-0.5, 0.4903925, -0.09754499999999999,	-0.0, 0.9808, -0.1951,	0.78125, 1.0,
+	-0.5, 0.4619395, -0.19134150000000005,	-0.0, 0.9239, -0.3827,	0.8125, 1.0,
+	0.5, 0.5, 0.0,	-0.0, 1.0, -0.0,	0.75, 0.5,
+	-0.5, 0.5, 0.0,	-0.0, 1.0, -0.0,	0.75, 1.0,
+	-0.5, 0.4903925, -0.09754499999999999,	-0.0, 0.9808, -0.1951,	0.78125, 1.0,
+	0.5, 0.4903925, 0.09754499999999999,	-0.0, 0.9808, 0.1951,	0.71875, 0.5,
+	-0.5, 0.4903925, 0.09754499999999999,	-0.0, 0.9808, 0.1951,	0.71875, 1.0,
+	-0.5, 0.5, 0.0,	-0.0, 1.0, -0.0,	0.75, 1.0,
+	0.5, 0.46194, 0.1913415,	-0.0, 0.9239, 0.3827,	0.6875, 0.5,
+	-0.5, 0.4619395, 0.1913415,	-0.0, 0.9239, 0.3827,	0.6875, 1.0,
+	-0.5, 0.4903925, 0.09754499999999999,	-0.0, 0.9808, 0.1951,	0.71875, 1.0,
+	0.5, 0.415735, 0.277785,	-0.0, 0.8315, 0.5556,	0.65625, 0.5,
+	-0.5, 0.415735, 0.277785,	-0.0, 0.8315, 0.5556,	0.65625, 1.0,
+	-0.5, 0.4619395, 0.1913415,	-0.0, 0.9239, 0.3827,	0.6875, 1.0,
+	0.5, 0.3535535, 0.3535535,	-0.0, 0.7071, 0.7071,	0.625, 0.5,
+	-0.5, 0.3535535, 0.3535535,	-0.0, 0.7071, 0.7071,	0.625, 1.0,
+	-0.5, 0.415735, 0.277785,	-0.0, 0.8315, 0.5556,	0.65625, 1.0,
+	0.5, 0.277785, 0.415735,	-0.0, 0.5556, 0.8315,	0.59375, 0.5,
+	-0.5, 0.277785, 0.415735,	-0.0, 0.5556, 0.8315,	0.59375, 1.0,
+	-0.5, 0.3535535, 0.3535535,	-0.0, 0.7071, 0.7071,	0.625, 1.0,
+	0.5, 0.1913415, 0.46194,	-0.0, 0.3827, 0.9239,	0.5625, 0.5,
+	-0.5, 0.1913415, 0.46194,	-0.0, 0.3827, 0.9239,	0.5625, 1.0,
+	-0.5, 0.277785, 0.415735,	-0.0, 0.5556, 0.8315,	0.59375, 1.0,
+	0.5, 0.09754499999999999, 0.4903925,	-0.0, 0.1951, 0.9808,	0.53125, 0.5,
+	-0.5, 0.09754499999999999, 0.4903925,	-0.0, 0.1951, 0.9808,	0.53125, 1.0,
+	-0.5, 0.1913415, 0.46194,	-0.0, 0.3827, 0.9239,	0.5625, 1.0,
+	0.5, 0.0, 0.5,	-0.0, -0.0, 1.0,	0.5, 0.5,
+	-0.5, 0.0, 0.5,	-0.0, -0.0, 1.0,	0.5, 1.0,
+	-0.5, 0.09754499999999999, 0.4903925,	-0.0, 0.1951, 0.9808,	0.53125, 1.0,
+	0.5, -0.09754499999999999, 0.4903925,	-0.0, -0.1951, 0.9808,	0.46875, 0.5,
+	-0.5, -0.09754499999999999, 0.4903925,	-0.0, -0.1951, 0.9808,	0.46875, 1.0,
+	-0.5, 0.0, 0.5,	-0.0, -0.0, 1.0,	0.5, 1.0,
+	0.5, -0.19134150000000005, 0.46194,	-0.0, -0.3827, 0.9239,	0.4375, 0.5,
+	-0.5, -0.19134150000000005, 0.46194,	-0.0, -0.3827, 0.9239,	0.4375, 1.0,
+	-0.5, -0.09754499999999999, 0.4903925,	-0.0, -0.1951, 0.9808,	0.46875, 1.0,
+	0.5, -0.27778499999999995, 0.415735,	-0.0, -0.5556, 0.8315,	0.40625, 0.5,
+	-0.5, -0.27778499999999995, 0.415735,	-0.0, -0.5556, 0.8315,	0.40625, 1.0,
+	-0.5, -0.19134150000000005, 0.46194,	-0.0, -0.3827, 0.9239,	0.4375, 1.0,
+	0.5, -0.3535535000000001, 0.3535535,	-0.0, -0.7071, 0.7071,	0.375, 0.5,
+	-0.5, -0.3535535000000001, 0.3535535,	-0.0, -0.7071, 0.7071,	0.375, 1.0,
+	-0.5, -0.27778499999999995, 0.415735,	-0.0, -0.5556, 0.8315,	0.40625, 1.0,
+	0.5, -0.41573499999999997, 0.277785,	-0.0, -0.8315, 0.5556,	0.34375, 0.5,
+	-0.5, -0.41573499999999997, 0.277785,	-0.0, -0.8315, 0.5556,	0.34375, 1.0,
+	-0.5, -0.3535535000000001, 0.3535535,	-0.0, -0.7071, 0.7071,	0.375, 1.0,
+	0.5, -0.46193949999999995, 0.1913415,	-0.0, -0.9239, 0.3827,	0.3125, 0.5,
+	-0.5, -0.46194, 0.1913415,	-0.0, -0.9239, 0.3827,	0.3125, 1.0,
+	-0.5, -0.41573499999999997, 0.277785,	-0.0, -0.8315, 0.5556,	0.34375, 1.0,
+	0.5, -0.4903925, 0.09754499999999999,	-0.0, -0.9808, 0.1951,	0.28125, 0.5,
+	-0.5, -0.4903925, 0.09754499999999999,	-0.0, -0.9808, 0.1951,	0.28125, 1.0,
+	-0.5, -0.46194, 0.1913415,	-0.0, -0.9239, 0.3827,	0.3125, 1.0,
+	0.5, -0.5, 0.0,	-0.0, -1.0, -0.0,	0.25, 0.5,
+	-0.5, -0.5, 0.0,	-0.0, -1.0, -0.0,	0.25, 1.0,
+	-0.5, -0.4903925, 0.09754499999999999,	-0.0, -0.9808, 0.1951,	0.28125, 1.0,
+	0.5, -0.4903925, -0.09754499999999999,	-0.0, -0.9808, -0.1951,	0.21875, 0.5,
+	-0.5, -0.4903925, -0.09754499999999999,	-0.0, -0.9808, -0.1951,	0.21875, 1.0,
+	-0.5, -0.5, 0.0,	-0.0, -1.0, -0.0,	0.25, 1.0,
+	0.5, -0.46193949999999995, -0.19134150000000005,	-0.0, -0.9239, -0.3827,	0.1875, 0.5,
+	-0.5, -0.46194, -0.19134150000000005,	-0.0, -0.9239, -0.3827,	0.1875, 1.0,
+	-0.5, -0.4903925, -0.09754499999999999,	-0.0, -0.9808, -0.1951,	0.21875, 1.0,
+	0.5, -0.41573499999999997, -0.27778499999999995,	-0.0, -0.8315, -0.5556,	0.15625, 0.5,
+	-0.5, -0.41573499999999997, -0.27778499999999995,	-0.0, -0.8315, -0.5556,	0.15625, 1.0,
+	-0.5, -0.46194, -0.19134150000000005,	-0.0, -0.9239, -0.3827,	0.1875, 1.0,
+	0.5, -0.3535535000000001, -0.3535535000000001,	-0.0, -0.7071, -0.7071,	0.125, 0.5,
+	-0.5, -0.3535535000000001, -0.3535535000000001,	-0.0, -0.7071, -0.7071,	0.125, 1.0,
+	-0.5, -0.41573499999999997, -0.27778499999999995,	-0.0, -0.8315, -0.5556,	0.15625, 1.0,
+	0.5, -0.27778499999999995, -0.41573499999999997,	-0.0, -0.5556, -0.8315,	0.09375, 0.5,
+	-0.5, -0.27778499999999995, -0.41573499999999997,	-0.0, -0.5556, -0.8315,	0.09375, 1.0,
+	-0.5, -0.3535535000000001, -0.3535535000000001,	-0.0, -0.7071, -0.7071,	0.125, 1.0,
+	0.5, -0.19134150000000005, -0.46194,	-0.0, -0.3827, -0.9239,	0.0625, 0.5,
+	-0.5, -0.19134150000000005, -0.46194,	-0.0, -0.3827, -0.9239,	0.0625, 1.0,
+	-0.5, -0.27778499999999995, -0.41573499999999997,	-0.0, -0.5556, -0.8315,	0.09375, 1.0,
+	-0.5, 0.0, -0.5,	-1.0, -0.0, -0.0,	0.25, 0.49,
+	-0.5, 0.09754499999999999, -0.4903925,	-1.0, -0.0, -0.0,	0.296822, 0.485388,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	-0.5, -0.09754499999999999, -0.4903925,	-1.0, -0.0, -0.0,	0.203178, 0.485388,
+	-0.5, 0.0, -0.5,	-1.0, -0.0, -0.0,	0.25, 0.49,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	-0.5, -0.19134150000000005, -0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.471731,
+	-0.5, -0.09754499999999999, -0.4903925,	-1.0, -0.0, -0.0,	0.203178, 0.485388,
+	-0.5, -0.3535535000000001, -0.3535535000000001,	-1.0, -0.0, -0.0,	0.080294, 0.419706,
+	-0.5, -0.27778499999999995, -0.41573499999999997,	-1.0, -0.0, -0.0,	0.116663, 0.449553,
+	-0.5, -0.19134150000000005, -0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.471731,
+	-0.5, -0.46194, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.028269, 0.341844,
+	-0.5, -0.41573499999999997, -0.27778499999999995,	-1.0, -0.0, -0.0,	0.050447, 0.383337,
+	-0.5, -0.3535535000000001, -0.3535535000000001,	-1.0, -0.0, -0.0,	0.080294, 0.419706,
+	-0.5, -0.5, 0.0,	-1.0, -0.0, -0.0,	0.01, 0.25,
+	-0.5, -0.4903925, -0.09754499999999999,	-1.0, -0.0, -0.0,	0.014612, 0.296822,
+	-0.5, -0.46194, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.028269, 0.341844,
+	-0.5, -0.46194, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.028269, 0.341844,
+	-0.5, -0.4903925, 0.09754499999999999,	-1.0, -0.0, -0.0,	0.014612, 0.203178,
+	-0.5, -0.5, 0.0,	-1.0, -0.0, -0.0,	0.01, 0.25,
+	-0.5, -0.46194, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.028269, 0.341844,
+	-0.5, -0.46194, 0.1913415,	-1.0, -0.0, -0.0,	0.028269, 0.158156,
+	-0.5, -0.4903925, 0.09754499999999999,	-1.0, -0.0, -0.0,	0.014612, 0.203178,
+	-0.5, -0.3535535000000001, 0.3535535,	-1.0, -0.0, -0.0,	0.080294, 0.080294,
+	-0.5, -0.41573499999999997, 0.277785,	-1.0, -0.0, -0.0,	0.050447, 0.116663,
+	-0.5, -0.46194, 0.1913415,	-1.0, -0.0, -0.0,	0.028269, 0.158156,
+	-0.5, -0.19134150000000005, 0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.028269,
+	-0.5, -0.27778499999999995, 0.415735,	-1.0, -0.0, -0.0,	0.116663, 0.050447,
+	-0.5, -0.3535535000000001, 0.3535535,	-1.0, -0.0, -0.0,	0.080294, 0.080294,
+	-0.5, 0.1913415, 0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.028269,
+	-0.5, -0.09754499999999999, 0.4903925,	-1.0, -0.0, -0.0,	0.203178, 0.014612,
+	-0.5, -0.19134150000000005, 0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.028269,
+	-0.5, 0.1913415, 0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.028269,
+	-0.5, 0.0, 0.5,	-1.0, -0.0, -0.0,	0.25, 0.01,
+	-0.5, -0.09754499999999999, 0.4903925,	-1.0, -0.0, -0.0,	0.203178, 0.014612,
+	-0.5, 0.1913415, 0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.028269,
+	-0.5, 0.09754499999999999, 0.4903925,	-1.0, -0.0, -0.0,	0.296822, 0.014612,
+	-0.5, 0.0, 0.5,	-1.0, -0.0, -0.0,	0.25, 0.01,
+	-0.5, 0.3535535, 0.3535535,	-1.0, -0.0, -0.0,	0.419706, 0.080294,
+	-0.5, 0.277785, 0.415735,	-1.0, -0.0, -0.0,	0.383337, 0.050447,
+	-0.5, 0.1913415, 0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.028269,
+	-0.5, 0.4619395, 0.1913415,	-1.0, -0.0, -0.0,	0.471731, 0.158156,
+	-0.5, 0.415735, 0.277785,	-1.0, -0.0, -0.0,	0.449553, 0.116663,
+	-0.5, 0.3535535, 0.3535535,	-1.0, -0.0, -0.0,	0.419706, 0.080294,
+	-0.5, 0.4619395, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.471731, 0.341844,
+	-0.5, 0.4903925, 0.09754499999999999,	-1.0, -0.0, -0.0,	0.485388, 0.203178,
+	-0.5, 0.4619395, 0.1913415,	-1.0, -0.0, -0.0,	0.471731, 0.158156,
+	-0.5, 0.4619395, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.471731, 0.341844,
+	-0.5, 0.5, 0.0,	-1.0, -0.0, -0.0,	0.49, 0.25,
+	-0.5, 0.4903925, 0.09754499999999999,	-1.0, -0.0, -0.0,	0.485388, 0.203178,
+	-0.5, 0.4619395, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.471731, 0.341844,
+	-0.5, 0.4903925, -0.09754499999999999,	-1.0, -0.0, -0.0,	0.485388, 0.296822,
+	-0.5, 0.5, 0.0,	-1.0, -0.0, -0.0,	0.49, 0.25,
+	-0.5, 0.3535535, -0.3535535000000001,	-1.0, -0.0, -0.0,	0.419706, 0.419706,
+	-0.5, 0.415735, -0.27778499999999995,	-1.0, -0.0, -0.0,	0.449553, 0.383337,
+	-0.5, 0.4619395, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.471731, 0.341844,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	-0.5, 0.277785, -0.41573499999999997,	-1.0, -0.0, -0.0,	0.383337, 0.449553,
+	-0.5, 0.3535535, -0.3535535000000001,	-1.0, -0.0, -0.0,	0.419706, 0.419706,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	-0.5, -0.3535535000000001, -0.3535535000000001,	-1.0, -0.0, -0.0,	0.080294, 0.419706,
+	-0.5, -0.19134150000000005, -0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.471731,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	-0.5, -0.46194, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.028269, 0.341844,
+	-0.5, -0.3535535000000001, -0.3535535000000001,	-1.0, -0.0, -0.0,	0.080294, 0.419706,
+	-0.5, -0.46194, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.028269, 0.341844,
+	-0.5, -0.3535535000000001, 0.3535535,	-1.0, -0.0, -0.0,	0.080294, 0.080294,
+	-0.5, -0.46194, 0.1913415,	-1.0, -0.0, -0.0,	0.028269, 0.158156,
+	-0.5, -0.46194, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.028269, 0.341844,
+	-0.5, -0.19134150000000005, 0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.028269,
+	-0.5, -0.3535535000000001, 0.3535535,	-1.0, -0.0, -0.0,	0.080294, 0.080294,
+	-0.5, -0.19134150000000005, 0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.028269,
+	-0.5, 0.3535535, 0.3535535,	-1.0, -0.0, -0.0,	0.419706, 0.080294,
+	-0.5, 0.1913415, 0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.028269,
+	-0.5, -0.19134150000000005, 0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.028269,
+	-0.5, 0.4619395, 0.1913415,	-1.0, -0.0, -0.0,	0.471731, 0.158156,
+	-0.5, 0.3535535, 0.3535535,	-1.0, -0.0, -0.0,	0.419706, 0.080294,
+	-0.5, 0.4619395, 0.1913415,	-1.0, -0.0, -0.0,	0.471731, 0.158156,
+	-0.5, 0.3535535, -0.3535535000000001,	-1.0, -0.0, -0.0,	0.419706, 0.419706,
+	-0.5, 0.4619395, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.471731, 0.341844,
+	-0.5, 0.4619395, 0.1913415,	-1.0, -0.0, -0.0,	0.471731, 0.158156,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	-0.5, 0.3535535, -0.3535535000000001,	-1.0, -0.0, -0.0,	0.419706, 0.419706,
+	-0.5, -0.19134150000000005, 0.46194,	-1.0, -0.0, -0.0,	0.158156, 0.028269,
+	-0.5, -0.46194, -0.19134150000000005,	-1.0, -0.0, -0.0,	0.028269, 0.341844,
+	-0.5, 0.1913415, -0.46194,	-1.0, -0.0, -0.0,	0.341844, 0.471731,
+	0.5, -0.09754499999999999, -0.4903925,	-0.0, -0.1951, -0.9808,	0.03125, 0.5,
+	-0.5, -0.09754499999999999, -0.4903925,	-0.0, -0.1951, -0.9808,	0.03125, 1.0,
+	-0.5, -0.19134150000000005, -0.46194,	-0.0, -0.3827, -0.9239,	0.0625, 1.0,
+	0.5, 0.0, -0.5,	-0.0, -0.0, -1.0,	0.0, 0.5,
+	-0.5, 0.0, -0.5,	-0.0, -0.0, -1.0,	0.0, 1.0,
+	-0.5, -0.09754499999999999, -0.4903925,	-0.0, -0.1951, -0.9808,	0.03125, 1.0,
+	0.5, 0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.485388,
+	0.5, 0.0, -0.5,	1.0, -0.0, -0.0,	0.75, 0.49,
+	0.5, -0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.485388,
+	0.5, 0.277785, -0.41573499999999997,	1.0, -0.0, -0.0,	0.883337, 0.449553,
+	0.5, 0.1913415, -0.46194,	1.0, -0.0, -0.0,	0.841844, 0.471731,
+	0.5, 0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.485388,
+	0.5, 0.415735, -0.27778499999999995,	1.0, -0.0, -0.0,	0.949553, 0.383337,
+	0.5, 0.3535535, -0.3535535000000001,	1.0, -0.0, -0.0,	0.919706, 0.419706,
+	0.5, 0.277785, -0.41573499999999997,	1.0, -0.0, -0.0,	0.883337, 0.449553,
+	0.5, 0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.296822,
+	0.5, 0.46194, -0.19134150000000005,	1.0, -0.0, -0.0,	0.971731, 0.341844,
+	0.5, 0.415735, -0.27778499999999995,	1.0, -0.0, -0.0,	0.949553, 0.383337,
+	0.5, 0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.203178,
+	0.5, 0.5, 0.0,	1.0, -0.0, -0.0,	0.99, 0.25,
+	0.5, 0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.296822,
+	0.5, 0.415735, 0.277785,	1.0, -0.0, -0.0,	0.949553, 0.116663,
+	0.5, 0.46194, 0.1913415,	1.0, -0.0, -0.0,	0.971731, 0.158156,
+	0.5, 0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.203178,
+	0.5, 0.277785, 0.415735,	1.0, -0.0, -0.0,	0.883337, 0.050447,
+	0.5, 0.3535535, 0.3535535,	1.0, -0.0, -0.0,	0.919706, 0.080294,
+	0.5, 0.415735, 0.277785,	1.0, -0.0, -0.0,	0.949553, 0.116663,
+	0.5, 0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.014612,
+	0.5, 0.1913415, 0.46194,	1.0, -0.0, -0.0,	0.841844, 0.028269,
+	0.5, 0.277785, 0.415735,	1.0, -0.0, -0.0,	0.883337, 0.050447,
+	0.5, -0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.014612,
+	0.5, 0.0, 0.5,	1.0, -0.0, -0.0,	0.75, 0.01,
+	0.5, 0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.014612,
+	0.5, -0.27778499999999995, 0.415735,	1.0, -0.0, -0.0,	0.616663, 0.050447,
+	0.5, -0.19134150000000005, 0.46194,	1.0, -0.0, -0.0,	0.658156, 0.028269,
+	0.5, -0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.014612,
+	0.5, -0.41573499999999997, 0.277785,	1.0, -0.0, -0.0,	0.550447, 0.116663,
+	0.5, -0.3535535000000001, 0.3535535,	1.0, -0.0, -0.0,	0.580294, 0.080294,
+	0.5, -0.27778499999999995, 0.415735,	1.0, -0.0, -0.0,	0.616663, 0.050447,
+	0.5, -0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.203178,
+	0.5, -0.46193949999999995, 0.1913415,	1.0, -0.0, -0.0,	0.528269, 0.158156,
+	0.5, -0.41573499999999997, 0.277785,	1.0, -0.0, -0.0,	0.550447, 0.116663,
+	0.5, -0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.296822,
+	0.5, -0.5, 0.0,	1.0, -0.0, -0.0,	0.51, 0.25,
+	0.5, -0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.203178,
+	0.5, -0.41573499999999997, -0.27778499999999995,	1.0, -0.0, -0.0,	0.550447, 0.383337,
+	0.5, -0.46193949999999995, -0.19134150000000005,	1.0, -0.0, -0.0,	0.528269, 0.341844,
+	0.5, -0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.296822,
+	0.5, -0.27778499999999995, -0.41573499999999997,	1.0, -0.0, -0.0,	0.616663, 0.449553,
+	0.5, -0.3535535000000001, -0.3535535000000001,	1.0, -0.0, -0.0,	0.580294, 0.419706,
+	0.5, -0.41573499999999997, -0.27778499999999995,	1.0, -0.0, -0.0,	0.550447, 0.383337,
+	0.5, -0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.485388,
+	0.5, -0.19134150000000005, -0.46194,	1.0, -0.0, -0.0,	0.658156, 0.471731,
+	0.5, -0.27778499999999995, -0.41573499999999997,	1.0, -0.0, -0.0,	0.616663, 0.449553,
+	0.5, 0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.296822,
+	0.5, 0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.485388,
+	0.5, -0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.485388,
+	0.5, 0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.296822,
+	0.5, 0.277785, -0.41573499999999997,	1.0, -0.0, -0.0,	0.883337, 0.449553,
+	0.5, 0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.485388,
+	0.5, 0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.296822,
+	0.5, 0.415735, -0.27778499999999995,	1.0, -0.0, -0.0,	0.949553, 0.383337,
+	0.5, 0.277785, -0.41573499999999997,	1.0, -0.0, -0.0,	0.883337, 0.449553,
+	0.5, 0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.014612,
+	0.5, 0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.203178,
+	0.5, 0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.296822,
+	0.5, 0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.014612,
+	0.5, 0.415735, 0.277785,	1.0, -0.0, -0.0,	0.949553, 0.116663,
+	0.5, 0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.203178,
+	0.5, 0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.014612,
+	0.5, 0.277785, 0.415735,	1.0, -0.0, -0.0,	0.883337, 0.050447,
+	0.5, 0.415735, 0.277785,	1.0, -0.0, -0.0,	0.949553, 0.116663,
+	0.5, -0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.203178,
+	0.5, -0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.014612,
+	0.5, 0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.014612,
+	0.5, -0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.203178,
+	0.5, -0.27778499999999995, 0.415735,	1.0, -0.0, -0.0,	0.616663, 0.050447,
+	0.5, -0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.014612,
+	0.5, -0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.203178,
+	0.5, -0.41573499999999997, 0.277785,	1.0, -0.0, -0.0,	0.550447, 0.116663,
+	0.5, -0.27778499999999995, 0.415735,	1.0, -0.0, -0.0,	0.616663, 0.050447,
+	0.5, -0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.485388,
+	0.5, -0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.296822,
+	0.5, -0.4903925, 0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.203178,
+	0.5, -0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.485388,
+	0.5, -0.41573499999999997, -0.27778499999999995,	1.0, -0.0, -0.0,	0.550447, 0.383337,
+	0.5, -0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.514612, 0.296822,
+	0.5, -0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.485388,
+	0.5, -0.27778499999999995, -0.41573499999999997,	1.0, -0.0, -0.0,	0.616663, 0.449553,
+	0.5, -0.41573499999999997, -0.27778499999999995,	1.0, -0.0, -0.0,	0.550447, 0.383337,
+	0.5, 0.09754499999999999, 0.4903925,	1.0, -0.0, -0.0,	0.796822, 0.014612,
+	0.5, 0.4903925, -0.09754499999999999,	1.0, -0.0, -0.0,	0.985388, 0.296822,
+	0.5, -0.09754499999999999, -0.4903925,	1.0, -0.0, -0.0,	0.703178, 0.485388,
+
+};
+
 Mesh* CUBE_MESH;
 Mesh* WEDGE_MESH;
 Mesh* SPHERE_MESH;
 Mesh* ARROW_MESH;
 Mesh* OUTLINE_MESH;
+Mesh* CYLINDER_CHEAP_MESH;
 Mesh* CYLINDER_MESH;
 
 void initMeshes() {
@@ -3119,6 +3499,7 @@ void initMeshes() {
     SPHERE_MESH = new Mesh(sizeof(SPHERE_VERTICES) / sizeof(float) / 8, SPHERE_VERTICES);
     ARROW_MESH = new Mesh(sizeof(ARROW_VERTICES) / sizeof(float) / 8, ARROW_VERTICES);
     OUTLINE_MESH = new Mesh(sizeof(OUTLINE_VERTICES) / sizeof(float) / 8, OUTLINE_VERTICES);
+    CYLINDER_CHEAP_MESH = new Mesh(sizeof(CYLINDER_CHEAP_VERTICES) / sizeof(float) / 8, CYLINDER_CHEAP_VERTICES);
     CYLINDER_MESH = new Mesh(sizeof(CYLINDER_VERTICES) / sizeof(float) / 8, CYLINDER_VERTICES);
 }
 

core/src/rendering/defaultmeshes.h

@@ -7,5 +7,6 @@ extern Mesh* SPHERE_MESH;
 extern Mesh* ARROW_MESH;
 extern Mesh* OUTLINE_MESH;
 extern Mesh* CYLINDER_MESH;
+extern Mesh* CYLINDER_CHEAP_MESH;
 
 void initMeshes();

core/src/rendering/renderer.cpp

@@ -5,6 +5,7 @@
 #include <glm/ext/matrix_clip_space.hpp>
 #include <glm/ext/matrix_float4x4.hpp>
 #include <glm/ext/matrix_transform.hpp>
+#include <glm/ext/scalar_constants.hpp>
 #include <glm/ext/vector_float3.hpp>
 #include <glm/glm.hpp>
 #include <glm/gtc/quaternion.hpp>
@@ -137,6 +138,7 @@ static void renderPart(std::shared_ptr<BasePart> part) {
     shader->set("normalMatrix", normalMatrix);
     shader->set("texScale", size);
     shader->set("transparency", part->transparency);
+    shader->set("reflectance", part->reflectance);
 
     shader->set("surfaces[" + std::to_string(NormalId::Right) + "]", (int)part->rightSurface);
     shader->set("surfaces[" + std::to_string(NormalId::Top) + "]", (int)part->topSurface);
@@ -156,6 +158,10 @@ static void renderPart(std::shared_ptr<BasePart> part) {
         glFrontFace(GL_CW);
         CUBE_MESH->bind();
         glDrawArrays(GL_TRIANGLES, 0, CUBE_MESH->vertexCount);
+    } else if (shape == PartType::Cylinder) {
+        glFrontFace(GL_CW);
+        CYLINDER_MESH->bind();
+        glDrawArrays(GL_TRIANGLES, 0, CYLINDER_MESH->vertexCount);
     }
 }
 
@@ -183,12 +189,15 @@ void renderParts() {
     shader->set("numPointLights", 0);
     studsTexture->activate(0);
     shader->set("studs", 0);
+    skyboxTexture->activate(1);
+    shader->set("skybox", 1);
 
     // Pre-calculate the normal matrix for the shader
 
     // Pass in the camera position
     shader->set("viewPos", camera.cameraPos);
     
+
     // Sort by nearest
     std::map<float, std::shared_ptr<BasePart>> sorted;
     for (auto it = gWorkspace()->GetDescendantsStart(); it != gWorkspace()->GetDescendantsEnd(); it++) {
@@ -255,8 +264,8 @@ void renderSurfaceExtras() {
             model = glm::scale(model, glm::vec3(0.4,0.4,0.4));
             ghostShader->set("model", model);
     
-            CYLINDER_MESH->bind();
-            glDrawArrays(GL_TRIANGLES, 0, CYLINDER_MESH->vertexCount);
+            CYLINDER_CHEAP_MESH->bind();
+            glDrawArrays(GL_TRIANGLES, 0, CYLINDER_CHEAP_MESH->vertexCount);
         }
     }
 }

core/src/rendering/skybox.cpp

@@ -22,6 +22,7 @@ Skybox::Skybox(std::array<std::string, 6> faces, unsigned int format) {
 
         glTexImage2D(GL_TEXTURE_CUBE_MAP_POSITIVE_X + i, 0, format, width, height, 0, format,
                     GL_UNSIGNED_BYTE, data);
+        glGenerateMipmap(GL_TEXTURE_CUBE_MAP);
         stbi_image_free(data);
     }
 
@@ -31,7 +32,7 @@ Skybox::Skybox(std::array<std::string, 6> faces, unsigned int format) {
     glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE);
 
     // Interpolation
-    glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+    glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
     glTexParameteri(GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 }
 
@@ -41,5 +42,5 @@ Skybox::~Skybox() {
 
 void Skybox::activate(unsigned int textureIdx) {
     glActiveTexture(GL_TEXTURE0 + textureIdx);
-    glBindTexture(GL_TEXTURE_2D, this->ID);
+    glBindTexture(GL_TEXTURE_CUBE_MAP, this->ID);
 }

editor/main.cpp

@@ -1,6 +1,7 @@
 #include "mainwindow.h"
 
 #include "logger.h"
+#include "physics/world.h"
 #include <QApplication>
 #include <QLocale>
 #include <QTranslator>
@@ -18,7 +19,7 @@ ma_engine miniaudio;
 int main(int argc, char *argv[])
 {
     Logger::init();
-
+    physicsInit();
     // Has to happen before Qt application initializes or we get an error in WASAPI initialization
     ma_result res = ma_engine_init(NULL, &miniaudio);
     if (res != MA_SUCCESS) {
@@ -41,6 +42,7 @@ int main(int argc, char *argv[])
     int result = a.exec();
 
     ma_engine_uninit(&miniaudio);
+    physicsDeinit();
     Logger::finish();
     return result;
 }

editor/mainglwidget.cpp

@@ -20,7 +20,6 @@
 #include "objects/pvinstance.h"
 #include "objects/service/selection.h"
 #include "partassembly.h"
-#include "physics/util.h"
 #include "rendering/renderer.h"
 #include "rendering/shader.h"
 #include "datatypes/variant.h"
@@ -148,10 +147,10 @@ void MainGLWidget::handleObjectDrag(QMouseEvent* evt) {
     
     if (!rayHit) return;
 
-    CFrame targetFrame = partFromBody(rayHit->body)->cframe;
+    CFrame targetFrame = rayHit->hitPart->cframe;
     Vector3 surfaceNormal = targetFrame.Inverse().Rotation() * rayHit->worldNormal;
     Vector3 inverseSurfaceNormal = Vector3::ONE - surfaceNormal.Abs();
-    glm::vec3 partSize = partFromBody(rayHit->body)->size;
+    glm::vec3 partSize = rayHit->hitPart->size;
     Vector3 tFormedHitPos = targetFrame * ((targetFrame.Inverse() * initialHitPos) * inverseSurfaceNormal);
     Vector3 tFormedInitialPos = targetFrame * ((targetFrame.Inverse() * initialFrame.Position()) * inverseSurfaceNormal);
     Vector3 vec = rayHit->worldPoint + (tFormedInitialPos - tFormedHitPos);
@@ -323,7 +322,7 @@ void MainGLWidget::handleRotationalTransform(QMouseEvent* evt) {
 
 std::optional<HandleFace> MainGLWidget::raycastHandle(glm::vec3 pointDir) {
     if (!editorToolHandles.active) return std::nullopt;
-    return ::raycastHandle(rp3d::Ray(glmToRp(camera.cameraPos), glmToRp(glm::normalize(pointDir)) * 50000));
+    return ::raycastHandle(camera.cameraPos, glm::normalize(pointDir) * 50000.f);
 }
 
 void MainGLWidget::handleCursorChange(QMouseEvent* evt) {
@@ -338,7 +337,7 @@ void MainGLWidget::handleCursorChange(QMouseEvent* evt) {
     };
 
     std::optional<const RaycastResult> rayHit = gWorkspace()->CastRayNearest(camera.cameraPos, pointDir, 50000);
-    if (rayHit && !partFromBody(rayHit->body)->locked) {
+    if (rayHit && !rayHit->hitPart->locked) {
         setCursor(Qt::OpenHandCursor);
         return;
     }
@@ -404,17 +403,17 @@ void MainGLWidget::mousePressEvent(QMouseEvent* evt) {
         // raycast part
         std::shared_ptr<Selection> selection = gDataModel->GetService<Selection>();
         std::optional<const RaycastResult> rayHit = gWorkspace()->CastRayNearest(camera.cameraPos, pointDir, 50000);
-        if (!rayHit || !partFromBody(rayHit->body)) { selection->Set({}); return; }
-        std::shared_ptr<BasePart> part = partFromBody(rayHit->body);
+        if (!rayHit || !rayHit->hitPart) { selection->Set({}); return; }
+        std::shared_ptr<BasePart> part = rayHit->hitPart;
         if (part->locked) { selection->Set({}); return; }
 
         std::shared_ptr<PVInstance> selObject = part;
 
         // Traverse to the root model
         if (~evt->modifiers() & Qt::AltModifier) {
-            std::optional<std::shared_ptr<Instance>> nextParent = selObject->GetParent();
-            while (nextParent.value() && nextParent.value()->IsA("Model")) {
-                selObject = std::dynamic_pointer_cast<PVInstance>(nextParent.value()); nextParent = selObject->GetParent();
+            nullable std::shared_ptr<Instance> nextParent = selObject->GetParent();
+            while (nextParent && nextParent->IsA("Model")) {
+                selObject = std::dynamic_pointer_cast<PVInstance>(nextParent); nextParent = selObject->GetParent();
             }
         }
 

editor/mainwindow.cpp

@@ -12,6 +12,8 @@
 #include "undohistory.h"
 #include "version.h"
 #include <memory>
+#include <QToolButton>
+#include <QSettings>
 #include <qclipboard.h>
 #include <qevent.h>
 #include <qglobal.h>
@@ -27,11 +29,9 @@
 #include <qtextcursor.h>
 #include <qtextedit.h>
 #include <miniaudio.h>
+#include <qtoolbutton.h>
 #include <vector>
-
-#ifdef _NDEBUG
-#define NDEBUG
-#endif
+#include <fstream>
 
 bool worldSpaceTransforms = false;
 
@@ -67,6 +67,8 @@ MainWindow::MainWindow(QWidget *parent)
     : QMainWindow(parent)
     , ui(new Ui::MainWindow)
 {
+    loadState();
+
     gDataModel->Init();
 
     ui->setupUi(this);
@@ -102,6 +104,28 @@ MainWindow::MainWindow(QWidget *parent)
 
     connectActionHandlers();
 
+    // Add open recents menu
+    refreshRecentsMenu();
+    for (QObject* child : ui->fileTools->children()) {
+        if (auto toolButton = dynamic_cast<QToolButton*>(child)) {
+            if (toolButton->defaultAction() != ui->actionOpen) continue;
+
+            // https://stackoverflow.com/a/12283957/16255372
+            // https://stackoverflow.com/a/5365184/16255372
+
+            toolButton->setMenu(recentsMenu);
+            toolButton->setPopupMode(QToolButton::MenuButtonPopup);
+        }
+    }
+
+    // Add open recents dropdown to file menu
+    auto actions = ui->menuFile->actions();
+    for (int i = 0; i < actions.size(); i++) {
+        if (actions[i] != ui->actionOpen) continue;
+
+        ui->menuFile->insertMenu((i+1) < actions.size() ? actions[i+1] : nullptr, recentsMenu);
+    }
+
     // ui->explorerView->Init(ui);
     placeDocument = new PlaceDocument(this);
     placeDocument->setAttribute(Qt::WA_DeleteOnClose, true);
@@ -122,9 +146,22 @@ MainWindow::MainWindow(QWidget *parent)
     });
 
     setUpCommandBar();
+
+    #ifndef NDEBUG
+    // https://stackoverflow.com/a/17631703/16255372
+    // Add shortcut for reloading most recent file
+    QAction* reloadMostRecent = new QAction();
+    reloadMostRecent->setShortcut(Qt::Key_R | Qt::CTRL);
+    connect(reloadMostRecent, &QAction::triggered, [this, reloadMostRecent]() {
+        recentsMenu->actions()[0]->trigger();
+        removeAction(reloadMostRecent);
+    });
+    addAction(reloadMostRecent);
+    #endif
 }
 
 void MainWindow::closeEvent(QCloseEvent* evt) {
+    saveState();
     #ifdef NDEBUG
     // Ask if the user wants to save their changes
     // https://stackoverflow.com/a/33890731
@@ -147,6 +184,64 @@ void MainWindow::closeEvent(QCloseEvent* evt) {
     #endif
 }
 
+void MainWindow::refreshRecentsMenu() {
+    if (!recentsMenu) recentsMenu = new QMenu();
+
+    recentsMenu->setTitle("Recent files...");
+
+    recentsMenu->clear(); // Actions not shown in any other menu are automatically deleted
+    for (QString item : recentFiles) {
+        QAction* itemAction = new QAction();
+        itemAction->setText(item.split('/').last());
+        recentsMenu->addAction(itemAction);
+        
+        connect(itemAction, &QAction::triggered, [item, this]{
+            if (!QFile::exists(item)) {
+                QMessageBox::warning(this, "File not found", "The file '" + item + "' could not longer be found at that location.");
+                return;
+            }
+
+            std::shared_ptr<DataModel> newModel = DataModel::LoadFromFile(item.toStdString());
+            editModeDataModel = newModel;
+            gDataModel = newModel;
+            newModel->Init();
+            ui->explorerView->updateRoot(newModel);
+
+            // Reset running state
+            placeDocument->setRunState(RUN_STOPPED);
+            undoManager.Reset();
+            updateToolbars();
+            pushRecentFile(item);
+        });
+    }
+
+    if (recentsMenu->isEmpty()) {
+        QAction* emptyAction = new QAction("No recents");
+        emptyAction->setEnabled(false);
+        recentsMenu->addAction(emptyAction);
+    }
+}
+
+void MainWindow::loadState() {
+    QSettings settings("openblocks");
+    recentFiles = settings.value("recentFiles").toStringList();
+}
+
+void MainWindow::saveState() {
+    QSettings settings("openblocks");
+    settings.setValue("recentFiles", recentFiles);
+}
+
+void MainWindow::pushRecentFile(QString file) {
+    // https://www.walletfox.com/course/qtopenrecentfiles.php
+    recentFiles.removeAll(file);
+    recentFiles.prepend(file);
+    while (recentFiles.size() > 10) recentFiles.removeLast();
+
+    refreshRecentsMenu();
+    saveState();
+}
+
 void MainWindow::setUpCommandBar() {
     CommandEdit* commandEdit;
     QToolBar* commandBar = ui->commandBar;
@@ -280,6 +375,7 @@ void MainWindow::connectActionHandlers() {
         }
 
         editModeDataModel->SaveToFile(path);
+        if (path) pushRecentFile(QString::fromStdString(path.value()));
     });
 
     connect(ui->actionSaveAs, &QAction::triggered, this, [&]() {
@@ -287,6 +383,7 @@ void MainWindow::connectActionHandlers() {
         if (!path || path == "") return;
 
         editModeDataModel->SaveToFile(path);
+        if (path) pushRecentFile(QString::fromStdString(path.value()));
     });
 
     connect(ui->actionOpen, &QAction::triggered, this, [&]() {
@@ -307,6 +404,7 @@ void MainWindow::connectActionHandlers() {
         placeDocument->setRunState(RUN_STOPPED);
         undoManager.Reset();
         updateToolbars();
+        if (path) pushRecentFile(QString::fromStdString(path.value()));
     });
 
     connect(ui->actionDelete, &QAction::triggered, this, [&]() {
@@ -314,8 +412,8 @@ void MainWindow::connectActionHandlers() {
         std::shared_ptr<Selection> selection = gDataModel->GetService<Selection>();
         for (std::weak_ptr<Instance> inst : selection->Get()) {
             if (inst.expired()) continue;
-            historyState.push_back(UndoStateInstanceRemoved { inst.lock(), inst.lock()->GetParent().value() });
-            inst.lock()->SetParent(std::nullopt);
+            historyState.push_back(UndoStateInstanceRemoved { inst.lock(), inst.lock()->GetParent() });
+            inst.lock()->SetParent(nullptr);
         }
         selection->Set({});
         historyState.push_back(UndoStateSelectionChanged {selection->Get(), {}});
@@ -344,9 +442,9 @@ void MainWindow::connectActionHandlers() {
         std::shared_ptr<Selection> selection = gDataModel->GetService<Selection>();
         for (std::weak_ptr<Instance> inst : selection->Get()) {
             if (inst.expired()) continue;
-            historyState.push_back(UndoStateInstanceRemoved { inst.lock(), inst.lock()->GetParent().value() });
+            historyState.push_back(UndoStateInstanceRemoved { inst.lock(), inst.lock()->GetParent() });
             inst.lock()->Serialize(rootDoc);
-            inst.lock()->SetParent(std::nullopt);
+            inst.lock()->SetParent(nullptr);
         }
         selection->Set({});
         historyState.push_back(UndoStateSelectionChanged {selection->Get(), {}});
@@ -413,8 +511,8 @@ void MainWindow::connectActionHandlers() {
         bool done = false;
         std::shared_ptr<Selection> selection = gDataModel->GetService<Selection>();
         for (auto object : selection->Get()) {
-            if (firstParent == nullptr && object->GetParent().has_value()) firstParent = object->GetParent().value();
-            historyState.push_back(UndoStateInstanceReparented { object, object->GetParent().value(), model });
+            if (!firstParent && object->GetParent() != nullptr) firstParent = object->GetParent();
+            historyState.push_back(UndoStateInstanceReparented { object, object->GetParent(), model });
             object->SetParent(model);
             done = true;
         }
@@ -446,13 +544,13 @@ void MainWindow::connectActionHandlers() {
             done = true;
 
             for (auto object : model->GetChildren()) {
-                historyState.push_back(UndoStateInstanceReparented { object, object->GetParent().value(), model->GetParent().value() });
+                historyState.push_back(UndoStateInstanceReparented { object, object->GetParent(), model->GetParent() });
                 object->SetParent(model->GetParent());
                 newSelection.push_back(object);
             }
 
-            historyState.push_back(UndoStateInstanceRemoved { model, model->GetParent().value() });
-            model->SetParent(std::nullopt);
+            historyState.push_back(UndoStateInstanceRemoved { model, model->GetParent() });
+            model->SetParent(nullptr);
         }
 
         if (!done)

editor/mainwindow.h

@@ -65,9 +65,17 @@ public:
 
     Ui::MainWindow *ui;
     
+    QMenu* recentsMenu = nullptr;
+    QStringList recentFiles;
+    void refreshRecentsMenu();
+    void pushRecentFile(QString);
+
+    void loadState();
+    void saveState();
+    
     friend PlaceDocument;
 private:
-    PlaceDocument* placeDocument;
+    PlaceDocument* placeDocument = nullptr;
 
     void setUpCommandBar();
     void connectActionHandlers();

editor/mainwindow.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1050</width>
+    <width>1100</width>
     <height>750</height>
    </rect>
   </property>

editor/panes/explorermodel.cpp

@@ -18,24 +18,24 @@ ExplorerModel::ExplorerModel(std::shared_ptr<Instance> dataRoot, QWidget *parent
     : QAbstractItemModel(parent)
     , rootItem(dataRoot) {
     // TODO: Don't use lambdas and handlers like that
-    hierarchyPreUpdateHandler = [&](std::shared_ptr<Instance> object, std::optional<std::shared_ptr<Instance>> oldParent, std::optional<std::shared_ptr<Instance>> newParent) {
-        if (oldParent.has_value()) {
-            auto children = oldParent.value()->GetChildren();
+    hierarchyPreUpdateHandler = [&](std::shared_ptr<Instance> object, nullable std::shared_ptr<Instance> oldParent, nullable std::shared_ptr<Instance> newParent) {
+        if (oldParent) {
+            auto children = oldParent->GetChildren();
             size_t idx = std::find(children.begin(), children.end(), object) - children.begin();
-            beginRemoveRows(toIndex(oldParent.value()), idx, idx);
+            beginRemoveRows(toIndex(oldParent), idx, idx);
         }
 
-        if (newParent.has_value()) {
-            size_t size = newParent.value()->GetChildren().size();
-            beginInsertRows(toIndex(newParent.value()), size, size);
+        if (newParent) {
+            size_t size = newParent->GetChildren().size();
+            beginInsertRows(toIndex(newParent), size, size);
         } else {
             // TODO:
         }
     };
 
-    hierarchyPostUpdateHandler = [&](std::shared_ptr<Instance> object, std::optional<std::shared_ptr<Instance>> oldParent, std::optional<std::shared_ptr<Instance>> newParent) {
-        if (newParent.has_value()) endInsertRows();
-        if (oldParent.has_value()) endRemoveRows();
+    hierarchyPostUpdateHandler = [&](std::shared_ptr<Instance> object, nullable std::shared_ptr<Instance> oldParent, nullable std::shared_ptr<Instance> newParent) {
+        if (newParent) endInsertRows();
+        if (oldParent) endRemoveRows();
     };
 }
 
@@ -49,16 +49,22 @@ QModelIndex ExplorerModel::index(int row, int column, const QModelIndex &parent)
         ? static_cast<Instance*>(parent.internalPointer())
         : rootItem.get();
 
+#ifdef NDEBUG
     if (parentItem->GetChildren().size() >= (size_t)row && !(parentItem->GetChildren()[row]->GetClass()->flags & INSTANCE_HIDDEN))
         return createIndex(row, column, parentItem->GetChildren()[row].get());
+#else
+    // Don't hide in debug builds
+    if (parentItem->GetChildren().size() >= (size_t)row)
+        return createIndex(row, column, parentItem->GetChildren()[row].get());
+#endif
     return {};
 }
 
 QModelIndex ExplorerModel::toIndex(std::shared_ptr<Instance> item) {
-    if (item == rootItem || !item->GetParent().has_value())
+    if (item == rootItem || !item->GetParent())
         return {};
 
-    std::shared_ptr<Instance> parentItem = item->GetParent().value();
+    std::shared_ptr<Instance> parentItem = item->GetParent();
     // Check above ensures this item is not root, so value() must be valid
     for (size_t i = 0; i < parentItem->GetChildren().size(); i++)
         if (parentItem->GetChildren()[i] == item)
@@ -76,13 +82,13 @@ QModelIndex ExplorerModel::parent(const QModelIndex &index) const {
 
     Instance* childItem = static_cast<Instance*>(index.internalPointer());
     // NORISK: The parent must exist if the child was obtained from it during this frame
-    std::shared_ptr<Instance> parentItem = childItem->GetParent().value();
+    std::shared_ptr<Instance> parentItem = childItem->GetParent();
 
     if (parentItem == rootItem)
         return {};
 
     // Check above ensures this item is not root, so value() must be valid
-    std::shared_ptr<Instance> parentParent = parentItem->GetParent().value();
+    std::shared_ptr<Instance> parentParent = parentItem->GetParent();
     for (size_t i = 0; i < parentParent->GetChildren().size(); i++)
         if (parentParent->GetChildren()[i] == parentItem)
             return createIndex(i, 0, parentItem.get());
@@ -97,7 +103,15 @@ int ExplorerModel::rowCount(const QModelIndex &parent) const {
         ? static_cast<Instance*>(parent.internalPointer())
         : rootItem.get();
 
+#ifdef NDEBUG
+    // Trim trailing hidden items as they make the branches look weird
+    int count = parentItem->GetChildren().size();
+    while (count > 0 && parentItem->GetChildren()[count-1]->GetClass()->flags & INSTANCE_HIDDEN) count--;
+    return count;
+#else
+    // Don't hide in debug builds
     return parentItem->GetChildren().size();
+#endif
 }
 
 int ExplorerModel::columnCount(const QModelIndex &parent) const {
@@ -223,7 +237,7 @@ bool ExplorerModel::dropMimeData(const QMimeData *data, Qt::DropAction action, i
     UndoState historyState;
     std::shared_ptr<Instance> parentInst = fromIndex(parent);
     for (std::shared_ptr<Instance> instance : slot->instances) {
-        historyState.push_back(UndoStateInstanceReparented { instance, instance->GetParent().value_or(nullptr), parentInst });
+        historyState.push_back(UndoStateInstanceReparented { instance, instance->GetParent(), parentInst });
         instance->SetParent(parentInst);
     }
 

editor/panes/propertiesview.cpp

@@ -333,11 +333,11 @@ void PropertiesView::drawBranches(QPainter *painter, const QRect &rect, const QM
     QTreeWidget::drawBranches(painter, rect, index);
 }
 
-void PropertiesView::setSelected(std::optional<std::shared_ptr<Instance>> instance) {
+void PropertiesView::setSelected(nullable std::shared_ptr<Instance> instance) {
     clear();
     currentInstance = {};
     if (!instance) return;
-    std::shared_ptr<Instance> inst = instance.value();
+    std::shared_ptr<Instance> inst = instance;
     currentInstance = inst;
 
     std::map<PropertyCategory, QTreeWidgetItem*> propertyCategories;

editor/panes/propertiesview.h

@@ -31,5 +31,5 @@ public:
 
     void init();
 
-    void setSelected(std::optional<std::shared_ptr<Instance>> instance);
+    void setSelected(nullable std::shared_ptr<Instance> instance);
 };

editor/placedocument.cpp

@@ -23,36 +23,6 @@
 #include "objects/service/selection.h"
 #include "timeutil.h"
 
-class PlaceDocumentPhysicsWorker {
-public:
-    std::mutex sync;
-    std::thread thread;
-    std::condition_variable runningCond;
-    bool running = false;
-    bool quit = false;
-
-    PlaceDocumentPhysicsWorker() : thread(&PlaceDocumentPhysicsWorker::doWork, this) {}
-private:
-    tu_time_t lastTime = tu_clock_micros();
-    void doWork() {
-        do {
-            tu_time_t deltaTime = tu_clock_micros() - lastTime;
-            lastTime = tu_clock_micros();
-
-            // First frame is always empty
-            if (deltaTime > 100) {
-                gWorkspace()->PhysicsStep(float(deltaTime)/1'000'000);
-            }
-
-            std::this_thread::sleep_for(std::chrono::microseconds(16'667 - deltaTime));
-
-            std::unique_lock lock(sync);
-            runningCond.wait(lock, [&]{ return running || quit; });
-            lock.unlock();
-        } while (!quit);
-    }
-};
-
 PlaceDocument::PlaceDocument(QWidget* parent):
     QMdiSubWindow(parent) {
     placeWidget = new MainGLWidget;
@@ -62,29 +32,15 @@ PlaceDocument::PlaceDocument(QWidget* parent):
 
     _runState = RUN_STOPPED;
     updateSelectionListeners(gDataModel->GetService<Selection>());
-
-    worker = new PlaceDocumentPhysicsWorker();
 }
 
 PlaceDocument::~PlaceDocument() {
-    worker->quit = true;
-    worker->runningCond.notify_all();
-    worker->thread.join();
-}
-
-void PlaceDocument::updatePhysicsWorker() {
-    {
-        std::lock_guard lock(worker->sync);
-        worker->running = _runState == RUN_RUNNING;
-    }
-    worker->runningCond.notify_all();
 }
 
 void PlaceDocument::setRunState(RunState newState) {
     if (newState == RUN_RUNNING && _runState != RUN_RUNNING) {
         if (_runState == RUN_PAUSED) {
             _runState = RUN_RUNNING;
-            updatePhysicsWorker();
             return;
         }
 
@@ -99,12 +55,13 @@ void PlaceDocument::setRunState(RunState newState) {
     } else if (newState == RUN_STOPPED) {
         _runState = RUN_STOPPED;
 
+#ifndef NDEBUG
+    printf("DataModel stopped. Remaning use counts (should be 1): %ld\n", gDataModel.use_count());
+#endif
         // TODO: GC: Check to make sure gDataModel gets properly garbage collected prior to this
         gDataModel = editModeDataModel;
         updateSelectionListeners(gDataModel->GetService<Selection>());
     }
-
-    updatePhysicsWorker();
 }
 
 void PlaceDocument::updateSelectionListeners(std::shared_ptr<Selection> selection) {
@@ -116,7 +73,7 @@ void PlaceDocument::updateSelectionListeners(std::shared_ptr<Selection> selectio
     selectionConnection = selection->SelectionChanged->Connect([selection, mainWnd](std::vector<Variant> _){
         // Update properties
         if (selection->Get().size() != 1)
-            mainWnd->ui->propertiesView->setSelected(std::nullopt);
+            mainWnd->ui->propertiesView->setSelected(nullptr);
         else
             mainWnd->ui->propertiesView->setSelected(selection->Get()[0]);
 
@@ -130,7 +87,6 @@ void PlaceDocument::closeEvent(QCloseEvent *closeEvent) {
     closeEvent->ignore();
 }
 
-std::shared_ptr<BasePart> shit;
 void PlaceDocument::timerEvent(QTimerEvent* evt) {
     if (evt->timerId() != timer.timerId()) {
         QWidget::timerEvent(evt);
@@ -140,7 +96,7 @@ void PlaceDocument::timerEvent(QTimerEvent* evt) {
     placeWidget->repaint();
     placeWidget->updateCycle();
     gDataModel->GetService<ScriptContext>()->RunSleepingThreads();
-    gDataModel->GetService<Workspace>()->ProcessContactEvents();
+    if (_runState == RUN_RUNNING) gDataModel->GetService<Workspace>()->PhysicsStep(0.033);
 }
 
 

editor/placedocument.h

@@ -13,7 +13,6 @@
 #include <type_traits>
 
 class Selection;
-class PlaceDocumentPhysicsWorker;
 
 enum RunState {
     RUN_STOPPED,
@@ -25,14 +24,10 @@ class PlaceDocument : public QMdiSubWindow {
     QBasicTimer timer;
     RunState _runState;
 
-    PlaceDocumentPhysicsWorker* worker;
-
     std::weak_ptr<SignalConnection> selectionConnection;
 
     void timerEvent(QTimerEvent*) override;
     void updateSelectionListeners(std::shared_ptr<Selection>);
-
-    void updatePhysicsWorker();
 public:
     MainGLWidget* placeWidget;
     PlaceDocument(QWidget* parent = nullptr);

editor/undohistory.cpp

@@ -27,7 +27,7 @@ void UndoHistory::Undo() {
             // The old value used to be valid, so it still should be...
             v->affectedInstance->SetProperty(v->property, v->oldValue).expect();
         } else if (auto v = std::get_if<UndoStateInstanceCreated>(&change)) {
-            v->instance->SetParent(std::nullopt);
+            v->instance->SetParent(nullptr);
         } else if (auto v = std::get_if<UndoStateInstanceRemoved>(&change)) {
             v->instance->SetParent(v->oldParent);
         } else if (auto v = std::get_if<UndoStateInstanceReparented>(&change)) {
@@ -57,7 +57,7 @@ void UndoHistory::Redo() {
         } else if (auto v = std::get_if<UndoStateInstanceCreated>(&change)) {
             v->instance->SetParent(v->newParent);
         } else if (auto v = std::get_if<UndoStateInstanceRemoved>(&change)) {
-            v->instance->SetParent(std::nullopt);
+            v->instance->SetParent(nullptr);
         } else if (auto v = std::get_if<UndoStateInstanceReparented>(&change)) {
             v->instance->SetParent(v->newParent);
         } else if (auto v = std::get_if<UndoStateSelectionChanged>(&change)) {