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 bool DefineTeethPlaneAndTangentLine(Common.Vbo handle, ref Common.Plane toothAxisPlane,
int selectedToothIndex, Common.Plane[] Planes)
{
try
{
List <uint> sel;
Vector3[] verts;
sel = handle.selectedVertices;
verts = handle.verticesData.vertices;
if (sel.Count != 3)
{
MessageBox.Show("Exactly 3 points must be selected to define the tooth axis", "Wrong number of points");
return(false);
}
if (Planes[OCCLUSALPLANE_INDEX] == null || !Planes[OCCLUSALPLANE_INDEX].valid)
{
MessageBox.Show("Occlusal Plane should be defined prior to calculatin tooth Inclination.", "No Occlusal Plane");
return(false);
}
Vector3 point1 = verts[sel[0]];
Vector3 point2 = verts[sel[1]];
Vector3 point3 = verts[sel[2]];
//making sure points are in correct order--> lowest:point1, middle: point2, top: point3
//cause occulsal plane sometimes goes lower than the upper and even middle point,
//couldn't find a solution at the moment and decided to go with this:
//the operator should Select the points in the correct order: lowest=first, highest=last
//Sort3PointsBasedOnDistanceToOcclusalPlane(ref point1, ref point2, ref point3, Planes[OCCLUSALPLANE_INDEX]);
toothAxisPlane = DefineAxisPlane(point1, point2, point3, Planes[OCCLUSALPLANE_INDEX]);
Vector3[] projectedPoints = ProjectToothOnAxisPlane(handle, toothAxisPlane, Planes[OCCLUSALPLANE_INDEX]);
Vector3[] projectedPointsOnBuccal = FindAndSortPointsOnBuccalSide(projectedPoints, toothAxisPlane, Planes[OCCLUSALPLANE_INDEX]);
Vector3 BBPUser = point2;
Vector3 projectedBBPUser = toothAxisPlane.ProjectPointOnPlane(point2);
toothAxisPlane.projectedPointsOnAxisPlane = projectedPointsOnBuccal;
toothAxisPlane.tangentPoints = CalculateTangentLine(projectedPointsOnBuccal, toothAxisPlane, projectedBBPUser);
Vector3 tangentVector = toothAxisPlane.tangentPoints[1] - toothAxisPlane.tangentPoints[0];
toothAxisPlane.Inclination = Planes[OCCLUSALPLANE_INDEX].Angle2Vector(tangentVector); //radian
toothAxisPlane.Inclination = (toothAxisPlane.Inclination / (float)Math.PI) * 180;
toothAxisPlane.Inclination = CorrectInclination_BiggerORSmallerThan90(toothAxisPlane, point1, Planes[OCCLUSALPLANE_INDEX]);
toothAxisPlane.validInclination = true;
return(true);
}
catch (Exception e)
{
Common.Logger.Log("MainForm", "TeethComputations", "DefineTeethPlaneAndTangentLine", e.Message, true);
return(false);
}
}
private float CorrectInclination_BiggerORSmallerThan90(Common.Plane toothAxisPlane, Vector3 lowestPoint, Common.Plane occlusalPlane)
{
Vector3 upperP, lowerP;
int lowestSide = occlusalPlane.PointSide(lowestPoint);
if (occlusalPlane.PointSide(toothAxisPlane.tangentPoints[0]) == lowestSide &&
occlusalPlane.PointSide(toothAxisPlane.tangentPoints[1]) != lowestSide)
{
upperP = toothAxisPlane.tangentPoints[1];
lowerP = toothAxisPlane.tangentPoints[0];
}
else if (occlusalPlane.PointSide(toothAxisPlane.tangentPoints[0]) != lowestSide &&
occlusalPlane.PointSide(toothAxisPlane.tangentPoints[1]) == lowestSide)
{
upperP = toothAxisPlane.tangentPoints[0];
lowerP = toothAxisPlane.tangentPoints[1];
}
else //error
{
upperP = toothAxisPlane.tangentPoints[1];
lowerP = toothAxisPlane.tangentPoints[0];
Common.Logger.Log("MainForm", "TeethComputations", "CorrectInclination_BiggerORSmallerThan90",
"Logical Error: both points of toothAxisPlane.tangentPoints were located on the same of the plane", true);
}
Vector3 upperPProj = occlusalPlane.ProjectPointOnPlane(upperP);
Vector3 lowerPProj = occlusalPlane.ProjectPointOnPlane(lowerP);
float distUpperPProj2Center = (upperPProj - occlusalPlane.centerForDraw).LengthSquared;
float distLowerPProj2Center = (lowerPProj - occlusalPlane.centerForDraw).LengthSquared;
if (distUpperPProj2Center < distLowerPProj2Center)
{
return(toothAxisPlane.Inclination - 90);
}
else
{
return(90 - toothAxisPlane.Inclination);
}
}
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;
}
