From c8be848cea35fac435775589703da869365d09d4 Mon Sep 17 00:00:00 2001 From: maelstrom Date: Sat, 6 Sep 2025 17:20:51 +0200 Subject: [PATCH] feat(physics): a better implemented motor6d --- core/src/objects/joint/jointinstance.cpp | 9 ++++++ core/src/objects/joint/jointinstance.h | 3 ++ core/src/objects/joint/motor6d.cpp | 38 ++++++++++++++++++++++++ core/src/objects/joint/motor6d.h | 4 +++ core/src/objects/part/basepart.cpp | 5 ++++ core/src/objects/part/basepart.h | 1 + core/src/objects/service/workspace.h | 6 ++-- core/src/physics/world.cpp | 26 ++++++++++++++-- core/src/physics/world.h | 7 +++++ 9 files changed, 94 insertions(+), 5 deletions(-) diff --git a/core/src/objects/joint/jointinstance.cpp b/core/src/objects/joint/jointinstance.cpp index dc7cbec..40900a3 100644 --- a/core/src/objects/joint/jointinstance.cpp +++ b/core/src/objects/joint/jointinstance.cpp @@ -20,12 +20,20 @@ void JointInstance::OnAncestryChanged(nullable std::shared_ptr, 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()); jointWorkspace.lock()->DestroyJoint(joint); if (!oldPart0.expired()) oldPart0.lock()->untrackJoint(shared()); @@ -54,6 +62,7 @@ void JointInstance::Update() { part0.lock()->trackJoint(shared()); part1.lock()->trackJoint(shared()); + jointWorkspace.lock()->TrackDrivenJoint(shared()); } nullable std::shared_ptr JointInstance::workspaceOfPart(std::shared_ptr part) { diff --git a/core/src/objects/joint/jointinstance.h b/core/src/objects/joint/jointinstance.h index e06b1c2..a9ef82f 100644 --- a/core/src/objects/joint/jointinstance.h +++ b/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(); }; diff --git a/core/src/objects/joint/motor6d.cpp b/core/src/objects/joint/motor6d.cpp index a32b3b8..14ceaa3 100644 --- a/core/src/objects/joint/motor6d.cpp +++ b/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); + } } \ No newline at end of file diff --git a/core/src/objects/joint/motor6d.h b/core/src/objects/joint/motor6d.h index 3872e08..0d2f8b6 100644 --- a/core/src/objects/joint/motor6d.h +++ b/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; diff --git a/core/src/objects/part/basepart.cpp b/core/src/objects/part/basepart.cpp index ee15df8..13c3ecb 100644 --- a/core/src/objects/part/basepart.cpp +++ b/core/src/objects/part/basepart.cpp @@ -305,6 +305,11 @@ void BasePart::MakeJoints() { } } +void BasePart::UpdateNoBreakJoints() { + if (workspace()) + workspace()->SyncPartPhysics(std::dynamic_pointer_cast(this->shared_from_this())); +} + void BasePart::trackJoint(std::shared_ptr joint) { if (!joint->part0.expired() && joint->part0.lock() == shared_from_this()) { for (auto it = primaryJoints.begin(); it != primaryJoints.end();) { diff --git a/core/src/objects/part/basepart.h b/core/src/objects/part/basepart.h index 6b9cc52..2835efd 100644 --- a/core/src/objects/part/basepart.h +++ b/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(); diff --git a/core/src/objects/service/workspace.h b/core/src/objects/service/workspace.h index 1cd1a0a..31ef49f 100644 --- a/core/src/objects/service/workspace.h +++ b/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 -#include #include #include #include @@ -56,6 +55,9 @@ public: inline PhysJoint CreateJoint(PhysJointInfo& info, std::shared_ptr part0, std::shared_ptr part1) { return physicsWorld->createJoint(info, part0, part1); } inline void DestroyJoint(PhysJoint joint) { physicsWorld->destroyJoint(joint); } + inline void TrackDrivenJoint(std::shared_ptr motor) { return physicsWorld->trackDrivenJoint(motor); } + inline void UntrackDrivenJoint(std::shared_ptr motor) { return physicsWorld->untrackDrivenJoint(motor); } + void PhysicsStep(float deltaTime); inline std::optional CastRayNearest(glm::vec3 point, glm::vec3 rotation, float maxLength, std::optional filter = std::nullopt, unsigned short categoryMaskBits = 0xFFFF) { return physicsWorld->castRay(point, rotation, maxLength, filter, categoryMaskBits); } }; \ No newline at end of file diff --git a/core/src/physics/world.cpp b/core/src/physics/world.cpp index 2e9c49e..7092772 100644 --- a/core/src/physics/world.cpp +++ b/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 #include #include +#include +#include #include static JPH::TempAllocator* allocator; @@ -194,6 +197,11 @@ void PhysWorld::step(float deltaTime) { part->cframe = CFrame(convert(interface.GetPosition(bodyID)), convert(interface.GetRotation(bodyID))); } + // Update joints + for (std::shared_ptr joint : drivenJoints) { + joint->OnPhysicsStep(deltaTime); + } + physTime = tu_clock_micros() - startTime; } @@ -232,9 +240,8 @@ PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr static_cast(constraint)->SetMotorState(JPH::EMotorState::Velocity); static_cast(constraint)->SetTargetAngularVelocity(-info->initialVelocity); } else if (PhysStepperJointInfo* info = dynamic_cast(&type)) { - static_cast(constraint)->SetMotorState(JPH::EMotorState::Position); - static_cast(constraint)->SetTargetAngularVelocity(-info->initialVelocity); - static_cast(constraint)->SetTargetAngle(info->initialAngle); + static_cast(constraint)->SetMotorState(JPH::EMotorState::Velocity); + static_cast(constraint)->SetTargetAngularVelocity(0); } } else { panic(); @@ -244,6 +251,19 @@ PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr return { constraint, this }; } +void PhysWorld::trackDrivenJoint(std::shared_ptr motor) { + drivenJoints.push_back(motor); +} + +void PhysWorld::untrackDrivenJoint(std::shared_ptr 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) { diff --git a/core/src/physics/world.h b/core/src/physics/world.h index ead6799..3e77691 100644 --- a/core/src/physics/world.h +++ b/core/src/physics/world.h @@ -14,6 +14,7 @@ #include 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 { ObjectLayerPairFilter objectLayerPairFilter; JPH::PhysicsSystem worldImpl; std::list> simulatedBodies; + std::list> drivenJoints; friend PhysJoint; public: @@ -130,6 +132,11 @@ public: PhysJoint createJoint(PhysJointInfo& type, std::shared_ptr part0, std::shared_ptr part1); void destroyJoint(PhysJoint joint); + void trackDrivenJoint(std::shared_ptr motor); + void untrackDrivenJoint(std::shared_ptr motor); + + void setCFrameInternal(std::shared_ptr part, CFrame frame); + inline const std::list>& getSimulatedBodies() { return simulatedBodies; } void syncBodyProperties(std::shared_ptr); std::optional castRay(Vector3 point, Vector3 rotation, float maxLength, std::optional filter, unsigned short categoryMaskBits);