double getAngle (const double p0[3], const double p1[3], const double p2[3])
{
const double v0[3] = {p0[0] - p1[0], p0[1] - p1[1], p0[2] - p1[2]};
const double v1[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
// apply Cauchy Schwarz inequality
return acos (scpr (v0,v1,3) / (sqrt(scpr(v0,v0,3)) * sqrt(scpr (v1,v1,3))));
}
double calcProjPntToLineAndDists(const double p[3], const double a[3], const double b[3], double& lambda, double& d0)
{
// g (lambda) = a + lambda v, v = b-a
double v[3] = {b[0] - a[0], b[1] - a[1], b[2] - a[2]};
// orthogonal projection: (g(lambda)-p) * v = 0 => in order to compute lambda we define a help vector u
double u[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]};
lambda = scpr(u, v, 3) / scpr(v, v, 3);
// compute projected point
double proj_pnt[3];
for (size_t k(0); k < 3; k++)
proj_pnt[k] = a[k] + lambda * v[k];
d0 = sqrt(sqrDist(proj_pnt, a));
return sqrt(sqrDist(p, proj_pnt));
}
double sqrDist (const GEOLIB::Point* p0, const GEOLIB::Point* p1)
{
const double v[3] = {(*p1)[0] - (*p0)[0], (*p1)[1] - (*p0)[1], (*p1)[2] - (*p0)[2]};
return scpr (v, v, 3);
}
double sqrNrm2 (const GEOLIB::Point* p0)
{
return scpr (p0->getData(), p0->getData(), 3);
}
unsigned CG(CRSMatrix<double,unsigned> const * mat, double const * const b,
double* const x, double& eps, unsigned& nsteps)
{
unsigned N = mat->getNRows();
double *p, *q, *r, *rhat, rho, rho1 = 0.0;
p = new double[4* N];
q = p + N;
r = q + N;
rhat = r + N;
double nrmb = sqrt(scpr(b, b, N));
if (nrmb < std::numeric_limits<double>::epsilon()) {
blas::setzero(N, x);
eps = 0.0;
nsteps = 0;
delete[] p;
return 0;
}
// r0 = b - Ax0
mat->amux(D_MONE, x, r);
for (unsigned k(0); k < N; k++) {
r[k] = b[k] - r[k];
}
double resid = blas::nrm2(N, r);
if (resid <= eps * nrmb) {
eps = resid / nrmb;
nsteps = 0;
delete[] p;
return 0;
}
for (unsigned l = 1; l <= nsteps; ++l) {
#ifndef NDEBUG
std::cout << "Step " << l << ", resid=" << resid / nrmb << std::endl;
#endif
// r^ = C r
blas::copy(N, r, rhat);
mat->precondApply(rhat);
// rho = r * r^;
rho = scpr(r, rhat, N); // num_threads);
if (l > 1) {
double beta = rho / rho1;
// p = r^ + beta * p
unsigned k;
for (k = 0; k < N; k++) {
p[k] = rhat[k] + beta * p[k];
}
} else blas::copy(N, rhat, p);
// q = Ap
blas::setzero(N, q);
mat->amux(D_ONE, p, q);
// alpha = rho / p*q
double alpha = rho / scpr(p, q, N);
// x += alpha * p
blas::axpy(N, alpha, p, x);
// r -= alpha * q
blas::axpy(N, -alpha, q, r);
resid = sqrt(scpr(r, r, N));
if (resid <= eps * nrmb) {
eps = resid / nrmb;
nsteps = l;
delete[] p;
return 0;
}
rho1 = rho;
}
eps = resid / nrmb;
delete[] p;
return 1;
}