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