public static void WriteTestOutputVertex(string nameDisplayed, Matrix4d m, PointCloudVertices mypointsSource, PointCloudVertices myPointsTransformed, PointCloudVertices myPointsTarget) { m.Print(nameDisplayed); long resultsWritten = mypointsSource.Count; if (resultsWritten > 5) { resultsWritten = 5; } System.Diagnostics.Debug.WriteLine("Points:"); double meanDistance = 0; for (int i = 0; i < resultsWritten; i++) { Vector3d pToBeMatched = mypointsSource[i].Vector; Vector3d pTransformed = myPointsTransformed[i].Vector; Vector3d pReference = myPointsTarget[i].Vector; string p1 = pToBeMatched[0].ToString("0.0") + " " + pToBeMatched[1].ToString("0.0") + " " + pToBeMatched[2].ToString("0.0"); string p2 = pTransformed[0].ToString("0.0") + " " + pTransformed[1].ToString("0.0") + " " + pTransformed[2].ToString("0.0"); string p3 = pReference[0].ToString("0.0") + " " + pReference[1].ToString("0.0") + " " + pReference[2].ToString("0.0"); double distance = MathBase.DistanceBetweenVectors(pTransformed, pReference); meanDistance += distance; Debug.WriteLine(i.ToString() + " : " + p1 + " :transformed: " + p2 + " :target: " + p3 + " : Distance: " + distance.ToString("0.0")); } //Debug.WriteLine("--Mean Distance: " + (meanDistance / resultsWritten).ToString("0.0")); }