private void CreateSprings(FluidParticle p) { for (int i = 0; i < p.Neighbours.Count; ++i) { FluidParticle neighbour = p.Neighbours[i]; if (p.Index >= neighbour.Index) { continue; } float q; Vector2.DistanceSquared(ref p.Position, ref neighbour.Position, out q); if (q > _influenceRadiusSquared) { continue; } SpringHash hash = new SpringHash { P0 = p, P1 = neighbour }; if (!_springs.ContainsKey(hash)) { //TODO: Use pool? Spring spring = new Spring(p, neighbour) { RestLength = (float)Math.Sqrt(q) }; _springs.Add(hash, spring); } } }
/// <summary> /// Create a new chainshape from the vertices. /// </summary> /// <param name="vertices">The vertices to use. Must contain 2 or more vertices.</param> /// <param name="createLoop">Set to true to create a closed loop. It connects the first vertice to the last, and automatically adjusts connectivity to create smooth collisions along the chain.</param> public ChainShape(Vertices vertices, bool createLoop = false) : base(0) { ShapeType = ShapeType.Chain; _radius = Settings.PolygonRadius; Debug.Assert(vertices != null && vertices.Count >= 3); Debug.Assert(vertices[0] != vertices[vertices.Count - 1]); // FPE. See http://www.box2d.org/forum/viewtopic.php?f=4&t=7973&p=35363 for (int i = 1; i < vertices.Count; ++i) { Vector2 v1 = vertices[i - 1]; Vector2 v2 = vertices[i]; // If the code crashes here, it means your vertices are too close together. Debug.Assert(Vector2.DistanceSquared(v1, v2) > Settings.LinearSlop * Settings.LinearSlop); } Vertices = new Vertices(vertices); if (createLoop) { Vertices.Add(vertices[0]); PrevVertex = Vertices[Vertices.Count - 2]; //FPE: We use the properties instead of the private fields here. NextVertex = Vertices[1]; //FPE: We use the properties instead of the private fields here. } }
private void DoubleDensityRelaxation() { float q; for (int i = 0; i < Particles.Count; i++) { Particle particle = Particles[i]; particle.Density = 0; particle.NearDensity = 0; _tempParticles = Particles.GetNearby(particle); int len2 = _tempParticles.Count; if (len2 > MaxNeighbors) { len2 = MaxNeighbors; } for (int j = 0; j < len2; j++) { Particle tempParticle = _tempParticles[j]; Vector2.DistanceSquared(ref particle.Position, ref tempParticle.Position, out q); if (q < InfluenceRadiusSquared && q != 0) { q = (float)Math.Sqrt(q); q /= InfluenceRadius; float qq = ((1 - q) * (1 - q)); particle.Density += qq; particle.NearDensity += qq * (1 - q); } } particle.Pressure = (Stiffness * (particle.Density - DensityRest)); particle.NearPressure = (StiffnessNear * particle.NearDensity); _dx = Vector2.Zero; for (int j = 0; j < len2; j++) { Particle tempParticle = _tempParticles[j]; Vector2.DistanceSquared(ref particle.Position, ref tempParticle.Position, out q); if ((q < InfluenceRadiusSquared) && (q != 0)) { q = (float)Math.Sqrt(q); Vector2.Subtract(ref tempParticle.Position, ref particle.Position, out _rij); Vector2.Divide(ref _rij, q, out _rij); q /= InfluenceRadius; float D = (_deltaTime2 * (particle.Pressure * (1 - q) + particle.NearPressure * (1 - q) * (1 - q))); Vector2.Multiply(ref _rij, (D * 0.5f), out _rij); tempParticle.Position = new Vector2(tempParticle.Position.X + _rij.X, tempParticle.Position.Y + _rij.Y); Vector2.Subtract(ref _dx, ref _rij, out _dx); } } particle.Position = particle.Position + _dx; } }
private void ApplyViscosity(FluidParticle p, float timeStep) { for (int i = 0; i < p.Neighbours.Count; ++i) { FluidParticle neighbour = p.Neighbours[i]; if (p.Index >= neighbour.Index) { continue; } float q; Vector2.DistanceSquared(ref p.Position, ref neighbour.Position, out q); if (q > _influenceRadiusSquared) { continue; } Vector2 direction; Vector2.Subtract(ref neighbour.Position, ref p.Position, out direction); if (direction.LengthSquared() < float.Epsilon) { continue; } direction.Normalize(); Vector2 deltaVelocity; Vector2.Subtract(ref p.Velocity, ref neighbour.Velocity, out deltaVelocity); float u; Vector2.Dot(ref deltaVelocity, ref direction, out u); if (u > 0.0f) { q = 1.0f - (float)Math.Sqrt(q) / Definition.InfluenceRadius; float impulseFactor = 0.5f * timeStep * q * (u * (Definition.ViscositySigma + Definition.ViscosityBeta * u)); Vector2 impulse; Vector2.Multiply(ref direction, -impulseFactor, out impulse); p.ApplyImpulse(ref impulse); Vector2.Multiply(ref direction, impulseFactor, out impulse); neighbour.ApplyImpulse(ref impulse); } } }
private void ApplyViscosity(float deltaTime) { float u, q; for (int i = 0; i < Particles.Count; i++) { Particle particle = Particles[i]; _tempParticles = Particles.GetNearby(particle); int len2 = _tempParticles.Count; if (len2 > MaxNeighbors) { len2 = MaxNeighbors; } for (int j = 0; j < len2; j++) { Particle tempParticle = _tempParticles[j]; Vector2.DistanceSquared(ref particle.Position, ref tempParticle.Position, out q); if ((q < InfluenceRadiusSquared) && (q != 0)) { q = (float)Math.Sqrt(q); Vector2.Subtract(ref tempParticle.Position, ref particle.Position, out _rij); Vector2.Divide(ref _rij, q, out _rij); Vector2.Subtract(ref particle.Velocity, ref tempParticle.Velocity, out _tempVect); Vector2.Dot(ref _tempVect, ref _rij, out u); if (u <= 0.0f) { continue; } q /= InfluenceRadius; float I = (deltaTime * (1 - q) * (ViscositySigma * u + ViscosityBeta * u * u)); Vector2.Multiply(ref _rij, (I * 0.5f), out _rij); Vector2.Subtract(ref particle.Velocity, ref _rij, out _tempVect); particle.Velocity = _tempVect; _tempVect = tempParticle.Velocity; _tempVect += _rij; tempParticle.Velocity = _tempVect; } } } }
/// <summary>Validates vertex input.</summary> internal static void ValidateVertices(IList <LocalPoint> vertices, int minCount) { if (vertices == null) { throw new ArgumentNullException("vertices"); } if (vertices.Count < minCount) { throw new ArgumentException("Too few vertices to create a chain.", "vertices"); } for (var i = 1; i < vertices.Count; ++i) { var v1 = vertices[i - 1]; var v2 = vertices[i]; if (LocalPoint.DistanceSquared(v1, v2) <= Settings.LinearSlop * Settings.LinearSlop) { throw new ArgumentException("Some vertices are too close together.", "vertices"); } } }
//private void DoubleDensityRelaxation1(FluidParticle p, float timeStep) //{ // _density = 0; // _densityNear = 0; // _len2 = p.Neighbours.Count; // if (_len2 > MaxNeighbors) // _len2 = MaxNeighbors; // for (_j = 0; _j < _len2; _j++) // { // _q = Vector2.DistanceSquared(p.Position, p.Neighbours[_j].Position); // _distanceCache[_j] = _q; // if (_q < _influenceRadiusSquared && _q != 0) // { // _q = (float)Math.Sqrt(_q); // _q /= Definition.InfluenceRadius; // _qq = ((1 - _q) * (1 - _q)); // _density += _qq; // _densityNear += _qq * (1 - _q); // } // } // _pressure = Definition.Stiffness * (_density - Definition.DensityRest); // _pressureNear = Definition.StiffnessNear * _densityNear; // _dx = Vector2.Zero; // for (_j = 0; _j < _len2; _j++) // { // _q = _distanceCache[_j]; // if (_q < _influenceRadiusSquared && _q != 0) // { // _q = (float)Math.Sqrt(_q); // _rij = p.Neighbours[_j].Position; // _rij -= p.Position; // _rij *= 1 / _q; // _q /= _influenceRadiusSquared; // _d = ((timeStep * timeStep) * (_pressure * (1 - _q) + _pressureNear * (1 - _q) * (1 - _q))); // _rij *= _d * 0.5f; // p.Neighbours[_j].Position += _rij; // _dx -= _rij; // } // } // p.Position += _dx; //} private void DoubleDensityRelaxation(FluidParticle particle, float deltaTime2) { _density = 0.0f; _densityNear = 0.0f; int neightborCount = particle.Neighbours.Count; if (neightborCount > MaxNeighbors) { neightborCount = MaxNeighbors; } for (int i = 0; i < neightborCount; ++i) { FluidParticle neighbour = particle.Neighbours[i]; if (particle.Index == neighbour.Index) { continue; } float q; Vector2.DistanceSquared(ref particle.Position, ref neighbour.Position, out q); _distanceCache[i] = q; if (q > _influenceRadiusSquared) { continue; } q = 1.0f - (float)Math.Sqrt(q) / Definition.InfluenceRadius; float densityDelta = q * q; _density += densityDelta; _densityNear += densityDelta * q; } _pressure = Definition.Stiffness * (_density - Definition.DensityRest); _pressureNear = Definition.StiffnessNear * _densityNear; // For gameplay purposes particle.Density = _density + _densityNear; particle.Pressure = _pressure + _pressureNear; Vector2 delta = Vector2.Zero; for (int i = 0; i < neightborCount; ++i) { FluidParticle neighbour = particle.Neighbours[i]; if (particle.Index == neighbour.Index) { continue; } float q = _distanceCache[i]; if (q > _influenceRadiusSquared) { continue; } q = 1.0f - (float)Math.Sqrt(q) / Definition.InfluenceRadius; float dispFactor = deltaTime2 * (q * (_pressure + _pressureNear * q)); Vector2 direction; Vector2.Subtract(ref neighbour.Position, ref particle.Position, out direction); if (direction.LengthSquared() < float.Epsilon) { continue; } direction.Normalize(); Vector2 disp; Vector2.Multiply(ref direction, dispFactor, out disp); Vector2.Add(ref neighbour.Position, ref disp, out neighbour.Position); Vector2.Multiply(ref direction, -dispFactor, out disp); Vector2.Add(ref delta, ref disp, out delta); } Vector2.Add(ref particle.Position, ref delta, out particle.Position); }
public static float DistanceSquared(Vector2 a, Vector2 b) { return(MonoGameVector2.DistanceSquared(a.MonoGameVector, b.MonoGameVector)); }
public static float DistanceSquared(Vector2 v1, Vector2 v2) { return(Vector.DistanceSquared(v1, v2)); }
/// <summary> /// Evaluate the manifold with supplied transforms. This assumes /// modest motion from the original state. This does not change the /// point count, impulses, etc. The radii must come from the Shapes /// that generated the manifold. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="xfA">The transform for A.</param> /// <param name="radiusA">The radius for A.</param> /// <param name="xfB">The transform for B.</param> /// <param name="radiusB">The radius for B.</param> /// <param name="normal">World vector pointing from A to B</param> /// <param name="points">Torld contact point (point of intersection).</param> public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2 <Vector2> points) { normal = Vector2.Zero; points = new FixedArray2 <Vector2>(); if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case ManifoldType.Circles: { normal = new Vector2(1.0f, 0.0f); Vector2 pointA = Transform.Multiply(ref manifold.LocalPoint, ref xfA); Vector2 pointB = Transform.Multiply(manifold.Points[0].LocalPoint, ref xfB); if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon) { normal = pointB - pointA; normal.Normalize(); } Vector2 cA = pointA + radiusA * normal; Vector2 cB = pointB - radiusB * normal; points[0] = 0.5f * (cA + cB); } break; case ManifoldType.FaceA: { normal = Complex.Multiply(ref manifold.LocalNormal, ref xfA.q); Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfA); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfB); Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cB = clipPoint - radiusB * normal; points[i] = 0.5f * (cA + cB); } } break; case ManifoldType.FaceB: { normal = Complex.Multiply(ref manifold.LocalNormal, ref xfB.q); Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfB); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfA); Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cA = clipPoint - radiusA * normal; points[i] = 0.5f * (cA + cB); } // Ensure normal points from A to B. normal = -normal; } break; } }
private void CalculateElasticity(float deltaTime) { float sqDist; for (int i = 0; i < Particles.Count; i++) { Particle pa = Particles[i]; if (Particles.CountNearBy(pa) <= 1) { continue; } _tempParticles = Particles.GetNearby(pa); int len2 = _tempParticles.Count; if (len2 > MaxNeighbors) { len2 = MaxNeighbors; } for (int j = 0; j < len2; j++) { Particle pb = Particles[j]; Vector2.DistanceSquared(ref pa.Position, ref pb.Position, out sqDist); if (sqDist > RestLengthSquared) { continue; } if (pa.GetHashCode() == pb.GetHashCode()) { continue; } if (!_springPresenceTable[pa.GetHashCode()].Contains(pb.GetHashCode())) { _springs.Add(new Spring2(pa, pb, RestLength)); _springPresenceTable[pa.GetHashCode()].Add(pb.GetHashCode()); } } } for (int i = _springs.Count - 1; i >= 0; i--) { Spring2 spring = _springs[i]; spring.Update(); // Stretch if (spring.CurrentDistance > (spring.RestLength + DeformationFactor)) { spring.RestLength += deltaTime * Plasticity * (spring.CurrentDistance - spring.RestLength - (YieldRatioStretch * spring.RestLength)); } // Compress else if (spring.CurrentDistance < (spring.RestLength - DeformationFactor)) { spring.RestLength -= deltaTime * Plasticity * (spring.RestLength - (YieldRatioCompress * spring.RestLength) - spring.CurrentDistance); } // Remove springs with restLength longer than REST_LENGTH if (spring.RestLength > RestLength) { _springs.RemoveAt(i); _springPresenceTable[spring.PA.GetHashCode()].Remove(spring.PB.GetHashCode()); } else { if (spring.CurrentDistance == 0) { continue; } Vector2.Subtract(ref spring.PB.Position, ref spring.PA.Position, out _rij); _rij /= spring.CurrentDistance; float D = deltaTime * KSpring * (spring.RestLength - spring.CurrentDistance); _rij *= (D * 0.5f); spring.PA.Position = new Vector2(spring.PA.Position.X - _rij.X, spring.PA.Position.Y - _rij.Y); spring.PB.Position = new Vector2(spring.PB.Position.X + _rij.X, spring.PB.Position.Y + _rij.Y); } } }