private void btn_pathInit_Click(object sender, RoutedEventArgs e)
{
int t_int;
double t_double;
blocker_cnt = int.TryParse(txt_blockers.Text, out t_int) ? t_int : blocker_cnt;
blocker_segs = int.TryParse(txt_segs.Text, out t_int) ? t_int : blocker_segs;
attempts = int.TryParse(txt_attempts.Text, out t_int) ? t_int : attempts;
tau = double.TryParse(txt_tau.Text, out t_double) ? t_double : tau;
tauM = double.TryParse(txt_tauM.Text, out t_double) ? t_double : tauM;
mu = double.TryParse(txt_mu.Text, out t_double) ? t_double : mu;
ceil = double.TryParse(txt_ceil.Text, out t_double) ? t_double : ceil;
ceilby = int.TryParse(txt_ceilby.Text, out t_int) ? t_int : ceilby;
seed = int.TryParse(txt_seed.Text, out t_int) ? t_int : seed;
btn_pathStep.IsEnabled = true;
blocker_cntrs = new Point[blocker_cnt];
blocker_rads = new double[blocker_cnt];
mainCanvas.ClearCanvas();
ExUtility.SetSeed(seed);
double width = mainCanvas.Width;
for (int i = 0; i < blocker_rads.Length; i++)
{
blocker_rads[i] = ExUtility.RandRange(width / 10, width / 6);
blocker_cntrs[i] = new Point(ExUtility.RandRange(blocker_rads[i], width - blocker_rads[i]), ExUtility.RandRange(blocker_rads[i], width - blocker_rads[i]));
mainCanvas.DrawCircle(Colors.LightSalmon, Colors.LightSalmon, 0, blocker_cntrs[i], blocker_rads[i]);
}
}
private void btn_circInit_Click(object sender, RoutedEventArgs e)
{
int t_int;
double t_double;
circ_cnt = int.TryParse(txt_circleCnt.Text, out t_int) ? t_int : circ_cnt;
attempts = int.TryParse(txt_attempts.Text, out t_int) ? t_int : attempts;
tau = double.TryParse(txt_tau.Text, out t_double) ? t_double : tau;
tauM = double.TryParse(txt_tauM.Text, out t_double) ? t_double : tauM;
mu = double.TryParse(txt_mu.Text, out t_double) ? t_double : mu;
ceil = double.TryParse(txt_ceil.Text, out t_double) ? t_double : ceil;
ceilby = int.TryParse(txt_ceilby.Text, out t_int) ? t_int : ceilby;
seed = int.TryParse(txt_seed.Text, out t_int) ? t_int : seed;
circ_cntrs = new Point[circ_cnt];
circ_rads = new double[circ_cnt];
btn_circStep.IsEnabled = true;
mainCanvas.ClearCanvas();
ExUtility.SetSeed(seed);
double width = mainCanvas.Width;
for (int i = 0; i < circ_rads.Length; i++)
{
circ_rads[i] = ExUtility.RandRange(width / 20, width / 6);
circ_cntrs[i] = new Point(ExUtility.RandRange(circ_rads[i], width - circ_rads[i]), ExUtility.RandRange(circ_rads[i], width - circ_rads[i]));
mainCanvas.DrawCircle(Colors.LightSalmon.Fade(127), Colors.Gray, 2, circ_cntrs[i], circ_rads[i]);
}
}
private void btn_cutInit_Click(object sender, RoutedEventArgs e)
{
int t_int;
double t_double;
node_cnt = int.TryParse(txt_nodeCnt.Text, out t_int) ? t_int : node_cnt;
edge_prob = double.TryParse(txt_edgeProb.Text, out t_double) ? t_double : edge_prob;
attempts = int.TryParse(txt_attempts.Text, out t_int) ? t_int : attempts;
tau = double.TryParse(txt_tau.Text, out t_double) ? t_double : tau;
tauM = double.TryParse(txt_tauM.Text, out t_double) ? t_double : tauM;
mu = double.TryParse(txt_mu.Text, out t_double) ? t_double : mu;
ceil = double.TryParse(txt_ceil.Text, out t_double) ? t_double : ceil;
ceilby = int.TryParse(txt_ceilby.Text, out t_int) ? t_int : ceilby;
seed = int.TryParse(txt_seed.Text, out t_int) ? t_int : seed;
graph = new bool[node_cnt, node_cnt];
points = new Point[node_cnt];
classes = new bool[node_cnt];
double width = mainCanvas.Width;
mainCanvas.ClearCanvas();
ExUtility.SetSeed(seed);
double prox = 30;
List <Vector> disks = new List <Vector>();
while (disks.Count < node_cnt)
{
disks = ExUtility.PoissonDiskSample(width * 0.8, prox, 30);
disks.Shuffle();
prox *= 0.8;
}
btn_cutStep.IsEnabled = true;
btn_cutPreStep.IsEnabled = true;
for (int i = 0; i < node_cnt; i++)
{
for (int j = i + 1; j < node_cnt; j++)
{
if (ExUtility.rand.NextDouble() > edge_prob)
{
continue;
}
graph[i, j] = true;
graph[j, i] = true;
}
points[i] = new Point(width * 0.1 + disks[i].X, width * 0.1 + disks[i].Y);
}
DrawGraph(false);
}
private void btn_collideInit_Click(object sender, RoutedEventArgs e)
{
SetParameters();
btn_collidePreStep.IsEnabled = true;
btn_collideStep.IsEnabled = true;
double width = mainCanvas.Width;
mainCanvas.ClearCanvas();
ExUtility.SetSeed(col_seed);
startPos = new Point[col_cnt];
endPos = new Point[col_cnt];
startVel = new Point[col_cnt];
endVel = new Point[col_cnt];
List <Vector> disks = ExUtility.PoissonDiskSample(width * 0.8, col_dmin + 2, 30);
disks.Shuffle();
if (disks.Count < 2 * col_cnt)
{
return;
}
int cnt = 0;
// x [obj] [time] [x,y,vx,vy]
for (int i = 0; i < col_cnt; i++)
{
startPos[i] = new Point(width * 0.1 + disks[cnt].X, width * 0.1 + disks[cnt].Y); cnt++;
endPos[i] = new Point(width * 0.1 + disks[cnt].X, width * 0.1 + disks[cnt].Y); cnt++;
startVel[i] = new Point(ExUtility.RandRange(-col_velrng, col_velrng), ExUtility.RandRange(-col_velrng, col_velrng));
endVel[i] = new Point(ExUtility.RandRange(-col_velrng, col_velrng), ExUtility.RandRange(-col_velrng, col_velrng));
Point s1 = startPos[i];
Point e1 = new Point(s1.X + startVel[i].X, s1.Y + startVel[i].Y);
Point s2 = endPos[i];
Point e2 = new Point(s2.X + endVel[i].X, s2.Y + endVel[i].Y);
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, startPos[i]);
mainCanvas.DrawPoints(Colors.Orange, 8, endPos[i]);
}
}
private void btn_polyInit_Click(object sender, RoutedEventArgs e)
{
mainCanvas.ClearCanvas();
int t_int;
double t_double;
poly_cnt = int.TryParse(txt_polyCnt.Text, out t_int) ? t_int : poly_cnt;
poly_tri_cnt = int.TryParse(txt_polyTriCnt.Text, out t_int) ? t_int : poly_tri_cnt;
attempts = int.TryParse(txt_attempts.Text, out t_int) ? t_int : attempts;
tau = double.TryParse(txt_tau.Text, out t_double) ? t_double : tau;
tauM = double.TryParse(txt_tauM.Text, out t_double) ? t_double : tauM;
mu = double.TryParse(txt_mu.Text, out t_double) ? t_double : mu;
ceil = double.TryParse(txt_ceil.Text, out t_double) ? t_double : ceil;
ceilby = int.TryParse(txt_ceilby.Text, out t_int) ? t_int : ceilby;
seed = int.TryParse(txt_seed.Text, out t_int) ? t_int : seed;
btn_polyStep.IsEnabled = true;
double width = mainCanvas.Width;
polys = new Mesh[poly_cnt];
ExUtility.SetSeed(seed);
for (int i = 0; i < poly_cnt; i++)
{
Triangle[] ts = new Triangle[poly_tri_cnt];
ts[0] = new Triangle(new Point(ExUtility.RandRange(0, width), ExUtility.RandRange(0, width)),
new Point(ExUtility.RandRange(0, width), ExUtility.RandRange(0, width)),
new Point(ExUtility.RandRange(0, width), ExUtility.RandRange(0, width)));
for (int j = 1; j < ts.Length; j++)
{
ts[j] = new Triangle(new Point(ts[j - 1].points[1].X, ts[j - 1].points[1].Y),
new Point(ts[j - 1].points[2].X, ts[j - 1].points[2].Y),
new Point(ExUtility.RandRange(0, width), ExUtility.RandRange(0, width)));
}
polys[i] = new Mesh(ts);
}
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]);
}
}
private void btn_phaseInit_Click(object sender, RoutedEventArgs e)
{
int t_int;
double t_double;
phase_length = int.TryParse(txt_len.Text, out t_int) ? t_int : phase_length;
phase_obsmult = double.TryParse(txt_fact.Text, out t_double) ? t_double : phase_obsmult;
attempts = int.TryParse(txt_attempts.Text, out t_int) ? t_int : attempts;
tau = double.TryParse(txt_tau.Text, out t_double) ? t_double : tau;
tauM = double.TryParse(txt_tauM.Text, out t_double) ? t_double : tauM;
mu = double.TryParse(txt_mu.Text, out t_double) ? t_double : mu;
ceil = double.TryParse(txt_ceil.Text, out t_double) ? t_double : ceil;
ceilby = int.TryParse(txt_ceilby.Text, out t_int) ? t_int : ceilby;
seed = int.TryParse(txt_seed.Text, out t_int) ? t_int : seed;
btn_phaseStep.IsEnabled = true;
mainCanvas.ClearCanvas();
double width = mainCanvas.Width;
ExUtility.SetSeed(seed);
phase_signal = new Point[phase_length];
for (int i = 0; i < phase_length; i++)
{
phase_signal[i] = new Point(ExUtility.RandRange(-1, 1), ExUtility.RandRange(-1, 1));
}
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);
}
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();
}