///<summary>
/// Concatenates a rigid transform with another rigid transform.
///</summary>
///<param name="a">The first rigid transform.</param>
///<param name="b">The second rigid transform.</param>
///<param name="combined">Concatenated rigid transform.</param>
public static void Multiply(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined)
{
Vector3 intermediate;
Quaternion.Transform(ref a.Position, ref b.Orientation, out intermediate);
combined.Position = intermediate + b.Position;
Quaternion.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation);
}
/// <summary>
/// Inverts a rigid transform.
/// </summary>
/// <param name="transform">Transform to invert.</param>
/// <param name="inverse">Inverse of the transform.</param>
public static void Invert(ref RigidTransform transform, out RigidTransform inverse)
{
Quaternion.Conjugate(ref transform.Orientation, out inverse.Orientation);
Quaternion.Transform(ref transform.Position, ref inverse.Orientation, out inverse.Position);
inverse.Position = -inverse.Position;
}
///<summary>
/// Transforms a position by a rigid transform.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to apply.</param>
///<param name="result">Transformed position.</param>
public static void Transform(ref Vector3 position, ref RigidTransform transform, out Vector3 result)
{
Vector3 intermediate;
Quaternion.Transform(ref position, ref transform.Orientation, out intermediate);
result = intermediate + transform.Position;
}
///<summary>
/// Transforms a position by a rigid transform's inverse.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to invert and apply.</param>
///<param name="result">Transformed position.</param>
public static void TransformByInverse(ref Vector3 position, ref RigidTransform transform, out Vector3 result)
{
Quaternion orientation;
Vector3 intermediate = position - transform.Position;
Quaternion.Conjugate(ref transform.Orientation, out orientation);
Quaternion.Transform(ref intermediate, ref orientation, out result);
}
///<summary>
/// Concatenates a rigid transform with another rigid transform's inverse.
///</summary>
///<param name="a">The first rigid transform.</param>
///<param name="b">The second rigid transform whose inverse will be concatenated to the first.</param>
///<param name="combinedTransform">Combined rigid transform.</param>
public static void MultiplyByInverse(ref RigidTransform a, ref RigidTransform b, out RigidTransform combinedTransform)
{
Invert(ref b, out combinedTransform);
Multiply(ref a, ref combinedTransform, out combinedTransform);
}
