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