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); }