Above all, sorry for my bad english.
I make a physic engine based on sequential impulses but i have a problem of bouncing, i have debugged this for 2 weeks but I do not find the problem.
In my previous attemps, i try to solve the contact velocity with an impulse and a velocity bias but i have too much bouncing with this.
So i throught that it was the bias that produced this, so i have changed the method for positional correction and it's OK.
But when i add restitution the problem is reappeared so i think i really have a problem in my code...
the simulation :
- I make a narrowphase and broadphase to detect collision
- I update or create or delete the contacts with contact matching
- I integrate force
- I initialize the contacts and warmstart it
- I apply an impulse for each contacts with n itérations
- I integrate the Velocity
- I correct the position
- I clear force
notes :
- the problem is here even where the bodies have not rotation and have not friction.
- the problem is here even without the positional correction.
This is the update code :
Code: Select all
private void broadphase() {
Body a = null;
Body b = null;
Manifold m = null;
Manifold old = null;
for (int i = 0; i < bodies.size(); i++) {
a = bodies.get(i);
for (int j = i + 1; j < bodies.size(); j++) {
b = bodies.get(j);
if (a.invMass == 0f && b.invMass == 0f) {
continue;
}
m = new Manifold(a, b);
if (m.collide() > 0) {
old = findManifold(m);
if (old != null) {
old.updateContacts(m.contacts, m.numContacts);
} else {
contacts.add(m);
}
} else {
contacts.remove(m);
}
}
}
}
private Manifold findManifold(Manifold mNew) {
for (Manifold mOld : contacts) {
if (mOld.equals(mNew)) {
return mOld;
}
}
return null;
}
public void update(float dt) {
broadphase();
integrateForces(dt);
// Initialize collision
for (Manifold c : contacts) {
c.preStep();
}
// Solve collisions
for (int i = 0; i < iterations; i++) {
for (Manifold c : contacts) {
c.applyImpulse();
}
}
integrateVelocities(dt);
for (int i = 0; i <= iterations >> 1; i++) {
for (Manifold c : contacts) {
c.solvePositionConstraint();
}
}
clearForces();
}
// Semi-Implicit (Symplectic) Euler
// v = ∫a*dt where a = F/m
// ang_v = ∫ang_a*dt where ang_a = T/I
private void integrateForces(float dt) {
Vec2 acc = new Vec2();
float angAcc = 0f;
for (Body b : bodies) {
if (b.invMass == 0f) {
continue;
}
// linear
acc.set(b.force).sclLocal(b.invMass);
acc.y += PhysicSettings.gravity * b.gravityScale;// gravity will
// always act on
// the
// body
b.velocity.addSclLocal(acc, dt);
// angular
angAcc = b.torque * b.invInertia;
b.angularVelocity += angAcc * dt;
}
}
// Semi-Implicit (Symplectic) Euler
// x = ∫v*dt
private void integrateVelocities(float dt) {
for (Body b : bodies) {
if (b.invMass == 0f) {
continue;
}
b.position.addSclLocal(b.velocity, dt);
b.rotation += b.angularVelocity * dt;
}
}
private void clearForces() {
for (Body b : bodies) {
b.force.setZero();
b.torque = 0f;
}
}
Code: Select all
private float getMassCoefficient(Vec2 rA, Vec2 rB, Vec2 n) {
float raXn = rA.cross(n);
float rbXn = rB.cross(n);
float kn = bodyA.invMass + bodyB.invMass;
kn += bodyA.invInertia * raXn * raXn;
kn += bodyB.invInertia * rbXn * rbXn;
return kn != 0f ? 1f / kn : 0f;
}
private Vec2 getRelativeVelocity(Vec2 rA, Vec2 rB) {
// v_sum = v + r x ω
// v_rel = vB_sum - vA_sum
Vec2 va = rA.crossr(bodyA.angularVelocity).addLocal(bodyA.velocity);
Vec2 vb = rB.crossr(bodyB.angularVelocity).addLocal(bodyB.velocity);
return vb.sub(va);
}
private Contact findContact(int id) {
for (int i = 0; i < numContacts; i++) {
if (contacts[i].id == id) {
return contacts[i];
}
}
return null;
}
public void updateContacts(Contact[] newContacts, int numNewContacts) {
for (int i = 0; i < numNewContacts; i++) {
Contact cNew = newContacts[i];
Contact cOld = findContact(cNew.id);
if (cOld != null) {
cNew.pn = cOld.pn;
cNew.pt = cOld.pt;
} else { // optional
cNew.pn = 0f;
cNew.pt = 0f;
}
}
for (int i = 0; i < numNewContacts; i++) {
contacts[i] = newContacts[i];
}
numContacts = numNewContacts;
}
public void preStep() {
// Compute the friction coefficient
f = (float) Math.sqrt(bodyA.friction * bodyB.friction);
// Compute the coefficient of elasticity
e = Math.min(bodyA.restitution, bodyB.restitution);
float g = PhysicSettings.gravity * dt;
float resting = (float) (g * Math.sqrt(bodyA.gravityScale * bodyB.gravityScale)) + MathUtils.EPSILON;
for (int i = 0; i < numContacts; i++) {
Contact c = contacts[i];
// compute COM to contact for body A
c.position.sub(bodyA.position, c.rA);
// compute COM to contact for body B
c.position.sub(bodyB.position, c.rB);
// Get the relative velocity between A and B
Vec2 rv = getRelativeVelocity(c.rA, c.rB);
// Compute the velocity bias
c.vb = 0f;
float rvn = rv.dot(c.normal);
if (rvn < -resting) {
// apply the coefficient of elasticity
c.vb += -e * rvn;
}
// compute the tangent
Vec2 remN = c.normal.scl(c.normal.dot(rv));
c.tangent.set(rv).subLocal(remN).normalizeLocal();
// compute the sum of effective masses along normal
c.kn = getMassCoefficient(c.rA, c.rB, c.normal);
// compute the sum of effective masses along tangent
c.kt = getMassCoefficient(c.rA, c.rB, c.tangent);
}
warmStart();
}
private void warmStart() {
for (int i = 0; i < numContacts; i++) {
Contact c = contacts[i];
// warm-start
Vec2 J = new Vec2();
J.x = c.normal.x * c.pn + c.tangent.x * c.pt;
J.y = c.normal.y * c.pn + c.tangent.y * c.pt;
bodyA.applyImpulse(J.negate(), c.rA);
bodyB.applyImpulse(J, c.rB);
}
}
public void applyImpulse() {
// apply friction impulse
for (int i = 0; i < numContacts; i++) {
Contact c = contacts[i];
// Get the relative velocity between A and B
Vec2 rv = getRelativeVelocity(c.rA, c.rB);
// Get the relative velocity along the normal
// v_rel . n
float rvt = rv.dot(c.tangent);
// Calculate normal impulse scalar relative to A's normal
float j = c.kt * -rvt;
// Coulomb's Law : -f * lambdaN <= lambdaT <= f * lambdaN
float maxJ = f * c.pn;
// clamp the accumulated tangential impulse
float pt0 = c.pt;
c.pt = MathUtils.clamp(pt0 + j, -maxJ, maxJ);
j = c.pt - pt0;
// Apply tangent impulse
Vec2 jt = c.tangent.scl(j);
bodyA.applyImpulse(jt.negate(), c.rA);
bodyB.applyImpulse(jt, c.rB);
}
// apply normal impulse
for (int i = 0; i < numContacts; i++) {
Contact c = contacts[i];
// Get the relative velocity between A and B
Vec2 rv = getRelativeVelocity(c.rA, c.rB);
// Get the relative velocity along the normal
// v_rel . n
float rvn = rv.dot(c.normal);
// Calculate normal impulse scalar relative to A's normal
float j = -c.kn * (rvn - c.vb);
// accumulate impulses
float pn0 = c.pn;
c.pn = Math.max(pn0 + j, 0f);
j = c.pn - pn0;
// Apply normal impulse
Vec2 jn = c.normal.scl(j);
bodyA.applyImpulse(jn.negate(), c.rA);
bodyB.applyImpulse(jn, c.rB);
}
}
thank you in advance for the time you're going to devote to me.