forked from: Box2d Lite in ActionScript 3
/**
* Copyright kazukazu989 ( http://wonderfl.net/user/kazukazu989 )
* MIT License ( http://www.opensource.org/licenses/mit-license.php )
* Downloaded from: http://wonderfl.net/c/cm7X
*/
// forked from nitoyon's 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.
*/
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;
}
}
}