public void Sweep(Image <Gray, byte> trajectoryimg_)
{
for (int i = 0; i < trajectoryimg_.Height; i++)
{
for (int j = 0; j < trajectoryimg_.Width; j++)
{
if (trajectoryimg_[i, j].Intensity > 80)
{
trajpoints.Add(new MyVector2(j, i));
}
}
}
trajpoints = CurveFitting(trajpoints);
trajpoints = this.ResetPath(trajpoints, 10);
trajpoints_3d = this.ProjectTrajAccording2Profile(trajpoints);
body = new SweepMesh(this.topCircle, trajpoints_3d, null);
}
public void CylinderSnapping()
{
// Get Boundary2
if (boundaryPoints_2d == null)
{
boundaryPoints_2d = GetBoundaryPoints(mark);
}
List <MyVector2> boundary2 = ExtractOutline(edgeImage, boundaryPoints_2d);
// Project 2D edge points
//topCircle = new MyCircle(topCircle.Center, topCircle.Radius, -topCircle.Normal);
MyVector3 normal = topCircle.Normal.Cross(this.camera.target).Cross(topCircle.Normal);
MyPlane sectionPlane = new MyPlane(topCircle.Center, normal);
boundary3 = Proj2dToPlane(sectionPlane, boundary2);
topCircle = CiriFixTopCircle(topCircle, boundary3);
// UpdateCircleNormal
// foreach (var pbondary3 in pbondary3)
// {
// }
// if (topCircle.Center)
//{
//}
// Algorithm Init Params
double offset = topCircle.Radius / 50;
cur_p = topCircle.Center - offset * topCircle.Normal;
cur_dire = 1.0 * topCircle.Normal;
MyVector3 cur_dire_new = new MyVector3(cur_dire);
MyVector3 cur_p_new = new MyVector3(-1 * cur_p);
Insection1 = new MyVector3(1, 1, 1);
Insection2 = new MyVector3(0, 0, 0);
int norInsec = -1;
int notNorInsec = -1;
MyVector3 tangential1 = new MyVector3(1, 1, 1);
MyVector3 tangential2 = new MyVector3(1, 1, 1);
List <MyCircle> CircleLists = new List <MyCircle>();
CircleLists.Add(topCircle); // Fix first circle
int iter = 0;
double r = double.MaxValue;
System.Console.WriteLine(Insection1.Dot(tangential2));
System.Console.WriteLine(Math.Cos(2.0 / 3.0 * Math.PI));
int MaxInter = 1000;
GeneratedCenters = new List <MyVector3>();
List <double> radius = new List <double>();
List <double> weights = new List <double>();
List <MyVector3> dires = new List <MyVector3>();
while (--MaxInter > 0) //
{
if (Insection1 == Insection2) // 交点一直保持相同
{
System.Console.WriteLine("Warning: Insection is same!"); // 半径过小
break;
}
if (cur_dire.Dot(cur_dire_new) < 0) // 移动方向反向
{
System.Console.WriteLine("Warning: Move Direction！");
break;
}
if (cur_p + offset * cur_dire == cur_p_new) // 中心点没有移动
{
System.Console.WriteLine("Warning: Center not move!");
break;
}
RayTracein3DPlane(boundary3,
cur_p_new,
cur_dire_new.Cross(sectionPlane.Normal()),
sectionPlane.Normal(),
out norInsec,
out notNorInsec);
System.Console.WriteLine("{0} , {1}",
MyVector3.Distance(boundary3[norInsec], cur_p_new),
MyVector3.Distance(boundary3[notNorInsec], cur_p_new));
test1 = new Line3(boundary3[norInsec], cur_p_new - boundary3[norInsec]);
test2 = new Line3(boundary3[notNorInsec], cur_p_new - boundary3[notNorInsec]);
if (MyVector3.Distance(boundary3[norInsec], cur_p_new) < topCircle.Radius / 20 || // close to bottom
MyVector3.Distance(boundary3[notNorInsec], cur_p_new) < topCircle.Radius / 20)
{
System.Console.WriteLine("Warning: Close to bottom!");
break;
}
if (tangential1.Dot(tangential2) < Math.Cos(2.0 / 3.0 * Math.PI)) //切线相向
{
System.Console.WriteLine("Warning: tangential get oppsite direction!");
break;
}
if (r < 0.0001)
{
System.Console.WriteLine("Warning: Radius is too small!"); // 半径过小
break;
}
//if (MyVector3.Distance(cur_p, cur_p_new) )
//{
// System.Console.WriteLine("Warning: Radius is too small!"); // 半径过小
// break;
//}
if (iter != 0)
{
//offset = 1 / MyVector3.Distance(cur_p, cur_p_new) * 0.000001 + 0.5 * offset;
offset = topCircle.Radius / 20;
//System.Console.WriteLine("{0}", offset);
cur_dire = cur_dire_new;
cur_p = cur_p_new + offset * cur_dire;
CircleLists.Add(new MyCircle(cur_p, r, cur_dire));
// Get Data for Fit
double weight = Math.Abs(cur_dire_new.Dot(cur_dire));
GeneratedCenters.Add(cur_p_new);
weights.Add(weight);
radius.Add(r);
dires.Add(cur_dire);
}
// Step1: Get IntersectionPoitn
RayTracein3DPlane(boundary3, cur_p, cur_dire, sectionPlane.Normal(), out norInsec, out notNorInsec);
// Step2 : Get Two Local Tangential
Insection1 = boundary3[norInsec];
Insection2 = boundary3[notNorInsec];
tangential1 = GetLocalTangential(norInsec, boundary3, cur_dire);
tangential2 = GetLocalTangential(notNorInsec, boundary3, cur_dire);
// Visualization
setdirecLine = new Line3(cur_p, cur_dire);
setLine1 = new Line3(Insection1, tangential1);
setLine2 = new Line3(Insection2, tangential2);
// Step3 : Get New Cur Direction and Cur Point
cur_dire_new = (tangential1 + tangential2) / 2;
RayTracein3DPlane(boundary3, cur_p, cur_dire_new, sectionPlane.Normal(), out norInsec, out notNorInsec);
cur_p_new = (boundary3[norInsec] + boundary3[notNorInsec]) / 2;
r = 0.5 * MyVector3.Distance(boundary3[norInsec], boundary3[notNorInsec]);
iter++;
this.view.Refresh();
}
// Fit centers and radius;
GeneratedCenters = FittingCentersCurve(GeneratedCenters, weights);
int inter = 1;
while (inter-- > 0)
{
radius = FittRadius(radius);
}
// ReBuild Object
CircleLists.Clear();
CircleLists.Add(topCircle); // Fix first circle
for (int i = 0; i < GeneratedCenters.Count; i++)
{
CircleLists.Add(new MyCircle(GeneratedCenters[i], radius[i], dires[i]));
}
CurveCyliner = new SweepMesh(CircleLists);
}
