RigidObject velocity control

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);

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_.

src/esp/physics/RigidObject.cpp

@@ -213,5 +213,39 @@ 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::Quaternion q;
+    Magnum::Vector3 ha = angVel * dt * 0.5;  // vector of half angle
+    float l = ha.length();                   // magnitude
+    if (l > 0) {
+      float ss = std::sin(l) / l;
+      q = Magnum::Quaternion(
+          Magnum::Vector3{ha.x() * ss, ha.y() * ss, ha.z() * ss}, std::cos(l));
+    } else {
+      q = Magnum::Quaternion(Magnum::Vector3{ha.x(), ha.y(), ha.z()}, 1.0);
+    }
+    newRotationScaling = q.toMatrix() * newRotationScaling;
+  }
+  return Magnum::Matrix4::from(newRotationScaling, newTranslation);
+}
+
 }  // namespace physics
 }  // namespace esp

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, class @ref
+ * VelocityControl
  */
 
 #include <Corrade/Containers/Optional.h>
@@ -80,6 +81,41 @@ enum class RigidObjectType {
 
 };
 
+/**@brief Convenience class for applying constant velocity control to a rigid
+ * body. */
+class 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 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 +313,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 +548,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_;

src/esp/physics/bullet/BulletPhysicsManager.cpp

@@ -172,6 +172,33 @@ 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) {
+        setAngularVelocity(objectItr.first, velControl.angVel);
+      }
+    }
+  }
+
   // ==== Physics stepforward ======
   // NOTE: worldTime_ will always be a multiple of sceneMetaData_.timestep
   int numSubStepsTaken =

src/tests/PhysicsTest.cpp

@@ -390,13 +390,15 @@ 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);
     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);
@@ -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);
+  }
 }