예제 #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);
        }
        public virtual List <MyPoint> getP(double u1, double v1)
        {
            double  step = 0.05;
            MyPoint p1   = new MyPoint();
            MyPoint p2   = new MyPoint();
            MyPoint p3   = new MyPoint();

            p1.SetCoordinates(this.ComputePointForParameter(u1, v1));
            p2.SetCoordinates(this.ComputePointForParameter(u1, v1 + step));
            p3.SetCoordinates(MyMath.Substract(p2, p1));
            p3.Normalized();

            //points.Clear();
            //_knots.Clear();
            double delta = 0.001f;
            //_knots2.Clear();
            int l = this.isCylinder ? 3 : 0;

            double step1, step2;
            int    m = DeBoorControlPoints.GetLength(0) + Degree + 1 + l;

            //for (int i = 0; i < m; i++)
            //    _knots.Add((double)i / (double)(m - 1));
            //m = DeBoorControlPoints.GetLength(1) + Degree + 1;
            //for (int i = 0; i < m; i++)
            //    _knots2.Add((double)i / (double)(m - 1));
            step1 = (_knots2[DeBoorControlPoints.GetLength(1)] - _knots2[Degree]) / (double)(samplesNumber - 1);
            step2 = (_knots[DeBoorControlPoints.GetLength(0) + l] - _knots[Degree]) / (double)(samplesNumber - 1);
            double         u, v;
            List <MyPoint> qPoints = new List <MyPoint>();

            v = _knots2[Degree]; //N
            double x = 0, y = 0, z = 0, a = 0, b = 0, c = 0;

            while (v - delta <= _knots2[DeBoorControlPoints.GetLength(1)])
            {
                for (int i = 0; i < mSize + l; i++)
                {
                    x = y = z = 0;
                    for (int j = 0; j < nSize; j++)
                    {
                        x += (DeBoorControlPoints[i % mSize, j].x * Nik2(v, j, Degree));
                        y += (DeBoorControlPoints[i % mSize, j].y * Nik2(v, j, Degree));
                        z += (DeBoorControlPoints[i % mSize, j].z * Nik2(v, j, Degree));
                    }
                    qPoints.Add(new MyPoint(x, y, z));
                    v += step1;
                }
            }

            List <MyPoint> qPointsDer = new List <MyPoint>();

            qPointsDer.Add(new MyPoint());
            for (int i = 0; i < qPoints.Count - 1; i++)
            {
                qPointsDer.Add(new MyPoint());
                qPointsDer[i].SetCoordinates(MyMath.Difference(qPoints[i + 1], qPoints[i]));
            }
            //qPointsDer[0].SetCoordinates(MyMath.AddDifference(qPointsDer[0], qPointsDer[1]));
            //policzyc krzywa stalego parametru
            //Qi = (delta P)
            _knotsDeriv.Clear();
            m = qPointsDer.Count + Degree - 1 + 1; //stopień = 2
            for (int i = 0; i < m; i++)
            {
                _knotsDeriv.Add((double)i / (double)(m - 1));
            }
            double t;

            t = u1;
            //        while (t < _knotsDeriv[qPointsDer.Count])
            //       {
            x = y = z = 0;
            for (int i = 0; i < qPointsDer.Count; i++)
            {
                x += (qPointsDer[i].x * Nik3(t, i, Degree - 1));
                y += (qPointsDer[i].y * Nik3(t, i, Degree - 1));
                z += (qPointsDer[i].z * Nik3(t, i, Degree - 1));
            }
            //        }
            //Ci = suma i = 0 .. n-1 Ni,p-1(u),Qi
            return(qPointsDer);
//            return p3;
        }
예제 #3
0
        public MyPoint FindCrossPoint(double u1, double v1, double s1, double t1)
        {
            points.Add(b1.ComputePointForParameter(u1, v1));
            points.Add(b2.ComputePointForParameter(s1, t1));

            /*   MyPoint grad = new MyPoint();
             * grad.x = MyMath.DotProduct(MyMath.Difference(b1.ComputePointForParameter(u1, v1), b2.ComputePointForParameter(s1, t1)).Multiply(2), b1.ComputeDU(u1, v1));
             * grad.y = MyMath.DotProduct(MyMath.Difference(b1.ComputePointForParameter(u1, v1), b2.ComputePointForParameter(s1, t1)).Multiply(2), b1.ComputeDV(u1, v1));
             * grad.z = MyMath.DotProduct(MyMath.Difference(b1.ComputePointForParameter(u1, v1), b2.ComputePointForParameter(s1, t1)).Multiply(2), b2.ComputeDU(s1, t1));
             * grad.w = MyMath.DotProduct(MyMath.Difference(b1.ComputePointForParameter(u1, v1), b2.ComputePointForParameter(s1, t1)).Multiply(2), b2.ComputeDV(s1, t1));
             *
             * double dist = MyMath.distance(b1.ComputePointForParameter(u1, v1), b2.ComputePointForParameter(s1,t1));
             * double alfa = 0.1;
             * u1 -= alfa*grad.x ;
             * v1 -= alfa*grad.y;
             * s1 -= alfa*grad.z;
             * t1 -= alfa * grad.w;
             * dist = MyMath.distance(b1.ComputePointForParameter(u1, v1), b2.ComputePointForParameter(s1, t1));
             */
            double step = 0.05f;
            int    counter = 0;
            double u, v, s, t;

            u = u1; v = v1; s = s1; t = t1;
            MyPoint pRight1     = new MyPoint();
            MyPoint pLeft1      = new MyPoint();
            double  distance    = MyMath.distance(points[0], points[1]);
            double  newDistance = 0;
            int     leftMax     = -1;

            MyPoint[] pLeft = new MyPoint[4];

            for (int i = 0; i < 4; i++)
            {
                pLeft[i] = new MyPoint();
            }
            MyPoint[] pRight = new MyPoint[4];

            for (int i = 0; i < 4; i++)
            {
                pRight[i] = new MyPoint();
            }

            while (counter < 10)
            {
                leftMax = -1;
                Console.WriteLine(distance);

                pRight1.SetCoordinates(b2.ComputePointForParameter(s, t));
                pLeft[0].SetCoordinates(b1.ComputePointForParameter(u + step, v));
                pLeft[1].SetCoordinates(b1.ComputePointForParameter(u - step, v));
                pLeft[2].SetCoordinates(b1.ComputePointForParameter(u, v + step));
                pLeft[3].SetCoordinates(b1.ComputePointForParameter(u, v - step));
                for (int i = 0; i < 4; i++)
                {
                    if (MyMath.distance(pRight1, pLeft[i]) < distance)
                    {
                        distance = MyMath.distance(pRight1, pLeft[i]);
                        leftMax  = i;
                    }
                }

                pLeft1.SetCoordinates(b1.ComputePointForParameter(u, v));
                pRight[0].SetCoordinates(b2.ComputePointForParameter(s + step, t));
                pRight[1].SetCoordinates(b2.ComputePointForParameter(s - step, t));
                pRight[2].SetCoordinates(b2.ComputePointForParameter(s, t + step));
                pRight[3].SetCoordinates(b2.ComputePointForParameter(s, t - step));
                for (int i = 0; i < 4; i++)
                {
                    if (MyMath.distance(pLeft1, pRight[i]) < distance)
                    {
                        distance = MyMath.distance(pLeft1, pRight[i]);
                        leftMax  = 4 + i;
                    }
                }

                switch (leftMax)
                {
                case 0: u += step;
                    break;

                case 1: u -= step;
                    break;

                case 2: v += step;
                    break;

                case 3: v -= step;
                    break;

                case 4: s += step;
                    break;

                case 5: s -= step;
                    break;

                case 6: t += step;
                    break;

                case 7: t -= step;
                    break;

                case -1:
                    step /= 2;
                    counter++;
                    break;
                }

                points[0].SetCoordinates(b1.ComputePointForParameter(u, v));
                points[1].SetCoordinates(b2.ComputePointForParameter(s, t));

                parameters.Clear();
                parameters.Add(new MyPoint(u, v, s, t));
            }
            points.RemoveAt(1);
            return(points.First());
        }
예제 #4
0
 public double[][] RotateX(double x)
 {
     Matrix = MyMath.Multiply(Matrix, CreateRotateXMatrix(x));
     return(Matrix);
 }
예제 #5
0
 public override List <MyPoint> UpdatePoints(int div)
 {
     Points.Clear();
     betaShift = 360 / (double)div;
     alfaShift = 360 / (double)div;
     for (int i = 0; i < div; i++)
     {
         for (int j = 0; j < div; j++)
         {
             Points.Add(new MyPoint((R + r * (double)Math.Cos(MyMath.Deg((j * betaShift)))) * (double)Math.Cos(MyMath.Deg((i * alfaShift))), (R + r * (double)Math.Cos(MyMath.Deg((j * betaShift)))) * (double)Math.Sin(MyMath.Deg((i * alfaShift))), r * (double)Math.Sin(MyMath.Deg((j * betaShift)))));
             Points.Last().Multiply(10);
         }
     }
     return(Points);
 }
 public double[][] RotateY(double x)
 {
     Matrix    = MyMath.Multiply(CreateRotateYMatrix(x), Matrix);
     RotMatrix = MyMath.Multiply(RotMatrix, Matrix);
     return(Matrix);
 }
 public double[][] Translate(double x, double y, double z)
 {
     Matrix   = MyMath.Multiply(Matrix, CreateTranslateMatrix(x, y, z));
     TrMatrix = MyMath.Multiply(TrMatrix, Matrix);
     return(Matrix);
 }
 public double[][] Scale(double x, double y, double z)
 {
     Matrix   = MyMath.Multiply(Matrix, CreateScaleMatrix(x, y, z));
     ScMatrix = MyMath.Multiply(ScMatrix, Matrix);
     return(Matrix);
 }