void calcCore(Point &p1, Point &p2)
{
double xM = (xA + xB) / 2, yM = (yA + yB) / 2;
double A = xA - xB, B = yA - yB;
double C = -(A * xM + B * yM);
double m = -2 * xA, n = -2 * yA, T = xA * xA + yA * yA - r * r, s = 2 * r;
double p = m * m - s * s, q = n * n - s * s, l = 2 * m * T, v = 2 * n * T, u = 2 * m * n;
if(fabs(B) > EPS)
{
double k = -A / B, b = -C / B;
double x1, x2;
solveEq(p + q * k * k + u * k, 2 * q * k * b + l + v * k + u * b, q * b * b + v * b + T * T, x1, x2);
p1.x = x1, p2.x = x2;
p1.y = k * x1 + b, p2.y = k * x2 + b;
}
else
{
double x1 = -C / A, x2 = -C / A;
double y1, y2;
solveEq(q, v + u * x1, p * x1 * x1 + l * x1 + T * T, y1, y2);
p1.x = x1, p1.y = y1;
p2.x = x2, p2.y = y2;
}
}
void getIntersect(double A, double B, double C, Point &p1, Point &p2)
{
Point p[2];
if(fabs(B) < EPS)
{
double k = -C / A;
double y1, y2;
solveEq(1, 0, k * k - r * r, y1, y2);
p[0].x = p[1].x = k;
p[0].y = y1, p[1].y = y2;
}
else
{
double k = -A / B, b = -C / B;
double x1, x2;
solveEq(1 + k * k, 2 * b * k, b * b - r * r, x1, x2);
p[0].x = x1, p[1].x = x2;
p[0].y = k * x1 + b, p[1].y = k * x2 + b;
}
p1 = p[0], p2 = p[1];
}
void BSInterpolation::CalculateControlPoints()
{
const int lowerDiagNo = 2; //m1
const int upperDiagNo = 2; //m2
const int diagNo = 5;
int n = parameters.length();
float** diagMatrix = (float**)malloc(sizeof(float*) * (n+1));
float** lowerMatrix = (float**)malloc(sizeof(float*) * (n+1));
for(int i = 1; i < n+1;i++){
diagMatrix[i] = (float*)malloc(sizeof(float) * (diagNo+1));
lowerMatrix[i] = (float*)malloc(sizeof(float) * (lowerDiagNo+1));
}
unsigned long* index = (unsigned long*)malloc(sizeof(unsigned long) * (n+1));
initializeDiagonals(diagMatrix, n);
float evenOdd = 0;
LU(diagMatrix, n, lowerDiagNo, upperDiagNo, lowerMatrix, index, &evenOdd);
//get d table
float* dx = (float*)malloc(sizeof(float) * (n+1));
float* dy = (float*)malloc(sizeof(float) * (n+1));
float* dz = (float*)malloc(sizeof(float) * (n+1));
for (int i = 1; i<n+1; i++) {
dx[i] = markers[i-1]->point.x();
dy[i] = markers[i-1]->point.y();
dz[i] = markers[i-1]->point.z();
}
solveEq(diagMatrix, n , lowerDiagNo, upperDiagNo, lowerMatrix, index, dx);
solveEq(diagMatrix, n , lowerDiagNo, upperDiagNo, lowerMatrix, index, dy);
solveEq(diagMatrix, n , lowerDiagNo, upperDiagNo, lowerMatrix, index, dz);
for (int i = 1; i<n+1; i++) {
ControlPoints.append(Marker(dx[i], dy[i], dz[i], Qt::gray));
//markers.append(&ControlPoints[i-1]);
}
delete dx;
delete dy;
delete dz;
for(int i = 1; i < n+1; i++){
delete diagMatrix[i];
delete lowerMatrix[i];
}
delete index;
delete diagMatrix;
delete lowerMatrix;
//3DIAG SOLVER
/*
//assume degree 3
int p = 3;
//basis functions
int n1 = parameters.length();
QList<float> a; //low diag a
QList<float> b; //middle diag b
QList<float> c; //upper diag c
QList<float> c_original;
a.append(0);
b.append(getBSplineWeight(parameters[0], 0, p, knots, n1-1)); //Ni{ti}
c_original.append(getBSplineWeight(parameters[0], 1, p, knots, n1-1)); //Ni+1{ti}
for (int i = 1; i<n1-1; i++) {
b.append(getBSplineWeight(parameters[i], i, p, knots, n1-1)); //Ni{ti}
a.append(getBSplineWeight(parameters[i], i-1, p, knots, n1-1)); //Ni-1{ti}
c_original.append(getBSplineWeight(parameters[i], i+1, p, knots, n1-1)); //Ni+1{ti}
}
b.append(getBSplineWeight(parameters[n1-1], n1-1, p, knots, n1-1)); //Ni{ti}
a.append(getBSplineWeight(parameters[n1-1], n1-2, p, knots, n1-1)); //Ni-1{ti}
c_original.append(0);
//get d table
QList<float> dx; //datapoints.x
QList<float> dy;
QList<float> dz;
for (int i = 0; i<DataPoints.length(); i++) {
dx.append(DataPoints[i]->point.x());
dy.append(DataPoints[i]->point.y());
dz.append(DataPoints[i]->point.z());
}
//SOLVE D=N*P to get P - control points
//dx dy dz
c = c_original;
int n = n1-1;
c[0] /= b[0];
dx[0] /= b[0];
dy[0] /= b[0];
dz[0] /= b[0];
for (int i = 1; i < n; i++) {
c[i] /= b[i] - a[i]*c[i-1];
dx[i] = (dx[i] - a[i]*dx[i-1]) / (b[i] - a[i]*c[i-1]);
dy[i] = (dy[i] - a[i]*dy[i-1]) / (b[i] - a[i]*c[i-1]);
dz[i] = (dz[i] - a[i]*dz[i-1]) / (b[i] - a[i]*c[i-1]);
}
dx[n] = (dx[n] - a[n]*dx[n-1]) / (b[n] - a[n]*c[n-1]);
dy[n] = (dy[n] - a[n]*dy[n-1]) / (b[n] - a[n]*c[n-1]);
dz[n] = (dz[n] - a[n]*dz[n-1]) / (b[n] - a[n]*c[n-1]);
for (int i = n; i-- > 0;) {
dx[i] -= c[i]*dx[i+1];
dy[i] -= c[i]*dy[i+1];
dz[i] -= c[i]*dz[i+1];
}
for (int i = 0; i<DataPoints.length(); i++) {
ControlPoints.append(Marker(dx[i], dy[i], dz[i]));
markers.append(&ControlPoints[i]);
}
*/
}