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); }