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