void compute_distance(logdata_p logdata) {
float distance = 0;
int i;
for (i = 1; i < logdata->num_pos; i++)
distance += hypot3(
logdata->pos[i].x-logdata->pos[i-1].x,
logdata->pos[i].y-logdata->pos[i-1].y,
logdata->pos[i].z-logdata->pos[i-1].z);
fprintf(stderr, "Trajectory length = %.2f meters.\n", distance);
}
int LocalNonsmoothNewtonSolver(FrictionContactProblem* localproblem, double * R, int *iparam, double *dparam)
{
double mu = localproblem->mu[0];
double * qLocal = localproblem->q;
double * MLocal = localproblem->M->matrix0;
double Tol = dparam[0];
double itermax = iparam[0];
int i, j, k, inew;
// store the increment
double dR[3] = {0., 0., 0.};
// store the value fo the function
double F[3] = {0., 0., 0.};
// Store the (sub)-gradient of the function
double A[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double B[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
// Value of AW+B
double AWplusB[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
// Compute values of Rho (should be here ?)
double rho[3] = {1., 1., 1.};
#ifdef OPTI_RHO
computerho(localproblem, rho);
#endif
// compute the velocity
double velocity[3] = {0., 0., 0.};
for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * R[0] + qLocal[i]
+ MLocal[i + 1 * 3] * R[1] +
+ MLocal[i + 2 * 3] * R[2] ;
for (inew = 0 ; inew < itermax ; ++inew) // Newton iteration
{
//Update function and gradient
Function(R, velocity, mu, rho, F, A, B);
#ifndef AC_Generated
#ifndef NDEBUG
double Fg[3] = {0., 0., 0.};
double Ag[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double Bg[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double AWpB[9];
assert(*rho > 0. && *(rho + 1) > 0. && *(rho + 2) > 0.);
#ifdef AC_STD
frictionContact3D_AlartCurnierFunctionGenerated(R, velocity, mu, rho, Fg, Ag, Bg);
#endif
#ifdef AC_JeanMoreau
frictionContact3D_AlartCurnierJeanMoreauFunctionGenerated(R, velocity, mu, rho, Fg, Ag, Bg);
#endif
sub3(F, Fg);
sub3x3(A, Ag);
sub3x3(B, Bg);
assert(hypot3(Fg) <= 1e-7);
assert(hypot9(Ag) <= 1e-7);
assert(hypot9(Bg) <= 1e-7);
cpy3x3(A, Ag);
cpy3x3(B, Bg);
mm3x3(A, MLocal, AWpB);
add3x3(B, AWpB);
#endif
#endif
// compute -(A MLocal +B)
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
AWplusB[i + 3 * j] = 0.0;
for (k = 0; k < 3; k++)
{
AWplusB[i + 3 * j] -= A[i + 3 * k] * MLocal[k + j * 3];
}
AWplusB[i + 3 * j] -= B[i + 3 * j];
}
}
#ifdef AC_STD
#ifndef NDEBUG
scal3x3(-1., AWpB);
sub3x3(AWplusB, AWpB);
assert(hypot9(AWpB) <= 1e-7);
#endif
#endif
// Solve the linear system
solv3x3(AWplusB, dR, F);
// upate iterates
R[0] += dR[0];
R[1] += dR[1];
R[2] += dR[2];
// compute new residue
for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * R[0] + qLocal[i]
+ MLocal[i + 1 * 3] * R[1] +
+ MLocal[i + 2 * 3] * R[2] ;
Function(R, velocity, mu, rho, F, NULL, NULL);
dparam[1] = 0.5 * (F[0] * F[0] + F[1] * F[1] + F[2] * F[2]) / (1.0 + sqrt(R[0] * R[0] + R[1] * R[1] + R[2] * R[2])) ; // improve with relative tolerance
/* dparam[2] =0.0;
FrictionContact3D_unitary_compute_and_add_error( R , velocity,mu, &(dparam[2]));*/
if (verbose > 1) printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \n", inew, dparam[1]);
if (dparam[1] < Tol)
{
/* printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \t error2 = %.10e \n",inew,dparam[1], dparam[2]); */
return 0;
}
}// End of the Newton iteration
/* printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \t error2 = %.10e \n",inew,dparam[1], dparam[2]); */
return 1;
}
static PyObject *div_unit_grad(PyObject *self, PyObject *args)
{
PyObject* f = NULL;
npy_intp Nx, Ny, Nz;
int i, j, k, im1, im2, ip1, jm1, jm2, jp1, km1, km2, kp1;
npy_float64* f_data_dp = NULL;
npy_float64* r_data_dp = NULL;
npy_float32* f_data_sp = NULL;
npy_float32* r_data_sp = NULL;
double hx, hy, hz;
double hx2, hy2, hz2;
PyArrayObject* r = NULL;
double fip, fim, fjp, fjm, fkp, fkm, fijk;
double fimkm, fipkm, fjmkm, fjpkm, fimjm, fipjm, fimkp, fjmkp, fimjp;
double aim, bjm, ckm, aijk, bijk, cijk;
double Dxpf, Dxmf, Dypf, Dymf, Dzpf, Dzmf;
double Dxma, Dymb, Dzmc;
if (!PyArg_ParseTuple(args, "O(ddd)", &f, &hx, &hy, &hz))
return NULL;
hx2 = 2*hx; hy2 = 2*hy; hz2 = 2*hz;
if (!PyArray_Check(f))
{
PyErr_SetString(PyExc_TypeError,"first argument must be array");
return NULL;
}
if (PyArray_NDIM(f) != 3)
{
PyErr_SetString(PyExc_TypeError,"array argument must have rank 3");
return NULL;
}
Nx = PyArray_DIM(f, 0);
Ny = PyArray_DIM(f, 1);
Nz = PyArray_DIM(f, 2);
r = (PyArrayObject*)PyArray_SimpleNew(3, PyArray_DIMS(f), PyArray_TYPE(f));
if (PyArray_TYPE(f) == PyArray_FLOAT32)
{
f_data_sp = (npy_float32*)PyArray_DATA(f);
r_data_sp = (npy_float32*)PyArray_DATA(r);
for (i=0; i<Nx; ++i)
{
im1 = (i?i-1:0);
im2 = (im1?im1-1:0);
ip1 = (i+1==Nx?i:i+1);
for (j=0; j<Ny; ++j)
{
jm1 = (j?j-1:0);
jm2 = (jm1?jm1-1:0);
jp1 = (j+1==Ny?j:j+1);
for (k=0; k<Nz; ++k)
{
km1 = (k?k-1:0);
km2 = (km1?km1-1:0);
kp1 = (k+1==Nz?k:k+1);
fimjm = *((npy_float32*)PyArray_GETPTR3(f, im1, jm1, k));
fim = *((npy_float32*)PyArray_GETPTR3(f, im1, j, k));
fimkm = *((npy_float32*)PyArray_GETPTR3(f, im1, j, km1));
fimkp = *((npy_float32*)PyArray_GETPTR3(f, im1, j, kp1));
fimjp = *((npy_float32*)PyArray_GETPTR3(f, im1, jp1, k));
fjmkm = *((npy_float32*)PyArray_GETPTR3(f, i, jm1, km1));
fjm = *((npy_float32*)PyArray_GETPTR3(f, i, jm1, k));
fjmkp = *((npy_float32*)PyArray_GETPTR3(f, i, jm1, kp1));
fkm = *((npy_float32*)PyArray_GETPTR3(f, i, j, km1));
fijk = *((npy_float32*)PyArray_GETPTR3(f, i, j, k));
fkp = *((npy_float32*)PyArray_GETPTR3(f, i, j, kp1));
fjpkm = *((npy_float32*)PyArray_GETPTR3(f, i, jp1, km1));
fjp = *((npy_float32*)PyArray_GETPTR3(f, i, jp1, k));
fipjm = *((npy_float32*)PyArray_GETPTR3(f, ip1, jm1, k));
fipkm = *((npy_float32*)PyArray_GETPTR3(f, ip1, j, km1));
fip = *((npy_float32*)PyArray_GETPTR3(f, ip1, j, k));
Dxpf = (fip - fijk) / hx;
Dxmf = (fijk - fim) / hx;
Dypf = (fjp - fijk) / hy;
Dymf = (fijk - fjm) / hy;
Dzpf = (fkp - fijk) / hz;
Dzmf = (fijk - fkm) / hz;
aijk = hypot3(Dxpf, m(Dypf, Dymf), m(Dzpf, Dzmf));
bijk = hypot3(Dypf, m(Dxpf, Dxmf), m(Dzpf, Dzmf));
cijk = hypot3(Dzpf, m(Dypf, Dymf), m(Dxpf, Dxmf));
aijk = (aijk>FLOAT32_EPS?Dxpf / aijk:0.0);
bijk = (bijk>FLOAT32_EPS?Dypf / bijk: 0.0);
cijk = (cijk>FLOAT32_EPS?Dzpf / cijk:0.0);
Dxpf = (fijk - fim) / hx;
Dypf = (fimjp - fim) / hy;
Dymf = (fim - fimjm) / hy;
Dzpf = (fimkp - fim) / hz;
Dzmf = (fim - fimkm) / hz;
aim = hypot3(Dxpf, m(Dypf, Dymf), m(Dzpf, Dzmf));
aim = (aim>FLOAT32_EPS?Dxpf/aim:0.0);
Dxpf = (fipjm - fjm) / hx;
Dxmf = (fjm - fimjm) / hx;
Dypf = (fijk - fjm) / hy;
Dzpf = (fjmkp - fjm) / hz;
Dzmf = (fjm - fjmkm) / hz;
bjm = hypot3(Dypf, m(Dxpf, Dxmf), m(Dzpf, Dzmf));
bjm = (bjm>FLOAT32_EPS?Dypf/bjm:0.0);
Dxpf = (fipkm - fkm) / hx;
Dxmf = (fjm - fimkm) / hx;
Dypf = (fjpkm - fkm) / hy;
Dymf = (fkm - fjmkm) / hy;
Dzpf = (fijk - fkm) / hz;
ckm = hypot3(Dzpf, m(Dypf, Dymf), m(Dxpf, Dxmf));
ckm = (ckm>FLOAT32_EPS?Dzpf/ckm:0.0);
Dxma = (aijk - aim) / hx;
Dymb = (bijk - bjm) / hy;
Dzmc = (cijk - ckm) / hz;
//*((npy_float32*)PyArray_GETPTR3(r, i, j, k)) = Dxma/hx + Dymb/hy + Dzmc/hz;
*((npy_float32*)PyArray_GETPTR3(r, i, j, k)) = Dxma + Dymb + Dzmc;
}
}
}
}
else if (PyArray_TYPE(f) == PyArray_FLOAT64)
{
f_data_dp = (npy_float64*)PyArray_DATA(f);
r_data_dp = (npy_float64*)PyArray_DATA(r);
for (i=0; i<Nx; ++i)
{
im1 = (i?i-1:0);
im2 = (im1?im1-1:0);
ip1 = (i+1==Nx?i:i+1);
for (j=0; j<Ny; ++j)
{
jm1 = (j?j-1:0);
jm2 = (jm1?jm1-1:0);
jp1 = (j+1==Ny?j:j+1);
for (k=0; k<Nz; ++k)
{
km1 = (k?k-1:0);
km2 = (km1?km1-1:0);
kp1 = (k+1==Nz?k:k+1);
fimjm = *((npy_float64*)PyArray_GETPTR3(f, im1, jm1, k));
fim = *((npy_float64*)PyArray_GETPTR3(f, im1, j, k));
fimkm = *((npy_float64*)PyArray_GETPTR3(f, im1, j, km1));
fimkp = *((npy_float64*)PyArray_GETPTR3(f, im1, j, kp1));
fimjp = *((npy_float64*)PyArray_GETPTR3(f, im1, jp1, k));
fjmkm = *((npy_float64*)PyArray_GETPTR3(f, i, jm1, km1));
fjm = *((npy_float64*)PyArray_GETPTR3(f, i, jm1, k));
fjmkp = *((npy_float64*)PyArray_GETPTR3(f, i, jm1, kp1));
fkm = *((npy_float64*)PyArray_GETPTR3(f, i, j, km1));
fijk = *((npy_float64*)PyArray_GETPTR3(f, i, j, k));
fkp = *((npy_float64*)PyArray_GETPTR3(f, i, j, kp1));
fjpkm = *((npy_float64*)PyArray_GETPTR3(f, i, jp1, km1));
fjp = *((npy_float64*)PyArray_GETPTR3(f, i, jp1, k));
fipjm = *((npy_float64*)PyArray_GETPTR3(f, ip1, jm1, k));
fipkm = *((npy_float64*)PyArray_GETPTR3(f, ip1, j, km1));
fip = *((npy_float64*)PyArray_GETPTR3(f, ip1, j, k));
Dxpf = (fip - fijk) / hx;
Dxmf = (fijk - fim) / hx;
Dypf = (fjp - fijk) / hy;
Dymf = (fijk - fjm) / hy;
Dzpf = (fkp - fijk) / hz;
Dzmf = (fijk - fkm) / hz;
aijk = hypot3(Dxpf, m(Dypf, Dymf), m(Dzpf, Dzmf));
aijk = (aijk>FLOAT64_EPS?Dxpf / aijk:0.0);
bijk = hypot3(Dypf, m(Dxpf, Dxmf), m(Dzpf, Dzmf));
bijk = (bijk>FLOAT64_EPS?Dypf / bijk: 0.0);
cijk = hypot3(Dzpf, m(Dypf, Dymf), m(Dxpf, Dxmf));
cijk = (cijk>FLOAT64_EPS?Dzpf/cijk:0.0);
Dxpf = (fijk - fim) / hx;
Dypf = (fimjp - fim) / hy;
Dymf = (fim - fimjm) / hy;
Dzpf = (fimkp - fim) / hz;
Dzmf = (fim - fimkm) / hz;
aim = hypot3(Dxpf, m(Dypf, Dymf), m(Dzpf, Dzmf));
aim = (aim>FLOAT64_EPS?Dxpf/aim:0.0);
Dxpf = (fipjm - fjm) / hx;
Dxmf = (fjm - fimjm) / hx;
Dypf = (fijk - fjm) / hy;
Dzpf = (fjmkp - fjm) / hz;
Dzmf = (fjm - fjmkm) / hz;
bjm = hypot3(Dypf, m(Dxpf, Dxmf), m(Dzpf, Dzmf));
bjm = (bjm>FLOAT64_EPS?Dypf/bjm:0.0);
Dxpf = (fipkm - fkm) / hx;
Dxmf = (fjm - fimkm) / hx;
Dypf = (fjpkm - fkm) / hy;
Dymf = (fkm - fjmkm) / hy;
Dzpf = (fijk - fkm) / hz;
ckm = hypot3(Dzpf, m(Dypf, Dymf), m(Dxpf, Dxmf));
ckm = (ckm>FLOAT64_EPS?Dzpf/ckm:0.0);
Dxma = (aijk - aim) / hx;
Dymb = (bijk - bjm) / hy;
Dzmc = (cijk - ckm) / hz;
//*((npy_float64*)PyArray_GETPTR3(r, i, j, k)) = Dxma/hx + Dymb/hy + Dzmc/hz;
*((npy_float64*)PyArray_GETPTR3(r, i, j, k)) = Dxma + Dymb + Dzmc;
}
}
}
}
else
{
PyErr_SetString(PyExc_TypeError,"array argument type must be float64");
return NULL;
}
return Py_BuildValue("N", r);
}
/* Wrapper function to find the normal vector of a surface at a given point */
scope_ray raytrace_get_n(scope_ray pos, raytrace_geom geom, int surf){
/* Variable declarations */
scope_ray n;
double x = pos.x;
double y = pos.y;
double z = pos.z;
double f = geom.f;
double v = geom.v;
double b = geom.b;
double e = geom.e;
double esm1 = (e*e - 1.);
double R = geom.Rrc;
double alpha = geom.alpha;
double norm;
double xyfact;
double xg,yg,zg;
double Rt,xz_rad,beta;
/* Select surface, and calculate n */
/* PRIMARY MIRROR */
switch(surf){
case(OPTIC_PRI):
/* Normalization */
norm = sqrt(x*x + y*y + 4.*f*f);
n.x = -x / norm;
n.y = -y / norm;
n.z = 2.*f / norm;
break;
/* SECONDARY MIRROR */
case(OPTIC_SEC):
/* Normalization */
xyfact = (x*x + y*y)/esm1 + (b+f)*(b+f)/(4.*e*e);
norm = sqrt(esm1*esm1 + (x*x + y*y)/xyfact);
n.x = -x / sqrt(xyfact) / norm;
n.y = -y / sqrt(xyfact) / norm;
n.z = esm1 / norm;
break;
/* FOCAL PLANE */
case(OPTIC_FP):
n.x = 0.0;
n.y = 0.0;
n.z = 1.0;
/* SPHERICAL GRATING */
case(OPTIC_GRS):
xg = -R*sin(alpha);
yg = 0.0;
zg = -(v+b);
/* Normalization */
norm = hypot3(xg-x, yg-y, zg- z);
n.x = (xg - x) / norm;
n.y = (yg - y) / norm;
n.z = (zg - z) / norm;
break;
/* TOROIDAL GRATING */
case(OPTIC_GRT):
xg = -R*sin(alpha);
yg = 0.0;
zg = -(v+b);
beta = asin(0.108); // Optimize for 1300A & 1900A
Rt = R*(1. - cos(alpha)*cos(beta)); // R from torus equation
xz_rad = sqrt((x-xg)*(x-xg) + (z-zg)*(z-zg)); // Simplification
/* Normal vecotr */
n.x = -(xg - x) * (Rt - xz_rad) / xz_rad;
n.y = (yg - y);
n.z = -(zg - z) * (Rt - xz_rad) / xz_rad;
/* Normalization */
norm = hypot3(n.x, n.y, n.z);
n.x /= norm;
n.y /= norm;
n.z /= norm;
break;
default:
printf("Help, something's gone horribly wrong!\n");
}
return n;
}
