/// <summary>
/// Updates the position and attitude of a movement tracking object based on the frame
/// </summary>
/// <param name="track">Movement tracking object</param>
/// <param name="newPosition">New position of the object</param>
/// <param name="newAttitude">Previous position of the object</param>
public static void UpdateState(ref MovementTracking track, float3 newPosition, quaternion newAttitude)
{
// Update state
track.previousPosition = track.position;
track.previousAttitude = track.attitude;
track.attitude = newAttitude;
track.position = newPosition;
// Ensure initialized if not already
track.updates += 1;
}
/// <summary>
/// Gets the absolute displacement of a point relative to the
/// tracking object based on the current change in attitude and position
/// </summary>
/// <param name="tracking"></param>
/// <param name="point"></param>
/// <returns></returns>
public static float3 GetDisplacementAtPoint(MovementTracking track, float3 point)
{
// Get relative position to previous start
float3 relativePosition = point - track.previousPosition;
// Rotate point around center by change in attitude
float3 rotatedFinalPosition = math.mul(track.ChangeAttitude, relativePosition);
// Get the delta due to rotation
float3 deltaRotation = rotatedFinalPosition - relativePosition;
// Shift point by total displacement
return(deltaRotation + track.Displacement);
}
