In case Flash no longer exists; a copy of this site is included in the Flashpoint archive's "ultimate" collection.

Dead Code Preservation :: Archived AS3 works from wonderfl.net

RK4 Cloth Simulation (mechanical optimization)

@author simoe
Get Adobe Flash player
by yonatan 13 Jul 2011
  • Forked from esimov's RK4 Cloth Simulation
  • Diff: 192
  • Related works: 2
  • Talk

    makc3d at 12 Jul 2011 22:02
    lol it passes through itself :)
    esimov at 13 Jul 2011 00:32
    nice, you obtain a pretty amazing fps
    bradsedito at 13 Jul 2011 00:54
    Yeah man, how are are you able to maintain that consistently high frame-rate? I would assume it's all in the math, or no?
    yonatan at 13 Jul 2011 02:37
    To be honest I didn't even read the math parts. I just replaced some code to avoid creating new objects during the calculation. (Vector3D's add() and clone() methods return new vectors so they had to go).
    yonatan at 13 Jul 2011 02:47
    Oh, and lines are rendered much faster with CapsStyle.NONE

    Tags

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

// forked from esimov's RK4 Cloth Simulation
package  
{
    import flash.display.Stage;
    import flash.display.DisplayObject;
    import flash.display.LoaderInfo;
    import flash.net.URLRequest;
    import flash.display.Loader;
    import flash.system.SecurityDomain;
    import flash.system.ApplicationDomain;
    import flash.system.LoaderContext;
    import flash.system.Security;
    import net.hires.debug.Stats;

    import flash.display.GradientType;
    import flash.display.Sprite;
    import flash.events.Event;
    import flash.events.KeyboardEvent;
    import flash.events.MouseEvent;
    import flash.geom.Matrix;
    import flash.geom.Vector3D;
    import flash.ui.Keyboard;

    [ SWF (width=480, height=480, backgroundColor=0xeaeaea, frameRate=60) ]

    /**
     * @author simoe
     */
    public class ClothSimulation extends Sprite
    {    
        private var ps : ParticleSystem;
        private var particles : Vector.<Vector.<Particle>>;
        private var springs : Vector.<Spring>;
        private var dragPoint : Particle;
        private var mouseDown : Boolean = false;
        private var handle1Down:Boolean = false;
        private var handle2Down:Boolean = false;

        private var numRows : Number = 25;
        private var numCols : Number = 25;
        private var offset : Number = 14;

        private var handle1:DisplayObject;
        private var handle2:DisplayObject;
        private var pin1:Sprite;
        private var pin2:Sprite;
        private var canvas : Sprite;
        private var stats : Stats;

        private const STAGE_WIDTH : Number = stage.stageWidth;
        private const STAGE_HEIGHT : Number = stage.stageHeight;

        public function ClothSimulation() : void
        {
            var i : int, j : int;
            var clothWidth : Number = (numRows - 1) * offset;
            var clothHeight : Number = (numCols - 1) * offset;
            var posX : Number = (STAGE_WIDTH - clothWidth) / 2;
            var posY : Number = (STAGE_HEIGHT - clothHeight) / 2;
            
            stage.quality = "medium";
            stats = new Stats();
            particles = new Vector.<Vector.<Particle>>();
            springs = new Vector.<Spring>();
            ps = new ParticleSystem(new Vector3D(0, 0.08, 0), 0.01);
                    
            //switch colums and rows if rows>cols
            if (numRows > numCols)
            {
                var temp : Number = numRows;
                numRows = numCols;
                numCols = temp;
            }
            
            //create a two dimensional vector array
            for (i = 0;i < numRows;i++)
            {
                var vector_rows : Vector.<Particle> = new Vector.<Particle>();
                for (j = 0;j < numCols;j++)
                {    
                    vector_rows.push(ps.makeParticle(0.9, new Vector3D(posX + j * offset, posY + i * offset, 0)));
                }
                
                particles.push(vector_rows);
            }
            
            //create Spring attractors between particles horozontally
            for (i = 0;i < numRows;i++)
            {
                for (j = 0;j < numCols - 1;j++)
                {
                    ps.makeSpring(particles[i][j], particles[i][j + 1], 0.9, 0.2, offset >> 1);
                }
            }
            
            //create Spring attractors between particles vertically
            for (i = 0;i < numRows - 1;i++)
            {
                for (j = 0;j < numCols;j++)
                {
                    ps.makeSpring(particles[i][j], particles[i + 1][j], 0.9, 0.2, offset >> 1);
                }
            }
            
            createBackground();
            canvas = new Sprite();
            pin1 = new Sprite();
            pin2 = new Sprite();
            
            pin1.x = particles[0][0].position.x - 35;
            pin1.y = particles[0][0].position.y - 25;
            
            pin2.x = particles[0][numRows - 1].position.x - 30;
            pin2.y = particles[0][numCols - 1].position.y - 25;
            
            addChild(canvas);
            canvas.addChild(pin1);
            canvas.addChild(pin2);
            addChild(stats);
            
            Security.loadPolicyFile("http://esimov.zxq.net/crossdomain.xml");
            var context:LoaderContext = new LoaderContext();
            context.checkPolicyFile = true;
            context.applicationDomain = ApplicationDomain.currentDomain;
            context.securityDomain = SecurityDomain.currentDomain;
            var loader1:Loader = new Loader();
            loader1.contentLoaderInfo.addEventListener(Event.COMPLETE, onLoadImage1);
            //loader.contentLoaderInfo.addEventListener(IOErrorEvent.IO_ERROR, IOErrorListener);
            loader1.load(new URLRequest("http://esimov.zxq.net/pin_handle.png"));
            
            var loader2:Loader = new Loader();
            loader2.contentLoaderInfo.addEventListener(Event.COMPLETE, onLoadImage2);
            //loader.contentLoaderInfo.addEventListener(IOErrorEvent.IO_ERROR, IOErrorListener);
            loader2.load(new URLRequest("http://esimov.zxq.net/pin_handle.png"));
            particles[0][0].makeFix();
            particles[0][numCols - 1].makeFix();
                    
            addEventListener(MouseEvent.MOUSE_DOWN, onClothPress);
            addEventListener(MouseEvent.MOUSE_UP, onClothRelease);
            addEventListener(KeyboardEvent.KEY_DOWN, keyDownListener);
            
            addEventListener(Event.ENTER_FRAME, renderCloth);
        }

        private function createBackground() : void 
        {
            var background : Sprite = new Sprite();
            var matrix : Matrix = new Matrix();
            matrix.createGradientBox(STAGE_WIDTH, STAGE_HEIGHT);
            background.graphics.beginGradientFill(GradientType.RADIAL, [0xEFEFEF, 0xEFEFEF, 0xEAEAEA], [1, 1, 1], [0x00, 0x7F, 0xFF], matrix);
            background.graphics.drawRect(0, 0, STAGE_WIDTH, STAGE_HEIGHT);
            background.graphics.endFill();
            addChild(background);
        }

        
        private function onLoadImage1(event:Event):void
        {
            var info:LoaderInfo = event.target as LoaderInfo;
            
            handle1= info.content;
            pin1.addChild(handle1);
            
            pin1.addEventListener(MouseEvent.MOUSE_DOWN, onHandleDown);
            pin1.addEventListener(MouseEvent.MOUSE_UP, onHandleDown);
        }

        private function onLoadImage2(event:Event):void
        {
            var info:LoaderInfo = event.target as LoaderInfo;
            
            handle2 = info.content;
            pin2.addChild(handle2);
            
            pin2.addEventListener(MouseEvent.MOUSE_DOWN, onHandleDown);
            pin2.addEventListener(MouseEvent.MOUSE_UP, onHandleDown);
        }
        
        private function onHandleDown(event:MouseEvent):void
        {
            event.stopPropagation();
            switch (event.type)
            {
                case "mouseDown":
                    if (event.target == pin1)
                    {
                        handle1Down = true;
                        handle2Down = false;
                    } else if (event.target == pin2)
                    {
                        handle1Down = false;
                        handle2Down = true;
                    }
                    break;
                    
                case "mouseUp":
                    handle1Down = false;
                    handle2Down = false;
                    break;
            }
        }

        private function keyDownListener(event : KeyboardEvent) : void
        {
            if (event.keyCode == Keyboard.SPACE)
            {
                stats.visible = !stats.visible;
            }
        }

        private function onClothPress(event : MouseEvent) : void
        {
            mouseDown = true;
            dragPoint = searchDraggingPoint();
            dragPoint.isDragging = true;
        }

        private function onClothRelease(event : MouseEvent) : void
        {
            mouseDown = false;
            dragPoint.isDragging = false;
            dragPoint = undefined;
        }

        
        private function searchDraggingPoint() : Particle
        {
            var target : Particle;
            var lastMinimumDist : Number = Infinity;
            
            for (var i : int = 0;i < numRows;i++)
            {
                for (var j : int = 0;j < numCols;j++)
                {        
                    var particle : Particle = particles[i][j];
                    var mousePos : Vector3D = new Vector3D(mouseX, mouseY, 0);
                    var dist : Number = particle.position.subtract(mousePos).lengthSquared;
                    
                    if (dist < lastMinimumDist)
                    {
                        lastMinimumDist = dist;
                        target = particle; 
                    }
                }
            }
            return target;
        }

        private function renderCloth(event : Event) : void 
        {
            particles[0][0].position.x = pin1.x + 35;
            particles[0][0].position.y = pin1.y + 25;
            
            particles[0][numCols - 1].position.x = pin2.x + 30;
            particles[0][numCols - 1].position.y = pin2.y + 25;
            
            if (handle1Down)
            {
                pin1.x = mouseX - 35;
                pin1.y = mouseY - 25;                
            }
            
            if (handle2Down)
            {
                pin2.x = mouseX - 35;
                pin2.y = mouseY - 25;
            }
            
            if(mouseDown)
            {
                dragPoint.position.x = mouseX;
                dragPoint.position.y = mouseY;
                dragPoint.velocity.scaleBy(dragPoint.mass);
            }
            
            ps.applyIntegrator(1);
            ps.clearForces();
            
            canvas.graphics.clear();
            canvas.graphics.lineStyle(1, 0x555555, 1, false, null, "none");
            
            for (var i : int = 0;i < numRows;i++)
            {
                for (var j : int = 0;j < numCols - 1;j++)
                {        
                    canvas.graphics.moveTo(particles[i][j].position.x, particles[i][j].position.y);
                    canvas.graphics.lineTo(particles[i][j + 1].position.x, particles[i][j + 1].position.y);
                }
            }
            
            for (i = 0;i < numRows - 1;i++)
            {
                for (j = 0;j < numCols;j++)
                {        
                    canvas.graphics.moveTo(particles[i][j].position.x, particles[i][j].position.y);
                    canvas.graphics.lineTo(particles[i + 1][j].position.x, particles[i + 1][j].position.y);
                }
            }
        }
    }
}

var s:Number;
var vel:Vector3D = new Vector3D;
var intVel:Vector3D = new Vector3D;
var intForce:Vector3D = new Vector3D;
var k1VelInt:Vector3D = new Vector3D;
var k2VelInt:Vector3D = new Vector3D;
var k3VelInt:Vector3D = new Vector3D;
var k4VelInt:Vector3D = new Vector3D;
var k1ForceInt:Vector3D = new Vector3D;
var k2ForceInt:Vector3D = new Vector3D;
var k3ForceInt:Vector3D = new Vector3D;
var k4ForceInt:Vector3D = new Vector3D;
var k1Vel:Vector3D = new Vector3D;
var k2Vel:Vector3D = new Vector3D;
var k3Vel:Vector3D = new Vector3D;
var k4Vel:Vector3D = new Vector3D;
var k1Force:Vector3D = new Vector3D;
var k2Force:Vector3D = new Vector3D;
var k3Force:Vector3D = new Vector3D;
var k4Force:Vector3D = new Vector3D;

import flash.geom.Vector3D;

internal class Particle 
{
    public var position : Vector3D;
    public var velocity : Vector3D;
    public var force : Vector3D;

    public var mass : Number;
    public var fixed : Boolean;
    public var isDragging : Boolean;

    public static const BOUNCE : String = "bounce";
    public static const WRAP : String = "wrap";

    public function Particle(mass : Number, position : Vector3D) : void 
    {
        this.position = (position) ? position : new Vector3D();
        this.velocity = new Vector3D;
        this.force = new Vector3D;
        this.mass = mass;
            
        isDragging = false; 
        fixed = false;
    }

    public final function distanceBetween(p : Particle) : Number
    {
        return Vector3D.distance(this.position, p.position);
    }

    public final function makeFree() : void
    {
        fixed = false;
    }

    public final function makeFix() : void
    {
        fixed = true;
        this.velocity.x = 0; 
        this.velocity.y = 0; 
        this.velocity.z = 0;
    }

    public final function isFree() : Boolean
    {
        return !fixed;
    }

    public final function isFixed() : Boolean
    {
        return fixed;
    }

    public function setMass(value : Number) : void
    {
        this.mass = value;
    }

    public function reset() : void
    {
        this.position.x = 0; 
        this.position.y = 0; 
        this.position.z = 0;
        this.velocity.y = 0; 
        this.velocity.y = 0; 
        this.velocity.z = 0;
        this.force.x = 0; 
        this.force.y = 0; 
        this.force.z = 0;
        this.mass = 1;
    }

    public function boundCheck(type : String, left : Number, right : Number, top : Number, bottom : Number) : void
    {
        switch (type)
        {
            case Particle.BOUNCE:
                
                if (this.position.x < left)
                {
                    position.x = left + (left - position.x);
                    velocity.x *= -1;
                }
                
                if (this.position.x > right)
                {
                    position.x = right - (position.x - right);
                    velocity.x *= -1;
                }
                
                if (this.position.y < top)
                {
                    position.y = top + (top - position.y);
                    velocity.y *= -1;
                }
                
                if (this.position.y > bottom)
                {
                    position.y = bottom - (position.y - bottom);
                    velocity.y *= -1;
                }
                break;
                
            case Particle.WRAP:
                break;
        }
    }
}

internal interface Integrator
{
    function apply(t : Number) : void;
}

internal class Spring
{
    private var p1 : Particle;
    private var p2 : Particle;
    private var springConst : Number;
    private var on : Boolean;

    private var damping : Number;
    private var restLength : Number;

    public function Spring(p1 : Particle, p2 : Particle, springConst : Number, damping : Number, restLength : Number) : void 
    {
        this.p1 = p1;
        this.p2 = p2;
        this.damping = damping;
        this.restLength = restLength;
        this.springConst = springConst;
            
        this.on = true;
    }

    public final function turnOn() : void
    {
        on = true;
    }

    public final function turnOff() : void
    {
        on = false;
    }

    public final function isOn() : Boolean
    {
        return on;
    }

    public final function isOff() : Boolean
    {
        return !on;
    }

    public final function getDistance(p1 : Particle, p2 : Particle) : Number
    {
        return Vector3D.distance(p1.position, p2.position);
    }

    public final function setDamping(value : Number) : void
    {
        this.damping = value;
    }

    public final function getDamping() : Number
    {
        return damping;
    }

    public final function setRestLength(value : Number) : void
    {
        this.restLength = value;
    }

    public final function getRestLength() : Number
    {
        return this.restLength;
    }

    public function update() : void
    {
        if (on && (!p1.fixed || !p2.fixed ))
        {
            var distX : Number = p2.position.x - p1.position.x;
            var distY : Number = p2.position.y - p1.position.y;
            var distZ : Number = p2.position.z - p1.position.z;
                
            var distSq : Number = Math.sqrt(distX * distX + distY * distY + distZ * distZ);
            if (distSq == 0)
            {
                distX = 0; 
                distY = 0; 
                distZ = 0;
            }
            distX /= distSq;
            distY /= distSq;
            distZ /= distSq;
                
            var springForce : Number = -(distSq - restLength) * springConst;
            var velX : Number = p2.velocity.x - p1.velocity.x;
            var velY : Number = p2.velocity.y - p1.velocity.y;
            var velZ : Number = p2.velocity.z - p1.velocity.z;
                
            var dampingForce : Number = -damping * (distX * velX + distY * velY + distZ * velZ);
            var aggregateForce : Number = springForce + dampingForce;
            distX *= aggregateForce;
            distY *= aggregateForce;
            distZ *= aggregateForce;
                
            if (!p1.fixed) 
            { 
                // p1.force = p1.force.add(new Vector3D(-distX, -distY, -distZ)); 
				p1.force.x -= distX;
				p1.force.y -= distY;
				p1.force.z -= distZ;
            }
            if (!p2.fixed) 
            { 
                // p2.force = p2.force.add(new Vector3D(distX, distY, distZ)); 
				p2.force.x += distX;
				p2.force.y += distY;
				p2.force.z += distZ;
            }
        }
    }
}

internal class Attraction 
{
    private var p1 : Particle;
    private var p2 : Particle;
    private var minDistance : Number;
    private var minDistanceSq : Number;
    private var strength : Number;

    private var on : Boolean;

    public function Attraction(p1 : Particle, p2 : Particle, strength : Number, minDistance : Number) : void 
    {
        this.p1 = p1;
        this.p2 = p2;
        this.minDistance = minDistance;
        this.minDistanceSq = minDistance * minDistance;
        this.strength = strength;
            
        on = true;
    }

    public final function turnOn() : void
    {
        on = true;
    }

    public final function turnOff() : void
    {
        on = false;
    }

    public final function isOn() : Boolean
    {
        return on;
    }

    public final function isOff() : Boolean
    {
        return !on;
    }

    public final function setStrength(value : Number) : void
    {
        this.strength = value;
    }

    public final function getStrength() : Number
    {
        return this.strength;
    }

    public final function setDistance(value : Number) : void
    {
        this.minDistance = value;
        this.minDistanceSq = value * value;
    }

    public final function getDistance() : Number
    {
        return minDistance;
    }

    public function update() : void
    {
        if (on && (!p1.fixed || !p2.fixed ))
        {
            var distX : Number = p2.position.x - p1.position.x;
            var distY : Number = p2.position.y - p1.position.y;
            var distZ : Number = p2.position.z - p1.position.z;
                
            var distanceSq : Number = distX * distX + distY * distY + distZ * distZ;
            if (distanceSq < minDistanceSq) 
            {
                distanceSq = minDistanceSq; 
            }
            var invLength : Number = 1 / Math.sqrt(distanceSq);
                
            var force : Number = strength * (p1.mass * p2.mass) / distanceSq;
            
            distX *= invLength;
            distY *= invLength;
            distZ *= invLength;
            
            distX *= force;
            distY *= force;
            distZ *= force;
                
            if (!p1.fixed) 
            { 
                //p1.force = p1.force.add(new Vector3D(distX, distY, distZ)); 
				p1.force.x += distX;
				p1.force.y += distY;
				p1.force.z += distZ;
            }
            if (!p2.fixed) 
            { 
                //p2.force = p2.force.add(new Vector3D(-distX, -distY, -distZ)); 
				p2.force.x -= distX;
				p2.force.y -= distY;
				p2.force.z -= distZ;
            }
        }
    }
}

internal class RKIntegrator implements Integrator 
{
    private var originalPosV : Vector.<Vector3D>;
    private var originalVelV : Vector.<Vector3D>;
    private var k1VelV : Vector.<Vector3D>;
    private var k1ForceV : Vector.<Vector3D>;
    private var k2VelV : Vector.<Vector3D>;
    private var k2ForceV : Vector.<Vector3D>;
    private var k3VelV : Vector.<Vector3D>;
    private var k3ForceV : Vector.<Vector3D>;
    private var k4VelV : Vector.<Vector3D>;
    private var k4ForceV : Vector.<Vector3D>;

    private var p : Particle;

    private var particleSystem : ParticleSystem;

    public function RKIntegrator(particleSystem : ParticleSystem) : void 
    {
        this.particleSystem = particleSystem;
            
        originalPosV = new Vector.<Vector3D>();
        originalVelV = new Vector.<Vector3D>();
        k1VelV = new Vector.<Vector3D>();
        k1ForceV = new Vector.<Vector3D>();
        k2VelV = new Vector.<Vector3D>();
        k2ForceV = new Vector.<Vector3D>();
        k3VelV = new Vector.<Vector3D>();
        k3ForceV = new Vector.<Vector3D>();
        k4VelV = new Vector.<Vector3D>();
        k4ForceV = new Vector.<Vector3D>();            
    }

    /* INTERFACE esimov.physics.Integrator */

    private function createParticles() : void
    {
        while(particleSystem.particles.length > originalPosV.length)
        {
            originalPosV.push(new Vector3D());
            originalVelV.push(new Vector3D());
            k1VelV.push(new Vector3D());
            k1ForceV.push(new Vector3D());
            k2VelV.push(new Vector3D());
            k2ForceV.push(new Vector3D());
            k3VelV.push(new Vector3D());
            k3ForceV.push(new Vector3D());
            k4VelV.push(new Vector3D());
            k4ForceV.push(new Vector3D());
        }
    }

    public function apply(t : Number) : void
    {
        createParticles();
            
        var numPart : Number = particleSystem.particles.length;
        var particles : Vector.<Particle> = particleSystem.particles;
        var i : int;
            
        var originalPos : Vector3D;
        var originalVel : Vector3D;
        var k1Vel : Vector3D;
        var k1Force : Vector3D;
        var k2Vel : Vector3D;
        var k2Force : Vector3D;
        var k3Vel : Vector3D;
        var k3Force : Vector3D;
        var k4Vel : Vector3D;
        var k4Force : Vector3D;
            
        /**
         * Get initial position and velocity,
         * apply forces and velocity, the result is K1
         */

        for (i = 0;i < numPart;i++)
        {
            if (!Particle(particles[i]).fixed)
            {
                originalPosV[i].x = Particle(particles[i]).position.x;
                originalPosV[i].y = Particle(particles[i]).position.y;
                originalPosV[i].z = Particle(particles[i]).position.z;
                originalVelV[i].x = Particle(particles[i]).velocity.x;
                originalVelV[i].y = Particle(particles[i]).velocity.y;
                originalVelV[i].z = Particle(particles[i]).velocity.z;
            }
                
            Particle(particles[i]).force.x = 0;
            Particle(particles[i]).force.y = 0;
            Particle(particles[i]).force.z = 0;
        }
            
        particleSystem.applyForces();
            
        for (i = 0;i < numPart;i++)
        {
            if (!Particle(particles[i]).fixed)
            {
                k1ForceV[i].x = Particle(particles[i]).force.x;
                k1ForceV[i].y = Particle(particles[i]).force.y;
                k1ForceV[i].z = Particle(particles[i]).force.z;
                k1VelV[i].x = Particle(particles[i]).velocity.x;
                k1VelV[i].y = Particle(particles[i]).velocity.y;
                k1VelV[i].z = Particle(particles[i]).velocity.z;
            }
                
            Particle(particles[i]).force.x = 0;
            Particle(particles[i]).force.y = 0;
            Particle(particles[i]).force.z = 0;
        }
            
        /**
         * Get initial position, K1 velocity
         * apply forces and velocity, the result is K2
         */


        for (i = 0;i < numPart;i++) 
        {
            p = particles[i];

            if (!p.fixed) 
            {    
                originalPos = originalPosV[i];
                k1Vel = k1VelV[i];
                    
                s = (0.5 * t);
                k1Vel.x *= s;
                k1Vel.y *= s;
                k1Vel.z *= s;
                p.position.x = originalPos.x+k1Vel.x;
                p.position.y = originalPos.y+k1Vel.y;
                p.position.z = originalPos.z+k1Vel.z;
                    
                originalVel = originalVelV[i];
                k1Force = k1ForceV[i];
                    
                s = (0.5 * t / p.mass);
                k1Force.x *= s;
                k1Force.y *= s;
                k1Force.z *= s;
                p.velocity.x = originalVel.x+k1Force.x;
                p.velocity.y = originalVel.y+k1Force.y;
                p.velocity.z = originalVel.z+k1Force.z;
            }
        }
            
        particleSystem.applyForces();
            
        for (i = 0;i < numPart;i++)
        {
            p = particles[i];
                
            if (!p.fixed) 
            {
                k2ForceV[i].x = p.force.x;
                k2ForceV[i].y = p.force.y;
                k2ForceV[i].z = p.force.z;
                k2VelV[i].x = p.velocity.x;
                k2VelV[i].y = p.velocity.y;
                k2VelV[i].z = p.velocity.z;
            }
                
            p.force.x = 0; 
            p.force.y = 0; 
            p.force.z = 0;
        }
            
        /**
         * Get initial position, K2 velocity
         * apply forces and velocity, the result is K3
         */

        for (i = 0;i < numPart;i++) 
        {
            p = particles[i];
                
            if (!p.fixed) 
            {    
                originalPos = originalPosV[i];
                k2Vel = k2VelV[i];
                    
                s = (0.5 * t);
                k2Vel.x *= s;
                k2Vel.y *= s;
                k2Vel.z *= s;
                p.position.x = originalPos.x+k2Vel.x;
                p.position.y = originalPos.y+k2Vel.y;
                p.position.z = originalPos.z+k2Vel.z;
                    
                originalVel = originalVelV[i];
                k2Force = k2ForceV[i];
                    
                s = (0.5 * t / p.mass);
                k2Force.x *= s;
                k2Force.y *= s;
                k2Force.z *= s;
                p.velocity.x = originalVel.x+k2Force.x;
                p.velocity.y = originalVel.y+k2Force.y;
                p.velocity.z = originalVel.z+k2Force.z;
            }
        }
            
        particleSystem.applyForces();
            
        for (i = 0;i < numPart;i++)
        {
            p = particles[i];
                
            if (!p.fixed) 
            {
                k3ForceV[i].x = p.force.x;
                k3ForceV[i].y = p.force.y;
                k3ForceV[i].z = p.force.z;
                k3VelV[i].x = p.velocity.x;
                k3VelV[i].y = p.velocity.y;
                k3VelV[i].z = p.velocity.z;
            }
                
            p.force.x = 0; 
            p.force.y = 0; 
            p.force.z = 0;
        }
            
        /**
         * Get initial position, K3 velocity
         * apply forces and velocity, the result is K4
         */

        for (i = 0;i < numPart;i++) 
        {
            p = particles[i];
                
            if (!p.fixed) 
            {    
                originalPos = originalPosV[i];
                k3Vel = k3VelV[i];
                    
                s = (0.5 * t);
                k3Vel.x *= s;
                k3Vel.y *= s;
                k3Vel.z *= s;
                p.position.x = originalPos.x+k2Vel.x;
                p.position.y = originalPos.y+k2Vel.y;
                p.position.z = originalPos.z+k2Vel.z;
                    
                originalVel = originalVelV[i];
                k3Force = k3ForceV[i];
                    
                s = (0.5 * t / p.mass);
                k3Force.x *= s;
                k3Force.y *= s;
                k3Force.z *= s;
                p.velocity.x = originalVel.x+k3Force.x;
                p.velocity.y = originalVel.y+k3Force.y;
                p.velocity.z = originalVel.z+k3Force.z;
            }
        }
            
        particleSystem.applyForces();
            
        for (i = 0;i < numPart;i++)
        {
            p = particles[i];
                
            if (!p.fixed) 
            {
                k4ForceV[i].x = p.force.x;
                k4ForceV[i].y = p.force.y;
                k4ForceV[i].z = p.force.z;
                k4VelV[i].x = p.velocity.x;
                k4VelV[i].y = p.velocity.y;
                k4VelV[i].z = p.velocity.z;
            }
                
            p.force.x = 0; 
            p.force.y = 0; 
            p.force.z = 0;
        }
            
        /**
         * Update initial position and velocity based on intermediate values
         */

        for (i = 0;i < numPart;i++)
        {
            p = particles[i];
                
            if (!p.fixed) 
            {    
                //position

                originalPos = originalPosV[i];
                k1Vel = k1VelV[i];
                k2Vel = k2VelV[i];
                k3Vel = k3VelV[i];
                k4Vel = k4VelV[i];
                  
//                var k2VelInt : Vector3D = k2Vel.clone();
                k2VelInt.x = k2Vel.x;
                k2VelInt.y = k2Vel.y;
                k2VelInt.z = k2Vel.z;
                s = (2);
                k2VelInt.x *= s;
                k2VelInt.y *= s;
                k2VelInt.z *= s;
//                var k3VelInt : Vector3D = k3Vel.clone();
                k3VelInt.x = k3Vel.x;
                k3VelInt.y = k3Vel.y;
                k3VelInt.z = k3Vel.z;
                s = (2);
                k3VelInt.x *= s;
                k3VelInt.y *= s;
                k3VelInt.z *= s;
                //var intVel : Vector3D = k1Vel.add(k2VelInt).add(k3VelInt).add(k4Vel);
				intVel.x = k1Vel.x + k2Vel.x + k3Vel.x + k4Vel.x;
				intVel.y = k1Vel.y + k2Vel.y + k3Vel.y + k4Vel.y;
				intVel.z = k1Vel.z + k2Vel.z + k3Vel.z + k4Vel.z;
                s = (t / 6);
                intVel.x *= s;
                intVel.y *= s;
                intVel.z *= s;
                p.position.x = originalPos.x+intVel.x;
                p.position.y = originalPos.y+intVel.y;
                p.position.z = originalPos.z+intVel.z;
                    
                // velocity

                originalVel = originalVelV[i];
                k1Force = k1ForceV[i];
                k2Force = k2ForceV[i];
                k3Force = k3ForceV[i];
                k4Force = k4ForceV[i];
                    
//                var k2ForceInt : Vector3D = k2Force.clone();
                k2ForceInt.x = k2Force.x;
                k2ForceInt.y = k2Force.y;
                k2ForceInt.z = k2Force.z;
                s = (2);
                k2ForceInt.x *= s;
                k2ForceInt.y *= s;
                k2ForceInt.z *= s;
//                var k3ForceInt : Vector3D = k3Force.clone();
                k3ForceInt.x = k3Force.x;
                k3ForceInt.y = k3Force.y;
                k3ForceInt.z = k3Force.z;
                s = (2);
                k3ForceInt.x *= s;
                k3ForceInt.y *= s;
                k3ForceInt.z *= s;
                //var intForce : Vector3D = k1Force.add(k2ForceInt).add(k3ForceInt).add(k4Force);
				intForce.x = k1Force.x + k2Force.x + k3Force.x + k4Force.x;
				intForce.y = k1Force.y + k2Force.y + k3Force.y + k4Force.y;
				intForce.z = k1Force.z + k2Force.z + k3Force.z + k4Force.z;
                s = (t / 6 * p.mass);
                intForce.x *= s;
                intForce.y *= s;
                intForce.z *= s;
                p.velocity.x = originalVel.x + intForce.x;
                p.velocity.y = originalVel.y + intForce.y;
                p.velocity.z = originalVel.z + intForce.z;
            }
        }
    }
}

internal class ParticleSystem 
{
    internal var particles : Vector.<Particle>;
    internal var springs : Vector.<Spring>;
    internal var attractors : Vector.<Attraction>;

    private var integrator : Integrator;
    private var gravity : Vector3D;
    private var drag : Number;
    private var restLength : Number = 200;

    public static const RK : String = "RUNGE_KUTTA";
    public static const EULER : String = "MODIFIED_EULER";
    public static const VERLET : String = "VERLET";

    public function ParticleSystem(gravity : Vector3D = null, drag : Number = 0.001) : void 
    {
        particles = new Vector.<Particle>();
        springs = new Vector.<Spring>();
        attractors = new Vector.<Attraction>();
            
        this.integrator = new RKIntegrator(this);
        this.gravity = (gravity) ? gravity : new Vector3D();
        this.drag = drag;
    }


    public final function getIntegrator() : Integrator
    {
        return this.integrator;
    }

    public final function applyIntegrator(t : Number = 1) : void
    {
        integrator.apply(t);
    }

    public final function setGravity(value : Number) : void
    {
        this.gravity.scaleBy(value);
    }

    public final function getGravity() : Vector3D
    {
        return this.gravity;
    }

    public final function setDrag(value : Number) : void
    {
        this.drag = value;
    }

    public final function getDrag() : Number
    {
        return drag;
    }

    public final function makeParticle(mass : Number = 1, position : Vector3D = null) : Particle
    {
        var particle : Particle = new Particle(mass, position);
        particle.isDragging = false;
        particles.push(particle);
        return particle;
    }

    public final function makeSpring(p1 : Particle, p2 : Particle, damping : Number, restLenght : Number, springConst : Number) : Spring
    {
        var spring : Spring = new Spring(p1, p2, damping, restLenght, springConst);
        springs.push(spring);
        return spring;
    }

    public final function makeAttractors(p1 : Particle, p2 : Particle, strength : Number, minDist : Number) : Attraction
    {
        var attraction : Attraction = new Attraction(p1, p2, strength, minDist);
        attractors.push(attraction);
        return attraction;
    }

    public function getParticlesLength() : Number
    {
        return particles.length;
    }

    public function getSpringsLength() : Number
    {
        return springs.length;
    }

    public function getAttractorsLength() : Number
    {
        return attractors.length;
    }

    public final function applyForces() : void
    {
        var i : int;
            
        if (gravity.x != 0 || gravity.y != 0 || gravity.z != 0)
        {
            for (i = 0;i < getParticlesLength();i++)
            {
                var particle : Particle = Particle(particles[i]);
                    
                if (!particle.isDragging) 
                {
                     //particle.force = particle.force.add(gravity);
					particle.force.x += gravity.x;
					particle.force.y += gravity.y;
					particle.force.z += gravity.z;
                } 
                else 
                {
                    particle.force = new Vector3D();
                }
            }
        }
            
        for (i = 0;i < getParticlesLength();i++)
        {
            particle = Particle(particles[i]);
            if (!particle.isDragging) 
            {
//                var vel : Vector3D = particle.velocity.clone();
                vel.x = particle.velocity.x;
                vel.y = particle.velocity.y;
                vel.z = particle.velocity.z;
                s = (-drag);
                vel.x *= s;
                vel.y *= s;
                vel.z *= s;
                particle.force.x += vel.x;
                particle.force.y += vel.y;
                particle.force.z += vel.z;
            }
            else 
            {
                particle.velocity = new Vector3D();
            }
        }
            
        for (i = 0;i < getSpringsLength();i++)
        {
            var spring : Spring = Spring(springs[i]);
            spring.update();
        }
            
        for (i = 0;i < getAttractorsLength();i++)
        {
            var attractor : Attraction = Attraction(attractors[i]);
            attractor.update();
        }
    }

    public final function clear() : void
    {
        var i : int;
        for (i = 0;i <= getParticlesLength();i++)
            particles[i] = null;
            
        for (i = 0;i <= getAttractorsLength();i++)
            attractors[i] = null;
            
        for (i = 0;i <= getSpringsLength();i++)
            springs[i] = null;
            
        particles = new Vector.<Particle>();
        attractors = new Vector.<Attraction>();
        springs = new Vector.<Spring>();
    }

    public final function clearForces() : void
    {
        for (var i : int = i;i < getParticlesLength();i++)
        {
            Particle(particles[i]).force.x = 0;
            Particle(particles[i]).force.y = 0;
            Particle(particles[i]).force.z = 0;
        }
    }

    public final function getParticle(index : Number) : Particle
    {
        return particles[index]; 
    }

    public final function getAttractor(index : Number) : Attraction
    {
        return attractors[index];
    }

    public final function getSpring(index : Number) : Spring
    {
        return springs[index];
    }

    public final function removeParticle(n : int) : void
    {
        particles[n] = null;
        particles.splice(n, 1);
    }

    public final function removeSprings(n : int) : void
    {
        springs[n] = null;
        springs.splice(n, 1);
    }

    public final function removeAttractors(n : int) : void
    {
        attractors[n] = null;
        attractors.splice(n, 1);
    } 

    public final function removeParticleByIndex(p : Particle) : Boolean
    {
        var i : int;
        var n : Number = -1;
        for (i = 0;i <= getParticlesLength();i++)
        {
            if (particles[i] == p)
            {
                n = i;
                break;
            }
        }
        if (n != -1)
        {
            particles[n] = null;
            particles.splice(n, 1);
            return true;
        }
            else return false;
    }

    public final function removeSpringByIndex(s : Spring) : Boolean
    {
        var i : int;
        var n : Number = -1;
        for (i = 0;i <= getParticlesLength();i++)
        {
            if (springs[i] == s)
            {
                n = i;
                break;
            }
        }
        if (n != -1)
        {
            springs[n] = null;
            springs.splice(n, 1);
            return true;
        }
            else return false;
    }

    public final function removeAttractorByIndex(a : Attraction) : Boolean
    {
        var i : int;
        var n : Number = -1;
        for (i = 0;i <= getParticlesLength();i++)
        {
            if (attractors[i] == a)
            {
                n = i;
                break;
            }
        }
        if (n != -1)
        {
            attractors[n] = null;
            attractors.splice(n, 1);
            return true;
        }
            else return false;
    }

    public function constraintSolve() : void
    {
        for (var i : int = 0;i < getParticlesLength() - 1;i++)
        {
            var p1 : Particle = particles[i];
            var p2 : Particle = particles[i + 1];
                
            var dx : Number = p2.position.x - p1.position.x;
            var dy : Number = p2.position.y - p1.position.y;
            var delta : Number = Math.sqrt(dx * dx + dy * dy);
            var diff : Number = restLength - delta;
            var offsetX : Number = (diff * dx / delta) * 0.5;
            var offsetY : Number = (diff * dy / delta) * 0.5;
            p1.position.x -= offsetX;
            p1.position.y -= offsetY;
            p2.position.x += offsetX;
            p2.position.y += offsetY;
        }
    }
}