public static Vector <double> GetCartesianPoint(Vector <double> currentCartesianPoint, Circle3DData circle3DData, int nbStep, double stepLength)
{
if (circle3DData == null)
{
throw new ArgumentNullException("circle3DData");
}
double circumference = 2 * Math.PI * circle3DData.CircleRadius;
double theta = stepLength * 2 * Math.PI / circumference;
if (circle3DData.IsAscending)
{
circle3DData.Theta = circle3DData.Theta + theta;
}
else
{
circle3DData.Theta = circle3DData.Theta - theta;
}
Vector <double> newPoint = GpsHelper.ConvertPolarToCartesian(circle3DData.CircleRadius, circle3DData.Theta, circle3DData.Z);
// Map to the initial coordinates system
var pointMatrix = Matrix <double> .Build.Dense(1, 3);
pointMatrix[0, 0] = newPoint[0] + circle3DData.CircleCentroid[0] + circle3DData.RotationCentroid[0];
pointMatrix[0, 1] = newPoint[1] + circle3DData.CircleCentroid[1] + circle3DData.RotationCentroid[1];
pointMatrix[0, 2] = newPoint[2] + circle3DData.RotationCentroid[2];
pointMatrix = (circle3DData.RotationMatrix.Transpose() * pointMatrix.Transpose()).Transpose();
newPoint[0] = pointMatrix[0, 0] + circle3DData.PlaneCentroid[0];
newPoint[1] = pointMatrix[0, 1] + circle3DData.PlaneCentroid[1];
newPoint[2] = pointMatrix[0, 2] + circle3DData.PlaneCentroid[2];
return(newPoint);
}
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);
}
