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