double vecSqrLen(int n, double v[]) { return vecDot(n, v, v); }
double ConjGrad(int n, implicitMatrix *A, double x[], double b[], double epsilon, // how low should we go? int *steps) { int i, iMax; double alpha, beta, rSqrLen, rSqrLenOld, u; double *r = (double *) malloc(sizeof(double) * n); double *d = (double *) malloc(sizeof(double) * n); double *t = (double *) malloc(sizeof(double) * n); double *temp = (double *) malloc(sizeof(double) * n); vecAssign(n, x, b); vecAssign(n, r, b); A->matVecMult(x, temp); vecDiffEqual(n, r, temp); rSqrLen = vecSqrLen(n, r); vecAssign(n, d, r); i = 0; if (*steps) iMax = *steps; else iMax = MAX_STEPS; if (rSqrLen > epsilon) while (i < iMax) { i++; A->matVecMult(d, t); u = vecDot(n, d, t); if (u == 0) { printf("(SolveConjGrad) d'Ad = 0\n"); break; } // How far should we go? alpha = rSqrLen / u; // Take a step along direction d vecAssign(n, temp, d); vecTimesScalar(n, temp, alpha); vecAddEqual(n, x, temp); if (i & 0x3F) { vecAssign(n, temp, t); vecTimesScalar(n, temp, alpha); vecDiffEqual(n, r, temp); } else { // For stability, correct r every 64th iteration vecAssign(n, r, b); A->matVecMult(x, temp); vecDiffEqual(n, r, temp); } rSqrLenOld = rSqrLen; rSqrLen = vecSqrLen(n, r); // Converged! Let's get out of here if (rSqrLen <= epsilon) break; // Change direction: d = r + beta * d beta = rSqrLen/rSqrLenOld; vecTimesScalar(n, d, beta); vecAddEqual(n, d, r); } // free memory free(r); free(d); free(t); free(temp); *steps = i; return(rSqrLen); }
void Phy::resolveCollision(Contact* c) { Circle* circle = c->circle; Box* box = &c->axis.player[c->player]; box->position = Vec2(c->axis.position.x + c->axis.x_offset, c->axis.position.y + c->axis.player[c->player].y_offset); if (!c->axis.collide) return; Vec2 direction; if (circle->velocity.equal(0, 0)) { direction = c->normal.normalise().scale(-1); } else { direction = circle->velocity.normalise(); } if (vecDot(direction, c->normal) > 0) direction = direction.scale(-1); // return; //std::cout << "OKK\n"; Vec2 temp; //unused variable, delete from calculateNormal after debugging while (absf(c->penetration) > 1) { if (!direction.equal(0, 0)) { Vec2 error = direction.scale(c->penetration * -1); circle->position = vecSum(circle->position, error); } c->normal = calculateNormal(circle, box, &temp); c->penetration = circle->radius - sqrt(c->normal.lengthSquared()); } c->normal = c->normal.normalise(); Vec2 velAlongNormal = c->normal.scale(vecDot(c->normal, c->axis.velocity)); //std::cout << c->axis.velocity.x << " " << c->axis.velocity.y << " " << sqrt(c->axis.velocity.lengthSquared()) << " DING\n"; //std::cout << circle->velocity.x << " " << circle->velocity.y << " " << sqrt(circle->velocity.lengthSquared()) << "BANG\n"; //std::cout << velAlongNormal.x << " " << velAlongNormal.y << "DOOM\n"; circle->velocity = vecSum(c->normal.scale(vecDot(c->normal, circle->velocity) * -2), circle->velocity).scale(bounce_ratio); circle->velocity = vecSum(circle->velocity, velAlongNormal); if (c->normal.y == 0) { if (absf(c->axis.angular_velocity) < 10) { circle->velocity = circle->velocity.scale((450 + c->axis.angle)*(450 - c->axis.angle) / 202500); } else { // circle->velocity.y /= 1; } } //std::cout << c->normal.x << " " << c->normal.y << " " << sqrt(c->normal.lengthSquared()) << "BOOM\n"; //std::cout << circle->velocity.x << " " << circle->velocity.y << " " << sqrt(circle->velocity.lengthSquared()) << "BOING\n"; }