diff --git a/Nez-PCL/Nez.csproj b/Nez-PCL/Nez.csproj index 83a03af65..5eafb4564 100644 --- a/Nez-PCL/Nez.csproj +++ b/Nez-PCL/Nez.csproj @@ -37,7 +37,6 @@ - @@ -91,6 +90,7 @@ + @@ -123,7 +123,6 @@ - @@ -508,12 +507,24 @@ + + + + + + + + + + + + diff --git a/Nez-PCL/Physics/Verlet/Composites/Ball.cs b/Nez-PCL/Physics/Verlet/Composites/Ball.cs new file mode 100644 index 000000000..b4d9764fa --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Composites/Ball.cs @@ -0,0 +1,16 @@ +using Microsoft.Xna.Framework; + + +namespace Nez.Verlet +{ + /// + /// single Particle composite + /// + public class Ball : Composite + { + public Ball( Vector2 position, float radius = 10 ) + { + addParticle( new Particle( position ) ).radius = radius; + } + } +} diff --git a/Nez-PCL/Physics/Verlet/Composites/Cloth.cs b/Nez-PCL/Physics/Verlet/Composites/Cloth.cs new file mode 100644 index 000000000..9797d084b --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Composites/Cloth.cs @@ -0,0 +1,48 @@ +using Microsoft.Xna.Framework; + + +namespace Nez.Verlet +{ + public class Cloth : Composite + { + /// + /// creates a Cloth. If connectHorizontalParticles is false it will not link horizontal Particles and create a hair-like cloth + /// + /// Top left position. + /// Width. + /// Height. + /// Segments. + /// Stiffness. + /// Tear sensitivity. + /// If set to true connect horizontal particles. + public Cloth( Vector2 topLeftPosition, float width, float height, int segments = 20, float stiffness = 0.25f, float tearSensitivity = 5, bool connectHorizontalParticles = true ) + { + var xStride = width / segments; + var yStride = height / segments; + + for( var y = 0; y < segments; y++ ) + { + for( var x = 0; x < segments; x++ ) + { + var px = topLeftPosition.X + x * xStride; + var py = topLeftPosition.Y + y * yStride; + var particle = addParticle( new Particle( new Vector2( px, py ) ) ); + + // remove this constraint to make only vertical constaints for a hair-like cloth + if( connectHorizontalParticles && x > 0 ) + addConstraint( new DistanceConstraint( particles[y * segments + x], particles[y * segments + x - 1], stiffness ) ) + .setTearSensitvity( tearSensitivity ) + .setCollidesWithColliders( false ); + + if( y > 0 ) + addConstraint( new DistanceConstraint( particles[y * segments + x], particles[( y - 1 ) * segments + x], stiffness ) ) + .setTearSensitvity( tearSensitivity ) + .setCollidesWithColliders( false ); + + if( y == 0 ) + particle.pin(); + } + } + } + } +} diff --git a/Nez-PCL/Physics/Verlet/Composites/Composite.cs b/Nez-PCL/Physics/Verlet/Composites/Composite.cs new file mode 100644 index 000000000..bcc71f85c --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Composites/Composite.cs @@ -0,0 +1,185 @@ +using System.Runtime.CompilerServices; +using Microsoft.Xna.Framework; + + +namespace Nez.Verlet +{ + /// + /// represents an object in the Verlet world. Consists of Particles and Constraints and handles updating them + /// + public class Composite + { + /// + /// friction applied to all Particle movement to dampen it. Value should be very close to 1. + /// + public Vector2 friction = new Vector2( 0.98f, 1 ); + + /// + /// should Particles be rendered when doing a debugRender? + /// + public bool drawParticles = true; + + /// + /// should Constraints be rendered when doing a debugRender? + /// + public bool drawConstraints = true; + + /// + /// layer mask of all the layers this Collider should collide with when Entity.move methods are used. defaults to all layers. + /// + public int collidesWithLayers = Physics.allLayers; + + public FastList particles = new FastList(); + FastList _constraints = new FastList(); + + + #region Particle/Constraint management + + /// + /// adds a Particle to the Composite + /// + /// The particle. + /// Particle. + public Particle addParticle( Particle particle ) + { + particles.add( particle ); + return particle; + } + + + /// + /// removes the Particle from the Composite + /// + /// Particle. + public void removeParticle( Particle particle ) + { + particles.remove( particle ); + } + + + /// + /// removes all Particles and Constraints from the Composite + /// + public void removeAll() + { + particles.clear(); + _constraints.clear(); + } + + + /// + /// adds a Constraint to the Composite + /// + /// The constraint. + /// Constraint. + /// The 1st type parameter. + public T addConstraint( T constraint ) where T : Constraint + { + _constraints.add( constraint ); + constraint.composite = this; + return constraint; + } + + + /// + /// removes a Constraint from the Composite + /// + /// Constraint. + public void removeConstraint( Constraint constraint ) + { + _constraints.remove( constraint ); + } + + #endregion + + + /// + /// applies a force to all Particles in this Composite + /// + /// Force. + public void applyForce( Vector2 force ) + { + for( var j = 0; j < particles.length; j++ ) + particles.buffer[j].applyForce( force ); + } + + + /// + /// handles solving all Constraints + /// + [MethodImpl( MethodImplOptions.AggressiveInlining )] + public void solveConstraints() + { + // loop backwards in case any Constraints break and are removed + for( var i = _constraints.length - 1; i >= 0; i-- ) + _constraints.buffer[i].solve(); + } + + + /// + /// applies gravity to each Particle and does the verlet integration + /// + /// Delta time. + /// Gravity. + [MethodImpl( MethodImplOptions.AggressiveInlining )] + public void updateParticles( float deltaTimeSquared, Vector2 gravity ) + { + for( var j = 0; j < particles.length; j++ ) + { + var p = particles.buffer[j]; + if( p.isPinned ) + { + p.position = p.pinnedPosition; + continue; + } + + p.applyForce( p.mass * gravity ); + + // calculate velocity and dampen it with friction + var vel = ( p.position - p.lastPosition ) * friction; + + // calculate the next position using Verlet Integration + var nextPos = p.position + vel + 0.5f * p.acceleration * deltaTimeSquared; + + // reset variables + p.lastPosition = p.position; + p.position = nextPos; + p.acceleration.X = p.acceleration.Y = 0; + } + } + + + [MethodImpl( MethodImplOptions.AggressiveInlining )] + public void handleConstraintCollisions() + { + // loop backwards in case any Constraints break and are removed + for( var i = _constraints.length - 1; i >= 0; i-- ) + { + if( _constraints.buffer[i].collidesWithColliders ) + _constraints.buffer[i].handleCollisions( collidesWithLayers ); + } + } + + + public void debugRender( Batcher batcher ) + { + if( drawConstraints ) + { + for( var i = 0; i < _constraints.length; i++ ) + _constraints.buffer[i].debugRender( batcher ); + } + + if( drawParticles ) + { + for( var i = 0; i < particles.length; i++ ) + { + if( particles.buffer[i].radius == 0 ) + batcher.drawPixel( particles.buffer[i].position, DefaultColors.verletParticle, 4 ); + else + batcher.drawCircle( particles.buffer[i].position, (int)particles.buffer[i].radius, DefaultColors.verletParticle, 1, 4 ); + } + } + } + + } +} diff --git a/Nez-PCL/Physics/Verlet/Composites/LineSegments.cs b/Nez-PCL/Physics/Verlet/Composites/LineSegments.cs new file mode 100644 index 000000000..08619183a --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Composites/LineSegments.cs @@ -0,0 +1,35 @@ +using Microsoft.Xna.Framework; + + +namespace Nez.Verlet +{ + /// + /// a series of points connected with DistanceConstraints + /// + public class LineSegments : Composite + { + public LineSegments( Vector2[] vertices, float stiffness ) + { + for( var i = 0; i < vertices.Length; i++ ) + { + var p = new Particle( vertices[i] ); + addParticle( p ); + + if( i > 0 ) + addConstraint( new DistanceConstraint( particles.buffer[i], particles.buffer[i - 1], stiffness ) ); + } + } + + + /// + /// pins the Particle at the given index + /// + /// Index. + public LineSegments pinParticleAtIndex( int index ) + { + particles.buffer[index].pin(); + return this; + } + + } +} diff --git a/Nez-PCL/Physics/Verlet/Composites/Tire.cs b/Nez-PCL/Physics/Verlet/Composites/Tire.cs new file mode 100644 index 000000000..0d46cf8e3 --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Composites/Tire.cs @@ -0,0 +1,31 @@ +using Microsoft.Xna.Framework; + + +namespace Nez.Verlet +{ + public class Tire : Composite + { + public Tire( Vector2 origin, float radius, int segments, float spokeStiffness = 1, float treadStiffness = 1 ) + { + var stride = 2 * MathHelper.Pi / segments; + + // particles + for( var i = 0; i < segments; i++ ) + { + var theta = i * stride; + addParticle( new Particle( new Vector2( origin.X + Mathf.cos( theta ) * radius, origin.Y + Mathf.sin( theta ) * radius ) ) ); + } + + var centerParticle = addParticle( new Particle( origin ) ); + + // constraints + for( var i = 0; i < segments; i++ ) + { + addConstraint( new DistanceConstraint( particles[i], particles[( i + 1 ) % segments], treadStiffness ) ); + addConstraint( new DistanceConstraint( particles[i], centerParticle, spokeStiffness ) ) + .setCollidesWithColliders( false ); + addConstraint( new DistanceConstraint( particles[i], particles[( i + 5 ) % segments], treadStiffness ) ); + } + } + } +} diff --git a/Nez-PCL/Physics/Verlet/Composites/Tree.cs b/Nez-PCL/Physics/Verlet/Composites/Tree.cs new file mode 100644 index 000000000..6c1ee993d --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Composites/Tree.cs @@ -0,0 +1,50 @@ +using Microsoft.Xna.Framework; + + +namespace Nez.Verlet +{ + /// + /// fractal tree. Converted from https://github.com/subprotocol/verlet-js/blob/master/examples/tree.html + /// + public class Tree : Composite + { + public Tree( Vector2 origin, int depth = 5, float branchLength = 70, float theta = 0.4f, float segmentCoef = 0.95f ) + { + var root = addParticle( new Particle( origin ) ).pin(); + var trunk = addParticle( new Particle( origin + new Vector2( 0, 10 ) ) ).pin(); + + var firstBranch = createTreeBranch( root, 0, depth, segmentCoef, new Vector2( 0, -1 ), branchLength, theta ); + addConstraint( new AngleConstraint( trunk, root, firstBranch, 3 ) ); + + // animates the tree at the beginning + var noise = 3; + for( var i = 0; i < particles.length; ++i ) + particles.buffer[i].position += ( new Vector2( Mathf.floor( Random.nextFloat() * noise ), Mathf.floor( Random.nextFloat() * noise ) ) ); + } + + + Particle createTreeBranch( Particle parent, int i, int nMax, float segmentCoef, Vector2 normal, float branchLength, float theta ) + { + var particle = new Particle( parent.position + ( normal * ( branchLength * segmentCoef ) ) ); + addParticle( particle ); + + addConstraint( new DistanceConstraint( parent, particle, 0.7f ) ); + + if( i < nMax ) + { + var aRot = Mathf.rotateAround( normal, Vector2.Zero, -theta * Mathf.rad2Deg ); + var bRot = Mathf.rotateAround( normal, Vector2.Zero, theta * Mathf.rad2Deg ); + var a = createTreeBranch( particle, i + 1, nMax, segmentCoef * segmentCoef, aRot, branchLength, theta ); + var b = createTreeBranch( particle, i + 1, nMax, segmentCoef * segmentCoef, bRot, branchLength, theta ); + + var jointStrength = Mathf.lerp( 0.9f, 0, (float)i / nMax ); + + addConstraint( new AngleConstraint( parent, particle, a, jointStrength ) ); + addConstraint( new AngleConstraint( parent, particle, b, jointStrength ) ); + } + + return particle; + } + + } +} diff --git a/Nez-PCL/Physics/Verlet/Constraints/AngleConstraint.cs b/Nez-PCL/Physics/Verlet/Constraints/AngleConstraint.cs new file mode 100644 index 000000000..6e32b66b0 --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Constraints/AngleConstraint.cs @@ -0,0 +1,68 @@ +using Microsoft.Xna.Framework; + + +namespace Nez.Verlet +{ + /// + /// constrains 3 particles to an angle + /// + public class AngleConstraint : Constraint + { + /// + /// [0-1]. the stiffness of the Constraint. Lower values are more springy and higher are more rigid. + /// + public float stiffness; + + /// + /// the angle in radians that the Constraint will attempt to maintain + /// + public float angleInRadians; + + Particle _particleA; + Particle _centerParticle; + Particle _particleC; + + + public AngleConstraint( Particle a, Particle center, Particle c, float stiffness ) + { + _particleA = a; + _centerParticle = center; + _particleC = c; + this.stiffness = stiffness; + + // not need for this Constraint to collide. There will be DistanceConstraints to do that if necessary + collidesWithColliders = false; + + angleInRadians = angleBetweenParticles(); + } + + + float angleBetweenParticles() + { + var first = _particleA.position - _centerParticle.position; + var second = _particleC.position - _centerParticle.position; + + return Mathf.atan2( first.X * second.Y - first.Y * second.X, first.X * second.X + first.Y * second.Y ); + } + + + public override void solve() + { + var angleBetween = angleBetweenParticles(); + var diff = angleBetween - angleInRadians; + + if( diff <= -MathHelper.Pi ) + diff += 2 * MathHelper.Pi; + else if( diff >= MathHelper.Pi ) + diff -= 2 * MathHelper.Pi; + + diff *= stiffness; + + _particleA.position = Mathf.rotateAround( _particleA.position, _centerParticle.position, diff ); + _particleC.position = Mathf.rotateAround( _particleC.position, _centerParticle.position, -diff ); + _centerParticle.position = Mathf.rotateAround( _centerParticle.position, _particleA.position, diff ); + _centerParticle.position = Mathf.rotateAround( _centerParticle.position, _particleC.position, -diff ); + } + + } +} diff --git a/Nez-PCL/Physics/Verlet/Constraints/Constraint.cs b/Nez-PCL/Physics/Verlet/Constraints/Constraint.cs new file mode 100644 index 000000000..ce988d7f7 --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Constraints/Constraint.cs @@ -0,0 +1,37 @@ + + +namespace Nez.Verlet +{ + public abstract class Constraint + { + /// + /// the Composite that owns this Constraint. Required so that Constraints can be broken. + /// + internal Composite composite; + + /// + /// if true, the Constraint will check for collisions with standard Nez Colliders. Inner Constraints do not need to have this set to + /// true. + /// + public bool collidesWithColliders = true; + + /// + /// solves the Constraint + /// + public abstract void solve(); + + /// + /// if collidesWithColliders is true this will be called + /// + public virtual void handleCollisions( int collidesWithLayers ) + {} + + /// + /// debug renders the Constraint + /// + /// Batcher. + public virtual void debugRender( Batcher batcher ) + {} + + } +} diff --git a/Nez-PCL/Physics/Verlet/Constraints/DistanceConstraint.cs b/Nez-PCL/Physics/Verlet/Constraints/DistanceConstraint.cs new file mode 100644 index 000000000..90d398875 --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Constraints/DistanceConstraint.cs @@ -0,0 +1,232 @@ +using System; +using System.Runtime.CompilerServices; +using Microsoft.Xna.Framework; +using Nez.PhysicsShapes; + + +namespace Nez.Verlet +{ + /// + /// maintains a specified distance betweeen two Particles. The stiffness adjusts how rigid or springy the constraint will be. + /// + public class DistanceConstraint : Constraint + { + /// + /// [0-1]. the stiffness of the Constraint. Lower values are more springy and higher are more rigid. + /// + public float stiffness; + + /// + /// the resting distnace of the Constraint. It will always try to get to this distance. + /// + public float restingDistance; + + /// + /// if the ratio of the current distance / restingDistance is greater than tearSensitivity the Constaint will be removed. Values + /// should be above 1 and higher values mean rupture wont occur until the Constaint is stretched further. + /// + public float tearSensitivity = float.PositiveInfinity; + + /// + /// sets whether collisions should be approximated by points. This should be used for Constraints that need to collided on both + /// sides. SAT only works with single sided collisions. + /// + public bool shouldApproximateCollisionsWithPoints = false; + + /// + /// if shouldApproximateCollisionsWithPoints is true, this controls how accurate the collisions check will be. Higher numbers mean + /// more collisions checks. + /// + public int totalPointsToApproximateCollisionsWith = 10; + + /// + /// the first Particle in the Constraint + /// + Particle _particleOne; + + /// + /// the second particle in the Constraint + /// + Particle _particleTwo; + + /// + /// Polygon shared amongst all DistanceConstraints. Used for collision detection. + /// + static Polygon _polygon = new Polygon( 2, 1 ); + + + public DistanceConstraint( Particle first, Particle second, float stiffness, float distance = -1 ) + { + _particleOne = first; + _particleTwo = second; + this.stiffness = stiffness; + + if( distance > -1 ) + restingDistance = distance; + else + restingDistance = Vector2.Distance( first.position, second.position ); + } + + + /// + /// creates a faux angle constraint by figuring out the required distance from a to c for the given angle + /// + /// The alpha component. + /// Center. + /// C. + /// Stiffness. + /// Angle in degrees. + public static DistanceConstraint create( Particle a, Particle center, Particle c, float stiffness, float angleInDegrees ) + { + var aToCenter = Vector2.Distance( a.position, center.position ); + var cToCenter = Vector2.Distance( c.position, center.position ); + var distance = Mathf.sqrt( aToCenter * aToCenter + cToCenter * cToCenter - ( 2 * aToCenter * cToCenter * Mathf.cos( angleInDegrees * Mathf.deg2Rad ) ) ); + + return new DistanceConstraint( a, c, stiffness, distance ); + } + + + /// + /// sets the tear sensitivity. if the ratio of the current distance / restingDistance is greater than tearSensitivity the + /// Constaint will be removed + /// + /// The tear sensitvity. + /// Tear sensitivity. + public DistanceConstraint setTearSensitvity( float tearSensitivity ) + { + this.tearSensitivity = tearSensitivity; + return this; + } + + + /// + /// sets whether this Constraint should collide with standard Colliders + /// + /// The collides with colliders. + /// If set to true collides with colliders. + public DistanceConstraint setCollidesWithColliders( bool collidesWithColliders ) + { + this.collidesWithColliders = collidesWithColliders; + return this; + } + + + /// + /// sets whether collisions should be approximated by points. This should be used for Constraints that need to collided on both + /// sides. SAT only works with single sided collisions. + /// + /// The should approximate collisions with points. + /// If set to true should approximate collisions with points. + public DistanceConstraint setShouldApproximateCollisionsWithPoints( bool shouldApproximateCollisionsWithPoints ) + { + this.shouldApproximateCollisionsWithPoints = shouldApproximateCollisionsWithPoints; + return this; + } + + + public override void solve() + { + // calculate the distance between the two Particles + var diff = _particleOne.position - _particleTwo.position; + var d = diff.Length(); + + // find the difference, or the ratio of how far along the restingDistance the actual distance is. + var difference = ( restingDistance - d ) / d; + + // if the distance is more than tearSensitivity we remove the Constraint + if( d / restingDistance > tearSensitivity ) + { + composite.removeConstraint( this ); + return; + } + + // inverse the mass quantities + var im1 = 1f / _particleOne.mass; + var im2 = 1f / _particleTwo.mass; + var scalarP1 = ( im1 / ( im1 + im2 ) ) * stiffness; + var scalarP2 = stiffness - scalarP1; + + // push/pull based on mass + // heavier objects will be pushed/pulled less than attached light objects + _particleOne.position += diff * scalarP1 * difference; + _particleTwo.position -= diff * scalarP2 * difference; + } + + + [MethodImpl( MethodImplOptions.AggressiveInlining )] + public override void handleCollisions( int collidesWithLayers ) + { + if( shouldApproximateCollisionsWithPoints ) + { + approximateCollisionsWithPoints( collidesWithLayers ); + return; + } + + // get a proper bounds for our line and update the Polygons bounds + var minX = Math.Min( _particleOne.position.X, _particleTwo.position.X ); + var maxX = Math.Max( _particleOne.position.X, _particleTwo.position.X ); + var minY = Math.Min( _particleOne.position.Y, _particleTwo.position.Y ); + var maxY = Math.Max( _particleOne.position.Y, _particleTwo.position.Y ); + _polygon.bounds = RectangleF.fromMinMax( minX, minY, maxX, maxY ); + + preparePolygonForCollisionChecks(); + + var colliders = Physics.boxcastBroadphase( ref _polygon.bounds, collidesWithLayers ); + foreach( var collider in colliders ) + { + CollisionResult result; + if( _polygon.collidesWithShape( collider.shape, out result ) ) + { + _particleOne.position -= result.minimumTranslationVector; + _particleTwo.position -= result.minimumTranslationVector; + } + } + } + + + [MethodImpl( MethodImplOptions.AggressiveInlining )] + void approximateCollisionsWithPoints( int collidesWithLayers ) + { + Vector2 pt; + for( var j = 0; j < totalPointsToApproximateCollisionsWith - 1; j++ ) + { + pt = Vector2.Lerp( _particleOne.position, _particleTwo.position, ( j + 1 ) / (float)totalPointsToApproximateCollisionsWith ); + var collidedCount = Physics.overlapCircleAll( pt, 3, World._colliders, collidesWithLayers ); + for( var i = 0; i < collidedCount; i++ ) + { + var collider = World._colliders[i]; + CollisionResult collisionResult; + if( collider.shape.pointCollidesWithShape( pt, out collisionResult ) ) + { + _particleOne.position -= collisionResult.minimumTranslationVector; + _particleTwo.position -= collisionResult.minimumTranslationVector; + } + } + } + } + + + [MethodImpl( MethodImplOptions.AggressiveInlining )] + void preparePolygonForCollisionChecks() + { + // we need to setup a Polygon with one edge. It needs the center to be in the opposite direction of it's normal. + // this is necessary so that SAT knows which way to calculate the MTV, which uses Shape positions. + var perp = Vector2Ext.perpendicular( _particleTwo.position - _particleOne.position ); + perp.Normalize(); + + // set our Polygon points + var midPoint = Vector2.Lerp( _particleOne.position, _particleTwo.position, 0.5f ); + _polygon.position = midPoint + perp * 50; + _polygon.points[0] = _particleOne.position - _polygon.position; + _polygon.points[1] = _particleTwo.position - _polygon.position; + _polygon.recalculateCenterAndEdgeNormals(); + } + + + public override void debugRender( Batcher batcher ) + { + batcher.drawLine( _particleOne.position, _particleTwo.position, DefaultColors.verletConstraintEdge ); + } + + } +} diff --git a/Nez-PCL/Physics/Verlet/Particle.cs b/Nez-PCL/Physics/Verlet/Particle.cs new file mode 100644 index 000000000..1972ef650 --- /dev/null +++ b/Nez-PCL/Physics/Verlet/Particle.cs @@ -0,0 +1,80 @@ +using Microsoft.Xna.Framework; + + +namespace Nez.Verlet +{ + public class Particle + { + /// + /// the current position of the Particle + /// + public Vector2 position; + + /// + /// the position of the Particle prior to its latest move + /// + public Vector2 lastPosition; + + /// + /// the mass of the Particle. Taken into account for all forces and constraints + /// + public float mass = 1; + + /// + /// the radius of the Particle + /// + public float radius; + + /// + /// if true, the Particle will collide with standard Nez Colliders + /// + public bool collidesWithColliders = true; + + internal bool isPinned; + internal Vector2 acceleration; + internal Vector2 pinnedPosition; + + + public Particle( Vector2 position ) + { + this.position = position; + lastPosition = position; + } + + + /// + /// applies a force taking mass into account to the Particle + /// + /// Force. + public void applyForce( Vector2 force ) + { + // acceleration = (1 / mass) * force + acceleration += force / mass; + } + + + /// + /// pins the Particle to its current position + /// + public Particle pin() + { + isPinned = true; + pinnedPosition = position; + return this; + } + + + /// + /// pins the particle to the specified position + /// + /// Position. + public Particle pinTo( Vector2 position ) + { + isPinned = true; + pinnedPosition = position; + this.position = pinnedPosition; + return this; + } + + } +} diff --git a/Nez-PCL/Physics/Verlet/World.cs b/Nez-PCL/Physics/Verlet/World.cs new file mode 100644 index 000000000..7fb744328 --- /dev/null +++ b/Nez-PCL/Physics/Verlet/World.cs @@ -0,0 +1,293 @@ +using System.Runtime.CompilerServices; +using Microsoft.Xna.Framework; +using Nez.PhysicsShapes; + + +namespace Nez.Verlet +{ + /// + /// the root of the Verlet simulation. Create a World and call its update method each frame. + /// + public class World + { + /// + /// gravity for the simulation + /// + public Vector2 gravity = new Vector2( 0, 980f ); + + /// + /// number of iterations that will be used for Constraint solving + /// + public int constraintIterations = 3; + + /// + /// max number of iterations for the simulation as a whole + /// + public int maximumStepIterations = 5; + + /// + /// Bounds of the Verlet World. Particles will be confined to this space if set. + /// + public Rectangle? simulationBounds; + + /// + /// should Particles be allowed to be dragged? + /// + public bool allowDragging = true; + + /// + /// squared selection radius of the mouse pointer + /// + public float selectionRadiusSquared = 20 * 20; + + Particle _draggedParticle; + + FastList _composites = new FastList(); + + // collision helpers + internal static Collider[] _colliders = new Collider[4]; + Circle _tempCircle = new Circle( 1 ); + + // timing + float _leftOverTime; + float _fixedDeltaTime = 1f / 60; + int _iterationSteps; + float _fixedDeltaTimeSecondsSq; + + + public World( Rectangle? simulationBounds = null ) + { + this.simulationBounds = simulationBounds; + _fixedDeltaTimeSecondsSq = Mathf.pow( _fixedDeltaTime, 2 ); + } + + + #region verlet simulation + + public void update() + { + updateTiming(); + + if( allowDragging ) + handleDragging(); + + for( var iteration = 1; iteration <= _iterationSteps; iteration++ ) + { + for( var i = 0; i < _composites.length; i++ ) + { + var composite = _composites.buffer[i]; + + // solve constraints. we loop backwards in case any rupture + for( var s = 0; s < constraintIterations; s++ ) + composite.solveConstraints(); + + // do the verlet integration + composite.updateParticles( _fixedDeltaTimeSecondsSq, gravity ); + + // handle collisions with Nez Colliders + composite.handleConstraintCollisions(); + + for( var j = 0; j < composite.particles.length; j++ ) + { + var p = composite.particles.buffer[j]; + + // optinally constrain to bounds + if( simulationBounds.HasValue ) + constrainParticleToBounds( p ); + + // optionally handle collisions with Nez Colliders + if( p.collidesWithColliders ) + handleCollisions( p, composite.collidesWithLayers ); + } + } + } + } + + + [MethodImpl( MethodImplOptions.AggressiveInlining )] + void constrainParticleToBounds( Particle p ) + { + var tempPos = p.position; + var bounds = simulationBounds.Value; + + if( p.radius == 0 ) + { + if( tempPos.Y > bounds.Height ) + tempPos.Y = bounds.Height; + else if( tempPos.Y < bounds.Y ) + tempPos.Y = bounds.Y; + + if( tempPos.X < bounds.X ) + tempPos.X = bounds.X; + else if( tempPos.X > bounds.Width ) + tempPos.X = bounds.Width; + } + else + { + // special care for larger particles + if( tempPos.Y < bounds.Y + p.radius ) + tempPos.Y = 2f * ( bounds.Y + p.radius ) - tempPos.Y; + if( tempPos.Y > bounds.Height - p.radius ) + tempPos.Y = 2f * ( bounds.Height - p.radius ) - tempPos.Y; + if( tempPos.X > bounds.Width - p.radius ) + tempPos.X = 2f * ( bounds.Width - p.radius ) - tempPos.X; + if( tempPos.X < bounds.X + p.radius ) + tempPos.X = 2f * ( bounds.X + p.radius ) - tempPos.X; + } + + p.position = tempPos; + } + + + [MethodImpl( MethodImplOptions.AggressiveInlining )] + void handleCollisions( Particle p, int collidesWithLayers ) + { + var collidedCount = Physics.overlapCircleAll( p.position, p.radius, _colliders, collidesWithLayers ); + for( var i = 0; i < collidedCount; i++ ) + { + var collider = _colliders[i]; + if( collider.isTrigger ) + continue; + + CollisionResult collisionResult; + + // if we have a large enough Particle radius use a Circle for the collision check else fall back to a point + if( p.radius < 2 ) + { + if( collider.shape.pointCollidesWithShape( p.position, out collisionResult ) ) + { + // TODO: add a Dictionary of Collider,float that lets Colliders be setup as force volumes. The float can then be + // multiplied by the mtv here. It should be very small values, like 0.002f for example. + p.position -= collisionResult.minimumTranslationVector; + } + } + else + { + _tempCircle.radius = p.radius; + _tempCircle.position = p.position; + + if( _tempCircle.collidesWithShape( collider.shape, out collisionResult ) ) + { + p.position -= collisionResult.minimumTranslationVector; + } + } + } + } + + + [MethodImpl( MethodImplOptions.AggressiveInlining )] + void updateTiming() + { + _leftOverTime += Time.deltaTime; + _iterationSteps = Mathf.truncateToInt( _leftOverTime / _fixedDeltaTime ); + _leftOverTime -= (float)_iterationSteps * _fixedDeltaTime; + + _iterationSteps = System.Math.Min( _iterationSteps, maximumStepIterations ); + } + + #endregion + + + #region Composite management + + /// + /// adds a Composite to the simulation + /// + /// The composite. + /// Composite. + /// The 1st type parameter. + public T addComposite( T composite ) where T : Composite + { + _composites.add( composite ); + return composite; + } + + + /// + /// removes a Composite from the simulation + /// + /// Composite. + public void removeComposite( Composite composite ) + { + _composites.remove( composite ); + } + + #endregion + + + [MethodImpl( MethodImplOptions.AggressiveInlining )] + void handleDragging() + { + if( Input.leftMouseButtonPressed ) + { + _draggedParticle = getNearestParticle( Input.mousePosition ); + } + else if( Input.leftMouseButtonDown ) + { + if( _draggedParticle != null ) + _draggedParticle.position = Input.mousePosition; + } + else if( Input.leftMouseButtonReleased ) + { + if( _draggedParticle != null ) + _draggedParticle.position = Input.mousePosition; + _draggedParticle = null; + } + } + + + /// + /// gets the nearest Particle to the position. Uses the selectionRadiusSquared to determine if a Particle is near enough for consideration. + /// + /// The nearest particle. + /// Position. + [MethodImpl( MethodImplOptions.AggressiveInlining )] + public Particle getNearestParticle( Vector2 position ) + { + // less than 64 and we count it + var nearestSquaredDistance = selectionRadiusSquared; + Particle particle = null; + + // find nearest point + for( var j = 0; j < _composites.length; j++ ) + { + var particles = _composites.buffer[j].particles; + for( var i = 0; i < particles.length; i++ ) + { + var p = particles.buffer[i]; + var squaredDistanceToParticle = Vector2.DistanceSquared( p.position, position ); + if( squaredDistanceToParticle <= selectionRadiusSquared && ( particle == null || squaredDistanceToParticle < nearestSquaredDistance ) ) + { + particle = p; + nearestSquaredDistance = squaredDistanceToParticle; + } + } + } + + return particle; + } + + + public void debugRender( Batcher batcher ) + { + for( var i = 0; i < _composites.length; i++ ) + _composites.buffer[i].debugRender( batcher ); + + if( allowDragging ) + { + if( _draggedParticle != null ) + { + batcher.drawCircle( _draggedParticle.position, 8, Color.White ); + } + else + { + // Highlight the nearest particle within the selection radius + var particle = getNearestParticle( Input.mousePosition ); + if( particle != null ) + batcher.drawCircle( particle.position, 8, Color.White * 0.4f ); + } + } + } + + } +}