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