private void btn_circStep_Click(object sender, RoutedEventArgs e)
{
mainCanvas.ClearCanvas();
double[] maxs = ExUtility.Constant(circ_cnt, mainCanvas.Width);
maxs.SubtractBy(circ_rads);
dc_Model model = new dc_Model();
dc_Var z = model.AddVar("Z", 0, mainCanvas.Width, mainCanvas.Width);
dc_Var[] x = model.AddVarArray("X", circ_rads, maxs, ExUtility.RandRange);
dc_Var[] y = model.AddVarArray("Y", circ_rads, maxs, ExUtility.RandRange);
model.SetObjective(z, model.Fn_Const(0));
for (int i = 0; i < circ_cnt; i++)
{
model.AddLE(model.Fn_Affine(1, circ_rads[i], x[i]), z);
model.AddLE(model.Fn_Affine(1, circ_rads[i], y[i]), z);
}
for (int i = 0; i < circ_cnt; i++)
{
for (int j = i + 1; j < circ_cnt; j++)
{
dc_Func dfx = model.Fn_Diff(x[i], x[j]);
dc_Func dfy = model.Fn_Diff(y[i], y[j]);
model.AddGE(model.Fn_SumSquares(dfx, dfy), model.Fn_Const((circ_rads[i] + circ_rads[j]) * (circ_rads[i] + circ_rads[j])));
}
}
model.param_tau = tau;
model.param_tauM = tauM;
model.param_mu = mu;
model.param_ceil = ceil;
model.param_ceilby = ceilby;
double bound = model.Solve(attempts, ExUtility.RandRange);
mainCanvas.DrawBounds(bound);
for (int i = 0; i < circ_cnt; i++)
{
mainCanvas.DrawCircle(Colors.LightSalmon.Fade(127), Colors.Gray, 2, new Point(x[i].lastValue, y[i].lastValue), circ_rads[i]);
}
model.CleanUp();
}
private void Collision_Test(bool avoidCollision)
{
SetParameters();
double width = mainCanvas.Width;
double[] stateLB = new double[] { 0, 0, double.MinValue, double.MinValue };
double[] stateUB = new double[] { width, width, double.MaxValue, double.MaxValue };
mainCanvas.ClearCanvas();
ExUtility.SetSeed(col_seed);
// u [obj] [time] [x,y]
// x [obj] [time] [x,y,vx,vy]
dc_Model model = new dc_Model();
dc_Var[][][] u = new dc_Var[col_cnt][][]; for (int i = 0; i < col_cnt; i++)
{
u[i] = new dc_Var[col_T][]; for (int j = 0; j < col_T; j++)
{
u[i][j] = model.AddVarArray("U", -col_maxpush, col_maxpush, 2, ExUtility.RandRange);
}
}
dc_Var[][][] x = new dc_Var[col_cnt][][]; for (int i = 0; i < col_cnt; i++)
{
x[i] = new dc_Var[col_T + 1][]; for (int j = 0; j <= col_T; j++)
{
x[i][j] = model.AddVarArray("X", stateLB, stateUB, ExUtility.RandRange);
}
}
for (int i = 0; i < col_cnt; i++)
{
model.AddEQ(x[i][0][0], model.Fn_Const(startPos[i].X));
model.AddEQ(x[i][0][1], model.Fn_Const(startPos[i].Y));
model.AddEQ(x[i][0][2], model.Fn_Const(startVel[i].X));
model.AddEQ(x[i][0][3], model.Fn_Const(startVel[i].Y));
model.AddEQ(x[i][col_T][0], model.Fn_Const(endPos[i].X));
model.AddEQ(x[i][col_T][1], model.Fn_Const(endPos[i].Y));
model.AddEQ(x[i][col_T][2], model.Fn_Const(endVel[i].X));
model.AddEQ(x[i][col_T][3], model.Fn_Const(endVel[i].Y));
}
model.SetObjective(model.Fn_Sum(Array.ConvertAll(u.SelectMany(p => p).ToArray(), p => model.Fn_L1Norm(p))), model.Fn_Const(0));
for (int i = 0; i < col_cnt; i++)
{
for (int j = 0; j < col_T; j++)
{
model.AddEQ(x[i][j + 1][0],
model.Fn_Dot(new double[] { col_A[0, 0], col_A[0, 1], col_A[0, 2], col_A[0, 3], col_B[0, 0], col_B[0, 1] },
x[i][j][0], x[i][j][1], x[i][j][2], x[i][j][3], u[i][j][0], u[i][j][1]));
model.AddEQ(x[i][j + 1][1],
model.Fn_Dot(new double[] { col_A[1, 0], col_A[1, 1], col_A[1, 2], col_A[1, 3], col_B[1, 0], col_B[1, 1] },
x[i][j][0], x[i][j][1], x[i][j][2], x[i][j][3], u[i][j][0], u[i][j][1]));
model.AddEQ(x[i][j + 1][2],
model.Fn_Dot(new double[] { col_A[2, 0], col_A[2, 1], col_A[2, 2], col_A[2, 3], col_B[2, 0], col_B[2, 1] },
x[i][j][0], x[i][j][1], x[i][j][2], x[i][j][3], u[i][j][0], u[i][j][1]));
model.AddEQ(x[i][j + 1][3],
model.Fn_Dot(new double[] { col_A[3, 0], col_A[3, 1], col_A[3, 2], col_A[3, 3], col_B[3, 0], col_B[3, 1] },
x[i][j][0], x[i][j][1], x[i][j][2], x[i][j][3], u[i][j][0], u[i][j][1]));
}
}
if (avoidCollision)
{
for (int i = 0; i <= col_T; i++)
{
for (int j = 0; j < col_cnt; j++)
{
for (int k = j + 1; k < col_cnt; k++)
{
dc_Func dfx = model.Fn_Diff(x[j][i][0], x[k][i][0]);
dc_Func dfy = model.Fn_Diff(x[j][i][1], x[k][i][1]);
model.AddGE(model.Fn_SumSquares(dfx, dfy), model.Fn_Const(col_dmin * col_dmin));
}
}
}
}
model.param_tau = tau;
model.param_tauM = tauM;
model.param_mu = mu;
model.param_ceil = ceil;
model.param_ceilby = ceilby;
model.Solve(avoidCollision ? attempts : 1, ExUtility.RandRange);
model.CleanUp();
Tuple <Point, double>[] pts = new Tuple <Point, double> [col_T + 1];
for (int i = 0; i < col_cnt; i++)
{
for (int j = 0; j <= col_T; j++)
{
double collide = -1;
for (int k = 0; k < col_cnt; k++)
{
if (k == i)
{
continue;
}
double dfx = x[i][j][0].lastValue - x[k][j][0].lastValue;
double dfy = x[i][j][1].lastValue - x[k][j][1].lastValue;
if (dfx * dfx + dfy * dfy < col_dmin * col_dmin - ExUtility.c_tolerance)
{
collide = Math.Sqrt(dfx * dfx + dfy * dfy);
}
}
pts[j] = new Tuple <Point, double>(new Point(x[i][j][0].lastValue, x[i][j][1].lastValue), collide);
}
for (int k = 0; k <= col_T; k++)
{
if (pts[k].Item2 >= 0)
{
mainCanvas.DrawPoints(Colors.Red.Fade(10), pts[k].Item2, pts[k].Item1);
}
}
mainCanvas.DrawPath(Colors.Transparent, Colors.LightSteelBlue, 0, 1.5, pts.Select(p => p.Item1).ToArray());
for (int k = 0; k <= col_T; k++)
{
mainCanvas.DrawPoints(pts[k].Item2 == -1 ? Colors.DarkSlateBlue.Fade((byte)(k / (double)col_T * 200 + 55)) : Colors.Red,
pts[k].Item2 == -1 ? 3 : 5, pts[k].Item1);
}
Point s1 = new Point(x[i][0][0].lastValue, x[i][0][1].lastValue);
Point e1 = new Point(s1.X + x[i][0][2].lastValue, s1.Y + x[i][0][3].lastValue);
Point s2 = new Point(x[i][col_T][0].lastValue, x[i][col_T][1].lastValue);
Point e2 = new Point(s2.X + x[i][col_T][2].lastValue, s2.Y + x[i][col_T][3].lastValue);
mainCanvas.DrawEdges(Colors.ForestGreen, 2, new Tuple <Point, Point>(s1, e1));
mainCanvas.DrawEdges(Colors.Orange, 2, new Tuple <Point, Point>(s2, e2));
mainCanvas.DrawPoints(Colors.ForestGreen, 8, pts[0].Item1);
mainCanvas.DrawPoints(Colors.Orange, 8, pts[col_T].Item1);
}
}
private void btn_pathStep_Click(object sender, RoutedEventArgs e)
{
mainCanvas.ClearCanvas();
double width = mainCanvas.Width;
dc_Model model = new dc_Model();
dc_Var L = model.AddVar("L", 0, width * width, ExUtility.RandRange);
dc_Var[] px = model.AddVarArray("Px", 0, width, blocker_segs + 1, ExUtility.RandRange);
dc_Var[] py = model.AddVarArray("Py", 0, width, blocker_segs + 1, ExUtility.RandRange);
model.AddEQ(px[0], model.Fn_Const(0));
model.AddEQ(py[0], model.Fn_Const(0));
model.AddEQ(px[blocker_segs], model.Fn_Const(width));
model.AddEQ(py[blocker_segs], model.Fn_Const(width));
for (int j = 1; j < blocker_segs; j++)
{
for (int i = 0; i < blocker_cnt; i++)
{
dc_Func dfx = model.Fn_Diff(px[j], model.Fn_Const(blocker_cntrs[i].X));
dc_Func dfy = model.Fn_Diff(py[j], model.Fn_Const(blocker_cntrs[i].Y));
model.AddGE(model.Fn_SumSquares(dfx, dfy), model.Fn_Const(blocker_rads[i] * blocker_rads[i]));
}
}
for (int j = 1; j <= blocker_segs; j++)
{
dc_Func dfx = model.Fn_Diff(px[j], px[j - 1]);
dc_Func dfy = model.Fn_Diff(py[j], py[j - 1]);
model.AddLE(model.Fn_SumSquares(dfx, dfy), L);
//model.AddGE(model.Fn_SumSquares(dfx, dfy), model.Fn_Const(width * Math.Sqrt(2) / (segments)));
}
model.SetObjective(L, model.Fn_Const(0));
model.param_tau = tau;
model.param_tauM = tauM;
model.param_mu = mu;
model.param_ceil = ceil;
model.param_ceilby = ceilby;
model.Solve(attempts, ExUtility.RandRange);
model.CleanUp();
for (int i = 0; i < blocker_rads.Length; i++)
{
mainCanvas.DrawCircle(Colors.LightSalmon, Colors.LightSalmon, 0, blocker_cntrs[i], blocker_rads[i]);
}
Point[] path = new Point[blocker_segs + 1];
for (int j = 0; j <= blocker_segs; j++)
{
path[j] = new Point(px[j].lastValue, py[j].lastValue);
}
mainCanvas.DrawPath(Colors.DarkSlateBlue, Colors.LightSteelBlue, 6, 2, path);
}
public dc_FGPair(dc_Func f, dc_Func g)
{
this.f = f;
this.g = g;
}
private void btn_polyStep_Click(object sender, RoutedEventArgs e)
{
mainCanvas.ClearCanvas();
double width = mainCanvas.Width;
int pairs = 0;
for (int i = 0; i < poly_cnt; i++)
{
for (int j = i + 1; j < poly_cnt; j++)
{
pairs += polys[i].tris.Length * polys[j].tris.Length;
}
}
dc_Model model = new dc_Model();
dc_Var z = model.AddVar("Z", 0, 200, ExUtility.RandRange);
dc_Var[][] r = new dc_Var[poly_cnt][]; for (int i = 0; i < poly_cnt; i++)
{
r[i] = model.AddVarArray("RotS_" + i, -10, 10, 2, ExUtility.RandRange);
}
dc_Var[][] t = new dc_Var[poly_cnt][]; for (int i = 0; i < poly_cnt; i++)
{
t[i] = model.AddVarArray("Trans_" + i, -width, width, 2, ExUtility.RandRange);
}
dc_Var[][][] px = new dc_Var[poly_cnt][][]; for (int i = 0; i < poly_cnt; i++)
{
px[i] = new dc_Var[polys[i].tris.Length][]; for (int j = 0; j < polys[i].tris.Length; j++)
{
px[i][j] = model.AddVarArray("Poly_" + i + "_Tri_" + j + "_Ptx", 0, width, 3, ExUtility.RandRange);
}
}
dc_Var[][][] py = new dc_Var[poly_cnt][][]; for (int i = 0; i < poly_cnt; i++)
{
py[i] = new dc_Var[polys[i].tris.Length][]; for (int j = 0; j < polys[i].tris.Length; j++)
{
py[i][j] = model.AddVarArray("Poly_" + i + "_Tri_" + j + "_Pty", 0, width, 3, ExUtility.RandRange);
}
}
dc_Var[][] w = new dc_Var[pairs][]; for (int i = 0; i < pairs; i++)
{
w[i] = model.AddVarArray("W_" + i, -1, 1, 2, ExUtility.RandRange);
}
dc_Var[] b = new dc_Var[pairs]; for (int i = 0; i < pairs; i++)
{
b[i] = model.AddVar("b_" + i, -width * Math.Sqrt(2), width * Math.Sqrt(2), ExUtility.RandRange);
}
dc_Func[] bp = new dc_Func[pairs];
dc_Func[] bm = new dc_Func[pairs];
model.SetObjective(model.Fn_Const(0), z);
for (int i = 0; i < poly_cnt; i++)
{
model.AddLE(z, model.Fn_L2NormSquared(r[i]));
for (int k = 0; k < polys[i].tris.Length; k++)
{
for (int j = 0; j < 3; j++)
{
model.AddEQ(px[i][k][j], model.Fn_Dot(new double[] { polys[i].tris[k].points[j].X, -polys[i].tris[k].points[j].Y, 1 }, r[i][0], r[i][1], t[i][0]));
model.AddEQ(py[i][k][j], model.Fn_Dot(new double[] { polys[i].tris[k].points[j].X, polys[i].tris[k].points[j].Y, 1 }, r[i][1], r[i][0], t[i][1]));
}
}
}
for (int i = 0; i < pairs; i++)
{
bp[i] = model.Fn_Affine(-2, -2, b[i]);
bm[i] = model.Fn_Affine(2, -2, b[i]);
}
int pcnt = 0;
for (int j = 0; j < poly_cnt; j++)
{
for (int k = j + 1; k < poly_cnt; k++)
{
for (int m = 0; m < polys[j].tris.Length; m++)
{
for (int n = 0; n < polys[k].tris.Length; n++)
{
for (int i = 0; i < 3; i++)
{
dc_Func[] pPw = new dc_Func[2];
dc_Func[] qPw = new dc_Func[2];
pPw[0] = model.Fn_Sum(px[j][m][i], w[pcnt][0]);
pPw[1] = model.Fn_Sum(py[j][m][i], w[pcnt][1]);
qPw[0] = model.Fn_Sum(px[k][n][i], w[pcnt][0]);
qPw[1] = model.Fn_Sum(py[k][n][i], w[pcnt][1]);
model.AddLE(model.Fn_SumSquares(pPw), model.Fn_Sum(bm[pcnt], model.Fn_L2NormSquared(w[pcnt][0], w[pcnt][1], px[j][m][i], py[j][m][i])));
model.AddLE(model.Fn_L2NormSquared(w[pcnt][0], w[pcnt][1], px[k][n][i], py[k][n][i]), model.Fn_Sum(bp[pcnt], model.Fn_SumSquares(qPw[0], qPw[1])));
}
pcnt++;
}
}
}
}
model.param_tau = tau;
model.param_tauM = tauM;
model.param_mu = mu;
model.param_ceil = ceil;
model.param_ceilby = ceilby;
model.Solve(attempts, ExUtility.RandRange);
dc_Utility.WriteLine("");
dc_Utility.WriteLine(dc_Utility.c_stars);
double scale = double.PositiveInfinity;
for (int i = 0; i < poly_cnt; i++)
{
double news = Math.Sqrt(r[i][0].lastValue * r[i][0].lastValue + r[i][1].lastValue * r[i][1].lastValue);
dc_Utility.WriteLine("Poly. " + i + " growth: " + news);
scale = Math.Min(scale, news);
}
dc_Utility.WriteLine(dc_Utility.c_stars);
double bound = 0;
for (int i = 0; i < poly_cnt; i++)
{
for (int k = 0; k < polys[i].tris.Length; k++)
{
for (int j = 0; j < 3; j++)
{
polys[i].tris[k].points[j].X = px[i][k][j].lastValue / scale;
polys[i].tris[k].points[j].Y = py[i][k][j].lastValue / scale;
bound = Math.Max(polys[i].tris[k].points[j].Y, Math.Max(polys[i].tris[k].points[j].X, bound));
}
}
}
mainCanvas.DrawBounds(bound);
//for(int i = 0; i < pairs; i++)
//{
// DrawPlane(w[i][0].value, w[i][1].value, b[i].value);
//}
ExUtility.SetSeed(245);
for (int i = 0; i < polys.Length; i++)
{
int[] cs = ExUtility.RandomInts(0, 255, 3);
DrawMeshes(mainCanvas, Color.FromRgb((byte)cs[0], (byte)cs[1], (byte)cs[2]), Color.FromRgb((byte)cs[0], (byte)cs[1], (byte)cs[2]), 1, polys[i]);
}
model.CleanUp();
//Utility.ResetSeed();
}
private void btn_phaseStep_Click(object sender, RoutedEventArgs e)
{
mainCanvas.ClearCanvas();
double width = mainCanvas.Width;
dc_Model model = new dc_Model();
dc_Var[][] x = new dc_Var[phase_length][]; for (int i = 0; i < phase_length; i++)
{
x[i] = model.AddVarArray("X", -1, 1, 2, ExUtility.RandRange);
}
int phase_obs = (int)(phase_length * phase_obsmult);
for (int i = 0; i < phase_obs; i++)
{
double[] aix = ExUtility.RandomDoubles(-1, 1, phase_length);
double[] aiy = ExUtility.RandomDoubles(-1, 1, phase_length);
double yx = 0;
double yy = 0;
for (int j = 0; j < phase_length; j++)
{
yx += phase_signal[j].X * aix[j] + phase_signal[j].Y * aiy[j];
yy += -phase_signal[j].X * aiy[j] + phase_signal[j].Y * aix[j];
}
double y = yx * yx + yy * yy;
dc_Func[] zx = new dc_Func[phase_length];
dc_Func[] zy = new dc_Func[phase_length];
for (int j = 0; j < phase_length; j++)
{
zx[j] = model.Fn_Dot(new double[] { aix[j], aiy[j] }, x[j]);
zy[j] = model.Fn_Dot(new double[] { -aiy[j], aix[j] }, x[j]);
}
dc_Func zxsum = model.Fn_Sum(zx);
dc_Func zysum = model.Fn_Sum(zy);
model.AddGE(model.Fn_Negative(model.Fn_SumSquares(zxsum, zysum)), model.Fn_Const(-y));
model.AddGE(model.Fn_SumSquares(zxsum, zysum), model.Fn_Const(y));
}
model.SetObjective(model.Fn_Const(0), model.Fn_Const(0));
model.param_tau = tau;
model.param_tauM = tauM;
model.param_mu = mu;
model.param_ceil = ceil;
model.param_ceilby = ceilby;
model.Solve(attempts, ExUtility.RandRange);
model.CleanUp();
double maxN = phase_signal.Max(p => ExUtility.NormComplex(p.X, p.Y));
Point[] pts = new Point[phase_length];
for (int i = 0; i < phase_length; i++)
{
pts[i] = new Point(i / (double)(phase_length - 1) * width, (ExUtility.NormComplex(phase_signal[i].X, phase_signal[i].Y) - maxN / 2) * width * 0.25 + width / 2);
}
mainCanvas.DrawPath(Colors.DarkSlateBlue, Colors.LightSteelBlue, 5, 2, pts);
for (int i = 0; i < phase_length; i++)
{
pts[i] = new Point(i / (double)(phase_length - 1) * width, (ExUtility.NormComplex(x[i][0].lastValue, x[i][1].lastValue) - maxN / 2) * width * 0.25 + width / 2);
}
mainCanvas.DrawPath(Colors.Red, Colors.LightSalmon, 5, 2, pts);
}
