CharacterController: added hard limit to follow displacement

To prevent tunneling when the rigid body cannot reach it's target due to being blocked by a wall or some other obstacle.

Also, added unit tests for physics/BulletUtil rotateVector and clampLength functions.

libraries/physics/src/BulletUtil.h

@@ -74,4 +74,12 @@ inline btVector3 rotateVector(const btQuaternion& q, const btVector3& vec) {
     return glmToBullet(bulletToGLM(q) * bulletToGLM(vec));
 }
 
+inline btVector3 clampLength(const btVector3& v, const float& maxLength) {
+    if (v.length() > maxLength) {
+        return v.normalized() * maxLength;
+    } else {
+        return v;
+    }
+}
+
 #endif // hifi_BulletUtil_h

libraries/physics/src/CharacterController.cpp

@@ -194,6 +194,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
     // Rather then add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal.
     // This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate().
     // These two computations must be kept in sync.
+    const float MAX_DISPLACEMENT = 0.5f * _radius;
     _followTimeRemaining -= dt;
     if (_followTimeRemaining >= 0.005f) {
         btTransform bodyTransform = _rigidBody->getWorldTransform();
@@ -201,7 +202,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
         btVector3 startPos = bodyTransform.getOrigin();
         btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
         btVector3 vel = deltaPos / _followTimeRemaining;
-        btVector3 linearDisplacement = vel * dt;
+        btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT);  // clamp displacement to prevent tunneling.
         btVector3 endPos = startPos + linearDisplacement;
 
         btQuaternion startRot = bodyTransform.getRotation();

tests/physics/src/BulletUtilTests.cpp

@@ -127,3 +127,37 @@ void BulletUtilTests::fromGLMToBullet() {
     QCOMPARE(gV.y, bV.getY());
     QCOMPARE(gV.z, bV.getZ());
 }
+
+void BulletUtilTests::rotateVectorTest() {
+
+    float angle = 0.317f * PI;
+    btVector3 axis(1.23f, 2.34f, 3.45f);
+    axis.normalize();
+    btQuaternion q(axis, angle);
+
+    btVector3 xAxis(1.0f, 0.0f, 0.0f);
+
+    btVector3 result0 = rotateVector(q, xAxis);
+
+    btTransform m(q);
+    btVector3 result1 = m * xAxis;
+
+    QCOMPARE(result0.getX(), result0.getX());
+    QCOMPARE(result0.getY(), result1.getY());
+    QCOMPARE(result0.getZ(), result1.getZ());
+}
+
+void BulletUtilTests::clampLengthTest() {
+    btVector3 vec(1.0f, 3.0f, 2.0f);
+    btVector3 clampedVec1 = clampLength(vec, 1.0f);
+    btVector3 clampedVec2 = clampLength(vec, 2.0f);
+    btVector3 normalizedVec = vec.normalized();
+
+    QCOMPARE(clampedVec1.getX(), normalizedVec.getX());
+    QCOMPARE(clampedVec1.getY(), normalizedVec.getY());
+    QCOMPARE(clampedVec1.getZ(), normalizedVec.getZ());
+
+    QCOMPARE(clampedVec2.getX(), normalizedVec.getX() * 2.0f);
+    QCOMPARE(clampedVec2.getY(), normalizedVec.getY() * 2.0f);
+    QCOMPARE(clampedVec2.getZ(), normalizedVec.getZ() * 2.0f);
+}

tests/physics/src/BulletUtilTests.h

@@ -20,6 +20,8 @@ class BulletUtilTests : public QObject {
 private slots:
     void fromBulletToGLM();
     void fromGLMToBullet();
+    void rotateVectorTest();
+    void clampLengthTest();
 };
 
 #endif // hifi_BulletUtilTests_h