private Vector3[] ProjectToothOnAxisPlane(Common.Vbo handle, Common.Plane axisP, Common.Plane occlusalP) { const float dist2CenterTh = 5; const float dist2PlaneTh = 0.2f; List <Vector3> projectedPoints = new List <Vector3>(); double distanceToLowest = occlusalP.Distance2Point(axisP.lowestPoint); double distanceToHighest = occlusalP.Distance2Point(axisP.highestPoint); //int sideLowest = occlusalP.PointSide for (int i = 0; i < handle.verticesData.vertices.Length; i++) { if (axisP.Distance2Point(handle.verticesData.vertices[i]) < dist2PlaneTh && (handle.verticesData.vertices[i] - axisP.center).Length <dist2CenterTh && //occlusalP.Distance2Point(handle.verticesData.vertices[i]) < distanceToLowest && //occlusalP.Distance2Point(handle.verticesData.vertices[i]) > distanceToHighest handle.verticesData.vertices[i].Z> axisP.lowestPoint.Z && // ASSUMING CAST IS FACED UPWARD = Z handle.verticesData.vertices[i].Z < axisP.highestPoint.Z //ASSUMING CAST IS FACED UPWARD = Z ) { projectedPoints.Add(axisP.ProjectPointOnPlane(handle.verticesData.vertices[i])); } } return(projectedPoints.ToArray());//projectedPointsOnAxisPlane[selectedToothIndex].ToArray(); ; }
private void Sort3PointsBasedOnDistanceToOcclusalPlane(ref Vector3 point1, ref Vector3 point2, ref Vector3 point3, Common.Plane occlusalP) { double dist1 = occlusalP.Distance2Point(point1); double dist2 = occlusalP.Distance2Point(point2); double dist3 = occlusalP.Distance2Point(point3); if (dist1 > dist2 && dist2 > dist3) { //points are in correct order } else { if (dist1 < dist2) { Vector3 t = point1; point1 = point2; point2 = t; double d = dist1; dist1 = dist2; dist2 = d; } if (dist2 < dist3) { Vector3 t = point3; point3 = point2; point2 = t; double d = dist3; dist3 = dist2; dist2 = d; } if (dist1 < dist2) { Vector3 t = point1; point1 = point2; point2 = t; double d = dist1; dist1 = dist2; dist2 = d; } } }
public int PointCompareOnDistanceToOcclusalPlane(Vector3 a, Vector3 b) { double da = tempOcclusalPlaneForPointCompareSort.Distance2Point(a); double db = tempOcclusalPlaneForPointCompareSort.Distance2Point(b); if (da > db) { return(-1); } else if (da == db) { return(0); } else { return(1); } }
void CalculateCurveFit(Common.Vbo handle) { int num = handle.selectedVertices.Count; var pointsMat = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(num, 3); var points = new Vector3[num]; //get selected points for (int i = 0; i < num; i++) { pointsMat[i, 0] = handle.verticesData.vertices[handle.selectedVertices[i]].X; pointsMat[i, 1] = handle.verticesData.vertices[handle.selectedVertices[i]].Y; pointsMat[i, 2] = handle.verticesData.vertices[handle.selectedVertices[i]].Z; points[i] = handle.verticesData.vertices[handle.selectedVertices[i]]; } //find the plane which holds the least distance to all ponits using SVD Common.Plane curve_plane = NormalPlane(pointsMat); curve_plane.valid = true; Planes[GetSelectedVbOIndex() - 1][CURVEPLANE_INDEX] = curve_plane; if (Planes[GetSelectedVbOIndex() - 1][OCCLUSALPLANE_INDEX] != null && Planes[GetSelectedVbOIndex() - 1][OCCLUSALPLANE_INDEX].valid) { double angle = Planes[GetSelectedVbOIndex() - 1][CURVEPLANE_INDEX].Angle2Plane(Planes[0][OCCLUSALPLANE_INDEX]); lb_curve2occlusalPlane.Text = "Angle to Occlusal Plane: " + angle.ToString("F2"); } //calculate sum of distances to this plane double sumOfSquaredDistances = 0; double[] z_distances = new double[num]; for (int i = 0; i < num; ++i) { double d = curve_plane.Distance2Point(points[i]); z_distances[i] = d; sumOfSquaredDistances += Math.Pow(d, 2); } double rmse_z = Math.Sqrt(sumOfSquaredDistances / num); // find the x and y direction on the plane var p0 = new Vector3(curve_plane.ProjectPointOnPlane(new Vector3(0, 0, 0))); var px = new Vector3(curve_plane.ProjectPointOnPlane(points[num - 1] - points[0])); //new Vector3(1, 0, 0))); var py = new Vector3(curve_plane.ProjectPointOnPlane(Vector3.Cross(curve_plane.GetNormal(), px))); //new Vector3(0, 1, 0))); var pvx = (px - p0); var pvy = (py - p0); pvx.Normalize(); pvy.Normalize(); //project 3D points on the plane and create a 2D point set var points2DX = new double[num]; var points2DY = new double[num]; for (int i = 0; i < num; ++i) { points2DX[i] = Vector3.Dot(pvx, points[i] - p0); points2DY[i] = Vector3.Dot(pvy, points[i] - p0); } //check and fix inverse y mode if (points2DY[0] < points2DY[points2DY.Length / 2]) { points2DY = points2DY.Select(el => - el).ToArray(); } //move points to fit on the screen var xMin = points2DX.Min(); var yMin = points2DY.Min(); for (int i = 0; i < num; ++i) { points2DX[i] -= xMin; points2DY[i] -= yMin; points2DX[i] += 10; points2DY[i] += 10; } //drawing projected points var graphics = pl_curveFit.CreateGraphics(); graphics.Clear(Color.White); for (int i = 0; i < num; ++i) { DrawPoint2DOnControl(graphics, new PointF((float)points2DX[i], (float)points2DY[i]), Color.Black, 4); } //DrawPoint2DOnControl(graphics, new PointF((float)10, (float)20), Color.Red, 10); //change of parameter var diffx = Diff(points2DX); var diffy = Diff(points2DY); diffx = diffx.Select(el => el * el).ToArray(); diffy = diffy.Select(el => el * el).ToArray(); double[] t = new double[num]; for (int i = 0; i < num; ++i) { if (i > 0) { t[i] = Math.Sqrt(diffx[i] + diffy[i]) + t[i - 1]; } else { t[i] = Math.Sqrt(diffx[i] + diffy[i]); } } //fit curve params int deg = (int)nUpDown_order.Value; FitFunction fitFunction = config.fitFunction; //fit x and y to t DenseVector Wx; DenseVector Wy; try { //f = Fit.Polynomial(xdata, ydata, order); Wx = FitLeastSquaresBasis(t, points2DX, fitFunction, deg); Wy = FitLeastSquaresBasis(t, points2DY, fitFunction, deg); } catch (Exception e) { Common.Logger.Log("MainForm", "curveFit.cs", "CalculateCurveFit", e.Message); MessageBox.Show("The order of polynomial is too high and " + "results are unstable. Please use a lower " + "degree polynomial.", "Error in Curve Fitting"); return; } // create polynomial curve struct (for later to match to the wire brackets) CurveFit[GetSelectedVbOIndex() - 1].CoeffsX = Wx; CurveFit[GetSelectedVbOIndex() - 1].CoeffsY = Wy; CurveFit[GetSelectedVbOIndex() - 1].degree = deg; CurveFit[GetSelectedVbOIndex() - 1].minT = t.Min() - 5; CurveFit[GetSelectedVbOIndex() - 1].maxT = t.Max() + 5; CurveFit[GetSelectedVbOIndex() - 1].fitFunction = fitFunction; //calculate xx and yy //const double stepSize = 0.01; //double mint = t.Min(); //double maxt = t.Max(); //int numCurve = (int)((maxt - mint) / stepSize); //double[] tt = new double[numCurve]; //for (int i = 0; i < numCurve; i++) //{ // tt[i] = mint + stepSize * i; //} //var TT = MakeBasis(tt, fitFunction, deg); //var XX = TT * Wx; //var YY = TT * Wy; Tuple <DenseVector, DenseVector> tempTuple = ParametricPolynomial_to_2DPoints(CurveFit[GetSelectedVbOIndex() - 1]); DenseVector XX = tempTuple.Item1; DenseVector YY = tempTuple.Item2; DrawCurve(graphics, XX, YY, Color.Blue); //least squares //DenseVector W; //try //{ // //f = Fit.Polynomial(xdata, ydata, order); // W = FitLeastSquaresBasis(xdata, ydata, fitFunction, deg); //} //catch (Exception e) //{ // MessageBox.Show("The order of polynomial is too high and results are unstable. Please use a lower degree polynomial.", "Error in Curve Fitting"); // return; //} //draw the blue curve //DrawCurve(graphics, W, xdata.Min()-4, xdata.Max()+4, fitFunction, deg); //calculate Error //var rmse_xy = CalculatePerpendicularDistancePointToCurve(xdata, ydata, W, fitFunction, deg, graphics); double[] xy_distances; int[] closestPointsIndexOnCurve; var rmse_xy = CalculatePerpendicularDistancePointToCurve(points2DX, points2DY, XX, YY, graphics, out xy_distances, out closestPointsIndexOnCurve); //fill the list view with point distances lv_pointDistance.Clear(); lv_pointDistance.Columns.Add("Pt", 20, HorizontalAlignment.Center); lv_pointDistance.Columns.Add("off", 40, HorizontalAlignment.Center); lv_pointDistance.Columns.Add("intra", 40, HorizontalAlignment.Center); for (int i = 0; i < num; ++i) { lv_pointDistance.Items.Add(new ListViewItem(new string[] { i.ToString(), z_distances[i].ToString("F2"), xy_distances[i].ToString("F2") })); } if (lv_pointDistance.Items.Count > 0) { lv_pointDistance.Items[0].Selected = true; } lb_curvefit_rmse_xy.Text = "in-plane RMS Error: " + rmse_xy.ToString("F2"); lb_curvefit_rmse_z.Text = "off-plane RMS Error: " + rmse_z.ToString("F2"); // write results to disk string resultsDir = "Results"; if (!System.IO.Directory.Exists(resultsDir)) { System.IO.Directory.CreateDirectory(resultsDir); } string save_path = System.IO.Path.Combine( resultsDir, "CurveFitResults_" + GetSelectedVbOIndex().ToString() + ".csv"); var writer = new StreamWriter(save_path); writer.WriteLine("Point Number,X,Y,Z,Projected on WirePlane (X),Projected on WirePlane (Y)," + "Nearest Point on 2D Wire (projected wire) (X),Nearest Point on 2D Wire(projected wire)(Y)," + "InPlane Distance,OffPlane Distance"); for (int i = 0; i < num; ++i) { writer.Write(i.ToString() + ","); writer.Write(points[i].X.ToString() + ","); writer.Write(points[i].Y.ToString() + ","); writer.Write(points[i].Z.ToString() + ","); writer.Write(points2DX[i].ToString() + ","); writer.Write(points2DY[i].ToString() + ","); writer.Write(XX[closestPointsIndexOnCurve[i]].ToString() + ","); writer.Write(YY[closestPointsIndexOnCurve[i]].ToString() + ","); writer.Write(xy_distances[i].ToString() + ","); writer.Write(z_distances[i].ToString()); writer.WriteLine(""); } writer.Close(); status.Text = "Results of curve fitting are saved in: " + save_path; }