/// <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); }