コード例 #1
0
        public bool FindAllPoints(double delta2)
        {
            double[,] Jacobian = new double[4, 4];
            double[,] F        = new double[4, 1];
            MyPoint p1 = new MyPoint();
            MyPoint p2 = new MyPoint();
            double  u, v, s, t;
            double  delta1 = delta2;
            double  delta  = delta2;

            MyPoint T   = new MyPoint();
            MyPoint P1u = new MyPoint();
            MyPoint P1v = new MyPoint();
            MyPoint P2u = new MyPoint();
            MyPoint P2v = new MyPoint();
            MyPoint P0u = new MyPoint();
            MyPoint P0v = new MyPoint();
            MyPoint P0s = new MyPoint();
            MyPoint P0t = new MyPoint();

            MyPoint k0        = new MyPoint();
            MyPoint k1        = new MyPoint();
            int     counter2  = 0;
            bool    smallFlag = true;
            int     counter   = 0;
            bool    isInit    = false;
            bool    isInit2   = true;

            while (BigFlag)
            {
                if (isInit && isInit2)
                {
                    k0.SetCoordinatesParam(parameters.First());
                }
                else
                {
                    k0.SetCoordinatesParam(parameters.Last());
                }
                u = k0.x; v = k0.y; s = k0.z; t = k0.w;

                //P0u.SetCoordinates(b1.ComputeDU(u, v));
                //P0v.SetCoordinates(b1.ComputeDV(u, v));
                //P0s.SetCoordinates(b2.ComputeDU(s, t));
                //P0t.SetCoordinates(b2.ComputeDV(s, t));

                smallFlag = true;

                P1u.SetCoordinates(b1.ComputeDU(u, v));
                P1v.SetCoordinates(b1.ComputeDV(u, v));
                P2u.SetCoordinates(b2.ComputeDU(s, t));
                P2v.SetCoordinates(b2.ComputeDV(s, t));
                T.SetCoordinates(MyMath.ScalarProduct(MyMath.ScalarProduct(P1u, P1v), MyMath.ScalarProduct(P2u, P2v)));
                T.Normalized();
                while (smallFlag)
                {
                    //if (T.x != 0 || T.y != 0 || T.z != 0)
                    DotNumerics.LinearAlgebra.Matrix m = new DotNumerics.LinearAlgebra.Matrix(4, 4);
                    // MyPoint Tu, Tv, Ts, Tt;
                    //  Tu = MyMath
                    MyPoint start = new MyPoint();
                    start.SetCoordinates(b1.ComputePointForParameter(u, v));

                    //     if(isI
                    k1.SetCoordinates(b2.ComputePointForParameter(s, t));
                    p1.SetCoordinates(b1.ComputePointForParameter(u, v));
                    p2.SetCoordinates(b2.ComputePointForParameter(s, t));

                    P1u.SetCoordinates(b1.ComputeDU(u, v));
                    P1v.SetCoordinates(b1.ComputeDV(u, v));
                    P2u.SetCoordinates(b2.ComputeDU(s, t));
                    P2v.SetCoordinates(b2.ComputeDV(s, t));


                    F[0, 0] = p1.x - p2.x;
                    F[1, 0] = p1.y - p2.y;
                    F[2, 0] = p1.z - p2.z;
                    if (isInit && isInit2)
                    {
                        F[3, 0] = (k1.x - points.First().x) * T.x + (k1.y - points.First().y) * T.y + (k1.z - points.First().z) * T.z - delta;
                        isInit2 = false;
                    }
                    else
                    {
                        F[3, 0] = (k1.x - points.Last().x) * T.x + (k1.y - points.Last().y) * T.y + (k1.z - points.Last().z) * T.z - delta;
                    }

                    Jacobian[0, 0] = P1u.x;                                         //b1.ComputeDU(u, v).x;
                    Jacobian[0, 1] = P1v.x;                                         //b1.ComputeDV(u, v).x;
                    Jacobian[0, 2] = -P2u.x;                                        // -b2.ComputeDU(s, t).x;
                    Jacobian[0, 3] = -P2v.x;                                        // -b2.ComputeDV(s, t).x;
                    Jacobian[1, 0] = P1u.y;                                         //    b1.ComputeDU(u, v).y;
                    Jacobian[1, 1] = P1v.y;                                         //    b1.ComputeDV(u, v).y;
                    Jacobian[1, 2] = -P2u.y;                                        // -b2.ComputeDU(s, t).y;
                    Jacobian[1, 3] = -P2v.y;                                        // -b2.ComputeDV(s, t).y;
                    Jacobian[2, 0] = P1u.z;                                         // b1.ComputeDU(u, v).z;
                    Jacobian[2, 1] = P1v.z;                                         // b1.ComputeDV(u, v).z;
                    Jacobian[2, 2] = -P2u.z;                                        // -b2.ComputeDU(s, t).z;
                    Jacobian[2, 3] = -P2v.z;                                        // -b2.ComputeDV(s, t).z;
                    Jacobian[3, 0] = (P1u.x) * T.x + (P1u.y) * T.y + (P1u.z) * T.z; // dodac odwrotnie
                    Jacobian[3, 1] = (P1v.x) * T.x + (P1v.y) * T.y + (P1v.z) * T.z;

                    Jacobian[3, 2] = 0; //wartosc punktu * pochodna po T
                    Jacobian[3, 3] = 0; // (0 - P0t.x) * T.x + (0 - P0t.y) * T.y + (0 - P0t.z) * T.z;

                    try
                    {
                        for (int i = 0; i < 4; i++)
                        {
                            for (int j = 0; j < 4; j++)
                            {
                                m[i, j] = Jacobian[i, j];
                            }
                        }
                        m           = m.Inverse();
                        double[,] l = new double[4, 4];
                        for (int i = 0; i < 4; i++)
                        {
                            for (int j = 0; j < 4; j++)
                            {
                                l[i, j] = m[i, j];
                            }
                        }
                        var res = MyMath.Multiply2(l, F);

                        k0.x -= res[0, 0];
                        k0.y -= res[1, 0];
                        k0.z -= res[2, 0];
                        k0.w -= res[3, 0];
                        k0.CheckParam();
                        u = k0.x; v = k0.y; s = k0.z; t = k0.w;
                    }
                    catch
                    {
                        smallFlag = false;
                        delta    /= 2;
                        delta1   /= 2;
                        counter   = 0;
                        if (Math.Abs(delta) < 0.00000000001)
                        {
                            if (isInit)
                            {
                                BigFlag = false;
                                CreateMap();
                                return(true);
                            }
                            else
                            {
                                isInit = true;
                                //                           delta = delta1;
                                //                         delta1 = -delta;
                            }
                        }
                    }

                    double dist  = MyMath.distance(b1.ComputePointForParameter(u, v), start);
                    double dist1 = MyMath.distance(b2.ComputePointForParameter(s, t), start);
                    dist  = Math.Min(dist, dist1);
                    dist1 = MyMath.distance(b1.ComputePointForParameter(u, v), b2.ComputePointForParameter(s, t));
                    dist  = Math.Min(dist, dist1);

                    counter++;

                    //if (dist < 0.00000001)
                    //{
                    //    counter2++;
                    //    if (counter2 < 100 && smallFlag)
                    //        delta = 0.05;
                    //}

                    if (dist < Math.Max(0.001, 0.000000000001) && smallFlag && (u >= 0 && u <= 1 && v >= 0 && v <= 1 && s >= 0 && s <= 1 && t >= 0 && t <= 1))
                    {
                        points.Add(new MyPoint());
                        points.Last().SetCoordinates(b1.ComputePointForParameter(u, v));
                        parameters.Add(new MyPoint(u, v, s, t));
                        smallFlag = false;
                        counter   = 0;
                        //if (MyMath.distance(points.First(), points.Last()) < 0.5 * delta )
                        //{
                        //    BigFlag = false;
                        //    CreateMap();
                        //    return true;
                        //}
                    }
                    //         if (points.Count > 200)
                    //             delta *= 8;

                    if (points.Count > 1000 && isInit2)
                    {
                        isInit = true;
                        // delta = delta1;
                        //delta1 = -delta;
                        smallFlag = false;
                    }
                    if (points.Count > 1000)
                    {
                        BigFlag = false;
                        CreateMap();
                        return(true);
                    }
                    if (Math.Abs(delta) < 0.0000000000000001)
                    {
                        if (isInit)
                        {
                            BigFlag = false;
                            CreateMap();
                            return(true);
                        }
                        else
                        {
                            isInit = true;
                            //delta = delta1;
                            //delta1 = -delta;
                            smallFlag = false;
                        }
                    }
                    if (counter > 700 || (counter > 30 && dist > 20))
                    {
                        smallFlag = false;
                        delta    /= 2;
                        BigFlag   = false;
                        delta1   /= 2;
                        counter   = 0;
                        //     if (delta < 0.001)
                        //         BigFlag = false;
                    }
                }
            }
            return(false);
        }
コード例 #2
0
        private void UpdateCurve()
        {
            int l = ControlPoints.Count;

            if (l > 2)
            {
                var MatrixA  = new BandMatrix(4 * (l - 1), 4 * (l - 1), 2, 2); //zamienic na 2x2
                var MatrixBx = new DotNumerics.LinearAlgebra.Matrix(4 * (l - 1), 1);
                var MatrixBy = new DotNumerics.LinearAlgebra.Matrix(4 * (l - 1), 1);
                var MatrixBz = new DotNumerics.LinearAlgebra.Matrix(4 * (l - 1), 1);

                for (int i = 0; i < l; i++)
                {
                    if (i == 0)  //pierwszy element
                    {
                        MatrixA[0, 2] = 2;
                        //MatrixA[0,3] = 6 * chords[i]; //blad
                        MatrixA[1, 0] = 1;
                        //MatrixA[1,1] = chords[i];
                        //MatrixA[1,2] = Math.Pow(chords[i], 2);
                        //MatrixA[1,3] = Math.Pow(chords[i], 3);
                        MatrixBx[1, 0] = ControlPoints[0].x;
                        MatrixBx[0, 0] = 0;
                        MatrixBy[1, 0] = ControlPoints[0].y;
                        MatrixBy[0, 0] = 0;
                        MatrixBz[1, 0] = ControlPoints[0].z;
                        MatrixBz[0, 0] = 0;
                    }
                    else if (i == ControlPoints.Count - 1)
                    {
                        //ostatni element
                        MatrixA[4 * (l - 1) - 2, 4 * (l - 1) - 4] = 1;
                        MatrixA[4 * (l - 1) - 2, 4 * (l - 1) - 3] = chords[i - 1];
                        MatrixA[4 * (l - 1) - 2, 4 * (l - 1) - 2] = Math.Pow(chords[i - 1], 2);
                        MatrixA[4 * (l - 1) - 2, 4 * (l - 1) - 1] = Math.Pow(chords[i - 1], 3);
                        MatrixA[4 * (l - 1) - 1, 4 * (l - 1) - 2] = 2;

                        MatrixBx[4 * (l - 1) - 2, 0] = ControlPoints[i].x;
                        MatrixBx[4 * (l - 1) - 1, 0] = 0;
                        MatrixBy[4 * (l - 1) - 2, 0] = ControlPoints[i].y;
                        MatrixBy[4 * (l - 1) - 1, 0] = 0;
                        MatrixBz[4 * (l - 1) - 2, 0] = ControlPoints[i].z;
                        MatrixBz[4 * (l - 1) - 1, 0] = 0;
                    }
                    //pozostale elementy
                    else
                    {
                        //C0 z poprzednim
                        MatrixA[2 + 4 * (i - 1), 4 * (i - 1)]     = 1;
                        MatrixA[2 + 4 * (i - 1), 4 * (i - 1) + 1] = chords[i - 1];
                        MatrixA[2 + 4 * (i - 1), 4 * (i - 1) + 2] = Math.Pow(chords[i - 1], 2);
                        MatrixA[2 + 4 * (i - 1), 4 * (i - 1) + 3] = Math.Pow(chords[i - 1], 3);
                        MatrixBx[2 + 4 * (i - 1), 0] = ControlPoints[i].x;
                        MatrixBy[2 + 4 * (i - 1), 0] = ControlPoints[i].y;
                        MatrixBz[2 + 4 * (i - 1), 0] = ControlPoints[i].z;

                        //ciaglosc C1
                        MatrixA[2 + 4 * (i - 1) + 1, 4 * (i - 1) + 1] = -1;
                        MatrixA[2 + 4 * (i - 1) + 1, 4 * (i - 1) + 2] = -2 * chords[i - 1];
                        MatrixA[2 + 4 * (i - 1) + 1, 4 * (i - 1) + 3] = -3 * Math.Pow(chords[i - 1], 2);
                        MatrixA[2 + 4 * (i - 1) + 1, 4 * (i) + 1]     = 1;
                        MatrixBx[2 + 4 * (i - 1) + 1, 0] = 0;
                        MatrixBy[2 + 4 * (i - 1) + 1, 0] = 0;
                        MatrixBz[2 + 4 * (i - 1) + 1, 0] = 0;

                        //ciaglosc C2
                        MatrixA[2 + 4 * (i - 1) + 2, 4 * (i - 1) + 2] = -2;
                        MatrixA[2 + 4 * (i - 1) + 2, 4 * (i - 1) + 3] = -6 * chords[i - 1];
                        MatrixA[2 + 4 * (i - 1) + 2, 4 * (i) + 2]     = 2;
                        MatrixBx[2 + 4 * (i - 1) + 2, 0] = 0;
                        MatrixBy[2 + 4 * (i - 1) + 2, 0] = 0;
                        MatrixBz[2 + 4 * (i - 1) + 2, 0] = 0;

                        //C0 z nastepnym
                        MatrixA[2 + 4 * (i - 1) + 3, 4 * i] = 1;
                        MatrixBx[2 + 4 * (i - 1) + 3, 0]    = ControlPoints[i].x;
                        MatrixBy[2 + 4 * (i - 1) + 3, 0]    = ControlPoints[i].y;
                        MatrixBz[2 + 4 * (i - 1) + 3, 0]    = ControlPoints[i].z;
                    }
                }

                LinearEquations leq = new LinearEquations();
                var             x   = leq.Solve(MatrixA, MatrixBx);
                var             y   = leq.Solve(MatrixA, MatrixBy);
                var             z   = leq.Solve(MatrixA, MatrixBz);
                _polyX.Clear();
                _polyY.Clear();
                _polyZ.Clear();
                for (int i = 0; i < l - 1; i++)
                {
                    _polyX.Add(new Polynomial(3));
                    _polyX[i][0] = x[4 * i, 0];
                    _polyX[i][1] = x[4 * i + 1, 0];
                    _polyX[i][2] = x[4 * i + 2, 0];
                    _polyX[i][3] = x[4 * i + 3, 0];

                    _polyY.Add(new Polynomial(3));
                    _polyY[i][0] = y[4 * i, 0];
                    _polyY[i][1] = y[4 * i + 1, 0];
                    _polyY[i][2] = y[4 * i + 2, 0];
                    _polyY[i][3] = y[4 * i + 3, 0];

                    _polyZ.Add(new Polynomial(3));
                    _polyZ[i][0] = z[4 * i, 0];
                    _polyZ[i][1] = z[4 * i + 1, 0];
                    _polyZ[i][2] = z[4 * i + 2, 0];
                    _polyZ[i][3] = z[4 * i + 3, 0];
                }
            }
        }