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