/// <summary>
/// Calculates rigid transformations between all permutations of pSetA and pSetB
/// and returns the best one (having the smallest offset).
/// This is done due to the expected indexing in function CalculateRigidTransform
/// </summary>
public static CRigidTransform GetRigidTransform(List <Vector3> pSetA, List <Vector3> pSetB)
{
CDebug.WriteLine($"GetRigidTransform from set : {CDebug.GetString(pSetA)} to {CDebug.GetString(pSetB)}");
if (pSetA.Count != pSetB.Count)
{
CDebug.Error("Sets copunt dont match");
return(null);
}
IEnumerable <IEnumerable <Vector3> > setApermutations = pSetA.Permute();
List <CRigidTransform> rigTransforms = new List <CRigidTransform>();
foreach (var permutation in setApermutations)
{
CRigidTransform rigTransform = CalculateRigidTransform(permutation.ToList(), pSetB);
rigTransforms.Add(rigTransform);
//CDebug.WriteLine($"{rigTransform}");
if (rigTransform.offset < MAX_OFFSET)
{
break;
}
}
CRigidTransform minOffsetRigTransform = rigTransforms.Aggregate(
(curMin, x) => x.offset < curMin.offset ? x : curMin);
CDebug.WriteLine($"Selected {minOffsetRigTransform}", true, true);
return(minOffsetRigTransform);
}