fix(physics): motor6d relies on constraints

core/CMakeLists.txt

@@ -89,6 +89,8 @@ set(SOURCES
     src/objects/joint/weld.h
     src/objects/joint/snap.cpp
     src/objects/joint/rotatev.cpp
+    src/objects/joint/motor6d.h
+    src/objects/joint/motor6d.cpp
     src/objects/base/service.h
     src/objects/base/member.h
     src/objects/base/instance.h
@@ -142,6 +144,7 @@ set(AUTOGEN_SOURCES
     src/objects/script.h
     src/objects/joint/snap.h
     src/objects/joint/jointinstance.h
+    src/objects/joint/motor6d.h
     src/objects/joint/rotatev.h
     src/objects/joint/rotate.h
     src/objects/joint/weld.h

core/src/error/instance.h

@@ -9,7 +9,7 @@ class NoSuchInstance : public Error {
 
 class NoSuchService : public Error {
     public:
-    inline NoSuchService(std::string className) : Error("NoSuchService", "Cannot insert service of unknown type " + className) {}
+    inline NoSuchService(std::string className) : Error("NoSuchService", "Unknown service type " + className) {}
 };
 
 class ServiceAlreadyExists : public Error {

core/src/objects/datamodel.cpp

@@ -34,8 +34,13 @@ void DataModel::Init(bool runMode) {
     // Init all services
     for (auto [_, service] : this->services) {
         service->InitService();
-        if (runMode) service->OnRun();
     }
+
+    // Deterministic run order
+    if (!runMode) return;
+    
+    if (auto service = FindService<Workspace>()) service->OnRun();
+    if (auto service = FindService<ServerScriptService>()) service->OnRun();
 }
 
 void DataModel::SaveToFile(std::optional<std::string> path) {
@@ -114,12 +119,13 @@ result<std::shared_ptr<Service>, NoSuchService> DataModel::GetService(std::strin
 }
 
 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) {
+    if (services.count(className) != 0)
+        return std::dynamic_pointer_cast<Service>(services[className]);
+    
+    if (!INSTANCE_MAP[className] || ~(INSTANCE_MAP[className]->flags & (INSTANCE_NOTCREATABLE | INSTANCE_SERVICE)) == 0) {
         return NoSuchService(className);
     }
     
-    if (services.count(className) != 0)
-        return std::dynamic_pointer_cast<Service>(services[className]);
     return nullptr;
 }
 

core/src/objects/joint/jointinstance.cpp

@@ -20,12 +20,20 @@ void JointInstance::OnAncestryChanged(nullable std::shared_ptr<Instance>, nullab
 void JointInstance::OnPartParamsUpdated() {
 }
 
+void JointInstance::OnPhysicsStep(float deltaTime) {
+}
+
+bool JointInstance::isDrivenJoint() {
+    return false;
+}
+
 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()) {
+        if (isDrivenJoint()) jointWorkspace.lock()->UntrackDrivenJoint(shared<JointInstance>());
         jointWorkspace.lock()->DestroyJoint(joint);
         if (!oldPart0.expired())
             oldPart0.lock()->untrackJoint(shared<JointInstance>());
@@ -54,6 +62,7 @@ void JointInstance::Update() {
 
     part0.lock()->trackJoint(shared<JointInstance>());
     part1.lock()->trackJoint(shared<JointInstance>());
+    jointWorkspace.lock()->TrackDrivenJoint(shared<JointInstance>());
 }
 
 nullable std::shared_ptr<Workspace> JointInstance::workspaceOfPart(std::shared_ptr<BasePart> part) {

core/src/objects/joint/jointinstance.h

@@ -32,6 +32,7 @@ protected:
     inline void onUpdated(std::string property) { Update(); };
 
     virtual void buildJoint() = 0;
+    virtual bool isDrivenJoint();
 public:
     void Update();
     virtual void OnPartParamsUpdated();
@@ -41,6 +42,8 @@ public:
     DEF_PROP_PHYS CFrame c0;
     DEF_PROP_PHYS CFrame c1;
 
+    virtual void OnPhysicsStep(float deltaTime);
+
     JointInstance(const InstanceType*);
     ~JointInstance();
 };

core/src/objects/joint/motor6d.cpp

@@ -0,0 +1,49 @@
+#include "motor6d.h"
+#include "datatypes/vector.h"
+#include "objects/part/part.h"
+#include "objects/service/workspace.h"
+#include "rendering/renderer.h"
+
+#define PI 3.14159
+
+Motor6D::Motor6D(): JointInstance(&TYPE) {
+}
+
+Motor6D::~Motor6D() {
+}
+
+static CFrame XYZToZXY(glm::vec3(0, 0, 0), -glm::vec3(1, 0, 0), glm::vec3(0, 0, 1));
+
+void Motor6D::buildJoint() {
+    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;
+    PhysStepperJointInfo jointInfo(c0, c1, desiredAngle, maxVelocity);
+    
+    this->joint = workspace->CreateJoint(jointInfo, part0.lock(), part1.lock());
+    jointWorkspace = workspace;
+}
+
+bool Motor6D::isDrivenJoint() {
+    return true;
+}
+
+void Motor6D::OnPhysicsStep(float deltaTime) {
+    // Tween currentAngle
+    float diffAngle = abs(currentAngle - desiredAngle);
+    if (diffAngle > abs(maxVelocity)) { // Don't tween if we're already close enough to being there
+        if (currentAngle < desiredAngle)
+            currentAngle += maxVelocity;
+        else
+            currentAngle -= maxVelocity;
+    }
+
+    // Shouldn't in theory be necessary, but just in case.
+    if (part0.expired() || part1.expired()) return;
+
+    // TODO: Re-add rotating only one part when both are unanchored, maybe?
+    joint.setTargetAngle(currentAngle);
+}

core/src/objects/joint/motor6d.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "objects/annotation.h"
+#include "objects/base/instance.h"
+#include "objects/joint/jointinstance.h"
+#include <memory>
+
+class DEF_INST Motor6D : public JointInstance {
+    AUTOGEN_PREAMBLE
+
+    virtual void buildJoint() override;
+    void onUpdated(std::string);
+
+    void OnPhysicsStep(float deltaTime) override;
+    bool isDrivenJoint() override;
+public:
+    Motor6D();
+    ~Motor6D();
+
+    DEF_PROP float currentAngle = 0;
+    DEF_PROP float desiredAngle = 0;
+    DEF_PROP float maxVelocity = 0.1;
+
+    static inline std::shared_ptr<Motor6D> New() { return std::make_shared<Motor6D>(); };
+    static inline std::shared_ptr<Instance> Create() { return std::make_shared<Motor6D>(); };
+};

core/src/objects/joint/rotatev.cpp

@@ -22,7 +22,7 @@ void RotateV::buildJoint() {
     part1.lock()->cframe = newFrame;
     // Do NOT use Abs() in this scenario. For some reason that breaks it
     float vel = part0.lock()->GetSurfaceParamB(-c0.LookVector().Unit()) * 6.28;
-    PhysRotatingJointInfo jointInfo(c0, c1, vel);
+    PhysMotorizedJointInfo jointInfo(c0, c1, vel);
     
     this->joint = workspace->CreateJoint(jointInfo, part0.lock(), part1.lock());
     jointWorkspace = workspace;

core/src/objects/meta.cpp

@@ -1,22 +1,27 @@
 #include "meta.h"
-#include "objects/folder.h"
+
-#include "objects/hint.h"
+#define DECLTYPE(className) class className { public: const static InstanceType TYPE; };
-#include "objects/joint/jointinstance.h"
+
-#include "objects/joint/rotate.h"
+DECLTYPE(DataModel);
-#include "objects/joint/rotatev.h"
+DECLTYPE(BasePart);
-#include "objects/joint/weld.h"
+DECLTYPE(Part);
-#include "objects/message.h"
+DECLTYPE(WedgePart);
-#include "objects/part/wedgepart.h"
+DECLTYPE(Snap);
-#include "objects/service/jointsservice.h"
+DECLTYPE(Weld);
-#include "objects/model.h"
+DECLTYPE(Rotate);
-#include "objects/part/part.h"
+DECLTYPE(RotateV);
-#include "objects/joint/snap.h"
+DECLTYPE(Motor6D);
-#include "objects/script.h"
+DECLTYPE(JointInstance);
-#include "objects/service/script/scriptcontext.h"
+DECLTYPE(Script);
-#include "objects/service/script/serverscriptservice.h"
+DECLTYPE(Model);
-#include "objects/service/selection.h"
+DECLTYPE(Message);
-#include "objects/service/workspace.h"
+DECLTYPE(Hint);
-#include "objects/datamodel.h"
+// DECLTYPE(Folder);
+DECLTYPE(Workspace);
+DECLTYPE(JointsService);
+DECLTYPE(ScriptContext);
+DECLTYPE(ServerScriptService);
+DECLTYPE(Selection);
 
 std::map<std::string, const InstanceType*> INSTANCE_MAP = {
     { "Instance", &Instance::TYPE },
@@ -29,6 +34,7 @@ std::map<std::string, const InstanceType*> INSTANCE_MAP = {
     { "Weld", &Weld::TYPE },
     { "Rotate", &Rotate::TYPE },
     { "RotateV", &RotateV::TYPE },
+    { "Motor6D", &Motor6D::TYPE },
     { "JointInstance", &JointInstance::TYPE },
     { "Script", &Script::TYPE },
     { "Model", &Model::TYPE },

core/src/objects/part/basepart.cpp

@@ -305,6 +305,11 @@ void BasePart::MakeJoints() {
     }
 }
 
+void BasePart::UpdateNoBreakJoints() {    
+    if (workspace() != nullptr)
+        workspace()->SyncPartPhysics(std::dynamic_pointer_cast<BasePart>(this->shared_from_this()));
+}
+
 void BasePart::trackJoint(std::shared_ptr<JointInstance> joint) {
     if (!joint->part0.expired() && joint->part0.lock() == shared_from_this()) {
         for (auto it = primaryJoints.begin(); it != primaryJoints.end();) {

core/src/objects/part/basepart.h

@@ -119,6 +119,7 @@ public:
 
     void MakeJoints();
     void BreakJoints();
+    void UpdateNoBreakJoints();
 
     // Calculate size of axis-aligned bounding box
     Vector3 GetAABB();

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

@@ -21,13 +21,14 @@ class DEF_INST_SERVICE_(hidden) ScriptContext : public Service {
     std::vector<SleepingThread> sleepingThreads;
     int lastScriptSourceId = 0;
 protected:
-    void InitService() override;
     bool initialized = false;
 
 public:
     ScriptContext();
     ~ScriptContext();
 
+    void InitService() override;
+    
     lua_State* state;
     void PushThreadSleep(lua_State* thread, float delay);
     void RunSleepingThreads();

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

@@ -8,13 +8,14 @@
 class DEF_INST_SERVICE_(explorer_icon="server-scripts", hidden) ServerScriptService : public Service {
     AUTOGEN_PREAMBLE
 protected:
-    void InitService() override;
-    void OnRun() override;
     bool initialized = false;
 
 public:
     ServerScriptService();
     ~ServerScriptService();
 
+    void InitService() override;
+    void OnRun() override;
+    
     static inline std::shared_ptr<Instance> Create() { return std::make_shared<ServerScriptService>(); };
 };

core/src/objects/service/workspace.h

@@ -2,10 +2,9 @@
 
 #include "objects/annotation.h"
 #include "objects/base/service.h"
+#include "objects/joint/jointinstance.h"
 #include "physics/world.h"
-#include "utils.h"
 #include <glm/ext/vector_float3.hpp>
-#include <list>
 #include <memory>
 #include <mutex>
 #include <queue>
@@ -34,14 +33,15 @@ class DEF_INST_SERVICE_(explorer_icon="workspace") Workspace : public Service {
     std::shared_ptr<PhysWorld> physicsWorld;
     friend PhysWorld;
 protected:
-    void InitService() override;
-    void OnRun() override;
     bool initialized = false;
 
 public:
     Workspace();
     ~Workspace();
     
+    void InitService() override;
+    void OnRun() override;
+
     std::recursive_mutex queueLock;
 
     DEF_PROP float fallenPartsDestroyHeight = -500;
@@ -56,6 +56,9 @@ public:
     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); }
 
+    inline void TrackDrivenJoint(std::shared_ptr<JointInstance> motor) { return physicsWorld->trackDrivenJoint(motor); }
+    inline void UntrackDrivenJoint(std::shared_ptr<JointInstance> motor) { return physicsWorld->untrackDrivenJoint(motor); }
+
     void PhysicsStep(float deltaTime);
     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/physics/world.cpp

@@ -2,6 +2,7 @@
 #include "datatypes/vector.h"
 #include "enum/part.h"
 #include "logger.h"
+#include "objects/joint/jointinstance.h"
 #include "objects/part/basepart.h"
 #include "objects/part/part.h"
 #include "objects/part/wedgepart.h"
@@ -37,6 +38,8 @@
 #include <Jolt/Physics/Collision/NarrowPhaseQuery.h>
 #include <Jolt/Physics/Constraints/FixedConstraint.h>
 #include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <algorithm>
+#include <cstdio>
 #include <memory>
 
 static JPH::TempAllocator* allocator;
@@ -194,6 +197,11 @@ void PhysWorld::step(float deltaTime) {
         part->cframe = CFrame(convert<Vector3>(interface.GetPosition(bodyID)), convert<glm::quat>(interface.GetRotation(bodyID)));
     }
 
+    // Update joints
+    for (std::shared_ptr<JointInstance> joint : drivenJoints) {
+        joint->OnPhysicsStep(deltaTime);
+    }
+
     physTime = tu_clock_micros() - startTime;
 }
 
@@ -226,28 +234,56 @@ PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr<BasePart>
         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);
+        
+        // settings for Motor6D
+        settings.mMotorSettings.mSpringSettings.mFrequency = 20;
+        settings.mMotorSettings.mSpringSettings.mDamping = 1;
         constraint = settings.Create(*part0->rigidBody.bodyImpl, *part1->rigidBody.bodyImpl);
-        if (info->motorized) {
+        
+        if (PhysMotorizedJointInfo* info = dynamic_cast<PhysMotorizedJointInfo*>(&type)) {
             static_cast<JPH::HingeConstraint*>(constraint)->SetMotorState(JPH::EMotorState::Velocity);
             static_cast<JPH::HingeConstraint*>(constraint)->SetTargetAngularVelocity(-info->initialVelocity);
+        } else if (PhysStepperJointInfo* _ = dynamic_cast<PhysStepperJointInfo*>(&type)) {
+            static_cast<JPH::HingeConstraint*>(constraint)->SetMotorState(JPH::EMotorState::Position);
         }
     } else {
         panic();
     }
 
     worldImpl.AddConstraint(constraint);
-    return { constraint };
+    return { constraint, this };
+}
+
+void PhysWorld::trackDrivenJoint(std::shared_ptr<JointInstance> motor) {
+    drivenJoints.push_back(motor);
+}
+
+void PhysWorld::untrackDrivenJoint(std::shared_ptr<JointInstance> motor) {
+    for (auto it = drivenJoints.begin(); it != drivenJoints.end();) {
+        if (*it == motor)
+            it = drivenJoints.erase(it);
+        else
+            it++;
+    }
 }
 
 // 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 PhysJoint::setTargetAngle(float angle) {
+    JPH::HingeConstraint* constraint = static_cast<JPH::HingeConstraint*>(jointImpl);
+    constraint->SetTargetAngle(angle);
+
+    // Wake up the part as it could be sleeping
+    JPH::BodyInterface& interface = parentWorld->worldImpl.GetBodyInterface();
+    JPH::BodyID bodies[] = {constraint->GetBody1()->GetID(), constraint->GetBody2()->GetID()};
+    interface.ActivateBodies(bodies, 2);
+}
+
 void PhysWorld::destroyJoint(PhysJoint joint) {
     worldImpl.RemoveConstraint(joint.jointImpl);
 }

core/src/physics/world.h

@@ -14,6 +14,7 @@
 #include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
 
 class BasePart;
+class JointInstance;
 class PhysWorld;
 
 struct PhysJointInfo { virtual ~PhysJointInfo() = default; protected: PhysJointInfo() = default; };
@@ -28,19 +29,31 @@ struct PhysFixedJointInfo : PhysJointInfo {
 struct PhysRotatingJointInfo : PhysJointInfo {
     CFrame c0;
     CFrame c1;
-    bool motorized;
+    
+    inline PhysRotatingJointInfo(CFrame c0, CFrame c1) : c0(c0), c1(c1) {}
+};
+
+struct PhysMotorizedJointInfo : PhysRotatingJointInfo {
     float initialVelocity;
     
-    inline PhysRotatingJointInfo(CFrame c0, CFrame c1) : c0(c0), c1(c1), motorized(false), initialVelocity(0.f){}
+    inline PhysMotorizedJointInfo(CFrame c0, CFrame c1, float initialVelocity) : PhysRotatingJointInfo(c0, c1), initialVelocity(initialVelocity) {}
-    inline PhysRotatingJointInfo(CFrame c0, CFrame c1, float initialVelocity) : c0(c0), c1(c1), motorized(true), initialVelocity(initialVelocity) {}
+};
+
+struct PhysStepperJointInfo : PhysRotatingJointInfo {
+    float initialAngle;
+    float initialVelocity;
+    
+    inline PhysStepperJointInfo(CFrame c0, CFrame c1, float initialAngle, float initialVelocity) : PhysRotatingJointInfo(c0, c1), initialAngle(initialAngle), initialVelocity(initialVelocity) {}
 };
 
 class PhysWorld;
 struct PhysJoint {
 public:
     JPH::TwoBodyConstraint* jointImpl;
+    PhysWorld* parentWorld;
 
     void setAngularVelocity(float velocity);
+    void setTargetAngle(float angle);
 };
 
 struct RaycastResult;
@@ -104,7 +117,9 @@ class PhysWorld : public std::enable_shared_from_this<PhysWorld> {
     ObjectLayerPairFilter objectLayerPairFilter;
     JPH::PhysicsSystem worldImpl;
     std::list<std::shared_ptr<BasePart>> simulatedBodies;
+    std::list<std::shared_ptr<JointInstance>> drivenJoints;
 
+    friend PhysJoint;
 public:
     PhysWorld();
     ~PhysWorld();
@@ -117,6 +132,11 @@ public:
     PhysJoint createJoint(PhysJointInfo& type, std::shared_ptr<BasePart> part0, std::shared_ptr<BasePart> part1);
     void destroyJoint(PhysJoint joint);
 
+    void trackDrivenJoint(std::shared_ptr<JointInstance> motor);
+    void untrackDrivenJoint(std::shared_ptr<JointInstance> motor);
+
+    void setCFrameInternal(std::shared_ptr<BasePart> part, CFrame frame);
+
     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);