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