private void UpdateChords() { chords.Clear(); for (int i = 0; i < ControlPoints.Count - 1; i++) { chords.Add(MyMath.distance(ControlPoints[i], ControlPoints[i + 1])); if (chords.Last() == 0) { chords[chords.Count - 1] = 0.0001F; } } }
public void CreateControlPoints(MyPoint start, MyPoint end, int dir) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { ControlPoints[i, j] = new MyPoint(); } } ControlPoints[0, 0].SetCoordinates(start); ControlPoints[0, 3].SetCoordinates(end); ControlPoints[0, 1].SetInterpolatedCoordinates(ControlPoints[0, 0], ControlPoints[0, 3], 0.66); ControlPoints[0, 2].SetInterpolatedCoordinates(ControlPoints[0, 0], ControlPoints[0, 3], 0.33); double distance = MyMath.distance(start, end); if (dir == 0) { for (int i = 1; i < 4; i++) { for (int j = 0; j < 4; j++) { ControlPoints[i, j].SetCoordinates(ControlPoints[i - 1, j].x, ControlPoints[i - 1, j].y - 0.33 * distance, ControlPoints[i - 1, j].z); } } } else if (dir == 1) { for (int i = 1; i < 4; i++) { for (int j = 0; j < 4; j++) { ControlPoints[i, j].SetCoordinates(ControlPoints[i - 1, j].x + 0.2 * distance, ControlPoints[i - 1, j].y + 0.2 * distance, ControlPoints[i - 1, j].z); } } } else { for (int i = 1; i < 4; i++) { for (int j = 0; j < 4; j++) { ControlPoints[i, j].SetCoordinates(ControlPoints[i - 1, j].x - 0.2 * distance, ControlPoints[i - 1, j].y + 0.2 * distance, ControlPoints[i - 1, j].z); } } } CalculatePoints(); }
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 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()); }