portal: PortalConstraint

package example.portal
{
    import nape.constraint.UserConstraint;
    import nape.geom.Vec2;
    import nape.geom.Vec3;
    import nape.phys.Body;
    import nape.util.Debug;

    /**
     * ...
     * @author liu wong
     *
     */

    public class PortalConstraint extends UserConstraint
    {
        public var body1:Body;
        public var body2:Body;
        public var portalBody1:Body;
        public var portalBody2:Body;

        /*
            Portal positions+directions defined locally to each portalBody
        */
        public var position1:Vec2;
        public var position2:Vec2;
        public var direction1:Vec2;
        public var direction2:Vec2;

        /*
        Portal scaling from portal1 to portal2
        */
        public var scale:Number;

        private var unit_dir1:Vec2;
        private var unit_dir2:Vec2;

        private var p1:Vec2;
        private var p2:Vec2;
        private var s1:Vec2;
        private var s2:Vec2;
        private var n1:Vec2;
        private var n2:Vec2;
        private var a1:Number;
        private var a2:Number;

        public function PortalConstraint(prtlBody1:Body, pos1:Vec2, dir1:Vec2,
            prtlBody2:Body, pos2:Vec2, dir2:Vec2, scl:Number, bd1:Body, bd2:Body):void
        {
            super(3);

            this.portalBody1 = __registerBody(null, prtlBody1);
            this.portalBody2 = __registerBody(null, prtlBody2);

            position1 = __bindVec2();
            position1.set(pos1);

            position2 = __bindVec2();
            position2.set(pos2);

            direction1 = __bindVec2();
            direction1.set(dir1);

            direction2 = __bindVec2();
            direction2.set(dir2);

            __invalidate();

            this.scale = scl;

            this.body1 = __registerBody(null,bd1);
            this.body2 = __registerBody(null, bd2);

            //init Vec2's that are reused across methods
            unit_dir1 = Vec2.get();
            unit_dir2 = Vec2.get();

            p1 = Vec2.get();
            p2 = Vec2.get();
            s1 = Vec2.get();
            s2 = Vec2.get();
            n1 = Vec2.get();
            n2 = Vec2.get();
        }

        override public function __copy():UserConstraint
        {
            return new PortalConstraint(portalBody1, position1, direction1, portalBody2, position2, direction2, scale, body1, body2);
        }

        override public function __validate():void
        {
            unit_dir1.set(direction1.mul(1/direction1.length, true));
            unit_dir2.set(direction2.mul(1/direction2.length, true));
        }

        override public function __prepare():void
        {
            p1.set(portalBody1.localVectorToWorld(position1, true));
            p2.set(portalBody2.localVectorToWorld(position2, true));

            s1.set(body1.position.sub(p1,true).subeq(portalBody1.position));
            s2.set(body2.position.sub(p2,true).subeq(portalBody2.position));

            n1.set(portalBody1.localVectorToWorld(unit_dir1, true));
            n2.set(portalBody2.localVectorToWorld(unit_dir2, true));

            a1 = unit_dir1.angle + portalBody1.rotation;
            a2 = unit_dir2.angle + portalBody2.rotation;
        }

        public function setProperties(clone:Body, original:Body):void
        {
            //ensure our required pre-calced values are correct.
            __validate();
            __prepare();

            //compute velocity error so we can set velocity correctly.
            var v:Vector.<Number> = new Vector.<Number>();
            __velocity(v);

            //modify clone so that position and velocity errors are 0
            if (clone == body2) {
                clone.position = portalBody2.position.add(p2,true);
                clone.position.x -= (n2.x * s1.dot(n1) + n2.y * s1.cross(n1))*scale;
                clone.position.y -= (n2.y * s1.dot(n1) - n2.x * s1.cross(n1))*scale;
                clone.rotation = -Math.PI + original.rotation - a1 + a2;

                clone.velocity.x -= n2.x * v[0] + n2.y * v[1];
                clone.velocity.y -= n2.y * v[0] - n2.x * v[1];
                clone.angularVel += v[2];
            }
            else {
                clone.position = portalBody1.position.add(p1, true);
                clone.position.x -= (n1.x * s2.dot(n2) + n1.y * s2.cross(n2))/scale;
                clone.position.y -= (n1.y * s2.dot(n2) - n1.x * s2.cross(n2))/scale;
                clone.rotation = Math.PI + original.rotation - a2 + a1;

                clone.velocity.x += (n1.x * v[0] + n1.y * v[1]) / scale;
                clone.velocity.y += (n1.y * v[0] - n1.x * v[1]) / scale;
                clone.angularVel += v[2];
            }
        }

        override public function __position(err:Vector.<Number>):void
        {
            err[0] = scale*s1.dot(n1) + s2.dot(n2);
            err[1] = scale*s1.cross(n1) + s2.cross(n2);
            err[2] = (body1.rotation - a1) - (body2.rotation - a2) - Math.PI;
        }

        override public function __velocity(err:Vector.<Number>):void
        {
            var v1:Vec3 = body1.constraintVelocity;
            var v2:Vec3 = body2.constraintVelocity;
            var pv1:Vec3 = portalBody1.constraintVelocity;
            var pv2:Vec3 = portalBody2.constraintVelocity;

            var u1:Vec2 = v1.xy().subeq(p1.perp(true).muleq(pv1.z)).subeq(pv1.xy(true));
            var u2:Vec2 = v2.xy().subeq(p2.perp(true).muleq(pv2.z)).subeq(pv2.xy(true));

            err[0] = scale*(u1.dot(n1) + pv1.z*s1.cross(n1))
                         + (u2.dot(n2) + pv2.z*s2.cross(n2));
            err[1] = scale*(u1.cross(n1) + pv1.z*s1.dot(n1))
                         + (u2.cross(n2) + pv2.z*s2.dot(n2));
            err[2] = (v1.z - pv1.z) - (v2.z - pv2.z);

            u1.dispose();
            u2.dispose();
        }

        override public function __eff_mass(eff:Vector.<Number>):void
        {
            eff[0]=eff[3]=body1.constraintMass*scale*scale + body2.constraintMass;
            eff[1]=eff[2]=eff[4] = 0.0;
            eff[5]= body1.constraintInertia + body2.constraintInertia;
        }

        override public function __impulse(imp:Vector.<Number>, body:Body, out:Vec3):void
        {
            if (body == portalBody1 || body == portalBody2)
                out.setxyz(0,0,0);
            else
            {
                var sc1:Number;
                var sc2:Number;
                var norm:Vec2;
                if (body == body1)
                {
                    sc1 = scale;
                    sc2 =  1.0;
                    norm = n1;
                }
                else
                {
                    sc1 = 1.0;
                    sc2 = -1.0;
                    norm = n2;
                }
                out.x = sc1*(norm.x*imp[0] + norm.y*imp[1]);
                out.y = sc1*(norm.y*imp[0] - norm.x*imp[1]);
                out.z = sc2*imp[2];
            }
        }

        override public function __draw(debug:Debug):void
        {
            __validate();
            var p1:Vec2 = portalBody1.localPointToWorld(position1);
            var p2:Vec2 = portalBody2.localPointToWorld(position2);

            debug.drawCircle(p1,2,0xff);
            debug.drawCircle(p2,2,0xff0000);
            debug.drawLine(p1,p1.add(portalBody1.localVectorToWorld(unit_dir1,true).muleq(20),true),0xff);
            debug.drawLine(p2,p2.add(portalBody2.localVectorToWorld(unit_dir2,true).muleq(20),true),0xff0000);
            debug.drawLine(p1,body1.position,0xffff);
            debug.drawLine(p2,body2.position,0xff00ff);

            p1.dispose();
            p2.dispose();
        }
    }

}