public static CircleData Ls2DCircle(Matrix <double> data)
{
if (data == null)
{
throw new ArgumentNullException("data");
}
var dataTmp = data.Clone();
var dataOut = new CircleData();
// Check number of data points
if (dataTmp.RowCount < 2)
{
return(dataOut);
}
// init A and b
var A = Matrix <double> .Build.Dense(dataTmp.RowCount, 3);
var b = Vector <double> .Build.Dense(dataTmp.RowCount);
for (int i = 0; i < dataTmp.RowCount; i++)
{
A[i, 0] = -2 * dataTmp[i, 0];
A[i, 1] = -2 * dataTmp[i, 1];
A[i, 2] = 1.0f;
b[i] = -(Math.Pow(dataTmp[i, 0], 2) + Math.Pow(dataTmp[i, 1], 2));
}
Vector <double> C = A.QR(QRMethod.Full).Solve(b);
// set circle centroid
dataOut.CircleCentroid[0] = C[0];
dataOut.CircleCentroid[1] = C[1];
// compute circle radius
dataOut.CircleRadius = Math.Sqrt(Math.Pow(C[0], 2) + Math.Pow(C[1], 2) - C[2]);
// Convert last to polar coordinates
Vector <double> lastPolar = GpsHelper.ConvertCartesianToPolar(dataTmp[dataTmp.RowCount - 1, 0], dataTmp[dataTmp.RowCount - 1, 1], dataTmp[dataTmp.RowCount - 1, 2]);
dataOut.Inclination = lastPolar[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 Vector <double> GetCartesianPoint(Vector <double> currentCartesianPoint, CircleData circleData, int nbStep, double stepLength)
{
if (circleData == null)
{
throw new ArgumentNullException("circleData");
}
double initialTheta = GpsHelper.ConvertCartesianToPolar(currentCartesianPoint[0], currentCartesianPoint[1])[1];
double theta = initialTheta + nbStep * stepLength;
return(Vector <double> .Build.Dense(new[] {
circleData.CircleRadius *Math.Cos(theta) + circleData.CircleCentroid[0],
circleData.CircleRadius *Math.Sin(theta) + circleData.CircleCentroid[1],
0
}));
}
