private void ComputeSteps()
{
var validBalls = m_Balls.Where(b => b.InGame).ToList();
m_BallSteps.Clear();
for (int i = 0; i < validBalls.Count; i++)
{
var info = new BallStepInfo(double.PositiveInfinity);
for (int j = i + 1; j < validBalls.Count; j++)
{
CheckBallCollision(ref info, validBalls[i], validBalls[j]);
}
m_BallSteps[validBalls[i]] = info;
}
}
private void HandleBallCollision(IBallInternal ball, BallStepInfo info, Dictionary <IBallInternal, NewVelocity> newVelocities)
{
var validBalls = info.CollidedBalls.Where(b => b.InGame);
//prepare linear system wich solutions gives the impulses
Matrix A = new Matrix(validBalls.Count());
Vector B = new Vector(validBalls.Count());
foreach (var iBall in validBalls.Select((Value, Idx) => new { Value, Idx }))
{
var iNorm = info.GetNormalVersor(iBall.Value);
//build matrix A
foreach (var jBall in Enumerable.Where(validBalls.Select((Value, Idx) => new { Value, Idx }), e => e.Idx <= iBall.Idx))
{
if (iBall.Idx == jBall.Idx)
{
A[iBall.Idx, iBall.Idx] = (1 / ball.Mass + 1 / iBall.Value.Mass);
}
else
{
var jNorm = info.GetNormalVersor(jBall.Value);
A[iBall.Idx, jBall.Idx] = Coordinates.Dot(iNorm, jNorm) / ball.Mass;
A[jBall.Idx, iBall.Idx] = A[iBall.Idx, jBall.Idx];
}
}
//build known terms vector
B[iBall.Idx] = (1 + Math.Min(iBall.Value.Elasticity, ball.Elasticity)) * Coordinates.Dot(Coordinates.Sub(iBall.Value.Velocity, ball.Velocity), iNorm);
}
//solve the system
var x = LinearSystem.Solve(A, B);
//apply the impulses
foreach (var iBall in validBalls.Select((Value, Idx) => new { Value, Idx }))
{
var iNorm = info.GetNormalVersor(iBall.Value);
newVelocities[ball].Value = Coordinates.Add(newVelocities[ball].Value, x[iBall.Idx] / ball.Mass * iNorm);
newVelocities[iBall.Value].Value = Coordinates.Sub(newVelocities[iBall.Value].Value, x[iBall.Idx] / iBall.Value.Mass * iNorm);
iBall.Value.SetHitSomethingFlag(true);
}
ball.SetHitSomethingFlag(validBalls.Count() != 0);
}
private void CheckBallCollision(ref BallStepInfo info, IBallInternal first, IBallInternal second)
{
if (first.Velocity.Module != 0 || second.Velocity.Module != 0)
{
int N = 0;
double distance;
double nextDistance;
double step;
Coordinates normal;
var tmp = ComponentManager.Cache.Get(first.Position, first.Velocity, first.Friction, second.Position, second.Velocity, second.Friction);
if (tmp != null)
{
int iSteps = (int)Math.Floor(tmp.Step);
distance = GeometryUtils.Distance(first.ForeseenPosition(iSteps + 1), second.ForeseenPosition(iSteps + 1));
nextDistance = GeometryUtils.Distance(first.ForeseenPosition(iSteps + 2), second.ForeseenPosition(iSteps + 2));
step = tmp.Step;
normal = tmp.Normal;
}
else
{
nextDistance = GeometryUtils.Distance(first.ForeseenPosition(N), second.ForeseenPosition(N));
do
{
distance = nextDistance;
nextDistance = GeometryUtils.Distance(first.ForeseenPosition(N + 1), second.ForeseenPosition(N + 1));
N++;
}while (distance > nextDistance && distance.ToleranceGreater(first.Radius + second.Radius));
N -= 2;
if (N < 0)
{
step = 0;
normal = Coordinates.Normalize(Coordinates.Sub(second.Position, first.Position));
}
else
{
var p1 = first.ForeseenPosition(N);
var p2 = second.ForeseenPosition(N);
var v1 = first.ForeseenVelocity(N);
var v2 = second.ForeseenVelocity(N);
var deltaP = Coordinates.Sub(p1, p2);
var deltaV = Coordinates.Sub(v1, v2);
var a = Coordinates.Dot(deltaV, deltaV);
var b = 2 * Coordinates.Dot(deltaP, deltaV);
var c = Coordinates.Dot(deltaP, deltaP) - Math.Pow(first.Radius + second.Radius, 2);
var t1 = (-b + Math.Sqrt(b * b - 4 * a * c)) / (2 * a);
var t2 = (-b - Math.Sqrt(b * b - 4 * a * c)) / (2 * a);
double t = (t1.ToleranceGreaterEqual(0) && t2.ToleranceGreaterEqual(0)) ? Math.Min(t1, t2) : (t1.ToleranceGreaterEqual(0) ? t1 : (t2.ToleranceGreaterEqual(0) ? t2 : 0));
step = N + t;
normal = Coordinates.Normalize(Coordinates.Sub(Coordinates.Add(p2, t * v2), (Coordinates.Add(p1, t * v1))));
}
ComponentManager.Cache.Add(step, normal, Coordinates.Zero, first.Position, first.Velocity, first.Friction, second.Position, second.Velocity, second.Friction);
}
if (distance.ToleranceLessEqual(first.Radius + second.Radius) && ((step.ToleranceGreater(0) && step.ToleranceLessEqual(info.Step)) || (step.ToleranceEqual(0) && distance > nextDistance)))
{
if (!step.ToleranceEqual(info.Step))
{
info = new BallStepInfo(step);
}
info.AddCollidedBall(second, normal);
}
}
}
