public void Update(GameClock clock)
{
bool hasTarget = Target.HasValue;
if (hasTarget)
{
Matrix4 parentWorldTransform = Transform;
Bone parentBone;
if ((parentBone = Skeleton.Bones[Bone].Parent) != null)
{
parentWorldTransform = Skeleton.GetAbsoluteBoneTransform(parentBone.Index) * Transform;
}
Vector3 parentLocal = Vector3.Transform(Target.Value, Matrix4.Invert(parentWorldTransform));
// compute the rotation to the target position.
Vector3 desiredLookAt = Vector3.Normalize(parentLocal - new Vector3(Skeleton.Bones[Bone].Transform.Row3));
if (desiredLookAt == Vector3.Zero)
{
hasTarget = false;
desiredLookAt = Forward;
}
float horizontalAngle = (float)Math.Acos(Vector3.Dot(desiredLookAt, Forward));
float verticalAngle = (float)Math.Acos(Vector3.Dot(desiredLookAt, Up));
// clamp the rotation
bool clamped = false;
if (horizontalAngle > HorizontalRotationMax)
{
horizontalAngle = HorizontalRotationMax;
clamped = true;
}
else if (horizontalAngle < HorizontalRotationMin)
{
horizontalAngle = HorizontalRotationMin;
clamped = true;
}
if (verticalAngle > VerticalRotationMax)
{
verticalAngle = VerticalRotationMax;
clamped = true;
}
else if (verticalAngle < VerticalRotationMin)
{
verticalAngle = VerticalRotationMin;
clamped = true;
}
if (clamped)
{
desiredLookAt = Vector3.Transform(Vector3.One, Quaternion.FromAxisAngle(_up, verticalAngle) * Quaternion.FromAxisAngle(_forward, horizontalAngle));
}
float maxRotation = (float)(RotationSpeed * clock.ElapsedSeconds);
float currentRotation = (float)Math.Acos(Vector3.Dot(desiredLookAt, _currentLookAt));
if (currentRotation > maxRotation)
{
desiredLookAt = Vector3.Normalize(Vector3.Lerp(_currentLookAt, desiredLookAt, maxRotation / currentRotation));
}
_currentLookAt = desiredLookAt;
Quaternion parentRotation = QuaternionHelper.FromMatrix(parentWorldTransform);
// not sure of this final calculation
_desiredRotation = parentRotation * QuaternionHelper.RotationBetweenVectors(desiredLookAt, Up);
}
}