From f2e5af60384d80f54b04ae03de4ed59b21fffd58 Mon Sep 17 00:00:00 2001 From: Lars Ivar Hatledal Date: Mon, 26 Feb 2024 22:44:27 +0100 Subject: [PATCH] physx wip --- examples/extras/physics/PxEngine.hpp | 84 +++++++++++++++++++++----- examples/extras/physics/physx_demo.cpp | 71 +++++++++++++++++++--- 2 files changed, 131 insertions(+), 24 deletions(-) diff --git a/examples/extras/physics/PxEngine.hpp b/examples/extras/physics/PxEngine.hpp index 31ee4078..a9653a0c 100644 --- a/examples/extras/physics/PxEngine.hpp +++ b/examples/extras/physics/PxEngine.hpp @@ -11,7 +11,7 @@ class PxEngine { public: - PxEngine(float timeStep = 1.f / 60) + explicit PxEngine(float timeStep = 1.f / 60) : timeStep(timeStep), sceneDesc(physics->getTolerancesScale()), onMeshRemovedListener(this) { @@ -51,38 +51,81 @@ class PxEngine { obj->position.set(pos.x, pos.y, pos.z); obj->quaternion.set(quat.x, quat.y, quat.z, quat.w); } - } - void registerMeshDynamic(threepp::Mesh& mesh) { + void registerMeshDynamic(threepp::Object3D& obj) { threepp::Vector3 worldPos; - mesh.getWorldPosition(worldPos); + obj.getWorldPosition(worldPos); physx::PxTransform transform(toPxVector3(worldPos)); - auto geometry = toPxGeometry(mesh.geometry()); + auto geometry = toPxGeometry(obj.geometry()); if (!geometry) return; - geometries[&mesh] = std::move(geometry); - auto* actor = PxCreateDynamic(*physics, transform, *geometries[&mesh], *defaultMaterial, 10.0f); + geometries[&obj] = std::move(geometry); + auto* actor = PxCreateDynamic(*physics, transform, *geometries[&obj], *defaultMaterial, 10.0f); - bodies[&mesh] = actor; + bodies[&obj] = actor; scene->addActor(*actor); - mesh.addEventListener("remove", &onMeshRemovedListener); + obj.addEventListener("remove", &onMeshRemovedListener); } - void registerMeshStatic(threepp::Mesh& mesh) { + void registerMeshStatic(threepp::Object3D& obj) { threepp::Vector3 worldPos; - mesh.getWorldPosition(worldPos); + obj.getWorldPosition(worldPos); physx::PxTransform transform(toPxVector3(worldPos)); - auto geometry = toPxGeometry(mesh.geometry()); + auto geometry = toPxGeometry(obj.geometry()); if (!geometry) return; - geometries[&mesh] = std::move(geometry); - auto* actor = PxCreateStatic(*physics, transform, *geometries[&mesh], *defaultMaterial); + geometries[&obj] = std::move(geometry); + auto* actor = PxCreateStatic(*physics, transform, *geometries[&obj], *defaultMaterial); + bodies[&obj] = actor; scene->addActor(*actor); + + obj.addEventListener("remove", &onMeshRemovedListener); + } + +// physx::PxRevoluteJoint* createRevoluteJoint(threepp::Object3D& o1, threepp::Vector3 anchor, threepp::Vector3 axis) { +// +// auto rb1 = bodies.at(&o1); +// +// threepp::Matrix4 f1; +// f1.makeRotationFromQuaternion(threepp::Quaternion().setFromUnitVectors({0, 0, 1}, axis)); +//// f1.setPosition(anchor); +// +// threepp::Matrix4 f2; +// f2.setPosition(anchor); +// +// physx::PxTransform frame1 = toPxTransform(f1); +// physx::PxTransform frame2 = toPxTransform(f2); +// +// physx::PxRevoluteJoint* joint = physx::PxRevoluteJointCreate(*physics, nullptr, frame1, rb1, frame2); +// +//// joint->set +// +// return joint; +// } + + physx::PxRevoluteJoint* createRevoluteJoint(threepp::Object3D& o1, threepp::Object3D& o2, threepp::Vector3 anchor, threepp::Vector3 axis) { + + auto rb1 = bodies.at(&o1); + auto rb2 = bodies.at(&o2); + + threepp::Matrix4 f1; + f1.makeRotationFromQuaternion(threepp::Quaternion().setFromUnitVectors({0, 0, 1}, axis)); + f1.setPosition(anchor); + + threepp::Matrix4 f2 = *o2.matrixWorld; + f2.invert().multiply(*o1.matrixWorld).multiply(f1); + + physx::PxTransform frame1 = toPxTransform(f1); + physx::PxTransform frame2 = toPxTransform(f2); + + physx::PxRevoluteJoint* joint = physx::PxRevoluteJointCreate(*physics, rb1, frame1, rb2, frame2); + + return joint; } physx::PxActor* get(threepp::Mesh& mesh) { @@ -107,7 +150,6 @@ class PxEngine { float timeStep; float internalTime{}; - // Initialize the Physics SDK physx::PxDefaultAllocator allocator; physx::PxDefaultErrorCallback errorCallback; @@ -120,7 +162,7 @@ class PxEngine { physx::PxMaterial* defaultMaterial; std::unordered_map> geometries; - std::unordered_map bodies; + std::unordered_map bodies; static physx::PxVec3 toPxVector3(const threepp::Vector3& v) { @@ -131,6 +173,16 @@ class PxEngine { return {q.x, q.y, q.z, q.w}; } + static physx::PxTransform toPxTransform(threepp::Matrix4& m) { + threepp::Vector3 pos; + threepp::Quaternion quat; + threepp::Vector3 scale; + + m.decompose(pos, quat, scale); + + return physx::PxTransform(toPxVector3(pos), toPxQuat(quat)); + } + static std::unique_ptr toPxGeometry(const threepp::BufferGeometry* geometry) { if (!geometry) return nullptr; diff --git a/examples/extras/physics/physx_demo.cpp b/examples/extras/physics/physx_demo.cpp index 121659ea..d7a48118 100644 --- a/examples/extras/physics/physx_demo.cpp +++ b/examples/extras/physics/physx_demo.cpp @@ -4,6 +4,46 @@ using namespace threepp; +namespace { + + struct MyKeyListener: KeyListener { + + explicit MyKeyListener(physx::PxRevoluteJoint* j1, physx::PxRevoluteJoint* j2): j1(j1), j2(j2) {} + + void onKeyPressed(KeyEvent evt) override { + if (evt.key == Key::NUM_1) { + j1->setDriveVelocity(-5);// Set motor speed + } else if (evt.key == Key::NUM_2) { + j1->setDriveVelocity(5);// Set motor speed + } + if (evt.key == Key::NUM_3) { + j2->setDriveVelocity(-5);// Set motor speed + } else if (evt.key == Key::NUM_4) { + j2->setDriveVelocity(5);// Set motor speed + } + } + void onKeyReleased(KeyEvent evt) override { + switch (evt.key) { + case Key::NUM_1: + case Key::NUM_2: { + j1->setDriveVelocity(0);// Set motor speed + } + + case Key::NUM_3: + case Key::NUM_4: { + j2->setDriveVelocity(0);// Set motor speed + } + } + } + + private: + float speed = 5; + physx::PxRevoluteJoint* j1; + physx::PxRevoluteJoint* j2; + }; + +}// namespace + int main() { Canvas canvas("PhysX"); @@ -15,13 +55,14 @@ int main() { PerspectiveCamera camera(60, canvas.aspect()); camera.position.z = 10; - auto box = Mesh::create(BoxGeometry::create(1,1,1), MeshStandardMaterial::create()); - box->position.y = 10; - scene.add(box); + auto box1 = Mesh::create(BoxGeometry::create(0.5, 2, 0.5), MeshStandardMaterial::create()); + box1->position.y = 1; + scene.add(box1); - auto sphere = Mesh::create(SphereGeometry::create(0.5), MeshStandardMaterial::create({{"color", Color::green}})); - sphere->position.y = 12; - scene.add(sphere); + auto box2 = Mesh::create(BoxGeometry::create(1, 0.5, 0.5), MeshStandardMaterial::create()); + box2->position.y = 2 + 0.25; + box2->position.x = 0.5 + 0.25; + scene.add(box2); auto ground = Mesh::create(BoxGeometry::create(10, 0.1, 10), MeshStandardMaterial::create({{"color", Color::blueviolet}})); scene.add(ground); @@ -31,11 +72,25 @@ int main() { scene.add(light); PxEngine engine; - engine.registerMeshDynamic(*sphere); - engine.registerMeshDynamic(*box); + engine.registerMeshDynamic(*box1); + engine.registerMeshDynamic(*box2); engine.registerMeshStatic(*ground); + auto joint1 = engine.createRevoluteJoint(*ground, *box1, {0, 0, 0}, {0, 1, 0}); + auto joint2 = engine.createRevoluteJoint(*box1, *box2, {0.25, 1, 0}, {1, 0, 0}); + + joint1->setRevoluteJointFlag(physx::PxRevoluteJointFlag::eDRIVE_ENABLED, true); + joint1->setRevoluteJointFlag(physx::PxRevoluteJointFlag::eLIMIT_ENABLED, true); + joint1->setLimit(physx::PxJointAngularLimitPair(-math::degToRad(90), math::degToRad(90))); + + joint2->setRevoluteJointFlag(physx::PxRevoluteJointFlag::eDRIVE_ENABLED, true); + joint2->setRevoluteJointFlag(physx::PxRevoluteJointFlag::eLIMIT_ENABLED, true); + joint2->setLimit(physx::PxJointAngularLimitPair(-math::degToRad(45), math::degToRad(45))); + + MyKeyListener keyListener(joint1, joint2); + canvas.addKeyListener(keyListener); OrbitControls controls(camera, canvas); + controls.enableKeys = false; Clock clock; canvas.animate([&] {