/// <summary> /// Compute the line that best fit a set of input points (least squared orthogonal distance) /// </summary> /// <param name="points">The input points</param> /// <param name="lineOrigin">Origin of the best fit line</param> /// <param name="lineDirection">Direction of the best fit line</param> /// <param name="tolerance">The tolerance that is used the determined if the input points are coincidental, collinear, or non-collinear</param> /// <returns>0 if the input points are coincidental; 1 if they are collinear; 2 otherwise</returns> public static int ComputeBestFitLine(List <Triple> points, out Triple lineOrigin, out Triple lineDirection, float tolerance = 1E-10f) { Triple centroid = Triple.Zero; for (int i = 0; i < points.Count; i++) { centroid += points[i]; } centroid /= points.Count; float[,] P = new float[points.Count, 3]; for (int i = 0; i < points.Count; i++) { P[i, 0] = points[i].X - centroid.X; P[i, 1] = points[i].Y - centroid.Y; P[i, 2] = points[i].Z - centroid.Z; } float c00 = 0f, c01 = 0f, c02 = 0f; float c10 = 0f, c11 = 0f, c12 = 0f; float c20 = 0f, c21 = 0f, c22 = 0f; for (int k = 0; k < points.Count; k++) { c00 += P[k, 0] * P[k, 0]; c01 += P[k, 0] * P[k, 1]; c02 += P[k, 0] * P[k, 2]; c10 += P[k, 1] * P[k, 0]; c11 += P[k, 1] * P[k, 1]; c12 += P[k, 1] * P[k, 2]; c20 += P[k, 2] * P[k, 0]; c21 += P[k, 2] * P[k, 1]; c22 += P[k, 2] * P[k, 2]; } FastSvd3x3.Compute( c00, c01, c02, c10, c11, c12, c20, c21, c22, out var u00, out _, out _, out var u10, out _, out _, out var u20, out _, out _, out var s00, out var s11, out _, out _, out _, out _, out _, out _, out _, out _, out _, out _); lineOrigin = centroid; // Case 0: The input points are coincidental, so we just need to pick an arbitrary line direction if (s00 < tolerance) { lineDirection = Triple.BasisX; return(0); } // Case 1: The input points are not coincidental, therefore we pick the dominant eigen vector as the line direction lineDirection = new Triple(u00, u10, u20); return(s11 < tolerance ? 1 // The input points are collinear : 2); // The input points are NOT collinear }
/// <summary> /// Compute the sphere that best fit a set of input points (least squared orthogonal distance) /// </summary> /// <param name="points">The input points</param> /// <param name="sphereCenter">Center of the best fit sphere</param> /// <param name="sphereRadius">Radius of the best fit sphere</param> /// <param name="tolerance">The tolerance that is used the determined if the input points are coincidental, collinear, coplanar, or non-coplanar</param> /// <returns>False if the input points are coincidental, collinear or coplanar; True otherwise</returns> public static bool ComputeBestFitSphere(List <Triple> points, out Triple sphereCenter, out float sphereRadius, float tolerance = 1E-10f) { // Reference: Sumith YD: Fast Geometric Fit Algorithm for Sphere Using Exact Solution float sX = 0f; float sY = 0f; float sZ = 0f; float sXX = 0f; float sYY = 0f; float sZZ = 0f; float sXXX = 0f; float sYYY = 0f; float sZZZ = 0f; float sXY = 0f; float sXZ = 0f; float sYZ = 0f; float sXYY = 0f; float sXZZ = 0f; float sXXY = 0f; float sYZZ = 0f; float sXXZ = 0f; float sYYZ = 0f; for (int i = 0; i < points.Count; i++) { Triple p = points[i]; sX += p.X; sY += p.Y; sZ += p.Z; sXX += p.X * p.X; sYY += p.Y * p.Y; sZZ += p.Z * p.Z; sXXX += p.X * p.X * p.X; sYYY += p.Y * p.Y * p.Y; sZZZ += p.Z * p.Z * p.Z; sXY += p.X * p.Y; sXZ += p.X * p.Z; sYZ += p.Y * p.Z; sXYY += p.X * p.Y * p.Y; sXZZ += p.X * p.Z * p.Z; sXXY += p.X * p.X * p.Y; sYZZ += p.Y * p.Z * p.Z; sXXZ += p.X * p.X * p.Z; sYYZ += p.Y * p.Y * p.Z; } float N = points.Count; float temp = sXX + sYY + sZZ; float a = 2f * sX * sX - 2f * N * sXX; float b = 2f * sX * sY - 2f * N * sXY; float c = 2f * sX * sZ - 2f * N * sXZ; float d = sX * temp - N * (sXXX + sXYY + sXZZ); float e = b; float f = 2f * sY * sY - 2f * N * sYY; float g = 2f * sY * sZ - 2f * N * sYZ; float h = sY * temp - N * (sXXY + sYYY + sYZZ); float j = c; float k = g; float l = 2f * sZ * sZ - 2f * N * sZZ; float m = sZ * temp - N * (sXXZ + sYYZ + sZZZ); float delta = a * (f * l - g * k) - e * (b * l - c * k) + j * (b * g - c * f); if (Math.Abs(delta) < tolerance) { sphereCenter = new Triple(float.NaN); sphereRadius = float.NaN; return(false); } delta = 1f / delta; float x0 = delta * (d * (f * l - g * k) - h * (b * l - c * k) + m * (b * g - c * f)); float y0 = delta * (a * (h * l - m * g) - e * (d * l - m * c) + j * (d * g - h * c)); float z0 = delta * (a * (f * m - h * k) - e * (b * m - d * k) + j * (b * h - d * f)); sphereCenter = new Triple(x0, y0, z0); sphereRadius = (float)Math.Sqrt( x0 * x0 + y0 * y0 + z0 * z0 + (temp - 2f * (x0 * sX + y0 * sY + z0 * sZ)) / N); return(true); }
public static Vector3 ToVector3(this Triple triple) => new Vector3(triple.X, triple.Z, -triple.Y);
/// <summary> /// Compute the plane that best fit a set of input points (least squared orthogonal distance) /// </summary> /// <param name="points">The input points</param> /// <param name="planeOrigin">Origin of the best fit plane</param> /// <param name="planeNormal">Normal of the best fit plane</param> /// <param name="tolerance">The tolerance that is used the determined if the input points are coincidental, collinear, coplanar, or non-coplanar</param> /// <returns>0 if the input points are coincidental; 1 if they are collinear; 2 if they are coplanar; 3 otherwise</returns> public static int ComputeBestFitPlane(List <Triple> points, out Triple planeOrigin, out Triple planeNormal, float tolerance = 1E-10f) { Triple centroid = Triple.Zero; for (int i = 0; i < points.Count; i++) { centroid += points[i]; } centroid /= points.Count; float[,] P = new float[points.Count, 3]; for (int i = 0; i < points.Count; i++) { P[i, 0] = points[i].X - centroid.X; P[i, 1] = points[i].Y - centroid.Y; P[i, 2] = points[i].Z - centroid.Z; } float c00 = 0f, c01 = 0f, c02 = 0f; float c10 = 0f, c11 = 0f, c12 = 0f; float c20 = 0f, c21 = 0f, c22 = 0f; for (int k = 0; k < points.Count; k++) { c00 += P[k, 0] * P[k, 0]; c01 += P[k, 0] * P[k, 1]; c02 += P[k, 0] * P[k, 2]; c10 += P[k, 1] * P[k, 0]; c11 += P[k, 1] * P[k, 1]; c12 += P[k, 1] * P[k, 2]; c20 += P[k, 2] * P[k, 0]; c21 += P[k, 2] * P[k, 1]; c22 += P[k, 2] * P[k, 2]; } FastSvd3x3.Compute( c00, c01, c02, c10, c11, c12, c20, c21, c22, out float u00, out float u01, out _, out float u10, out float u11, out _, out float u20, out float u21, out _, out float s00, out float s11, out float s22, out _, out _, out _, out _, out _, out _, out _, out _, out _); planeOrigin = centroid; // Case 0: The input points are coincidental, so we just need to pick an arbitrary normal vector if (s00 < tolerance) { planeNormal = Triple.BasisZ; return(0); } // Case 1: The input points are collinear, so we just pick an arbitrary vector perpendicular to the dominant eigenvector if (s11 < tolerance) { planeNormal = new Triple(u00, u10, u20).GeneratePerpendicular(); return(1); } // Case 2: The input points are neighbor coincidental nor collinear, therefore the best fit plane is determined by the two dominant eigenvectors planeNormal = new Triple(u00, u10, u20).Cross(new Triple(u01, u11, u21)).Normalise(); return(s22 < tolerance ? 2 // The input points are coplanar : 3); // The input points are NOT coplanar }
/// <summary> /// Approximate the circle that best fit a set of input points /// </summary> /// <param name="points">The input points</param> /// <param name="circleCenter">Center of the best fit circle</param> /// <param name="circleNormal">Normal of the best fit circle</param> /// <param name="circleRadius">Radius of the best fit circle</param> /// <param name="tolerance">The tolerance that is used the determined if the input points are coincidental, collinear, or non-collinear</param> /// <returns>False if the input points are coincidental or collinear; True otherwise</returns> public static bool ComputeBestFitCircle(List <Triple> points, out Triple circleCenter, out Triple circleNormal, out float circleRadius, float tolerance = 1E-10f) { // The core idea is to project the input points to the best fit plane, then fit a circle through these points via an analytical approach. // Reference: Thomas S & Chan Y: "A simple approach for the estimation of circular arc center and its radius int planeFittingResult = ComputeBestFitPlane(points, out Triple planeOrigin, out Triple planeNormal, tolerance); // Case 0: The input points are either coincidental or collinear if (planeFittingResult < 2) { circleCenter = circleNormal = new Triple(float.NaN); circleRadius = float.NaN; return(false); } // Case 1: The input points are neither coincidental nor collinear Triple planeBasisX = planeNormal.GeneratePerpendicular().Normalise(); Triple planeBasisY = planeNormal.Cross(planeBasisX); float sumX = 0f; float sumY = 0f; float sumX2 = 0f; float sumY2 = 0f; float sumXY = 0f; float sumX3 = 0f; float sumY3 = 0f; float sumX2Y = 0f; float sumXY2 = 0f; float x, y; for (int i = 0; i < points.Count; i++) { Triple p = points[i] - planeOrigin; x = p.Dot(planeBasisX); y = p.Dot(planeBasisY); sumX += x; sumY += y; sumX2 += x * x; sumY2 += y * y; sumXY += x * y; sumX3 += x * x * x; sumY3 += y * y * y; sumX2Y += x * x * y; sumXY2 += x * y * y; } float n = points.Count; float a1 = 2f * (sumX * sumX - n * sumX2); float a2 = 2f * (sumX * sumY - n * sumXY); float b1 = a2; float b2 = 2f * (sumY * sumY - n * sumY2); float c1 = sumX2 * sumX - n * sumX3 + sumX * sumY2 - n * sumXY2; float c2 = sumX2 * sumY - n * sumY3 + sumY * sumY2 - n * sumX2Y; float temp = 1f / (a1 * b2 - a2 * b1); x = (c1 * b2 - c2 * b1) * temp; y = (a1 * c2 - a2 * c1) * temp; circleCenter = planeOrigin + (x * planeBasisX + y * planeBasisY); circleRadius = (float)Math.Sqrt((sumX2 - 2f * sumX * x + n * x * x + sumY2 - 2f * sumY * y + n * y * y) / n); circleNormal = planeNormal; return(true); }
public static Point ToPoint(this Triple t) => Point.ByCoordinates(t.X, t.Y, t.Z);
public static Vector ToVector(this Triple t) => Vector.ByCoordinates(t.X, t.Y, t.Z);
public void Reset() { Position = StartingPosition; Velocity = Triple.Zero; }
public void Iterate() { CurrentIteration++; //================================================================================= // Apply momentum //================================================================================= if (EnableMomentum) { foreach (Node node in Nodes) { node.Position += node.Velocity; } } //================================================================================= // Process each goal indepently, in parallel //================================================================================= Parallel.ForEach(Goals, goal => goal.Compute(Nodes)); //================================================================================= // Compute the total move vector that acts on each node //================================================================================= Triple[] nodeMoveSums = new Triple[Nodes.Count]; float[] nodeWeightSums = new float[Nodes.Count]; for (int j = 0; j < Goals.Count; j++) { Goal goal = Goals[j]; for (int i = 0; i < goal.NodeCount; i++) { nodeMoveSums[goal.NodeIndices[i]] += goal.Moves[i] * goal.Weight; nodeWeightSums[goal.NodeIndices[i]] += goal.Weight; } } //================================================================================= // Move the manipulated node toward the mouse ray //================================================================================= if (HandleNodeIndex != -1) { float mouseInteractionWeight = 30f; nodeWeightSums[HandleNodeIndex] += mouseInteractionWeight; Triple v = Nodes[HandleNodeIndex].Position - DynaShapeViewExtension.MouseRayOrigin; Triple mouseRayPull = v.Dot(DynaShapeViewExtension.MouseRayDirection) * DynaShapeViewExtension.MouseRayDirection - v; nodeMoveSums[HandleNodeIndex] += mouseInteractionWeight * mouseRayPull; } //============================================================================================= // Move the nodes to their new positions //============================================================================================= if (EnableMomentum) { for (int i = 0; i < Nodes.Count; i++) { Triple move = nodeMoveSums[i] / nodeWeightSums[i]; Nodes[i].Position += move; Nodes[i].Velocity += move; if (Nodes[i].Velocity.Dot(move) < 0.0) { Nodes[i].Velocity *= 0.99f; } } } else { for (int i = 0; i < Nodes.Count; i++) { Triple move = nodeMoveSums[i] / nodeWeightSums[i]; Nodes[i].Position += move; Nodes[i].Velocity = Triple.Zero; } } }
public Node(Triple startingPosition) { StartingPosition = startingPosition; Position = startingPosition; Velocity = Triple.Zero; }
/// <summary> /// Compute the line that best fit a set of input points (least squared orthogonal distance) /// </summary> /// <param name="points">The input points</param> /// <param name="lineOrigin">The output line origin</param> /// <param name="lineDirection">The output line direction</param> /// <returns>0 if the input points are identical, 1 if the input points are already colinear, 2 otherwise</returns> public static int ComputeBestFitLine(List <Triple> points, out Triple lineOrigin, out Triple lineDirection) { Triple centroid = Triple.Zero; for (int i = 0; i < points.Count; i++) { centroid += points[i]; } centroid /= points.Count; float[,] P = new float[points.Count, 3]; for (int i = 0; i < points.Count; i++) { P[i, 0] = points[i].X - centroid.X; P[i, 1] = points[i].Y - centroid.Y; P[i, 2] = points[i].Z - centroid.Z; } Matrix <float> covariance = Matrix <float> .Build.Dense(3, 3); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { for (int k = 0; k < points.Count; k++) { covariance[i, j] += P[k, i] * P[k, j]; } } } Evd <float> evd = covariance.Evd(); lineOrigin = centroid; if (evd.Rank == 0) // The input points are idendtical, so we just pick an arbitrary direction for the line { lineDirection = Triple.BasisX; } else // Otherwise the direction of the best fit line is the most dominant eigen vector { lineDirection = new Triple(evd.EigenVectors[0, 2], evd.EigenVectors[1, 2], evd.EigenVectors[2, 2]); } return(evd.Rank > 2 ? 2 : evd.Rank); }
public static bool ComputeBestFitCircle(List <Triple> points, out Triple circleCenter, out Triple circleNormal, out float circleRadius) { // Apporach: Project the input points to the best fit plane, then fit a circle through these points using the the analytical approach ... // ... described in the paper "A simple approach for the estimation of circular arc center and its radius" by Thomas S & Chan Y. Triple planeOrigin, planeNormal; int planeFittingResult = ComputeBestFitPlane(points, out planeOrigin, out planeNormal); if (planeFittingResult < 2 ) // The points are either all identical, or colinear (i.e. already on the same "circle" of infinite radius) { circleCenter = circleNormal = new Triple(float.NaN); circleRadius = float.NaN; return(false); } Triple planeBasisX = planeNormal.GeneratePerpendicular().Normalise(); Triple planeBasisY = planeNormal.Cross(planeBasisX); float sumX = 0f; float sumY = 0f; float sumX2 = 0f; float sumY2 = 0f; float sumXY = 0f; float sumX3 = 0f; float sumY3 = 0f; float sumX2Y = 0f; float sumXY2 = 0f; float x, y; for (int i = 0; i < points.Count; i++) { Triple p = points[i] - planeOrigin; x = p.Dot(planeBasisX); y = p.Dot(planeBasisY); sumX += x; sumY += y; sumX2 += x * x; sumY2 += y * y; sumXY += x * y; sumX3 += x * x * x; sumY3 += y * y * y; sumX2Y += x * x * y; sumXY2 += x * y * y; } float n = points.Count; float a1 = 2f * (sumX * sumX - n * sumX2); float a2 = 2f * (sumX * sumY - n * sumXY); float b1 = a2; float b2 = 2f * (sumY * sumY - n * sumY2); float c1 = sumX2 * sumX - n * sumX3 + sumX * sumY2 - n * sumXY2; float c2 = sumX2 * sumY - n * sumY3 + sumY * sumY2 - n * sumX2Y; float temp = 1f / (a1 * b2 - a2 * b1); x = (c1 * b2 - c2 * b1) * temp; y = (a1 * c2 - a2 * c1) * temp; circleCenter = planeOrigin + (x * planeBasisX + y * planeBasisY); circleRadius = (float)Math.Sqrt((sumX2 - 2f * sumX * x + n * x * x + sumY2 - 2f * sumY * y + n * y * y) / n); circleNormal = planeNormal; return(true); }
public static Vector Duplicate(this Triple vector) => Vector.ByCoordinates(vector.X, vector.Y, vector.Z);
/// <summary> /// Compute the plane that best fit a set of input points (least squared orthogonal distance) /// </summary> /// <param name="points">The input points</param> /// <param name="planeOrigin">The output plane origin</param> /// <param name="planeNormal">The output plane normal vector</param> /// <returns>0 if the input points are identical, 1 if the input points are colinear, 2 if the input points are coplanar (already on a plane), 3 otherwise</returns> public static int ComputeBestFitPlane(List <Triple> points, out Triple planeOrigin, out Triple planeNormal) { Triple centroid = Triple.Zero; for (int i = 0; i < points.Count; i++) { centroid += points[i]; } centroid /= points.Count; float[,] P = new float[points.Count, 3]; for (int i = 0; i < points.Count; i++) { P[i, 0] = points[i].X - centroid.X; P[i, 1] = points[i].Y - centroid.Y; P[i, 2] = points[i].Z - centroid.Z; } Matrix <float> covariance = Matrix <float> .Build.Dense(3, 3); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { for (int k = 0; k < points.Count; k++) { covariance[i, j] += P[k, i] * P[k, j]; } } } Evd <float> evd = covariance.Evd(); planeOrigin = centroid; if (evd.Rank == 0) // The input points are idendtical, so we just pick an arbitrary normal vector { planeNormal = Triple.BasisZ; } else if (evd.Rank == 1 ) // The input points are colinear, so we just pick an arbitrary vector perpendicular to the only eigen vector { planeNormal = new Triple(evd.EigenVectors[0, 1], evd.EigenVectors[1, 1], evd.EigenVectors[2, 1]) .GeneratePerpendicular(); } else // The normal is perpendicular to the two dominant eigen vectors { Triple e1 = new Triple(evd.EigenVectors[0, 1], evd.EigenVectors[1, 1], evd.EigenVectors[2, 1]); Triple e2 = new Triple(evd.EigenVectors[0, 2], evd.EigenVectors[1, 2], evd.EigenVectors[2, 2]); planeNormal = e2.Cross(e1).Normalise(); } return(evd.Rank); }