Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Geometry_Engine: Singular Value Decomposition implemented and applied to FitLine #3280

Merged
merged 3 commits into from
Feb 19, 2024
Merged

Geometry_Engine: Singular Value Decomposition implemented and applied to FitLine #3280

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
19144dc
#3278 fixed
pawelbaran Feb 12, 2024
a7d4c98
implementation based on Singular Value Decomposition algorithm
pawelbaran Feb 17, 2024
4afbe28
method parameter name tweaked
pawelbaran Feb 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
96 changes: 12 additions & 84 deletions Geometry_Engine/Compute/FitLine.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,9 @@
* along with this code. If not, see <https://www.gnu.org/licenses/lgpl-3.0.html>.
*/

using BH.oM.Base;
using BH.oM.Base.Attributes;
using BH.oM.Geometry;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Linq;
Expand All @@ -32,7 +32,7 @@ namespace BH.Engine.Geometry
public static partial class Compute
{
/***************************************************/
/**** public Methods - Vectors ****/
/**** Public Methods ****/
/***************************************************/

[Description("Fits a line into a set of points using Orthogonal Least Squares algorithm.")]
Expand All @@ -46,92 +46,20 @@ public static Line FitLine(this IEnumerable<Point> points, double tolerance = To
if (n < 2)
return null;

// Three cases: collinear, coplanar in XYZ and general covered separately
// Required due to the fact that general solution degrades to zero length line for edge cases
Point C = points.Average();
if (asList.IsCollinear(tolerance))
{
List<Point> sorted = asList.SortCollinear(tolerance);
return new Line { Start = sorted[0], End = sorted[sorted.Count - 1] };
}
else if (points.All(x => Math.Abs(C.Z - x.Z) < tolerance))

double[,] A = new double[n,3];
for (int i = 0; i < n; i++)
{
// Based on https://iojes.net/Makaleler/386bbbe6-b98d-4766-a717-2c945a97f267.pdf
double sumX = points.Sum(x => x.X);
double sumY = points.Sum(x => x.Y);
double sumXsq = points.Sum(x => x.X * x.X);
double sumYsq = points.Sum(x => x.Y * x.Y);
double sumXY = points.Sum(x => x.X * x.Y);
double p1 = sumX * sumX - sumY * sumY - n * (sumXsq - sumYsq);
double p2 = n * sumXY - sumX * sumY;
double b = (p1 + Math.Sqrt(p1 * p1 + 4 * p2 * p2)) / (2 * p2);
Vector dir = new Vector { X = 1, Y = b };
return new Line { Start = C, End = C + dir };
A[i, 0] = asList[i].X - C.X;
A[i, 1] = asList[i].Y - C.Y;
A[i, 2] = asList[i].Z - C.Z;
}
else
{
// Based on https://www.scribd.com/doc/31477970/Regressions-et-trajectoires-3D
double xx = 0.0; double xy = 0.0; double xz = 0.0;
double yy = 0.0; double yz = 0.0; double zz = 0.0;

foreach (Point P in points)
{
xx += P.X * P.X;
xy += P.X * P.Y;
xz += P.X * P.Z;
yy += P.Y * P.Y;
yz += P.Y * P.Z;
zz += P.Z * P.Z;
}

double Sxx = xx / n - C.X * C.X;
double Sxy = xy / n - C.X * C.Y;
double Sxz = xz / n - C.X * C.Z;
double Syy = yy / n - C.Y * C.Y;
double Syz = yz / n - C.Y * C.Z;
double Szz = zz / n - C.Z * C.Z;

double theta = Math.Atan(2 * Sxy / (Sxx - Syy)) * 0.5;
double stheta = Math.Sin(theta);
double ctheta = Math.Cos(theta);
double K11 = (Syy + Szz) * ctheta * ctheta + (Sxx + Szz) * stheta * stheta - 2 * Sxy * ctheta * stheta;
double K22 = (Syy + Szz) * stheta * stheta + (Sxx + Szz) * ctheta * ctheta + 2 * Sxy * ctheta * stheta;
double K12 = -Sxy * (ctheta * ctheta - stheta * stheta) + (Sxx - Syy) * ctheta * stheta;
double K10 = Sxz * ctheta + Syz * stheta;
double K01 = -Sxz * stheta + Syz * ctheta;
double K00 = Sxx + Syy;

double c0 = K01 * K01 * K11 + K10 * K10 * K22 - K00 * K11 * K22;
double c1 = K00 * K11 + K00 * K22 + K11 * K22 - K01 * K01 - K10 * K10;
double c2 = -K00 - K11 - K22;

double p = c1 - c2 * c2 / 3;
double q = c2 * c2 * c2 * 2 / 27 - c1 * c2 / 3 + c0;
double R = q * q * 0.25 + p * p * p / 27;

double sqrDeltaM;
double cc = -c2 / 3;

if (R > tolerance)
sqrDeltaM = cc + Math.Pow(-q * 0.5 + Math.Sqrt(R), 1.0 / 3.0) + Math.Pow(-q * 0.5 - Math.Sqrt(R), 1.0 / 3.0);
else
{
double rho = Math.Sqrt(-p * p * p / 27);
double fi = Math.Acos(-q / rho * 0.5);
double doubleRhoRoot = 2 * Math.Pow(rho, 1.0 / 3.0);
double minCos = Math.Min(Math.Cos(fi / 3), Math.Min(Math.Cos((fi + 2 * Math.PI) / 3), Math.Cos((fi + 4 * Math.PI))));
sqrDeltaM = cc + doubleRhoRoot * minCos;
}

double a = -K10 / (K11 - sqrDeltaM) * ctheta + K01 / (K22 - sqrDeltaM) * stheta;
double b = -K10 / (K11 - sqrDeltaM) * stheta - K01 / (K22 - sqrDeltaM) * ctheta;
double u = ((1 + b * b) * C.X - a * b * C.Y + a * C.Z) / (1 + a * a + b * b);
double v = (-a * b * C.X + (1 + a * a) * C.Y + b * C.Z) / (1 + a * a + b * b);
double w = (a * C.X + b * C.Y + (a * a + b * b) * C.Z) / (1 + a * a + b * b);

Point H = new Point { X = u, Y = v, Z = w };
return new Line { Start = C + (C - H), End = H };
}
Output<double[,], double[], double[,]> svd = A.SingularValueDecomposition();
double[,] Vh = svd.Item3;
Vector dir = new Vector { X = Vh[0, 0], Y = Vh[0, 1], Z = Vh[0, 2] };
return new Line { Start = C - dir, End = C + dir };
}

/***************************************************/
Expand Down
Loading