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);
        }