/// Get the depth of the @c ith contact point.
public float GetDepth(int i)
{
// return this.contacts[i].dist;
cp.AssertHard(0 <= i && i < GetCount(), "Index error: The specified contact index is invalid for this arbiter");
cpContact con = contacts[i];
return(cpVect.cpvdot(cpVect.cpvadd(cpVect.cpvsub(con.r2, con.r1), cpVect.cpvsub(this.body_b.p, this.body_a.p)), this.n));
}
/// Calculate the total impulse that was applied by this arbiter.
/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback.
public cpVect TotalImpulse()
{
cpVect sum = cpVect.Zero;
for (int i = 0, count = GetCount(); i < count; i++)
{
cpContact con = contacts[i];
// sum.Add(con.n.Multiply(con.jnAcc));
sum = cpVect.cpvadd(sum, cpVect.cpvrotate(n, new cpVect(con.jnAcc, con.jtAcc)));
}
return(this.swapped ? sum : cpVect.cpvneg(sum));
}
public void ApplyCachedImpulse(float dt_coef)
{
if (this.IsFirstContact())
{
return;
}
var a = this.body_a;
var b = this.body_b;
cpVect n = this.n;
for (int i = 0; i < this.Count; i++)
{
cpContact con = this.contacts[i];
cpVect j = cpVect.cpvrotate(n, new cpVect(con.jnAcc, con.jtAcc));
cp.apply_impulses(a, b, con.r1, con.r2, cpVect.cpvmult(j, dt_coef));
}
}
/// Calculate the amount of energy lost in a collision including static, but not dynamic friction.
/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback.
public float TotalKE()
{
float eCoef = (1f - this.e) / (1f + this.e);
float sum = 0.0f;
//cpContact contacts = contacts;
for (int i = 0, count = GetCount(); i < count; i++)
{
cpContact con = contacts[i];
float jnAcc = con.jnAcc;
float jtAcc = con.jtAcc;
sum += eCoef * jnAcc * jnAcc / con.nMass + jtAcc * jtAcc / con.tMass;
}
return(sum);
}
public void ApplyImpulse(float dt)
{
cpBody a = this.body_a;
cpBody b = this.body_b;
cpVect n = this.n;
cpVect surface_vr = this.surface_vr;
float friction = this.u;
for (int i = 0; i < this.Count; i++)
{
cpContact con = this.contacts[i];
float nMass = con.nMass;
cpVect r1 = con.r1;
cpVect r2 = con.r2;
cpVect vb1 = cpVect.cpvadd(a.v_bias, cpVect.cpvmult(cpVect.cpvperp(r1), a.w_bias));
cpVect vb2 = cpVect.cpvadd(b.v_bias, cpVect.cpvmult(cpVect.cpvperp(r2), b.w_bias));
cpVect vr = cpVect.cpvadd(cp.relative_velocity(a, b, r1, r2), surface_vr);
float vbn = cpVect.cpvdot(cpVect.cpvsub(vb2, vb1), n);
float vrn = cpVect.cpvdot(vr, n);
float vrt = cpVect.cpvdot(vr, cpVect.cpvperp(n));
float jbn = (con.bias - vbn) * nMass;
float jbnOld = con.jBias;
con.jBias = cp.cpfmax(jbnOld + jbn, 0.0f);
float jn = -(con.bounce + vrn) * nMass;
float jnOld = con.jnAcc;
con.jnAcc = cp.cpfmax(jnOld + jn, 0.0f);
float jtMax = friction * con.jnAcc;
float jt = -vrt * con.tMass;
float jtOld = con.jtAcc;
con.jtAcc = cp.cpfclamp(jtOld + jt, -jtMax, jtMax);
cp.apply_bias_impulses(a, b, r1, r2, cpVect.cpvmult(n, con.jBias - jbnOld));
cp.apply_impulses(a, b, r1, r2, cpVect.cpvrotate(n, new cpVect(con.jnAcc - jnOld, con.jtAcc - jtOld)));
}
;
}
public void Update(cpCollisionInfo info, cpSpace space)
{
cpShape a = info.a, b = info.b;
// For collisions between two similar primitive types, the order could have been swapped since the last frame.
this.a = a; this.body_a = a.body;
this.b = b; this.body_b = b.body;
// Iterate over the possible pairs to look for hash value matches.
for (int i = 0; i < info.count; i++)
{
cpContact con = info.arr[i];
// r1 and r2 store absolute offsets at init time.
// Need to convert them to relative offsets.
con.r1 = cpVect.cpvsub(con.r1, a.body.p);
con.r2 = cpVect.cpvsub(con.r2, b.body.p);
// Cached impulses are not zeroed at init time.
con.jnAcc = con.jtAcc = 0.0f;
for (int j = 0; j < this.Count; j++)
{
cpContact old = this.contacts[j];
// This could trigger false positives, but is fairly unlikely nor serious if it does.
if (con.hash == old.hash)
{
// Copy the persistant contact information.
con.jnAcc = old.jnAcc;
con.jtAcc = old.jtAcc;
}
}
}
//TODO: revise
this.contacts = info.arr;
//this.count = info.count;
this.n = info.n;
this.e = a.e * b.e;
this.u = a.u * b.u;
cpVect surface_vr = cpVect.cpvsub(b.surfaceV, a.surfaceV);
this.surface_vr = cpVect.cpvsub(surface_vr, cpVect.cpvmult(info.n, cpVect.cpvdot(surface_vr, info.n)));
ulong typeA = info.a.type, typeB = info.b.type;
cpCollisionHandler defaultHandler = space.defaultHandler;
cpCollisionHandler handler = this.handler = space.LookupHandler(typeA, typeB, defaultHandler);
// Check if the types match, but don't swap for a default handler which use the wildcard for type A.
bool swapped = this.swapped = (typeA != handler.typeA && handler.typeA != cp.WILDCARD_COLLISION_TYPE);
if (handler != defaultHandler || space.usesWildcards)
{
// The order of the main handler swaps the wildcard handlers too. Uffda.
this.handlerA = space.LookupHandler(swapped ? typeB : typeA, cp.WILDCARD_COLLISION_TYPE, cpCollisionHandler.cpCollisionHandlerDoNothing);
this.handlerB = space.LookupHandler(swapped ? typeA : typeB, cp.WILDCARD_COLLISION_TYPE, cpCollisionHandler.cpCollisionHandlerDoNothing);
}
// mark it as new if it's been cached
if (this.state == cpArbiterState.Cached)
{
this.state = cpArbiterState.FirstCollision;
}
}