diff --git a/src/esp/physics/PhysicsManager.cpp b/src/esp/physics/PhysicsManager.cpp index c99b828bc2..34308edbbb 100644 --- a/src/esp/physics/PhysicsManager.cpp +++ b/src/esp/physics/PhysicsManager.cpp @@ -189,6 +189,16 @@ void PhysicsManager::stepPhysics(double dt) { double targetTime = worldTime_ + dt; while (worldTime_ < targetTime) { // per fixed-step operations can be added here + + // kinematic velocity control intergration + for (auto& object : existingObjects_) { + VelocityControl& velControl = object.second->getVelocityControl(); + if (velControl.controllingAngVel || velControl.controllingLinVel) { + scene::SceneNode& objectSceneNode = object.second->node(); + objectSceneNode.setTransformation(velControl.integrateTransform( + fixedTimeStep_, objectSceneNode.transformation())); + } + } worldTime_ += fixedTimeStep_; } } @@ -361,6 +371,11 @@ Magnum::Vector3 PhysicsManager::getAngularVelocity( return existingObjects_.at(physObjectID)->getAngularVelocity(); } +VelocityControl& PhysicsManager::getVelocityControl(const int physObjectID) { + assertIDValidity(physObjectID); + return existingObjects_.at(physObjectID)->getVelocityControl(); +} + //============ 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 f6bf4b7de6..8d2b816c18 100644 --- a/src/esp/physics/PhysicsManager.h +++ b/src/esp/physics/PhysicsManager.h @@ -726,6 +726,10 @@ class PhysicsManager { */ Magnum::Vector3 getAngularVelocity(const int physObjectID) const; + /**@brief Retrieves a reference to the VelocityControl struct for this object. + */ + VelocityControl& getVelocityControl(const int physObjectID); + /** @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.cpp b/src/esp/physics/RigidObject.cpp index 7f5ddb0bcf..d467cd8284 100644 --- a/src/esp/physics/RigidObject.cpp +++ b/src/esp/physics/RigidObject.cpp @@ -213,5 +213,35 @@ Magnum::Matrix3 RigidObject::getInertiaMatrix() { return inertia; } +////////////////// +// VelocityControl +Magnum::Matrix4 VelocityControl::integrateTransform( + const float dt, + const Magnum::Matrix4& objectTransform) { + // linear first + Magnum::Vector3 newTranslation = objectTransform.translation(); + if (controllingLinVel) { + if (linVelIsLocal) { + newTranslation += objectTransform.rotation() * + (linVel * dt); // avoid local scaling of the velocity + } else { + newTranslation += linVel * dt; + } + } + + Magnum::Matrix3 newRotationScaling = objectTransform.rotationScaling(); + // then angular + if (controllingAngVel) { + Magnum::Vector3 globalAngVel = angVel; + if (angVelIsLocal) { + globalAngVel = objectTransform.rotation() * angVel; + } + Magnum::Quaternion q = Magnum::Quaternion::rotation( + Magnum::Rad{(globalAngVel * dt).length()}, globalAngVel.normalized()); + newRotationScaling = q.toMatrix() * newRotationScaling; + } + return Magnum::Matrix4::from(newRotationScaling, newTranslation); +} + } // namespace physics } // namespace esp diff --git a/src/esp/physics/RigidObject.h b/src/esp/physics/RigidObject.h index f12a76e47a..166df89854 100644 --- a/src/esp/physics/RigidObject.h +++ b/src/esp/physics/RigidObject.h @@ -6,7 +6,8 @@ /** @file * @brief Class @ref esp::physics::RigidObject, enum @ref - * esp::physics::MotionType, enum @ref esp::physics::RigidObjectType + * esp::physics::MotionType, enum @ref esp::physics::RigidObjectType, struct + * @ref VelocityControl */ #include @@ -80,6 +81,47 @@ enum class RigidObjectType { }; +/**@brief Convenience struct for applying constant velocity control to a rigid + * body. */ +struct VelocityControl { + public: + /**@brief Constant linear velocity. */ + Magnum::Vector3 linVel; + /**@brief Constant angular velocity. */ + Magnum::Vector3 angVel; + /**@brief Whether or not to set linear control velocity before stepping. */ + bool controllingLinVel = false; + /**@brief Whether or not to set linear control velocity in local space. + * + * Useful for commanding actions such as "forward", or "strafe". + */ + bool linVelIsLocal = false; + + /**@brief Whether or not to set angular control velocity before stepping. */ + bool controllingAngVel = false; + + /**@brief Whether or not to set angular control velocity in local space. + * + * Useful for commanding actions such as "roll" and "yaw". + */ + bool angVelIsLocal = false; + + /** + * @brief Compute the result of applying constant control velocities to the + * provided object transform. + * + * Default implementation uses explicit Euler integration. + * @param dt The discrete timestep over which to integrate. + * @param objectTransformation The initial state of the object before applying + * velocity control. + * @return The new state of the object after applying velocity control over + * dt. + */ + virtual Magnum::Matrix4 integrateTransform( + const float dt, + const Magnum::Matrix4& objectTransform); +}; + /** @brief An AbstractFeature3D representing an individual rigid object instance attached to a SceneNode, updating its state through simulation. This may be a @@ -277,6 +319,10 @@ class RigidObject : public Magnum::SceneGraph::AbstractFeature3D { return Magnum::Vector3(); }; + /**@brief Retrieves a reference to the VelocityControl struct for this object. + */ + VelocityControl& getVelocityControl() { return velControl_; }; + // ==== Transformations === /** @brief Set the 4x4 transformation matrix of the object kinematically. @@ -508,6 +554,12 @@ class RigidObject : public Magnum::SceneGraph::AbstractFeature3D { scene::SceneNode* BBNode_ = nullptr; protected: + /** + * @brief Convenience variable: specifies a constant control velocity (linear + * | angular) applied to the rigid body before each step. + */ + VelocityControl velControl_; + /** @brief The @ref MotionType of the object. Determines what operations can * be performed on this object. */ MotionType objectMotionType_; diff --git a/src/esp/physics/bullet/BulletPhysicsManager.cpp b/src/esp/physics/bullet/BulletPhysicsManager.cpp index a2094f5dd4..1026522566 100644 --- a/src/esp/physics/bullet/BulletPhysicsManager.cpp +++ b/src/esp/physics/bullet/BulletPhysicsManager.cpp @@ -172,6 +172,40 @@ void BulletPhysicsManager::stepPhysics(double dt) { dt = fixedTimeStep_; } + // set specified control velocities + for (auto& objectItr : existingObjects_) { + VelocityControl& velControl = objectItr.second->getVelocityControl(); + if (objectItr.second->getMotionType() == MotionType::KINEMATIC) { + // kinematic velocity control intergration + if (velControl.controllingAngVel || velControl.controllingLinVel) { + scene::SceneNode& objectSceneNode = objectItr.second->node(); + objectSceneNode.setTransformation(velControl.integrateTransform( + dt, objectSceneNode.transformation())); + objectItr.second->setActive(); + } + } else if (objectItr.second->getMotionType() == MotionType::DYNAMIC) { + if (velControl.controllingLinVel) { + if (velControl.linVelIsLocal) { + setLinearVelocity(objectItr.first, + objectItr.second->node().rotation().transformVector( + velControl.linVel)); + } else { + setLinearVelocity(objectItr.first, velControl.linVel); + } + } + if (velControl.controllingAngVel) { + if (velControl.angVelIsLocal) { + setAngularVelocity( + objectItr.first, + objectItr.second->node().rotation().transformVector( + velControl.angVel)); + } else { + setAngularVelocity(objectItr.first, velControl.angVel); + } + } + } + } + // ==== Physics stepforward ====== // NOTE: worldTime_ will always be a multiple of sceneMetaData_.timestep int numSubStepsTaken = diff --git a/src/tests/PhysicsTest.cpp b/src/tests/PhysicsTest.cpp index b0b7b88ec1..2309dd0166 100644 --- a/src/tests/PhysicsTest.cpp +++ b/src/tests/PhysicsTest.cpp @@ -390,6 +390,7 @@ TEST_F(PhysicsManagerTest, TestVelocityControl) { Magnum::Vector3 commandLinVel(1.0, 1.0, 1.0); Magnum::Vector3 commandAngVel(1.0, 1.0, 1.0); + // test results of getting/setting if (physicsManager_->getPhysicsSimulationLibrary() == PhysicsManager::PhysicsSimulationLibrary::BULLET) { physicsManager_->setLinearVelocity(objectId, commandLinVel); @@ -397,6 +398,7 @@ TEST_F(PhysicsManagerTest, TestVelocityControl) { ASSERT_EQ(physicsManager_->getLinearVelocity(objectId), commandLinVel); ASSERT_EQ(physicsManager_->getAngularVelocity(objectId), commandAngVel); + } else if (physicsManager_->getPhysicsSimulationLibrary() == PhysicsManager::PhysicsSimulationLibrary::NONE) { physicsManager_->setLinearVelocity(objectId, commandLinVel); @@ -406,4 +408,67 @@ TEST_F(PhysicsManagerTest, TestVelocityControl) { ASSERT_EQ(physicsManager_->getLinearVelocity(objectId), Magnum::Vector3{}); ASSERT_EQ(physicsManager_->getAngularVelocity(objectId), Magnum::Vector3{}); } + + // test constant velocity control mechanism + esp::physics::VelocityControl& velControl = + physicsManager_->getVelocityControl(objectId); + velControl.controllingAngVel = true; + velControl.controllingLinVel = true; + velControl.linVel = Magnum::Vector3{1.0, -1.0, 1.0}; + velControl.angVel = Magnum::Vector3{1.0, 0, 0}; + + // first kinematic + physicsManager_->setObjectMotionType(objectId, + esp::physics::MotionType::KINEMATIC); + physicsManager_->setTranslation(objectId, Magnum::Vector3{0, 2.0, 0}); + + float targetTime = 2.0; + while (physicsManager_->getWorldTime() < targetTime) { + physicsManager_->stepPhysics(targetTime - physicsManager_->getWorldTime()); + } + Magnum::Vector3 posGroundTruth{2.0, 0.0, 2.0}; + Magnum::Quaternion qGroundTruth{{0.842602, 0, 0}, 0.538537}; + + float errorEps = 0.01; // fairly loose due to discrete timestep + ASSERT_LE( + (physicsManager_->getTranslation(objectId) - posGroundTruth).length(), + errorEps); + Magnum::Rad angleError = + Magnum::Math::angle(physicsManager_->getRotation(objectId), qGroundTruth); + if (!std::isnan(float(angleError))) { // nan results close to equality + ASSERT_LE(float(angleError), errorEps); + } + + if (physicsManager_->getPhysicsSimulationLibrary() == + PhysicsManager::PhysicsSimulationLibrary::BULLET) { + physicsManager_->setObjectMotionType(objectId, + esp::physics::MotionType::DYNAMIC); + physicsManager_->resetTransformation(objectId); + physicsManager_->setTranslation(objectId, Magnum::Vector3{0, 2.0, 0}); + physicsManager_->setGravity({}); // 0 gravity interference + physicsManager_->reset(); // reset time to 0 + + // should closely follow kinematic result while uninhibited in 0 gravity + float targetTime = 0.5; + Magnum::Matrix4 kinematicResult = velControl.integrateTransform( + targetTime, physicsManager_->getTransformation(objectId)); + while (physicsManager_->getWorldTime() < targetTime) { + physicsManager_->stepPhysics(physicsManager_->getTimestep()); + } + ASSERT_LE((physicsManager_->getTranslation(objectId) - + kinematicResult.translation()) + .length(), + errorEps); + angleError = Magnum::Math::angle( + physicsManager_->getRotation(objectId), + Magnum::Quaternion::fromMatrix(kinematicResult.rotation())); + ASSERT_LE(float(angleError), errorEps); + + // should then get blocked by ground plane collision + targetTime = 2.0; + while (physicsManager_->getWorldTime() < targetTime) { + physicsManager_->stepPhysics(physicsManager_->getTimestep()); + } + ASSERT_GE(physicsManager_->getTranslation(objectId)[1], 1.0 - errorEps); + } }