WIP testing forked from: Continuous Elastic Collision

/**
 * Copyright Glidias ( http://wonderfl.net/user/Glidias )
 * MIT License ( http://www.opensource.org/licenses/mit-license.php )
 * Downloaded from: http://wonderfl.net/c/lmBt
 */

package 
{
    
    import de.polygonal.ds.PriorityQueue;
    import flash.display.Sprite;
    import flash.display.StageAlign;
    import flash.display.StageScaleMode;
    import flash.events.Event;
    import flash.events.KeyboardEvent;

    [SWF(backgroundColor="#DDDDDD", frameRate=60)]
    public class ElasticCollisionDemo extends Sprite
    {
        private var simulator:Simulator;
        
        public function ElasticCollisionDemo()
        {
             
            stage.align = StageAlign.TOP_LEFT;
            stage.scaleMode = StageScaleMode.NO_SCALE;
            
            initSimulation();
            
        }
        
        private function initSimulation() : void
        {
            
            simulator = new Simulator();
            stage.addEventListener(KeyboardEvent.KEY_DOWN, simulator.onKeydown);
            
            simulator.addWall( new Wall( 40, 40, 400, 10 ) );
            simulator.addWall( new Wall( 400, 10, 400, 200 ) );
            simulator.addWall( new Wall( 400, 200, 300, 400 ) );
            simulator.addWall( new Wall( 300, 400, 10, 400 ) );
            simulator.addWall( new Wall( 10, 400, 40, 40 ) );
            
           // simulator.addParticle( new Particle( 400, 300, 0, 0, 900, 90 ) );
           simulator.addParticle( new Particle( 100, 300, 0, 0, 400, 70 ) );
            
            for( var i:int = 0; i < 90; i++ )
            {
                
                var canAdd:Boolean = true;
                
                var tx:Number = 50 + Math.random() * 240;
                var ty:Number = 50 + Math.random() * 240;
                
                for each( var particle:Particle in simulator.particles )
                {
                    if( Math.abs( particle.x.x - tx ) < particle.r + 5 && Math.abs( particle.x.y - ty ) < particle.r + 5 )
                    {
                        canAdd = false;
                        i--;
                        break;
                    }
                    
                }
                
                if( canAdd ) simulator.addParticle( new Particle( tx, ty, Math.random() * 900, Math.random() * 900 ) );
                
            }
            
            simulator.addEventListener( Simulator.STEP, onSimulationStep );
           simulator.run( this, Event.ENTER_FRAME );
            render();
            //simulator.run
            
        }
        
        private function onSimulationStep( event:Event ) : void
        {
            render();
        }
        
        public function render() : void
        {
            
            graphics.clear();
            
            graphics.lineStyle( 1 );
            
            for each( var wall:Wall in simulator.walls )
            {
                graphics.moveTo( wall.A.x, wall.A.y );
                graphics.lineTo( wall.B.x, wall.B.y );
            }
            
            
            for each( var particle:Particle in simulator.particles )
            {
                graphics.lineStyle(1, particle.color);
                
                graphics.drawCircle( particle.x.x, particle.x.y, particle.r );
                
                graphics.lineStyle(1, 0);
                if (particle.pair && particle.pair.p2) {
                    graphics.moveTo(particle.pair.p1.x.x, particle.pair.p1.x.y);
                    graphics.lineTo(particle.pair.p2.x.x, particle.pair.p2.x.y);
                }
                if (particle.color == 0xFF0000) {
                    graphics.lineStyle(1, 0xFFFF00);
                    
                    graphics.moveTo(particle.pair.p1.x.x, particle.pair.p1.x.y);
                    graphics.lineTo(particle.pair.p1.x.x + particle.pair.p1.v.x,  particle.pair.p1.x.y + particle.pair.p1.v.y);
                    if (particle.pair.p2) {
                        graphics.lineStyle(1, 0xFF00FF);
                        graphics.moveTo(particle.pair.p2.x.x, particle.pair.p2.x.y);
                        graphics.lineTo(particle.pair.p2.x.x + particle.pair.p2.v.x,  particle.pair.p2.x.y + particle.pair.p2.v.y);
                    }
                    
                }
            }
            
        }
        
    }
}




import __AS3__.vec.Vector;
import de.polygonal.ds.Prioritizable;
import de.polygonal.ds.PriorityQueue;
import flash.events.EventDispatcher;
import flash.events.Event;
import flash.events.KeyboardEvent;
import flash.utils.getTimer;


    
[Event(name="step", type="Simulator")]


class Simulator extends EventDispatcher
{
    
    public static const STEP:String = "step";
    
    public var particles:Vector.<Particle>;
    public var walls:Vector.<Wall>;
    
    //holds all the pairs that passed coarse collision detection
    private var coarsePass:Vector.<ICollidablePair>;
    
    private var time:uint;
    
    
    private var _queue:PriorityQueue = new PriorityQueue(true);
    private var _elapsed:Number;
    
    
    public function Simulator()
    {
        
        super();
        
        particles = new Vector.<Particle>();
        walls = new Vector.<Wall>();
        
    
        
        
    }
    
    public function onKeydown(e:Event):void 
    {
        var particle:Particle
        if (_stepDone) {
            newStep();

            _stepDone  = false;
            
             
            dispatchEvent( new Event(STEP));
        }
        else {
           //_stepDone = true;
            unqueue();  // to resolve closet collision  from stepDone
        
            
            for each( particle in particles )
            {
                particle.integrateTo(_t);
            }   
            dispatchEvent( new Event(STEP));
            
        }
    }
    
    private var _stepDone:Boolean = true;
    private var _t:Number= 0;
    private function onStepDone(e:Event):void 
    {
        _stepDone = true;
    

    }
    
    
    public function addParticle( particle:Particle ) : void
    {
        particles.push( particle );
    }
    
    
    public function addWall( wall:Wall ) : void
    {
        walls.push( wall );
    }
    
    
    //advances the simulation at each dispatch of the passed event type
    public function run( updateDispatcher:EventDispatcher, eventType:String = Event.ENTER_FRAME ) : void
    {
       // time = getTimer();
        //updateDispatcher.addEventListener( eventType, onKeydown, false, 0, true );
    
    }
    
    
    private var _minT:Number;
    private var _minPair:ICollidablePair;
    
    private function unqueue():void {
        if (_queue.isEmpty() || _t >= _elapsed ) {
            _stepDone = true;
            _t = _elapsed;
            return;
        }
        
        //if (!_queue.contains(_minPair) ) throw new Error("DOn't have!:"  );
        
        var pair:ICollidablePair;
        var dt:Number;
        var t:Number = _t;
        var lastPair:ICollidablePair;
        
            pair  =  _queue.dequeue() as ICollidablePair;
            
    
                //if (lastPair && lastPair.priority > pair.priority) throw new Error("SHOUDL NOT BE PRIROITY:!" + lastPair.priority + ',' + pair.priority);
                if (pair.p2 != null) {
                    pair.p2.pair = null;
                }
                pair.p1.pair = null;
                
                pair.p1.color = 0x00FF00;
                if (pair.p2) pair.p2.color = 0x00FF00;
                
                dt = pair.t - t;
                
                //if (dt < 0) throw new Error("SHOULD NOT BE!" + pair.t + ", " +t);
                
                t =  pair.t;
                _t = t;
    
                var rt:Number = _elapsed - t;
                
            
                if (!_stepDone) {
                    pair.update(t, rt)
                }
                else {
                    /*
                    for each(var p:Particle in particles) {
                        p.integrateTo(t);
                    }
                    pair.resolve();
                    */
                    pair.update(t, rt)
                    return;
                }
                //pair.p1.integrateTo(t);
                
                checkParticleAgainstOthers(pair.p1, t, dt, rt, pair.p2);
                if (pair.p2 != null) {
                    //pair.p2.integrateTo(t);
                    checkParticleAgainstOthers(pair.p2, t, dt, rt, pair.p1);
                }
                lastPair = pair;
                
                
                if (!_queue.isEmpty()) {
                    pair = _queue.front() as ICollidablePair;
                    pair.p1.color = 0x3325AA;
                    if (pair.p2) pair.p2.color = 0x3325AA;
                }
    }
    
    private function newStep():void {
        _queue.clear();
        
        const MAX_ITERATIONS:uint = 100;
        
        //delta time in seconds
        var elapsed:Number = 30 / 1000;
        _elapsed = elapsed;
        
        //start this step at 0 and advance to elapsed
        var t:Number = 0;
        _t = 0;
        var dt:Number;
        var iteration:uint = 0;
        
       // while( t < elapsed && ++iteration <= MAX_ITERATIONS )
      //  {
            
            //start by trying to step over the entire remainder
            dt = elapsed - t;
            
            //neglect pairs whose bounding boxes don't overlap
            doCoarsePhase( dt );
            
            //holds the next future collision
          //  var minPair:ICollidablePair = null;
           // var minT:Number = Number.POSITIVE_INFINITY;
           
             _minPair = null;
           _minT = Number.POSITIVE_INFINITY;

            for each( var pair:ICollidablePair in coarsePass )
            {
                

                //if the collision will happen within the current time-step
                //compare the time against the current minimum
                if ( pair.willCollide( dt ) ) {
                    

                    // This code sucks need to rethink better the logic!!
                    var checkTA:Boolean = pair.p1.pair == null;
                    var checkTB:Boolean = pair.p2 ? pair.p2.pair == null : true;
            
                    // need to remove/add to queue as needed if t is found lower
                    
                    if ( pair.p1.pair != null ) {
                        if ( pair.t < pair.p1.pair.t ) {
                            pair.p1.pair.removeFromParent();
                            checkTA = true;
                        }    
                    }
                    
                    if (pair.p2 != null && pair.p2.pair != null ) {
                        if (pair.t < pair.p2.pair.t) {
                            pair.p2.pair.removeFromParent();
                            checkTB = true;
                        }    
                    }
                    if (checkTA && checkTB) {
                        pair.commit();
                    }
                    
                    // For debugging purposes only (to check!)
                    if (pair.t < _minT) {
                        _minT = pair.t;
                        _minPair = pair;
                    }
                    

                
                }
                
            }
            
    }

    
    // TODO: this is buggy
    // particle velocitiy has changed due to collision in timeframe, need to recheck against other moving particles and environment
    public function checkParticleAgainstOthers(p:Particle, t:Number, dt:Number, rt:Number, exclude:Particle):void {

    
        var aabb:AABB = p.aabb;
    
        //var testPP:ParticleParticlePair = new ParticleParticlePair(null, null);
        //var testPW:ParticleWallPair = new ParticleWallPair(null, null);
        
        var minPair:ICollidablePair;
        var minT:Number = Number.MAX_VALUE;
        var testPair:ICollidablePair;
            
            for each( var w:Wall in walls )
            {
                if (aabb.isOverlapping(w.aabb)) {
                    testPair = new ParticleWallPair(_queue, p, w);
                    if (testPair.willCollide(rt)) {
                        testPair.t += t;
                        if (testPair.t < minT) {
                            minPair = testPair;
                            minT = testPair.t;
                        }
                    }
                }
            }
        
            for each( var particle:Particle in particles )
            {
                if (particle === exclude) continue;
                if (particle!= p) {
                    particle.integrateTo(t);
                    //particle.update(dt);
                    
                    
                    if (aabb.isOverlapping(particle.aabb)) {
                        testPair = new ParticleParticlePair(_queue, p, particle);
                        if (testPair.willCollide(rt)) {
                            testPair.t += t;
                            if (testPair.t < minT) {
                                if (particle.pair != null && particle.pair.t <= testPair.t) {
                                    continue;
                                }
                                minPair = testPair;
                                minT = testPair.t;
                            }
                        }
                    }
                }
            }
            
        
            //return;
            if (minPair != null) {
                //if (minPair.p2 != null && exclude != null && exclude === minPair.p2) return;
                //minPair.updatePriority();
                
                if (minPair.p2 != null && minPair.p2.pair !=null) {
                  // if (minPair.p2.pair.t <= minPair.t ) {
                       //throw new Error("Exit still got earlier pair!");
                      // return;
                  // }
                   minPair.p2.pair.removeFromParent();
                }
                minPair.p1.color = 0xFF0000;
                if (minPair.p2) minPair.p2.color = 0xFF0000;
                minPair.commit();
            }
    }
    
    
    //rules out some unnecessary collision checks
    public function doCoarsePhase( dt:Number ) : void
    {
        
        coarsePass = new Vector.<ICollidablePair>();
        
        var aabb:AABB;
        
        for each( var particle:Particle in particles )
        {
            
            //update the particle's bounding box to account for its velocity
            particle.t = 0;
            particle.color = 0;
            particle.pair = null;
            
            particle.update( dt );
            aabb = particle.aabb;
            
            //check each particle against each wall
            for each( var wall:Wall in walls )
            {
                
                if( aabb.isOverlapping( wall.aabb ) )
                {
                    coarsePass.push(  new ParticleWallPair(_queue, particle, wall ) );
                }
                
            }
            
        }
        
        var n:int = particles.length;
        
        //check each particle against each other
        for( var i:int = 0; i < n - 1; i++ )
        {
            
            var p1:Particle = particles[ i ];
            aabb = p1.aabb;
            
            for( var j:int = i + 1; j < n; j++ )
            {
                
                var p2:Particle = particles[ j ];
                
                if( aabb.isOverlapping( p2.aabb ) ) 
                {
                    
            
                    coarsePass.push( new ParticleParticlePair( _queue, p1, p2 ) );
            
                }
                
            }
            
        }
        
    }
    
}




//describes a common interface for collision pairs 
class ICollidablePair implements Prioritizable
{
    public function ICollidablePair(queue:PriorityQueue) {
        parentQueue = queue;
    }
    
    public var priority:int;
    public var position:int;
    
    public var t:Number;
    public  function willCollide( dt:Number ) : Boolean  {return false }
    public  function resolve() : void  { }
    public function update(t:Number, rt:Number):void { }
    public var p1:Particle;
    public var p2:Particle;
    
    public function commit():void {
        //if (p2!=null && p2.pair!=null)  p2.pair.removeFromParent();
        if (p1.pair != null || (p2 && p2.pair != null) ) throw new Error("SHOuld not commit until removal!");
        updatePriority();
        parentQueue.enqueue(this);
        p1.pair = this;
    
        if (p2 != null) p2.pair = this;
    }
    
    public var parentQueue:PriorityQueue;
    public function removeFromParent():void {
         parentQueue.remove(this);
        p1.pair = null;
        
        p1.color = 0;
        if (p2 != null) {
            p2.pair = null;
            p2.color = 0;
        }
    }
    
    public function updatePriority():void {
        priority =  t * (int.MAX_VALUE/30);
        //t = priority * 0.0000000001;
    }
    
    
    
    
}




class ParticleParticlePair extends ICollidablePair
{

    
    
    public function ParticleParticlePair(queue:PriorityQueue, p1:Particle, p2:Particle )
    {
        super(queue);
        this.p1 = p1;
        this.p2 = p2;
    }
    

    
   override public function willCollide( dt:Number ) : Boolean
    {
        
    
        const EPSILON:Number =  1e-2;
        
        //points from 1 -> 2
        var dx:Vec2D = p2.x.minus( p1.x );
        
        //if the circle's are already overlapped, return true (this brings the sim to a halt)
        var c:Number = dx.dot( dx ) - ( p1.r + p2.r ) * ( p1.r + p2.r );
        if( c < 0 )
        {
            t = EPSILON;
            
            
            return true;
        }
        
        //relative velocity
        var dv:Vec2D = p2.v.minus( p1.v );
        
        var a:Number = dv.dot( dv );
        if( a < EPSILON ) return false; //not moving enough toward each other to warrant a response
        
        var b:Number = dv.dot( dx );
        if( b >= 0 ) return false; //moving apart
        
        var d:Number = b * b - a * c;
        if( d < 0 ) return false; //no intersection
        
        t = ( -b - Math.sqrt( d ) ) / a;
        
        //priority =  t * 100000;
        //t = priority * 0.00001;
        
        //circle's collide if the time of collision is within the current time-step
        return t <= dt;

    }
    
    //simulation has been updated so that the particles are just colliding
   override public function resolve() : void
    {
        
        //points from 1 -> 2
        var cn:Vec2D = p2.x.minus( p1.x );
        
        cn.normalize();
        
        //relative velocity
        var dv:Vec2D = p2.v.minus( p1.v );
        
        //perfectly elastic impulse
        var impulse:Number = cn.dot( dv.times( -2 ) ) / cn.dot( cn.times( 1 / p1.mass + 1 / p2.mass ) );
        
        //scale normal by the impulse
        p1.v.plusEquals( cn.times( -impulse / p1.mass ) );
        p2.v.plusEquals( cn.times(  impulse / p2.mass ) );
        
        //p1.v.x = 0; p1.v.y = 0;
        //p2.v.x = 0; p2.v.y = 0;
        
    }
    
    override public function update(t:Number, rt:Number):void {
        p1.integrateTo(t);
        p2.integrateTo(t);
        resolve();
        p1.update(rt);
        p2.update(rt);
    }

    
}





class ParticleWallPair extends ICollidablePair
{
    

    public var w:Wall;
    

    
    public function ParticleWallPair( queue:PriorityQueue, p:Particle, w:Wall )
    {
        super(queue);
        this.p1 = p;
        this.w = w;
        
    }
    

    
             
   override  public function willCollide( dt:Number ) : Boolean
    {
  
        //this is line/line intersection
        
        //A is the position of the particle
        //B is the position + velocity
        //together they make the segment AB
        
        
        
        //CD is the line segment made up of the wall's end points
        
        var A:Vec2D = p1.x;
        var B:Vec2D = p1.x.plus( p1.v );
        
        var AB:Vec2D = B.minus( A );
        
        //inflate the normal by the particle's radius
        var normScaledRadius:Vec2D = w.normal.times( -p1.r );
        
        //push the wall segment in by this amount
        var C:Vec2D = w.A.plus( normScaledRadius );
        var D:Vec2D = w.B.plus( normScaledRadius );
        
        var CD:Vec2D = D.minus( C )
        var AC:Vec2D = C.minus( A );
        
        t = w.normal.dot( AC ) / w.normal.dot( AB );
        
        if( isNaN( t ) ) t = 0;
        
        
        //priority =  t * 100000;
        //t = priority * 0.00001;
        
        return t <= dt && t >= 0;

    }
    
    override public function update(t:Number, rt:Number):void {
        p1.integrateTo(t);
        resolve();
        p1.update(rt);
    }
    
    //simulation has been updated so that the particles are coincident
   override  public function resolve() : void
    {
                
        var cn:Vec2D = w.normal;        
                        
        //relative velocity
        var dv:Vec2D = p1.v;
        
        //perfectly elastic
        var impulse:Number = cn.dot( dv.times( -2 ) ) / ( 1 / p1.mass );
        
        p1.v.plusEquals( cn.times( impulse / p1.mass ) );
        
        //p1.v.x = -cn.x*6;
        //p1.v.y = -cn.y*6;
    //    
        
    }
    
}





class Wall
{
    
    public var A:Vec2D;
    public var B:Vec2D;
    
    public var aabb:AABB;
    
    public var normal:Vec2D;
    
    public function Wall( ax:Number, ay:Number, bx:Number, by:Number ) 
    {
        
        A = new Vec2D( ax, ay );
        B = new Vec2D( bx, by );
        
        normal = new Vec2D( B.y - A.y, -( B.x - A.x ) );
        normal.normalize();
        
        aabb = new AABB();
        
        aabb.minx = Math.min( ax, bx );
        aabb.maxx = Math.max( ax, bx );
        aabb.miny = Math.min( ay, by );
        aabb.maxy = Math.max( ay, by );
        
    }
    
}


class Particle
{
    
    //position
    public var x:Vec2D;
    
    //velocity
    public var v:Vec2D;
    
    public var mass:Number;
    
    //radius
    public var r:Number;
    
    //bounding box
    public var aabb:AABB;
    
    public var t:Number;
    
    public var pair:ICollidablePair;
    
    public var color:uint = 0;
    

    
    public function Particle( xx:Number, xy:Number, vx:Number, vy:Number, mass:Number = 1.0, radius:Number = 5 )
    {
        
        x = new Vec2D( xx, xy );
        v = new Vec2D( vx, vy );
        
        this.mass = mass;
        this.r = radius;
        
        aabb = new AABB();
        
    }
    
    public function update( t:Number ) : void
    {
        
        var xt:Number = x.x + v.x * t;
        var yt:Number = x.y + v.y * t;
        
        var minx:Number = Math.min( x.x, xt );
        var maxx:Number = Math.max( x.x, xt );
        
        var miny:Number = Math.min( x.y, yt );
        var maxy:Number = Math.max( x.y, yt );
        
        aabb.minx = minx - r;
        aabb.maxx = maxx + r;
        aabb.miny = miny - r;
        aabb.maxy = maxy + r;
        
    }
    
    public function integrate( dt:Number ) : void
    {
        x.x += v.x * dt;
        x.y += v.y * dt;
        //if (pair) pair.t -= dt;
    }
    
    public function integrateTo(globalT:Number):void {
    
        if (globalT == t) return;
        var dt:Number = globalT - t - 1e-4;
        x.x += v.x * dt;
        x.y += v.y * dt;
        t = globalT;
        //if (pair) pair.t -= dt;
    }
    
}


class AABB
{
    
    public var minx:Number = 0;
    public var maxx:Number = 0;
    public var miny:Number = 0;
    public var maxy:Number = 0;
    
    public function isOverlapping( aabb:AABB ) : Boolean
    {
        
        if( minx > aabb.maxx ) return false;
        if( miny > aabb.maxy ) return false;
        if( maxx < aabb.minx ) return false;
        if( maxy < aabb.miny ) return false;
        
        return true;
        
    }
    
}


class Vec2D
{
    
    public var x:Number;
    public var y:Number;
    
    public function Vec2D( x:Number = 0.0, y:Number = 0.0 )
    {
        this.x = x;
        this.y = y;
    }
    
    public function plusEquals( vec2D:Vec2D ) : void
    {
        x += vec2D.x;
        y += vec2D.y;
    }
    
    public function plus( vec2D:Vec2D ) : Vec2D
    {
        return new Vec2D( x + vec2D.x, y + vec2D.y );
    }
    
    public function minus( vec2D:Vec2D ) : Vec2D
    {
        return new Vec2D( x - vec2D.x, y - vec2D.y );
    }
    
    public function times( s:Number ) : Vec2D
    {
        return new Vec2D( x * s, y * s );
    }
    
    public function dot( vec2D:Vec2D ) : Number
    {
        return x * vec2D.x + y * vec2D.y;
    }
    
    public function get magnitude() : Number
    {
        return Math.sqrt( x * x + y * y );
    }
    
    public function normalize() : void
    {
        
        var length:Number = magnitude;
        
        if( length == 0 ) return;
        
        x /= length;
        y /= length;
        
    }
    
}