Rigidbody velocity get set

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

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

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

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 &&

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

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{});
+  }
+}