Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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();
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement