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);
            }
        }
Beispiel #3
0
        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);
        }
Beispiel #4
0
        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();
        }
Beispiel #5
0
        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);
        }