diff --git a/libraries/physics/src/BulletUtil.h b/libraries/physics/src/BulletUtil.h index 79761a5462..0122e86288 100644 --- a/libraries/physics/src/BulletUtil.h +++ b/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 diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 81132d9ece..72d8220d4f 100644 --- a/libraries/physics/src/CharacterController.cpp +++ b/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(); diff --git a/tests/physics/src/BulletUtilTests.cpp b/tests/physics/src/BulletUtilTests.cpp index 5496c6ea2b..b81742df65 100644 --- a/tests/physics/src/BulletUtilTests.cpp +++ b/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); +} diff --git a/tests/physics/src/BulletUtilTests.h b/tests/physics/src/BulletUtilTests.h index e8fee1e473..3f19484a60 100644 --- a/tests/physics/src/BulletUtilTests.h +++ b/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