public void resolve()
{
if (p1.getFixed() & p2.getFixed())
{
return;
return;
}
delta = p1.curr.minus(p2.curr);
deltaLength = p1.curr.distance(p2.curr);
if (_collidable)
{
orientCollisionRectangle();
}
double diff = (deltaLength - restLen) / deltaLength;
Vector dmd = delta.mult(diff * base.getStiffness());
double invM1 = p1.getInvMass();
double invM2 = p2.getInvMass();
double sumInvMass = invM1 + invM2;
if (!p1.getFixed())
{
p1.curr.minusEquals(dmd.mult(invM1 / sumInvMass));
}
if (!p2.getFixed())
{
p2.curr.plusEquals(dmd.mult(invM2 / sumInvMass));
}
}
public static void resolveParticleParticle(AbstractParticle pa, AbstractParticle pb, Vector normal, double depth)
{
Vector mtd = normal.mult(depth);
double te = pa.getElasticity() + pb.getElasticity();
double tf = 1 - (pa.getFriction() + pb.getFriction());
if (tf > 1)
{
tf = 1;
}
else if (tf < 0)
{
tf = 0;
}
double ma = (pa.getFixed()) ? 100000 : pa.getMass();
double mb = (pb.getFixed()) ? 100000 : pb.getMass();
double tm = ma + mb;
Collision ca = pa.getComponents(normal);
Collision cb = pb.getComponents(normal);
if (ca.vn.x > 5)
{
int i = 9;
}
Vector vnA = (cb.vn.mult((te + 1) * mb).plus(ca.vn.mult(ma - te * mb))).divEquals(tm);
Vector vnB = (ca.vn.mult((te + 1) * ma).plus(cb.vn.mult(mb - te * ma))).divEquals(tm);
ca.vt.multEquals(tf);
cb.vt.multEquals(tf);
Vector mtdA = mtd.mult(mb / tm);
Vector mtdB = mtd.mult(-ma / tm);
if (!pa.getFixed())
{
pa.resolveCollision(mtdA, vnA.plusEquals(ca.vt), normal, depth, -1);
//My.Computer.Audio.Play(System.AppDomain.CurrentDomain.BaseDirectory & "\fall.wav")
if (vnA.x > 5)
{
int i = 9;
}
}
if (!pb.getFixed())
{
pb.resolveCollision(mtdB, vnB.plusEquals(cb.vt), normal, depth, 1);
}
}
public override void resolveCollision(Vector mtd, Vector vel, Vector n, double d, double o)
{
if (!p1.getFixed())
{
p1.curr.plusEquals(mtd);
p1.setVelocity(vel);
}
if (!p2.getFixed())
{
p2.curr.plusEquals(mtd);
p2.setVelocity(vel);
}
}
public static void test(AbstractParticle objA, AbstractParticle objB)
{
if (objA.getFixed() && objB.getFixed())
{
return;
return;
}
//// rectangle to rectangle
if (objA is RectangleParticle & objB is RectangleParticle)
{
RectangleParticle objArect = (RectangleParticle)objA;
RectangleParticle objBrect = (RectangleParticle)objB;
testOBBvsOBB(objArect, objBrect);
//// circle to circle
}
else if (objA is CircleParticle & objB is CircleParticle)
{
CircleParticle objAcir = (CircleParticle)objA;
CircleParticle objBcir = (CircleParticle)objB;
testCirclevsCircle(objAcir, objBcir);
//// rectangle to circle - two ways
}
else if (objA is RectangleParticle & objB is CircleParticle)
{
RectangleParticle objArect = (RectangleParticle)objA;
CircleParticle objBcir = (CircleParticle)objB;
testOBBvsCircle(objArect, objBcir);
}
else if (objA is CircleParticle & objB is RectangleParticle)
{
CircleParticle objAcir = (CircleParticle)objA;
RectangleParticle objBrect = (RectangleParticle)objB;
testOBBvsCircle(objBrect, objAcir);
}
//testOBBvsOBB(DirectCast(objA, RectangleParticle), DirectCast(objB, RectangleParticle))
}