public static PlaneData LsPlane(Matrix <double> data) { if (data == null) { throw new ArgumentNullException("data"); } var dataTmp = data.Clone(); PlaneData dataOut = new PlaneData(dataTmp.RowCount); // Check number of data points if (dataTmp.RowCount < 3) { return(dataOut); } // Calculate centroid dataOut.Centroid[0] = 0; dataOut.Centroid[1] = 0; dataOut.Centroid[2] = 0; for (int i = 0; i < dataTmp.RowCount; i++) { dataOut.Centroid[0] += dataTmp[i, 0]; dataOut.Centroid[1] += dataTmp[i, 1]; dataOut.Centroid[2] += dataTmp[i, 2]; } dataOut.Centroid[0] /= dataTmp.RowCount; dataOut.Centroid[1] /= dataTmp.RowCount; dataOut.Centroid[2] /= dataTmp.RowCount; // Form matrix a of translated points for (int i = 0; i < dataTmp.RowCount; i++) { dataTmp[i, 0] -= dataOut.Centroid[0]; dataTmp[i, 1] -= dataOut.Centroid[1]; dataTmp[i, 2] -= dataOut.Centroid[2]; } var svd = dataTmp.Svd(computeVectors: true); // Direction cosines of the normal to the best-fit plane. // Find the smallest singular value in S and extract from V the // corresponding right singular vector. dataOut.PlaneParams[0] = svd.VT[2, 0]; dataOut.PlaneParams[1] = svd.VT[2, 1]; dataOut.PlaneParams[2] = svd.VT[2, 2]; 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); }