public void ApplyFriction(Vertex3D hitNormal, float dTime, float frictionCoeff, PlayerPhysics physics)
{
// surface contact point relative to center of mass
var surfP = hitNormal.Clone().MultiplyScalar(-_data.Radius);
var surfVel = SurfaceVelocity(surfP);
// calc the tangential slip velocity
var slip = surfVel.Clone().Sub(hitNormal.Clone().MultiplyScalar(surfVel.Dot(hitNormal)));
var maxFriction = frictionCoeff * _data.Mass * -physics.Gravity.Dot(hitNormal);
var slipSpeed = slip.Length();
Vertex3D slipDir;
float numer;
//#ifdef C_BALL_SPIN_HACK
var normVel = Vel.Dot(hitNormal);
if (normVel <= 0.025 || slipSpeed < PhysicsConstants.Precision)
{
// check for <=0.025 originated from ball<->rubber collisions pushing the ball upwards, but this is still not enough, some could even use <=0.2
// slip speed zero - static friction case
var surfAcc = SurfaceAcceleration(surfP, physics);
// calc the tangential slip acceleration
var slipAcc = surfAcc.Clone()
.Sub(hitNormal.Clone().MultiplyScalar(surfAcc.Dot(hitNormal)));
// neither slip velocity nor slip acceleration? nothing to do here
if (slipAcc.LengthSq() < 1e-6)
{
return;
}
slipDir = slipAcc.Clone().Normalize();
numer = -slipDir.Dot(surfAcc);
}
else
{
// nonzero slip speed - dynamic friction case
slipDir = slip.Clone().DivideScalar(slipSpeed);
numer = -slipDir.Dot(surfVel);
}
var cp = Vertex3D.CrossProduct(surfP, slipDir);
var p1 = cp.Clone().DivideScalar(Inertia);
var denom = InvMass + slipDir.Dot(Vertex3D.CrossProduct(p1, surfP));
var friction = Functions.Clamp(numer / denom, -maxFriction, maxFriction);
if (!float.IsNaN(friction) && !float.IsInfinity(friction))
{
ApplySurfaceImpulse(
cp.Clone().MultiplyScalar(dTime * friction),
slipDir.Clone().MultiplyScalar(dTime * friction)
);
}
}
public static void ComputeNormals(Vertex3DNoTex2[] vertices, int numVertices, int[] indices, int numIndices)
{
for (var i = 0; i < numVertices; i++)
{
var v = vertices[i];
v.Nx = v.Ny = v.Nz = 0.0f;
}
for (var i = 0; i < numIndices; i += 3)
{
var a = vertices[indices[i]];
var b = vertices[indices[i + 1]];
var c = vertices[indices[i + 2]];
var e0 = new Vertex3D(b.X - a.X, b.Y - a.Y, b.Z - a.Z);
var e1 = new Vertex3D(c.X - a.X, c.Y - a.Y, c.Z - a.Z);
var normal = e0.Clone().Cross(e1).Normalize();
a.Nx += normal.X; a.Ny += normal.Y; a.Nz += normal.Z;
b.Nx += normal.X; b.Ny += normal.Y; b.Nz += normal.Z;
c.Nx += normal.X; c.Ny += normal.Y; c.Nz += normal.Z;
}
for (var i = 0; i < numVertices; i++)
{
var v = vertices[i];
var l = v.Nx * v.Nx + v.Ny * v.Ny + v.Nz * v.Nz;
var invL = l >= Constants.FloatMin ? 1.0f / MathF.Sqrt(l) : 0.0f;
v.Nx *= invL;
v.Ny *= invL;
v.Nz *= invL;
}
}
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 HitLine3D(Vertex3D v1, Vertex3D v2, ItemType itemType, IItem item) : base(new Vertex2D(), itemType, item)
{
var vLine = v2.Clone().Sub(v1);
vLine.Normalize();
// Axis of rotation to make 3D cylinder a cylinder along the z-axis
var transAxis = new Vertex3D(vLine.Y, -vLine.X, 0);
var l = transAxis.LengthSq();
if (l <= 1e-6)
{
// line already points in z axis?
transAxis.Set(1, 0, 0); // choose arbitrary rotation vector
}
else
{
transAxis.DivideScalar(MathF.Sqrt(l));
}
// Angle to rotate the line into the z-axis
var dot = vLine.Z; //vLine.Dot(&vup);
Matrix.RotationAroundAxis(transAxis, -MathF.Sqrt(1 - dot * dot), dot);
var vTrans1 = v1.Clone().ApplyMatrix2D(Matrix);
var vTrans2 = v2.Clone().ApplyMatrix2D(Matrix);
var vTrans2Z = vTrans2.Z;
// set up HitLineZ parameters
Xy.Set(vTrans1.X, vTrans1.Y);
ZLow = MathF.Min(vTrans1.Z, vTrans2Z);
ZHigh = MathF.Max(vTrans1.Z, vTrans2Z);
V1 = v1;
V2 = v2;
HitBBox.Left = MathF.Min(v1.X, v2.X);
HitBBox.Right = MathF.Max(v1.X, v2.X);
HitBBox.Top = MathF.Min(v1.Y, v2.Y);
HitBBox.Bottom = MathF.Max(v1.Y, v2.Y);
HitBBox.ZLow = MathF.Min(v1.Z, v2.Z);
HitBBox.ZHigh = MathF.Max(v1.Z, v2.Z);
}
public override void Collide(CollisionEvent coll, PlayerPhysics physics)
{
coll.Ball.Hit.Collide3DWall(coll.HitNormal, Elasticity, ElasticityFalloff, Friction,
Scatter);
// distance from plane to ball surface
var bnd = _normal.Dot(coll.Ball.State.Pos) - coll.Ball.Data.Radius - _d;
if (bnd < 0)
{
// if ball has penetrated, push it out of the plane
var v = _normal.Clone().MultiplyScalar(bnd);
coll.Ball.State.Pos.Add(v);
}
}
private void GetRelativeVelocity(Vertex3D normal, Ball.Ball ball, Vertex3D vRel, Vertex3D rB, Vertex3D rF)
{
rB.Set(normal.Clone().MultiplyScalar(-ball.Data.Radius));
var hitPos = ball.State.Pos.Clone().Add(rB);
var cF = new Vertex3D(
_mover.HitCircleBase.Center.X,
_mover.HitCircleBase.Center.Y,
ball.State.Pos.Z // make sure collision happens in same z plane where ball is
);
rF.Set(hitPos.Clone().Sub(cF)); // displacement relative to flipper center
var vB = ball.Hit.SurfaceVelocity(rB);
var vF = _mover.SurfaceVelocity(rF);
vRel.Set(vB.Clone().Sub(vF));
}
internal Mesh GetMesh(Table.Table table, int acc = -1, bool createHitShape = false)
{
var mesh = new Mesh(_data.Name);
const int accuracy = (int)(10.0f * 1.2f); // see also above
var splineAccuracy = acc != -1 ? 4.0f * MathF.Pow(10.0f, (10.0f - PhysicsConstants.HitShapeDetailLevel) * (float)(1.0 / 1.5)) : -1.0f;
var sv = new SplineVertex(_data.DragPoints, _data.Thickness, table.GetDetailLevel(), (int)splineAccuracy);
var numRings = sv.VertexCount - 1;
var numSegments = accuracy;
var numVertices = numRings * numSegments;
var numIndices = 6 * numVertices; //m_numVertices*2+2;
var height = _data.HitHeight + table.TableHeight;
mesh.Vertices = new Vertex3DNoTex2[numVertices];
mesh.Indices = new int[numIndices];
var prevB = new Vertex3D();
var invNr = 1.0f / numRings;
var invNs = 1.0f / numSegments;
var index = 0;
for (var i = 0; i < numRings; i++)
{
var i2 = i == numRings - 1 ? 0 : i + 1;
var tangent = new Vertex3D(sv.MiddlePoints[i2].X - sv.MiddlePoints[i].X,
sv.MiddlePoints[i2].Y - sv.MiddlePoints[i].Y, 0.0f);
Vertex3D biNormal;
Vertex3D normal;
if (i == 0)
{
var up = new Vertex3D(sv.MiddlePoints[i2].X + sv.MiddlePoints[i].X, sv.MiddlePoints[i2].Y + sv.MiddlePoints[i].Y, height * 2.0f);
normal = new Vertex3D(tangent.Y * up.Z, -tangent.X * up.Z, tangent.X * up.Y - tangent.Y * up.X); // = CrossProduct(tangent, up)
biNormal = new Vertex3D(tangent.Y * normal.Z, -tangent.X * normal.Z, tangent.X * normal.Y - tangent.Y * normal.X); // = CrossProduct(tangent, normal)
}
else
{
normal = prevB.Clone().Cross(tangent);
biNormal = tangent.Clone().Cross(normal);
}
biNormal.Normalize();
normal.Normalize();
prevB = biNormal;
var u = i * invNr;
for (var j = 0; j < numSegments; j++)
{
var v = ((float)j + u) * invNs;
var tmp = Vertex3D.GetRotatedAxis(j * (360.0f * invNs), tangent, normal)
.MultiplyScalar(_data.Thickness * 0.5f);
mesh.Vertices[index] = new Vertex3DNoTex2 {
X = sv.MiddlePoints[i].X + tmp.X,
Y = sv.MiddlePoints[i].Y + tmp.Y
};
if (createHitShape && (j == 0 || j == 3))
{
//!! hack, adapt if changing detail level for hitshape
// for a hit shape create a more rectangle mesh and not a smooth one
tmp.Z *= 0.6f;
}
mesh.Vertices[index].Z = height + tmp.Z;
//texel
mesh.Vertices[index].Tu = u;
mesh.Vertices[index].Tv = v;
index++;
}
}
// calculate faces
for (var i = 0; i < numRings; i++)
{
for (var j = 0; j < numSegments; j++)
{
var quad = new int[4];
quad[0] = i * numSegments + j;
if (j != numSegments - 1)
{
quad[1] = i * numSegments + j + 1;
}
else
{
quad[1] = i * numSegments;
}
if (i != numRings - 1)
{
quad[2] = (i + 1) * numSegments + j;
if (j != numSegments - 1)
{
quad[3] = (i + 1) * numSegments + j + 1;
}
else
{
quad[3] = (i + 1) * numSegments;
}
}
else
{
quad[2] = j;
if (j != numSegments - 1)
{
quad[3] = j + 1;
}
else
{
quad[3] = 0;
}
}
mesh.Indices[(i * numSegments + j) * 6] = quad[0];
mesh.Indices[(i * numSegments + j) * 6 + 1] = quad[1];
mesh.Indices[(i * numSegments + j) * 6 + 2] = quad[2];
mesh.Indices[(i * numSegments + j) * 6 + 3] = quad[3];
mesh.Indices[(i * numSegments + j) * 6 + 4] = quad[2];
mesh.Indices[(i * numSegments + j) * 6 + 5] = quad[1];
}
}
Mesh.ComputeNormals(mesh.Vertices, numVertices, mesh.Indices, numIndices);
var maxX = Constants.FloatMin;
var minX = Constants.FloatMax;
var maxY = Constants.FloatMin;
var minY = Constants.FloatMax;
var maxZ = Constants.FloatMin;
var minZ = Constants.FloatMax;
for (var i = 0; i < numVertices; i++)
{
if (maxX < mesh.Vertices[i].X)
{
maxX = mesh.Vertices[i].X;
}
if (minX > mesh.Vertices[i].X)
{
minX = mesh.Vertices[i].X;
}
if (maxY < mesh.Vertices[i].Y)
{
maxY = mesh.Vertices[i].Y;
}
if (minY > mesh.Vertices[i].Y)
{
minY = mesh.Vertices[i].Y;
}
if (maxZ < mesh.Vertices[i].Z)
{
maxZ = mesh.Vertices[i].Z;
}
if (minZ > mesh.Vertices[i].Z)
{
minZ = mesh.Vertices[i].Z;
}
}
MiddlePoint.X = (maxX + minX) * 0.5f;
MiddlePoint.Y = (maxY + minY) * 0.5f;
MiddlePoint.Z = (maxZ + minZ) * 0.5f;
return(mesh);
}
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));
}
}
public override void Collide(CollisionEvent coll, PlayerPhysics physics)
{
var ball = coll.Ball;
var normal = coll.HitNormal;
var vRel = new Vertex3D();
var rB = new Vertex3D();
var rF = new Vertex3D();
GetRelativeVelocity(normal, ball, vRel, rB, rF);
var bnv = normal.Dot(vRel); // relative normal velocity
if (bnv >= -PhysicsConstants.LowNormVel)
{
// nearly receding ... make sure of conditions
if (bnv > PhysicsConstants.LowNormVel)
{
// otherwise if clearly approaching .. process the collision
// is this velocity clearly receding (i.E must > a minimum)
return;
}
//#ifdef PhysicsConstants.EMBEDDED
if (coll.HitDistance < -PhysicsConstants.Embedded)
{
bnv = -PhysicsConstants.EmbedShot; // has ball become embedded???, give it a kick
}
else
{
return;
}
//#endif
}
//#ifdef PhysicsConstants.DISP_GAIN
// correct displacements, mostly from low velocity blindness, an alternative to true acceleration processing
var hitDist = -PhysicsConstants.DispGain * coll.HitDistance; // distance found in hit detection
if (hitDist > 1.0e-4)
{
if (hitDist > PhysicsConstants.DispLimit)
{
hitDist = PhysicsConstants.DispLimit; // crossing ramps, delta noise
}
// push along norm, back to free area; use the norm, but is not correct
ball.State.Pos.Add(coll.HitNormal.Clone().MultiplyScalar(hitDist));
}
//#endif
// angular response to impulse in normal direction
var angResp = Vertex3D.CrossProduct(rF, normal);
/*
* Check if flipper is in contact with its stopper and the collision impulse
* would push it beyond the stopper. In that case, don"t allow any transfer
* of kinetic energy from ball to flipper. This avoids overly dead bounces
* in that case.
*/
var angImp = -angResp.Z; // minus because impulse will apply in -normal direction
var flipperResponseScaling = 1.0f;
if (_mover.IsInContact && _mover.ContactTorque * angImp >= 0f)
{
// if impulse pushes against stopper, allow no loss of kinetic energy to flipper
// (still allow flipper recoil, but a diminished amount)
angResp.SetZero();
flipperResponseScaling = 0.5f;
}
/*
* Rubber has a coefficient of restitution which decreases with the impact velocity.
* We use a heuristic model which decreases the COR according to a falloff parameter:
* 0 = no falloff, 1 = half the COR at 1 m/s (18.53 speed units)
*/
var epsilon = Functions.ElasticityWithFalloff(Elasticity, ElasticityFalloff, bnv);
var pv1 = angResp.Clone().DivideScalar(_mover.Inertia);
var impulse = -(1.0f + epsilon) * bnv / (ball.Hit.InvMass + normal.Dot(Vertex3D.CrossProduct(pv1, rF)));
var flipperImp = normal.Clone().MultiplyScalar(-(impulse * flipperResponseScaling));
var rotI = Vertex3D.CrossProduct(rF, flipperImp);
if (_mover.IsInContact)
{
if (rotI.Z * _mover.ContactTorque < 0)
{
// pushing against the solenoid?
// Get a bound on the time the flipper needs to return to static conditions.
// If it"s too short, we treat the flipper as static during the whole collision.
var recoilTime = -rotI.Z / _mover.ContactTorque; // time flipper needs to eliminate this impulse, in 10ms
// Check ball normal velocity after collision. If the ball rebounded
// off the flipper, we need to make sure it does so with full
// reflection, i.E., treat the flipper as static, otherwise
// we get overly dead bounces.
var bnvAfter = bnv + impulse * ball.Hit.InvMass;
if (recoilTime <= 0.5 || bnvAfter > 0)
{
// treat flipper as static for this impact
impulse = -(1.0f + epsilon) * bnv * ball.Data.Mass;
flipperImp.SetZero();
rotI.SetZero();
}
}
}
ball.Hit.Vel.Add(normal.Clone().MultiplyScalar(impulse * ball.Hit.InvMass)); // new velocity for ball after impact
_mover.ApplyImpulse(rotI);
// apply friction
var tangent = vRel.Clone().Sub(normal.Clone().MultiplyScalar(vRel.Dot(normal))); // calc the tangential velocity
var tangentSpSq = tangent.LengthSq();
if (tangentSpSq > 1e-6)
{
tangent.DivideScalar(MathF.Sqrt(tangentSpSq)); // normalize to get tangent direction
var vt = vRel.Dot(tangent); // get speed in tangential direction
// compute friction impulse
var crossB = Vertex3D.CrossProduct(rB, tangent);
var pv12 = crossB.Clone().DivideScalar(ball.Hit.Inertia);
var kt = ball.Hit.InvMass + tangent.Dot(Vertex3D.CrossProduct(pv12, rB));
var crossF = Vertex3D.CrossProduct(rF, tangent);
var pv13 = crossF.Clone().DivideScalar(_mover.Inertia);
kt += tangent.Dot(Vertex3D.CrossProduct(pv13, rF)); // flipper only has angular response
// friction impulse can't be greater than coefficient of friction times collision impulse (Coulomb friction cone)
var maxFriction = Friction * impulse;
var jt = Functions.Clamp(-vt / kt, -maxFriction, maxFriction);
ball.Hit.ApplySurfaceImpulse(
crossB.Clone().MultiplyScalar(jt),
tangent.Clone().MultiplyScalar(jt)
);
_mover.ApplyImpulse(crossF.Clone().MultiplyScalar(-jt));
}
if (bnv < -0.25 && physics.TimeMsec - _lastHitTime > 250)
{
// limit rate to 250 milliseconds per event
var flipperHit =
coll.HitMomentBit ? -1.0 : -bnv; // move event processing to end of collision handler...
if (flipperHit < 0)
{
_events.FireGroupEvent(Event.HitEventsHit); // simple hit event
}
else
{
// collision velocity (normal to face)
_events.FireVoidEventParam(Event.FlipperEventsCollide, flipperHit);
}
}
_lastHitTime = physics.TimeMsec; // keep resetting until idle for 250 milliseconds
}
public override float HitTest(Ball ball, float dTime, CollisionEvent coll, PlayerPhysics physics)
{
if (!IsEnabled)
{
return(-1.0f);
}
// speed in Normal-vector direction
var bnv = _normal.Dot(ball.Hit.Vel);
// return if clearly ball is receding from object
if (ObjType != CollisionType.Trigger && bnv > PhysicsConstants.LowNormVel)
{
return(-1.0f);
}
// Point on the ball that will hit the polygon, if it hits at all
var normRadius = _normal.Clone().MultiplyScalar(ball.Data.Radius);
var hitPos = ball.State.Pos.Clone().Sub(normRadius); // nearest point on ball ... projected radius along norm
var planeToBall = hitPos.Clone().Sub(_rgv[0]);
var bnd = _normal.Dot(planeToBall); // distance from plane to ball
var bUnHit = bnv > PhysicsConstants.LowNormVel;
var inside = bnd <= 0; // in ball inside object volume
var rigid = ObjType != CollisionType.Trigger;
float hitTime;
if (rigid)
{
// rigid polygon
if (bnd < -ball.Data.Radius)
{
// (ball normal distance) excessive penetration of object skin ... no collision HACK //!! *2 necessary?
return(-1.0f);
}
if (bnd <= PhysicsConstants.PhysTouch)
{
if (inside || MathF.Abs(bnv) > PhysicsConstants.ContactVel // fast velocity, return zero time
//zero time for rigid fast bodies
|| bnd <= -PhysicsConstants.PhysTouch)
{
// slow moving but embedded
hitTime = 0;
}
else
{
hitTime = bnd * (float)(1.0 / (2.0 * PhysicsConstants.PhysTouch)) + 0.5f; // don't compete for fast zero time events
}
}
else if (MathF.Abs(bnv) > PhysicsConstants.LowNormVel)
{
// not velocity low?
hitTime = bnd / -bnv; // rate ok for safe divide
}
else
{
return(-1.0f); // wait for touching
}
}
else
{
// non-rigid polygon
if (bnv * bnd >= 0)
{
// outside-receding || inside-approaching
if (!ball.Hit.IsRealBall() || // temporary ball
MathF.Abs(bnd) >= ball.Data.Radius * 0.5 || // not too close ... nor too far away
inside == ball.Hit.VpVolObjs.Contains(Obj))
{
// ...Ball outside and hit set or ball inside and no hit set
return(-1.0f);
}
hitTime = 0;
bUnHit = !inside; // ball on outside is UnHit, otherwise it"s a Hit
}
else
{
hitTime = bnd / -bnv;
}
}
if (float.IsNaN(hitTime) || float.IsInfinity(hitTime) || hitTime < 0 || hitTime > dTime)
{
// time is outside this frame ... no collision
return(-1.0f);
}
var adv = ball.Hit.Vel.Clone().MultiplyScalar(hitTime);
hitPos.Add(adv); // advance hit point to contact
// Do a point in poly test, using the xy plane, to see if the hit point is inside the polygon
// this need to be changed to a point in polygon on 3D plane
var x2 = _rgv[0].X;
var y2 = _rgv[0].Y;
var hx2 = hitPos.X >= x2;
var hy2 = hitPos.Y <= y2;
var crossCount = 0; // count of lines which the hit point is to the left of
for (var i = 0; i < _rgv.Length; i++)
{
var x1 = x2;
var y1 = y2;
var hx1 = hx2;
var hy1 = hy2;
var j = i < _rgv.Length - 1 ? i + 1 : 0;
x2 = _rgv[j].X;
y2 = _rgv[j].Y;
hx2 = hitPos.X >= x2;
hy2 = hitPos.Y <= y2;
if (y1 == y2 || hy1 && hy2 || !hy1 && !hy2 || hx1 && hx2)
{
// Hit point is on the right of the line
continue;
}
if (!hx1 && !hx2)
{
crossCount ^= 1;
continue;
}
if (x2 == x1)
{
if (!hx2)
{
crossCount ^= 1;
}
continue;
}
// Now the hard part - the hit point is in the line bounding box
if (x2 - (y2 - hitPos.Y) * (x1 - x2) / (y1 - y2) > hitPos.X)
{
crossCount ^= 1;
}
}
if ((crossCount & 1) != 0)
{
coll.HitNormal.Set(_normal);
if (!rigid)
{
// non rigid body collision? return direction
coll.HitFlag = bUnHit; // UnHit signal is receding from outside target
}
coll.HitDistance = bnd; // 3dhit actual contact distance ...
//coll.M_hitRigid = rigid; // collision type
return(hitTime);
}
return(-1.0f);
}
private Vertex3DNoTex2[] CreateWire(int numRings, int numSegments, IReadOnlyList <Vertex2D> midPoints, IReadOnlyList <float> initialHeights)
{
var vertices = new Vertex3DNoTex2[numRings * numSegments];
var prev = new Vertex3D();
var index = 0;
for (var i = 0; i < numRings; i++)
{
var i2 = i == numRings - 1 ? i : i + 1;
var height = initialHeights[i];
var tangent = new Vertex3D(
midPoints[i2].X - midPoints[i].X,
midPoints[i2].Y - midPoints[i].Y,
initialHeights[i2] - initialHeights[i]
);
if (i == numRings - 1)
{
// for the last spline point use the previous tangent again, otherwise we won"t see the complete wire (it stops one control point too early)
tangent.X = midPoints[i].X - midPoints[i - 1].X;
tangent.Y = midPoints[i].Y - midPoints[i - 1].Y;
}
Vertex3D biNormal;
Vertex3D normal;
if (i == 0)
{
var up = new Vertex3D(
midPoints[i2].X + midPoints[i].X,
midPoints[i2].Y + midPoints[i].Y,
initialHeights[i2] - height
);
normal = tangent.Clone().Cross(up); //normal
biNormal = tangent.Clone().Cross(normal);
}
else
{
normal = prev.Clone().Cross(tangent);
biNormal = tangent.Clone().Cross(normal);
}
biNormal.Normalize();
normal.Normalize();
prev = biNormal;
var invNumRings = 1.0f / numRings;
var invNumSegments = 1.0f / numSegments;
var u = i * invNumRings;
for (var j = 0; j < numSegments; j++, index++)
{
var v = (j + u) * invNumSegments;
var tmp = Vertex3D
.GetRotatedAxis(j * (360.0f * invNumSegments), tangent, normal)
.MultiplyScalar(_data.WireDiameter * 0.5f);
vertices[index] = new Vertex3DNoTex2 {
X = midPoints[i].X + tmp.X,
Y = midPoints[i].Y + tmp.Y,
Z = height + tmp.Z,
Tu = u,
Tv = v
};
// normals
var n = new Vertex3D(
vertices[index].X - midPoints[i].X,
vertices[index].Y - midPoints[i].Y,
vertices[index].Z - height
);
var len = 1.0f / MathF.Sqrt(n.X * n.X + n.Y * n.Y + n.Z * n.Z);
vertices[index].Nx = n.X * len;
vertices[index].Ny = n.Y * len;
vertices[index].Nz = n.Z * len;
}
}
return(vertices);
}
public override float HitTest(Ball ball, float dTime, CollisionEvent coll, PlayerPhysics physics)
{
var d = _state.Pos.Clone().Sub(ball.State.Pos); // delta position
var dv = Vel.Clone().Sub(ball.Hit.Vel); // delta velocity
var bcddSq = d.LengthSq(); // square of ball center"s delta distance
var bcdd = MathF.Sqrt(bcddSq); // length of delta
if (bcdd < 1.0e-8)
{
// two balls center-over-center embedded
d.Z = -1.0f; // patch up
ball.State.Pos.Z -= d.Z; // lift up
bcdd = 1.0f; // patch up
bcddSq = 1.0f; // patch up
dv.Z = 0.1f; // small speed difference
ball.Hit.Vel.Z -= dv.Z;
}
var b = dv.Dot(d); // inner product
var bnv = b / bcdd; // normal speed of balls toward each other
if (bnv > PhysicsConstants.LowNormVel)
{
// dot of delta velocity and delta displacement, positive if receding no collision
return(-1.0f);
}
var totalRadius = ball.Data.Radius + _data.Radius;
var bnd = bcdd - totalRadius; // distance between ball surfaces
float hitTime;
var isContact = false;
if (bnd <= PhysicsConstants.PhysTouch)
{
// in contact?
if (bnd < ball.Data.Radius * -2.0f)
{
return(-1.0f); // embedded too deep?
}
if (MathF.Abs(bnv) > PhysicsConstants.ContactVel || // >fast velocity, return zero time
bnd <= -PhysicsConstants.PhysTouch)
{
// zero time for rigid fast bodies
hitTime = 0; // slow moving but embedded
}
else
{
hitTime = bnd / -bnv;
}
if (MathF.Abs(bnv) <= PhysicsConstants.ContactVel)
{
isContact = true;
}
}
else
{
var a = dv.LengthSq(); // square of differential velocity
if (a < 1.0e-8)
{
// ball moving really slow, then wait for contact
return(-1.0f);
}
var sol = Functions.SolveQuadraticEq(a, 2.0f * b, bcddSq - totalRadius * totalRadius);
if (sol == null)
{
return(-1.0f);
}
var(time1, time2) = sol;
hitTime = time1 * time2 < 0
? MathF.Max(time1, time2)
: MathF.Min(time1, time2); // find smallest nonnegative solution
}
if (float.IsNaN(hitTime) || float.IsInfinity(hitTime) || hitTime < 0 || hitTime > dTime)
{
// .. was some time previous || beyond the next physics tick
return(-1.0f);
}
var hitPos = ball.State.Pos.Clone().Add(dv.MultiplyScalar(hitTime)); // new ball position
// calc unit normal of collision
var hitNormal = hitPos.Clone().Sub(_state.Pos);
if (MathF.Abs(hitNormal.X) <= Constants.FloatMin && MathF.Abs(hitNormal.Y) <= Constants.FloatMin &&
MathF.Abs(hitNormal.Z) <= Constants.FloatMin)
{
return(-1.0f);
}
coll.HitNormal.Set(hitNormal).Normalize();
coll.HitDistance = bnd; // actual contact distance
coll.IsContact = isContact;
if (isContact)
{
coll.HitOrgNormalVelocity = bnv;
}
return(hitTime);
}
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;
}
}