private void CheckFrozenEdge(
Vector2 origin, Vector2 neighbor,
Vector2 pos, Vector2 posNext, Vector2 velocity, ref List <CollisionSubframeBuffer> collisionBuffer,
CollisionSubframeBuffer subframeToAdd
)
{
Vector2 intersection = new Vector2();
if (CollideSweptSegments(new LineSegment(origin, neighbor), new LineSegment(pos, posNext), ref intersection))
{
double particleOffset = (intersection - pos).Length();
if (particleOffset > epsilon) // to prevent slipping of start point to just reflected edge // TODO: check if this condition is really usefull. may be (timeOffset > 0.0) is a sufficient condition
{
// reflect velocity relative edge
Vector2 reflectSurface = neighbor - origin;
Vector2 reflectNormal = new Vector2(-reflectSurface.Y, reflectSurface.X);
if (reflectNormal.Dot(velocity) < 0)
{
reflectNormal = -reflectNormal;
}
Vector2 newVelocity = velocity - (1.0 + coefficientElasticity) * reflectNormal * (reflectNormal.Dot(velocity) / reflectNormal.LengthSq());
subframeToAdd.vParticle = newVelocity;
subframeToAdd.vEdgeStart = new Vector2(0.0, 0.0);
subframeToAdd.vEdgeEnd = new Vector2(0.0, 0.0);
subframeToAdd.timeCoefficient = particleOffset / velocity.Length();
collisionBuffer.Add(subframeToAdd);
}
}
}
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;
}
}
}
public static CollisionSubframeBuffer GetEarliestSubframe(List <CollisionSubframeBuffer> subframes)
{
CollisionSubframeBuffer earliestSubframe = null;
foreach (CollisionSubframeBuffer subframe in subframes)
{
if (earliestSubframe == null || earliestSubframe.timeCoefficient > subframe.timeCoefficient)
{
earliestSubframe = subframe;
}
}
return(earliestSubframe);
}
private CollisionSubframeBuffer GetEarliestCollision(List <CollisionSubframeBuffer> collisions)
{
CollisionSubframeBuffer earliestCollision = null;
foreach (CollisionSubframeBuffer collision in collisions)
{
if (earliestCollision == null || earliestCollision.timeCoefficient > collision.timeCoefficient)
{
earliestCollision = collision;
}
}
return(earliestCollision);
}
private CollisionSubframeBuffer GenerateContact_Reflection(
Particle particle, Particle origin, Particle neighbor, Particle.CCDDebugInfo ccd, double ccdCollisionTime, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
double alpha = ccd.coordinateOfPointOnEdge;
Vector2 velocityEdgeCollisionPoint = origin.v + (neighbor.v - origin.v) * alpha;
Vector2 velocityPointRelativeEdge = particle.v - velocityEdgeCollisionPoint;
// compute velocity reflection relativly moving edge
Vector2 reflectSurface = ccd.edge.end - ccd.edge.start;
Vector2 reflectNormal = new Vector2(-reflectSurface.Y, reflectSurface.X);
if (reflectNormal.Dot(velocityPointRelativeEdge) < 0)
{
reflectNormal = -reflectNormal;
}
Vector2 newVelocity =
velocityPointRelativeEdge - (1.0 + coefficientElasticity) * reflectNormal * (reflectNormal.Dot(velocityPointRelativeEdge) / reflectNormal.LengthSq());
if (ccdCollisionTime <= 0.0)
{
Testbed.PostMessage(System.Drawing.Color.Red, "timeCoefficient = 0"); // Zero-Distance not allowed // DEBUG
}
double newTimeCoefficient = timeCoefficientPrediction * ccdCollisionTime;
newTimeCoefficient -= epsilon / newVelocity.Length(); // try to prevent Zero-Distances // HACK // TODO: check Length() > epsilon
if (newTimeCoefficient < 0.0)
{
newTimeCoefficient = 0.0; // don't move particle toward edge - just reflect velocity
}
newVelocity += velocityEdgeCollisionPoint; // newVelocity should be in global coordinates
subframeToAdd.vParticle = newVelocity;
subframeToAdd.timeCoefficient = newTimeCoefficient;
return(subframeToAdd);
}
private CollisionSubframeBuffer GenerateContact_Reflection(
Particle particle, Particle origin, Particle neighbor, Particle.CCDDebugInfo ccd, double ccdCollisionTime, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
double alpha = ccd.coordinateOfPointOnEdge;
Vector2 velocityEdgeCollisionPoint = origin.v + (neighbor.v - origin.v) * alpha;
Vector2 velocityPointRelativeEdge = particle.v - velocityEdgeCollisionPoint;
// compute velocity reflection relativly moving edge
Vector2 reflectSurface = ccd.edge.end - ccd.edge.start;
Vector2 reflectNormal = new Vector2(-reflectSurface.Y, reflectSurface.X);
if (reflectNormal.Dot(velocityPointRelativeEdge) < 0) reflectNormal = -reflectNormal;
Vector2 newVelocity =
velocityPointRelativeEdge - (1.0 + coefficientElasticity) * reflectNormal * (reflectNormal.Dot(velocityPointRelativeEdge) / reflectNormal.LengthSq());
if (ccdCollisionTime <= 0.0) Testbed.PostMessage(System.Drawing.Color.Red, "timeCoefficient = 0"); // Zero-Distance not allowed // DEBUG
double newTimeCoefficient = timeCoefficientPrediction * ccdCollisionTime;
newTimeCoefficient -= epsilon / newVelocity.Length(); // try to prevent Zero-Distances // HACK // TODO: check Length() > epsilon
if (newTimeCoefficient < 0.0) newTimeCoefficient = 0.0; // don't move particle toward edge - just reflect velocity
newVelocity += velocityEdgeCollisionPoint; // newVelocity should be in global coordinates
subframeToAdd.vParticle = newVelocity;
subframeToAdd.timeCoefficient = newTimeCoefficient;
return subframeToAdd;
}
private CollisionSubframeBuffer GenerateContact_ImpulseConservation(
Particle particle, Particle origin, Particle neighbor, Particle.CCDDebugInfo ccd, double ccdCollisionTime, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
double alpha = ccd.coordinateOfPointOnEdge;
Vector2 edge = neighbor.x - origin.x;
double edgeLengthSq = edge.LengthSq();
if (edgeLengthSq < epsilon) // don't collide with too short edges // TODO: figure out how to solve this case
return null;
// 1. find mass of EdgeCollisionPoint:
// double massEdgeCollisionPoint = origin.mass + neighbor.mass; // ??? // mass of virtual point // alternative:
// m = origin.mass, alpha = 0
// m = origin.mass + neighbor.mass, alpha = neighbor.mass/(origin.mass + neighbor.mass)
// m = neighbor.mass, alpha = 1
//
// 2. use rule of impact for 2 bodies - it defines normal components of velocity of EdgeCollisionPoint (v2) and collisionParticle (v1). tangent components of velocities have no changes.
// v1new = v1 - m2*(v1-v2)*(1+k)/(m1+m2)
// v2new = v2 + m1*(v1-v2)*(1+k)/(m1+m2)
//
// k is a coefficient of elasticity, it belongs to [0..1]
// note that system lose some kinetic energy: dT = (0.5*(1-k^2)*m1*m2*(v1-v2)^2)/(m1+m2)
//
// 3. find new origin.v and new neighbor.v (origin.v' and neighbor.v') from found velocity of EdgeCollisionPoint
// // Rule of distribution for the EdgeCollisionPoint velocity on origin and neighbor
// velocityEdgeCollisionPoint' = origin.v' + (neighbor.v' - origin.v') * ccd.coordinateOfPointOnEdge // linear velocity distribution on edge
// massEdgeCollisionPoint * velocityEdgeCollisionPoint' = origin.mass * origin.v' + neighbor.mass * neighbor.v' // impulse of virtual point = sum of impulses of edge-vertex points
// // to distribute velocity from virtual points to edge vertices with impulse conservation
double alphaCenterOfMass = neighbor.mass / (origin.mass + neighbor.mass);
double betaLeft = alpha / alphaCenterOfMass;
double betaRight = (alpha - alphaCenterOfMass) / (1.0 - alphaCenterOfMass);
/**/
double massEdgeCollisionPoint = origin.mass + neighbor.mass; // simple mass approach
/*/
double massEdgeCollisionPoint = alpha < alphaCenterOfMass ? // complex mass approach
origin.mass * (1.0 - betaLeft) + (origin.mass + neighbor.mass) * betaLeft :
(origin.mass + neighbor.mass) * (1.0 - betaRight ) + neighbor.mass * betaRight;
/**/
Vector2 velocityEdgeCollisionPoint = origin.v + (neighbor.v - origin.v) * alpha;
Vector2 velocityEdgeCollisionPoint_Tangent = (velocityEdgeCollisionPoint.Dot(edge) / edgeLengthSq) * edge;
Vector2 velocityEdgeCollisionPoint_Normal = velocityEdgeCollisionPoint - velocityEdgeCollisionPoint_Tangent;
Vector2 velocityParticle_Tangent = (particle.v.Dot(edge) / edgeLengthSq) * edge;
Vector2 velocityParticle_Normal = particle.v - velocityParticle_Tangent;
Vector2 newVelocityECP_Tangent = velocityEdgeCollisionPoint_Tangent; // it means that we have no perticle-edge friction // TODO: implement some friction model
Vector2 newVelocityECP_Normal = velocityEdgeCollisionPoint_Normal +
((1.0 + coefficientElasticity) * particle.mass / (particle.mass + massEdgeCollisionPoint)) * (velocityParticle_Normal - velocityEdgeCollisionPoint_Normal);
Vector2 newVelocityECP = newVelocityECP_Tangent + newVelocityECP_Normal;
Vector2 newVelocityParticle_Tangent = velocityParticle_Tangent; // it means that we have no perticle-edge friction // TODO: implement some friction model
Vector2 newVelocityParticle_Normal = velocityParticle_Normal -
((1.0 + coefficientElasticity) * massEdgeCollisionPoint / (particle.mass + massEdgeCollisionPoint)) * (velocityParticle_Normal - velocityEdgeCollisionPoint_Normal);
Vector2 newVelocityParticle = newVelocityParticle_Tangent + newVelocityParticle_Normal;
if (ccdCollisionTime <= 0.0) Testbed.PostMessage(System.Drawing.Color.Red, "timeCoefficient = 0"); // Zero-Distance not allowed // DEBUG
double newTimeCoefficient = timeCoefficientPrediction * ccdCollisionTime;
newTimeCoefficient -= epsilon / (newVelocityParticle - newVelocityECP).Length(); // try to prevent Zero-Distances // HACK // TODO: check Length() > epsilon
if (newTimeCoefficient < 0.0) newTimeCoefficient = 0.0; // don't move particle toward edge - just reflect velocity
Vector2 newVelocityOrigin = alpha < alphaCenterOfMass ?
newVelocityECP * (1.0 - betaLeft) + (origin.v + newVelocityECP - velocityEdgeCollisionPoint) * betaLeft :
(origin.v + newVelocityECP - velocityEdgeCollisionPoint) * (1.0 - betaRight) + origin.v * betaRight;
Vector2 newVelocityNeighbor = alpha < alphaCenterOfMass ?
neighbor.v * (1.0 - betaLeft) + (neighbor.v + newVelocityECP - velocityEdgeCollisionPoint) * betaLeft :
(neighbor.v + newVelocityECP - velocityEdgeCollisionPoint) * (1.0 - betaRight) + newVelocityECP * betaRight;
subframeToAdd.vParticle = newVelocityParticle;
subframeToAdd.vEdgeStart = newVelocityOrigin;
subframeToAdd.vEdgeEnd = newVelocityNeighbor;
subframeToAdd.timeCoefficient = newTimeCoefficient;
return subframeToAdd;
}
private CollisionSubframeBuffer GenerateContact_ImpulseConservation_F2D(
Particle particle, Particle origin, Particle neighbor, Particle.CCDDebugInfo ccd, double ccdCollisionTime, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
double alpha = ccd.coordinateOfPointOnEdge;
Vector2 edge = neighbor.x - origin.x;
double edgeLengthSq = edge.LengthSq();
if (edgeLengthSq < epsilon)
return null;
double alphaCenterOfMass = neighbor.mass / (origin.mass + neighbor.mass);
double betaLeft = alpha / alphaCenterOfMass;
double betaRight = (alpha - alphaCenterOfMass) / (1.0 - alphaCenterOfMass);
/**/
double massEdgeCollisionPoint = origin.mass + neighbor.mass; // simple mass approach
/*/
double massEdgeCollisionPoint = alpha < alphaCenterOfMass ? // complex mass approach
origin.mass * (1.0 - betaLeft) + (origin.mass + neighbor.mass) * betaLeft :
(origin.mass + neighbor.mass) * (1.0 - betaRight ) + neighbor.mass * betaRight;
/**/
Vector2 velocityEdgeCollisionPoint = origin.v + (neighbor.v - origin.v) * alpha;
Vector2 velocityEdgeCollisionPoint_Tangent = (velocityEdgeCollisionPoint.Dot(edge) / edgeLengthSq) * edge;
Vector2 velocityEdgeCollisionPoint_Normal = velocityEdgeCollisionPoint - velocityEdgeCollisionPoint_Tangent;
Vector2 velocityParticle_Tangent = Vector2.ZERO;
Vector2 velocityParticle_Normal = Vector2.ZERO;
// particle.mass = infinity
// particle.v = newVelocityParticle = Vector2.ZERO
Vector2 newVelocityECP_Tangent = velocityEdgeCollisionPoint_Tangent; // it means that we have no particle-edge friction // TODO: implement some friction model
Vector2 newVelocityECP_Normal = -(1.0 + coefficientElasticity) * velocityEdgeCollisionPoint_Normal;
Vector2 newVelocityECP = newVelocityECP_Tangent + newVelocityECP_Normal;
Vector2 newVelocityParticle = Vector2.ZERO;
if (ccdCollisionTime <= 0.0) Testbed.PostMessage(System.Drawing.Color.Red, "timeCoefficient = 0"); // Zero-Distance not allowed // DEBUG
double newTimeCoefficient = timeCoefficientPrediction * ccdCollisionTime;
newTimeCoefficient -= epsilon / (newVelocityParticle - newVelocityECP).Length(); // try to prevent Zero-Distances // HACK // TODO: check Length() > epsilon
if (newTimeCoefficient < 0.0) newTimeCoefficient = 0.0; // don't move particle toward edge - just reflect velocity
Vector2 newVelocityOrigin = alpha < alphaCenterOfMass ?
newVelocityECP * (1.0 - betaLeft) + (origin.v + newVelocityECP - velocityEdgeCollisionPoint) * betaLeft :
(origin.v + newVelocityECP - velocityEdgeCollisionPoint) * (1.0 - betaRight) + origin.v * betaRight;
Vector2 newVelocityNeighbor = alpha < alphaCenterOfMass ?
neighbor.v * (1.0 - betaLeft) + (neighbor.v + newVelocityECP - velocityEdgeCollisionPoint) * betaLeft :
(neighbor.v + newVelocityECP - velocityEdgeCollisionPoint) * (1.0 - betaRight) + newVelocityECP * betaRight;
subframeToAdd.vParticle = newVelocityParticle;
subframeToAdd.vEdgeStart = newVelocityOrigin;
subframeToAdd.vEdgeEnd = newVelocityNeighbor;
subframeToAdd.timeCoefficient = newTimeCoefficient;
return subframeToAdd;
}
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_D2F(
ref List<Particle.CCDDebugInfo> ccds,
Particle particle,
Particle origin, Particle neighbor,
ref List<CollisionSubframeBuffer> collisionBuffer, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
// TODO: avoid code coping
Vector2 pos = particle.x;
Vector2 velocity = particle.v;
Vector2 posNext = pos + velocity * timeCoefficientPrediction;
// simple collision for frozen body
CheckFrozenEdge(origin.goal, neighbor.goal, pos, posNext, velocity, ref collisionBuffer, subframeToAdd); // current edge position
}
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;
}
}
private void CheckFrozenEdge(
Vector2 origin, Vector2 neighbor,
Vector2 pos, Vector2 posNext, Vector2 velocity, ref List<CollisionSubframeBuffer> collisionBuffer,
CollisionSubframeBuffer subframeToAdd
)
{
Vector2 intersection = new Vector2();
if (CollideSweptSegments(new LineSegment(origin, neighbor), new LineSegment(pos, posNext), ref intersection))
{
double particleOffset = (intersection - pos).Length();
if (particleOffset > epsilon) // to prevent slipping of start point to just reflected edge // TODO: check if this condition is really usefull. may be (timeOffset > 0.0) is a sufficient condition
{
// reflect velocity relative edge
Vector2 reflectSurface = neighbor - origin;
Vector2 reflectNormal = new Vector2(-reflectSurface.Y, reflectSurface.X);
if (reflectNormal.Dot(velocity) < 0) reflectNormal = -reflectNormal;
Vector2 newVelocity = velocity - (1.0 + coefficientElasticity) * reflectNormal * (reflectNormal.Dot(velocity) / reflectNormal.LengthSq());
subframeToAdd.vParticle = newVelocity;
subframeToAdd.vEdgeStart = new Vector2(0.0, 0.0);
subframeToAdd.vEdgeEnd = new Vector2(0.0, 0.0);
subframeToAdd.timeCoefficient = particleOffset / velocity.Length();
collisionBuffer.Add(subframeToAdd);
}
}
}
private CollisionSubframeBuffer GenerateContact_ImpulseConservation(
Particle particle, Particle origin, Particle neighbor, Particle.CCDDebugInfo ccd, double ccdCollisionTime, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
double alpha = ccd.coordinateOfPointOnEdge;
Vector2 edge = neighbor.x - origin.x;
double edgeLengthSq = edge.LengthSq();
if (edgeLengthSq < epsilon) // don't collide with too short edges // TODO: figure out how to solve this case
{
return(null);
}
// 1. find mass of EdgeCollisionPoint:
// double massEdgeCollisionPoint = origin.mass + neighbor.mass; // ??? // mass of virtual point // alternative:
// m = origin.mass, alpha = 0
// m = origin.mass + neighbor.mass, alpha = neighbor.mass/(origin.mass + neighbor.mass)
// m = neighbor.mass, alpha = 1
//
// 2. use rule of impact for 2 bodies - it defines normal components of velocity of EdgeCollisionPoint (v2) and collisionParticle (v1). tangent components of velocities have no changes.
// v1new = v1 - m2*(v1-v2)*(1+k)/(m1+m2)
// v2new = v2 + m1*(v1-v2)*(1+k)/(m1+m2)
//
// k is a coefficient of elasticity, it belongs to [0..1]
// note that system lose some kinetic energy: dT = (0.5*(1-k^2)*m1*m2*(v1-v2)^2)/(m1+m2)
//
// 3. find new origin.v and new neighbor.v (origin.v' and neighbor.v') from found velocity of EdgeCollisionPoint
// // Rule of distribution for the EdgeCollisionPoint velocity on origin and neighbor
// velocityEdgeCollisionPoint' = origin.v' + (neighbor.v' - origin.v') * ccd.coordinateOfPointOnEdge // linear velocity distribution on edge
// massEdgeCollisionPoint * velocityEdgeCollisionPoint' = origin.mass * origin.v' + neighbor.mass * neighbor.v' // impulse of virtual point = sum of impulses of edge-vertex points
// // to distribute velocity from virtual points to edge vertices with impulse conservation
double alphaCenterOfMass = neighbor.mass / (origin.mass + neighbor.mass);
double betaLeft = alpha / alphaCenterOfMass;
double betaRight = (alpha - alphaCenterOfMass) / (1.0 - alphaCenterOfMass);
/**/
double massEdgeCollisionPoint = origin.mass + neighbor.mass; // simple mass approach
/*/
* double massEdgeCollisionPoint = alpha < alphaCenterOfMass ? // complex mass approach
* origin.mass * (1.0 - betaLeft) + (origin.mass + neighbor.mass) * betaLeft :
* (origin.mass + neighbor.mass) * (1.0 - betaRight ) + neighbor.mass * betaRight;
* /**/
Vector2 velocityEdgeCollisionPoint = origin.v + (neighbor.v - origin.v) * alpha;
Vector2 velocityEdgeCollisionPoint_Tangent = (velocityEdgeCollisionPoint.Dot(edge) / edgeLengthSq) * edge;
Vector2 velocityEdgeCollisionPoint_Normal = velocityEdgeCollisionPoint - velocityEdgeCollisionPoint_Tangent;
Vector2 velocityParticle_Tangent = (particle.v.Dot(edge) / edgeLengthSq) * edge;
Vector2 velocityParticle_Normal = particle.v - velocityParticle_Tangent;
Vector2 newVelocityECP_Tangent = velocityEdgeCollisionPoint_Tangent; // it means that we have no perticle-edge friction // TODO: implement some friction model
Vector2 newVelocityECP_Normal = velocityEdgeCollisionPoint_Normal +
((1.0 + coefficientElasticity) * particle.mass / (particle.mass + massEdgeCollisionPoint)) * (velocityParticle_Normal - velocityEdgeCollisionPoint_Normal);
Vector2 newVelocityECP = newVelocityECP_Tangent + newVelocityECP_Normal;
Vector2 newVelocityParticle_Tangent = velocityParticle_Tangent; // it means that we have no perticle-edge friction // TODO: implement some friction model
Vector2 newVelocityParticle_Normal = velocityParticle_Normal -
((1.0 + coefficientElasticity) * massEdgeCollisionPoint / (particle.mass + massEdgeCollisionPoint)) * (velocityParticle_Normal - velocityEdgeCollisionPoint_Normal);
Vector2 newVelocityParticle = newVelocityParticle_Tangent + newVelocityParticle_Normal;
if (ccdCollisionTime <= 0.0)
{
Testbed.PostMessage(System.Drawing.Color.Red, "timeCoefficient = 0"); // Zero-Distance not allowed // DEBUG
}
double newTimeCoefficient = timeCoefficientPrediction * ccdCollisionTime;
newTimeCoefficient -= epsilon / (newVelocityParticle - newVelocityECP).Length(); // try to prevent Zero-Distances // HACK // TODO: check Length() > epsilon
if (newTimeCoefficient < 0.0)
{
newTimeCoefficient = 0.0; // don't move particle toward edge - just reflect velocity
}
Vector2 newVelocityOrigin = alpha < alphaCenterOfMass ?
newVelocityECP * (1.0 - betaLeft) + (origin.v + newVelocityECP - velocityEdgeCollisionPoint) * betaLeft :
(origin.v + newVelocityECP - velocityEdgeCollisionPoint) * (1.0 - betaRight) + origin.v * betaRight;
Vector2 newVelocityNeighbor = alpha < alphaCenterOfMass ?
neighbor.v * (1.0 - betaLeft) + (neighbor.v + newVelocityECP - velocityEdgeCollisionPoint) * betaLeft :
(neighbor.v + newVelocityECP - velocityEdgeCollisionPoint) * (1.0 - betaRight) + newVelocityECP * betaRight;
subframeToAdd.vParticle = newVelocityParticle;
subframeToAdd.vEdgeStart = newVelocityOrigin;
subframeToAdd.vEdgeEnd = newVelocityNeighbor;
subframeToAdd.timeCoefficient = newTimeCoefficient;
return(subframeToAdd);
}
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;
}
}
}
Particle.CCDDebugInfo GenerateDebugInfo(EdgePointCCDSolver.SolverInput solverInput, double time) // DEBUG
{
Particle.CCDDebugInfo ccd = new Particle.CCDDebugInfo(
solverInput.currentPoint + (solverInput.nextPoint - solverInput.currentPoint) * time,
new LineSegment(
solverInput.currentEdge.start + (solverInput.nextEdge.start - solverInput.currentEdge.start) * time,
solverInput.currentEdge.end + (solverInput.nextEdge.end - solverInput.currentEdge.end) * time
)
);
return(ccd);
}
private void CheckParticleEdge_D2F(
ref List <Particle.CCDDebugInfo> ccds,
Particle particle,
Particle origin, Particle neighbor,
ref List <CollisionSubframeBuffer> collisionBuffer, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
// TODO: avoid code coping
Vector2 pos = particle.x;
Vector2 velocity = particle.v;
Vector2 posNext = pos + velocity * timeCoefficientPrediction;
// simple collision for frozen body
CheckFrozenEdge(origin.goal, neighbor.goal, pos, posNext, velocity, ref collisionBuffer, subframeToAdd); // current edge position
}
private CollisionSubframeBuffer GenerateContact_ImpulseConservation_F2D(
Particle particle, Particle origin, Particle neighbor, Particle.CCDDebugInfo ccd, double ccdCollisionTime, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
double alpha = ccd.coordinateOfPointOnEdge;
Vector2 edge = neighbor.x - origin.x;
double edgeLengthSq = edge.LengthSq();
if (edgeLengthSq < epsilon)
{
return(null);
}
double alphaCenterOfMass = neighbor.mass / (origin.mass + neighbor.mass);
double betaLeft = alpha / alphaCenterOfMass;
double betaRight = (alpha - alphaCenterOfMass) / (1.0 - alphaCenterOfMass);
/**/
double massEdgeCollisionPoint = origin.mass + neighbor.mass; // simple mass approach
/*/
* double massEdgeCollisionPoint = alpha < alphaCenterOfMass ? // complex mass approach
* origin.mass * (1.0 - betaLeft) + (origin.mass + neighbor.mass) * betaLeft :
* (origin.mass + neighbor.mass) * (1.0 - betaRight ) + neighbor.mass * betaRight;
* /**/
Vector2 velocityEdgeCollisionPoint = origin.v + (neighbor.v - origin.v) * alpha;
Vector2 velocityEdgeCollisionPoint_Tangent = (velocityEdgeCollisionPoint.Dot(edge) / edgeLengthSq) * edge;
Vector2 velocityEdgeCollisionPoint_Normal = velocityEdgeCollisionPoint - velocityEdgeCollisionPoint_Tangent;
Vector2 velocityParticle_Tangent = Vector2.ZERO;
Vector2 velocityParticle_Normal = Vector2.ZERO;
// particle.mass = infinity
// particle.v = newVelocityParticle = Vector2.ZERO
Vector2 newVelocityECP_Tangent = velocityEdgeCollisionPoint_Tangent; // it means that we have no particle-edge friction // TODO: implement some friction model
Vector2 newVelocityECP_Normal = -(1.0 + coefficientElasticity) * velocityEdgeCollisionPoint_Normal;
Vector2 newVelocityECP = newVelocityECP_Tangent + newVelocityECP_Normal;
Vector2 newVelocityParticle = Vector2.ZERO;
if (ccdCollisionTime <= 0.0)
{
Testbed.PostMessage(System.Drawing.Color.Red, "timeCoefficient = 0"); // Zero-Distance not allowed // DEBUG
}
double newTimeCoefficient = timeCoefficientPrediction * ccdCollisionTime;
newTimeCoefficient -= epsilon / (newVelocityParticle - newVelocityECP).Length(); // try to prevent Zero-Distances // HACK // TODO: check Length() > epsilon
if (newTimeCoefficient < 0.0)
{
newTimeCoefficient = 0.0; // don't move particle toward edge - just reflect velocity
}
Vector2 newVelocityOrigin = alpha < alphaCenterOfMass ?
newVelocityECP * (1.0 - betaLeft) + (origin.v + newVelocityECP - velocityEdgeCollisionPoint) * betaLeft :
(origin.v + newVelocityECP - velocityEdgeCollisionPoint) * (1.0 - betaRight) + origin.v * betaRight;
Vector2 newVelocityNeighbor = alpha < alphaCenterOfMass ?
neighbor.v * (1.0 - betaLeft) + (neighbor.v + newVelocityECP - velocityEdgeCollisionPoint) * betaLeft :
(neighbor.v + newVelocityECP - velocityEdgeCollisionPoint) * (1.0 - betaRight) + newVelocityECP * betaRight;
subframeToAdd.vParticle = newVelocityParticle;
subframeToAdd.vEdgeStart = newVelocityOrigin;
subframeToAdd.vEdgeEnd = newVelocityNeighbor;
subframeToAdd.timeCoefficient = newTimeCoefficient;
return(subframeToAdd);
}
public void Update()
{
// update internal processes in bodies, find velocities
foreach (LsmBody b in bodies)
{
if (!b.Frozen)
{
foreach (IEnvironmentForce force in environmentForces)
{
if (force is WallForce && !b.UseWallForce) // HACK to apply custom forces // TODO: make correct system
{
continue;
}
force.ApplyForce(b.particles);
}
b.Smooth();
b.DoFracture();
b.UpdateParticlesVelocity();
}
else
{
b.UpdateFrozenParticlesVelocity();
}
}
// iterate subframes for collision and integration system of bodies
List <CollisionSubframeBuffer> collisionBuffer = new List <CollisionSubframeBuffer>();
int iterationsCounter = 0; // to prevent deadlocks
const int maxIterations = 64; // to prevent deadlocks
double timeCoefficientPrediction = 1.0;
bool collisionFound = false;
do
{
for (int i = 0; i < bodies.Count; ++i)
{
LsmBody bLeft = bodies[i];
if (!bLeft.Frozen && !bLeft.UseWallForce)
{
bLeft.CollideWithWall(timeCoefficientPrediction, ref collisionBuffer); // TODO: refactor to remove 'ref collisionBuffer'
}
for (int j = i + 1; j < bodies.Count; ++j) // TODO: implement self-collisions for j == i.
{
LsmBody bRight = bodies[j];
LsmBody.CollideBodies(bLeft, bRight, timeCoefficientPrediction, ref collisionBuffer); // TODO: refactor to remove 'ref collisionBuffer'
LsmBody.CollideBodies(bRight, bLeft, timeCoefficientPrediction, ref collisionBuffer); // TODO: refactor to remove 'ref collisionBuffer'
}
}
double timeCoefficientIntegrate = 0.0;
if (collisionBuffer.Count > 0)
{
CollisionSubframeBuffer subframe = LsmBody.GetEarliestSubframe(collisionBuffer); // WARNING: now we assume that in 1 time moment we have maximum 1 collision in subframe.
// TODO: make system ready to handle multi-collision subframes. For example, we'll need to accumulate velocity deltas for every particle and then make summation - not just direct assign of values.
timeCoefficientIntegrate = subframe.timeCoefficient;
timeCoefficientPrediction -= subframe.timeCoefficient;
subframe.particle.v = subframe.vParticle;
if (subframe.edge != null)
{
subframe.edge.start.v = subframe.vEdgeStart;
subframe.edge.end.v = subframe.vEdgeEnd;
}
collisionBuffer.Clear();
collisionFound = true;
}
else
{
timeCoefficientIntegrate = timeCoefficientPrediction;
collisionFound = false;
}
if (timeCoefficientIntegrate > 0.0)
{
foreach (LsmBody b in bodies)
{
if (!b.Frozen)
{
b.UpdateParticlesPosition(timeCoefficientIntegrate);
}
else
{
b.UpdateFrozenParticlesPosition();
}
}
}
else
{
Testbed.PostMessage(System.Drawing.Color.Yellow, "Timestep <= 0.0 while subframes iterating!"); // DEBUG
}
if (++iterationsCounter >= maxIterations) // to prevent deadlocks
{
Testbed.PostMessage(System.Drawing.Color.Red, "Deadlock detected in HandleCollisions!"); // DEBUG
if (LsmBody.pauseOnDeadlock)
{
Testbed.Paused = true;
}
break;
}
}while (collisionFound);
}
