public static Vector <double> GetCartesianPoint(Vector <double> currentCartesianPoint, Line3DData line3DData, int nbStep, double stepLength) { return(Vector <double> .Build.Dense(new[] { nbStep *stepLength *line3DData.LineParams[0] + currentCartesianPoint[0], nbStep *stepLength *line3DData.LineParams[1] + currentCartesianPoint[1], nbStep *stepLength *line3DData.LineParams[2] + currentCartesianPoint[2] })); }
public static Line3DData Ls3Dline(Matrix <double> data, int startPointIndex) { if (data == null) { throw new ArgumentNullException("data"); } //------------------------------------------------------------- // DÉBUT Fit 3DLine //------------------------------------------------------------- var dataTmp = data.Clone(); Line3DData dataOut = new Line3DData(dataTmp.RowCount); // Check number of data points if (dataTmp.RowCount < 2) { return(dataOut); } // Calculate centroid dataOut.Centroid[0] = 0; dataOut.Centroid[1] = 0; dataOut.Centroid[2] = 0; for (int i = 0; i < dataTmp.RowCount; i++) { dataOut.Centroid[0] += dataTmp[i, 0]; dataOut.Centroid[1] += dataTmp[i, 1]; dataOut.Centroid[2] += dataTmp[i, 2]; } dataOut.Centroid[0] /= dataTmp.RowCount; dataOut.Centroid[1] /= dataTmp.RowCount; dataOut.Centroid[2] /= dataTmp.RowCount; // Form matrix a of translated points for (int i = 0; i < dataTmp.RowCount; i++) { dataTmp[i, 0] -= dataOut.Centroid[0]; dataTmp[i, 1] -= dataOut.Centroid[1]; dataTmp[i, 2] -= dataOut.Centroid[2]; } var svd = dataTmp.Svd(computeVectors: true); // Find the largest singular value in S and extract from V the // corresponding right singular vector dataOut.LineParams[0] = svd.VT[0, 0]; dataOut.LineParams[1] = svd.VT[0, 1]; dataOut.LineParams[2] = svd.VT[0, 2]; // Calculate residual distances for (int i = 0; i < dataTmp.RowCount; i++) { var vDiff = Vector <double> .Build.Dense(new[] { dataTmp[i, 0], dataTmp[i, 1], dataTmp[i, 2] }); dataOut.Residuals[i] = MatrixOperation.CrossProduct(vDiff, dataOut.LineParams).L2Norm(); } dataOut.ResidualsNorm = dataOut.Residuals.L2Norm(); //------------------------------------------------------------- // FIN Fit 3DLine //------------------------------------------------------------- //TODO: Michel Robert: Il faut refactoriser, il s'agit de 2 concepts, donc 2 fonctions //------------------------------------------------------------- // Début Extrapolation //------------------------------------------------------------- // Calculate intersection point var startPoint = Vector <double> .Build.Dense(new[] { data[0, 0], data[0, 1], data[0, 2] }); var endPoint = Vector <double> .Build.Dense(new[] { data[startPointIndex, 0], data[startPointIndex, 1], data[startPointIndex, 2] }); var intersectionPoint = GetIntersectionPoint(startPoint, endPoint, dataOut.LineParams, dataOut.Centroid); if (intersectionPoint == null) { //TODO: JFF: À revoir... Ajouté parce qu'il arrivait que lfU soit non numérique et que si // les intersections restaient à 0 alors d'autres erreurs arrivaient plus loin. // Pour l'instant c'est la correction la plus simple mais lorsqu'on aura un peu plus de temps, // se pencher d'avantage sur une correction/gestion plus viable de cette erreur. // Erreur qui arrivait par exemple lorsque la matrix d'entrée contenait 2 coordonnées et que // ces dernières étaient exactement les mêmes. dataOut.IntersectionPoint = endPoint; } else { dataOut.IntersectionPoint = intersectionPoint; } if (!GetLineDirection(startPoint, dataOut.IntersectionPoint, dataOut.LineParams)) { dataOut.LineParams = dataOut.LineParams * -1; } return(dataOut); }