diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index 0bbfa41a12..7f63fd9f5e 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -262,29 +262,54 @@ void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling. btVector3 endPos = startPos + linearDisplacement; + // resolve the simple linearDisplacement + _followLinearDisplacement += linearDisplacement; + + // now for the rotational part... btQuaternion startRot = bodyTransform.getRotation(); btQuaternion desiredRot = _followDesiredBodyTransform.getRotation(); - if (desiredRot.dot(startRot) < 0.0f) { - desiredRot = -desiredRot; - } - btQuaternion deltaRot = desiredRot * startRot.inverse(); - float angularSpeed = deltaRot.getAngle() / _followTimeRemaining; - glm::vec3 rotationAxis = glm::normalize(glm::axis(bulletToGLM(deltaRot))); // deltaRot.getAxis() is inaccurate - btQuaternion angularDisplacement = btQuaternion(glmToBullet(rotationAxis), angularSpeed * dt); - btQuaternion endRot = angularDisplacement * startRot; - - // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. - btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); - btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); - - if (!isNaN(bulletToGLM(endPos)) && !isNaN(bulletToGLM(endRot))) { - _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement; - _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; - - _rigidBody->setWorldTransform(btTransform(endRot, endPos)); - } else { - qCWarning(physics) << "CharacterController::playerStep produced NaN."; + + // startRot as default rotation + btQuaternion endRot = startRot; + + // the dot product between two quaternions is equal to +/- cos(angle/2) + // where 'angle' is that of the rotation between them + float qDot = desiredRot.dot(startRot); + + // when the abs() value of the dot product is approximately 1.0 + // then the two rotations are effectively adjacent + const float MIN_DOT_PRODUCT_OF_ADJACENT_QUATERNIONS = 0.99999f; // corresponds to approx 0.5 degrees + if (fabsf(qDot) < MIN_DOT_PRODUCT_OF_ADJACENT_QUATERNIONS) { + if (qDot < 0.0f) { + // the quaternions are actually on opposite hyperhemispheres + // so we move one to agree with the other and negate qDot + desiredRot = -desiredRot; + qDot = -qDot; + } + btQuaternion deltaRot = desiredRot * startRot.inverse(); + + // the axis is the imaginary part, but scaled by sin(angle/2) + btVector3 axis(deltaRot.getX(), deltaRot.getY(), deltaRot.getZ()); + axis /= sqrtf(1.0f - qDot*qDot); + + // compute the angle we will resolve for this dt, but don't overshoot + float angle = (2.0f * acosf(qDot)); + if ( dt < _followTimeRemaining) { + angle *= dt / _followTimeRemaining; + } + + // accumulate rotation + deltaRot = btQuaternion(axis, angle); + _followAngularDisplacement = (deltaRot * _followAngularDisplacement).normalize(); + + // in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account. + btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset); + + endRot = deltaRot * startRot; + btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset); + _followLinearDisplacement += swingDisplacement; } + _rigidBody->setWorldTransform(btTransform(endRot, endPos)); } _followTime += dt;