diff --git a/src/esp/physics/PhysicsManager.cpp b/src/esp/physics/PhysicsManager.cpp index 74584a8ed2..0f0cb92223 100644 --- a/src/esp/physics/PhysicsManager.cpp +++ b/src/esp/physics/PhysicsManager.cpp @@ -341,6 +341,30 @@ Magnum::Quaternion PhysicsManager::getRotation(const int physObjectID) const { return existingObjects_.at(physObjectID)->rotation(); } +void PhysicsManager::setLinearVelocity(const int physObjectID, + const Magnum::Vector3& linVel) { + assertIDValidity(physObjectID); + existingObjects_.at(physObjectID)->setLinearVelocity(linVel); +} + +void PhysicsManager::setAngularVelocity(const int physObjectID, + const Magnum::Vector3& angVel) { + assertIDValidity(physObjectID); + existingObjects_.at(physObjectID)->setAngularVelocity(angVel); +} + +Magnum::Vector3 PhysicsManager::getLinearVelocity( + const int physObjectID) const { + assertIDValidity(physObjectID); + return existingObjects_.at(physObjectID)->getLinearVelocity(); +} + +Magnum::Vector3 PhysicsManager::getAngularVelocity( + const int physObjectID) const { + assertIDValidity(physObjectID); + return existingObjects_.at(physObjectID)->getAngularVelocity(); +} + //============ Object Setter functions ============= void PhysicsManager::setMass(const int physObjectID, const double mass) { assertIDValidity(physObjectID); diff --git a/src/esp/physics/PhysicsManager.h b/src/esp/physics/PhysicsManager.h index c7e57e3772..4371eadd69 100644 --- a/src/esp/physics/PhysicsManager.h +++ b/src/esp/physics/PhysicsManager.h @@ -679,6 +679,52 @@ class PhysicsManager { void applyImpulseTorque(const int physObjectID, const Magnum::Vector3& impulse); + /** + * @brief Set linear velocity for an object with @ref MotionType::DYNAMIC. + * + * Does nothing for @ref MotionType::KINEMATIC or @ref MotionType::STATIC + * objects. + * @param physObjectID The object ID and key identifying the object in @ref + * PhysicsManager::existingObjects_. + * @param linVel Linear velocity to set. + */ + void setLinearVelocity(const int physObjectID, const Magnum::Vector3& linVel); + + /** + * @brief Set angular velocity for an object with @ref MotionType::DYNAMIC. + * + * Does nothing for @ref MotionType::KINEMATIC or @ref MotionType::STATIC + * objects. + * @param physObjectID The object ID and key identifying the object in @ref + * PhysicsManager::existingObjects_. + * @param angVel Angular velocity vector corresponding to world unit axis + * angles. + */ + void setAngularVelocity(const int physObjectID, + const Magnum::Vector3& angVel); + + /** + * @brief Get linear velocity of an object with @ref MotionType::DYNAMIC. + * + * Always zero for @ref MotionType::KINEMATIC or @ref MotionType::STATIC + * objects. + * @param physObjectID The object ID and key identifying the object in @ref + * PhysicsManager::existingObjects_. + * @return Linear velocity of the object. + */ + Magnum::Vector3 getLinearVelocity(const int physObjectID) const; + + /** + * @brief Get angular velocity of an object with @ref MotionType::DYNAMIC. + * + * Always zero for @ref MotionType::KINEMATIC or @ref MotionType::STATIC + * objects. + * @param physObjectID The object ID and key identifying the object in @ref + * PhysicsManager::existingObjects_. + * @return Angular velocity vector corresponding to world unit axis angles. + */ + Magnum::Vector3 getAngularVelocity(const int physObjectID) const; + /** @brief Set bounding box rendering for the object true or false. * @param physObjectID The object ID and key identifying the object in @ref * PhysicsManager::existingObjects_. diff --git a/src/esp/physics/RigidObject.h b/src/esp/physics/RigidObject.h index d8e7557469..9cb4b49693 100644 --- a/src/esp/physics/RigidObject.h +++ b/src/esp/physics/RigidObject.h @@ -218,6 +218,49 @@ class RigidObject : public scene::SceneNode { */ virtual void applyImpulseTorque(const Magnum::Vector3& impulse); + /** + * @brief Virtual linear velocity setter for an object. + * + * Does nothing for default @ref MotionType::KINEMATIC or @ref + * MotionType::STATIC objects. + * @param linVel Linear velocity to set. + */ + virtual void setLinearVelocity( + CORRADE_UNUSED const Magnum::Vector3& linVel){}; + + /** + * @brief Virtual angular velocity setter for an object. + * + * Does nothing for default @ref MotionType::KINEMATIC or @ref + * MotionType::STATIC objects. + * @param angVel Angular velocity vector corresponding to world unit axis + * angles. + */ + virtual void setAngularVelocity( + CORRADE_UNUSED const Magnum::Vector3& angVel){}; + + /** + * @brief Virtual linear velocity getter for an object. + * + * Returns zero for default @ref MotionType::KINEMATIC or @ref + * MotionType::STATIC objects. + * @return Linear velocity of the object. + */ + virtual Magnum::Vector3 getLinearVelocity() const { + return Magnum::Vector3(); + }; + + /** + * @brief Virtual angular velocity getter for an object. + * + * Returns zero for default @ref MotionType::KINEMATIC or @ref + * MotionType::STATIC objects. + * @return Angular velocity vector corresponding to world unit axis angles. + */ + virtual Magnum::Vector3 getAngularVelocity() const { + return Magnum::Vector3(); + }; + /** * @brief Remove the object from any connected physics simulator implemented * by a derived @ref PhysicsManager. Does nothing for default @ref diff --git a/src/esp/physics/bullet/BulletRigidObject.cpp b/src/esp/physics/bullet/BulletRigidObject.cpp index b785939fc0..f69f4207aa 100644 --- a/src/esp/physics/bullet/BulletRigidObject.cpp +++ b/src/esp/physics/bullet/BulletRigidObject.cpp @@ -364,6 +364,30 @@ void BulletRigidObject::applyForce(const Magnum::Vector3& force, } } +void BulletRigidObject::setLinearVelocity(const Magnum::Vector3& linVel) { + if (rigidObjectType_ == RigidObjectType::OBJECT && + objectMotionType_ == MotionType::DYNAMIC) { + setActive(); + bObjectRigidBody_->setLinearVelocity(btVector3(linVel)); + } +} + +void BulletRigidObject::setAngularVelocity(const Magnum::Vector3& angVel) { + if (rigidObjectType_ == RigidObjectType::OBJECT && + objectMotionType_ == MotionType::DYNAMIC) { + setActive(); + bObjectRigidBody_->setAngularVelocity(btVector3(angVel)); + } +} + +Magnum::Vector3 BulletRigidObject::getLinearVelocity() const { + return Magnum::Vector3{bObjectRigidBody_->getLinearVelocity()}; +} + +Magnum::Vector3 BulletRigidObject::getAngularVelocity() const { + return Magnum::Vector3{bObjectRigidBody_->getAngularVelocity()}; +} + void BulletRigidObject::applyImpulse(const Magnum::Vector3& impulse, const Magnum::Vector3& relPos) { if (rigidObjectType_ == RigidObjectType::OBJECT && diff --git a/src/esp/physics/bullet/BulletRigidObject.h b/src/esp/physics/bullet/BulletRigidObject.h index 2c4ac84011..a730ab38af 100644 --- a/src/esp/physics/bullet/BulletRigidObject.h +++ b/src/esp/physics/bullet/BulletRigidObject.h @@ -214,6 +214,41 @@ class BulletRigidObject : public RigidObject { */ void applyImpulseTorque(const Magnum::Vector3& impulse); + /** + * @brief Linear velocity setter for an object. + * + * Does nothing for @ref MotionType::KINEMATIC or @ref MotionType::STATIC + * objects. Sets internal @ref btRigidObject state. Treated as initial + * velocity during simulation simulation step. + * @param linVel Linear velocity to set. + */ + void setLinearVelocity(const Magnum::Vector3& linVel) override; + + /** + * @brief Angular velocity setter for an object. + * + * Does nothing for @ref MotionType::KINEMATIC or @ref MotionType::STATIC + * objects. Sets internal @ref btRigidObject state. Treated as initial + * velocity during simulation simulation step. + * @param angVel Angular velocity vector corresponding to world unit axis + * angles. + */ + void setAngularVelocity(const Magnum::Vector3& angVel) override; + + /** + * @brief Virtual linear velocity getter for an object. + * + * @return Linear velocity of the object. + */ + Magnum::Vector3 getLinearVelocity() const override; + + /** + * @brief Angular velocity getter for an object. + * + * @return Angular velocity vector corresponding to world unit axis angles. + */ + Magnum::Vector3 getAngularVelocity() const override; + /** * @brief Remove the object from the world. * See @ref btDiscreteDynamicsWorld::removeRigidBody for @ref diff --git a/src/tests/PhysicsTest.cpp b/src/tests/PhysicsTest.cpp index f17250152b..295039e087 100644 --- a/src/tests/PhysicsTest.cpp +++ b/src/tests/PhysicsTest.cpp @@ -364,3 +364,46 @@ TEST_F(PhysicsManagerTest, ConfigurableScaling) { #endif } } + +TEST_F(PhysicsManagerTest, TestVelocityControl) { + // test scaling of objects via template configuration (visual and collision) + LOG(INFO) << "Starting physics test: TestVelocityControl"; + + std::string objectFile = Cr::Utility::Directory::join( + dataDir, "test_assets/objects/transform_box.glb"); + + std::string sceneFile = + Cr::Utility::Directory::join(dataDir, "test_assets/scenes/plane.glb"); + + initScene(sceneFile); + + esp::assets::PhysicsObjectAttributes physicsObjectAttributes; + physicsObjectAttributes.setString("renderMeshHandle", objectFile); + physicsObjectAttributes.setDouble("margin", 0.0); + resourceManager_.loadObject(physicsObjectAttributes, objectFile); + + auto& drawables = sceneManager_.getSceneGraph(sceneID_).getDrawables(); + + int objectId = physicsManager_->addObject(objectFile, &drawables); + physicsManager_->setTranslation(objectId, Magnum::Vector3{0, 1.0, 0}); + + Magnum::Vector3 commandLinVel(1.0, 1.0, 1.0); + Magnum::Vector3 commandAngVel(1.0, 1.0, 1.0); + + if (physicsManager_->getPhysicsSimulationLibrary() == + PhysicsManager::PhysicsSimulationLibrary::BULLET) { + physicsManager_->setLinearVelocity(objectId, commandLinVel); + physicsManager_->setAngularVelocity(objectId, commandAngVel); + + ASSERT_EQ(physicsManager_->getLinearVelocity(objectId), commandLinVel); + ASSERT_EQ(physicsManager_->getAngularVelocity(objectId), commandAngVel); + } else if (physicsManager_->getPhysicsSimulationLibrary() == + PhysicsManager::PhysicsSimulationLibrary::NONE) { + physicsManager_->setLinearVelocity(objectId, commandLinVel); + physicsManager_->setAngularVelocity(objectId, commandAngVel); + + // default kinematics always 0 velocity when queried + ASSERT_EQ(physicsManager_->getLinearVelocity(objectId), Magnum::Vector3{}); + ASSERT_EQ(physicsManager_->getAngularVelocity(objectId), Magnum::Vector3{}); + } +}