static bool IsSufficientCondition(SolverInput input, double time)
{
Debug.Assert(time >= 0.0 && time <= 1.0);
LineSegment edge = new LineSegment(input.currentEdge.start + (input.nextEdge.start - input.currentEdge.start) * time, input.currentEdge.end + (input.nextEdge.end - input.currentEdge.end) * time);
Vector2 point = input.currentPoint + (input.nextPoint - input.currentPoint) * time;
return IsPointBelongsToEdge(edge, point);
}
private bool CollideSweptSegments(LineSegment first, LineSegment second, ref Vector2 intersection)
{
double Ua, Ub;
// Equations to determine whether lines intersect
Ua = ((second.end.X - second.start.X) * (first.start.Y - second.start.Y) - (second.end.Y - second.start.Y) * (first.start.X - second.start.X)) /
((second.end.Y - second.start.Y) * (first.end.X - first.start.X) - (second.end.X - second.start.X) * (first.end.Y - first.start.Y));
Ub = ((first.end.X - first.start.X) * (first.start.Y - second.start.Y) - (first.end.Y - first.start.Y) * (first.start.X - second.start.X)) /
((second.end.Y - second.start.Y) * (first.end.X - first.start.X) - (second.end.X - second.start.X) * (first.end.Y - first.start.Y));
if (Ua >= 0.0f && Ua <= 1.0f && Ub >= 0.0f && Ub <= 1.0f)
{
intersection.X = first.start.X + Ua*(first.end.X - first.start.X);
intersection.Y = first.start.Y + Ua*(first.end.Y - first.start.Y);
return true;
}
return false;
}
private void CheckParticleEdge_F2D(
ref List<Particle.CCDDebugInfo> ccds,
Particle particle,
Particle origin, Particle neighbor,
ref List<CollisionSubframeBuffer> collisionBuffer, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
// swept collision for body
LineSegment edge = new LineSegment(origin.x, neighbor.x);
LineSegment edgeNext = new LineSegment(edge.start + origin.v * timeCoefficientPrediction, edge.end + neighbor.v * timeCoefficientPrediction);
EdgePointCCDSolver.SolverInput solverInput = new EdgePointCCDSolver.SolverInput(edge, edgeNext, particle.x, particle.x);
double? ccdCollisionTime = EdgePointCCDSolver.Solve(solverInput);
if (ccdCollisionTime != null)
{
Particle.CCDDebugInfo ccd = GenerateDebugInfo(solverInput, ccdCollisionTime.Value);
CollisionSubframeBuffer contact =
GenerateContact_ImpulseConservation_F2D(
particle, origin, neighbor, ccd, ccdCollisionTime.Value, timeCoefficientPrediction,
subframeToAdd
);
if (contact != null)
{
collisionBuffer.Add(contact);
}
ccds.Add(ccd);
if (LsmBody.pauseOnBodyBodyCollision)
Testbed.Paused = true;
}
}
private void CheckParticleEdge_D2D(
ref List<Particle.CCDDebugInfo> ccds,
Particle particle,
Particle origin, Particle neighbor,
ref List<CollisionSubframeBuffer> collisionBuffer, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
// TODO: avoid code coping (see below)
Vector2 pos = particle.x;
Vector2 velocity = particle.v;
Vector2 posNext = pos + velocity * timeCoefficientPrediction;
// swept collision for body
LineSegment edge = new LineSegment(origin.x, neighbor.x);
LineSegment edgeNext = new LineSegment(edge.start + origin.v * timeCoefficientPrediction, edge.end + neighbor.v * timeCoefficientPrediction);
EdgePointCCDSolver.SolverInput solverInput = new EdgePointCCDSolver.SolverInput(edge, edgeNext, pos, posNext);
double? ccdCollisionTime = EdgePointCCDSolver.Solve(solverInput);
if (ccdCollisionTime != null)
{
Particle.CCDDebugInfo ccd = GenerateDebugInfo(solverInput, ccdCollisionTime.Value);
CollisionSubframeBuffer contact = // TODO: use the Rule of conservative impulse to handle this case. Simple reflection rule is not effective here.
GenerateContact_ImpulseConservation(
// GenerateContact_Reflection(
particle, origin, neighbor, ccd, ccdCollisionTime.Value, timeCoefficientPrediction, subframeToAdd
);
if (contact != null)
{
collisionBuffer.Add(contact);
}
ccds.Add(ccd);
if (LsmBody.pauseOnBodyBodyCollision)
Testbed.Paused = true;
}
}
public CCDDebugInfo(Vector2 point, LineSegment edge)
{
this.point = point;
this.edge = edge;
this.coordinateOfPointOnEdge = (point - edge.start).Dot(edge.end - edge.start) / (edge.end - edge.start).LengthSq();
}
static bool IsPointBelongsToEdge(LineSegment edge, Vector2 point)
{
Vector2 edgeDirection = edge.end - edge.start;
double edgeCoordinate = edgeDirection.Dot(point - edge.start) / edgeDirection.LengthSq();
return edgeCoordinate >= 0.0 && edgeCoordinate <= 1.0;
}
public SolverInput(LineSegment currentEdge, LineSegment nextEdge, Vector2 currentPoint, Vector2 nextPoint)
{
this.currentEdge = currentEdge;
this.nextEdge = nextEdge;
this.currentPoint = currentPoint;
this.nextPoint = nextPoint;
}
