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