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