Oval SymbolicConstraint demo

package;

import symbolic.SymbolicConstraint;

import nape.space.Space;
import nape.geom.Vec2;
import nape.util.ShapeDebug;
import nape.phys.BodyType;
import nape.phys.Body;
import nape.shape.Circle;
import nape.shape.Polygon;
import nape.constraints.PivotJoint;

import flash.events.MouseEvent;
import flash.display.Sprite;

class Main {
    static function main() {
        var stage = flash.Lib.current.stage;

        var space = new Space();
        var debug = new ShapeDebug(1,1);
        stagae.addChild(debug.display);

        var walls = new Body(BodyType.STATIC);
        var w = stage.stageWidth;
        var h = stage.stageHeight;
        walls.shapes.add(new Polygon(Polygon.rect(0,0,-50,h)));
        walls.shapes.add(new Polygon(Polygon.rect(0,0,w,-50)));
        walls.shapes.add(new Polygon(Polygon.rect(w,0,50,h)));
        walls.shapes.add(new Polygon(Polygon.rect(0,h,w,50)));
        walls.space = space;

        var body1 = new Body(); body1.shapes.add(new Circle(20));
        var body2 = new Body(); body2.shapes.add(new Circle(20));
        body1.space = body2.space = space;
        body1.position.setxy(150,150);
        body2.position.setxy(250,150);

        var ovaldefn = "
            body body1, body2
            vector anchor1a, anchor1b, anchor2
            scalar jointMin jointMax

            limit jointMin 0 jointMax

            constraint
                let r1a = relative body1.rotation anchor1a in
                let r1b = relative body1.rotation anchor1b in
                let r2 = relative body2.rotation anchor2 in
                let del = (r2 + body2.position) - body1.position in
                  | del - r1a | + | del - r1b |

            limit constraint jointMin jointMax
        ");
        trace(ovaldefn);

        var oval = new SymbolicConstraint(ovaldefn);
        oval.setVector("anchor1a",new Vec2(40,0));
        oval.setVector("anchor1b",new Vec2(-40,0));
        oval.setVector("anchor2", new Vec2(20,0));
        oval.setScalar("jointMin",100);
        oval.setScalar("jointMax",120);

        oval.space = space;
        oval.setBody("body1",body1);
        oval.setBody("body2",body2);

        space.worldLinearDrag = space.worldAngularDrag = 0.8;

        var hand = new PivotJoint(space.world,null,new Vec2(), new Vec2());
        hand.active = hand.stiff = false;
        hand.frequency = 10;
        hand.space = space;

        stage.addEventListener(MouseEvent.MOUSE_DOWN, function (_) {
            var mp = new Vec2(stage.mouseX,stage.mouseY);
            for(b in space.bodiesUnderPoint(mp)) {
                if(!b.isDynamic()) continue;
                hand.body2 = b;
                hand.anchor2 = b.worldToLocal(mp);
                hand.active = true;
            }
        });
        stage.addEventListener(MouseEvent.MOUSE_UP, function (_) hand.active = false);

        var ellipse = new Sprite();
        stage.addChild(ellipse);

        (new haxe.Timer(0)).run = function() {
            hand.anchor1.setxy(stage.mouseX,stage.mouseY);
            space.step(1/60);
            debug.clear();
            debug.draw(space);

            var p0a = body1.localToWorld(oval.getVector("anchor1a"));
            var p0b = body1.localToWorld(oval.getVector("anchor1b"));
            debug.drawCircle(p0a,2,0xff00ff);
            debug.drawCircle(p0b,2,0xff00ff);
            debug.drawCircle(body2.localToWorld(oval.getVector("anchor2")),2,0xff);

            var centre = p0a.add(p0b).muleq(0.5);
            var f = (p0b.sub(p0a)).length/2;
            var min = oval.getScalar("jointMin"); var h0 = Math.sqrt(0.25*min*min-f*f)*2;
            var max = oval.getScalar("jointMax"); var h1 = Math.sqrt(0.25*max*max-f*f)*2;

            ellipse.rotation = body1.rotation*180/Math.PI+Math.PI/2;
            ellipse.x = centre.x;
            ellipse.y = centre.y;
            var g = ellipse.graphics;
            g.clear();
            g.lineStyle(1,0xff0000,1);
            g.drawEllipse(-min/2,-h0/2,min,h0);
            g.drawEllipse(-max/2,-h1/2,max,h1);
        };
    }
}