private void RayTracein3DPlane(List <MyVector3> points, MyVector3 curp, MyVector3 curdire, MyVector3 sectionPlaneNormal, out int norInsec, out int notNorInsec)
{
// Param
double insecPs_Dist_theshold = 0.01;
double insecP_DistBetweenRay_theshold = 20;
MyVector3 cutNormal = sectionPlaneNormal.Cross(curdire).Normalize();
ray = new Line3(curp, cutNormal);
norInsec = -1; // Normal side
notNorInsec = -1; // Not Normal side
double dist_left = double.MaxValue;
double dist_right = double.MaxValue;
for (int i = 0; i < points.Count; i++)
{
double dist_temp = ray.DistanceToLine(points[i]);
if ((points[i] - curp).Dot(cutNormal) > 0)
{
// Normal side
if (dist_left > dist_temp)
{
dist_left = dist_temp;
norInsec = i;
}
}
else
{
// Not Normal side
if (dist_right > dist_temp)
{
dist_right = dist_temp;
notNorInsec = i;
}
}
}
if (norInsec == -1)
{
norInsec = notNorInsec;
System.Console.WriteLine("Warining: norInsec == -1");
return;
}
else if (notNorInsec == -1)
{
notNorInsec = norInsec;
System.Console.WriteLine("Warining: notNorInsec == -1");
return;
}
else if (norInsec == -1 && notNorInsec == -1)
{
System.Console.WriteLine("Error: Ray Tracein3DPlane, no intersection points");
return;
}
if (MyVector3.Distance(points[norInsec], points[notNorInsec]) < insecPs_Dist_theshold)
{
// this two intersection is too close, so let them become same one.s
System.Console.WriteLine("Warining: two intersection is too close");
norInsec = notNorInsec;
return;
}
if (ray.DistanceToLine(points[norInsec]) > insecP_DistBetweenRay_theshold ||
ray.DistanceToLine(points[notNorInsec]) > insecP_DistBetweenRay_theshold)
{
System.Console.WriteLine("Warining: two intersection is too far");
// this two intersection is too far, so let them become same one.s
norInsec = notNorInsec;
return;
}
}
public Line3 Estimate(List <MyVector3> points)
{
int iter = 200;
if (points.Count == 0)
{
return(null);
}
Random rd = new Random();
MyVector3 A = new MyVector3();
MyVector3 B = new MyVector3();
int maxpointinline = int.MinValue;
for (int i = 0; i < iter; i++)
{
A = points[rd.Next(points.Count)];
B = points[rd.Next(points.Count)];
if (A == B)
{
continue; //if can't generate line
}
Line3 testline = new Line3(A, (B - A).Normalize());
List <MyVector3> tempinliers = new List <MyVector3>();
int inlierscount = 0;
for (int j = 0; j < points.Count; j++)
{
if (points[j] != A && points[j] != B)
{
if (testline.DistanceToLine(points[j]) < thres)
{
tempinliers.Add(points[j]);
inlierscount++;
}
}
}
if (inlierscount > maxpointinline)
{
maxpointinline = inlierscount;
this.bestline = testline.Copy();
this.inliers.Clear();
foreach (MyVector3 p in tempinliers)
{
this.inliers.Add(p);
}
}
if (inlierscount >= probability * points.Count)
{
break;
}
}
if (this.inliers.Count != 0)
{
double mint = double.MaxValue;
double maxt = double.MinValue;
foreach (MyVector3 p in this.inliers)
{
double t = this.bestline.ComputeT(p);
if (t > maxt)
{
maxt = t;
}
if (t < mint)
{
mint = t;
}
}
bestline.SetPoints(mint, maxt);
}
if (bestline != null && bestline.startpoint.x != double.NaN && bestline.startpoint.y != double.NaN && bestline.startpoint.z != double.NaN &&
bestline.endpoint.x != double.NaN && bestline.endpoint.y != double.NaN && bestline.endpoint.z != double.NaN &&
(!bestline.startpoint.IsNull() && !bestline.endpoint.IsNull()))
{
return(bestline);
}
else
{
return(null);
}
}