public bool CalculateOptimalTransformation()
{
var opo = new oapOptimizer(optTypeSelectorE.rotYtransXYZ);
opo.verbosity = oapOptimizer.verbosityE.info;
opo.addOapList(emlist);
opo.init();
opo.optimize();
rotvek_deg = opo.rotvek * 180 / Mathf.PI;
trnvek_met = opo.trnvek;
var plausible = true;
if (Mathf.Abs(rotvek_deg.y) > 10)
{
plausible = false;
}
if (Vector3.Magnitude(trnvek_met) > 1.0)
{
plausible = false;
}
RouteMan.Log("Opo status:" + opo.status + " bstval:" + opo.bstval + " bstidx:" + opo.bstiter + " Plausible:" + plausible);
RouteMan.Log("Optimized rotvek:" + rotvek_deg.ToString("f3"));
RouteMan.Log("Optimized trnvek:" + trnvek_met.ToString("f3"));
Debug.Log("Opo status:" + opo.status + " bstval:" + opo.bstval + " bstidx:" + opo.bstiter + " Plausible:" + plausible);
Debug.Log("Optimized rotvek:" + rotvek_deg.ToString("f3"));
Debug.Log("Optimized trnvek:" + trnvek_met.ToString("f3"));
return(plausible);
}
public void test1case(float rotrad, float trnx, float trny, float trnz, bool noise = false, float noisefak = 1)
{
var ranman = new Random(123);
var oaplist = new LinkedList <optimAnchorPoint>();
var o1 = new optimAnchorPoint("pt1", new Vk3(-1, 0, 0), new Vk3(-1, 0, 0));
var o2 = new optimAnchorPoint("pt2", new Vk3(0, 0, 1), new Vk3(0, 0, 1));
var o3 = new optimAnchorPoint("pt3", new Vk3(1, 0, 0), new Vk3(1, 0, 0));
oaplist.AddLast(o1);
oaplist.AddLast(o2);
oaplist.AddLast(o3);
float rx = 0, ry = 0, rz = 0;
var op = oaplist.First;
while (op != null)
{
var t1 = op.Value.target;
var t2 = Vk3.rotateY(rotrad, t1);
var t3 = new Vk3(t2.x + trnx, t2.y + trny, t2.z + trnz);
if (noise)
{
rx = noisefak * (float)(2 * ranman.NextDouble() - 1) / 10;
ry = noisefak * (float)(2 * ranman.NextDouble() - 1) / 10;
rz = noisefak * (float)(2 * ranman.NextDouble() - 1) / 10;
}
var t4 = new Vk3(t3.x + rx, t3.y + ry, t3.z + rz);
op.Value.target = t4;
op = op.Next;
}
// Add them all to the opimizer
var oapo = new oapOptimizer(optTypeSelectorE.rotYtransXYZ);
op = oaplist.First;
while (op != null)
{
oapo.addOap(op.Value);
op = op.Next;
}
oapo.verbose = false;
oapo.optimize();
var scavek = oapo.scavek;
var rotvek = oapo.rotvek;
var twopi = (float)(2 * Math.PI);
if (rotvek.y < 0)
{
rotvek.y += twopi;
}
if (twopi < rotvek.y)
{
rotvek.y -= twopi;
}
if (rotvek.y < 0)
{
rotvek.y += twopi; // can underflow twice
}
var trnvek = oapo.trnvek;
var rotdif = Math.Abs(rotvek.y - rotrad);
Assert.IsTrue(oapo.status == optStatusE.converged);
if (!noise)
{
Assert.IsTrue(Math.Abs(rotvek.y - rotrad) < 0.0005); // 0.028 degrees
}
Assert.IsTrue(Math.Abs(trnvek.x - trnx) < (noisefak * 0.1));
Assert.IsTrue(Math.Abs(trnvek.y - trny) < (noisefak * 0.1));
Assert.IsTrue(Math.Abs(trnvek.z - trnz) < (noisefak * 0.1));
Console.WriteLine(" rot:" + rotrad + " rot.y:" + rotvek.y + " diff:" + (rotrad - rotvek.y));
Console.WriteLine(" trnx:" + trnx + " trn.x:" + trnvek.x);
Console.WriteLine(" trny:" + trny + " trn.y:" + trnvek.y);
Console.WriteLine(" trnz:" + trnz + " trn.z:" + trnvek.z);
Console.WriteLine(oapo.status + " s:" + scavek + " r:" + rotvek + " t:" + trnvek +
" iter:" + oapo.iter + " fcalled:" + oapo.fcalled + " ferr:" + oapo.ferr);
}
