public void Optimize() { // Init FOV m_engine.m_fovOptimizer.SetFOV(20); double thetax, thetay, thetaz; Utility.NormalToRotation(topCircle.Normal, out thetax, out thetay, out thetaz); start_dire = ComputeStartDirecFromAxis(axis); float fov_init = m_projector.m_mainCamera.fieldOfView; // Optimal Solution double[] bndl = new double[] { 0, 0, 0, 0.001, 1 / fov_optimal_scale }; double[] x = new double[] { thetax, thetay, thetaz, topCircle.Radius, fov_init / fov_optimal_scale }; //add center double[] bndu = new double[] { Mathf.PI, Mathf.PI, Mathf.PI, 10, 179 / fov_optimal_scale }; int funNum = 4; double diffstep = 0.0002; double epsg = 0.000000000001; double epsf = 0; double epsx = 0; int maxits = 0; alglib.minlmstate state; alglib.minlmreport rep; //set timer System.Diagnostics.Stopwatch stopwatch = new System.Diagnostics.Stopwatch(); stopwatch.Start(); Inter_Num = 0; // Do the optima alglib.minlmcreatev(funNum, x, diffstep, out state); alglib.minlmsetbc(state, bndl, bndu); alglib.minlmsetcond(state, epsg, epsf, epsx, maxits); alglib.minlmoptimize(state, function_project, null, null); alglib.minlmresults(state, out x, out rep); stopwatch.Stop(); if (!m_engine.m_is_quiet) { Debug.Log("End fitting , Total time:" + stopwatch.ElapsedMilliseconds / 1000.0 + "s"); } // Update FOV m_engine.m_fovOptimizer.SetFOV((float)x[4] * fov_optimal_scale); fov_face = (float)x[4] * fov_optimal_scale; // Update Circle Vector3 center = Utility.NewVector3(topCircle.Center); Vector3 normal = Utility.RotationToNormal(x[0], x[1], x[2]); normal.Normalize(); //Vector3 normal = new Vector3((float)x[0], (float)x[1], (float)x[2]); normal.Normalize(); float radius = (float)x[3]; topCircle = new Circle3(center, radius, new Plane(normal, center)); CirclePoints_2d = m_projector.WorldToImage(topCircle.CirclePoints); m_renderEngine.DrawLine(axis.First(), axis.First() + 500 * start_dire.normalized, Color.red); m_renderEngine.DrawLine(topCircle.Center, topCircle.Center + 2 * topCircle.Normal, Color.blue); }
private bool HitMesh(Ray r, out int hitIndex, out Vector3 hitVertex, out GameObject go) { hitIndex = -1; go = null; hitVertex = new Vector3(); RaycastHit hit; if (!Physics.Raycast(r, out hit)) { return(false); } go = hit.transform.gameObject; Mesh mesh = go.GetComponent <MeshFilter>().mesh; MeshCollider meshCollider = hit.collider as MeshCollider; if (meshCollider == null || meshCollider.sharedMesh == null) { return(false); } Vector3[] vertices = mesh.vertices; int[] triangles = mesh.triangles; Vector3 p0 = vertices[triangles[hit.triangleIndex * 3 + 0]]; Vector3 p1 = vertices[triangles[hit.triangleIndex * 3 + 1]]; Vector3 p2 = vertices[triangles[hit.triangleIndex * 3 + 2]]; Transform hitTransform = hit.collider.transform; //p0 = hitTransform.TransformPoint(p0); //p1 = hitTransform.TransformPoint(p1); //p2 = hitTransform.TransformPoint(p2); List <Vector3> hitTrangle = new List <Vector3> { p0, p1, p2 }; double dist = double.MaxValue; hitVertex = new Vector3(); hitIndex = 0; for (int i = 0; i < hitTrangle.Count; i++) { double temp_dist = Vector3.Distance(hitTrangle[i], hit.point); if (temp_dist < dist) { dist = temp_dist; hitVertex = hitTrangle[i]; hitIndex = hit.triangleIndex * 3 + i; } } m_renderEngine.DrawLine(hitTransform.TransformPoint(p0), hitTransform.TransformPoint(p1), UnityEngine.Color.red); m_renderEngine.DrawLine(hitTransform.TransformPoint(p1), hitTransform.TransformPoint(p2), UnityEngine.Color.red); m_renderEngine.DrawLine(hitTransform.TransformPoint(p2), hitTransform.TransformPoint(p0), UnityEngine.Color.red); return(true); }
public void Optimize_allRects() { // Init FOV float fov_aver = 0; foreach (var fe in m_FEs) { fov_aver = fe.fov_face; } fov_aver /= m_FEs.Count; m_engine.m_fovOptimizer.SetFOV(fov_aver); float fov_init = m_projector.m_mainCamera.fieldOfView; // Count rect engines foreach (var fe in m_FEs) { if (!fe.isTopCircle_Current) { REs.Add(fe); } } // Params Prepare //foreach (var re in rectEngines) corners_list.Add(re.corner_points); int params_num = 4 * REs.Count + 1; double[] bndl = new double[params_num]; double[] x = new double[params_num]; double[] bndu = new double[params_num]; for (int i = 0; i < REs.Count; i++) { bndl[4 * i] = 0; bndl[4 * i + 1] = 0; bndl[4 * i + 2] = 0; bndl[4 * i + 3] = 0; x[4 * i] = REs[i].x0; x[4 * i + 1] = REs[i].x1; x[4 * i + 2] = REs[i].x2; x[4 * i + 3] = REs[i].x3; bndu[4 * i] = 20; bndu[4 * i + 1] = 20; bndu[4 * i + 2] = 20; bndu[4 * i + 3] = 20; } bndl[params_num - 1] = 1 / fov_optimal_scale; x[params_num - 1] = fov_aver / fov_optimal_scale; bndu[params_num - 1] = 179 / fov_optimal_scale; int funNum = 11; double diffstep = 0.0001; double epsg = 0.000000000001; double epsf = 0; double epsx = 0; int maxits = 0; alglib.minlmstate state; alglib.minlmreport rep; //set timer System.Diagnostics.Stopwatch stopwatch = new System.Diagnostics.Stopwatch(); stopwatch.Start(); Inter_Num = 0; alglib.minlmcreatev(funNum, x, diffstep, out state); alglib.minlmsetbc(state, bndl, bndu); alglib.minlmsetcond(state, epsg, epsf, epsx, maxits); alglib.minlmoptimize(state, function_multi, null, null); alglib.minlmresults(state, out x, out rep); stopwatch.Stop(); if (!m_engine.m_is_quiet) { Debug.Log("End fitting , Total time:" + stopwatch.ElapsedMilliseconds / 1000.0 + "s"); } m_engine.m_fovOptimizer.SetFOV((float)x[x.Length - 1] * fov_optimal_scale); for (int i = 0; i < REs.Count; i++) { Ray ray1 = m_projector.GenerateRay(REs[i].corner_points[0]); Ray ray2 = m_projector.GenerateRay(REs[i].corner_points[1]); Ray ray3 = m_projector.GenerateRay(REs[i].corner_points[2]); Ray ray4 = m_projector.GenerateRay(REs[i].corner_points[3]); Vector3 v1_3 = ray1.GetPoint((float)x[4 * i]); Vector3 v2_3 = ray2.GetPoint((float)x[4 * i + 1]); Vector3 v3_3 = ray3.GetPoint((float)x[4 * i + 2]); Vector3 v4_3 = ray4.GetPoint((float)x[4 * i + 3]); // project to a new plane List <Vector3> points = new List <Vector3>(); points.Add(v1_3); points.Add(v2_3); points.Add(v3_3); points.Add(v4_3); Vector3 mean = v1_3 + v2_3 + v3_3 + v4_3; mean /= 4; Vector3 normal = Utility.GetNormal(points); Plane newplane = new Plane(normal, mean); v1_3 = Utility.PlaneProjectPoint(newplane, v1_3); v2_3 = Utility.PlaneProjectPoint(newplane, v2_3); v3_3 = Utility.PlaneProjectPoint(newplane, v3_3); v4_3 = Utility.PlaneProjectPoint(newplane, v4_3); REs[i].topRect = new Quad(v1_3, v2_3, v3_3); REs[i].topRect.FitRect(); m_renderEngine.DrawRect3(REs[i].topRect, Color.green); m_renderEngine.DrawLine(REs[i].topRect.Center, REs[i].topRect.Center + 2 * REs[i].topRect.Normal, Color.blue); if (!m_engine.m_is_quiet) { Debug.Log("angle: " + Vector3.Angle(REs[i].topRect.CornerPoints3d[0] - REs[i].topRect.CornerPoints3d[1], REs[i].topRect.CornerPoints3d[2] - REs[i].topRect.CornerPoints3d[1])); } } }