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_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_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);
}
private void btn_cutPreStep_Click(object sender, RoutedEventArgs e)
{
mainCanvas.ClearCanvas();
double[] La = new double[node_cnt * node_cnt];
for (int i = 0; i < node_cnt; i++)
{
int deg = 0;
for (int j = 0; j < node_cnt; j++)
{
La[i * node_cnt + j] = graph[i, j] ? -1 : 0;
deg += graph[i, j] ? 1 : 0;
}
La[i * node_cnt + i] = deg;
}
dc_Model model = new dc_Model();
dc_Var[] x = model.AddVarArray("X", -0.05, 1.05, node_cnt, ExUtility.RandRange);
dc_Var[] eij = model.AddVarArray("E", -0.05, 1.05, node_cnt * node_cnt, ExUtility.RandRange);
for (int i = 0; i < node_cnt; i++)
{
model.AddLE(model.Fn_Square(x[i]), x[i]);
model.AddGE(model.Fn_Square(x[i]), x[i]);
}
for (int i = 0; i < node_cnt * node_cnt; i++)
{
model.AddLE(model.Fn_Square(eij[i]), eij[i]);
model.AddGE(model.Fn_Square(eij[i]), eij[i]);
}
for (int i = 0; i < node_cnt; i++)
{
for (int j = i; j < node_cnt; j++)
{
if (i != j)
{
model.AddEQ(eij[i * node_cnt + j], eij[j * node_cnt + i]);
}
model.AddLE(eij[i * node_cnt + j], x[i]);
model.AddLE(eij[i * node_cnt + j], x[j]);
model.AddLE(model.Fn_Sum(x[i], x[j]), model.Fn_Sum(eij[i * node_cnt + j], eij[i * node_cnt + j], model.Fn_Const(1)));
}
}
model.SetObjective(model.Fn_Const(0), model.Fn_Dot(La, eij));
model.param_tau = tau;
model.param_tauM = tauM;
model.param_mu = mu;
model.param_ceil = ceil;
model.param_ceilby = ceilby;
model.param_timeout = 100;
model.Solve(attempts, ExUtility.RandRange);
model.CleanUp();
for (int i = 0; i < node_cnt; i++)
{
classes[i] = x[i].lastValue > 0.5;
}
int val = 0;
for (int i = 0; i < node_cnt; i++)
{
for (int j = i + 1; j < node_cnt; j++)
{
val += classes[i] != classes[j] && graph[i, j] ? 1 : 0;
}
}
dc_Utility.WriteLine(dc_Utility.c_stars);
dc_Utility.WriteLine("Group A size: " + x.Count(p => p.lastValue > 0.5));
dc_Utility.WriteLine("Group B size: " + x.Count(p => p.lastValue < 0.5));
dc_Utility.WriteLine("Max Cut: " + val);
dc_Utility.WriteLine(dc_Utility.c_stars);
DrawGraph(true);
}