bool CCollisionBox::IsColliding(CCollisionBox& other) { CVector3f _min(CVector3f(m_position.x - (1 * m_scale.x), m_position.y - (1 * m_scale.y), m_position.z - (1 * m_scale.z))); CVector3f _max(CVector3f(m_position.x + (1 * m_scale.x), m_position.y + (1 * m_scale.y), m_position.z + (1 * m_scale.z))); CVector3f oMin(CVector3f(other.GetPosition().x - (1 * other.GetScale().x), other.GetPosition().y - (1 *other.GetScale().y), other.GetPosition().z - (1 * other.GetScale().z))); CVector3f oMax(CVector3f(other.GetPosition().x + (1 * other.GetScale().x), other.GetPosition().y + (1 *other.GetScale().y), other.GetPosition().z + (1 * other.GetScale().z))); return(_min.x > oMin.x && _min.x < oMax.x && _max.y > oMin.y && _min.y < oMax.y && _max.z > oMin.z && _min.z < oMax.z); }
int optMesh(Mesh *mesh, double cn_tol, int precond) { const double cg_tol = 1e-2; const double sigma = 1e-4; const double beta0 = 0.25; const double beta1 = 0.80; const double tol1 = 1e-8; const int max_iter = 500; const int max_cg_iter = 100; Precond *prec; double *v; double *w; double *z; double *d; double *p; double *r; double *t; double norm_r, norm_g; double alpha, beta; double rzm1, rzm2; double obj, objn; const int nn = mesh->nn; int iter = 0, cg_iter; int nf = 1, ng = 1, nh = 0; int ferr; double m, m1, m2; double s, s1, s2; double t1, t2; struct rusage r0, r1; #ifdef DISPLAY_MAX double fmax; int fidx; #endif if (nn <= 0) { /* No nodes! Just return. */ return 0; } getrusage(RUSAGE_SELF, &r0); hMesh(mesh); if (gFcn(&obj, mesh)) { fprintf(stderr, "Invalid starting point.\n"); exit(-1); } prec = preCreate(precond, nn, mesh->nz); v = (double *)malloc(3*sizeof(double)*nn); w = (double *)malloc(3*sizeof(double)*nn); z = (double *)malloc(3*sizeof(double)*nn); d = (double *)malloc(3*sizeof(double)*nn); p = (double *)malloc(3*sizeof(double)*nn); r = (double *)malloc(3*sizeof(double)*nn); gatherMesh(v, mesh); norm_r = norm(mesh->g, nn); norm_g = sqrt(norm_r); getrusage(RUSAGE_SELF, &r1); m1 = (double) r1.ru_utime.tv_usec; m2 = (double) r0.ru_utime.tv_usec; m = m1 - m2; s1 = (double) r1.ru_utime.tv_sec; s2 = (double) r0.ru_utime.tv_sec; s = s1 - s2; t1 = s + m / MICROSEC; m1 = (double) r1.ru_stime.tv_usec; m2 = (double) r0.ru_stime.tv_usec; m = m1 - m2; s1 = (double) r1.ru_stime.tv_sec; s2 = (double) r0.ru_stime.tv_sec; s = s1 - s2; t2 = s + m / MICROSEC; #ifdef DISPLAY_MAX oMax(&fmax, &fidx, mesh); printf("%4d I %10.9e %5.4e %5.4e %5d " "usr: %5.4e sys: %5.4e tot: %5.4e\n", iter, obj, norm_g, fmax, fidx, t1, t2, t1+t2); #else printf("%4d I %10.9e %5.4e " "usr: %5.4e sys: %5.4e tot: %5.4e\n", iter, obj, norm_g, t1, t2, t1+t2); #endif while ((norm_g > cn_tol) && (iter < max_iter)) { getrusage(RUSAGE_SELF, &r0); ++iter; ++nh; hOnly(mesh); tryADOLC(mesh,nh); prec->calc(prec, mesh); memset(d, 0, 3*sizeof(double)*nn); negatef(r, mesh->g, nn); norm_g *= cg_tol; prec->apply(z, r, prec, mesh); memcpy(p, z, 3*sizeof(double)*nn); rzm1 = inner(z, r, nn); cg_iter = 0; while ((sqrt(norm_r) > norm_g) && (cg_iter < max_cg_iter)) { ++cg_iter; memset(w, 0, 3*sizeof(double)*nn); matmul(w, mesh, p); alpha = inner(p, w, nn); if (alpha <= 0.0) { printf("Direction of Negative Curvature\n"); if (1 == cg_iter) { axpy(d, d, 1.0, p, nn); } break; } alpha = rzm1 / alpha; axpy(d, d, alpha, p, nn); axpy(r, r, -alpha, w, nn); norm_r = norm(r, nn); prec->apply(z, r, prec, mesh); rzm2 = rzm1; rzm1 = inner(z, r, nn); beta = rzm1 / rzm2; axpy(p, z, beta, p, nn); } alpha = inner(mesh->g, d, nn); if (alpha > 0.0) { printf("Not descent.\n"); exit(-1); } else { alpha *= sigma; } beta = 1.0; /* Unrolled for better performance. Only do a gradient evaluation when */ /* beta = 1.0. Do not do this at other times. */ axpy(w, v, beta, d, nn); scatterMesh(mesh, w); ++nf; ++ng; ferr = gFcn(&objn, mesh); if ((!ferr && (obj - objn >= -alpha*beta - epsilonf)) || (!ferr && (sqrt(norm(mesh->g, nn)) < 100*cn_tol))) { /* Iterate is acceptable */ } else { if (ferr) { /* Function not defined at trial point */ beta *= beta0; } else { /* Iterate not acceptable */ beta *= beta1; } while (beta >= tol1) { axpy(w, v, beta, d, nn); scatterMesh(mesh, w); ++nf; if (oFcn(&objn, mesh)) { /* Function not defined at trial point */ beta *= beta0; } else if (obj - objn >= -alpha*beta - epsilonf) { /* Iterate is acceptable */ break; } else { /* Iterate not acceptable */ beta *= beta1; } } if (beta < tol1) { printf("Newton step not good\n"); exit(-1); } ++ng; gOnly(mesh); } /* Update the iterate (v = current point, w = new point) so swap */ t = v; v = w; w = t; obj = objn; norm_r = norm(mesh->g, nn); norm_g = sqrt(norm_r); getrusage(RUSAGE_SELF, &r1); m1 = (double) r1.ru_utime.tv_usec; m2 = (double) r0.ru_utime.tv_usec; m = m1 - m2; s1 = (double) r1.ru_utime.tv_sec; s2 = (double) r0.ru_utime.tv_sec; s = s1 - s2; t1 = s + m / MICROSEC; m1 = (double) r1.ru_stime.tv_usec; m2 = (double) r0.ru_stime.tv_usec; m = m1 - m2; s1 = (double) r1.ru_stime.tv_sec; s2 = (double) r0.ru_stime.tv_sec; s = s1 - s2; t2 = s + m / MICROSEC; #ifdef DISPLAY_MAX oMax(&fmax, &fidx, mesh); printf("%4d N %4d %10.9e %5.4e %5.4e %5.4e %5d " "usr: %5.4e sys: %5.4e tot: %5.4e\n", iter, cg_iter, obj, norm_g, beta, fmax, fidx, t1, t2, t1+t2); #else printf("%4d N %4d %10.9e %5.4e %5.4e usr: %5.4e sys: %5.4e tot: %5.4e\n", iter, cg_iter, obj, norm_g, beta, t1, t2, t1+t2); #endif } printf("Function evals: %4d\nGradient evals: %4d\nHessian evals: %4d\n", nf, ng, nh); prec->destroy(prec); free(v); free(w); free(z); free(d); free(p); free(r); return 0; }