velocity-constraints.patch

From d2ef1e5505a7b3e9b3ad393df344b3cda5e2ad09 Mon Sep 17 00:00:00 2001
From: Julian <cahill.julian@gmail.com>
Date: Tue, 18 Dec 2018 03:34:32 +1300
Subject: [PATCH] added locking of linear and angular velocities with
 RigidBody::setLinearVelocityFactor(), RigidBody::setAngularVelocityFactor()

---
 src/body/RigidBody.cpp       | 45 +++++++++++++++++++++++++++++++++++++++++++-
 src/body/RigidBody.h         | 42 +++++++++++++++++++++++++++++++++++++----
 src/engine/DynamicsWorld.cpp | 14 +++++++++++++-
 3 files changed, 95 insertions(+), 6 deletions(-)

diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index 27fd75d..4913f2a 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -43,7 +43,8 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde
           : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)),
             mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
             mIsGravityEnabled(true), mMaterial(world.mConfig), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
-            mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
+            mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false),
+			mAngularVelocityFactor(decimal(1.0), decimal(1.0), decimal(1.0)), mLinearVelocityFactor(decimal(1.0), decimal(1.0), decimal(1.0)) {
 
     // Compute the inverse mass
     mMassInverse = decimal(1.0) / mInitMass;
@@ -405,6 +406,27 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
              "Body " + std::to_string(mID) + ": Set linearVelocity=" + mLinearVelocity.to_string());
 }
 
+// Set the linear velocity factors of the rigid body.
+/**
+ * @param linearVelocityFactor Linear velocity factors of the body
+ */
+void RigidBody::setLinearVelocityFactor(const Vector3& linearVelocityFactor) {
+
+    // If it is a static body, we do nothing
+    if (mType == BodyType::STATIC) return;
+
+    // Update the linear velocity of the current body state
+    mLinearVelocityFactor = linearVelocityFactor;
+
+    // If the linear velocity is not zero, awake the body
+    if (mLinearVelocity.lengthSquare() > decimal(0.0)) {
+        setIsSleeping(false);
+    }
+
+    RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
+             "Body " + std::to_string(mID) + ": Set linearVelocityFactor=" + mLinearVelocity.to_string());
+}
+
 // Set the angular velocity.
 /**
 * @param angularVelocity The angular velocity vector of the body
@@ -426,6 +448,27 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
              "Body " + std::to_string(mID) + ": Set angularVelocity=" + mAngularVelocity.to_string());
 }
 
+// Set the angular velocity factors.
+/**
+* @param angularVelocity The angular velocity factors of the body
+*/
+void RigidBody::setAngularVelocityFactor(const Vector3& angularVelocityFactor) {
+
+    // If it is a static body, we do nothing
+    if (mType == BodyType::STATIC) return;
+
+    // Set the angular velocity
+    mAngularVelocityFactor = angularVelocityFactor;
+
+    // If the velocity is not zero, awake the body
+    if (mAngularVelocity.lengthSquare() > decimal(0.0)) {
+        setIsSleeping(false);
+    }
+
+    RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
+             "Body " + std::to_string(mID) + ": Set angularVelocityFactor=" + mAngularVelocity.to_string());
+}
+
 // Set the current position and orientation
 /**
  * @param transform The transformation of the body that transforms the local-space
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index e9e78ee..272e64e 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -115,6 +115,12 @@ class RigidBody : public CollisionBody {
         /// True if the inertia tensor is set by the user
         bool mIsInertiaTensorSetByUser;
 
+		/// Angular velocity factor
+		Vector3 mAngularVelocityFactor;
+
+		/// Linear velocity factor
+		Vector3 mLinearVelocityFactor;
+
         // -------------------- Methods -------------------- //
 
         /// Remove a joint from the joints list
@@ -154,18 +160,30 @@ class RigidBody : public CollisionBody {
         /// Return the mass of the body
         decimal getMass() const;
 
-        /// Return the linear velocity
+		/// Return the linear velocity
         Vector3 getLinearVelocity() const;
 
-        /// Set the linear velocity of the body.
+		/// Return the linear velocity factor
+        Vector3 getLinearVelocityFactor() const;
+
+		/// Set the linear velocity of the body.
         void setLinearVelocity(const Vector3& linearVelocity);
 
-        /// Return the angular velocity
+		/// Set the linear velocity factor of the body.
+        void setLinearVelocityFactor(const Vector3& linearVelocityFactor);
+
+		/// Return the angular velocity
         Vector3 getAngularVelocity() const;
 
-        /// Set the angular velocity.
+		/// Return the angular velocity factor
+        Vector3 getAngularVelocityFactor() const;
+
+		/// Set the angular velocity.
         void setAngularVelocity(const Vector3& angularVelocity);
 
+		/// Set the angular velocity factor.
+        void setAngularVelocityFactor(const Vector3& angularVelocityFactor);
+
         /// Set the variable to know whether or not the body is sleeping
         virtual void setIsSleeping(bool isSleeping) override;
 
@@ -271,6 +289,14 @@ inline Vector3 RigidBody::getLinearVelocity() const {
     return mLinearVelocity;
 }
 
+// Return the linear velocity factor
+/**
+ * @return The linear velocity factors of the body
+ */
+inline Vector3 RigidBody::getLinearVelocityFactor() const {
+    return mLinearVelocityFactor;
+}
+
 // Return the angular velocity of the body
 /**
  * @return The angular velocity vector of the body
@@ -279,6 +305,14 @@ inline Vector3 RigidBody::getAngularVelocity() const {
     return mAngularVelocity;
 }
 
+// Return the angular velocity factor of the body
+/**
+ * @return The angular velocity factors of the body
+ */
+inline Vector3 RigidBody::getAngularVelocityFactor() const {
+    return mAngularVelocityFactor;
+}
+
 // Get the inverse local inertia tensor of the body (in body coordinates)
 inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
     return mInertiaTensorLocalInverse;
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 4ed879d..89a494f 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -161,7 +161,7 @@ void DynamicsWorld::update(decimal timeStep) {
 void DynamicsWorld::integrateRigidBodiesPositions() {
 
     RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler);
-    
+
     // For each island of the world
     for (uint i=0; i < mNbIslands; i++) {
 
@@ -183,6 +183,12 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
                 newAngVelocity += mSplitAngularVelocities[indexArray];
             }
 
+			// Multiply by velocity factors
+			for(uint j=0; j<3; j++){
+				newLinVelocity[j] *= bodies[b]->getLinearVelocityFactor()[j];
+				newAngVelocity[j] *= bodies[b]->getAngularVelocityFactor()[j];
+			}
+
             // Get current position and orientation of the body
             const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld;
             const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
@@ -332,6 +338,12 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
             mConstrainedLinearVelocities[indexBody] *= linearDamping;
             mConstrainedAngularVelocities[indexBody] *= angularDamping;
 
+			// Multiply by velocity factors
+			for(uint j=0; j<3; j++){
+				mConstrainedLinearVelocities[indexBody][j] *= bodies[b]->getLinearVelocityFactor()[j];
+				mConstrainedAngularVelocities[indexBody][j] *= bodies[b]->getAngularVelocityFactor()[j];
+			}
+
             indexBody++;
         }
     }
-- 
2.7.4