public static PlaneData LsPlane(Matrix <double> data)
{
if (data == null)
{
throw new ArgumentNullException("data");
}
var dataTmp = data.Clone();
PlaneData dataOut = new PlaneData(dataTmp.RowCount);
// Check number of data points
if (dataTmp.RowCount < 3)
{
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);
// Direction cosines of the normal to the best-fit plane.
// Find the smallest singular value in S and extract from V the
// corresponding right singular vector.
dataOut.PlaneParams[0] = svd.VT[2, 0];
dataOut.PlaneParams[1] = svd.VT[2, 1];
dataOut.PlaneParams[2] = svd.VT[2, 2];
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);
}