Box2d Lite in ActionScript 3
Box2 Lite in ActionScript 3.
(ported by nitoyon)
"Box2D Lite" is first release of Box2d.
Much simpler than current version of Box2d.
How to Use:
-------------------------------------------------------------
1～9: Change demo
Space: Launch bomb
-------------------------------------------------------------
Original Code Copyright:
-------------------------------------------------------------
Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
Permission to use, copy, modify, distribute and sell this software
and its documentation for any purpose is hereby granted without fee,
provided that the above copyright notice appear in all copies.
Erin Catto makes no representations about the suitability
of this software for any purpose.
It is provided "as is" without express or implied warranty.
/**
* Copyright nitoyon ( http://wonderfl.net/user/nitoyon )
* MIT License ( http://www.opensource.org/licenses/mit-license.php )
* Downloaded from: http://wonderfl.net/c/w1f7
*/
/**
* Box2 Lite in ActionScript 3.
* (ported by nitoyon)
*
* "Box2D Lite" is first release of Box2d.
* Much simpler than current version of Box2d.
*
* How to Use:
*-------------------------------------------------------------
* 1～9: Change demo
* Space: Launch bomb
*-------------------------------------------------------------
*
* Original Code Copyright:
*-------------------------------------------------------------
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* Permission to use, copy, modify, distribute and sell this software
* and its documentation for any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies.
* Erin Catto makes no representations about the suitability
* of this software for any purpose.
* It is provided "as is" without express or implied warranty.
*/
package{
import flash.display.*;
import flash.events.*;
import flash.ui.Keyboard;
[SWF(frameRate="30")]
public class Box2dLite extends Sprite{
private var bodies:Vector.<Body>;
private var joints:Vector.<Joint>;
private var bomb:Body;
private var timeStep:Number = 1.0 / 30.0;
private var iterations:int = 10;
private var gravity:Vec2 = new Vec2(0.0, -10.0);
private var demoIndex:int = 0;
private var world:World = new World(gravity, iterations);
private var demos:Array = [demo1, demo2, demo3, demo4, demo5, demo6, demo7, demo8, demo9];
public static const W:Number = 200;
public static const H:Number = 300;
public static const SCALE:Number = 20;
public function Box2dLite(){
stage.scaleMode = "noScale";
stage.align = "TL";
GraphicsCache.g = graphics;
initDemo(2);
simulationLoop();
addEventListener("enterFrame", function(event:*):void { simulationLoop(); });
stage.addEventListener("keyDown", function(event:KeyboardEvent):void{
if (event.charCode >= 49 && event.charCode - 49 < demos.length){
initDemo(event.charCode - 49);
} else if (event.keyCode == Keyboard.RIGHT) {
initDemo((demoIndex + 1) % demos.length);
} else if (event.keyCode == Keyboard.LEFT) {
initDemo((demoIndex - 1 + demos.length) % demos.length);
} else if (event.charCode == 32){
launchBomb();
}
});
}
private function initDemo(index:int):void{
world.clear();
bomb = null;
demoIndex = index;
demos[index]();
}
private function demo1():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b:Body = new Body();
b.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b.position.set(0.0, -0.5 * b.width.y);
bodies.push(b);
world.addBody(b);
b = new Body();
b.set(new Vec2(1.0, 1.0), 200.0);
b.position.set(0.0, 4.0);
bodies.push(b);
world.addBody(b);
}
private function demo2():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b1:Body = new Body();
b1.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b1.friction = 0.2;
b1.position.set(0.0, -0.5 * b1.width.y);
b1.rotation = 0.0;
bodies.push(b1);
world.addBody(b1);
var b2:Body = new Body();
b2.set(new Vec2(1.0, 1.0), 100.0);
b2.friction = 0.2;
b2.position.set(9.0, 11.0);
b2.rotation = 0.0;
bodies.push(b2);
world.addBody(b2);
var j:Joint = new Joint();
j.set(b1, b2, new Vec2(0.0, 11.0));
joints.push(j);
world.addJoint(j);
}
private function demo3():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b:Body = new Body();
b.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b.position.set(0.0, -0.5 * b.width.y);
bodies.push(b);
world.addBody(b);
b = new Body();
b.set(new Vec2(13.0, 0.25), Number.MAX_VALUE);
b.position.set(-2.0, 11.0);
b.rotation = -0.25;
bodies.push(b);
world.addBody(b);
b = new Body();
b.set(new Vec2(0.25, 1.0), Number.MAX_VALUE);
b.position.set(5.25, 9.5);
bodies.push(b);
world.addBody(b);
b = new Body();
b.set(new Vec2(13.0, 0.25), Number.MAX_VALUE);
b.position.set(2.0, 7.0);
b.rotation = 0.25;
bodies.push(b);
world.addBody(b);
b = new Body();
b.set(new Vec2(0.25, 1.0), Number.MAX_VALUE);
b.position.set(-5.25, 5.5);
bodies.push(b);
world.addBody(b);
b = new Body();
b.set(new Vec2(13.0, 0.25), Number.MAX_VALUE);
b.position.set(-2.0, 3.0);
b.rotation = -0.25;
bodies.push(b);
world.addBody(b);
var friction:Array = [0.75, 0.5, 0.35, 0.1, 0.0];
for (var i:int = 0; i < 5; ++i){
b = new Body();
b.set(new Vec2(0.5, 0.5), 25.0);
b.friction = friction[i];
b.position.set(-7.5 + 2.0 * i, 14.0);
bodies.push(b);
world.addBody(b);
}
}
// A vertical stack
private function demo4():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b:Body = new Body();
b.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b.friction = 0.2;
b.position.set(0.0, -0.5 * b.width.y);
b.rotation = 0.0;
bodies.push(b);
world.addBody(b);
for (var i:int = 0; i < 10; ++i){
b = new Body();
b.set(new Vec2(1.0, 1.0), 1.0);
b.friction = 0.2;
var x:Number = Math.random() * 0.2 - 0.1;
b.position.set(x, 0.51 + 1.05 * i);
bodies.push(b);
world.addBody(b);
}
}
// A pyramid
private function demo5():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b:Body = new Body();
b.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b.friction = 0.2;
b.position.set(0.0, -0.5 * b.width.y);
b.rotation = 0.0;
bodies.push(b);
world.addBody(b);
var x:Vec2 = new Vec2(-6.0, 0.75);
var y:Vec2 = new Vec2();
for (var i:int = 0; i < 12; ++i){
y = x;
for (var j:int = i; j < 12; ++j){
b = new Body();
b.set(new Vec2(1.0, 1.0), 10.0);
b.friction = 0.2;
b.position = y;
bodies.push(b);
world.addBody(b);
y = y.add(new Vec2(1.125, 0.0));
}
//x += Vec2(0.5625, 1.125);
x = x.add(new Vec2(0.5625, 2.0));
}
}
// A teeter
private function demo6():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b1:Body = new Body();
b1.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b1.position.set(0.0, -0.5 * b1.width.y);
bodies.push(b1);
world.addBody(b1);
var b2:Body = new Body();
b2.set(new Vec2(12.0, 0.25), 100.0);
b2.position.set(0.0, 1.0);
bodies.push(b2);
world.addBody(b2);
var b3:Body = new Body();
b3.set(new Vec2(0.5, 0.5), 25.0);
b3.position.set(-5.0, 2.0);
bodies.push(b3);
world.addBody(b3);
var b4:Body = new Body();
b4.set(new Vec2(0.5, 0.5), 25.0);
b4.position.set(-5.5, 2.0);
bodies.push(b4);
world.addBody(b4);
var b5:Body = new Body();
b5.set(new Vec2(1.0, 1.0), 100.0);
b5.position.set(5.5, 15.0);
bodies.push(b5);
world.addBody(b5);
var j:Joint = new Joint();
j.set(b1, b2, new Vec2(0.0, 1.0));
joints.push(j);
world.addJoint(j);
}
// A suspension bridge
private function demo7():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b:Body = new Body();
b.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b.friction = 0.2;
b.position.set(0.0, -0.5 * b.width.y);
b.rotation = 0.0;
bodies.push(b);
world.addBody(b);
var numPlanks:int = 15;
var mass:Number = 50.0;
for (var i:int = 0; i < numPlanks; ++i){
b = new Body();
b.set(new Vec2(1.0, 0.25), mass);
b.friction = 0.2;
b.position.set(-8.5 + 1.25 * i, 5.0);
bodies.push(b);
world.addBody(b);
}
// Tuning
var frequencyHz:Number = 2.0;
var dampingRatio:Number = 0.7;
// frequency in radians
var omega:Number = 2.0 * Math.PI * frequencyHz;
// damping coefficient
var d:Number = 2.0 * mass * dampingRatio * omega;
// spring stifness
var k:Number = mass * omega * omega;
// magic formulas
var softness:Number = 1.0 / (d + timeStep * k);
var biasFactor:Number = timeStep * k / (d + timeStep * k);
for (i = 0; i < numPlanks; ++i){
var j:Joint = new Joint();
j.set(bodies[i], bodies[i+1], new Vec2(-9.125 + 1.25 * i, 5.0));
j.softness = softness;
j.biasFactor = biasFactor;
joints.push(j);
world.addJoint(j);
}
j = new Joint();
j.set(bodies[numPlanks], bodies[0], new Vec2(-9.125 + 1.25 * numPlanks, 5.0));
j.softness = softness;
j.biasFactor = biasFactor;
joints.push(j);
world.addJoint(j);
}
// Dominos
private function demo8():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b1:Body = new Body();
var b:Body = b1;
b.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b.position.set(0.0, -0.5 * b.width.y);
bodies.push(b);
world.addBody(b);
b = new Body();
b.set(new Vec2(12.0, 0.5), Number.MAX_VALUE);
b.position.set(-1.5, 10.0);
bodies.push(b);
world.addBody(b);
for (var i:int = 0; i < 10; ++i){
b = new Body();
b.set(new Vec2(0.2, 2.0), 10.0);
b.position.set(-6.0 + 1.0 * i, 11.125);
b.friction = 0.1;
bodies.push(b);
world.addBody(b);
}
b = new Body();
b.set(new Vec2(14.0, 0.5), Number.MAX_VALUE);
b.position.set(1.0, 6.0);
b.rotation = 0.3;
bodies.push(b);
world.addBody(b);
var b2:Body = b = new Body();
b.set(new Vec2(0.5, 3.0), Number.MAX_VALUE);
b.position.set(-7.0, 4.0);
bodies.push(b);
world.addBody(b);
var b3:Body = b = new Body();
b.set(new Vec2(12.0, 0.25), 20.0);
b.position.set(-0.9, 1.0);
bodies.push(b);
world.addBody(b);
var j:Joint = new Joint();
j.set(b1, b3, new Vec2(-2.0, 1.0));
joints.push(j);
world.addJoint(j);
var b4:Body = b = new Body();
b.set(new Vec2(0.5, 0.5), 10.0);
b.position.set(-10.0, 15.0);
bodies.push(b);
world.addBody(b);
j = new Joint();
j.set(b2, b4, new Vec2(-7.0, 15.0));
joints.push(j);
world.addJoint(j);
var b5:Body = b = new Body();
b.set(new Vec2(2.0, 2.0), 20.0);
b.position.set(6.0, 2.5);
b.friction = 0.1;
bodies.push(b);
world.addBody(b);
j = new Joint();
j.set(b1, b5, new Vec2(6.0, 2.6));
joints.push(j);
world.addJoint(j);
var b6:Body = b = new Body();
b.set(new Vec2(2.0, 0.2), 10.0);
b.position.set(6.0, 3.6);
bodies.push(b);
world.addBody(b);
j = new Joint();
j.set(b5, b6, new Vec2(7.0, 3.5));
joints.push(j);
world.addJoint(j);
}
// A multi-pendulum
private function demo9():void{
bodies = new Vector.<Body>();
joints = new Vector.<Joint>();
var b:Body = new Body();
b.set(new Vec2(100.0, 20.0), Number.MAX_VALUE);
b.friction = 0.2;
b.position.set(0.0, -0.5 * b.width.y);
b.rotation = 0.0;
bodies.push(b);
world.addBody(b);
var b1:Body = b;
var mass:Number = 10.0;
// Tuning
var frequencyHz:Number = 4.0;
var dampingRatio:Number = 0.7;
// frequency in radians
var omega:Number = 2.0 * Math.PI * frequencyHz;
// damping coefficient
var d:Number = 2.0 * mass * dampingRatio * omega;
// spring stiffness
var k:Number = mass * omega * omega;
// magic formulas
var softness:Number = 1.0 / (d + timeStep * k);
var biasFactor:Number = timeStep * k / (d + timeStep * k);
var y:Number = 12.0;
for (var i:int = 0; i < 15; ++i){
var x:Vec2 = new Vec2(0.5 + i, y);
b = new Body();
b.set(new Vec2(0.75, 0.25), mass);
b.friction = 0.2;
b.position = x;
b.rotation = 0.0;
bodies.push(b);
world.addBody(b);
var j:Joint = new Joint();
j.set(b1, b, new Vec2(i, y));
j.softness = softness;
j.biasFactor = biasFactor;
joints.push(j);
world.addJoint(j);
b1 = b;
}
}
private function simulationLoop():void{
graphics.clear();
world.step(timeStep);
for each (var b:Body in bodies)
drawBody(b);
for each (var joint:Joint in joints)
drawJoint(joint);
}
private function drawBody(body:Body):void{
var R:Mat22 = Mat22.createRotate(body.rotation);
var x:Vec2 = body.position;
var h:Vec2 = body.width.mul(0.5);
var v1:Vec2 = x.add(R.mul(new Vec2(-h.x, -h.y)));
var v2:Vec2 = x.add(R.mul(new Vec2( h.x, -h.y)));
var v3:Vec2 = x.add(R.mul(new Vec2( h.x, h.y)));
var v4:Vec2 = x.add(R.mul(new Vec2(-h.x, h.y)));
if (body == bomb)
graphics.lineStyle(0, 0xcc0000);
else
graphics.lineStyle(0, 0x000080);
graphics.moveTo(v1.x * SCALE + W, -v1.y * SCALE + H);
graphics.lineTo(v2.x * SCALE + W, -v2.y * SCALE + H);
graphics.lineTo(v3.x * SCALE + W, -v3.y * SCALE + H);
graphics.lineTo(v4.x * SCALE + W, -v4.y * SCALE + H);
graphics.lineTo(v1.x * SCALE + W, -v1.y * SCALE + H);
graphics.lineStyle();
}
public function drawJoint(joint:Joint):void{
var b1:Body = joint.body1;
var b2:Body = joint.body2;
var R1:Mat22 = Mat22.createRotate(b1.rotation);
var R2:Mat22 = Mat22.createRotate(b2.rotation);
var x1:Vec2 = b1.position;
var p1:Vec2 = x1.add(R1.mul(joint.localAnchor1));
var x2:Vec2 = b2.position;
var p2:Vec2 = x2.add(R2.mul(joint.localAnchor2));
graphics.lineStyle(0, 0x66ee66);
graphics.moveTo(x1.x * SCALE + W, -x1.y * SCALE + H);
graphics.lineTo(p1.x * SCALE + W, -p1.y * SCALE + H);
graphics.lineTo(x2.x * SCALE + W, -x2.y * SCALE + H);
graphics.lineTo(p2.x * SCALE + W, -p2.y * SCALE + H);
graphics.lineStyle();
}
private function launchBomb():void{
if (!bomb){
bomb = new Body();
bomb.set(new Vec2(1.0, 1.0), 50.0);
bomb.friction = 0.2;
bodies.push(bomb);
world.addBody(bomb);
}
bomb.position.set(Math.random() * 30 - 15, 15.0);
bomb.rotation = Math.random() * 3.0 - 1.5;
bomb.velocity = bomb.position.mul(-1.5);
bomb.angularVelocity = Math.random() * 40.0 - 20.0;
}
}
}
import flash.display.Graphics;
import flash.utils.Dictionary;
class GraphicsCache{
private static var _g:Graphics;
public static function set g(g:Graphics):void{ _g = g; }
public static function get g():Graphics { return _g; }
}
/*******************************************************************************
*
* MathUtils.h
*
*******************************************************************************/
class Vec2{
public var x:Number;
public var y:Number;
public function Vec2(x:Number = 0, y:Number = 0){
this.x = x;
this.y = y;
}
public function set(_x:Number, _y:Number):void { x = _x; y = _y; }
public function negative():Vec2 { return new Vec2(-x, -y); }
public function add(v:Vec2):Vec2{
return new Vec2(x + v.x, y + v.y);
}
public function sub(v:Vec2):Vec2{
return new Vec2(x - v.x, y - v.y);
}
public function mul(a:Number):Vec2{
return new Vec2(x * a, y * a);
}
public function abs():Vec2{
return new Vec2(Math.abs(x), Math.abs(y));
}
public function dot(v:Vec2):Number{
return x * v.x + y * v.y;
}
public function length():Number{
return Math.sqrt(x * x + y * y);
}
public function clone():Vec2 { return new Vec2(x, y); }
public function toString():String{
return "Vec2(" + x + ", " + y + ")";
}
}
class Mat22{
public var col1:Vec2;
public var col2:Vec2;
public function Mat22(col1:Vec2 = null, col2:Vec2 = null){
this.col1 = col1;
this.col2 = col2;
if (!this.col1) { this.col1 = new Vec2(); }
if (!this.col2) { this.col2 = new Vec2(); }
}
public static function createRotate(angle:Number):Mat22{
var c:Number = Math.cos(angle), s:Number = Math.sin(angle);
var ret:Mat22 = new Mat22();
ret.col1.x = c; ret.col2.x = -s;
ret.col1.y = s; ret.col2.y = c;
return ret;
}
public function transpose():Mat22{
return new Mat22(new Vec2(col1.x, col2.x), new Vec2(col1.y, col2.y));
}
public function invert():Mat22{
var a:Number = col1.x, b:Number = col2.x, c:Number = col1.y, d:Number = col2.y;
var det:Number = a * d - b * c;
if (det == 0) throw new Error("det is 0");
det = 1.0 / det;
var B:Mat22 = new Mat22();
B.col1.x = det * d; B.col2.x = -det * b;
B.col1.y = -det * c; B.col2.y = det * a;
return B;
}
public function add(m:Mat22):Mat22{
return new Mat22(new Vec2(col1.x + m.col1.x, col2.x + m.col2.x),
new Vec2(col1.y + m.col1.y, col2.y + m.col2.y));
}
public function mul(v:Vec2):Vec2{
return new Vec2(col1.x * v.x + col2.x * v.y, col1.y * v.x + col2.y * v.y);
}
public function mulMat22(m:Mat22):Mat22{
return new Mat22(mul(m.col1), mul(m.col2));
}
public function abs():Mat22{
return new Mat22(col1.abs(), col2.abs());
}
public function toString():String{
return "Mat22(" + col1.toString() + ", " + col2.toString() + ")";
}
}
class MathUtils{
public static function dot(a:Vec2, b:Vec2):Number{
return a.x * b.x + a.y * b.y;
}
public static function crossVV(a:Vec2, b:Vec2):Number{
return a.x * b.y - a.y * b.x;
}
public static function crossVN(a:Vec2, s:Number):Vec2{
return new Vec2(s * a.y, -s * a.x);
}
public static function crossNV(s:Number, a:Vec2):Vec2{
return new Vec2(-s * a.y, s * a.x);
}
public static function mulNV(s:Number, v:Vec2):Vec2{
return new Vec2(s * v.x, s * v.y);
}
public static function clamp(a:Number, low:Number, high:Number):Number{
return Math.max(low, Math.min(a, high));
}
}
/*******************************************************************************
*
* Body.cpp, Body.h
*
*******************************************************************************/
class Body{
public var position:Vec2;
public var rotation:Number;
public var velocity:Vec2;
public var angularVelocity:Number;
public var force:Vec2;
public var torque:Number;
public var width:Vec2;
public var friction:Number;
public var mass:Number, invMass:Number;
public var I:Number, invI:Number;
private var _id:int;
private static var idCounter:int = 0;
public function get id():int { return _id; }
public function Body(){
position = new Vec2(0.0, 0.0);
rotation = 0.0;
velocity = new Vec2(0.0, 0.0);
angularVelocity = 0.0;
force = new Vec2(0.0, 0.0);
torque = 0.0;
friction = 0.2;
width = new Vec2(1.0, 1.0);
mass = Number.MAX_VALUE;
invMass = 0.0;
I = Number.MAX_VALUE;
invI = 0.0;
_id = ++idCounter;
}
public function set(w:Vec2, m:Number):void{
position.set(0.0, 0.0);
rotation = 0.0;
velocity.set(0.0, 0.0);
angularVelocity = 0.0;
force.set(0.0, 0.0);
torque = 0.0;
friction = 0.2;
width = w;
mass = m;
if (mass < Number.MAX_VALUE){
invMass = 1.0 / mass;
I = mass * (width.x * width.x + width.y * width.y) / 12.0;
invI = 1.0 / I;
}else{
invMass = 0.0;
I = Number.MAX_VALUE;
invI = 0.0;
}
}
}
/*******************************************************************************
*
* Joint.cpp, Joint.h
*
*******************************************************************************/
class Joint {
public var M:Mat22;
public var localAnchor1:Vec2 = new Vec2(), localAnchor2:Vec2 = new Vec2();
public var r1:Vec2 = new Vec2(), r2:Vec2 = new Vec2();
public var bias:Vec2 = new Vec2();
public var P:Vec2 = new Vec2(); // accumulated impulse
public var body1:Body = new Body();
public var body2:Body = new Body();
public var biasFactor:Number = 0;
public var softness:Number = 0;
public function set(b1:Body, b2:Body, anchor:Vec2):void{
body1 = b1;
body2 = b2;
var Rot1:Mat22 = Mat22.createRotate(body1.rotation);
var Rot2:Mat22 = Mat22.createRotate(body2.rotation);
var Rot1T:Mat22 = Rot1.transpose();
var Rot2T:Mat22 = Rot2.transpose();
localAnchor1 = Rot1T.mul(anchor.sub(body1.position));
localAnchor2 = Rot2T.mul(anchor.sub(body2.position));
P.set(0.0, 0.0);
softness = 0.0;
biasFactor = 0.2;
}
public function preStep(inv_dt:Number):void{
// Pre-compute anchors, mass matrix, and bias.
var Rot1:Mat22 = Mat22.createRotate(body1.rotation);
var Rot2:Mat22 = Mat22.createRotate(body2.rotation);
r1 = Rot1.mul(localAnchor1);
r2 = Rot2.mul(localAnchor2);
// deltaV = deltaV0 + K * impulse
// invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
var K1:Mat22 = new Mat22();
K1.col1.x = body1.invMass + body2.invMass; K1.col2.x = 0.0;
K1.col1.y = 0.0; K1.col2.y = body1.invMass + body2.invMass;
var K2:Mat22 = new Mat22();
K2.col1.x = body1.invI * r1.y * r1.y; K2.col2.x = -body1.invI * r1.x * r1.y;
K2.col1.y = -body1.invI * r1.x * r1.y; K2.col2.y = body1.invI * r1.x * r1.x;
var K3:Mat22 = new Mat22();
K3.col1.x = body2.invI * r2.y * r2.y; K3.col2.x = -body2.invI * r2.x * r2.y;
K3.col1.y = -body2.invI * r2.x * r2.y; K3.col2.y = body2.invI * r2.x * r2.x;
var K:Mat22 = K1.add(K2).add(K3);
K.col1.x += softness;
K.col2.y += softness;
M = K.invert();
var p1:Vec2 = body1.position.add(r1);
var p2:Vec2 = body2.position.add(r2);
var dp:Vec2 = p2.sub(p1);
if (World.positionCorrection){
bias = dp.mul(-biasFactor * inv_dt);
}else{
bias.set(0.0, 0.0);
}
if (World.warmStarting){
// Apply accumulated impulse.
body1.velocity = body1.velocity.sub(P.mul(body1.invMass));
body1.angularVelocity -= body1.invI * MathUtils.crossVV(r1, P);
body2.velocity = body2.velocity.add(P.mul(body2.invMass));
body2.angularVelocity += body2.invI * MathUtils.crossVV(r2, P);
}else{
P.set(0.0, 0.0);
}
}
public function applyImpulse():void{
var dv:Vec2 = body2.velocity.add(MathUtils.crossNV(body2.angularVelocity, r2)).sub(body1.velocity).sub(MathUtils.crossNV(body1.angularVelocity, r1));
var impulse:Vec2 = M.mul(bias.sub(dv).sub(P.mul(softness)));
body1.velocity = body1.velocity.sub(impulse.mul(body1.invMass));
body1.angularVelocity -= body1.invI * MathUtils.crossVV(r1, impulse);
body2.velocity = body2.velocity.add(impulse.mul(body2.invMass));
body2.angularVelocity += body2.invI * MathUtils.crossVV(r2, impulse);
P = P.add(impulse);
}
}
/*******************************************************************************
*
* Arbiter.cpp, Arbiter.h
*
*******************************************************************************/
class FeaturePair{
public var inEdge1:uint;
public var outEdge1:uint;
public var inEdge2:uint;
public var outEdge2:uint;
public function get value():String{
return inEdge1 + ":" + outEdge1 + ":" + inEdge2 + ":" + outEdge2;
}
};
class Contact{
public var position:Vec2;
public var normal:Vec2;
public var r1:Vec2, r2:Vec2;
public var separation:Number;
public var Pn:Number; // accumulated normal impulse
public var Pt:Number; // accumulated tangent impulse
public var Pnb:Number; // accumulated normal impulse for position bias
public var massNormal:Number, massTangent:Number;
public var bias:Number;
public var feature:FeaturePair;
public function Contact(){
Pn = Pt = Pnb = 0.0;
feature = new FeaturePair();
}
}
class ArbiterKey{
public var body1:Body;
public var body2:Body;
public function ArbiterKey(b1:Body, b2:Body){
if (b1.id < b2.id){
body1 = b1; body2 = b2;
}else{
body1 = b2; body2 = b1;
}
}
public function get id():String{
return body1.id + "," + body2.id;
}
}
class Arbiter{
private static const MAX_POINTS:int = 2;
public var contacts:Vector.<Contact>;
public var numContacts:int;
public var body1:Body;
public var body2:Body;
// Combined friction
public var friction:Number;
public function Arbiter(b1:Body, b2:Body){
if (b1.id < b2.id){
body1 = b1; body2 = b2;
}else{
body1 = b2; body2 = b1;
}
contacts = Collide.collide(body1, body2);
numContacts = contacts.length;
friction = Math.sqrt(body1.friction * body2.friction);
GraphicsCache.g.beginFill(0xff0000);
for each (var contact:Contact in contacts){
GraphicsCache.g.drawCircle(contact.position.x * Box2dLite.SCALE + Box2dLite.W, -contact.position.y * Box2dLite.SCALE + Box2dLite.H, 1);
}
GraphicsCache.g.endFill();
}
public function update(newContacts:Vector.<Contact>, numNewContacts:int):void{
var mergedContacts:Vector.<Contact> = Vector.<Contact>([new Contact(), new Contact()]);
for (var i:int = 0; i < numNewContacts; ++i){
var cNew:Contact = mergedContacts[i];
var k:int = -1;
for (var j:int = 0; j < numContacts; ++j){
var cOld:Contact = contacts[j];
if (cNew.feature.value == cOld.feature.value){
k = j;
break;
}
}
if (k > -1){
var c:Contact = mergedContacts[i];
cOld = contacts[k];
c = cNew;
if (World.warmStarting){
c.Pn = cOld.Pn;
c.Pt = cOld.Pt;
c.Pnb = cOld.Pnb;
}else{
c.Pn = 0.0;
c.Pt = 0.0;
c.Pnb = 0.0;
}
}else{
mergedContacts[i] = newContacts[i];
}
}
for (i = 0; i < numNewContacts; ++i)
contacts[i] = mergedContacts[i];
numContacts = numNewContacts;
}
public function preStep(inv_dt:Number):void{
var k_allowedPenetration:Number = 0.01;
var k_biasFactor:Number = World.positionCorrection ? 0.2 : 0.0;
for each (var c:Contact in contacts){
var r1:Vec2 = c.position.sub(body1.position);
var r2:Vec2 = c.position.sub(body2.position);
// Precompute normal mass, tangent mass, and bias.
var rn1:Number = MathUtils.dot(r1, c.normal);
var rn2:Number = MathUtils.dot(r2, c.normal);
var kNormal:Number = body1.invMass + body2.invMass;
kNormal += body1.invI * (MathUtils.dot(r1, r1) - rn1 * rn1) + body2.invI * (MathUtils.dot(r2, r2) - rn2 * rn2);
c.massNormal = 1.0 / kNormal;
var tangent:Vec2 = MathUtils.crossVN(c.normal, 1.0);
var rt1:Number = MathUtils.dot(r1, tangent);
var rt2:Number = MathUtils.dot(r2, tangent);
var kTangent:Number = body1.invMass + body2.invMass;
kTangent += body1.invI * (r1.dot(r1) - rt1 * rt1) + body2.invI * (r2.dot(r2) - rt2 * rt2);
c.massTangent = 1.0 / kTangent;
c.bias = -k_biasFactor * inv_dt * Math.min(0.0, c.separation + k_allowedPenetration);
if (World.accumulateImpulses){
// Apply normal + friction impulse
var P:Vec2 = c.normal.mul(c.Pn).add(tangent.mul(c.Pt));
body1.velocity = body1.velocity.sub(P.mul(body1.invMass));
body1.angularVelocity -= body1.invI * MathUtils.crossVV(r1, P);
body2.velocity = body2.velocity.add(P.mul(body2.invMass));
body2.angularVelocity += body2.invI * MathUtils.crossVV(r2, P);
}
}
}
public function applyImpulse():void{
var b1:Body = body1;
var b2:Body = body2;
for each (var c:Contact in contacts){
c.r1 = c.position.sub(b1.position);
c.r2 = c.position.sub(b2.position);
// Relative velocity at contact
var dv:Vec2 = b2.velocity.add(MathUtils.crossNV(b2.angularVelocity, c.r2)).sub(b1.velocity).sub(MathUtils.crossNV(b1.angularVelocity, c.r1));
// Compute normal impulse
var vn:Number = MathUtils.dot(dv, c.normal);
var dPn:Number = c.massNormal * (-vn + c.bias);
if (World.accumulateImpulses){
// Clamp the accumulated impulse
var Pn0:Number = c.Pn;
c.Pn = Math.max(Pn0 + dPn, 0.0);
dPn = c.Pn - Pn0;
}else{
dPn = Math.max(dPn, 0.0);
}
// Apply contact impulse
var Pn:Vec2 = c.normal.mul(dPn);
b1.velocity = b1.velocity.sub(Pn.mul(b1.invMass));
b1.angularVelocity -= b1.invI * MathUtils.crossVV(c.r1, Pn);
b2.velocity = b2.velocity.add(Pn.mul(b2.invMass));
b2.angularVelocity += b2.invI * MathUtils.crossVV(c.r2, Pn);
// Relative velocity at contact
dv = b2.velocity.add(MathUtils.crossNV(b2.angularVelocity, c.r2)).sub(b1.velocity).sub(MathUtils.crossNV(b1.angularVelocity, c.r1));
var tangent:Vec2 = MathUtils.crossVN(c.normal, 1.0);
var vt:Number = MathUtils.dot(dv, tangent);
var dPt:Number = c.massTangent * (-vt);
if (World.accumulateImpulses){
// Compute friction impulse
var maxPt:Number = friction * c.Pn;
// Clamp friction
var oldTangentImpulse:Number = c.Pt;
c.Pt = MathUtils.clamp(oldTangentImpulse + dPt, -maxPt, maxPt);
dPt = c.Pt - oldTangentImpulse;
}else{
maxPt = friction * dPn;
dPt = MathUtils.clamp(dPt, -maxPt, maxPt);
}
// Apply contact impulse
var Pt:Vec2 = tangent.mul(dPt);
b1.velocity = b1.velocity.sub(Pt.mul(b1.invMass));
b1.angularVelocity -= b1.invI * MathUtils.crossVV(c.r1, Pt);
b2.velocity = b2.velocity.add(Pt.mul(b2.invMass));
b2.angularVelocity += b2.invI * MathUtils.crossVV(c.r2, Pt);
}
}
}
/*******************************************************************************
*
* Collide.cpp
*
*******************************************************************************/
// Box vertex and edge numbering:
//
// ^ y
// |
// e1
// v2 ------ v1
// | |
// e2 | | e4 --> x
// | |
// v3 ------ v4
// e3
class Axis{
public static const FACE_A_X:String = "faceAX";
public static const FACE_A_Y:String = "faceAY";
public static const FACE_B_X:String = "faceBX";
public static const FACE_B_Y:String = "faceBY";
}
class EdgeNumbers{
public static const NO_EDGE:uint = 0;
public static const EDGE1:uint = 1;
public static const EDGE2:uint = 2;
public static const EDGE3:uint = 3;
public static const EDGE4:uint = 4;
}
class ClipVertex{
public var v:Vec2;
public var fp:FeaturePair;
public function ClipVertex() {
v = new Vec2();
fp = new FeaturePair();
}
}
class Collide{
private static function flip(fp:FeaturePair):void{
var tmp:uint;
tmp = fp.inEdge1;
fp.inEdge1 = fp.inEdge2;
fp.inEdge2 = tmp;
tmp = fp.outEdge1;
fp.outEdge1 = fp.outEdge2;
fp.outEdge2 = tmp;
}
private static function clipSegmentToLine(vOut:Vector.<ClipVertex>, vIn:Vector.<ClipVertex>,
normal:Vec2, offset:Number, clipEdge:uint):int{
// Start with no output points
var numOut:uint = 0;
// Calculate the distance of end points to the line
var distance0:Number = normal.dot(vIn[0].v) - offset;
var distance1:Number = normal.dot(vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0) vOut[numOut++] = vIn[0];
if (distance1 <= 0.0) vOut[numOut++] = vIn[1];
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0){
// Find intersection point of edge and plane
var interp:Number = distance0 / (distance0 - distance1);
vOut[numOut].v = vIn[0].v.add(vIn[1].v.sub(vIn[0].v).mul(interp));
if (distance0 > 0.0){
vOut[numOut].fp = vIn[0].fp;
vOut[numOut].fp.inEdge1 = clipEdge;
vOut[numOut].fp.inEdge2 = EdgeNumbers.NO_EDGE;
}else{
vOut[numOut].fp = vIn[1].fp;
vOut[numOut].fp.outEdge1 = clipEdge;
vOut[numOut].fp.outEdge2 = EdgeNumbers.NO_EDGE;
}
++numOut;
}
return numOut;
}
private static function computeIncidentEdge(c:Vector.<ClipVertex>, h:Vec2, pos:Vec2,
Rot:Mat22, normal:Vec2):void{
// The normal is from the reference box. Convert it
// to the incident boxe's frame and flip sign.
var RotT:Mat22 = Rot.transpose();
var n:Vec2 = RotT.mul(normal).negative();
var nAbs:Vec2 = n.abs();
if (nAbs.x > nAbs.y){
if (n.x > 0.0){
c[0].v.set(h.x, -h.y);
c[0].fp.inEdge2 = EdgeNumbers.EDGE3;
c[0].fp.outEdge2 = EdgeNumbers.EDGE4;
c[1].v.set(h.x, h.y);
c[1].fp.inEdge2 = EdgeNumbers.EDGE4;
c[1].fp.outEdge2 = EdgeNumbers.EDGE1;
}else{
c[0].v.set(-h.x, h.y);
c[0].fp.inEdge2 = EdgeNumbers.EDGE1;
c[0].fp.outEdge2 = EdgeNumbers.EDGE2;
c[1].v.set(-h.x, -h.y);
c[1].fp.inEdge2 = EdgeNumbers.EDGE2;
c[1].fp.outEdge2 = EdgeNumbers.EDGE3;
}
}else{
if (n.y > 0.0){
c[0].v.set(h.x, h.y);
c[0].fp.inEdge2 = EdgeNumbers.EDGE4;
c[0].fp.outEdge2 = EdgeNumbers.EDGE1;
c[1].v.set(-h.x, h.y);
c[1].fp.inEdge2 = EdgeNumbers.EDGE1;
c[1].fp.outEdge2 = EdgeNumbers.EDGE2;
}else{
c[0].v.set(-h.x, -h.y);
c[0].fp.inEdge2 = EdgeNumbers.EDGE2;
c[0].fp.outEdge2 = EdgeNumbers.EDGE3;
c[1].v.set(h.x, -h.y);
c[1].fp.inEdge2 = EdgeNumbers.EDGE3;
c[1].fp.outEdge2 = EdgeNumbers.EDGE4;
}
}
c[0].v = Rot.mul(c[0].v).add(pos);
c[1].v = Rot.mul(c[1].v).add(pos);
}
// The normal points from A to B
public static function collide(bodyA:Body, bodyB:Body):Vector.<Contact>{
var contacts:Vector.<Contact> = new Vector.<Contact>();
// Setup
var hA:Vec2 = bodyA.width.mul(0.5);
var hB:Vec2 = bodyB.width.mul(0.5);
var posA:Vec2 = bodyA.position;
var posB:Vec2 = bodyB.position;
var RotA:Mat22 = Mat22.createRotate(bodyA.rotation);
var RotB:Mat22 = Mat22.createRotate(bodyB.rotation);
var RotAT:Mat22 = RotA.transpose();
var RotBT:Mat22 = RotB.transpose();
var a1:Vec2 = RotA.col1, a2:Vec2 = RotA.col2;
var b1:Vec2 = RotB.col1, b2:Vec2 = RotB.col2;
var dp:Vec2 = posB.sub(posA);
var dA:Vec2 = RotAT.mul(dp);
var dB:Vec2 = RotBT.mul(dp);
var C:Mat22 = RotAT.mulMat22(RotB);
var absC:Mat22 = C.abs();
var absCT:Mat22 = absC.transpose();
GraphicsCache.g.lineStyle(0, 0xff0000);
// Box A faces
var faceA:Vec2 = dA.abs().sub(hA).sub(absC.mul(hB));
//GraphicsCache.g.moveTo(bodyA.position.x * 10, bodyA.position.y * 10);
//GraphicsCache.g.lineTo((bodyA.position.x + faceA.x) * 10, (bodyA.position.y + faceA.y) * 10);
if (faceA.x > 0.0 || faceA.y > 0.0)
return contacts;
// Box B faces
var faceB:Vec2 = dB.abs().sub(absCT.mul(hA)).sub(hB);
//GraphicsCache.g.moveTo(bodyB.position.x * 10, bodyB.position.y * 10);
//GraphicsCache.g.lineTo((bodyB.position.x + faceB.x) * 10, (bodyB.position.y + faceB.y) * 10);
if (faceB.x > 0.0 || faceB.y > 0.0)
return contacts;
// Find best axis
var axis:String;
var separation:Number;
var normal:Vec2;
// Box A faces
axis= Axis.FACE_A_X;
separation = faceA.x;
normal = dA.x > 0.0 ? RotA.col1 : RotA.col1.negative();
const relativeTol:Number = 0.95;
const absoluteTol:Number = 0.01;
if (faceA.y > relativeTol * separation + absoluteTol * hA.y){
axis = Axis.FACE_A_Y;
separation = faceA.y;
normal = dA.y > 0.0 ? RotA.col2 : RotA.col2.negative();
//GraphicsCache.g.moveTo(bodyA.position.x * 10, bodyA.position.y * 10);
//GraphicsCache.g.lineTo((bodyA.position.x + normal.x * 10) * 10, (bodyA.position.y + normal.y * 10) * 10);
}
// Box B faces
if (faceB.x > relativeTol * separation + absoluteTol * hB.x){
axis = Axis.FACE_B_X;
separation = faceB.x;
normal = dB.x > 0.0 ? RotB.col1 : RotB.col1.negative();
}
if (faceB.y > relativeTol * separation + absoluteTol * hB.y){
axis = Axis.FACE_B_Y;
separation = faceB.y;
normal = dB.y > 0.0 ? RotB.col2 : RotB.col2.negative();
}
// Setup clipping plane data based on the separating axis
var frontNormal:Vec2, sideNormal:Vec2;
var incidentEdge:Vector.<ClipVertex> = Vector.<ClipVertex>([new ClipVertex(), new ClipVertex()]);
var front:Number, negSide:Number, posSide:Number;
var negEdge:uint, posEdge:uint;
var side:Number;
// Compute the clipping lines and the line segment to be clipped.
switch (axis){
case Axis.FACE_A_X:
frontNormal = normal;
front = posA.dot(frontNormal) + hA.x;
sideNormal = RotA.col2;
side = posA.dot(sideNormal);
negSide = -side + hA.y;
posSide = side + hA.y;
negEdge = EdgeNumbers.EDGE3;
posEdge = EdgeNumbers.EDGE1;
computeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal);
break;
case Axis.FACE_A_Y:
frontNormal = normal;
front = posA.dot(frontNormal) + hA.y;
sideNormal = RotA.col1;
side = posA.dot(sideNormal);
negSide = -side + hA.x;
posSide = side + hA.x;
negEdge = EdgeNumbers.EDGE2;
posEdge = EdgeNumbers.EDGE4;
computeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal);
break;
case Axis.FACE_B_X:
frontNormal = normal.negative();
front = posB.dot(frontNormal) + hB.x;
sideNormal = RotB.col2;
side = posB.dot(sideNormal);
negSide = -side + hB.y;
posSide = side + hB.y;
negEdge = EdgeNumbers.EDGE3;
posEdge = EdgeNumbers.EDGE1;
computeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal);
break;
case Axis.FACE_B_Y:
frontNormal = normal.negative();
front = posB.dot(frontNormal) + hB.y;
sideNormal = RotB.col1;
side = posB.dot(sideNormal);
negSide = -side + hB.x;
posSide = side + hB.x;
negEdge = EdgeNumbers.EDGE2;
posEdge = EdgeNumbers.EDGE4;
computeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal);
break;
}
// clip other face with 5 box planes (1 face plane, 4 edge planes)
var clipPoints1:Vector.<ClipVertex> = Vector.<ClipVertex>([new ClipVertex(), new ClipVertex()]);
var clipPoints2:Vector.<ClipVertex> = Vector.<ClipVertex>([new ClipVertex(), new ClipVertex()]);
var np:int;
// Clip to box side 1
np = clipSegmentToLine(clipPoints1, incidentEdge, sideNormal.negative(), negSide, negEdge);
if (np < 2)
return contacts;
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge);
if (np < 2)
return contacts;
// Now clipPoints2 contains the clipping points.
// Due to roundoff, it is possible that clipping removes all points.
var numContacts:int = 0;
for (var i:int = 0; i < 2; ++i){
separation = frontNormal.dot(clipPoints2[i].v) - front;
if (separation <= 0){
contacts.push(new Contact());
contacts[numContacts].separation = separation;
contacts[numContacts].normal = normal;
// slide contact point onto reference face (easy to cull)
contacts[numContacts].position = clipPoints2[i].v.sub(frontNormal.mul(separation));
contacts[numContacts].feature = clipPoints2[i].fp;
if (axis == Axis.FACE_B_X || axis == Axis.FACE_B_Y)
flip(contacts[numContacts].feature);
++numContacts;
}
}
return contacts;
}
}
/*******************************************************************************
*
* World.cpp, World.h
*
*******************************************************************************/
class World{
public var bodies:Vector.<Body> = new Vector.<Body>();
public var joints:Vector.<Joint> = new Vector.<Joint>();
public var arbiters:Object = {};
public var gravity:Vec2;
public var iterations:int;
public static var accumulateImpulses:Boolean = true;
public static var warmStarting:Boolean = true;
public static var positionCorrection:Boolean = true;
public function World(gravity:Vec2, iterations:int){
this.gravity = gravity;
this.iterations = iterations;
}
public function addBody(body:Body):void{
bodies.push(body);
}
public function addJoint(joint:Joint):void{
joints.push(joint);
}
public function clear():void{
bodies.length = 0;
joints.length = 0;
arbiters = {};
}
private function broadPhase():void{
// O(n^2) broad-phase
for (var i:int = 0; i < bodies.length; ++i){
var bi:Body = bodies[i];
for (var j:int = i + 1; j < bodies.length; ++j){
var bj:Body = bodies[j];
if (bi.invMass == 0.0 && bj.invMass == 0.0)
continue;
var newArb:Arbiter = new Arbiter(bi, bj);
var key:ArbiterKey = new ArbiterKey(bi, bj);
if (newArb.numContacts > 0){
if (!arbiters.hasOwnProperty(key.id)){
arbiters[key.id] = newArb;
}else{
(arbiters[key.id] as Arbiter).update(newArb.contacts, newArb.numContacts);
}
}else{
delete arbiters[key.id];
}
}
}
}
public function step(dt:Number):void{
var inv_dt:Number = dt > 0.0 ? 1.0 / dt : 0.0;
// Determine overlapping bodies and update contact points.
broadPhase();
// Integrate forces.
for each (var b:Body in bodies){
if (b.invMass == 0.0)
continue;
b.velocity = gravity.add(b.force.mul(b.invMass)).mul(dt).add(b.velocity);
b.angularVelocity += dt * b.invI * b.torque;
}
// Perform pre-steps.
for each (var arb:Arbiter in arbiters){
arb.preStep(inv_dt);
}
for each (var joint:Joint in joints){
joint.preStep(inv_dt);
}
// Perform iterations
for (var i:int = 0; i < iterations; ++i){
for each (arb in arbiters){
arb.applyImpulse();
}
for each (joint in joints){
joint.applyImpulse();
}
}
// Integrate Velocities
for each (b in bodies){
b.position = b.velocity.mul(dt).add(b.position);
b.rotation += dt * b.angularVelocity;
b.force.set(0.0, 0.0);
b.torque = 0.0;
}
}
}