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