From 19144dc524d19c0ffb36443c023de4a1490c155c Mon Sep 17 00:00:00 2001 From: Pawel Baran Date: Mon, 12 Feb 2024 18:39:31 +0100 Subject: [PATCH 1/3] #3278 fixed --- Geometry_Engine/Compute/FitLine.cs | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Geometry_Engine/Compute/FitLine.cs b/Geometry_Engine/Compute/FitLine.cs index 23ae84070..f5bdfcfaf 100644 --- a/Geometry_Engine/Compute/FitLine.cs +++ b/Geometry_Engine/Compute/FitLine.cs @@ -64,8 +64,16 @@ public static Line FitLine(this IEnumerable points, double tolerance = To 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 }; + + Vector dir; + if (Math.Abs(p2) > tolerance) + { + double b = (p1 + Math.Sqrt(p1 * p1 + 4 * p2 * p2)) / (2 * p2); + dir = new Vector { X = 1, Y = b }; + } + else + dir = p1 < 0 ? Vector.XAxis : Vector.YAxis; + return new Line { Start = C, End = C + dir }; } else From a7d4c98302f2f20d60279214f6dd053c13562df6 Mon Sep 17 00:00:00 2001 From: Pawel Baran Date: Sat, 17 Feb 2024 23:16:23 +0100 Subject: [PATCH 2/3] implementation based on Singular Value Decomposition algorithm --- Geometry_Engine/Compute/FitLine.cs | 104 +----- .../Compute/SingularValueDecomposition.cs | 333 ++++++++++++++++++ 2 files changed, 345 insertions(+), 92 deletions(-) create mode 100644 Geometry_Engine/Compute/SingularValueDecomposition.cs diff --git a/Geometry_Engine/Compute/FitLine.cs b/Geometry_Engine/Compute/FitLine.cs index f5bdfcfaf..9517d27de 100644 --- a/Geometry_Engine/Compute/FitLine.cs +++ b/Geometry_Engine/Compute/FitLine.cs @@ -20,9 +20,9 @@ * along with this code. If not, see . */ +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; @@ -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.")] @@ -46,100 +46,20 @@ public static Line FitLine(this IEnumerable 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 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)) - { - // 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; - - Vector dir; - if (Math.Abs(p2) > tolerance) - { - double b = (p1 + Math.Sqrt(p1 * p1 + 4 * p2 * p2)) / (2 * p2); - dir = new Vector { X = 1, Y = b }; - } - else - dir = p1 < 0 ? Vector.XAxis : Vector.YAxis; - return new Line { Start = C, End = C + dir }; - } - else + double[,] A = new double[n,3]; + for (int i = 0; i < n; i++) { - // 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 }; + A[i, 0] = asList[i].X - C.X; + A[i, 1] = asList[i].Y - C.Y; + A[i, 2] = asList[i].Z - C.Z; } + + Output 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 }; } /***************************************************/ diff --git a/Geometry_Engine/Compute/SingularValueDecomposition.cs b/Geometry_Engine/Compute/SingularValueDecomposition.cs new file mode 100644 index 000000000..68cc6be36 --- /dev/null +++ b/Geometry_Engine/Compute/SingularValueDecomposition.cs @@ -0,0 +1,333 @@ +/* + * This file is part of the Buildings and Habitats object Model (BHoM) + * Copyright (c) 2015 - 2024, the respective contributors. All rights reserved. + * + * Each contributor holds copyright over their respective contributions. + * The project versioning (Git) records all such contribution source information. + * + * + * The BHoM is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3.0 of the License, or + * (at your option) any later version. + * + * The BHoM is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this code. If not, see . + */ + +using BH.oM.Base; +using BH.oM.Base.Attributes; +using System; +using System.ComponentModel; + +namespace BH.Engine.Geometry +{ + public static partial class Compute + { + /***************************************************/ + /**** Public Methods ****/ + /***************************************************/ + + [Description("Performs factorization of a matrix into three matrices known as Sigular Value Decomposition." + + "\nReturns 3 matrices that represent the formula A = U * S * Vh." + + "\nImplementation uses Jacobi eigenvalue algorithm.")] + [Input("A", "Matrix to run the factorization against.")] + [MultiOutput(0, "U", "U component in formula A = U * S * Vh.")] + [MultiOutput(1, "S", "S component in formula A = U * S * Vh.")] + [MultiOutput(2, "Vh", "Vh component in formula A = U * S * Vh.")] + public static Output SingularValueDecomposition(this double[,] A) + { + // Rewrite of https://github.com/ampl/gsl/blob/master/linalg/svd.c + double epsilon = 1e-15; + A = (double[,])A.Clone(); + + int m = A.GetLength(0); + int n = A.GetLength(1); + double[,] Q = Identity(n); + double[] S = new double[n]; + + // Initialize the rotation counter and the sweep counter + int count = 1; + int sweep = 0; + + double tolerance = 10 * m * epsilon; + + // Always at least 12 sweeps + int sweepmax = Math.Max(5 * n, 12); + + // Store the column error estimates in S, for use during orthogonalization + + for (int j = 0; j < n; j++) + { + double[] cj = A.GetColumn(j); + double sj = cj.Normalise(); + S[j] = epsilon * sj; + } + + // Orthogonalize A by plane rotations + while (count > 0 && sweep < sweepmax) + { + // Initialize rotation counter + count = n * (n - 1) / 2; + + for (int j = 0; j < n - 1; j++) + { + for (int k = j + 1; k < n; k++) + { + double cosine = 0; + double sine = 0; + + double[] cj = A.GetColumn(j); + double[] ck = A.GetColumn(k); + + double p = 2 * DotProduct(cj, ck); + double a = cj.Normalise(); + double b = ck.Normalise(); + + double q = a * a - b * b; + double v = Hypot(p, q); + + // Test for columns j,k orthogonal or dominant errors + double absErrA = S[j]; + double absErrB = S[k]; + + bool sorted = (a >= b); + bool orthog = (Math.Abs(p) <= tolerance * (a * b)); + bool noisya = (a < absErrA); + bool noisyb = (b < absErrB); + + if (sorted && (orthog || noisya || noisyb)) + { + count--; + continue; + } + + // Calculate rotation angles + if (v == 0 || !sorted) + { + cosine = 0; + sine = 1; + } + else + { + cosine = Math.Sqrt((v + q) / (2 * v)); + sine = p / (2 * v * cosine); + } + + // Apply rotation to A (U) + for (int i = 0; i < m; i++) + { + double Aik = A[i, k]; + double Aij = A[i, j]; + A[i, j] = Aij * cosine + Aik * sine; + A[i, k] = -Aij * sine + Aik * cosine; + } + + S[j] = Math.Abs(cosine) * absErrA + Math.Abs(sine) * absErrB; + S[k] = Math.Abs(sine) * absErrA + Math.Abs(cosine) * absErrB; + + // Apply rotation to Q (Vh) + for (int i = 0; i < n; i++) + { + double Qij = Q[i, j]; + double Qik = Q[i, k]; + Q[i, j] = Qij * cosine + Qik * sine; + Q[i, k] = -Qij * sine + Qik * cosine; + } + } + } + + // Sweep completed + sweep++; + } + + // Orthogonalization complete, compute singular values + double prevNorm = -1; + + for (int j = 0; j < n; j++) + { + double[] column = A.GetColumn(j); + double norm = column.Normalise(); + + // Determine if singular value is zero, according to the criteria used in the main loop above + // (i.e. comparison with norm of previous column) + if (norm == 0 || prevNorm == 0 || (j > 0 && norm <= tolerance * prevNorm)) + { + S[j] = 0; + for (int i = 0; i < m; i++) + { + A[i, j] = 0; + } + + prevNorm = 0; + } + else + { + S[j] = norm; + for (int i = 0; i < m; i++) + { + A[i, j] = A[i, j] / norm; + } + + prevNorm = norm; + } + } + + if (count > 0) + { + BH.Engine.Base.Compute.RecordError("Jacobi algorithm did not converge."); + return null; + } + + double[,] Vh = Q.Transpose(); + + // Trim the results to the number of rows if needed + if (m < n) + { + A = A.TakeColumns(m); + S = S.TakeComponents(m); + Vh = Vh.TakeRows(m); + } + + return new Output { Item1 = A, Item2 = S, Item3 = Vh }; + } + + + /***************************************************/ + /**** Private Methods ****/ + /***************************************************/ + + private static double Hypot(double x, double y) + { + // As described in https://pubs.opengroup.org/onlinepubs/009695399/functions/hypot.html + double xAbs = Math.Abs(x); + double yAbs = Math.Abs(y); + double min, max; + + if (xAbs < yAbs) + { + min = xAbs; + max = yAbs; + } + else + { + min = yAbs; + max = xAbs; + } + + if (min == 0) + return max; + else + { + double u = min / max; + return max * Math.Sqrt(1 + u * u); + } + } + + /***************************************************/ + + private static double[,] Identity(int n) + { + double[,] result = new double[n, n]; + for (int i = 0; i < n; i++) + { + result[i, i] = 1; + } + + return result; + } + + /***************************************************/ + + private static double Normalise(this double[] v) + { + double sum = 0; + for (int i = 0; i < v.Length; i++) + { + sum += v[i] * v[i]; + } + + return Math.Sqrt(sum); + } + + /***************************************************/ + + private static double DotProduct(double[] v1, double[] v2) + { + double sum = 0; + for (int i = 0; i < v1.Length; i++) + { + sum += v1[i] * v2[i]; + } + + return sum; + } + + /***************************************************/ + + private static double[] GetColumn(this double[,] m, int j) + { + int rows = m.GetLength(0); + double[] result = new double[rows]; + for (int i = 0; i < rows; i++) + { + result[i] = m[i, j]; + } + + return result; + } + + /***************************************************/ + + private static double[,] TakeColumns(this double[,] src, int n) + { + int rows = src.GetLength(0); + + double[,] result = new double[rows, n]; + for (int i = 0; i < rows; i++) + { + for (int j = 0; j < n; j++) + { + result[i, j] = src[i, j]; + } + } + + return result; + } + + /***************************************************/ + + private static double[,] TakeRows(this double[,] src, int n) + { + int columns = src.GetLength(1); + double[,] result = new double[n, columns]; + for (int i = 0; i < n; i++) + { + for (int j = 0; j < columns; j++) + result[i, j] = src[i, j]; + } + + return result; + } + + /***************************************************/ + + private static double[] TakeComponents(this double[] v, int n) + { + double[] result = new double[n]; + for (int i = 0; i < n; i++) + { + result[i] = v[i]; + } + + return result; + } + + /***************************************************/ + } +} From 4afbe284468c32233de20894dba56874c2f91596 Mon Sep 17 00:00:00 2001 From: Pawel Baran Date: Sat, 17 Feb 2024 23:42:05 +0100 Subject: [PATCH 3/3] method parameter name tweaked --- Geometry_Engine/Compute/SingularValueDecomposition.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Geometry_Engine/Compute/SingularValueDecomposition.cs b/Geometry_Engine/Compute/SingularValueDecomposition.cs index 68cc6be36..8cafa23ee 100644 --- a/Geometry_Engine/Compute/SingularValueDecomposition.cs +++ b/Geometry_Engine/Compute/SingularValueDecomposition.cs @@ -36,15 +36,15 @@ public static partial class Compute [Description("Performs factorization of a matrix into three matrices known as Sigular Value Decomposition." + "\nReturns 3 matrices that represent the formula A = U * S * Vh." + "\nImplementation uses Jacobi eigenvalue algorithm.")] - [Input("A", "Matrix to run the factorization against.")] + [Input("matrix", "Matrix to run the factorization against, A component in formula A = U * S * Vh.")] [MultiOutput(0, "U", "U component in formula A = U * S * Vh.")] [MultiOutput(1, "S", "S component in formula A = U * S * Vh.")] [MultiOutput(2, "Vh", "Vh component in formula A = U * S * Vh.")] - public static Output SingularValueDecomposition(this double[,] A) + public static Output SingularValueDecomposition(this double[,] matrix) { // Rewrite of https://github.com/ampl/gsl/blob/master/linalg/svd.c double epsilon = 1e-15; - A = (double[,])A.Clone(); + double [,] A = (double[,])matrix.Clone(); int m = A.GetLength(0); int n = A.GetLength(1);