public void Test(Random random, int innerIterations)
{
GenerateRandomNormalizedVector(random, out var v1);
GenerateRandomNormalizedVector(random, out var v2);
for (int i = 0; i < innerIterations; ++i)
{
Quaternion.GetQuaternionBetweenNormalizedVectors(v1, v2, out var v1ToV2);
Quaternion.GetQuaternionBetweenNormalizedVectors(v2, v1, out var v2ToV1);
#if DEBUG
Quaternion.ConcatenateWithoutOverlap(v1ToV2, v2ToV1, out var concatenated);
Quaternion.Transform(v1, v1ToV2, out var v1TransformedToV2);
Quaternion.Transform(v2, v2ToV1, out var v2TransformedToV1);
Quaternion.Transform(v1, concatenated, out var v1TransformedToV1);
var v1ToV2ErrorLength = (v1TransformedToV2 - v2).LengthSquared();
var v2ToV1ErrorLength = (v2TransformedToV1 - v1).LengthSquared();
var v1ToV1ErrorLength = (v1TransformedToV1 - v1).LengthSquared();
const float epsilon = 1e-6f;
Debug.Assert(
v1ToV2ErrorLength < epsilon &&
v2ToV1ErrorLength < epsilon &&
v1ToV1ErrorLength < epsilon);
#endif
}
}
///<summary>
/// Concatenates a rigid transform with another rigid transform. Assumes input and output do not overlap.
///</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 MultiplyWithoutOverlap(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.ConcatenateWithoutOverlap(ref a.Orientation, ref b.Orientation, out combined.Orientation);
}