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