diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp index aaf2c0a46b..cee0e6a1fa 100755 --- a/libraries/physics/src/CharacterController.cpp +++ b/libraries/physics/src/CharacterController.cpp @@ -262,24 +262,53 @@ 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; + + // 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; } - 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); - - _followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement; - _followAngularDisplacement = angularDisplacement * _followAngularDisplacement; - _rigidBody->setWorldTransform(btTransform(endRot, endPos)); } _followTime += dt;