feat(physics): a better implemented motor6d

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

@@ -1,4 +1,5 @@
 #include "motor6d.h"
+#include "datatypes/vector.h"
 #include "objects/part/part.h"
 #include "objects/service/workspace.h"
 #include "rendering/renderer.h"
@@ -10,6 +11,7 @@ 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() {
@@ -31,4 +33,40 @@ void Motor6D::onUpdated(std::string property) {
     } else if (property == "MaxVelocity") {
         joint.setAngularVelocity(maxVelocity);
     }
+}
+
+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;
+
+    if (!part1.lock()->anchored) {
+        CFrame anchorPoint = part0.lock()->cframe * c0;
+        Vector3 angles = anchorPoint.ToEulerAnglesXYZ();
+        CFrame rotatedAnchor = CFrame::FromEulerAnglesXYZ({angles.X(), angles.Y(), currentAngle}) + anchorPoint.Position();
+        CFrame newFrame = rotatedAnchor * c1.Inverse();
+        
+        part1.lock()->cframe = newFrame;
+        jointWorkspace.lock()->SetPhysicalCFrameInternal(part1.lock(), newFrame);
+    } else if (!part0.lock()->anchored) {
+        CFrame anchorPoint = part1.lock()->cframe * c1;
+        Vector3 angles = anchorPoint.ToEulerAnglesXYZ();
+        CFrame rotatedAnchor = CFrame::FromEulerAnglesXYZ({angles.X(), angles.Y(), currentAngle}) + anchorPoint.Position();
+        CFrame newFrame = rotatedAnchor * c0.Inverse();
+        
+        part0.lock()->cframe = newFrame;
+        jointWorkspace.lock()->SetPhysicalCFrameInternal(part0.lock(), newFrame);
+    }
 }

core/src/objects/joint/motor6d.h

@@ -12,10 +12,14 @@ class DEF_INST Motor6D : public JointInstance {
 
     virtual void buildJoint() override;
     void onUpdated(std::string);
+
+    void OnPhysicsStep(float deltaTime) override;
+    bool isDrivenJoint() override;
 public:
     Motor6D();
     ~Motor6D();
 
+    DEF_PROP float currentAngle;
     DEF_PROP_PHYS float desiredAngle;
     DEF_PROP_PHYS float maxVelocity;
 

core/src/objects/part/basepart.cpp

@@ -305,6 +305,11 @@ void BasePart::MakeJoints() {
     }
 }
 
+void BasePart::UpdateNoBreakJoints() {    
+    if (workspace())
+        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/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>
@@ -56,6 +55,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;
 }
 
@@ -232,9 +240,8 @@ PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr<BasePart>
             static_cast<JPH::HingeConstraint*>(constraint)->SetMotorState(JPH::EMotorState::Velocity);
             static_cast<JPH::HingeConstraint*>(constraint)->SetTargetAngularVelocity(-info->initialVelocity);
         } else if (PhysStepperJointInfo* info = dynamic_cast<PhysStepperJointInfo*>(&type)) {
-            static_cast<JPH::HingeConstraint*>(constraint)->SetMotorState(JPH::EMotorState::Position);
-            static_cast<JPH::HingeConstraint*>(constraint)->SetTargetAngularVelocity(-info->initialVelocity);
-            static_cast<JPH::HingeConstraint*>(constraint)->SetTargetAngle(info->initialAngle);
+            static_cast<JPH::HingeConstraint*>(constraint)->SetMotorState(JPH::EMotorState::Velocity);
+            static_cast<JPH::HingeConstraint*>(constraint)->SetTargetAngularVelocity(0);
         }
     } else {
         panic();
@@ -244,6 +251,19 @@ PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr<BasePart>
     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) {

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; };
@@ -116,6 +117,7 @@ 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:
@@ -130,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);