public Vertex3D SurfaceAcceleration(Vertex3D surfP)
{
// tangential acceleration = (0, 0, omega) x surfP
var tangAcc = Vertex3D.CrossZ(_angularAcceleration, surfP);
// centripetal acceleration = (0,0,omega) x ( (0,0,omega) x surfP )
var av2 = AngleSpeed * AngleSpeed;
var centrAcc = new Vertex3D(-av2 * surfP.X, -av2 * surfP.Y, 0);
return(tangAcc.Add(centrAcc));
}
public override void Contact(CollisionEvent coll, float dTime, PlayerPhysics physics)
{
var ball = coll.Ball;
var normal = coll.HitNormal;
//#ifdef PhysicsConstants.EMBEDDED
if (coll.HitDistance < -PhysicsConstants.Embedded)
{
// magic to avoid balls being pushed by each other through resting flippers!
ball.Hit.Vel.Add(normal.Clone().MultiplyScalar(0.1f));
}
//#endif
var vRel = new Vertex3D();
var rB = new Vertex3D();
var rF = new Vertex3D();
GetRelativeVelocity(normal, ball, vRel, rB, rF);
// this should be zero, but only up to +/- C_CONTACTVEL
var normVel = vRel.Dot(normal);
// If some collision has changed the ball's velocity, we may not have to do anything.
if (normVel <= PhysicsConstants.ContactVel)
{
// compute accelerations of point on ball and flipper
var aB = ball.Hit.SurfaceAcceleration(rB, physics);
var aF = _mover.SurfaceAcceleration(rF);
var aRel = aB.Clone().Sub(aF);
// time derivative of the normal vector
var normalDeriv = Vertex3D.CrossZ(_mover.AngleSpeed, normal);
// relative acceleration in the normal direction
var normAcc = aRel.Dot(normal) + 2.0f * normalDeriv.Dot(vRel);
if (normAcc >= 0)
{
return; // objects accelerating away from each other, nothing to do
}
// hypothetical accelerations arising from a unit contact force in normal direction
var aBc = normal.Clone().MultiplyScalar(ball.Hit.InvMass);
var pv2 = normal.Clone().MultiplyScalar(-1);
var cross = Vertex3D.CrossProduct(rF, pv2);
var pv1 = cross.Clone().DivideScalar(_mover.Inertia);
var aFc = Vertex3D.CrossProduct(pv1, rF);
var contactForceAcc = normal.Dot(aBc.Clone().Sub(aFc));
// find j >= 0 such that normAcc + j * contactForceAcc >= 0 (bodies should not accelerate towards each other)
var j = -normAcc / contactForceAcc;
// kill any existing normal velocity
ball.Hit.Vel.Add(normal.Clone().MultiplyScalar(j * dTime * ball.Hit.InvMass - coll.HitOrgNormalVelocity));
_mover.ApplyImpulse(cross.Clone().MultiplyScalar(j * dTime));
// apply friction
// first check for slippage
var slip = vRel.Clone().Sub(normal.Clone().MultiplyScalar(normVel)); // calc the tangential slip velocity
var maxFriction = j * Friction;
var slipSpeed = slip.Length();
Vertex3D slipDir;
Vertex3D crossF;
float numer;
float denomF;
Vertex3D pv13;
if (slipSpeed < PhysicsConstants.Precision)
{
// slip speed zero - static friction case
var slipAcc = aRel.Clone().Sub(normal.Clone().MultiplyScalar(aRel.Dot(normal))); // calc the tangential slip acceleration
// neither slip velocity nor slip acceleration? nothing to do here
if (slipAcc.LengthSq() < 1e-6)
{
return;
}
slipDir = slipAcc.Normalize();
numer = -slipDir.Dot(aRel);
crossF = Vertex3D.CrossProduct(rF, slipDir);
pv13 = crossF.Clone().DivideScalar(-_mover.Inertia);
denomF = slipDir.Dot(Vertex3D.CrossProduct(pv13, rF));
}
else
{
// nonzero slip speed - dynamic friction case
slipDir = slip.Clone().DivideScalar(slipSpeed);
numer = -slipDir.Dot(vRel);
crossF = Vertex3D.CrossProduct(rF, slipDir);
pv13 = crossF.Clone().DivideScalar(_mover.Inertia);
denomF = slipDir.Dot(Vertex3D.CrossProduct(pv13, rF));
}
var crossB = Vertex3D.CrossProduct(rB, slipDir);
var pv12 = crossB.Clone().DivideScalar(ball.Hit.Inertia);
var denomB = ball.Hit.InvMass + slipDir.Dot(Vertex3D.CrossProduct(pv12, rB));
var friction = Functions.Clamp(numer / (denomB + denomF), -maxFriction, maxFriction);
ball.Hit.ApplySurfaceImpulse(
crossB.Clone().MultiplyScalar(dTime * friction),
slipDir.Clone().MultiplyScalar(dTime * friction)
);
_mover.ApplyImpulse(crossF.Clone().MultiplyScalar(-dTime * friction));
}
}
// rigid body functions
public Vertex3D SurfaceVelocity(Vertex3D surfP)
{
return(Vertex3D.CrossZ(AngleSpeed, surfP));
}