/// <summary>
/// Applies max velocity limit to the given bone transform.
/// </summary>
/// <param name="originalTransform">
/// In: The original bone transform.
/// </param>
/// <param name="targetTransform">
/// In: The target bone transform.<br/>
/// Out: The limited bone transform.
/// </param>
/// <param name="maxRotationAngle">The max rotation angle.</param>
internal void LimitBoneTransform(ref SrtTransform originalTransform, ref SrtTransform targetTransform, float maxRotationAngle)
{
if (maxRotationAngle < ConstantsF.Pi)
{
// Compute relative rotation.
var rotationChange = targetTransform.Rotation * originalTransform.Rotation.Conjugated;
// Make sure we rotate around the shortest arc.
if (QuaternionF.Dot(originalTransform.Rotation, targetTransform.Rotation) < 0)
{
rotationChange = -rotationChange;
}
if (rotationChange.Angle > maxRotationAngle && !rotationChange.V.IsNumericallyZero)
{
// ReSharper disable EmptyGeneralCatchClause
try
{
// Limit rotation.
rotationChange.Angle = maxRotationAngle;
targetTransform.Rotation = rotationChange * originalTransform.Rotation;
}
catch
{
// rotationChange.Angle = xxx. Can cause DivideByZeroException or similar.
// The !rotationChange.V.IsNumericallyZero should avoid this. But just to go sure.
}
// ReSharper restore EmptyGeneralCatchClause
}
}
}
/// <overloads>
/// <summary>
/// Computes the angular velocity that rotates an object from the current orientation to a
/// target orientation.
/// </summary>
/// </overloads>
///
/// <summary>
/// Computes the angular velocity that rotates an object from the current orientation to a
/// target orientation.
/// </summary>
/// <param name="currentOrientation">The current orientation.</param>
/// <param name="targetOrientation">The target orientation.</param>
/// <param name="deltaTime">The time over which the rotation takes place (in seconds).</param>
/// <returns>
/// The angular velocity vector. If an object is rotated with this velocity starting at
/// <paramref name="currentOrientation"/>, it will arrive at <paramref name="targetOrientation"/>
/// after <paramref name="deltaTime"/> seconds.
/// </returns>
public static Vector3F ComputeAngularVelocity(QuaternionF currentOrientation, QuaternionF targetOrientation, float deltaTime)
{
if (Numeric.IsZero(deltaTime))
{
return(Vector3F.Zero);
}
// ----- Angular Velocity
QuaternionF orientationDelta = targetOrientation * currentOrientation.Conjugated;
// Make sure we move along the shortest arc.
if (QuaternionF.Dot(currentOrientation, targetOrientation) < 0)
{
orientationDelta = -orientationDelta;
}
// Determine the angular velocity that rotates the body.
Vector3F rotationAxis = orientationDelta.Axis;
if (!rotationAxis.IsNumericallyZero)
{
// The angular velocity is computed as rotationAxis * rotationSpeed.
float rotationSpeed = (orientationDelta.Angle / deltaTime);
return(rotationAxis * rotationSpeed);
}
// The axis of rotation is 0. That means the no rotation should be applied.
return(Vector3F.Zero);
}
public void DotProduct()
{
QuaternionF q1 = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f);
QuaternionF q2 = new QuaternionF(5.0f, 6.0f, 7.0f, 8.0f);
float dotProduct = QuaternionF.Dot(q1, q2);
Assert.AreEqual(70, dotProduct);
}
public void BlendTest()
{
var traits = SrtTransformTraits.Instance;
var value0 = NextRandomValue();
var value1 = NextRandomValue();
var value2 = NextRandomValue();
var w0 = 0.3f;
var w1 = 0.4f;
var w2 = 1 - w0 - w1;
SrtTransform result = new SrtTransform();
traits.BeginBlend(ref result);
traits.BlendNext(ref result, ref value0, w0);
traits.BlendNext(ref result, ref value1, w1);
traits.BlendNext(ref result, ref value2, w2);
traits.EndBlend(ref result);
Assert.IsTrue(Vector3F.AreNumericallyEqual(value0.Scale * w0 + value1.Scale * w1 + value2.Scale * w2, result.Scale));
Assert.IsTrue(Vector3F.AreNumericallyEqual(value0.Translation * w0 + value1.Translation * w1 + value2.Translation * w2, result.Translation));
QuaternionF expected;
expected = value0.Rotation * w0;
// Consider "selective negation" when blending quaternions!
if (QuaternionF.Dot(expected, value1.Rotation) < 0)
{
value1.Rotation = -value1.Rotation;
}
expected += value1.Rotation * w1;
if (QuaternionF.Dot(expected, value2.Rotation) < 0)
{
value2.Rotation = -value2.Rotation;
}
expected += value2.Rotation * w2;
expected.Normalize();
Assert.IsTrue(QuaternionF.AreNumericallyEqual(expected, result.Rotation));
}
public override void Update(GameTime gameTime)
{
float deltaTime = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Move an object with constant speed along the path.
const float speed = 5;
float newPathPosition = _currentPathPosition + deltaTime * speed;
// Get path parameter where the path length is equal to newPathPosition.
float parameter = _path.GetParameterFromLength(newPathPosition, 10, 0.01f);
// Get path point at the newPathPosition.
Vector3F position = _path.GetPoint(parameter);
// Get the path tangent at newPathPosition and use it as the forward direction.
Vector3F forward = _path.GetTangent(parameter).Normalized;
QuaternionF currentOrientation = QuaternionF.CreateRotation(_kinematicBody.Pose.Orientation);
QuaternionF targetOrientation = QuaternionF.CreateRotation(Vector3F.UnitY, forward);
QuaternionF orientationDelta = targetOrientation * currentOrientation.Conjugated;
// Selective Negation:
// A certain rotation can be described by two quaternions: q and -q. For example, if you look
// to the north and want to look to the east you can either rotate 90 degrees clockwise or
// 270 degrees counter-clockwise. In the game we always want to rotate using the
// smaller angle. This is done using "Selective Negation": The dot product of two quaternions
// is proportional to the cosine of the rotation angle. If the cosine of the angle is < 0,
// the angle is larger than +/- 90 degrees. In this case we must use -orientationDelta to
// rotate using the smaller angle.
if (QuaternionF.Dot(currentOrientation, targetOrientation) < 0)
{
orientationDelta = -orientationDelta;
}
// We could directly set the new position of the kinematic body. However, directly
// setting a position is like "teleporting" an object. The body would not interact
// properly with other objects in the physics simulation.
// Instead we apply a linear and angular velocity to the body. The simulation will
// automatically update the position of the body. If the body touches other objects
// along the way it will push these objects with the appropriate force.
// Note that the physics simulation may advance with a different time step than the
// rest of the game.
deltaTime = Math.Max(deltaTime, Simulation.Settings.Timing.FixedTimeStep);
// Determine the linear velocity that moves the body forward.
Vector3F linearVelocity = (position - _kinematicBody.Pose.Position) / deltaTime;
// Determine the angular velocity that rotates the body.
Vector3F angularVelocity;
Vector3F rotationAxis = orientationDelta.Axis;
if (!rotationAxis.IsNumericallyZero)
{
// The angular velocity is computed as rotationAxis * rotationSpeed.
// The rotation speed is computed as angle / time. (Note: The angle is given in radians.)
float rotationSpeed = (orientationDelta.Angle / deltaTime);
angularVelocity = rotationAxis * rotationSpeed;
}
else
{
// The axis of rotation is 0. That means the no rotation should be applied.
angularVelocity = Vector3F.Zero;
}
_kinematicBody.LinearVelocity = linearVelocity;
_kinematicBody.AngularVelocity = angularVelocity;
_currentPathPosition = newPathPosition;
// Let the base class render the rigid bodies.
base.Update(gameTime);
// Draw the 3D path.
var debugRenderer = GraphicsScreen.DebugRenderer;
for (int i = 0; i < _pointList.Length; i++)
{
debugRenderer.DrawLine(_pointList[i], _pointList[(i + 1) % _pointList.Length], Color.Black, false);
}
}
