public static PolynomialData LsPolynomial(Vector <double> data, Vector <double> index, int order)
{
if (data == null)
{
throw new ArgumentNullException("data");
}
if (index == null)
{
throw new ArgumentNullException("index");
}
if (order != 3 && order != 5)
{
throw new NotImplementedException("Uniquement le 3e et le 5e ordre sont implantés");
}
var dataOut = new PolynomialData(order);
Matrix <double> V = MatrixOperation.VandermondeMatrix(index, order);
Vector <double> p = V.QR(QRMethod.Full).Solve(data);
Vector <double> r = data - (V * p);
dataOut.PolynomialParams = Vector <double> .Build.DenseOfArray(p.ToArray());
dataOut.VandermondeMatrix = V;
dataOut.FreedomDegrees = data.Count - (order + 1);
dataOut.ResidualNorm = r.L2Norm();
dataOut.PolynomialOrder = order;
return(dataOut);
}
public static LineData Ls2Dline(Matrix <double> data)
{
if (data == null)
{
throw new ArgumentNullException("data");
}
var dataTmp = data.Clone();
LineData dataOut = new LineData(dataTmp.RowCount);
// Check number of data points
if (dataTmp.RowCount < 2)
{
return(dataOut);
}
// Calculate centroid
dataOut.Centroid[0] = 0;
dataOut.Centroid[1] = 0;
for (int i = 0; i < dataTmp.RowCount; i++)
{
dataOut.Centroid[0] += dataTmp[i, 0];
dataOut.Centroid[1] += dataTmp[i, 1];
}
dataOut.Centroid[0] /= dataTmp.RowCount;
dataOut.Centroid[1] /= 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];
}
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];
// Calculate residual distances
for (int i = 0; i < dataTmp.RowCount; i++)
{
var vDiff = Vector <double> .Build.Dense(new[] { dataTmp[i, 0], dataTmp[i, 1] });
dataOut.Residuals[i] = MatrixOperation.CrossProduct(vDiff, dataOut.LineParams).L2Norm();
dataOut.ResidualsNorm += Math.Pow(dataOut.Residuals[i], 2);
}
dataOut.ResidualsNorm = Math.Sqrt(dataOut.ResidualsNorm);
// Calculate intersection point
var startPoint = Vector <double> .Build.Dense(new[] { data[0, 0], data[0, 1] });
var endPoint = Vector <double> .Build.Dense(new[] { data[data.RowCount - 1, 0], data[data.RowCount - 1, 1] });
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é contenait 2 coordonnées et que ses dernières étaient exactement les mêmes.
dataOut.IntersectionPoint[0] = data[data.RowCount - 1, 0];
dataOut.IntersectionPoint[1] = data[data.RowCount - 1, 1];
}
return(dataOut);
}
public static Circle3DData Ls3DCircle(Matrix <double> data, int startPointIndex)
{
if (data == null)
{
throw new ArgumentNullException("data");
}
var dataTmp = data.Clone();
var dataOut = new Circle3DData();
// Check number of data points
if (dataTmp.RowCount < 2)
{
return(dataOut);
}
PlaneData planeData = LsPlane(dataTmp);
dataOut.PlaneCentroid = planeData.Centroid;
// Form matrix A of translated points
for (int i = 0; i < dataTmp.RowCount; i++)
{
dataTmp[i, 0] -= dataOut.PlaneCentroid[0];
dataTmp[i, 1] -= dataOut.PlaneCentroid[1];
dataTmp[i, 2] -= dataOut.PlaneCentroid[2];
}
// Transform the data to close to standard position via a rotation
// followed by a translation
dataOut.RotationMatrix = MatrixOperation.RotationZ_3D(planeData.PlaneParams);
dataOut.RotationCentroid = dataOut.RotationMatrix * dataOut.PlaneCentroid;
Matrix <double> rotatedData = (dataOut.RotationMatrix * dataTmp.Transpose()).Transpose();
//Translate data to the rotated origin
for (int i = 0; i < dataTmp.RowCount; i++)
{
rotatedData[i, 0] -= dataOut.RotationCentroid[0];
rotatedData[i, 1] -= dataOut.RotationCentroid[1];
rotatedData[i, 2] -= dataOut.RotationCentroid[2];
}
CircleData circle2DData = Ls2DCircle(rotatedData);
dataOut.CircleCentroid = circle2DData.CircleCentroid;
dataOut.CircleRadius = circle2DData.CircleRadius;
// THETA is a counterclockwise angular displacement in radians from the
// positive x-axis, RHO is the distance from the origin to a point in the x-y plane
for (int i = 0; i < rotatedData.RowCount; i++)
{
rotatedData[i, 0] -= dataOut.CircleCentroid[0];
rotatedData[i, 1] -= dataOut.CircleCentroid[1];
}
//Convert start point to polar coordinate
Vector <double> startPolar = GpsHelper.ConvertCartesianToPolar(rotatedData[startPointIndex, 0], rotatedData[startPointIndex, 1], rotatedData[startPointIndex, 2]);
dataOut.IsAscending = GetCircleDirection(rotatedData.Row(0), rotatedData.Row(startPointIndex), startPolar[0], dataOut.CircleRadius);
dataOut.Theta = startPolar[0];
dataOut.Z = startPolar[2];
return(dataOut);
}
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);
}
