public void ApplySurfaceImpulse(Vertex3D rotI, Vertex3D impulse)
{
Vel.Add(impulse.Clone().MultiplyScalar(InvMass));
AngularMomentum.Add(rotI);
var angularMomentum = AngularMomentum.Clone();
AngularVelocity.Set(angularMomentum.DivideScalar(Inertia));
}
public void Collide3DWall(Vertex3D hitNormal, float elasticity, float elasticityFalloff, float friction, float scatterAngle)
{
// speed normal to wall
var dot = Vel.Dot(hitNormal);
if (dot >= -PhysicsConstants.LowNormVel)
{
// nearly receding ... make sure of conditions
if (dot > PhysicsConstants.LowNormVel)
{
// otherwise if clearly approaching .. process the collision
return; // is this velocity clearly receding (i.E must > a minimum)
}
//#ifdef PhysicsConstants.Embedded
if (Coll.HitDistance < -PhysicsConstants.Embedded)
{
dot = -PhysicsConstants.EmbedShot; // has ball become embedded???, give it a kick
}
else
{
return;
}
//#endif
}
//#ifdef PhysicsConstants.DispGain
// correct displacements, mostly from low velocity, alternative to acceleration processing
var hDist = -PhysicsConstants.DispGain * Coll.HitDistance; // limit delta noise crossing ramps,
if (hDist > 1.0e-4)
{
// when hit detection checked it what was the displacement
if (hDist > PhysicsConstants.DispLimit)
{
hDist = PhysicsConstants.DispLimit; // crossing ramps, delta noise
}
// push along norm, back to free area
_state.Pos.Add(hitNormal.Clone().MultiplyScalar(hDist));
// use the norm, but this is not correct, reverse time is correct
}
//#endif
// magnitude of the impulse which is just sufficient to keep the ball from
// penetrating the wall (needed for friction computations)
var reactionImpulse = _data.Mass * MathF.Abs(dot);
elasticity = Functions.ElasticityWithFalloff(elasticity, elasticityFalloff, dot);
dot *= -(1.0f + elasticity);
Vel.Add(hitNormal.Clone().MultiplyScalar(dot)); // apply collision impulse (along normal, so no torque)
// compute friction impulse
var surfP = hitNormal.Clone().MultiplyScalar(-_data.Radius); // surface contact point relative to center of mass
var surfVel = SurfaceVelocity(surfP); // velocity at impact point
var tangent = surfVel.Clone() // calc the tangential velocity
.Sub(hitNormal.Clone()
.MultiplyScalar(surfVel.Dot(hitNormal)));
var tangentSpSq = tangent.LengthSq();
if (tangentSpSq > 1e-6)
{
tangent.DivideScalar(MathF.Sqrt(tangentSpSq)); // normalize to get tangent direction
var vt = surfVel.Dot(tangent); // get speed in tangential direction
// compute friction impulse
var cross = Vertex3D.CrossProduct(surfP, tangent);
var crossInertia = cross.Clone().DivideScalar(Inertia);
var kt = InvMass + tangent.Dot(Vertex3D.CrossProduct(crossInertia, surfP));
// friction impulse can"t be greather than coefficient of friction times collision impulse (Coulomb friction cone)
var maxFric = friction * reactionImpulse;
var jt = Functions.Clamp(-vt / kt, -maxFric, maxFric);
if (!float.IsNaN(jt) && !float.IsInfinity(jt))
{
ApplySurfaceImpulse(
cross.Clone().MultiplyScalar(jt),
tangent.Clone().MultiplyScalar(jt)
);
}
}
if (scatterAngle < 0.0)
{
scatterAngle = HardScatter;
} // if < 0 use global value
scatterAngle *= _tableData.GlobalDifficulty; // apply difficulty weighting
if (dot > 1.0 && scatterAngle > 1.0e-5)
{
// no scatter at low velocity
var scatter = MathF.Random() * 2 - 1; // -1.0f..1.0f
scatter *= (1.0f - scatter * scatter) * 2.59808f * scatterAngle; // shape quadratic distribution and scale
var radSin = MathF.Sin(scatter); // Green's transform matrix... rotate angle delta
var radCos = MathF.Cos(scatter); // rotational transform from current position to position at time t
var vxt = Vel.X;
var vyt = Vel.Y;
Vel.X = vxt * radCos - vyt * radSin; // rotate to random scatter angle
Vel.Y = vyt * radCos + vxt * radSin;
}
}