From a6e2c3ca440ee0d66056f3ec2447120e462e2a7b Mon Sep 17 00:00:00 2001 From: maelstrom Date: Thu, 4 Sep 2025 00:12:33 +0200 Subject: [PATCH] feat(physics): some semblence of motor6d --- core/CMakeLists.txt | 3 +++ core/src/objects/joint/motor6d.cpp | 34 ++++++++++++++++++++++++ core/src/objects/joint/motor6d.h | 26 ++++++++++++++++++ core/src/objects/joint/rotatev.cpp | 2 +- core/src/objects/meta.cpp | 42 +++++++++++++++++------------- core/src/physics/world.cpp | 21 ++++++++++++--- core/src/physics/world.h | 19 +++++++++++--- 7 files changed, 121 insertions(+), 26 deletions(-) create mode 100644 core/src/objects/joint/motor6d.cpp create mode 100644 core/src/objects/joint/motor6d.h diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index b67fd6b..7a14c9e 100644 --- a/core/CMakeLists.txt +++ b/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 diff --git a/core/src/objects/joint/motor6d.cpp b/core/src/objects/joint/motor6d.cpp new file mode 100644 index 0000000..a32b3b8 --- /dev/null +++ b/core/src/objects/joint/motor6d.cpp @@ -0,0 +1,34 @@ +#include "motor6d.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 = 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; +} + +void Motor6D::onUpdated(std::string property) { + if (property == "DesiredAngle") { + joint.setTargetAngle(desiredAngle); + } else if (property == "MaxVelocity") { + joint.setAngularVelocity(maxVelocity); + } +} \ No newline at end of file diff --git a/core/src/objects/joint/motor6d.h b/core/src/objects/joint/motor6d.h new file mode 100644 index 0000000..3872e08 --- /dev/null +++ b/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 + +#define DEF_PROP_PHYS DEF_PROP_(on_update=onUpdated) + +class DEF_INST Motor6D : public JointInstance { + AUTOGEN_PREAMBLE + + virtual void buildJoint() override; + void onUpdated(std::string); +public: + Motor6D(); + ~Motor6D(); + + DEF_PROP_PHYS float desiredAngle; + DEF_PROP_PHYS float maxVelocity; + + static inline std::shared_ptr New() { return std::make_shared(); }; + static inline std::shared_ptr Create() { return std::make_shared(); }; +}; + +#undef DEF_PROP_PHYS \ No newline at end of file diff --git a/core/src/objects/joint/rotatev.cpp b/core/src/objects/joint/rotatev.cpp index 669913b..68ce89b 100644 --- a/core/src/objects/joint/rotatev.cpp +++ b/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; diff --git a/core/src/objects/meta.cpp b/core/src/objects/meta.cpp index 55443d3..ca89284 100644 --- a/core/src/objects/meta.cpp +++ b/core/src/objects/meta.cpp @@ -1,22 +1,27 @@ #include "meta.h" -#include "objects/folder.h" -#include "objects/hint.h" -#include "objects/joint/jointinstance.h" -#include "objects/joint/rotate.h" -#include "objects/joint/rotatev.h" -#include "objects/joint/weld.h" -#include "objects/message.h" -#include "objects/part/wedgepart.h" -#include "objects/service/jointsservice.h" -#include "objects/model.h" -#include "objects/part/part.h" -#include "objects/joint/snap.h" -#include "objects/script.h" -#include "objects/service/script/scriptcontext.h" -#include "objects/service/script/serverscriptservice.h" -#include "objects/service/selection.h" -#include "objects/service/workspace.h" -#include "objects/datamodel.h" + +#define DECLTYPE(className) class className { public: const static InstanceType TYPE; }; + +DECLTYPE(DataModel); +DECLTYPE(BasePart); +DECLTYPE(Part); +DECLTYPE(WedgePart); +DECLTYPE(Snap); +DECLTYPE(Weld); +DECLTYPE(Rotate); +DECLTYPE(RotateV); +DECLTYPE(Motor6D); +DECLTYPE(JointInstance); +DECLTYPE(Script); +DECLTYPE(Model); +DECLTYPE(Message); +DECLTYPE(Hint); +// DECLTYPE(Folder); +DECLTYPE(Workspace); +DECLTYPE(JointsService); +DECLTYPE(ScriptContext); +DECLTYPE(ServerScriptService); +DECLTYPE(Selection); std::map INSTANCE_MAP = { { "Instance", &Instance::TYPE }, @@ -29,6 +34,7 @@ std::map INSTANCE_MAP = { { "Weld", &Weld::TYPE }, { "Rotate", &Rotate::TYPE }, { "RotateV", &RotateV::TYPE }, + { "Motor6D", &Motor6D::TYPE }, { "JointInstance", &JointInstance::TYPE }, { "Script", &Script::TYPE }, { "Model", &Model::TYPE }, diff --git a/core/src/physics/world.cpp b/core/src/physics/world.cpp index 06f3ab0..2e9c49e 100644 --- a/core/src/physics/world.cpp +++ b/core/src/physics/world.cpp @@ -226,28 +226,41 @@ PhysJoint PhysWorld::createJoint(PhysJointInfo& type, std::shared_ptr settings.mPoint2 = convert(info->c1.Position()); settings.mNormalAxis2 = convert(info->c1.RightVector()); settings.mHingeAxis2 = convert(info->c1.LookVector()); - // settings.mMotorSettings = JPH::MotorSettings(1.0f, 1.0f); constraint = settings.Create(*part0->rigidBody.bodyImpl, *part1->rigidBody.bodyImpl); - if (info->motorized) { + + if (PhysMotorizedJointInfo* info = dynamic_cast(&type)) { 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); } } else { panic(); } worldImpl.AddConstraint(constraint); - return { constraint }; + return { constraint, this }; } // 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(jointImpl); - if (!constraint) return; constraint->SetTargetAngularVelocity(-velocity); } +void PhysJoint::setTargetAngle(float angle) { + JPH::HingeConstraint* constraint = static_cast(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); } diff --git a/core/src/physics/world.h b/core/src/physics/world.h index 5612555..ead6799 100644 --- a/core/src/physics/world.h +++ b/core/src/physics/world.h @@ -28,19 +28,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 PhysRotatingJointInfo(CFrame c0, CFrame c1, float initialVelocity) : c0(c0), c1(c1), motorized(true), initialVelocity(initialVelocity) {} + inline PhysMotorizedJointInfo(CFrame c0, CFrame c1, float initialVelocity) : PhysRotatingJointInfo(c0, c1), 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; @@ -105,6 +117,7 @@ class PhysWorld : public std::enable_shared_from_this { JPH::PhysicsSystem worldImpl; std::list> simulatedBodies; + friend PhysJoint; public: PhysWorld(); ~PhysWorld();