public void ApplyImpulse(
LsmBody applyBody, Particle applyParticle, LsmBody otherBody, // HACK // TODO: try to don't use such information for collisions or formilize this ussage
double timeCoefficientPrediction, ref List<CollisionSubframeBuffer> collisionBuffer // HACK // TODO: remove ref List<>
)
{
Debug.Assert(!applyBody.Equals(otherBody)); // don't allow self-collision here
Debug.Assert(!applyBody.Frozen && !otherBody.Frozen);
// iterate all possible edges of body and test them with current subframed point
foreach (Particle bodyt in otherBody.particles)
{
if (bodyt.xPos != null)
{
CheckParticleEdge_D2D(
ref applyParticle.ccdDebugInfos, applyParticle, bodyt, bodyt.xPos,
ref collisionBuffer, timeCoefficientPrediction,
new CollisionSubframeBuffer(applyParticle, applyParticle.v, new Edge(bodyt, bodyt.xPos), bodyt.v, bodyt.xPos.v, 0.0)
);
}
if (bodyt.yPos != null)
{
CheckParticleEdge_D2D(
ref applyParticle.ccdDebugInfos, applyParticle, bodyt, bodyt.yPos,
ref collisionBuffer, timeCoefficientPrediction,
new CollisionSubframeBuffer(applyParticle, applyParticle.v, new Edge(bodyt, bodyt.yPos), bodyt.v, bodyt.yPos.v, 0.0)
);
}
}
}
public void ApplyImpulse_DynamicToFrozen(LsmBody applyBody, Particle applyParticle, LsmBody otherBody, double timeCoefficientPrediction, ref List<CollisionSubframeBuffer> collisionBuffer)
{
Debug.Assert(!applyBody.Equals(otherBody)); // don't allow self-collision here
Debug.Assert(!applyBody.Frozen && otherBody.Frozen);
foreach (Particle bodyt in otherBody.particles)
{
if (bodyt.xPos != null)
{
CheckParticleEdge_D2F(
ref applyParticle.ccdDebugInfos, applyParticle, bodyt, bodyt.xPos,
ref collisionBuffer, timeCoefficientPrediction,
new CollisionSubframeBuffer(applyParticle, applyParticle.v, new Edge(bodyt, bodyt.xPos), bodyt.v, bodyt.xPos.v, 0.0)
);
}
if (bodyt.yPos != null)
{
CheckParticleEdge_D2F(
ref applyParticle.ccdDebugInfos, applyParticle, bodyt, bodyt.yPos,
ref collisionBuffer, timeCoefficientPrediction,
new CollisionSubframeBuffer(applyParticle, applyParticle.v, new Edge(bodyt, bodyt.yPos), bodyt.v, bodyt.yPos.v, 0.0)
);
}
}
}
public void ApplyImpulse(
LsmBody applyBody, Particle applyParticle,
double timeCoefficientPrediction, ref List<CollisionSubframeBuffer> collisionBuffer // HACK // TODO: remove ref List<>
)
{
float left = this.left + border, right = this.right - border, bottom = this.bottom + border, top = this.top - border;
if (left < 0.0 || right < 0.0 || bottom < 0.0 || top < 0.0) // to prevent computation for incorrect case
return;
Vector2 pos = applyParticle.x;
Vector2 velocity = applyParticle.v;
Vector2 posNext = pos + velocity * timeCoefficientPrediction;
if (posNext.X < left)
{
collisionBuffer.Add(new CollisionSubframeBuffer(applyParticle, new Vector2(-velocity.X * BodyRepulse.coefficientElasticity, velocity.Y), null, Vector2.ZERO, Vector2.ZERO, (left - pos.X) / velocity.X));
}
if (posNext.X > right)
{
collisionBuffer.Add(new CollisionSubframeBuffer(applyParticle, new Vector2(-velocity.X * BodyRepulse.coefficientElasticity, velocity.Y), null, Vector2.ZERO, Vector2.ZERO, (right - pos.X) / velocity.X));
}
if (posNext.Y < bottom)
{
collisionBuffer.Add(new CollisionSubframeBuffer(applyParticle, new Vector2(velocity.X, -velocity.Y * BodyRepulse.coefficientElasticity), null, Vector2.ZERO, Vector2.ZERO, (bottom - pos.Y) / velocity.Y));
}
if (posNext.Y > top)
{
collisionBuffer.Add(new CollisionSubframeBuffer(applyParticle, new Vector2(velocity.X, -velocity.Y * BodyRepulse.coefficientElasticity), null, Vector2.ZERO, Vector2.ZERO, (top - pos.Y) / velocity.Y));
}
}
public CollisionSubframeBuffer(Particle particle, Vector2 vParticle, Edge edge, Vector2 vEdgeStart, Vector2 vEdgeEnd, double timeCoefficient)
{
Debug.Assert(!double.IsInfinity(timeCoefficient) && !double.IsNaN(timeCoefficient));
this.particle = particle;
this.vParticle = vParticle;
this.edge = edge;
this.vEdgeStart = vEdgeStart;
this.vEdgeEnd = vEdgeEnd;
this.timeCoefficient = timeCoefficient;
}
public CollisionSubframe(Particle particle, Vector2 vParticle, Edge edge, Vector2 vEdgeStart, Vector2 vEdgeEnd, double timeCoefficient)
{
//Debug.Assert(timeCoefficient > 0.0);
this.particle = particle;
this.vParticle = vParticle;
this.edge = edge;
this.vEdgeStart = vEdgeStart;
this.vEdgeEnd = vEdgeEnd;
this.timeCoefficient = timeCoefficient;
}
public override void ApplyForce(IEnumerable particles)
{
if (dragBodyFound)
return;
const int CUTOFF = 50;
const float PAUSED_MULT = 2f;
Vector2 mouseVec = new Vector2(mouse.X, mouse.Y);
if (lmbDown == true && oldLmbDown == false)
{
selected = null;
double closestDist = CUTOFF;
foreach (Particle t in particles)
{
double dist = (t.goal - mouseVec).Length();
if (dist < closestDist)
{
closestDist = dist;
selected = t;
}
}
}
if (selected != null)
{
if (selected.locked == false)
{
if (!Testbed.Paused && lmbDown)
{
selected.fExt += (mouseVec - selected.x) * selected.mass;
selected.v = Vector2.ZERO;
}
else if (Testbed.Paused && !lmbDown && oldLmbDown) // TODO: fix this HACK
{
selected.fExt += (mouseVec - selected.x) * PAUSED_MULT * selected.mass;
}
}
else if (lmbDown)
{
selected.x = mouseVec;
selected.v = Vector2.ZERO;
}
dragBodyFound = true;
}
}
public void RemoveParticle(Particle p)
{
p.parentRegions.Remove(this);
particles.Remove(p);
}
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;
}
}
public void AddParticle(Particle p)
{
particles.Add(p);
p.parentRegions.Add(this);
}
static Vector2 GetParticlePositionAtTime(Particle t, double givenTimeCoefficient)
{
return t.xPrior + givenTimeCoefficient * (t.x - t.xPrior);
}
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;
}
public bool ContainsParticle(Particle p)
{
return (particles.Contains(p));
}
public void GenerateFromBlueprint(bool[,] blueprint)
{
this.blueprint = blueprint;
width = blueprint.GetLength(0);
height = blueprint.GetLength(1);
lattice = new LatticePoint[width, height];
int x, y;
for (x = 0; x < width; x++)
{
for (y = 0; y < height; y++)
{
lattice[x, y] = new LatticePoint();
lattice[x,y].index = new Point(x,y);
if (blueprint[x, y] == true)
{
// Generate particle
Particle p = new Particle();
lattice[x, y].particle = p;
p.body = this;
p.x0 = new Vector2(spacing.X * x, spacing.Y * y);
p.x = offset + p.x0;
p.latticePoint = lattice[x, y];
p.goal = p.x;
particles.Add(p);
}
else
{
}
}
}
// Set up the neighbors
for (x = 0; x < width; x++)
{
for (y = 0; y < height; y++)
{
if (blueprint[x, y] == true)
{
if (InBounds(x + 1, y) && blueprint[x + 1, y] == true)
{
lattice[x, y].particle.xPos = lattice[x + 1, y].particle;
lattice[x + 1, y].particle.xNeg = lattice[x, y].particle;
}
if (InBounds(x, y + 1) && blueprint[x, y + 1] == true)
{
lattice[x, y].particle.yPos = lattice[x, y + 1].particle;
lattice[x, y + 1].particle.yNeg = lattice[x, y].particle;
}
}
}
}
// Create the smoothing regions and have them generate themselves
foreach (Particle p in particles)
{
SmoothingRegion s = new SmoothingRegion(w);
s.body = this;
s.latticePoint = p.latticePoint;
s.latticePoint.smoothingRegion = s;
s.RegenerateRegion();
smoothingRegions.Add(s);
}
// Create one chunk with all the particles in it
Chunk c = new Chunk();
foreach (Particle p in particles)
{
c.particles.Add(p);
p.chunk = c;
}
c.CalculateInvariants();
chunks.Add(c);
// Setup the regions
foreach (SmoothingRegion s in smoothingRegions)
{
s.CalculateInvariants();
}
}
public bool DoFracture(ref Particle other, ref Particle me)
{
bool somethingBroke = false;
double len = (other.goal - goal).Length();
double rest = (other.x0 - x0).Length();
double off = Math.Abs((len / rest) - 1.0);
if (off > LsmBody.fractureLengthTolerance)
{
somethingBroke = true;
Testbed.PostMessage("Length fracture: Rest = " + rest + ", actual = " + len);
}
if (!somethingBroke)
{
Vector2 a = new Vector2(other.R[0, 0], other.R[1, 0]);
Vector2 b = new Vector2(R[0, 0], R[1, 0]);
a.Normalize();
b.Normalize();
double angleDifference = Math.Acos(a.Dot(b));
if (angleDifference > LsmBody.fractureAngleTolerance)
{
somethingBroke = true;
Testbed.PostMessage("Angle fracture: angle difference = " + angleDifference);
}
}
if (somethingBroke)
{
Particle saved = other;
me = null;
other = null;
// Check if the chunks are still connected
Queue<Particle> edge = new Queue<Particle>();
List<Particle> found = new List<Particle>();
edge.Enqueue(this);
bool connected = false;
while (edge.Count > 0)
{
Particle p = edge.Dequeue();
if (!found.Contains(p))
{
found.Add(p);
if (p == saved)
{
// Connected
connected = true;
break;
}
if (p.xPos != null)
edge.Enqueue(p.xPos);
if (p.xNeg != null)
edge.Enqueue(p.xNeg);
if (p.yPos != null)
edge.Enqueue(p.yPos);
if (p.yNeg != null)
edge.Enqueue(p.yNeg);
}
}
if (connected == false)
{
// The chunks broke - there are now two separate chunks (maximally connected subgraphs)
chunk.particles.Clear();
chunk.particles.AddRange(found);
chunk.CalculateInvariants();
Chunk newChunk = new Chunk();
edge.Clear();
found.Clear();
edge.Enqueue(saved);
while (edge.Count > 0)
{
Particle p = edge.Dequeue();
if (!found.Contains(p))
{
found.Add(p);
p.chunk = newChunk;
if (p.xPos != null)
edge.Enqueue(p.xPos);
if (p.xNeg != null)
edge.Enqueue(p.xNeg);
if (p.yPos != null)
edge.Enqueue(p.yPos);
if (p.yNeg != null)
edge.Enqueue(p.yNeg);
}
}
newChunk.particles.AddRange(found);
newChunk.CalculateInvariants();
body.chunks.Add(newChunk);
Testbed.PostMessage("Chunk broken: the original chunk now has " + chunk.particles.Count + " particles, the new chunk has " + newChunk.particles.Count + " particles.");
Testbed.PostMessage("Number of chunks / particles: " + body.chunks.Count + " / " + body.particles.Count);
}
}
return somethingBroke;
}
public Edge(Particle start, Particle end)
{
this.start = start;
this.end = end;
}
public ParticleAndDepth(Particle t, int depth)
{
this.particle = t;
this.depth = depth;
}
static void RenderCollisionTrace(Particle t)
{
/*
Vector2 hPosition = t.xPrior;
Gl.glColor4d(0, 0, 0, 0.5);
Gl.glLineWidth(1.0f);
Gl.glBegin(Gl.GL_LINE_STRIP);
Gl.glVertex2d(hPosition.X, hPosition.Y);
foreach (CollisionSubframe subframe in t.collisionSubframes.Buffer)
{
hPosition += subframe.v * subframe.timeCoefficient;
Gl.glVertex2d(hPosition.X, hPosition.Y);
}
Gl.glEnd();
hPosition = t.xPrior;
Gl.glColor4d(1, 0.5, 0, 0.5);
Gl.glPointSize(4.0f);
Gl.glBegin(Gl.GL_POINTS);
foreach (CollisionSubframe subframe in t.collisionSubframes.Buffer)
{
hPosition += subframe.v * subframe.timeCoefficient;
Gl.glVertex2d(hPosition.X, hPosition.Y);
}
Gl.glEnd();
*/
}