OimoPhysics wonderfl test
1-6: change demos
WASD: move
SPACE: reset
P.S. dominoes demo added.
1～6キーでデモ変更
WASDキーで球を動かす
SPACEキーでリセット
追記：ドミノ追加。
github: https://github.com/saharan/OimoPhysics
/**
* Copyright saharan ( http://wonderfl.net/user/saharan )
* MIT License ( http://www.opensource.org/licenses/mit-license.php )
* Downloaded from: http://wonderfl.net/c/72Mf
*/
package {
import flash.display.Bitmap;
import flash.display.BitmapData;
import flash.display.Sprite;
import flash.display.Stage3D;
import flash.events.*;
import flash.text.*;
import flash.ui.*;
import net.hires.debug.Stats;
/**
* OimoPhysics wonderfl test
* @author saharan
*/
[SWF(width = "465", height = "465", frameRate = "60")]
public class WonderflTest extends Sprite {
private var s3d:Stage3D;
private var world:World;
private var renderer:DebugDraw;
private var rigid:RigidBody;
private var count:uint;
private var tf:TextField;
private var fps:Number;
private var l:Boolean;
private var r:Boolean;
private var u:Boolean;
private var d:Boolean;
private var ctr:RigidBody;
private var type:uint;
private var demoName:String;
private var dragX:Number;
private var dragY:Number;
private var rotX:Number;
private var rotY:Number;
private var pmouseX:Number;
private var pmouseY:Number;
private var press:Boolean;
public function WonderflTest() {
if (stage) init();
else addEventListener(Event.ADDED_TO_STAGE, init);
}
private function init(e:Event = null):void {
removeEventListener(Event.ADDED_TO_STAGE, init);
contextMenu = new ContextMenu();
contextMenu.hideBuiltInItems();
var debug:Stats = new Stats();
debug.x = 395;
debug.alpha = 0.6;
addChild(debug);
tf = new TextField();
tf.defaultTextFormat = new TextFormat("_monospace", 11, 0xffffff);
tf.alpha = 0.8;
tf.selectable = false;
tf.x = 0;
tf.y = 0;
tf.width = 400;
tf.height = 400;
dragX = 0;
dragY = 100;
rotX = 0;
rotY = 0;
addChild(tf);
type = 0;
initWorld();
fps = 0;
s3d = stage.stage3Ds[0];
s3d.addEventListener(Event.CONTEXT3D_CREATE, onContext3DCreated);
s3d.requestContext3D();
stage.addEventListener(MouseEvent.MOUSE_DOWN, function(e:Event):void {
press = true;
});
stage.addEventListener(MouseEvent.MOUSE_UP, function(e:Event):void {
press = false;
});
stage.addEventListener(KeyboardEvent.KEY_DOWN, function(e:KeyboardEvent):void {
var code:uint = e.keyCode;
if (code == Keyboard.W) {
u = true;
}
if (code == Keyboard.S) {
d = true;
}
if (code == Keyboard.A) {
l = true;
}
if (code == Keyboard.D) {
r = true;
}
if (code == Keyboard.SPACE) {
initWorld();
}
if (code == Keyboard.NUMBER_1) {
type = 0;
initWorld();
}
if (code == Keyboard.NUMBER_2) {
type = 1;
initWorld();
}
if (code == Keyboard.NUMBER_3) {
type = 2;
initWorld();
}
if (code == Keyboard.NUMBER_4) {
type = 3;
initWorld();
}
if (code == Keyboard.NUMBER_5) {
type = 4;
initWorld();
}
if (code == Keyboard.NUMBER_6) {
type = 5;
initWorld();
}
});
stage.addEventListener(KeyboardEvent.KEY_UP, function(e:KeyboardEvent):void {
var code:uint = e.keyCode;
if (code == Keyboard.W) {
u = false;
}
if (code == Keyboard.S) {
d = false;
}
if (code == Keyboard.A) {
l = false;
}
if (code == Keyboard.D) {
r = false;
}
});
addEventListener(Event.ENTER_FRAME, frame);
}
private function initWorld():void {
world = new World();
if (!renderer) renderer = new DebugDraw(466, 466);
renderer.setWorld(world);
var rb:RigidBody;
var s:Shape;
var c:ShapeConfig = new ShapeConfig();
rb = new RigidBody();
c.position.init(0, -0.5, 0);
c.rotation.init();
s = new BoxShape(32, 1, 32, c);
rb.addShape(s);
rb.setupMass(RigidBody.BODY_STATIC);
world.addRigidBody(rb);
c.rotation.init();
var width:uint;
var height:uint;
var depth:uint;
var bw:Number = 0.75;
var bh:Number = 0.75;
var bd:Number = 0.75;
var i:uint;
var j:uint;
var k:uint;
var size:Number;
switch(type) {
case 0:
demoName = "Basic demo";
width = 6;
height = 6;
depth = 6;
bw = 0.75;
bh = 0.75;
bd = 0.75;
for (i = 0; i < width; i++) {
for (j = 0; j < height; j++) {
for (k = 0; k < depth; k++) {
rb = new RigidBody();
c.position.init(
(i - (width - 1) * 0.5) * bw,
j * bh + bh * 0.5,
(k - (depth - 1) * 0.5) * bd
);
s = new BoxShape(bw, bh, bd, c);
rb.addShape(s);
rb.setupMass(RigidBody.BODY_DYNAMIC);
world.addRigidBody(rb);
}
Shape.nextID++; // little trick for shape color
}
Shape.nextID++;
}
break;
case 1:
demoName = "Tower stack";
height = 20;
bw = 0.6;
bh = 0.75;
bd = 1.2;
for (j = 0; j < height; j++) {
for (i = 0; i < 10; i++) {
var ang:Number = Math.PI * 2 / 10 * (i + (j & 1) * 0.5);
rb = new RigidBody(-ang, 0, 1, 0);
c.position.init(
Math.cos(ang) * 2.5,
j * bh + bh * 0.5,
Math.sin(ang) * 2.5
);
s = new BoxShape(bw, bh, bd, c);
rb.addShape(s);
rb.setupMass(RigidBody.BODY_DYNAMIC);
world.addRigidBody(rb);
}
}
break;
case 2:
demoName = "Compound shapes";
width = 200;
for (i = 0; i < width; i++) {
rb = new RigidBody(Math.PI, Math.random(), Math.random(), Math.random());
if (i & 1) {
c.position.init(
Math.random() * 8 - 4,
i * 0.25 + 2,
Math.random() * 8 - 4
);
size = 0.25 + Math.random() * 0.4;
s = new SphereShape(size, c);
rb.addShape(s);
c.position.x += size + (size = 0.25 + Math.random() * 0.4);
s = new SphereShape(size, c);
rb.addShape(s);
} else {
c.position.init(
Math.random() * 8 - 4,
i * 0.25 + 2,
Math.random() * 8 - 4
);
size = 0.7 + Math.random() * 0.3;
s = new BoxShape((0.8 + Math.random() * 0.4) * size, 0.5 * size, (0.8 + Math.random() * 0.4) * size, c);
rb.addShape(s);
c.position.y -= size;
s = new BoxShape(0.5 * size, 1.5 * size, 0.5 * size, c);
rb.addShape(s);
}
rb.setupMass(RigidBody.BODY_DYNAMIC);
world.addRigidBody(rb);
}
break;
case 3:
demoName = "Pyramid";
width = 20;
bw = 0.8;
bh = 0.5;
bd = 0.7;
for (i = 0; i < width; i++) {
for (j = i; j < width; j++) {
rb = new RigidBody();
c.position.init(
(j - i * 0.5 - (width - 1) * 0.5) * bw * 1.1,
i * bh * 1.1 + bh * 0.5,
0
);
s = new BoxShape(bw, bh, bd, c);
rb.addShape(s);
rb.setupMass(RigidBody.BODY_DYNAMIC);
world.addRigidBody(rb);
}
}
break;
case 4:
demoName = "Spining tops";
width = 6;
depth = 4;
for (i = 0; i < width; i++) {
for (j = 0; j < depth; j++) {
rb = new RigidBody(0.2, 1, 0, 0);
size = 0.5 + Math.random() * 0.25;
c.position.init(
(i - (width - 1) * 0.5) * 4,
0.2 * size,
(j - (depth - 1) * 0.5) * 4 - 3
);
c.density = 32; // move down center of gravity
s = new SphereShape(0.2 * size, c);
rb.addShape(s);
c.position.y += 0.75 * size;
c.density = 0.5;
s = new SphereShape(size, c);
rb.addShape(s);
c.position.y += 1.2 * size;
s = new BoxShape(0.4 * size, 0.4 * size, 0.4 * size, c);
rb.addShape(s);
c.position.y += 0.45 * size;
s = new SphereShape(0.25 * size, c);
rb.addShape(s);
rb.setupMass(RigidBody.BODY_DYNAMIC);
rb.angularVelocity.y = Math.PI * 2 * (15 + Math.random() * 4 - 2); // 900(+-120) rpm
world.addRigidBody(rb);
Shape.nextID++;
}
Shape.nextID++;
}
c.density = 1;
break;
case 5:
demoName = "Dominoes";
bw = 0.7;
bh = 1.5;
bd = 0.3;
ang = 0;
for (i = 0; i < 200; i++) {
var rad:Number = 0.5 + ang * 0.25;
ang += 5 / (Math.PI * rad * 2);
rb = new RigidBody(-ang, 0, 1, 0);
c.position.init(
Math.cos(ang) * rad,
bh * 0.5,
Math.sin(ang) * rad - 3
);
s = new BoxShape(bw, bh, bd, c);
rb.addShape(s);
rb.setupMass(RigidBody.BODY_DYNAMIC);
world.addRigidBody(rb);
}
rb.angularVelocity.x = Math.cos(ang) * -3;
rb.angularVelocity.z = Math.sin(ang) * -3;
rb.linearVelocity.x = Math.sin(ang);
rb.linearVelocity.z = -Math.cos(ang);
break;
}
c.friction = 2;
c.position.init(0, 1, 10);
c.density = 10;
c.rotation.init();
s = new SphereShape(1, c);
ctr = new RigidBody();
ctr.addShape(s);
ctr.setupMass(RigidBody.BODY_DYNAMIC);
world.addRigidBody(ctr);
}
private function onContext3DCreated(e:Event = null):void {
renderer.setContext3D(s3d.context3D);
renderer.camera(0, 2, 4);
}
private function frame(e:Event = null):void {
count++;
if (press) {
dragX += mouseX - pmouseX;
dragY -= mouseY - pmouseY;
}
if (dragY < 0) dragY = 0;
if (dragY > 512) dragY = 512;
rotX += (dragX - rotX) * 0.25;
rotY += (dragY - rotY) * 0.25;
pmouseX = mouseX;
pmouseY = mouseY;
var ang:Number = rotX * 0.01 + Math.PI * 0.5;
var sin:Number = Math.sin(ang);
var cos:Number = Math.cos(ang);
renderer.camera(
ctr.position.x + cos * 8,
ctr.position.y + rotY * 0.1,
ctr.position.z + sin * 8,
ctr.position.x - cos * rotY * 0.025,
ctr.position.y,
ctr.position.z - sin * rotY * 0.025
);
if (l) {
ctr.linearVelocity.x -= Math.cos(ang - Math.PI * 0.5) * 0.8;
ctr.linearVelocity.z -= Math.sin(ang - Math.PI * 0.5) * 0.8;
}
if (r) {
ctr.linearVelocity.x -= Math.cos(ang + Math.PI * 0.5) * 0.8;
ctr.linearVelocity.z -= Math.sin(ang + Math.PI * 0.5) * 0.8;
}
if (u) {
ctr.linearVelocity.x -= cos * 0.8;
ctr.linearVelocity.z -= sin * 0.8;
}
if (d) {
ctr.linearVelocity.x += cos * 0.8;
ctr.linearVelocity.z += sin * 0.8;
}
world.step();
fps += (1000 / world.performance.totalTime - fps) * 0.5;
if (fps > 1000 || fps != fps) {
fps = 1000;
}
tf.text =
demoName + "\n\n" +
"Rigid Body Count: " + world.numRigidBodies + "\n" +
"Shape Count: " + world.numShapes + "\n" +
"Contacts Count: " + world.numContacts + "\n\n" +
"Broad Phase Time: " + world.performance.broadPhaseTime + "ms\n" +
"Narrow Phase Time: " + world.performance.narrowPhaseTime + "ms\n" +
"Constraints Time: " + world.performance.constraintsTime + "ms\n" +
"Update Time: " + world.performance.updateTime + "ms\n" +
"Total Time: " + world.performance.totalTime + "ms\n" +
"Physics FPS: " + fps.toFixed(2) + "\n"
;
tf.setTextFormat(new TextFormat("_monospace", 16, 0xffffff), 0, demoName.length);
renderer.render();
var len:uint = world.numRigidBodies;
var rbs:Vector.<RigidBody> = world.rigidBodies;
for (var i:int = 0; i < len; i++) {
var r:RigidBody = rbs[i];
if (r.position.y < -12) {
r.position.init(Math.random() * 8 - 4, Math.random() * 4 + 8, Math.random() * 8 - 4);
r.linearVelocity.x *= 0.8;
r.linearVelocity.z *= 0.8;
}
}
}
}
}
/* Copyright (c) 2012 EL-EMENT saharan
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this
* software and associated documentation * files (the "Software"), to deal in the Software
* without restriction, including without limitation the rights to use, copy, * modify, merge,
* publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to
* whom the Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all copies or
* substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
* INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
* PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR
* ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
import com.adobe.utils.AGALMiniAssembler;
import flash.display3D.*;
import flash.utils.*;
class OimoGLMini {
private static const VERTEX_POISITION_INDEX:uint = 0;
private static const VERTEX_NORMAL_INDEX:uint = 1;
private static const FRAGMENT_COLOR_INDEX:uint = 0;
private static const FRAGMENT_AMB_DIF_EMI_INDEX:uint = 1;
private static const FRAGMENT_SPC_SHN_INDEX:uint = 2;
private static const FRAGMENT_AMB_LIGHT_COLOR_INDEX:uint = 3;
private static const FRAGMENT_DIR_LIGHT_COLOR_INDEX:uint = 4;
private static const FRAGMENT_DIR_LIGHT_DIRECTION_INDEX:uint = 5;
private static const FRAGMENT_CAMERA_POSITION_INDEX:uint = 6;
private var c3d:Context3D;
private var w:uint;
private var h:uint;
private var aspect:Number;
private var worldM:Mat44;
private var viewM:Mat44;
private var projM:Mat44;
private var viewProjM:Mat44;
private var up:Vector.<Number>;
private var stackM:Vector.<Mat44>;
private var numStack:uint;
private var vertexB:Vector.<VertexBuffer3D>;
private var numVerticesB:Vector.<uint>;
private var indexB:Vector.<IndexBuffer3D>;
private var numIndicesB:Vector.<uint>;
public function OimoGLMini(c3d:Context3D, w:uint, h:uint, antiAlias:uint = 0) {
this.c3d = c3d;
c3d.configureBackBuffer(w, h, antiAlias, true);
c3d.setBlendFactors("sourceAlpha", "oneMinusSourceAlpha");
c3d.setCulling("front"); // ClockWise
this.w = w;
this.h = h;
aspect = w / h;
worldM = new Mat44();
viewM = new Mat44();
projM = new Mat44();
viewProjM = new Mat44();
up = new Vector.<Number>(4, true);
stackM = new Vector.<Mat44>(256, true);
vertexB = new Vector.<VertexBuffer3D>(256, true);
numVerticesB = new Vector.<uint>(256, true);
indexB = new Vector.<IndexBuffer3D>(256, true);
numIndicesB = new Vector.<uint>(256, true);
numStack = 0;
var program:Program3D = c3d.createProgram();
var vs:AGALMiniAssembler = new AGALMiniAssembler();
vs.assemble("vertex", createBasicVertexShaderCode(VERTEX_POISITION_INDEX, VERTEX_NORMAL_INDEX));
var fs:AGALMiniAssembler = new AGALMiniAssembler();
fs.assemble("fragment",
createBasicFragmentShaderCode(
VERTEX_POISITION_INDEX, VERTEX_NORMAL_INDEX,
FRAGMENT_COLOR_INDEX, FRAGMENT_AMB_DIF_EMI_INDEX, FRAGMENT_SPC_SHN_INDEX,
FRAGMENT_AMB_LIGHT_COLOR_INDEX, FRAGMENT_DIR_LIGHT_COLOR_INDEX, FRAGMENT_DIR_LIGHT_DIRECTION_INDEX,
FRAGMENT_CAMERA_POSITION_INDEX
)
);
program.upload(vs.agalcode, fs.agalcode);
c3d.setProgram(program);
material(1, 1, 0, 0, 0);
color(1, 1, 1);
ambientLightColor(0.2, 0.2, 0.2);
directionalLightColor(0.8, 0.8, 0.8);
directionalLightDirection(0, 0, -1);
camera(0, 0, 100, 0, 0, 0, 0, 1, 0);
perspective(Math.PI / 3);
}
public function material(
ambient:Number, diffuse:Number, emission:Number,
specular:Number, shininess:Number
):void {
setProgramConstantsNumber("fragment", FRAGMENT_AMB_DIF_EMI_INDEX, ambient, diffuse, emission, 1);
setProgramConstantsNumber("fragment", FRAGMENT_SPC_SHN_INDEX, specular, shininess, 0, 1);
}
public function color(r:Number, g:Number, b:Number, a:Number = 1):void {
setProgramConstantsNumber("fragment", FRAGMENT_COLOR_INDEX, r, g, b, a);
}
public function ambientLightColor(r:Number, g:Number, b:Number):void {
setProgramConstantsNumber("fragment", FRAGMENT_AMB_LIGHT_COLOR_INDEX, r, g, b, 1);
}
public function directionalLightColor(r:Number, g:Number, b:Number):void {
setProgramConstantsNumber("fragment", FRAGMENT_DIR_LIGHT_COLOR_INDEX, r, g, b, 1);
}
public function directionalLightDirection(x:Number, y:Number, z:Number):void {
setProgramConstantsNumber("fragment", FRAGMENT_DIR_LIGHT_DIRECTION_INDEX, x, y, z, 1);
}
public function camera(
eyeX:Number, eyeY:Number, eyeZ:Number,
atX:Number, atY:Number, atZ:Number,
upX:Number, upY:Number, upZ:Number
):void {
setProgramConstantsNumber("fragment", FRAGMENT_CAMERA_POSITION_INDEX, eyeX, eyeY, eyeZ, 1);
viewM.lookAt(eyeX, eyeY, eyeZ, atX, atY, atZ, upX, upY, upZ);
}
public function perspective(fovY:Number, near:Number = 0.5, far:Number = 10000):void {
projM.perspective(fovY, aspect, near, far);
}
public function beginScene(r:Number, g:Number, b:Number):void {
worldM.init();
c3d.clear(r, g, b);
}
public function endScene():void {
c3d.present();
}
public function registerBox(bufferIndex:uint, width:Number, height:Number, depth:Number):void {
width *= 0.5;
height *= 0.5;
depth *= 0.5;
registerBuffer(bufferIndex, 24, 36);
uploadVertexBuffer(bufferIndex, Vector.<Number>([
-width, height, -depth, // top face
-width, height, depth,
width, height, depth,
width, height, -depth,
-width, -height, -depth, // bottom face
width, -height, -depth,
width, -height, depth,
-width, -height, depth,
-width, height, -depth, // left face
-width, -height, -depth,
-width, -height, depth,
-width, height, depth,
width, height, -depth, // right face
width, height, depth,
width, -height, depth,
width, -height, -depth,
-width, height, depth, // front face
-width, -height, depth,
width, -height, depth,
width, height, depth,
-width, height, -depth, // back face
width, height, -depth,
width, -height, -depth,
-width, -height, -depth
]), Vector.<Number>([
0, 1, 0, // top face
0, 1, 0,
0, 1, 0,
0, 1, 0,
0, -1, 0, // bottom face
0, -1, 0,
0, -1, 0,
0, -1, 0,
-1, 0, 0, // left face
-1, 0, 0,
-1, 0, 0,
-1, 0, 0,
1, 0, 0, // right face
1, 0, 0,
1, 0, 0,
1, 0, 0,
0, 0, 1, // front face
0, 0, 1,
0, 0, 1,
0, 0, 1,
0, 0, -1, // back face
0, 0, -1,
0, 0, -1,
0, 0, -1
]));
uploadIndexBuffer(bufferIndex, Vector.<uint>([
0, 1, 2, // top face
0, 2, 3,
4, 5, 6, // bottom face
4, 6, 7,
8, 9, 10, // left face
8, 10, 11,
12, 13, 14, // right face
12, 14, 15,
16, 17, 18, // front face
16, 18, 19,
20, 21, 22, // back face
20, 22, 23
]));
}
public function registerSphere(bufferIndex:uint, radius:Number, divisionH:uint, divisionV:uint):void {
var count:uint = 0;
var theta:Number;
var phi:Number;
var dTheta:Number = Math.PI * 2 / divisionH;
var dPhi:Number = Math.PI / divisionV;
var numVertices:uint = (divisionV + 1) * divisionH - ((divisionH - 1) << 1);
var numFaces:uint = (divisionV - 1 << 1) * divisionH;
var vtx:Vector.<Number> = new Vector.<Number>(numVertices * 3, true);
var nrm:Vector.<Number> = new Vector.<Number>(numVertices * 3, true);
vtx[count] = 0;
vtx[count + 1] = radius;
vtx[count + 2] = 0;
nrm[count] = 0;
nrm[count + 1] = 1;
nrm[count + 2] = 0;
count += 3;
phi = dPhi;
for (var i:int = 1; i < divisionV; i++) {
theta = 0;
for (var j:int = 0; j < divisionH; j++) {
var sp:Number = Math.sin(phi);
var cp:Number = Math.cos(phi);
var st:Number = Math.sin(theta);
var ct:Number = Math.cos(theta);
vtx[count] = radius * sp * ct;
vtx[count + 1] = radius * cp;
vtx[count + 2] = radius * sp * st;
nrm[count] = sp * ct;
nrm[count + 1] = cp;
nrm[count + 2] = sp * st;
count += 3;
theta += dTheta;
}
phi += dPhi;
}
vtx[count] = 0;
vtx[count + 1] = -radius;
vtx[count + 2] = 0;
nrm[count] = 0;
nrm[count + 1] = -1;
nrm[count + 2] = 0;
var idx:Vector.<uint> = new Vector.<uint>(numFaces * 3, true);
count = 0;
for (i = 0; i < divisionV; i++) {
for (j = 0; j < divisionH; j++) {
if (i == 0) {
idx[count] = 0;
idx[count + 1] = (j + 1) % divisionH + 1;
idx[count + 2] = j + 1;
count += 3;
} else if (i == divisionV - 1) {
idx[count] = numVertices - 1;
idx[count + 1] = (i - 1) * divisionH + j + 1;
idx[count + 2] = (i - 1) * divisionH + (j + 1) % divisionH + 1;
count += 3;
} else {
idx[count] = (i - 1) * divisionH + j + 1;
idx[count + 1] = (i - 1) * divisionH + (j + 1) % divisionH + 1;
idx[count + 2] = i * divisionH + (j + 1) % divisionH + 1;
count += 3;
idx[count] = (i - 1) * divisionH + j + 1;
idx[count + 1] = i * divisionH + (j + 1) % divisionH + 1;
idx[count + 2] = i * divisionH + j + 1;
count += 3;
}
}
}
registerBuffer(bufferIndex, numVertices, numFaces * 3);
uploadVertexBuffer(bufferIndex, vtx, nrm);
uploadIndexBuffer(bufferIndex, idx);
}
public function registerBuffer(bufferIndex:uint, numVertices:uint, numIndices:uint):void {
if (vertexB[bufferIndex]) {
vertexB[bufferIndex].dispose();
indexB[bufferIndex].dispose();
}
vertexB[bufferIndex] = c3d.createVertexBuffer(numVertices, 6);
numVerticesB[bufferIndex] = numVertices;
indexB[bufferIndex] = c3d.createIndexBuffer(numIndices);
numIndicesB[bufferIndex] = numIndices;
}
public function uploadVertexBuffer(bufferIndex:uint, vertices:Vector.<Number>, normals:Vector.<Number>):void {
var numVertices:uint = numVerticesB[bufferIndex];
var arrayCount:uint = numVertices * 3;
var upload:Vector.<Number> = new Vector.<Number>(arrayCount << 1, true);
var num:uint = 0;
for (var i:int = 0; i < arrayCount; i += 3) {
upload[num++] = vertices[i];
upload[num++] = vertices[i + 1];
upload[num++] = vertices[i + 2];
upload[num++] = normals[i];
upload[num++] = normals[i + 1];
upload[num++] = normals[i + 2];
}
vertexB[bufferIndex].uploadFromVector(upload, 0, numVertices);
}
public function uploadIndexBuffer(bufferIndex:uint, indices:Vector.<uint>):void {
indexB[bufferIndex].uploadFromVector(indices, 0, numIndicesB[bufferIndex]);
}
public function drawTriangles(bufferIndex:uint):void {
c3d.setVertexBufferAt(0, vertexB[bufferIndex], 0, "float3");
c3d.setVertexBufferAt(1, vertexB[bufferIndex], 3, "float3");
setProgramConstantsMatrix("vertex", 0, worldM);
viewProjM.mul(projM, viewM);
setProgramConstantsMatrix("vertex", 4, viewProjM);
c3d.drawTriangles(indexB[bufferIndex]);
}
public function translate(tx:Number, ty:Number, tz:Number):void {
worldM.mulTranslate(worldM, tx, ty, tz);
}
public function scale(sx:Number, sy:Number, sz:Number):void {
worldM.mulScale(worldM, sx, sy, sz);
}
public function rotate(rad:Number, ax:Number, ay:Number, az:Number):void {
worldM.mulRotate(worldM, rad, ax, ay, az);
}
public function transform(m:Mat44):void {
worldM.mul(worldM, m);
}
public function push():void {
if (numStack < 256) {
if (!stackM[numStack]) stackM[numStack] = new Mat44();
stackM[numStack++].copy(worldM);
} else {
throw new Error("too many stacks.");
}
}
public function pop():void {
if (numStack > 0) {
worldM.copy(stackM[--numStack]);
} else {
throw new Error("there is no stack.");
}
}
public function loadWorldMatrix(m:Mat44):void {
worldM.copy(m);
}
public function loadViewMatrix(m:Mat44):void {
viewM.copy(m);
}
public function loadProjectionMatrix(m:Mat44):void {
projM.copy(m);
}
public function getWorldMatrix(m:Mat44):void {
m.copy(worldM);
}
public function getViewMatrix(m:Mat44):void {
m.copy(viewM);
}
public function getProjectionMatrix(m:Mat44):void {
m.copy(projM);
}
private function setProgramConstantsMatrix(type:String, index:uint, m:Mat44):void {
up[0] = m.e00; up[1] = m.e01; up[2] = m.e02; up[3] = m.e03;
c3d.setProgramConstantsFromVector(type, index, up);
up[0] = m.e10; up[1] = m.e11; up[2] = m.e12; up[3] = m.e13;
c3d.setProgramConstantsFromVector(type, index + 1, up);
up[0] = m.e20; up[1] = m.e21; up[2] = m.e22; up[3] = m.e23;
c3d.setProgramConstantsFromVector(type, index + 2, up);
up[0] = m.e30; up[1] = m.e31; up[2] = m.e32; up[3] = m.e33;
c3d.setProgramConstantsFromVector(type, index + 3, up);
}
private function setProgramConstantsNumber(type:String, index:uint, x:Number, y:Number, z:Number, w:Number):void {
up[0] = x; up[1] = y; up[2] = z; up[3] = w;
c3d.setProgramConstantsFromVector(type, index, up);
}
private function createBasicVertexShaderCode(vertexPositionIndex:uint, vertexNormalIndex:uint):String {
var pos:String = "v" + vertexPositionIndex;
var nor:String = "v" + vertexNormalIndex;
var code:String =
"m44 vt0, va0, vc0; \n" +
"mov " + pos + ", vt0; \n" +
"m44 op, vt0, vc4; \n" +
"m33 vt0.xyz, va1, vc0; \n" +
"nrm vt0.xyz, vt0.xyz; \n" +
"mov " + nor + " vt0; \n"
;
return code;
}
private function createBasicFragmentShaderCode(
vertexPositionIndex:uint, vertexNormalIndex:uint,
programColorIndex:uint, programAmbDifEmiIndex:uint, programSpcShnIndex:uint,
programAmbLightColorIndex:uint, programDirLightColorIndex:uint, programDirLightDirectionIndex:uint,
programCameraPosIndex:uint
):String {
var pos:String = "v" + vertexPositionIndex;
var nor:String = "v" + vertexNormalIndex;
var col:String = "fc" + programColorIndex;
var mat:String = "fc" + programAmbDifEmiIndex;
var spc:String = "fc" + programSpcShnIndex;
var alc:String = "fc" + programAmbLightColorIndex;
var dlc:String = "fc" + programDirLightColorIndex;
var dld:String = "fc" + programDirLightDirectionIndex;
var cam:String = "fc" + programCameraPosIndex;
var code:String =
"nrm ft1.xyz, " + nor + ".xyz \n" + // ft1 = normal
"mov ft2, " + col + " \n" + // ft2 = col
"mul ft0, ft2, " + alc + " \n" + // ft0 = ambColor
"mul ft0, ft0.xyz, " + mat + ".xxx \n" + // ft0 = ft0 * ambFactor
"mul ft3, ft2.xyz, " + mat + ".zzz \n" + // ft3 = col * emiFactor
"add ft0, ft0, ft3 \n" + // ft0 = ft0 + ft3
"mul ft3, ft2, " + dlc + " \n" + // ft3 = dirColor
"mul ft3, ft3.xyz, " + mat + ".yyy \n" + // ft3 = ft2 * dirFactor
"mov ft4, " + dld + " \n" + // ft4 = lightDir
"neg ft4, ft4 \n" + // ft4 = -ft4
"nrm ft4.xyz, ft4.xyz \n" + // ft4 = nrm(ft4)
"dp3 ft0.w, ft1.xyz, ft4.xyz \n" + // dot = normal * lightDir
"sat ft0.w, ft0.w \n" + // dot = sat(dot)
"mul ft3, ft3.xyz, ft0.www \n" + // ft3 = ft3 * dot
"add ft0, ft0, ft3 \n" + // ft0 = ft0 + ft3
"mul ft3, ft1.xyz, ft0.www \n" + // ft3 = normal * dot
"add ft3, ft3, ft3 \n" + // ft3 = ft3 * 2
"sub ft3, ft3, ft4 \n" + // ft3 = ft3 - lightDir
"nrm ft3.xyz, ft3.xyz \n" + // ft3 = nrm(ft3)
"mov ft5, " + cam + " \n" + // ft5 = cam
"sub ft5, ft5, " + pos + " \n" + // ft5 = ft5 - pos
"nrm ft5.xyz, ft5.xyz \n" + // ft5 = nrm(ft5)
"dp3 ft3.w, ft3.xyz, ft5.xyz \n" + // ref = ft3 * ft5
"sat ft3.w, ft3.w \n" + // ref = sat(ref)
"pow ft3.w, ft3.w, " + spc + ".yyy \n" + // ref = ref ^ shn
"mul ft3, ft3.www, " + dlc + ".xyz \n" + // rfc = ref * dirColor
"mul ft3, ft3, " + spc + ".xxx \n" + // rfc = rfc * spc
"sub ft3.w, ft3.w, ft3.w \n" + // zer = zer - zer
"slt ft3.w, ft3.w, ft0.w \n" + // zer = zer < dot ? 1 : 0
"mul ft3, ft3, ft3.www \n" + // rfc = rfc * zer
"add ft0, ft0, ft3 \n" + // ft0 = ft0 + rfc
"mov ft0.w, ft2.w \n" + // ft0 = alp
"mov oc, ft0 \n" // col = ft0
;
return code;
}
}
class Mat33 {
public var e00:Number;
public var e01:Number;
public var e02:Number;
public var e10:Number;
public var e11:Number;
public var e12:Number;
public var e20:Number;
public var e21:Number;
public var e22:Number;
public function Mat33(
e00:Number = 1, e01:Number = 0, e02:Number = 0,
e10:Number = 0, e11:Number = 1, e12:Number = 0,
e20:Number = 0, e21:Number = 0, e22:Number = 1
) {
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
}
public function init(
e00:Number = 1, e01:Number = 0, e02:Number = 0,
e10:Number = 0, e11:Number = 1, e12:Number = 0,
e20:Number = 0, e21:Number = 0, e22:Number = 1
):Mat33 {
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
return this;
}
public function add(m1:Mat33, m2:Mat33):Mat33 {
e00 = m1.e00 + m2.e00;
e01 = m1.e01 + m2.e01;
e02 = m1.e02 + m2.e02;
e10 = m1.e10 + m2.e10;
e11 = m1.e11 + m2.e11;
e12 = m1.e12 + m2.e12;
e20 = m1.e20 + m2.e20;
e21 = m1.e21 + m2.e21;
e22 = m1.e22 + m2.e22;
return this;
}
public function sub(m1:Mat33, m2:Mat33):Mat33 {
e00 = m1.e00 - m2.e00;
e01 = m1.e01 - m2.e01;
e02 = m1.e02 - m2.e02;
e10 = m1.e10 - m2.e10;
e11 = m1.e11 - m2.e11;
e12 = m1.e12 - m2.e12;
e20 = m1.e20 - m2.e20;
e21 = m1.e21 - m2.e21;
e22 = m1.e22 - m2.e22;
return this;
}
public function scale(m:Mat33, s:Number):Mat33 {
e00 = m.e00 * s;
e01 = m.e01 * s;
e02 = m.e02 * s;
e10 = m.e10 * s;
e11 = m.e11 * s;
e12 = m.e12 * s;
e20 = m.e20 * s;
e21 = m.e21 * s;
e22 = m.e22 * s;
return this;
}
public function mul(m1:Mat33, m2:Mat33):Mat33 {
var e00:Number = m1.e00 * m2.e00 + m1.e01 * m2.e10 + m1.e02 * m2.e20;
var e01:Number = m1.e00 * m2.e01 + m1.e01 * m2.e11 + m1.e02 * m2.e21;
var e02:Number = m1.e00 * m2.e02 + m1.e01 * m2.e12 + m1.e02 * m2.e22;
var e10:Number = m1.e10 * m2.e00 + m1.e11 * m2.e10 + m1.e12 * m2.e20;
var e11:Number = m1.e10 * m2.e01 + m1.e11 * m2.e11 + m1.e12 * m2.e21;
var e12:Number = m1.e10 * m2.e02 + m1.e11 * m2.e12 + m1.e12 * m2.e22;
var e20:Number = m1.e20 * m2.e00 + m1.e21 * m2.e10 + m1.e22 * m2.e20;
var e21:Number = m1.e20 * m2.e01 + m1.e21 * m2.e11 + m1.e22 * m2.e21;
var e22:Number = m1.e20 * m2.e02 + m1.e21 * m2.e12 + m1.e22 * m2.e22;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
return this;
}
public function mulScale(m:Mat33, sx:Number, sy:Number, sz:Number, prepend:Boolean = false):Mat33 {
var e00:Number;
var e01:Number;
var e02:Number;
var e10:Number;
var e11:Number;
var e12:Number;
var e20:Number;
var e21:Number;
var e22:Number;
if (prepend) {
e00 = sx * m.e00;
e01 = sx * m.e01;
e02 = sx * m.e02;
e10 = sy * m.e10;
e11 = sy * m.e11;
e12 = sy * m.e12;
e20 = sz * m.e20;
e21 = sz * m.e21;
e22 = sz * m.e22;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
} else {
e00 = m.e00 * sx;
e01 = m.e01 * sy;
e02 = m.e02 * sz;
e10 = m.e10 * sx;
e11 = m.e11 * sy;
e12 = m.e12 * sz;
e20 = m.e20 * sx;
e21 = m.e21 * sy;
e22 = m.e22 * sz;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
}
return this;
}
public function mulRotate(m:Mat33, rad:Number, ax:Number, ay:Number, az:Number, prepend:Boolean = false):Mat33 {
var s:Number = Math.sin(rad);
var c:Number = Math.cos(rad);
var c1:Number = 1 - c;
var r00:Number = ax * ax * c1 + c;
var r01:Number = ax * ay * c1 - az * s;
var r02:Number = ax * az * c1 + ay * s;
var r10:Number = ay * ax * c1 + az * s;
var r11:Number = ay * ay * c1 + c;
var r12:Number = ay * az * c1 - ax * s;
var r20:Number = az * ax * c1 - ay * s;
var r21:Number = az * ay * c1 + ax * s;
var r22:Number = az * az * c1 + c;
var e00:Number;
var e01:Number;
var e02:Number;
var e10:Number;
var e11:Number;
var e12:Number;
var e20:Number;
var e21:Number;
var e22:Number;
if (prepend) {
e00 = r00 * m.e00 + r01 * m.e10 + r02 * m.e20;
e01 = r00 * m.e01 + r01 * m.e11 + r02 * m.e21;
e02 = r00 * m.e02 + r01 * m.e12 + r02 * m.e22;
e10 = r10 * m.e00 + r11 * m.e10 + r12 * m.e20;
e11 = r10 * m.e01 + r11 * m.e11 + r12 * m.e21;
e12 = r10 * m.e02 + r11 * m.e12 + r12 * m.e22;
e20 = r20 * m.e00 + r21 * m.e10 + r22 * m.e20;
e21 = r20 * m.e01 + r21 * m.e11 + r22 * m.e21;
e22 = r20 * m.e02 + r21 * m.e12 + r22 * m.e22;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
} else {
e00 = m.e00 * r00 + m.e01 * r10 + m.e02 * r20;
e01 = m.e00 * r01 + m.e01 * r11 + m.e02 * r21;
e02 = m.e00 * r02 + m.e01 * r12 + m.e02 * r22;
e10 = m.e10 * r00 + m.e11 * r10 + m.e12 * r20;
e11 = m.e10 * r01 + m.e11 * r11 + m.e12 * r21;
e12 = m.e10 * r02 + m.e11 * r12 + m.e12 * r22;
e20 = m.e20 * r00 + m.e21 * r10 + m.e22 * r20;
e21 = m.e20 * r01 + m.e21 * r11 + m.e22 * r21;
e22 = m.e20 * r02 + m.e21 * r12 + m.e22 * r22;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
}
return this;
}
public function transpose(m:Mat33):Mat33 {
var e01:Number = m.e10;
var e02:Number = m.e20;
var e10:Number = m.e01;
var e12:Number = m.e21;
var e20:Number = m.e02;
var e21:Number = m.e12;
e00 = m.e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
e11 = m.e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
e22 = m.e22;
return this;
}
public function setQuat(q:Quat):Mat33 {
var x2:Number = 2 * q.x;
var y2:Number = 2 * q.y;
var z2:Number = 2 * q.z;
var xx:Number = q.x * x2;
var yy:Number = q.y * y2;
var zz:Number = q.z * z2;
var xy:Number = q.x * y2;
var yz:Number = q.y * z2;
var xz:Number = q.x * z2;
var sx:Number = q.s * x2;
var sy:Number = q.s * y2;
var sz:Number = q.s * z2;
e00 = 1 - yy - zz;
e01 = xy - sz;
e02 = xz + sy;
e10 = xy + sz;
e11 = 1 - xx - zz;
e12 = yz - sx;
e20 = xz - sy;
e21 = yz + sx;
e22 = 1 - xx - yy;
return this;
}
public function invert(m:Mat33):Mat33 {
var det:Number =
m.e00 * (m.e11 * m.e22 - m.e21 * m.e12) +
m.e10 * (m.e21 * m.e02 - m.e01 * m.e22) +
m.e20 * (m.e01 * m.e12 - m.e11 * m.e02)
;
if (det != 0) det = 1 / det;
var t00:Number = m.e11 * m.e22 - m.e12 * m.e21;
var t01:Number = m.e02 * m.e21 - m.e01 * m.e22;
var t02:Number = m.e01 * m.e12 - m.e02 * m.e11;
var t10:Number = m.e12 * m.e20 - m.e10 * m.e22;
var t11:Number = m.e00 * m.e22 - m.e02 * m.e20;
var t12:Number = m.e02 * m.e10 - m.e00 * m.e12;
var t20:Number = m.e10 * m.e21 - m.e11 * m.e20;
var t21:Number = m.e01 * m.e20 - m.e00 * m.e21;
var t22:Number = m.e00 * m.e11 - m.e01 * m.e10;
e00 = det * t00;
e01 = det * t01;
e02 = det * t02;
e10 = det * t10;
e11 = det * t11;
e12 = det * t12;
e20 = det * t20;
e21 = det * t21;
e22 = det * t22;
return this;
}
public function copy(m:Mat33):Mat33 {
e00 = m.e00;
e01 = m.e01;
e02 = m.e02;
e10 = m.e10;
e11 = m.e11;
e12 = m.e12;
e20 = m.e20;
e21 = m.e21;
e22 = m.e22;
return this;
}
public function copyMat44(m:Mat44):Mat33 {
e00 = m.e00;
e01 = m.e01;
e02 = m.e02;
e10 = m.e10;
e11 = m.e11;
e12 = m.e12;
e20 = m.e20;
e21 = m.e21;
e22 = m.e22;
return this;
}
public function clone():Mat33 {
return new Mat33(e00, e01, e02, e10, e11, e12, e20, e21, e22);
}
public function toString():String {
var text:String =
"Mat33|" + e00.toFixed(4) + ", " + e01.toFixed(4) + ", " + e02.toFixed(4) + "|\n" +
" |" + e10.toFixed(4) + ", " + e11.toFixed(4) + ", " + e12.toFixed(4) + "|\n" +
" |" + e20.toFixed(4) + ", " + e21.toFixed(4) + ", " + e22.toFixed(4) + "|"
;
return text;
}
}
class Mat44 {
public var e00:Number;
public var e01:Number;
public var e02:Number;
public var e03:Number;
public var e10:Number;
public var e11:Number;
public var e12:Number;
public var e13:Number;
public var e20:Number;
public var e21:Number;
public var e22:Number;
public var e23:Number;
public var e30:Number;
public var e31:Number;
public var e32:Number;
public var e33:Number;
public function Mat44(
e00:Number = 1, e01:Number = 0, e02:Number = 0, e03:Number = 0,
e10:Number = 0, e11:Number = 1, e12:Number = 0, e13:Number = 0,
e20:Number = 0, e21:Number = 0, e22:Number = 1, e23:Number = 0,
e30:Number = 0, e31:Number = 0, e32:Number = 0, e33:Number = 1
) {
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
}
public function init(
e00:Number = 1, e01:Number = 0, e02:Number = 0, e03:Number = 0,
e10:Number = 0, e11:Number = 1, e12:Number = 0, e13:Number = 0,
e20:Number = 0, e21:Number = 0, e22:Number = 1, e23:Number = 0,
e30:Number = 0, e31:Number = 0, e32:Number = 0, e33:Number = 1
):Mat44 {
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
return this;
}
public function add(m1:Mat44, m2:Mat44):Mat44 {
e00 = m1.e00 + m2.e00;
e01 = m1.e01 + m2.e01;
e02 = m1.e02 + m2.e02;
e03 = m1.e03 + m2.e03;
e10 = m1.e10 + m2.e10;
e11 = m1.e11 + m2.e11;
e12 = m1.e12 + m2.e12;
e13 = m1.e13 + m2.e13;
e20 = m1.e20 + m2.e20;
e21 = m1.e21 + m2.e21;
e22 = m1.e22 + m2.e22;
e23 = m1.e23 + m2.e23;
e30 = m1.e30 + m2.e30;
e31 = m1.e31 + m2.e31;
e32 = m1.e32 + m2.e32;
e33 = m1.e33 + m2.e33;
return this;
}
public function sub(m1:Mat44, m2:Mat44):Mat44 {
e00 = m1.e00 - m2.e00;
e01 = m1.e01 - m2.e01;
e02 = m1.e02 - m2.e02;
e03 = m1.e03 - m2.e03;
e10 = m1.e10 - m2.e10;
e11 = m1.e11 - m2.e11;
e12 = m1.e12 - m2.e12;
e13 = m1.e13 - m2.e13;
e20 = m1.e20 - m2.e20;
e21 = m1.e21 - m2.e21;
e22 = m1.e22 - m2.e22;
e23 = m1.e23 - m2.e23;
e30 = m1.e30 - m2.e30;
e31 = m1.e31 - m2.e31;
e32 = m1.e32 - m2.e32;
e33 = m1.e33 - m2.e33;
return this;
}
public function scale(m:Mat44, s:Number):Mat44 {
e00 = m.e00 * s;
e01 = m.e01 * s;
e02 = m.e02 * s;
e03 = m.e03 * s;
e10 = m.e10 * s;
e11 = m.e11 * s;
e12 = m.e12 * s;
e13 = m.e13 * s;
e20 = m.e20 * s;
e21 = m.e21 * s;
e22 = m.e22 * s;
e23 = m.e23 * s;
e30 = m.e30 * s;
e31 = m.e31 * s;
e32 = m.e32 * s;
e33 = m.e33 * s;
return this;
}
public function mul(m1:Mat44, m2:Mat44):Mat44 {
var e00:Number = m1.e00 * m2.e00 + m1.e01 * m2.e10 + m1.e02 * m2.e20 + m1.e03 * m2.e30;
var e01:Number = m1.e00 * m2.e01 + m1.e01 * m2.e11 + m1.e02 * m2.e21 + m1.e03 * m2.e31;
var e02:Number = m1.e00 * m2.e02 + m1.e01 * m2.e12 + m1.e02 * m2.e22 + m1.e03 * m2.e32;
var e03:Number = m1.e00 * m2.e03 + m1.e01 * m2.e13 + m1.e02 * m2.e23 + m1.e03 * m2.e33;
var e10:Number = m1.e10 * m2.e00 + m1.e11 * m2.e10 + m1.e12 * m2.e20 + m1.e13 * m2.e30;
var e11:Number = m1.e10 * m2.e01 + m1.e11 * m2.e11 + m1.e12 * m2.e21 + m1.e13 * m2.e31;
var e12:Number = m1.e10 * m2.e02 + m1.e11 * m2.e12 + m1.e12 * m2.e22 + m1.e13 * m2.e32;
var e13:Number = m1.e10 * m2.e03 + m1.e11 * m2.e13 + m1.e12 * m2.e23 + m1.e13 * m2.e33;
var e20:Number = m1.e20 * m2.e00 + m1.e21 * m2.e10 + m1.e22 * m2.e20 + m1.e23 * m2.e30;
var e21:Number = m1.e20 * m2.e01 + m1.e21 * m2.e11 + m1.e22 * m2.e21 + m1.e23 * m2.e31;
var e22:Number = m1.e20 * m2.e02 + m1.e21 * m2.e12 + m1.e22 * m2.e22 + m1.e23 * m2.e32;
var e23:Number = m1.e20 * m2.e03 + m1.e21 * m2.e13 + m1.e22 * m2.e23 + m1.e23 * m2.e33;
var e30:Number = m1.e30 * m2.e00 + m1.e31 * m2.e10 + m1.e32 * m2.e20 + m1.e33 * m2.e30;
var e31:Number = m1.e30 * m2.e01 + m1.e31 * m2.e11 + m1.e32 * m2.e21 + m1.e33 * m2.e31;
var e32:Number = m1.e30 * m2.e02 + m1.e31 * m2.e12 + m1.e32 * m2.e22 + m1.e33 * m2.e32;
var e33:Number = m1.e30 * m2.e03 + m1.e31 * m2.e13 + m1.e32 * m2.e23 + m1.e33 * m2.e33;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
return this;
}
public function mulScale(m:Mat44, sx:Number, sy:Number, sz:Number, prepend:Boolean = false):Mat44 {
var e00:Number;
var e01:Number;
var e02:Number;
var e03:Number;
var e10:Number;
var e11:Number;
var e12:Number;
var e13:Number;
var e20:Number;
var e21:Number;
var e22:Number;
var e23:Number;
var e30:Number;
var e31:Number;
var e32:Number;
var e33:Number;
if (prepend) {
e00 = sx * m.e00;
e01 = sx * m.e01;
e02 = sx * m.e02;
e03 = sx * m.e03;
e10 = sy * m.e10;
e11 = sy * m.e11;
e12 = sy * m.e12;
e13 = sy * m.e13;
e20 = sz * m.e20;
e21 = sz * m.e21;
e22 = sz * m.e22;
e23 = sz * m.e23;
e30 = m.e30;
e31 = m.e31;
e32 = m.e32;
e33 = m.e33;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
} else {
e00 = m.e00 * sx;
e01 = m.e01 * sy;
e02 = m.e02 * sz;
e03 = m.e03;
e10 = m.e10 * sx;
e11 = m.e11 * sy;
e12 = m.e12 * sz;
e13 = m.e13;
e20 = m.e20 * sx;
e21 = m.e21 * sy;
e22 = m.e22 * sz;
e23 = m.e23;
e30 = m.e30 * sx;
e31 = m.e31 * sy;
e32 = m.e32 * sz;
e33 = m.e33;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
}
return this;
}
public function mulRotate(m:Mat44, rad:Number, ax:Number, ay:Number, az:Number, prepend:Boolean = false):Mat44 {
var s:Number = Math.sin(rad);
var c:Number = Math.cos(rad);
var c1:Number = 1 - c;
var r00:Number = ax * ax * c1 + c;
var r01:Number = ax * ay * c1 - az * s;
var r02:Number = ax * az * c1 + ay * s;
var r10:Number = ay * ax * c1 + az * s;
var r11:Number = ay * ay * c1 + c;
var r12:Number = ay * az * c1 - ax * s;
var r20:Number = az * ax * c1 - ay * s;
var r21:Number = az * ay * c1 + ax * s;
var r22:Number = az * az * c1 + c;
var e00:Number;
var e01:Number;
var e02:Number;
var e03:Number;
var e10:Number;
var e11:Number;
var e12:Number;
var e13:Number;
var e20:Number;
var e21:Number;
var e22:Number;
var e23:Number;
var e30:Number;
var e31:Number;
var e32:Number;
var e33:Number;
if (prepend) {
e00 = r00 * m.e00 + r01 * m.e10 + r02 * m.e20;
e01 = r00 * m.e01 + r01 * m.e11 + r02 * m.e21;
e02 = r00 * m.e02 + r01 * m.e12 + r02 * m.e22;
e03 = r00 * m.e03 + r01 * m.e13 + r02 * m.e23;
e10 = r10 * m.e00 + r11 * m.e10 + r12 * m.e20;
e11 = r10 * m.e01 + r11 * m.e11 + r12 * m.e21;
e12 = r10 * m.e02 + r11 * m.e12 + r12 * m.e22;
e13 = r10 * m.e03 + r11 * m.e13 + r12 * m.e23;
e20 = r20 * m.e00 + r21 * m.e10 + r22 * m.e20;
e21 = r20 * m.e01 + r21 * m.e11 + r22 * m.e21;
e22 = r20 * m.e02 + r21 * m.e12 + r22 * m.e22;
e23 = r20 * m.e03 + r21 * m.e13 + r22 * m.e23;
e30 = m.e30;
e31 = m.e31;
e32 = m.e32;
e33 = m.e33;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
} else {
e00 = m.e00 * r00 + m.e01 * r10 + m.e02 * r20;
e01 = m.e00 * r01 + m.e01 * r11 + m.e02 * r21;
e02 = m.e00 * r02 + m.e01 * r12 + m.e02 * r22;
e03 = m.e03;
e10 = m.e10 * r00 + m.e11 * r10 + m.e12 * r20;
e11 = m.e10 * r01 + m.e11 * r11 + m.e12 * r21;
e12 = m.e10 * r02 + m.e11 * r12 + m.e12 * r22;
e13 = m.e13;
e20 = m.e20 * r00 + m.e21 * r10 + m.e22 * r20;
e21 = m.e20 * r01 + m.e21 * r11 + m.e22 * r21;
e22 = m.e20 * r02 + m.e21 * r12 + m.e22 * r22;
e23 = m.e23;
e30 = m.e30 * r00 + m.e31 * r10 + m.e32 * r20;
e31 = m.e30 * r01 + m.e31 * r11 + m.e32 * r21;
e32 = m.e30 * r02 + m.e31 * r12 + m.e32 * r22;
e33 = m.e33;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
}
return this;
}
public function mulTranslate(m:Mat44, tx:Number, ty:Number, tz:Number, prepend:Boolean = false):Mat44 {
var e00:Number;
var e01:Number;
var e02:Number;
var e03:Number;
var e10:Number;
var e11:Number;
var e12:Number;
var e13:Number;
var e20:Number;
var e21:Number;
var e22:Number;
var e23:Number;
var e30:Number;
var e31:Number;
var e32:Number;
var e33:Number;
if (prepend) {
e00 = m.e00 + tx * m.e30;
e01 = m.e01 + tx * m.e31;
e02 = m.e02 + tx * m.e32;
e03 = m.e03 + tx * m.e33;
e10 = m.e10 + ty * m.e30;
e11 = m.e11 + ty * m.e31;
e12 = m.e12 + ty * m.e32;
e13 = m.e13 + ty * m.e33;
e20 = m.e20 + tz * m.e30;
e21 = m.e21 + tz * m.e31;
e22 = m.e22 + tz * m.e32;
e23 = m.e23 + tz * m.e33;
e30 = m.e30;
e31 = m.e31;
e32 = m.e32;
e33 = m.e33;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
} else {
e00 = m.e00;
e01 = m.e01;
e02 = m.e02;
e03 = m.e00 * tx + m.e01 * ty + m.e02 * tz + m.e03;
e10 = m.e10;
e11 = m.e11;
e12 = m.e12;
e13 = m.e10 * tx + m.e11 * ty + m.e12 * tz + m.e13;
e20 = m.e20;
e21 = m.e21;
e22 = m.e22;
e23 = m.e20 * tx + m.e21 * ty + m.e22 * tz + m.e23;
e30 = m.e30;
e31 = m.e31;
e32 = m.e32;
e33 = m.e30 * tx + m.e31 * ty + m.e32 * tz + m.e33;
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
}
return this;
}
public function transpose(m:Mat44):Mat44 {
var e01:Number = m.e10;
var e02:Number = m.e20;
var e03:Number = m.e30;
var e10:Number = m.e01;
var e12:Number = m.e21;
var e13:Number = m.e31;
var e20:Number = m.e02;
var e21:Number = m.e12;
var e23:Number = m.e32;
var e30:Number = m.e03;
var e31:Number = m.e13;
var e32:Number = m.e23;
e00 = m.e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
e11 = m.e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
e22 = m.e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
e33 = m.e33;
return this;
}
public function setQuat(q:Quat):Mat44 {
var x2:Number = 2 * q.x;
var y2:Number = 2 * q.y;
var z2:Number = 2 * q.z;
var xx:Number = q.x * x2;
var yy:Number = q.y * y2;
var zz:Number = q.z * z2;
var xy:Number = q.x * y2;
var yz:Number = q.y * z2;
var xz:Number = q.x * z2;
var sx:Number = q.s * x2;
var sy:Number = q.s * y2;
var sz:Number = q.s * z2;
e00 = 1 - yy - zz;
e01 = xy - sz;
e02 = xz + sy;
e03 = 0;
e10 = xy + sz;
e11 = 1 - xx - zz;
e12 = yz - sx;
e13 = 0;
e20 = xz - sy;
e21 = yz + sx;
e22 = 1 - xx - yy;
e23 = 0;
e30 = 0;
e31 = 0;
e32 = 0;
e33 = 1;
return this;
}
public function invert(m:Mat44):Mat44 {
var e1021_1120:Number = m.e10 * m.e21 - m.e11 * m.e20;
var e1022_1220:Number = m.e10 * m.e22 - m.e12 * m.e20;
var e1023_1320:Number = m.e10 * m.e23 - m.e13 * m.e20;
var e1031_1130:Number = m.e10 * m.e31 - m.e11 * m.e30;
var e1032_1230:Number = m.e10 * m.e32 - m.e12 * m.e30;
var e1033_1330:Number = m.e10 * m.e33 - m.e13 * m.e30;
var e1122_1221:Number = m.e11 * m.e22 - m.e12 * m.e21;
var e1123_1321:Number = m.e11 * m.e23 - m.e13 * m.e21;
var e1132_1231:Number = m.e11 * m.e32 - m.e12 * m.e31;
var e1133_1331:Number = m.e11 * m.e33 - m.e13 * m.e31;
var e1220_2022:Number = m.e12 * m.e20 - m.e20 * m.e22;
var e1223_1322:Number = m.e12 * m.e23 - m.e13 * m.e22;
var e1223_2223:Number = m.e12 * m.e33 - m.e22 * m.e23;
var e1233_1332:Number = m.e12 * m.e33 - m.e13 * m.e32;
var e2031_2130:Number = m.e20 * m.e31 - m.e21 * m.e30;
var e2032_2033:Number = m.e20 * m.e32 - m.e20 * m.e33;
var e2032_2230:Number = m.e20 * m.e32 - m.e22 * m.e30;
var e2033_2330:Number = m.e20 * m.e33 - m.e23 * m.e30;
var e2132_2231:Number = m.e21 * m.e32 - m.e22 * m.e31;
var e2133_2331:Number = m.e21 * m.e33 - m.e23 * m.e31;
var e2230_2330:Number = m.e22 * m.e30 - m.e23 * m.e30;
var e2233_2332:Number = m.e22 * m.e33 - m.e23 * m.e32;
var det:Number =
m.e00 * (m.e11 * e2233_2332 - m.e12 * e2133_2331 + m.e13 * e2132_2231) +
m.e01 * (-m.e10 * e2233_2332 - m.e12 * e2032_2033 + m.e13 * e2230_2330) +
m.e02 * (m.e10 * e2133_2331 - m.e11 * e2033_2330 + m.e13 * e2031_2130) +
m.e03 * (-m.e10 * e2132_2231 + m.e11 * e2032_2230 - m.e12 * e2031_2130)
;
if (det != 0) det = 1 / det;
var t00:Number = m.e11 * e2233_2332 - m.e12 * e2133_2331 + m.e13 * e2132_2231;
var t01:Number = -m.e01 * e2233_2332 + m.e02 * e2133_2331 - m.e03 * e2132_2231;
var t02:Number = m.e01 * e1233_1332 - m.e02 * e1133_1331 + m.e03 * e1132_1231;
var t03:Number = -m.e01 * e1223_2223 + m.e02 * e1123_1321 - m.e03 * e1122_1221;
var t10:Number = -m.e10 * e2233_2332 + m.e12 * e2033_2330 - m.e13 * e2032_2230;
var t11:Number = m.e00 * e2233_2332 - m.e02 * e2033_2330 + m.e03 * e2032_2230;
var t12:Number = -m.e00 * e1233_1332 + m.e02 * e1033_1330 - m.e03 * e1032_1230;
var t13:Number = m.e00 * e1223_1322 - m.e02 * e1023_1320 - m.e03 * e1220_2022;
var t20:Number = m.e10 * e2133_2331 - m.e11 * e2033_2330 + m.e13 * e2031_2130;
var t21:Number = -m.e00 * e2133_2331 + m.e01 * e2033_2330 - m.e03 * e2031_2130;
var t22:Number = m.e00 * e1133_1331 - m.e01 * e1033_1330 + m.e03 * e1031_1130;
var t23:Number = -m.e00 * e1123_1321 + m.e01 * e1023_1320 - m.e03 * e1021_1120;
var t30:Number = -m.e10 * e2132_2231 + m.e11 * e2032_2230 - m.e12 * e2031_2130;
var t31:Number = m.e00 * e2132_2231 - m.e01 * e2032_2230 + m.e02 * e2031_2130;
var t32:Number = -m.e00 * e1132_1231 + m.e01 * e1032_1230 - m.e02 * e1031_1130;
var t33:Number = m.e00 * e1122_1221 - m.e01 * e1022_1220 + m.e02 * e1021_1120;
e00 = det * t00;
e01 = det * t01;
e02 = det * t02;
e03 = det * t03;
e10 = det * t10;
e11 = det * t11;
e12 = det * t12;
e13 = det * t13;
e20 = det * t20;
e21 = det * t21;
e22 = det * t22;
e23 = det * t23;
e30 = det * t30;
e31 = det * t31;
e32 = det * t32;
e33 = det * t33;
return this;
}
public function lookAt(
eyeX:Number, eyeY:Number, eyeZ:Number,
atX:Number, atY:Number, atZ:Number,
upX:Number, upY:Number, upZ:Number
):Mat44 {
var zx:Number = eyeX - atX;
var zy:Number = eyeY - atY;
var zz:Number = eyeZ - atZ;
var tmp:Number = 1 / Math.sqrt(zx * zx + zy * zy + zz * zz);
zx *= tmp;
zy *= tmp;
zz *= tmp;
var xx:Number = upY * zz - upZ * zy;
var xy:Number = upZ * zx - upX * zz;
var xz:Number = upX * zy - upY * zx;
tmp = 1 / Math.sqrt(xx * xx + xy * xy + xz * xz);
xx *= tmp;
xy *= tmp;
xz *= tmp;
var yx:Number = zy * xz - zz * xy;
var yy:Number = zz * xx - zx * xz;
var yz:Number = zx * xy - zy * xx;
e00 = xx;
e01 = xy;
e02 = xz;
e03 = -(xx * eyeX + xy * eyeY + xz * eyeZ);
e10 = yx;
e11 = yy;
e12 = yz;
e13 = -(yx * eyeX + yy * eyeY + yz * eyeZ);
e20 = zx;
e21 = zy;
e22 = zz;
e23 = -(zx * eyeX + zy * eyeY + zz * eyeZ);
e30 = 0;
e31 = 0;
e32 = 0;
e33 = 1;
return this;
}
public function perspective(fovY:Number, aspect:Number, near:Number, far:Number):Mat44 {
var h:Number = 1 / Math.tan(fovY * 0.5);
var fnf:Number = far / (near - far);
e00 = h / aspect;
e01 = 0;
e02 = 0;
e03 = 0;
e10 = 0;
e11 = h;
e12 = 0;
e13 = 0;
e20 = 0;
e21 = 0;
e22 = fnf;
e23 = near * fnf;
e30 = 0;
e31 = 0;
e32 = -1;
e33 = 0;
return this;
}
public function ortho(width:Number, height:Number, near:Number, far:Number):Mat44 {
var nf:Number = 1 / (near - far);
e00 = 2 / width;
e01 = 0;
e02 = 0;
e03 = 0;
e10 = 0;
e11 = 2 / height;
e12 = 0;
e13 = 0;
e20 = 0;
e21 = 0;
e22 = nf;
e23 = near * nf;
e30 = 0;
e31 = 0;
e32 = 0;
e33 = 0;
return this;
}
public function copy(m:Mat44):Mat44 {
e00 = m.e00;
e01 = m.e01;
e02 = m.e02;
e03 = m.e03;
e10 = m.e10;
e11 = m.e11;
e12 = m.e12;
e13 = m.e13;
e20 = m.e20;
e21 = m.e21;
e22 = m.e22;
e23 = m.e23;
e30 = m.e30;
e31 = m.e31;
e32 = m.e32;
e33 = m.e33;
return this;
}
public function copyMat33(m:Mat33):Mat44 {
e00 = m.e00;
e01 = m.e01;
e02 = m.e02;
e03 = 0;
e10 = m.e10;
e11 = m.e11;
e12 = m.e12;
e13 = 0;
e20 = m.e20;
e21 = m.e21;
e22 = m.e22;
e23 = 0;
e30 = 0;
e31 = 0;
e32 = 0;
e33 = 1;
return this;
}
public function clone():Mat44 {
return new Mat44(
e00, e01, e02, e03,
e10, e11, e12, e13,
e20, e21, e22, e23,
e30, e31, e32, e33
);
}
public function toString():String {
var text:String =
"Mat44|" + e00.toFixed(4) + ", " + e01.toFixed(4) + ", " + e02.toFixed(4) + ", " + e03.toFixed(4) + "|\n" +
" |" + e10.toFixed(4) + ", " + e11.toFixed(4) + ", " + e12.toFixed(4) + ", " + e13.toFixed(4) + "|\n" +
" |" + e20.toFixed(4) + ", " + e21.toFixed(4) + ", " + e22.toFixed(4) + ", " + e23.toFixed(4) + "|\n" +
" |" + e30.toFixed(4) + ", " + e31.toFixed(4) + ", " + e32.toFixed(4) + ", " + e33.toFixed(4) + "|\n"
;
return text;
}
}
class Quat {
public var s:Number;
public var x:Number;
public var y:Number;
public var z:Number;
public function Quat(s:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) {
this.s = s;
this.x = x;
this.y = y;
this.z = z;
}
public function init(s:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quat {
this.s = s;
this.x = x;
this.y = y;
this.z = z;
return this;
}
public function add(q1:Quat, q2:Quat):Quat {
s = q1.s + q2.s;
x = q1.x + q2.x;
y = q1.y + q2.y;
z = q1.z + q2.z;
return this;
}
public function sub(q1:Quat, q2:Quat):Quat {
s = q1.s - q2.s;
x = q1.x - q2.x;
y = q1.y - q2.y;
z = q1.z - q2.z;
return this;
}
public function scale(q:Quat, s:Number):Quat {
this.s = q.s * s;
x = q.x * s;
y = q.y * s;
z = q.z * s;
return this;
}
public function mul(q1:Quat, q2:Quat):Quat {
var s:Number = q1.s * q2.s - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
var x:Number = q1.s * q2.x + q1.x * q2.s + q1.y * q2.z - q1.z * q2.y;
var y:Number = q1.s * q2.y - q1.x * q2.z + q1.y * q2.s + q1.z * q2.x;
var z:Number = q1.s * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.s;
this.s = s;
this.x = x;
this.y = y;
this.z = z;
return this;
}
public function normalize(q:Quat):Quat {
var len:Number = Math.sqrt(q.s * q.s + q.x * q.x + q.y * q.y + q.z * q.z);
if (len > 0) len = 1 / len;
s = q.s * len;
x = q.x * len;
y = q.y * len;
z = q.z * len;
return this;
}
public function invert(q:Quat):Quat {
s = -q.s;
x = -q.x;
y = -q.y;
z = -q.z;
return this;
}
public function length():Number {
return Math.sqrt(s * s + x * x + y * y + z * z);
}
public function copy(q:Quat):Quat {
s = q.s;
x = q.x;
y = q.y;
z = q.z;
return this;
}
public function clone(q:Quat):Quat {
return new Quat(s, x, y, z);
}
public function toString():String {
return "Quat[" + s.toFixed(4) + ", (" + x.toFixed(4) + ", " + y.toFixed(4) + ", " + z.toFixed(4) + ")]";
}
}
class Vec3 {
public var x:Number;
public var y:Number;
public var z:Number;
public function Vec3(x:Number = 0, y:Number = 0, z:Number = 0) {
this.x = x;
this.y = y;
this.z = z;
}
public function init(x:Number = 0, y:Number = 0, z:Number = 0):Vec3 {
this.x = x;
this.y = y;
this.z = z;
return this;
}
public function add(v1:Vec3, v2:Vec3):Vec3 {
x = v1.x + v2.x;
y = v1.y + v2.y;
z = v1.z + v2.z;
return this;
}
public function sub(v1:Vec3, v2:Vec3):Vec3 {
x = v1.x - v2.x;
y = v1.y - v2.y;
z = v1.z - v2.z;
return this;
}
public function scale(v:Vec3, s:Number):Vec3 {
x = v.x * s;
y = v.y * s;
z = v.z * s;
return this;
}
public function dot(v:Vec3):Number {
return x * v.x + y * v.y + z * v.z;
}
public function cross(v1:Vec3, v2:Vec3):Vec3 {
var x:Number = v1.y * v2.z - v1.z * v2.y;
var y:Number = v1.z * v2.x - v1.x * v2.z;
var z:Number = v1.x * v2.y - v1.y * v2.x;
this.x = x;
this.y = y;
this.z = z;
return this;
}
public function mulMat(m:Mat33, v:Vec3):Vec3 {
var x:Number = m.e00 * v.x + m.e01 * v.y + m.e02 * v.z;
var y:Number = m.e10 * v.x + m.e11 * v.y + m.e12 * v.z;
var z:Number = m.e20 * v.x + m.e21 * v.y + m.e22 * v.z;
this.x = x;
this.y = y;
this.z = z;
return this;
}
public function normalize(v:Vec3):Vec3 {
var length:Number = Math.sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
if (length > 0) length = 1 / length;
x = v.x * length;
y = v.y * length;
z = v.z * length;
return this;
}
public function invert(v:Vec3):Vec3 {
x = -v.x;
y = -v.y;
z = -v.z;
return this;
}
public function length():Number {
return Math.sqrt(x * x + y * y + z * z);
}
public function copy(v:Vec3):Vec3 {
x = v.x;
y = v.y;
z = v.z;
return this;
}
public function clone():Vec3 {
return new Vec3(x, y, z);
}
public function toString():String {
return "Vec3[" + x.toFixed(4) + ", " + y.toFixed(4) + ", " + z.toFixed(4) + "]";
}
}
class BroadPhase {
public var numPairChecks:uint;
public function BroadPhase() {
}
public function addProxy(proxy:Proxy):void {
throw new Error("addProxy 関数が継承されていません");
}
public function removeProxy(proxy:Proxy):void {
throw new Error("removeProxy 関数が継承されていません");
}
public function detectPairs(pairs:Vector.<Pair>):uint {
throw new Error("collectPairs 関数が継承されていません");
}
}
class Pair {
public var shape1:Shape;
public var shape2:Shape;
public function Pair() {
}
}
class Proxy {
public var minX:Number;
public var maxX:Number;
public var minY:Number;
public var maxY:Number;
public var minZ:Number;
public var maxZ:Number;
public var parent:Shape;
public function Proxy(
minX:Number = 0, maxX:Number = 0,
minY:Number = 0, maxY:Number = 0,
minZ:Number = 0, maxZ:Number = 0
) {
this.minX = minX;
this.maxX = maxX;
this.minY = minY;
this.maxY = maxY;
this.minZ = minZ;
this.maxZ = maxZ;
}
public function init(
minX:Number = 0, maxX:Number = 0,
minY:Number = 0, maxY:Number = 0,
minZ:Number = 0, maxZ:Number = 0
):void {
this.minX = minX;
this.maxX = maxX;
this.minY = minY;
this.maxY = maxY;
this.minZ = minZ;
this.maxZ = maxZ;
}
public function intersect(proxy:Proxy):Boolean {
return maxX > proxy.minX && minX < proxy.maxX && maxY > proxy.minY && minY < proxy.maxY && maxZ > proxy.minZ && minZ < proxy.maxZ;
}
}
class SweepAndPruneBroadPhase extends BroadPhase {
private var proxyPoolAxis:Vector.<Vector.<Proxy>>;
private var sortAxis:uint;
private var numProxies:uint;
public function SweepAndPruneBroadPhase() {
sortAxis = 0;
proxyPoolAxis = new Vector.<Vector.<Proxy>>(3, true);
proxyPoolAxis[0] = new Vector.<Proxy>(World.MAX_SHAPES, true);
proxyPoolAxis[1] = new Vector.<Proxy>(World.MAX_SHAPES, true);
proxyPoolAxis[2] = new Vector.<Proxy>(World.MAX_SHAPES, true);
}
override public function addProxy(proxy:Proxy):void {
proxyPoolAxis[0][numProxies] = proxy;
proxyPoolAxis[1][numProxies] = proxy;
proxyPoolAxis[2][numProxies] = proxy;
numProxies++;
}
override public function removeProxy(proxy:Proxy):void {
removeProxyAxis(proxy, proxyPoolAxis[0]);
removeProxyAxis(proxy, proxyPoolAxis[1]);
removeProxyAxis(proxy, proxyPoolAxis[2]);
numProxies--;
}
override public function detectPairs(pairs:Vector.<Pair>):uint {
numPairChecks = 0;
var proxyPool:Vector.<Proxy> = proxyPoolAxis[sortAxis];
var result:uint;
if (sortAxis == 0) {
insertionSortX(proxyPool);
return sweepX(pairs, proxyPool);
} else if (sortAxis == 1) {
insertionSortY(proxyPool);
return sweepY(pairs, proxyPool);
} else {
insertionSortZ(proxyPool);
return sweepZ(pairs, proxyPool);
}
}
private function sweepX(pairs:Vector.<Pair>, proxyPool:Vector.<Proxy>):uint {
var numPairs:uint = 0;
var center:Number;
var sumX:Number = 0;
var sumX2:Number = 0;
var sumY:Number = 0;
var sumY2:Number = 0;
var sumZ:Number = 0;
var sumZ2:Number = 0;
var invNum:Number = 1 / numProxies;
var bodyStatic:uint = RigidBody.BODY_STATIC;
var maxPairs:uint = World.MAX_PAIRS;
for (var i:int = 0; i < numProxies; i++) {
var p1:Proxy = proxyPool[i];
center = p1.minX + p1.maxX;
sumX += center;
sumX2 += center * center;
center = p1.minY + p1.maxY;
sumY += center;
sumY2 += center * center;
center = p1.minZ + p1.maxZ;
sumZ += center;
sumZ2 += center * center;
var s1:Shape = p1.parent;
var b1:RigidBody = s1.parent;
for (var j:int = i + 1; j < numProxies; j++) {
var p2:Proxy = proxyPool[j];
numPairChecks++;
if (p1.maxX < p2.minX) {
break;
}
var s2:Shape = p2.parent;
var b2:RigidBody = s2.parent;
if (
b1 == b2 ||
p1.maxY < p2.minY || p1.minY > p2.maxY ||
p1.maxZ < p2.minZ || p1.minZ > p2.maxZ ||
b1.type == bodyStatic && b2.type == bodyStatic
) {
continue;
}
if (numPairs >= maxPairs) {
return numPairs;
}
var pair:Pair = pairs[numPairs];
if (!pair) {
pair = new Pair();
pairs[numPairs] = pair;
}
pair.shape1 = s1;
pair.shape2 = s2;
numPairs++;
}
}
sumX = sumX2 - sumX * sumX * invNum;
sumY = sumY2 - sumY * sumY * invNum;
sumZ = sumZ2 - sumZ * sumZ * invNum;
if (sumX > sumY) {
if (sumX > sumZ) {
sortAxis = 0;
} else {
sortAxis = 2;
}
} else if (sumY > sumZ) {
sortAxis = 1;
} else {
sortAxis = 2;
}
return numPairs;
}
private function sweepY(pairs:Vector.<Pair>, proxyPool:Vector.<Proxy>):uint {
var numPairs:uint = 0;
var center:Number;
var sumX:Number = 0;
var sumX2:Number = 0;
var sumY:Number = 0;
var sumY2:Number = 0;
var sumZ:Number = 0;
var sumZ2:Number = 0;
var invNum:Number = 1 / numProxies;
var bodyStatic:uint = RigidBody.BODY_STATIC;
var maxPairs:uint = World.MAX_PAIRS;
for (var i:int = 0; i < numProxies; i++) {
var p1:Proxy = proxyPool[i];
center = p1.minX + p1.maxX;
sumX += center;
sumX2 += center * center;
center = p1.minY + p1.maxY;
sumY += center;
sumY2 += center * center;
center = p1.minZ + p1.maxZ;
sumZ += center;
sumZ2 += center * center;
var s1:Shape = p1.parent;
var b1:RigidBody = s1.parent;
for (var j:int = i + 1; j < numProxies; j++) {
var p2:Proxy = proxyPool[j];
numPairChecks++;
if (p1.maxY < p2.minY) {
break;
}
var s2:Shape = p2.parent;
var b2:RigidBody = s2.parent;
if (
b1 == b2 ||
p1.maxX < p2.minX || p1.minX > p2.maxX ||
p1.maxZ < p2.minZ || p1.minZ > p2.maxZ ||
b1.type == bodyStatic && b2.type == bodyStatic
) {
continue;
}
if (numPairs >= maxPairs) {
return numPairs;
}
var pair:Pair = pairs[numPairs];
if (!pair) {
pair = new Pair();
pairs[numPairs] = pair;
}
pair.shape1 = s1;
pair.shape2 = s2;
numPairs++;
}
}
sumX = sumX2 - sumX * sumX * invNum;
sumY = sumY2 - sumY * sumY * invNum;
sumZ = sumZ2 - sumZ * sumZ * invNum;
if (sumX > sumY) {
if (sumX > sumZ) {
sortAxis = 0;
} else {
sortAxis = 2;
}
} else if (sumY > sumZ) {
sortAxis = 1;
} else {
sortAxis = 2;
}
return numPairs;
}
private function sweepZ(pairs:Vector.<Pair>, proxyPool:Vector.<Proxy>):uint {
var numPairs:uint = 0;
var center:Number;
var sumX:Number = 0;
var sumX2:Number = 0;
var sumY:Number = 0;
var sumY2:Number = 0;
var sumZ:Number = 0;
var sumZ2:Number = 0;
var invNum:Number = 1 / numProxies;
var bodyStatic:uint = RigidBody.BODY_STATIC;
var maxPairs:uint = World.MAX_PAIRS;
for (var i:int = 0; i < numProxies; i++) {
var p1:Proxy = proxyPool[i];
center = p1.minX + p1.maxX;
sumX += center;
sumX2 += center * center;
center = p1.minY + p1.maxY;
sumY += center;
sumY2 += center * center;
center = p1.minZ + p1.maxZ;
sumZ += center;
sumZ2 += center * center;
var s1:Shape = p1.parent;
var b1:RigidBody = s1.parent;
for (var j:int = i + 1; j < numProxies; j++) {
var p2:Proxy = proxyPool[j];
numPairChecks++;
if (p1.maxZ < p2.minZ) {
break;
}
var s2:Shape = p2.parent;
var b2:RigidBody = s2.parent;
if (
b1 == b2 ||
p1.maxX < p2.minX || p1.minX > p2.maxX ||
p1.maxY < p2.minY || p1.minY > p2.maxY ||
b1.type == bodyStatic && b2.type == bodyStatic
) {
continue;
}
if (numPairs >= maxPairs) {
return numPairs;
}
var pair:Pair = pairs[numPairs];
if (!pair) {
pair = new Pair();
pairs[numPairs] = pair;
}
pair.shape1 = s1;
pair.shape2 = s2;
numPairs++;
}
}
sumX = sumX2 - sumX * sumX * invNum;
sumY = sumY2 - sumY * sumY * invNum;
sumZ = sumZ2 - sumZ * sumZ * invNum;
if (sumX > sumY) {
if (sumX > sumZ) {
sortAxis = 0;
} else {
sortAxis = 2;
}
} else if (sumY > sumZ) {
sortAxis = 1;
} else {
sortAxis = 2;
}
return numPairs;
}
private function removeProxyAxis(proxy:Proxy, proxyPool:Vector.<Proxy>):void {
var idx:int = -1;
for (var i:int = 0; i < numProxies; i++) {
if (proxyPool[i] == proxy) {
idx = i;
break;
}
}
if (idx == -1) {
return;
}
for (var j:int = idx; j < numProxies - 1; j++) {
proxyPool[j] = proxyPool[j + 1];
}
proxyPool[numProxies] = null;
}
private function insertionSortX(proxyPool:Vector.<Proxy>):void {
if (numProxies == 1)
return;
for (var i:int = 1; i < numProxies; i++) {
var insert:Proxy = proxyPool[i];
if (proxyPool[i - 1].minX > insert.minX) {
var j:int = i;
do {
proxyPool[j] = proxyPool[j - 1];
j--;
} while (j > 0 && proxyPool[j - 1].minX > insert.minX);
proxyPool[j] = insert;
}
}
}
private function insertionSortY(proxyPool:Vector.<Proxy>):void {
if (numProxies == 1)
return;
for (var i:int = 1; i < numProxies; i++) {
var insert:Proxy = proxyPool[i];
if (proxyPool[i - 1].minY > insert.minY) {
var j:int = i;
do {
proxyPool[j] = proxyPool[j - 1];
j--;
} while (j > 0 && proxyPool[j - 1].minY > insert.minY);
proxyPool[j] = insert;
}
}
}
private function insertionSortZ(proxyPool:Vector.<Proxy>):void {
if (numProxies == 1)
return;
for (var i:int = 1; i < numProxies; i++) {
var insert:Proxy = proxyPool[i];
if (proxyPool[i - 1].minZ > insert.minZ) {
var j:int = i;
do {
proxyPool[j] = proxyPool[j - 1];
j--;
} while (j > 0 && proxyPool[j - 1].minZ > insert.minZ);
proxyPool[j] = insert;
}
}
}
}
class CollisionDetector {
public var flip:Boolean;
public function CollisionDetector() {
}
public function detectCollision(shape1:Shape, shape2:Shape, contactInfos:Vector.<ContactInfo>, numContactInfos:uint):uint {
throw new Error("detectCollision 関数が継承されていません");
return -1;
}
}
class BoxBoxCollisionDetector extends CollisionDetector {
private var clipVertices1:Vector.<Number>;
private var clipVertices2:Vector.<Number>;
private var used:Vector.<Boolean>;
private const INF:Number = 1 / 0;
public function BoxBoxCollisionDetector() {
clipVertices1 = new Vector.<Number>(24, true); // 8 vertices x,y,z
clipVertices2 = new Vector.<Number>(24, true);
used = new Vector.<Boolean>(8, true);
}
override public function detectCollision(shape1:Shape, shape2:Shape, contactInfos:Vector.<ContactInfo>, numContactInfos:uint):uint {
if (numContactInfos == contactInfos.length) return numContactInfos;
var b1:BoxShape;
var b2:BoxShape;
if (shape1.id < shape2.id) {
b1 = shape1 as BoxShape;
b2 = shape2 as BoxShape;
} else {
b1 = shape2 as BoxShape;
b2 = shape1 as BoxShape;
}
var p1:Vec3 = b1.position;
var p2:Vec3 = b2.position;
var p1x:Number = p1.x;
var p1y:Number = p1.y;
var p1z:Number = p1.z;
var p2x:Number = p2.x;
var p2y:Number = p2.y;
var p2z:Number = p2.z;
// diff
var dx:Number = p2x - p1x;
var dy:Number = p2y - p1y;
var dz:Number = p2z - p1z;
// distance
var w1:Number = b1.halfWidth;
var h1:Number = b1.halfHeight;
var d1:Number = b1.halfDepth;
var w2:Number = b2.halfWidth;
var h2:Number = b2.halfHeight;
var d2:Number = b2.halfDepth;
// direction
var d1x:Number = b1.halfDirectionWidth.x;
var d1y:Number = b1.halfDirectionWidth.y;
var d1z:Number = b1.halfDirectionWidth.z;
// b1.y
var d2x:Number = b1.halfDirectionHeight.x;
var d2y:Number = b1.halfDirectionHeight.y;
var d2z:Number = b1.halfDirectionHeight.z;
// b1.z
var d3x:Number = b1.halfDirectionDepth.x;
var d3y:Number = b1.halfDirectionDepth.y;
var d3z:Number = b1.halfDirectionDepth.z;
// b2.x
var d4x:Number = b2.halfDirectionWidth.x;
var d4y:Number = b2.halfDirectionWidth.y;
var d4z:Number = b2.halfDirectionWidth.z;
// b2.y
var d5x:Number = b2.halfDirectionHeight.x;
var d5y:Number = b2.halfDirectionHeight.y;
var d5z:Number = b2.halfDirectionHeight.z;
// b2.z
var d6x:Number = b2.halfDirectionDepth.x;
var d6y:Number = b2.halfDirectionDepth.y;
var d6z:Number = b2.halfDirectionDepth.z;
// b1.x
var a1x:Number = b1.normalDirectionWidth.x;
var a1y:Number = b1.normalDirectionWidth.y;
var a1z:Number = b1.normalDirectionWidth.z;
// b1.y
var a2x:Number = b1.normalDirectionHeight.x;
var a2y:Number = b1.normalDirectionHeight.y;
var a2z:Number = b1.normalDirectionHeight.z;
// b1.z
var a3x:Number = b1.normalDirectionDepth.x;
var a3y:Number = b1.normalDirectionDepth.y;
var a3z:Number = b1.normalDirectionDepth.z;
// b2.x
var a4x:Number = b2.normalDirectionWidth.x;
var a4y:Number = b2.normalDirectionWidth.y;
var a4z:Number = b2.normalDirectionWidth.z;
// b2.y
var a5x:Number = b2.normalDirectionHeight.x;
var a5y:Number = b2.normalDirectionHeight.y;
var a5z:Number = b2.normalDirectionHeight.z;
// b2.z
var a6x:Number = b2.normalDirectionDepth.x;
var a6y:Number = b2.normalDirectionDepth.y;
var a6z:Number = b2.normalDirectionDepth.z;
// b1.x * b2.x
var a7x:Number = a1y * a4z - a1z * a4y;
var a7y:Number = a1z * a4x - a1x * a4z;
var a7z:Number = a1x * a4y - a1y * a4x;
// b1.x * b2.y
var a8x:Number = a1y * a5z - a1z * a5y;
var a8y:Number = a1z * a5x - a1x * a5z;
var a8z:Number = a1x * a5y - a1y * a5x;
// b1.x * b2.z
var a9x:Number = a1y * a6z - a1z * a6y;
var a9y:Number = a1z * a6x - a1x * a6z;
var a9z:Number = a1x * a6y - a1y * a6x;
// b1.y * b2.x
var aax:Number = a2y * a4z - a2z * a4y;
var aay:Number = a2z * a4x - a2x * a4z;
var aaz:Number = a2x * a4y - a2y * a4x;
// b1.y * b2.y
var abx:Number = a2y * a5z - a2z * a5y;
var aby:Number = a2z * a5x - a2x * a5z;
var abz:Number = a2x * a5y - a2y * a5x;
// b1.y * b2.z
var acx:Number = a2y * a6z - a2z * a6y;
var acy:Number = a2z * a6x - a2x * a6z;
var acz:Number = a2x * a6y - a2y * a6x;
// b1.z * b2.x
var adx:Number = a3y * a4z - a3z * a4y;
var ady:Number = a3z * a4x - a3x * a4z;
var adz:Number = a3x * a4y - a3y * a4x;
// b1.z * b2.y
var aex:Number = a3y * a5z - a3z * a5y;
var aey:Number = a3z * a5x - a3x * a5z;
var aez:Number = a3x * a5y - a3y * a5x;
// b1.z * b2.z
var afx:Number = a3y * a6z - a3z * a6y;
var afy:Number = a3z * a6x - a3x * a6z;
var afz:Number = a3x * a6y - a3y * a6x;
// right or left flag
var right1:Boolean;
var right2:Boolean;
var right3:Boolean;
var right4:Boolean;
var right5:Boolean;
var right6:Boolean;
var right7:Boolean;
var right8:Boolean;
var right9:Boolean;
var righta:Boolean;
var rightb:Boolean;
var rightc:Boolean;
var rightd:Boolean;
var righte:Boolean;
var rightf:Boolean;
// overlap distance
var overlap1:Number;
var overlap2:Number;
var overlap3:Number;
var overlap4:Number;
var overlap5:Number;
var overlap6:Number;
var overlap7:Number;
var overlap8:Number;
var overlap9:Number;
var overlapa:Number;
var overlapb:Number;
var overlapc:Number;
var overlapd:Number;
var overlape:Number;
var overlapf:Number;
// invalid flag
var invalid7:Boolean = false;
var invalid8:Boolean = false;
var invalid9:Boolean = false;
var invalida:Boolean = false;
var invalidb:Boolean = false;
var invalidc:Boolean = false;
var invalidd:Boolean = false;
var invalide:Boolean = false;
var invalidf:Boolean = false;
// temporary variables
var len:Number;
var len1:Number;
var len2:Number;
var dot1:Number;
var dot2:Number;
var dot3:Number;
// try axis 1
len = a1x * dx + a1y * dy + a1z * dz;
right1 = len > 0;
if (!right1) len = -len;
len1 = w1;
dot1 = a1x * a4x + a1y * a4y + a1z * a4z;
dot2 = a1x * a5x + a1y * a5y + a1z * a5z;
dot3 = a1x * a6x + a1y * a6y + a1z * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
if (dot3 < 0) dot3 = -dot3;
len2 = dot1 * w2 + dot2 * h2 + dot3 * d2;
overlap1 = len - len1 - len2;
if (overlap1 > 0) return numContactInfos;
// try axis 2
len = a2x * dx + a2y * dy + a2z * dz;
right2 = len > 0;
if (!right2) len = -len;
len1 = h1;
dot1 = a2x * a4x + a2y * a4y + a2z * a4z;
dot2 = a2x * a5x + a2y * a5y + a2z * a5z;
dot3 = a2x * a6x + a2y * a6y + a2z * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
if (dot3 < 0) dot3 = -dot3;
len2 = dot1 * w2 + dot2 * h2 + dot3 * d2;
overlap2 = len - len1 - len2;
if (overlap2 > 0) return numContactInfos;
// try axis 3
len = a3x * dx + a3y * dy + a3z * dz;
right3 = len > 0;
if (!right3) len = -len;
len1 = d1;
dot1 = a3x * a4x + a3y * a4y + a3z * a4z;
dot2 = a3x * a5x + a3y * a5y + a3z * a5z;
dot3 = a3x * a6x + a3y * a6y + a3z * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
if (dot3 < 0) dot3 = -dot3;
len2 = dot1 * w2 + dot2 * h2 + dot3 * d2;
overlap3 = len - len1 - len2;
if (overlap3 > 0) return numContactInfos;
// try axis 4
len = a4x * dx + a4y * dy + a4z * dz;
right4 = len > 0;
if (!right4) len = -len;
dot1 = a4x * a1x + a4y * a1y + a4z * a1z;
dot2 = a4x * a2x + a4y * a2y + a4z * a2z;
dot3 = a4x * a3x + a4y * a3y + a4z * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
if (dot3 < 0) dot3 = -dot3;
len1 = dot1 * w1 + dot2 * h1 + dot3 * d1;
len2 = w2;
overlap4 = len - len1 - len2;
if (overlap4 > 0) return numContactInfos;
// try axis 5
len = a5x * dx + a5y * dy + a5z * dz;
right5 = len > 0;
if (!right5) len = -len;
dot1 = a5x * a1x + a5y * a1y + a5z * a1z;
dot2 = a5x * a2x + a5y * a2y + a5z * a2z;
dot3 = a5x * a3x + a5y * a3y + a5z * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
if (dot3 < 0) dot3 = -dot3;
len1 = dot1 * w1 + dot2 * h1 + dot3 * d1;
len2 = h2;
overlap5 = len - len1 - len2;
if (overlap5 > 0) return numContactInfos;
// try axis 6
len = a6x * dx + a6y * dy + a6z * dz;
right6 = len > 0;
if (!right6) len = -len;
dot1 = a6x * a1x + a6y * a1y + a6z * a1z;
dot2 = a6x * a2x + a6y * a2y + a6z * a2z;
dot3 = a6x * a3x + a6y * a3y + a6z * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
if (dot3 < 0) dot3 = -dot3;
len1 = dot1 * w1 + dot2 * h1 + dot3 * d1;
len2 = d2;
overlap6 = len - len1 - len2;
if (overlap6 > 0) return numContactInfos;
// try axis 7
len = a7x * a7x + a7y * a7y + a7z * a7z;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
a7x *= len;
a7y *= len;
a7z *= len;
len = a7x * dx + a7y * dy + a7z * dz;
right7 = len > 0;
if (!right7) len = -len;
dot1 = a7x * a2x + a7y * a2y + a7z * a2z;
dot2 = a7x * a3x + a7y * a3y + a7z * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * h1 + dot2 * d1;
dot1 = a7x * a5x + a7y * a5y + a7z * a5z;
dot2 = a7x * a6x + a7y * a6y + a7z * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * h2 + dot2 * d2;
overlap7 = (len - len1 - len2) * 1.1;
if (overlap7 > 0) return numContactInfos;
} else {
right7 = false;
overlap7 = 0;
invalid7 = true;
}
// try axis 8
len = a8x * a8x + a8y * a8y + a8z * a8z;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
a8x *= len;
a8y *= len;
a8z *= len;
len = a8x * dx + a8y * dy + a8z * dz;
right8 = len > 0;
if (!right8) len = -len;
dot1 = a8x * a2x + a8y * a2y + a8z * a2z;
dot2 = a8x * a3x + a8y * a3y + a8z * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * h1 + dot2 * d1;
dot1 = a8x * a4x + a8y * a4y + a8z * a4z;
dot2 = a8x * a6x + a8y * a6y + a8z * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * w2 + dot2 * d2;
overlap8 = (len - len1 - len2) * 1.1;
if (overlap8 > 0) return numContactInfos;
} else {
right8 = false;
overlap8 = 0;
invalid8 = true;
}
// try axis 9
len = a9x * a9x + a9y * a9y + a9z * a9z;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
a9x *= len;
a9y *= len;
a9z *= len;
len = a9x * dx + a9y * dy + a9z * dz;
right9 = len > 0;
if (!right9) len = -len;
dot1 = a9x * a2x + a9y * a2y + a9z * a2z;
dot2 = a9x * a3x + a9y * a3y + a9z * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * h1 + dot2 * d1;
dot1 = a9x * a4x + a9y * a4y + a9z * a4z;
dot2 = a9x * a5x + a9y * a5y + a9z * a5z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * w2 + dot2 * h2;
overlap9 = (len - len1 - len2) * 1.1;
if (overlap9 > 0) return numContactInfos;
} else {
right9 = false;
overlap9 = 0;
invalid9 = true;
}
// try axis 10
len = aax * aax + aay * aay + aaz * aaz;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
aax *= len;
aay *= len;
aaz *= len;
len = aax * dx + aay * dy + aaz * dz;
righta = len > 0;
if (!righta) len = -len;
dot1 = aax * a1x + aay * a1y + aaz * a1z;
dot2 = aax * a3x + aay * a3y + aaz * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * w1 + dot2 * d1;
dot1 = aax * a5x + aay * a5y + aaz * a5z;
dot2 = aax * a6x + aay * a6y + aaz * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * h2 + dot2 * d2;
overlapa = (len - len1 - len2) * 1.1;
if (overlapa > 0) return numContactInfos;
} else {
righta = false;
overlapa = 0;
invalida = true;
}
// try axis 11
len = abx * abx + aby * aby + abz * abz;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
abx *= len;
aby *= len;
abz *= len;
len = abx * dx + aby * dy + abz * dz;
rightb = len > 0;
if (!rightb) len = -len;
dot1 = abx * a1x + aby * a1y + abz * a1z;
dot2 = abx * a3x + aby * a3y + abz * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * w1 + dot2 * d1;
dot1 = abx * a4x + aby * a4y + abz * a4z;
dot2 = abx * a6x + aby * a6y + abz * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * w2 + dot2 * d2;
overlapb = (len - len1 - len2) * 1.1;
if (overlapb > 0) return numContactInfos;
} else {
rightb = false;
overlapb = 0;
invalidb = true;
}
// try axis 12
len = acx * acx + acy * acy + acz * acz;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
acx *= len;
acy *= len;
acz *= len;
len = acx * dx + acy * dy + acz * dz;
rightc = len > 0;
if (!rightc) len = -len;
dot1 = acx * a1x + acy * a1y + acz * a1z;
dot2 = acx * a3x + acy * a3y + acz * a3z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * w1 + dot2 * d1;
dot1 = acx * a4x + acy * a4y + acz * a4z;
dot2 = acx * a5x + acy * a5y + acz * a5z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * w2 + dot2 * h2;
overlapc = (len - len1 - len2) * 1.1;
if (overlapc > 0) return numContactInfos;
} else {
rightc = false;
overlapc = 0;
invalidc = true;
}
// try axis 13
len = adx * adx + ady * ady + adz * adz;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
adx *= len;
ady *= len;
adz *= len;
len = adx * dx + ady * dy + adz * dz;
rightd = len > 0;
if (!rightd) len = -len;
dot1 = adx * a1x + ady * a1y + adz * a1z;
dot2 = adx * a2x + ady * a2y + adz * a2z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * w1 + dot2 * h1;
dot1 = adx * a5x + ady * a5y + adz * a5z;
dot2 = adx * a6x + ady * a6y + adz * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * h2 + dot2 * d2;
overlapd = (len - len1 - len2) * 1.1;
if (overlapd > 0) return numContactInfos;
} else {
rightd = false;
overlapd = 0;
invalidd = true;
}
// try axis 14
len = aex * aex + aey * aey + aez * aez;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
aex *= len;
aey *= len;
aez *= len;
len = aex * dx + aey * dy + aez * dz;
righte = len > 0;
if (!righte) len = -len;
dot1 = aex * a1x + aey * a1y + aez * a1z;
dot2 = aex * a2x + aey * a2y + aez * a2z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * w1 + dot2 * h1;
dot1 = aex * a4x + aey * a4y + aez * a4z;
dot2 = aex * a6x + aey * a6y + aez * a6z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * w2 + dot2 * d2;
overlape = (len - len1 - len2) * 1.1;
if (overlape > 0) return numContactInfos;
} else {
righte = false;
overlape = 0;
invalide = true;
}
// try axis 15
len = afx * afx + afy * afy + afz * afz;
if (len > 1e-5) {
len = 1 / Math.sqrt(len);
afx *= len;
afy *= len;
afz *= len;
len = afx * dx + afy * dy + afz * dz;
rightf = len > 0;
if (!rightf) len = -len;
dot1 = afx * a1x + afy * a1y + afz * a1z;
dot2 = afx * a2x + afy * a2y + afz * a2z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len1 = dot1 * w1 + dot2 * h1;
dot1 = afx * a4x + afy * a4y + afz * a4z;
dot2 = afx * a5x + afy * a5y + afz * a5z;
if (dot1 < 0) dot1 = -dot1;
if (dot2 < 0) dot2 = -dot2;
len2 = dot1 * w2 + dot2 * h2;
overlapf = (len - len1 - len2) * 1.1;
if (overlapf > 0) return numContactInfos;
} else {
rightf = false;
overlapf = 0;
invalidf = true;
}
// boxes are overlapping
var depth:Number = overlap1;
var minIndex:uint = 0;
var right:Boolean = right1;
if (overlap2 > depth) {
depth = overlap2;
minIndex = 1;
right = right2;
}
if (overlap3 > depth) {
depth = overlap3;
minIndex = 2;
right = right3;
}
if (overlap4 > depth) {
depth = overlap4;
minIndex = 3;
right = right4;
}
if (overlap5 > depth) {
depth = overlap5;
minIndex = 4;
right = right5;
}
if (overlap6 > depth) {
depth = overlap6;
minIndex = 5;
right = right6;
}
if (overlap7 > depth && !invalid7) {
depth = overlap7;
minIndex = 6;
right = right7;
}
if (overlap8 > depth && !invalid8) {
depth = overlap8;
minIndex = 7;
right = right8;
}
if (overlap9 > depth && !invalid9) {
depth = overlap9;
minIndex = 8;
right = right9;
}
if (overlapa > depth && !invalida) {
depth = overlapa;
minIndex = 9;
right = righta;
}
if (overlapb > depth && !invalidb) {
depth = overlapb;
minIndex = 10;
right = rightb;
}
if (overlapc > depth && !invalidc) {
depth = overlapc;
minIndex = 11;
right = rightc;
}
if (overlapd > depth && !invalidd) {
depth = overlapd;
minIndex = 12;
right = rightd;
}
if (overlape > depth && !invalide) {
depth = overlape;
minIndex = 13;
right = righte;
}
if (overlapf > depth && !invalidf) {
depth = overlapf;
minIndex = 14;
right = rightf;
}
// normal
var nx:Number = 0;
var ny:Number = 0;
var nz:Number = 0;
// edge line or face side normal
var n1x:Number = 0;
var n1y:Number = 0;
var n1z:Number = 0;
var n2x:Number = 0;
var n2y:Number = 0;
var n2z:Number = 0;
// center of current face
var cx:Number = 0;
var cy:Number = 0;
var cz:Number = 0;
// face side
var s1x:Number = 0;
var s1y:Number = 0;
var s1z:Number = 0;
var s2x:Number = 0;
var s2y:Number = 0;
var s2z:Number = 0;
// swap b1 b2
var swap:Boolean = false;
switch(minIndex) {
case 0: // b1.x * b2
if (right) {
cx = p1x + d1x;
cy = p1y + d1y;
cz = p1z + d1z;
nx = a1x;
ny = a1y;
nz = a1z;
} else {
cx = p1x - d1x;
cy = p1y - d1y;
cz = p1z - d1z;
nx = -a1x;
ny = -a1y;
nz = -a1z;
}
s1x = d2x;
s1y = d2y;
s1z = d2z;
n1x = -a2x;
n1y = -a2y;
n1z = -a2z;
s2x = d3x;
s2y = d3y;
s2z = d3z;
n2x = -a3x;
n2y = -a3y;
n2z = -a3z;
break;
case 1: // b1.y * b2
if (right) {
cx = p1x + d2x;
cy = p1y + d2y;
cz = p1z + d2z;
nx = a2x;
ny = a2y;
nz = a2z;
} else {
cx = p1x - d2x;
cy = p1y - d2y;
cz = p1z - d2z;
nx = -a2x;
ny = -a2y;
nz = -a2z;
}
s1x = d1x;
s1y = d1y;
s1z = d1z;
n1x = -a1x;
n1y = -a1y;
n1z = -a1z;
s2x = d3x;
s2y = d3y;
s2z = d3z;
n2x = -a3x;
n2y = -a3y;
n2z = -a3z;
break;
case 2: // b1.z * b2
if (right) {
cx = p1x + d3x;
cy = p1y + d3y;
cz = p1z + d3z;
nx = a3x;
ny = a3y;
nz = a3z;
} else {
cx = p1x - d3x;
cy = p1y - d3y;
cz = p1z - d3z;
nx = -a3x;
ny = -a3y;
nz = -a3z;
}
s1x = d1x;
s1y = d1y;
s1z = d1z;
n1x = -a1x;
n1y = -a1y;
n1z = -a1z;
s2x = d2x;
s2y = d2y;
s2z = d2z;
n2x = -a2x;
n2y = -a2y;
n2z = -a2z;
break;
case 3: // b2.x * b1
swap = true;
if (!right) {
cx = p2x + d4x;
cy = p2y + d4y;
cz = p2z + d4z;
nx = a4x;
ny = a4y;
nz = a4z;
} else {
cx = p2x - d4x;
cy = p2y - d4y;
cz = p2z - d4z;
nx = -a4x;
ny = -a4y;
nz = -a4z;
}
s1x = d5x;
s1y = d5y;
s1z = d5z;
n1x = -a5x;
n1y = -a5y;
n1z = -a5z;
s2x = d6x;
s2y = d6y;
s2z = d6z;
n2x = -a6x;
n2y = -a6y;
n2z = -a6z;
break;
case 4: // b2.y * b1
swap = true;
if (!right) {
cx = p2x + d5x;
cy = p2y + d5y;
cz = p2z + d5z;
nx = a5x;
ny = a5y;
nz = a5z;
} else {
cx = p2x - d5x;
cy = p2y - d5y;
cz = p2z - d5z;
nx = -a5x;
ny = -a5y;
nz = -a5z;
}
s1x = d4x;
s1y = d4y;
s1z = d4z;
n1x = -a4x;
n1y = -a4y;
n1z = -a4z;
s2x = d6x;
s2y = d6y;
s2z = d6z;
n2x = -a6x;
n2y = -a6y;
n2z = -a6z;
break;
case 5: // b2.z * b1
swap = true;
if (!right) {
cx = p2x + d6x;
cy = p2y + d6y;
cz = p2z + d6z;
nx = a6x;
ny = a6y;
nz = a6z;
} else {
cx = p2x - d6x;
cy = p2y - d6y;
cz = p2z - d6z;
nx = -a6x;
ny = -a6y;
nz = -a6z;
}
s1x = d4x;
s1y = d4y;
s1z = d4z;
n1x = -a4x;
n1y = -a4y;
n1z = -a4z;
s2x = d5x;
s2y = d5y;
s2z = d5z;
n2x = -a5x;
n2y = -a5y;
n2z = -a5z;
break;
case 6: // b1.x * b2.x
nx = a7x;
ny = a7y;
nz = a7z;
n1x = a1x;
n1y = a1y;
n1z = a1z;
n2x = a4x;
n2y = a4y;
n2z = a4z;
break;
case 7: // b1.x * b2.y
nx = a8x;
ny = a8y;
nz = a8z;
n1x = a1x;
n1y = a1y;
n1z = a1z;
n2x = a5x;
n2y = a5y;
n2z = a5z;
break;
case 8: // b1.x * b2.z
nx = a9x;
ny = a9y;
nz = a9z;
n1x = a1x;
n1y = a1y;
n1z = a1z;
n2x = a6x;
n2y = a6y;
n2z = a6z;
break;
case 9: // b1.y * b2.x
nx = aax;
ny = aay;
nz = aaz;
n1x = a2x;
n1y = a2y;
n1z = a2z;
n2x = a4x;
n2y = a4y;
n2z = a4z;
break;
case 10: // b1.y * b2.y
nx = abx;
ny = aby;
nz = abz;
n1x = a2x;
n1y = a2y;
n1z = a2z;
n2x = a5x;
n2y = a5y;
n2z = a5z;
break;
case 11: // b1.y * b2.z
nx = acx;
ny = acy;
nz = acz;
n1x = a2x;
n1y = a2y;
n1z = a2z;
n2x = a6x;
n2y = a6y;
n2z = a6z;
break;
case 12: // b1.z * b2.x
nx = adx;
ny = ady;
nz = adz;
n1x = a3x;
n1y = a3y;
n1z = a3z;
n2x = a4x;
n2y = a4y;
n2z = a4z;
break;
case 13: // b1.z * b2.y
nx = aex;
ny = aey;
nz = aez;
n1x = a3x;
n1y = a3y;
n1z = a3z;
n2x = a5x;
n2y = a5y;
n2z = a5z;
break;
case 14: // b1.z * b2.z
nx = afx;
ny = afy;
nz = afz;
n1x = a3x;
n1y = a3y;
n1z = a3z;
n2x = a6x;
n2y = a6y;
n2z = a6z;
break;
}
var c:ContactInfo;
var v:Vec3;
if (minIndex > 5) { // edge-edge collision
if (!right) {
nx = -nx;
ny = -ny;
nz = -nz;
}
// temp
var distance:Number;
var maxDistance:Number;
var vx:Number;
var vy:Number;
var vz:Number;
var v1x:Number;
var v1y:Number;
var v1z:Number;
var v2x:Number;
var v2y:Number;
var v2z:Number;
// get support vertex 1
v = b1.vertex1;
v1x = v.x;
v1y = v.y;
v1z = v.z;
maxDistance = nx * v1x + ny * v1y + nz * v1z;
v = b1.vertex2;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance > maxDistance) {
maxDistance = distance;
v1x = vx;
v1y = vy;
v1z = vz;
}
v = b1.vertex3;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance > maxDistance) {
maxDistance = distance;
v1x = vx;
v1y = vy;
v1z = vz;
}
v = b1.vertex4;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance > maxDistance) {
maxDistance = distance;
v1x = vx;
v1y = vy;
v1z = vz;
}
v = b1.vertex5;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance > maxDistance) {
maxDistance = distance;
v1x = vx;
v1y = vy;
v1z = vz;
}
v = b1.vertex6;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance > maxDistance) {
maxDistance = distance;
v1x = vx;
v1y = vy;
v1z = vz;
}
v = b1.vertex7;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance > maxDistance) {
maxDistance = distance;
v1x = vx;
v1y = vy;
v1z = vz;
}
v = b1.vertex8;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance > maxDistance) {
maxDistance = distance;
v1x = vx;
v1y = vy;
v1z = vz;
}
// get support vertex 2
v = b2.vertex1;
v2x = v.x;
v2y = v.y;
v2z = v.z;
maxDistance = nx * v2x + ny * v2y + nz * v2z;
v = b2.vertex2;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance < maxDistance) {
maxDistance = distance;
v2x = vx;
v2y = vy;
v2z = vz;
}
v = b2.vertex3;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance < maxDistance) {
maxDistance = distance;
v2x = vx;
v2y = vy;
v2z = vz;
}
v = b2.vertex4;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance < maxDistance) {
maxDistance = distance;
v2x = vx;
v2y = vy;
v2z = vz;
}
v = b2.vertex5;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance < maxDistance) {
maxDistance = distance;
v2x = vx;
v2y = vy;
v2z = vz;
}
v = b2.vertex6;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance < maxDistance) {
maxDistance = distance;
v2x = vx;
v2y = vy;
v2z = vz;
}
v = b2.vertex7;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance < maxDistance) {
maxDistance = distance;
v2x = vx;
v2y = vy;
v2z = vz;
}
v = b2.vertex8;
vx = v.x;
vy = v.y;
vz = v.z;
distance = nx * vx + ny * vy + nz * vz;
if (distance < maxDistance) {
maxDistance = distance;
v2x = vx;
v2y = vy;
v2z = vz;
}
// closest point
vx = v2x - v1x;
vy = v2y - v1y;
vz = v2z - v1z;
dot1 = n1x * n2x + n1y * n2y + n1z * n2z;
var t:Number = (vx * (n1x - n2x * dot1) + vy * (n1y - n2y * dot1) + vz * (n1z - n2z * dot1)) / (1 - dot1 * dot1);
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
c = contactInfos[numContactInfos++];
c.normal.x = nx;
c.normal.y = ny;
c.normal.z = nz;
c.position.x = v1x + n1x * t + nx * depth * 0.5;
c.position.y = v1y + n1y * t + ny * depth * 0.5;
c.position.z = v1z + n1z * t + nz * depth * 0.5;
c.overlap = depth;
c.shape1 = b1;
c.shape2 = b2;
c.id.data1 = 0;
c.id.data2 = 0;
c.id.flip = false;
return numContactInfos;
}
// now detect face-face collision...
// target quad
var q1x:Number;
var q1y:Number;
var q1z:Number;
var q2x:Number;
var q2y:Number;
var q2z:Number;
var q3x:Number;
var q3y:Number;
var q3z:Number;
var q4x:Number;
var q4y:Number;
var q4z:Number;
// search support face and vertex
var minDot:Number = 1;
var dot:Number = 0;
var minDotIndex:Number = 0;
if (swap) {
dot = a1x * nx + a1y * ny + a1z * nz;
if (dot < minDot) {
minDot = dot;
minDotIndex = 0;
}
if (-dot < minDot) {
minDot = -dot;
minDotIndex = 1;
}
dot = a2x * nx + a2y * ny + a2z * nz;
if (dot < minDot) {
minDot = dot;
minDotIndex = 2;
}
if (-dot < minDot) {
minDot = -dot;
minDotIndex = 3;
}
dot = a3x * nx + a3y * ny + a3z * nz;
if (dot < minDot) {
minDot = dot;
minDotIndex = 4;
}
if (-dot < minDot) {
minDot = -dot;
minDotIndex = 5;
}
switch(minDotIndex) {
case 0: // x+ face
v = b1.vertex1;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b1.vertex3;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b1.vertex4;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b1.vertex2;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 1: // x- face
v = b1.vertex6;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b1.vertex8;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b1.vertex7;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b1.vertex5;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 2: // y+ face
v = b1.vertex5;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b1.vertex1;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b1.vertex2;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b1.vertex6;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 3: // y- face
v = b1.vertex8;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b1.vertex4;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b1.vertex3;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b1.vertex7;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 4: // z+ face
v = b1.vertex5;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b1.vertex7;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b1.vertex3;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b1.vertex1;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 5: // z- face
v = b1.vertex2;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b1.vertex4;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b1.vertex8;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b1.vertex6;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
}
} else {
dot = a4x * nx + a4y * ny + a4z * nz;
if (dot < minDot) {
minDot = dot;
minDotIndex = 0;
}
if (-dot < minDot) {
minDot = -dot;
minDotIndex = 1;
}
dot = a5x * nx + a5y * ny + a5z * nz;
if (dot < minDot) {
minDot = dot;
minDotIndex = 2;
}
if (-dot < minDot) {
minDot = -dot;
minDotIndex = 3;
}
dot = a6x * nx + a6y * ny + a6z * nz;
if (dot < minDot) {
minDot = dot;
minDotIndex = 4;
}
if (-dot < minDot) {
minDot = -dot;
minDotIndex = 5;
}
switch(minDotIndex) {
case 0: // x+ face
v = b2.vertex1;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b2.vertex3;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b2.vertex4;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b2.vertex2;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 1: // x- face
v = b2.vertex6;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b2.vertex8;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b2.vertex7;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b2.vertex5;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 2: // y+ face
v = b2.vertex5;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b2.vertex1;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b2.vertex2;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b2.vertex6;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 3: // y- face
v = b2.vertex8;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b2.vertex4;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b2.vertex3;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b2.vertex7;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 4: // z+ face
v = b2.vertex5;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b2.vertex7;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b2.vertex3;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b2.vertex1;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
case 5: // z- face
v = b2.vertex2;
q1x = v.x;
q1y = v.y;
q1z = v.z;
v = b2.vertex4;
q2x = v.x;
q2y = v.y;
q2z = v.z;
v = b2.vertex8;
q3x = v.x;
q3y = v.y;
q3z = v.z;
v = b2.vertex6;
q4x = v.x;
q4y = v.y;
q4z = v.z;
break;
}
}
// clip vertices
var numClipVertices:uint;
var numAddedClipVertices:uint;
var index:uint;
var x1:Number;
var y1:Number;
var z1:Number;
var x2:Number;
var y2:Number;
var z2:Number;
// i gave up inline-expansion...
clipVertices1[0] = q1x;
clipVertices1[1] = q1y;
clipVertices1[2] = q1z;
clipVertices1[3] = q2x;
clipVertices1[4] = q2y;
clipVertices1[5] = q2z;
clipVertices1[6] = q3x;
clipVertices1[7] = q3y;
clipVertices1[8] = q3z;
clipVertices1[9] = q4x;
clipVertices1[10] = q4y;
clipVertices1[11] = q4z;
numAddedClipVertices = 0;
x1 = clipVertices1[9];
y1 = clipVertices1[10];
z1 = clipVertices1[11];
dot1 = (x1 - cx - s1x) * n1x + (y1 - cy - s1y) * n1y + (z1 - cz - s1z) * n1z;
for (var i:int = 0; i < 4; i++) {
index = i * 3;
x2 = clipVertices1[index];
y2 = clipVertices1[index + 1];
z2 = clipVertices1[index + 2];
dot2 = (x2 - cx - s1x) * n1x + (y2 - cy - s1y) * n1y + (z2 - cz - s1z) * n1z;
if (dot1 > 0) {
if (dot2 > 0) {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
clipVertices2[index] = x2;
clipVertices2[index + 1] = y2;
clipVertices2[index + 2] = z2;
} else {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
t = dot1 / (dot1 - dot2);
clipVertices2[index] = x1 + (x2 - x1) * t;
clipVertices2[index + 1] = y1 + (y2 - y1) * t;
clipVertices2[index + 2] = z1 + (z2 - z1) * t;
}
} else {
if (dot2 > 0) {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
t = dot1 / (dot1 - dot2);
clipVertices2[index] = x1 + (x2 - x1) * t;
clipVertices2[index + 1] = y1 + (y2 - y1) * t;
clipVertices2[index + 2] = z1 + (z2 - z1) * t;
index = numAddedClipVertices * 3;
numAddedClipVertices++;
clipVertices2[index] = x2;
clipVertices2[index + 1] = y2;
clipVertices2[index + 2] = z2;
}
}
x1 = x2;
y1 = y2;
z1 = z2;
dot1 = dot2;
}
numClipVertices = numAddedClipVertices;
if (numClipVertices == 0) return numContactInfos; // what's happened
numAddedClipVertices = 0;
index = (numClipVertices - 1) * 3;
x1 = clipVertices2[index];
y1 = clipVertices2[index + 1];
z1 = clipVertices2[index + 2];
dot1 = (x1 - cx - s2x) * n2x + (y1 - cy - s2y) * n2y + (z1 - cz - s2z) * n2z;
for (i = 0; i < numClipVertices; i++) {
index = i * 3;
x2 = clipVertices2[index];
y2 = clipVertices2[index + 1];
z2 = clipVertices2[index + 2];
dot2 = (x2 - cx - s2x) * n2x + (y2 - cy - s2y) * n2y + (z2 - cz - s2z) * n2z;
if (dot1 > 0) {
if (dot2 > 0) {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
clipVertices1[index] = x2;
clipVertices1[index + 1] = y2;
clipVertices1[index + 2] = z2;
} else {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
t = dot1 / (dot1 - dot2);
clipVertices1[index] = x1 + (x2 - x1) * t;
clipVertices1[index + 1] = y1 + (y2 - y1) * t;
clipVertices1[index + 2] = z1 + (z2 - z1) * t;
}
} else {
if (dot2 > 0) {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
t = dot1 / (dot1 - dot2);
clipVertices1[index] = x1 + (x2 - x1) * t;
clipVertices1[index + 1] = y1 + (y2 - y1) * t;
clipVertices1[index + 2] = z1 + (z2 - z1) * t;
index = numAddedClipVertices * 3;
numAddedClipVertices++;
clipVertices1[index] = x2;
clipVertices1[index + 1] = y2;
clipVertices1[index + 2] = z2;
}
}
x1 = x2;
y1 = y2;
z1 = z2;
dot1 = dot2;
}
numClipVertices = numAddedClipVertices;
if (numClipVertices == 0) return numContactInfos;
numAddedClipVertices = 0;
index = (numClipVertices - 1) * 3;
x1 = clipVertices1[index];
y1 = clipVertices1[index + 1];
z1 = clipVertices1[index + 2];
dot1 = (x1 - cx + s1x) * -n1x + (y1 - cy + s1y) * -n1y + (z1 - cz + s1z) * -n1z;
for (i = 0; i < numClipVertices; i++) {
index = i * 3;
x2 = clipVertices1[index];
y2 = clipVertices1[index + 1];
z2 = clipVertices1[index + 2];
dot2 = (x2 - cx + s1x) * -n1x + (y2 - cy + s1y) * -n1y + (z2 - cz + s1z) * -n1z;
if (dot1 > 0) {
if (dot2 > 0) {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
clipVertices2[index] = x2;
clipVertices2[index + 1] = y2;
clipVertices2[index + 2] = z2;
} else {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
t = dot1 / (dot1 - dot2);
clipVertices2[index] = x1 + (x2 - x1) * t;
clipVertices2[index + 1] = y1 + (y2 - y1) * t;
clipVertices2[index + 2] = z1 + (z2 - z1) * t;
}
} else {
if (dot2 > 0) {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
t = dot1 / (dot1 - dot2);
clipVertices2[index] = x1 + (x2 - x1) * t;
clipVertices2[index + 1] = y1 + (y2 - y1) * t;
clipVertices2[index + 2] = z1 + (z2 - z1) * t;
index = numAddedClipVertices * 3;
numAddedClipVertices++;
clipVertices2[index] = x2;
clipVertices2[index + 1] = y2;
clipVertices2[index + 2] = z2;
}
}
x1 = x2;
y1 = y2;
z1 = z2;
dot1 = dot2;
}
numClipVertices = numAddedClipVertices;
if (numClipVertices == 0) return numContactInfos;
numAddedClipVertices = 0;
index = (numClipVertices - 1) * 3;
x1 = clipVertices2[index];
y1 = clipVertices2[index + 1];
z1 = clipVertices2[index + 2];
dot1 = (x1 - cx + s2x) * -n2x + (y1 - cy + s2y) * -n2y + (z1 - cz + s2z) * -n2z;
for (i = 0; i < numClipVertices; i++) {
index = i * 3;
x2 = clipVertices2[index];
y2 = clipVertices2[index + 1];
z2 = clipVertices2[index + 2];
dot2 = (x2 - cx + s2x) * -n2x + (y2 - cy + s2y) * -n2y + (z2 - cz + s2z) * -n2z;
if (dot1 > 0) {
if (dot2 > 0) {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
clipVertices1[index] = x2;
clipVertices1[index + 1] = y2;
clipVertices1[index + 2] = z2;
} else {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
t = dot1 / (dot1 - dot2);
clipVertices1[index] = x1 + (x2 - x1) * t;
clipVertices1[index + 1] = y1 + (y2 - y1) * t;
clipVertices1[index + 2] = z1 + (z2 - z1) * t;
}
} else {
if (dot2 > 0) {
index = numAddedClipVertices * 3;
numAddedClipVertices++;
t = dot1 / (dot1 - dot2);
clipVertices1[index] = x1 + (x2 - x1) * t;
clipVertices1[index + 1] = y1 + (y2 - y1) * t;
clipVertices1[index + 2] = z1 + (z2 - z1) * t;
index = numAddedClipVertices * 3;
numAddedClipVertices++;
clipVertices1[index] = x2;
clipVertices1[index + 1] = y2;
clipVertices1[index + 2] = z2;
}
}
x1 = x2;
y1 = y2;
z1 = z2;
dot1 = dot2;
}
numClipVertices = numAddedClipVertices;
if (numClipVertices == 0) return numContactInfos;
if (numClipVertices > 4) {
// sweep vertices
x1 = (q1x + q2x + q3x + q4x) * 0.25;
y1 = (q1y + q2y + q3y + q4y) * 0.25;
z1 = (q1z + q2z + q3z + q4z) * 0.25;
n1x = q1x - x1;
n1y = q1y - y1;
n1z = q1z - z1;
n2x = q2x - x1;
n2y = q2y - y1;
n2z = q2z - z1;
var index1:uint = 0;
var index2:uint = 0;
var index3:uint = 0;
var index4:uint = 0;
var maxDot:Number = -INF;
minDot = INF;
for (i = 0; i < numClipVertices; i++) {
used[i] = false;
index = i * 3;
x1 = clipVertices1[index];
y1 = clipVertices1[index + 1];
z1 = clipVertices1[index + 2];
dot = x1 * n1x + y1 * n1y + z1 * n1z;
if (dot < minDot) {
minDot = dot;
index1 = i;
}
if (dot > maxDot) {
maxDot = dot;
index3 = i;
}
}
used[index1] = true;
used[index3] = true;
maxDot = -INF;
minDot = INF;
for (i = 0; i < numClipVertices; i++) {
if (used[i]) continue;
index = i * 3;
x1 = clipVertices1[index];
y1 = clipVertices1[index + 1];
z1 = clipVertices1[index + 2];
dot = x1 * n2x + y1 * n2y + z1 * n2z;
if (dot < minDot) {
minDot = dot;
index2 = i;
}
if (dot > maxDot) {
maxDot = dot;
index4 = i;
}
}
index = index1 * 3;
x1 = clipVertices1[index];
y1 = clipVertices1[index + 1];
z1 = clipVertices1[index + 2];
dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;
if (dot < 0) {
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
c = contactInfos[numContactInfos++];
c.normal.x = nx;
c.normal.y = ny;
c.normal.z = nz;
c.position.x = x1;
c.position.y = y1;
c.position.z = z1;
c.overlap = dot;
if (swap) {
c.shape1 = b2;
c.shape2 = b1;
} else {
c.shape1 = b1;
c.shape2 = b2;
}
c.id.data1 = 0;
c.id.data2 = 0;
c.id.flip = false;
}
index = index2 * 3;
x1 = clipVertices1[index];
y1 = clipVertices1[index + 1];
z1 = clipVertices1[index + 2];
dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;
if (dot < 0) {
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
c = contactInfos[numContactInfos++];
c.normal.x = nx;
c.normal.y = ny;
c.normal.z = nz;
c.position.x = x1;
c.position.y = y1;
c.position.z = z1;
c.overlap = dot;
if (swap) {
c.shape1 = b2;
c.shape2 = b1;
} else {
c.shape1 = b1;
c.shape2 = b2;
}
c.id.data1 = 1;
c.id.data2 = 0;
c.id.flip = false;
}
index = index3 * 3;
x1 = clipVertices1[index];
y1 = clipVertices1[index + 1];
z1 = clipVertices1[index + 2];
dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;
if (dot < 0) {
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
c = contactInfos[numContactInfos++];
c.normal.x = nx;
c.normal.y = ny;
c.normal.z = nz;
c.position.x = x1;
c.position.y = y1;
c.position.z = z1;
c.overlap = dot;
if (swap) {
c.shape1 = b2;
c.shape2 = b1;
} else {
c.shape1 = b1;
c.shape2 = b2;
}
c.id.data1 = 2;
c.id.data2 = 0;
c.id.flip = false;
}
index = index4 * 3;
x1 = clipVertices1[index];
y1 = clipVertices1[index + 1];
z1 = clipVertices1[index + 2];
dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;
if (dot < 0) {
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
c = contactInfos[numContactInfos++];
c.normal.x = nx;
c.normal.y = ny;
c.normal.z = nz;
c.position.x = x1;
c.position.y = y1;
c.position.z = z1;
c.overlap = dot;
if (swap) {
c.shape1 = b2;
c.shape2 = b1;
} else {
c.shape1 = b1;
c.shape2 = b2;
}
c.id.data1 = 3;
c.id.data2 = 0;
c.id.flip = false;
}
} else {
for (i = 0; i < numClipVertices; i++) {
index = i * 3;
x1 = clipVertices1[index];
y1 = clipVertices1[index + 1];
z1 = clipVertices1[index + 2];
dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;
if (dot < 0) {
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
c = contactInfos[numContactInfos++];
c.normal.x = nx;
c.normal.y = ny;
c.normal.z = nz;
c.position.x = x1;
c.position.y = y1;
c.position.z = z1;
c.overlap = dot;
if (swap) {
c.shape1 = b2;
c.shape2 = b1;
} else {
c.shape1 = b1;
c.shape2 = b2;
}
c.id.data1 = i;
c.id.data2 = 0;
c.id.flip = false;
}
}
}
return numContactInfos;
}
}
class ContactID {
public var data1:uint;
public var data2:uint;
public var flip:Boolean;
public function ContactID() {
}
public function equals(id:ContactID):Boolean {
return flip == id.flip ? data1 == id.data1 && data2 == id.data2 : data2 == id.data1 && data1 == id.data2;
}
}
class ContactInfo {
public var position:Vec3;
public var normal:Vec3;
public var overlap:Number;
public var shape1:Shape;
public var shape2:Shape;
public var id:ContactID;
public function ContactInfo() {
position = new Vec3();
normal = new Vec3();
id = new ContactID();
}
}
class SphereBoxCollisionDetector extends CollisionDetector {
public function SphereBoxCollisionDetector(flip:Boolean) {
this.flip = flip;
}
override public function detectCollision(shape1:Shape, shape2:Shape, contactInfos:Vector.<ContactInfo>, numContactInfos:uint):uint {
if (numContactInfos == contactInfos.length) return numContactInfos;
var s:SphereShape;
var b:BoxShape;
if (flip) {
s = shape2 as SphereShape;
b = shape1 as BoxShape;
} else {
s = shape1 as SphereShape;
b = shape2 as BoxShape;
}
var ps:Vec3 = s.position;
var psx:Number = ps.x;
var psy:Number = ps.y;
var psz:Number = ps.z;
var pb:Vec3 = b.position;
var pbx:Number = pb.x;
var pby:Number = pb.y;
var pbz:Number = pb.z;
var rad:Number = s.radius;
// normal
var nw:Vec3 = b.normalDirectionWidth;
var nh:Vec3 = b.normalDirectionHeight;
var nd:Vec3 = b.normalDirectionDepth;
// half
var hw:Number = b.halfWidth;
var hh:Number = b.halfHeight;
var hd:Number = b.halfDepth;
// diff
var dx:Number = psx - pbx;
var dy:Number = psy - pby;
var dz:Number = psz - pbz;
// shadow
var sx:Number = nw.x * dx + nw.y * dy + nw.z * dz;
var sy:Number = nh.x * dx + nh.y * dy + nh.z * dz;
var sz:Number = nd.x * dx + nd.y * dy + nd.z * dz;
// closest
var cx:Number;
var cy:Number;
var cz:Number;
var len:Number;
var invLen:Number;
var c:ContactInfo;
var overlap:uint = 0;
if (sx > hw) {
sx = hw;
} else if (sx < -hw) {
sx = -hw;
} else {
overlap = 1;
}
if (sy > hh) {
sy = hh;
} else if (sy < -hh) {
sy = -hh;
} else {
overlap |= 2;
}
if (sz > hd) {
sz = hd;
} else if (sz < -hd) {
sz = -hd;
} else {
overlap |= 4;
}
if (overlap == 7) {
// center of sphere is in the box
if (sx < 0) {
dx = hw + sx;
} else {
dx = hw - sx;
}
if (sy < 0) {
dy = hh + sy;
} else {
dy = hh - sy;
}
if (sz < 0) {
dz = hd + sz;
} else {
dz = hd - sz;
}
if (dx < dy) {
if (dx < dz) {
// x
len = dx - hw;
if (sx < 0) {
sx = -hw;
dx = nw.x;
dy = nw.y;
dz = nw.z;
} else {
sx = hw;
dx = -nw.x;
dy = -nw.y;
dz = -nw.z;
}
} else {
// z
len = dz - hd;
if (sz < 0) {
sz = -hd;
dx = nd.x;
dy = nd.y;
dz = nd.z;
} else {
sz = hd;
dx = -nd.x;
dy = -nd.y;
dz = -nd.z;
}
}
} else {
if (dy < dz) {
// y
len = dy - hh;
if (sy < 0) {
sy = -hh;
dx = nh.x;
dy = nh.y;
dz = nh.z;
} else {
sy = hh;
dx = -nh.x;
dy = -nh.y;
dz = -nh.z;
}
} else {
// z
len = dz - hd;
if (sz < 0) {
sz = -hd;
dx = nd.x;
dy = nd.y;
dz = nd.z;
} else {
sz = hd;
dx = -nd.x;
dy = -nd.y;
dz = -nd.z;
}
}
}
cx = pbx + sx * nw.x + sy * nh.x + sz * nd.x;
cy = pby + sx * nw.y + sy * nh.y + sz * nd.y;
cz = pbz + sx * nw.z + sy * nh.z + sz * nd.z;
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
c = contactInfos[numContactInfos++];
c.normal.x = dx;
c.normal.y = dy;
c.normal.z = dz;
c.position.x = psx + rad * dx;
c.position.y = psy + rad * dy;
c.position.z = psz + rad * dz;
c.overlap = len;
c.shape1 = s;
c.shape2 = b;
c.id.data1 = 0;
c.id.data2 = 0;
c.id.flip = false;
} else {
// closest
cx = pbx + sx * nw.x + sy * nh.x + sz * nd.x;
cy = pby + sx * nw.y + sy * nh.y + sz * nd.y;
cz = pbz + sx * nw.z + sy * nh.z + sz * nd.z;
dx = cx - ps.x;
dy = cy - ps.y;
dz = cz - ps.z;
len = dx * dx + dy * dy + dz * dz;
if (len > 0 && len < rad * rad) {
len = Math.sqrt(len);
invLen = 1 / len;
dx *= invLen;
dy *= invLen;
dz *= invLen;
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
c = contactInfos[numContactInfos++];
c.normal.x = dx;
c.normal.y = dy;
c.normal.z = dz;
c.position.x = psx + rad * dx;
c.position.y = psy + rad * dy;
c.position.z = psz + rad * dz;
c.overlap = len - rad;
c.shape1 = s;
c.shape2 = b;
c.id.data1 = 0;
c.id.data2 = 0;
c.id.flip = false;
}
}
return numContactInfos;
}
}
class SphereSphereCollisionDetector extends CollisionDetector {
public function SphereSphereCollisionDetector() {
}
override public function detectCollision(shape1:Shape, shape2:Shape, contactInfos:Vector.<ContactInfo>, numContactInfos:uint):uint {
var s1:SphereShape = shape1 as SphereShape;
var s2:SphereShape = shape2 as SphereShape;
var p1:Vec3 = s1.position;
var p2:Vec3 = s2.position;
var dx:Number = p2.x - p1.x;
var dy:Number = p2.y - p1.y;
var dz:Number = p2.z - p1.z;
var len:Number = dx * dx + dy * dy + dz * dz;
var r1:Number = s1.radius;
var r2:Number = s2.radius;
var rad:Number = r1 + r2;
if (len > 0 && len < rad * rad && contactInfos.length > numContactInfos) {
len = Math.sqrt(len);
var invLen:Number = 1 / len;
dx *= invLen;
dy *= invLen;
dz *= invLen;
if (!contactInfos[numContactInfos]) {
contactInfos[numContactInfos] = new ContactInfo();
}
var c:ContactInfo = contactInfos[numContactInfos++];
c.normal.x = dx;
c.normal.y = dy;
c.normal.z = dz;
c.position.x = p1.x + dx * r1;
c.position.y = p1.y + dy * r1;
c.position.z = p1.z + dz * r1;
c.overlap = len - rad;
c.shape1 = s1;
c.shape2 = s2;
c.id.data1 = 0;
c.id.data2 = 0;
c.id.flip = false;
}
return numContactInfos;
}
}
class Shape {
public static var nextID:uint = 0;
public static const SHAPE_NULL:uint = 0x0;
public static const SHAPE_SPHERE:uint = 0x1;
public static const SHAPE_BOX:uint = 0x2;
public static const MAX_CONTACTS:uint = 1024;
public var id:uint;
public var type:uint;
public var position:Vec3;
public var relativePosition:Vec3;
public var localRelativePosition:Vec3;
public var rotation:Mat33;
public var relativeRotation:Mat33;
public var mass:Number;
public var localInertia:Mat33;
public var proxy:Proxy;
public var friction:Number;
public var restitution:Number;
public var parent:RigidBody;
public var contacts:Vector.<Contact>;
public var numContacts:uint;
public function Shape() {
id = ++nextID;
position = new Vec3();
relativePosition = new Vec3();
localRelativePosition = new Vec3();
rotation = new Mat33();
relativeRotation = new Mat33();
localInertia = new Mat33();
proxy = new Proxy();
proxy.parent = this;
contacts = new Vector.<Contact>(MAX_CONTACTS, true);
}
public function updateProxy():void {
throw new Error("updateProxy メソッドが継承されていません");
}
}
class BoxShape extends Shape {
public var width:Number;
public var halfWidth:Number;
public var height:Number;
public var halfHeight:Number;
public var depth:Number;
public var halfDepth:Number;
public var normalDirectionWidth:Vec3;
public var normalDirectionHeight:Vec3;
public var normalDirectionDepth:Vec3;
public var halfDirectionWidth:Vec3;
public var halfDirectionHeight:Vec3;
public var halfDirectionDepth:Vec3;
public var vertex1:Vec3;
public var vertex2:Vec3;
public var vertex3:Vec3;
public var vertex4:Vec3;
public var vertex5:Vec3;
public var vertex6:Vec3;
public var vertex7:Vec3;
public var vertex8:Vec3;
public function BoxShape(width:Number, height:Number, depth:Number, config:ShapeConfig) {
this.width = width;
halfWidth = width * 0.5;
this.height = height;
halfHeight = height * 0.5;
this.depth = depth;
halfDepth = depth * 0.5;
position.copy(config.position);
rotation.copy(config.rotation);
friction = config.friction;
restitution = config.restitution;
mass = width * height * depth * config.density;
var inertia:Number = mass / 12;
localInertia.init(
inertia * (height * height + depth * depth), 0, 0,
0, inertia * (width * width + depth * depth), 0,
0, 0, inertia * (width * width + height * height)
);
normalDirectionWidth = new Vec3();
normalDirectionHeight = new Vec3();
normalDirectionDepth = new Vec3();
halfDirectionWidth = new Vec3();
halfDirectionHeight = new Vec3();
halfDirectionDepth = new Vec3();
vertex1 = new Vec3();
vertex2 = new Vec3();
vertex3 = new Vec3();
vertex4 = new Vec3();
vertex5 = new Vec3();
vertex6 = new Vec3();
vertex7 = new Vec3();
vertex8 = new Vec3();
type = SHAPE_BOX;
}
override public function updateProxy():void {
normalDirectionWidth.x = rotation.e00;
normalDirectionWidth.y = rotation.e10;
normalDirectionWidth.z = rotation.e20;
normalDirectionHeight.x = rotation.e01;
normalDirectionHeight.y = rotation.e11;
normalDirectionHeight.z = rotation.e21;
normalDirectionDepth.x = rotation.e02;
normalDirectionDepth.y = rotation.e12;
normalDirectionDepth.z = rotation.e22;
halfDirectionWidth.x = rotation.e00 * halfWidth;
halfDirectionWidth.y = rotation.e10 * halfWidth;
halfDirectionWidth.z = rotation.e20 * halfWidth;
halfDirectionHeight.x = rotation.e01 * halfHeight;
halfDirectionHeight.y = rotation.e11 * halfHeight;
halfDirectionHeight.z = rotation.e21 * halfHeight;
halfDirectionDepth.x = rotation.e02 * halfDepth;
halfDirectionDepth.y = rotation.e12 * halfDepth;
halfDirectionDepth.z = rotation.e22 * halfDepth;
var wx:Number = halfDirectionWidth.x;
var wy:Number = halfDirectionWidth.y;
var wz:Number = halfDirectionWidth.z;
var hx:Number = halfDirectionHeight.x;
var hy:Number = halfDirectionHeight.y;
var hz:Number = halfDirectionHeight.z;
var dx:Number = halfDirectionDepth.x;
var dy:Number = halfDirectionDepth.y;
var dz:Number = halfDirectionDepth.z;
var x:Number = position.x;
var y:Number = position.y;
var z:Number = position.z;
vertex1.x = x + wx + hx + dx;
vertex1.y = y + wy + hy + dy;
vertex1.z = z + wz + hz + dz;
vertex2.x = x + wx + hx - dx;
vertex2.y = y + wy + hy - dy;
vertex2.z = z + wz + hz - dz;
vertex3.x = x + wx - hx + dx;
vertex3.y = y + wy - hy + dy;
vertex3.z = z + wz - hz + dz;
vertex4.x = x + wx - hx - dx;
vertex4.y = y + wy - hy - dy;
vertex4.z = z + wz - hz - dz;
vertex5.x = x - wx + hx + dx;
vertex5.y = y - wy + hy + dy;
vertex5.z = z - wz + hz + dz;
vertex6.x = x - wx + hx - dx;
vertex6.y = y - wy + hy - dy;
vertex6.z = z - wz + hz - dz;
vertex7.x = x - wx - hx + dx;
vertex7.y = y - wy - hy + dy;
vertex7.z = z - wz - hz + dz;
vertex8.x = x - wx - hx - dx;
vertex8.y = y - wy - hy - dy;
vertex8.z = z - wz - hz - dz;
var w:Number;
var h:Number;
var d:Number;
if (halfDirectionWidth.x < 0) {
w = -halfDirectionWidth.x;
} else {
w = halfDirectionWidth.x;
}
if (halfDirectionWidth.y < 0) {
h = -halfDirectionWidth.y;
} else {
h = halfDirectionWidth.y;
}
if (halfDirectionWidth.z < 0) {
d = -halfDirectionWidth.z;
} else {
d = halfDirectionWidth.z;
}
if (halfDirectionHeight.x < 0) {
w -= halfDirectionHeight.x;
} else {
w += halfDirectionHeight.x;
}
if (halfDirectionHeight.y < 0) {
h -= halfDirectionHeight.y;
} else {
h += halfDirectionHeight.y;
}
if (halfDirectionHeight.z < 0) {
d -= halfDirectionHeight.z;
} else {
d += halfDirectionHeight.z;
}
if (halfDirectionDepth.x < 0) {
w -= halfDirectionDepth.x;
} else {
w += halfDirectionDepth.x;
}
if (halfDirectionDepth.y < 0) {
h -= halfDirectionDepth.y;
} else {
h += halfDirectionDepth.y;
}
if (halfDirectionDepth.z < 0) {
d -= halfDirectionDepth.z;
} else {
d += halfDirectionDepth.z;
}
proxy.init(
position.x - w, position.x + w,
position.y - h, position.y + h,
position.z - d, position.z + d
);
}
}
class ShapeConfig {
public var position:Vec3;
public var rotation:Mat33;
public var friction:Number;
public var restitution:Number;
public var density:Number;
public function ShapeConfig() {
position = new Vec3();
rotation = new Mat33();
friction = 0.5;
restitution = 0.5;
density = 1;
}
}
class SphereShape extends Shape {
public var radius:Number;
public function SphereShape(radius:Number, config:ShapeConfig) {
this.radius = radius;
position.copy(config.position);
rotation.copy(config.rotation);
friction = config.friction;
restitution = config.restitution;
mass = 4 / 3 * Math.PI * radius * radius * radius * config.density;
var inertia:Number = 2 / 5 * radius * radius * mass;
localInertia.init(
inertia, 0, 0,
0, inertia, 0,
0, 0, inertia
);
type = SHAPE_SPHERE;
}
override public function updateProxy():void {
proxy.init(
position.x - radius, position.x + radius,
position.y - radius, position.y + radius,
position.z - radius, position.z + radius
);
}
}
class Constraint {
public var parent:World;
public function Constraint() {
}
public function preSolve(timeStep:Number, invTimeStep:Number):void {
throw new Error("preSolve メソッドが継承されていません");
}
public function solve():void {
throw new Error("solve メソッドが継承されていません");
}
public function postSolve():void {
throw new Error("postSolve メソッドが継承されていません");
}
}
class Contact extends Constraint {
public var position:Vec3;
public var relativePosition1:Vec3;
public var relativePosition2:Vec3;
public var normal:Vec3;
public var tangent:Vec3;
public var binormal:Vec3;
public var overlap:Number;
public var normalImpulse:Number;
public var tangentImpulse:Number;
public var binormalImpulse:Number;
public var shape1:Shape;
public var shape2:Shape;
public var rigid1:RigidBody;
public var rigid2:RigidBody;
public var id:ContactID;
public var warmStarted:Boolean;
private var lVel1:Vec3;
private var lVel2:Vec3;
private var aVel1:Vec3;
private var aVel2:Vec3;
private var relPos1X:Number;
private var relPos1Y:Number;
private var relPos1Z:Number;
private var relPos2X:Number;
private var relPos2Y:Number;
private var relPos2Z:Number;
private var relVelX:Number;
private var relVelY:Number;
private var relVelZ:Number;
private var norX:Number;
private var norY:Number;
private var norZ:Number;
private var tanX:Number;
private var tanY:Number;
private var tanZ:Number;
private var binX:Number;
private var binY:Number;
private var binZ:Number;
private var norTorque1X:Number;
private var norTorque1Y:Number;
private var norTorque1Z:Number;
private var norTorque2X:Number;
private var norTorque2Y:Number;
private var norTorque2Z:Number;
private var tanTorque1X:Number;
private var tanTorque1Y:Number;
private var tanTorque1Z:Number;
private var tanTorque2X:Number;
private var tanTorque2Y:Number;
private var tanTorque2Z:Number;
private var binTorque1X:Number;
private var binTorque1Y:Number;
private var binTorque1Z:Number;
private var binTorque2X:Number;
private var binTorque2Y:Number;
private var binTorque2Z:Number;
private var norTorqueUnit1X:Number;
private var norTorqueUnit1Y:Number;
private var norTorqueUnit1Z:Number;
private var norTorqueUnit2X:Number;
private var norTorqueUnit2Y:Number;
private var norTorqueUnit2Z:Number;
private var tanTorqueUnit1X:Number;
private var tanTorqueUnit1Y:Number;
private var tanTorqueUnit1Z:Number;
private var tanTorqueUnit2X:Number;
private var tanTorqueUnit2Y:Number;
private var tanTorqueUnit2Z:Number;
private var binTorqueUnit1X:Number;
private var binTorqueUnit1Y:Number;
private var binTorqueUnit1Z:Number;
private var binTorqueUnit2X:Number;
private var binTorqueUnit2Y:Number;
private var binTorqueUnit2Z:Number;
private var invM1:Number;
private var invM2:Number;
private var invI1e00:Number;
private var invI1e01:Number;
private var invI1e02:Number;
private var invI1e10:Number;
private var invI1e11:Number;
private var invI1e12:Number;
private var invI1e20:Number;
private var invI1e21:Number;
private var invI1e22:Number;
private var invI2e00:Number;
private var invI2e01:Number;
private var invI2e02:Number;
private var invI2e10:Number;
private var invI2e11:Number;
private var invI2e12:Number;
private var invI2e20:Number;
private var invI2e21:Number;
private var invI2e22:Number;
private var normalDenominator:Number;
private var tangentDenominator:Number;
private var binormalDenominator:Number;
private var targetNormalVelocity:Number;
private var targetSeparateVelocity:Number;
private var friction:Number;
private var restitution:Number;
private var relativeVelocity:Vec3;
private var tmp1:Vec3;
private var tmp2:Vec3;
public function Contact() {
position = new Vec3();
relativePosition1 = new Vec3();
relativePosition2 = new Vec3();
normal = new Vec3();
tangent = new Vec3();
binormal = new Vec3();
id = new ContactID();
normalImpulse = 0;
tangentImpulse = 0;
binormalImpulse = 0;
relativeVelocity = new Vec3();
tmp1 = new Vec3();
tmp2 = new Vec3();
}
public function setupFromContactInfo(contactInfo:ContactInfo):void {
position.x = contactInfo.position.x;
position.y = contactInfo.position.y;
position.z = contactInfo.position.z;
norX = contactInfo.normal.x;
norY = contactInfo.normal.y;
norZ = contactInfo.normal.z;
overlap = contactInfo.overlap;
shape1 = contactInfo.shape1;
shape2 = contactInfo.shape2;
rigid1 = shape1.parent;
rigid2 = shape2.parent;
relPos1X = position.x - rigid1.position.x;
relPos1Y = position.y - rigid1.position.y;
relPos1Z = position.z - rigid1.position.z;
relPos2X = position.x - rigid2.position.x;
relPos2Y = position.y - rigid2.position.y;
relPos2Z = position.z - rigid2.position.z;
lVel1 = rigid1.linearVelocity;
lVel2 = rigid2.linearVelocity;
aVel1 = rigid1.angularVelocity;
aVel2 = rigid2.angularVelocity;
invM1 = rigid1.invertMass;
invM2 = rigid2.invertMass;
var tmpI:Mat33;
tmpI = rigid1.invertInertia;
invI1e00 = tmpI.e00;
invI1e01 = tmpI.e01;
invI1e02 = tmpI.e02;
invI1e10 = tmpI.e10;
invI1e11 = tmpI.e11;
invI1e12 = tmpI.e12;
invI1e20 = tmpI.e20;
invI1e21 = tmpI.e21;
invI1e22 = tmpI.e22;
tmpI = rigid2.invertInertia;
invI2e00 = tmpI.e00;
invI2e01 = tmpI.e01;
invI2e02 = tmpI.e02;
invI2e10 = tmpI.e10;
invI2e11 = tmpI.e11;
invI2e12 = tmpI.e12;
invI2e20 = tmpI.e20;
invI2e21 = tmpI.e21;
invI2e22 = tmpI.e22;
id.data1 = contactInfo.id.data1;
id.data2 = contactInfo.id.data2;
id.flip = contactInfo.id.flip;
friction = shape1.friction * shape2.friction;
restitution = shape1.restitution * shape2.restitution;
overlap = contactInfo.overlap;
normalImpulse = 0;
tangentImpulse = 0;
binormalImpulse = 0;
warmStarted = false;
}
override public function preSolve(timeStep:Number, invTimeStep:Number):void {
relVelX = (lVel2.x + aVel2.y * relPos2Z - aVel2.z * relPos2Y) - (lVel1.x + aVel1.y * relPos1Z - aVel1.z * relPos1Y);
relVelY = (lVel2.y + aVel2.z * relPos2X - aVel2.x * relPos2Z) - (lVel1.y + aVel1.z * relPos1X - aVel1.x * relPos1Z);
relVelZ = (lVel2.z + aVel2.x * relPos2Y - aVel2.y * relPos2X) - (lVel1.z + aVel1.x * relPos1Y - aVel1.y * relPos1X);
var rvn:Number = norX * relVelX + norY * relVelY + norZ * relVelZ;
tanX = relVelX - rvn * norX;
tanY = relVelY - rvn * norY;
tanZ = relVelZ - rvn * norZ;
var len:Number = tanX * tanX + tanY * tanY + tanZ * tanZ;
if (len > 1e-2) {
len = 1 / Math.sqrt(len);
} else {
tanX = norY * norX - norZ * norZ;
tanY = norZ * -norY - norX * norX;
tanZ = norX * norZ + norY * norY;
len = 1 / Math.sqrt(tanX * tanX + tanY * tanY + tanZ * tanZ);
}
tanX *= len;
tanY *= len;
tanZ *= len;
binX = norY * tanZ - norZ * tanY;
binY = norZ * tanX - norX * tanZ;
binZ = norX * tanY - norY * tanX;
norTorque1X = relPos1Y * norZ - relPos1Z * norY;
norTorque1Y = relPos1Z * norX - relPos1X * norZ;
norTorque1Z = relPos1X * norY - relPos1Y * norX;
norTorque2X = relPos2Y * norZ - relPos2Z * norY;
norTorque2Y = relPos2Z * norX - relPos2X * norZ;
norTorque2Z = relPos2X * norY - relPos2Y * norX;
tanTorque1X = relPos1Y * tanZ - relPos1Z * tanY;
tanTorque1Y = relPos1Z * tanX - relPos1X * tanZ;
tanTorque1Z = relPos1X * tanY - relPos1Y * tanX;
tanTorque2X = relPos2Y * tanZ - relPos2Z * tanY;
tanTorque2Y = relPos2Z * tanX - relPos2X * tanZ;
tanTorque2Z = relPos2X * tanY - relPos2Y * tanX;
binTorque1X = relPos1Y * binZ - relPos1Z * binY;
binTorque1Y = relPos1Z * binX - relPos1X * binZ;
binTorque1Z = relPos1X * binY - relPos1Y * binX;
binTorque2X = relPos2Y * binZ - relPos2Z * binY;
binTorque2Y = relPos2Z * binX - relPos2X * binZ;
binTorque2Z = relPos2X * binY - relPos2Y * binX;
norTorqueUnit1X = norTorque1X * invI1e00 + norTorque1Y * invI1e01 + norTorque1Z * invI1e02;
norTorqueUnit1Y = norTorque1X * invI1e10 + norTorque1Y * invI1e11 + norTorque1Z * invI1e12;
norTorqueUnit1Z = norTorque1X * invI1e20 + norTorque1Y * invI1e21 + norTorque1Z * invI1e22;
norTorqueUnit2X = norTorque2X * invI2e00 + norTorque2Y * invI2e01 + norTorque2Z * invI2e02;
norTorqueUnit2Y = norTorque2X * invI2e10 + norTorque2Y * invI2e11 + norTorque2Z * invI2e12;
norTorqueUnit2Z = norTorque2X * invI2e20 + norTorque2Y * invI2e21 + norTorque2Z * invI2e22;
tanTorqueUnit1X = tanTorque1X * invI1e00 + tanTorque1Y * invI1e01 + tanTorque1Z * invI1e02;
tanTorqueUnit1Y = tanTorque1X * invI1e10 + tanTorque1Y * invI1e11 + tanTorque1Z * invI1e12;
tanTorqueUnit1Z = tanTorque1X * invI1e20 + tanTorque1Y * invI1e21 + tanTorque1Z * invI1e22;
tanTorqueUnit2X = tanTorque2X * invI2e00 + tanTorque2Y * invI2e01 + tanTorque2Z * invI2e02;
tanTorqueUnit2Y = tanTorque2X * invI2e10 + tanTorque2Y * invI2e11 + tanTorque2Z * invI2e12;
tanTorqueUnit2Z = tanTorque2X * invI2e20 + tanTorque2Y * invI2e21 + tanTorque2Z * invI2e22;
binTorqueUnit1X = binTorque1X * invI1e00 + binTorque1Y * invI1e01 + binTorque1Z * invI1e02;
binTorqueUnit1Y = binTorque1X * invI1e10 + binTorque1Y * invI1e11 + binTorque1Z * invI1e12;
binTorqueUnit1Z = binTorque1X * invI1e20 + binTorque1Y * invI1e21 + binTorque1Z * invI1e22;
binTorqueUnit2X = binTorque2X * invI2e00 + binTorque2Y * invI2e01 + binTorque2Z * invI2e02;
binTorqueUnit2Y = binTorque2X * invI2e10 + binTorque2Y * invI2e11 + binTorque2Z * invI2e12;
binTorqueUnit2Z = binTorque2X * invI2e20 + binTorque2Y * invI2e21 + binTorque2Z * invI2e22;
var tmp1X:Number;
var tmp1Y:Number;
var tmp1Z:Number;
var tmp2X:Number;
var tmp2Y:Number;
var tmp2Z:Number;
tmp1X = norTorque1X * invI1e00 + norTorque1Y * invI1e01 + norTorque1Z * invI1e02;
tmp1Y = norTorque1X * invI1e10 + norTorque1Y * invI1e11 + norTorque1Z * invI1e12;
tmp1Z = norTorque1X * invI1e20 + norTorque1Y * invI1e21 + norTorque1Z * invI1e22;
tmp2X = tmp1Y * relPos1Z - tmp1Z * relPos1Y;
tmp2Y = tmp1Z * relPos1X - tmp1X * relPos1Z;
tmp2Z = tmp1X * relPos1Y - tmp1Y * relPos1X;
tmp1X = norTorque2X * invI2e00 + norTorque2Y * invI2e01 + norTorque2Z * invI2e02;
tmp1Y = norTorque2X * invI2e10 + norTorque2Y * invI2e11 + norTorque2Z * invI2e12;
tmp1Z = norTorque2X * invI2e20 + norTorque2Y * invI2e21 + norTorque2Z * invI2e22;
tmp2X += tmp1Y * relPos2Z - tmp1Z * relPos2Y;
tmp2Y += tmp1Z * relPos2X - tmp1X * relPos2Z;
tmp2Z += tmp1X * relPos2Y - tmp1Y * relPos2X;
normalDenominator = 1 / (invM1 + invM2 + norX * tmp2X + norY * tmp2Y + norZ * tmp2Z);
tmp1X = tanTorque1X * invI1e00 + tanTorque1Y * invI1e01 + tanTorque1Z * invI1e02;
tmp1Y = tanTorque1X * invI1e10 + tanTorque1Y * invI1e11 + tanTorque1Z * invI1e12;
tmp1Z = tanTorque1X * invI1e20 + tanTorque1Y * invI1e21 + tanTorque1Z * invI1e22;
tmp2X = tmp1Y * relPos1Z - tmp1Z * relPos1Y;
tmp2Y = tmp1Z * relPos1X - tmp1X * relPos1Z;
tmp2Z = tmp1X * relPos1Y - tmp1Y * relPos1X;
tmp1X = tanTorque2X * invI2e00 + tanTorque2Y * invI2e01 + tanTorque2Z * invI2e02;
tmp1Y = tanTorque2X * invI2e10 + tanTorque2Y * invI2e11 + tanTorque2Z * invI2e12;
tmp1Z = tanTorque2X * invI2e20 + tanTorque2Y * invI2e21 + tanTorque2Z * invI2e22;
tmp2X += tmp1Y * relPos2Z - tmp1Z * relPos2Y;
tmp2Y += tmp1Z * relPos2X - tmp1X * relPos2Z;
tmp2Z += tmp1X * relPos2Y - tmp1Y * relPos2X;
tangentDenominator = 1 / (invM1 + invM2 + tanX * tmp2X + tanY * tmp2Y + tanZ * tmp2Z);
tmp1X = binTorque1X * invI1e00 + binTorque1Y * invI1e01 + binTorque1Z * invI1e02;
tmp1Y = binTorque1X * invI1e10 + binTorque1Y * invI1e11 + binTorque1Z * invI1e12;
tmp1Z = binTorque1X * invI1e20 + binTorque1Y * invI1e21 + binTorque1Z * invI1e22;
tmp2X = tmp1Y * relPos1Z - tmp1Z * relPos1Y;
tmp2Y = tmp1Z * relPos1X - tmp1X * relPos1Z;
tmp2Z = tmp1X * relPos1Y - tmp1Y * relPos1X;
tmp1X = binTorque2X * invI2e00 + binTorque2Y * invI2e01 + binTorque2Z * invI2e02;
tmp1Y = binTorque2X * invI2e10 + binTorque2Y * invI2e11 + binTorque2Z * invI2e12;
tmp1Z = binTorque2X * invI2e20 + binTorque2Y * invI2e21 + binTorque2Z * invI2e22;
tmp2X += tmp1Y * relPos2Z - tmp1Z * relPos2Y;
tmp2Y += tmp1Z * relPos2X - tmp1X * relPos2Z;
tmp2Z += tmp1X * relPos2Y - tmp1Y * relPos2X;
binormalDenominator = 1 / (invM1 + invM2 + binX * tmp2X + binY * tmp2Y + binZ * tmp2Z);
if (warmStarted) {
tmp1X = norX * normalImpulse + tanX * tangentImpulse + binX * binormalImpulse;
tmp1Y = norY * normalImpulse + tanY * tangentImpulse + binY * binormalImpulse;
tmp1Z = norZ * normalImpulse + tanZ * tangentImpulse + binZ * binormalImpulse;
lVel1.x += tmp1X * invM1;
lVel1.y += tmp1Y * invM1;
lVel1.z += tmp1Z * invM1;
aVel1.x += norTorqueUnit1X * normalImpulse + tanTorqueUnit1X * tangentImpulse + binTorqueUnit1X * binormalImpulse;
aVel1.y += norTorqueUnit1Y * normalImpulse + tanTorqueUnit1Y * tangentImpulse + binTorqueUnit1Y * binormalImpulse;
aVel1.z += norTorqueUnit1Z * normalImpulse + tanTorqueUnit1Z * tangentImpulse + binTorqueUnit1Z * binormalImpulse;
lVel2.x -= tmp1X * invM2;
lVel2.y -= tmp1Y * invM2;
lVel2.z -= tmp1Z * invM2;
aVel2.x -= norTorqueUnit2X * normalImpulse + tanTorqueUnit2X * tangentImpulse + binTorqueUnit2X * binormalImpulse;
aVel2.y -= norTorqueUnit2Y * normalImpulse + tanTorqueUnit2Y * tangentImpulse + binTorqueUnit2Y * binormalImpulse;
aVel2.z -= norTorqueUnit2Z * normalImpulse + tanTorqueUnit2Z * tangentImpulse + binTorqueUnit2Z * binormalImpulse;
rvn = 0; // disabling bounce
}
if (rvn > -1) {
rvn = 0; // disabling bounce
}
targetNormalVelocity = restitution * -rvn;
var separationalVelocity:Number = -overlap - 0.05; // allow 5cm overlap
if (separationalVelocity > 0) {
separationalVelocity *= invTimeStep * 0.05;
if (targetNormalVelocity < separationalVelocity) {
targetNormalVelocity = separationalVelocity;
}
}
}
override public function solve():void {
var oldImpulse1:Number;
var newImpulse1:Number;
var oldImpulse2:Number;
var newImpulse2:Number;
var rvn:Number;
var forceX:Number;
var forceY:Number;
var forceZ:Number;
var tmpX:Number;
var tmpY:Number;
var tmpZ:Number;
rvn =
(lVel2.x - lVel1.x) * norX + (lVel2.y - lVel1.y) * norY + (lVel2.z - lVel1.z) * norZ +
aVel2.x * norTorque2X + aVel2.y * norTorque2Y + aVel2.z * norTorque2Z -
aVel1.x * norTorque1X - aVel1.y * norTorque1Y - aVel1.z * norTorque1Z
;
oldImpulse1 = normalImpulse;
newImpulse1 = (rvn - targetNormalVelocity) * normalDenominator;
normalImpulse += newImpulse1;
if (normalImpulse > 0) normalImpulse = 0;
newImpulse1 = normalImpulse - oldImpulse1;
forceX = norX * newImpulse1;
forceY = norY * newImpulse1;
forceZ = norZ * newImpulse1;
lVel1.x += forceX * invM1;
lVel1.y += forceY * invM1;
lVel1.z += forceZ * invM1;
aVel1.x += norTorqueUnit1X * newImpulse1;
aVel1.y += norTorqueUnit1Y * newImpulse1;
aVel1.z += norTorqueUnit1Z * newImpulse1;
lVel2.x -= forceX * invM2;
lVel2.y -= forceY * invM2;
lVel2.z -= forceZ * invM2;
aVel2.x -= norTorqueUnit2X * newImpulse1;
aVel2.y -= norTorqueUnit2Y * newImpulse1;
aVel2.z -= norTorqueUnit2Z * newImpulse1;
var max:Number = -normalImpulse * friction;
relVelX = lVel2.x - lVel1.x;
relVelY = lVel2.y - lVel1.y;
relVelZ = lVel2.z - lVel1.z;
rvn =
relVelX * tanX + relVelY * tanY + relVelZ * tanZ +
aVel2.x * tanTorque2X + aVel2.y * tanTorque2Y + aVel2.z * tanTorque2Z -
aVel1.x * tanTorque1X - aVel1.y * tanTorque1Y - aVel1.z * tanTorque1Z
;
oldImpulse1 = tangentImpulse;
newImpulse1 = rvn * tangentDenominator;
tangentImpulse += newImpulse1;
rvn =
relVelX * binX + relVelY * binY + relVelZ * binZ +
aVel2.x * binTorque2X + aVel2.y * binTorque2Y + aVel2.z * binTorque2Z -
aVel1.x * binTorque1X - aVel1.y * binTorque1Y - aVel1.z * binTorque1Z
;
oldImpulse2 = binormalImpulse;
newImpulse2 = rvn * binormalDenominator;
binormalImpulse += newImpulse2;
var len:Number = tangentImpulse * tangentImpulse + binormalImpulse * binormalImpulse;
if (len > max * max) {
len = max / Math.sqrt(len);
tangentImpulse *= len;
binormalImpulse *= len;
}
newImpulse1 = tangentImpulse - oldImpulse1;
newImpulse2 = binormalImpulse - oldImpulse2;
forceX = tanX * newImpulse1 + binX * newImpulse2;
forceY = tanY * newImpulse1 + binY * newImpulse2;
forceZ = tanZ * newImpulse1 + binZ * newImpulse2;
lVel1.x += forceX * invM1;
lVel1.y += forceY * invM1;
lVel1.z += forceZ * invM1;
aVel1.x += tanTorqueUnit1X * newImpulse1 + binTorqueUnit1X * newImpulse2;
aVel1.y += tanTorqueUnit1Y * newImpulse1 + binTorqueUnit1Y * newImpulse2;
aVel1.z += tanTorqueUnit1Z * newImpulse1 + binTorqueUnit1Z * newImpulse2;
lVel2.x -= forceX * invM2;
lVel2.y -= forceY * invM2;
lVel2.z -= forceZ * invM2;
aVel2.x -= tanTorqueUnit2X * newImpulse1 + binTorqueUnit2X * newImpulse2;
aVel2.y -= tanTorqueUnit2Y * newImpulse1 + binTorqueUnit2Y * newImpulse2;
aVel2.z -= tanTorqueUnit2Z * newImpulse1 + binTorqueUnit2Z * newImpulse2;
}
override public function postSolve():void {
if (shape1.numContacts < Shape.MAX_CONTACTS) {
shape1.contacts[shape1.numContacts++] = this;
}
if (shape2.numContacts < Shape.MAX_CONTACTS) {
shape2.contacts[shape2.numContacts++] = this;
}
relativePosition1.x = relPos1X;
relativePosition1.y = relPos1Y;
relativePosition1.z = relPos1Z;
relativePosition2.x = relPos2X;
relativePosition2.y = relPos2Y;
relativePosition2.z = relPos2Z;
relativeVelocity.x = (lVel2.x + aVel2.y * relPos2Z - aVel2.z * relPos2Y) - (lVel1.x + aVel1.y * relPos1Z - aVel1.z * relPos1Y);
relativeVelocity.y = (lVel2.y + aVel2.z * relPos2X - aVel2.x * relPos2Z) - (lVel1.y + aVel1.z * relPos1X - aVel1.x * relPos1Z);
relativeVelocity.z = (lVel2.z + aVel2.x * relPos2Y - aVel2.y * relPos2X) - (lVel1.z + aVel1.x * relPos1Y - aVel1.y * relPos1X);
normal.x = norX;
normal.y = norY;
normal.z = norZ;
tangent.x = tanX;
tangent.y = tanY;
tangent.z = tanZ;
binormal.x = binX;
binormal.y = binY;
binormal.z = binZ;
}
}
class RigidBody {
public static const BODY_DYNAMIC:uint = 0x0;
public static const BODY_STATIC:uint = 0x1;
public static const MAX_SHAPES:uint = 64;
public var type:uint;
public var position:Vec3;
public var linearVelocity:Vec3;
public var orientation:Quat;
public var rotation:Mat33;
public var angularVelocity:Vec3;
public var mass:Number;
public var invertMass:Number;
public var invertInertia:Mat33;
public var localInertia:Mat33;
public var invertLocalInertia:Mat33;
public var shapes:Vector.<Shape>;
public var numShapes:uint;
public var parent:World;
public function RigidBody(rad:Number = 0, ax:Number = 0, ay:Number = 0, az:Number = 0) {
position = new Vec3();
linearVelocity = new Vec3();
var len:Number = ax * ax + ay * ay + az * az;
if (len > 0) {
len = 1 / Math.sqrt(len);
ax *= len;
ay *= len;
az *= len;
}
var sin:Number = Math.sin(rad * 0.5);
var cos:Number = Math.cos(rad * 0.5);
orientation = new Quat(cos, sin * ax, sin * ay, sin * az);
rotation = new Mat33();
angularVelocity = new Vec3();
invertInertia = new Mat33();
localInertia = new Mat33();
invertLocalInertia = new Mat33();
shapes = new Vector.<Shape>(MAX_SHAPES, true);
}
public function addShape(shape:Shape):void {
if (numShapes == MAX_SHAPES) {
throw new Error("これ以上剛体に形状を追加することはできません");
}
if (shape.parent) {
throw new Error("一つの形状を複数剛体に追加することはできません");
}
shapes[numShapes++] = shape;
shape.parent = this;
if (parent) {
parent.addShape(shape);
}
}
public function removeShape(shape:Shape, index:int = -1):void {
if (index < 0) {
for (var i:int = 0; i < numShapes; i++) {
if (shape == shapes[i]) {
index = i;
break;
}
}
if (index == -1) {
return;
}
} else if (index >= numShapes) {
throw new Error("削除する形状のインデックスが範囲外です");
}
var remove:Shape = shapes[index];
remove.parent = null;
if (parent) {
parent.removeShape(remove);
}
for (var j:int = index; j < numShapes - 1; j++) {
shapes[j] = shapes[j + 1];
}
shapes[--numShapes] = null;
}
public function setupMass(type:uint = BODY_DYNAMIC):void {
this.type = type;
position.init();
mass = 0;
localInertia.init(0, 0, 0, 0, 0, 0, 0, 0, 0);
var invRot:Mat33 = new Mat33(); // = rotation ^ -1
invRot.transpose(rotation);
var tmpM:Mat33 = new Mat33();
var tmpV:Vec3 = new Vec3();
var denom:Number = 0;
for (var i:int = 0; i < numShapes; i++) {
var shape:Shape = shapes[i];
// relativeRotation = (rotation ^ -1) * shape.rotation
shape.relativeRotation.mul(invRot, shape.rotation);
position.add(position, tmpV.scale(shape.position, shape.mass));
denom += shape.mass;
mass += shape.mass;
// inertia = rotation * localInertia * (rotation ^ -1)
tmpM.mul(shape.relativeRotation, tmpM.mul(shape.localInertia, tmpM.transpose(shape.relativeRotation)));
localInertia.add(localInertia, tmpM);
}
position.scale(position, 1 / denom);
invertMass = 1 / mass;
var xy:Number = 0;
var yz:Number = 0;
var zx:Number = 0;
for (var j:int = 0; j < numShapes; j++) {
shape = shapes[j];
var relPos:Vec3 = shape.localRelativePosition;
relPos.sub(shape.position, position).mulMat(invRot, relPos);
shape.updateProxy();
localInertia.e00 += shape.mass * (relPos.y * relPos.y + relPos.z * relPos.z);
localInertia.e11 += shape.mass * (relPos.x * relPos.x + relPos.z * relPos.z);
localInertia.e22 += shape.mass * (relPos.x * relPos.x + relPos.y * relPos.y);
xy -= shape.mass * relPos.x * relPos.y;
yz -= shape.mass * relPos.y * relPos.z;
zx -= shape.mass * relPos.z * relPos.x;
}
localInertia.e01 = xy;
localInertia.e10 = xy;
localInertia.e02 = zx;
localInertia.e20 = zx;
localInertia.e12 = yz;
localInertia.e21 = yz;
invertLocalInertia.invert(localInertia);
if (type == BODY_STATIC) {
invertMass = 0;
invertLocalInertia.init(0, 0, 0, 0, 0, 0, 0, 0, 0);
}
var r00:Number = rotation.e00;
var r01:Number = rotation.e01;
var r02:Number = rotation.e02;
var r10:Number = rotation.e10;
var r11:Number = rotation.e11;
var r12:Number = rotation.e12;
var r20:Number = rotation.e20;
var r21:Number = rotation.e21;
var r22:Number = rotation.e22;
var i00:Number = invertLocalInertia.e00;
var i01:Number = invertLocalInertia.e01;
var i02:Number = invertLocalInertia.e02;
var i10:Number = invertLocalInertia.e10;
var i11:Number = invertLocalInertia.e11;
var i12:Number = invertLocalInertia.e12;
var i20:Number = invertLocalInertia.e20;
var i21:Number = invertLocalInertia.e21;
var i22:Number = invertLocalInertia.e22;
var e00:Number = r00 * i00 + r01 * i10 + r02 * i20;
var e01:Number = r00 * i01 + r01 * i11 + r02 * i21;
var e02:Number = r00 * i02 + r01 * i12 + r02 * i22;
var e10:Number = r10 * i00 + r11 * i10 + r12 * i20;
var e11:Number = r10 * i01 + r11 * i11 + r12 * i21;
var e12:Number = r10 * i02 + r11 * i12 + r12 * i22;
var e20:Number = r20 * i00 + r21 * i10 + r22 * i20;
var e21:Number = r20 * i01 + r21 * i11 + r22 * i21;
var e22:Number = r20 * i02 + r21 * i12 + r22 * i22;
invertInertia.e00 = e00 * r00 + e01 * r01 + e02 * r02;
invertInertia.e01 = e00 * r10 + e01 * r11 + e02 * r12;
invertInertia.e02 = e00 * r20 + e01 * r21 + e02 * r22;
invertInertia.e10 = e10 * r00 + e11 * r01 + e12 * r02;
invertInertia.e11 = e10 * r10 + e11 * r11 + e12 * r12;
invertInertia.e12 = e10 * r20 + e11 * r21 + e12 * r22;
invertInertia.e20 = e20 * r00 + e21 * r01 + e22 * r02;
invertInertia.e21 = e20 * r10 + e21 * r11 + e22 * r12;
invertInertia.e22 = e20 * r20 + e21 * r21 + e22 * r22;
}
public function updateVelocity(timeStep:Number, gravity:Vec3):void {
if (type == BODY_DYNAMIC) {
linearVelocity.x += gravity.x * timeStep;
linearVelocity.y += gravity.y * timeStep;
linearVelocity.z += gravity.z * timeStep;
}
}
public function updatePosition(timeStep:Number):void {
var s:Number;
var x:Number;
var y:Number;
var z:Number;
if (type == BODY_STATIC) {
linearVelocity.x = 0;
linearVelocity.y = 0;
linearVelocity.z = 0;
angularVelocity.x = 0;
angularVelocity.y = 0;
angularVelocity.z = 0;
} else if (type == BODY_DYNAMIC) {
position.x += linearVelocity.x * timeStep;
position.y += linearVelocity.y * timeStep;
position.z += linearVelocity.z * timeStep;
var ax:Number = angularVelocity.x;
var ay:Number = angularVelocity.y;
var az:Number = angularVelocity.z;
var os:Number = orientation.s;
var ox:Number = orientation.x;
var oy:Number = orientation.y;
var oz:Number = orientation.z;
timeStep *= 0.5;
s = (-ax * ox - ay * oy - az * oz) * timeStep;
x = (ax * os + ay * oz - az * oy) * timeStep;
y = (-ax * oz + ay * os + az * ox) * timeStep;
z = (ax * oy - ay * ox + az * os) * timeStep;
os += s;
ox += x;
oy += y;
oz += z;
s = 1 / Math.sqrt(os * os + ox * ox + oy * oy + oz * oz);
orientation.s = os * s;
orientation.x = ox * s;
orientation.y = oy * s;
orientation.z = oz * s;
} else {
throw new Error("未定義の剛体の種類です");
}
s = orientation.s;
x = orientation.x;
y = orientation.y;
z = orientation.z;
var x2:Number = 2 * x;
var y2:Number = 2 * y;
var z2:Number = 2 * z;
var xx:Number = x * x2;
var yy:Number = y * y2;
var zz:Number = z * z2;
var xy:Number = x * y2;
var yz:Number = y * z2;
var xz:Number = x * z2;
var sx:Number = s * x2;
var sy:Number = s * y2;
var sz:Number = s * z2;
rotation.e00 = 1 - yy - zz;
rotation.e01 = xy - sz;
rotation.e02 = xz + sy;
rotation.e10 = xy + sz;
rotation.e11 = 1 - xx - zz;
rotation.e12 = yz - sx;
rotation.e20 = xz - sy;
rotation.e21 = yz + sx;
rotation.e22 = 1 - xx - yy;
var r00:Number = rotation.e00;
var r01:Number = rotation.e01;
var r02:Number = rotation.e02;
var r10:Number = rotation.e10;
var r11:Number = rotation.e11;
var r12:Number = rotation.e12;
var r20:Number = rotation.e20;
var r21:Number = rotation.e21;
var r22:Number = rotation.e22;
var i00:Number = invertLocalInertia.e00;
var i01:Number = invertLocalInertia.e01;
var i02:Number = invertLocalInertia.e02;
var i10:Number = invertLocalInertia.e10;
var i11:Number = invertLocalInertia.e11;
var i12:Number = invertLocalInertia.e12;
var i20:Number = invertLocalInertia.e20;
var i21:Number = invertLocalInertia.e21;
var i22:Number = invertLocalInertia.e22;
var e00:Number = r00 * i00 + r01 * i10 + r02 * i20;
var e01:Number = r00 * i01 + r01 * i11 + r02 * i21;
var e02:Number = r00 * i02 + r01 * i12 + r02 * i22;
var e10:Number = r10 * i00 + r11 * i10 + r12 * i20;
var e11:Number = r10 * i01 + r11 * i11 + r12 * i21;
var e12:Number = r10 * i02 + r11 * i12 + r12 * i22;
var e20:Number = r20 * i00 + r21 * i10 + r22 * i20;
var e21:Number = r20 * i01 + r21 * i11 + r22 * i21;
var e22:Number = r20 * i02 + r21 * i12 + r22 * i22;
invertInertia.e00 = e00 * r00 + e01 * r01 + e02 * r02;
invertInertia.e01 = e00 * r10 + e01 * r11 + e02 * r12;
invertInertia.e02 = e00 * r20 + e01 * r21 + e02 * r22;
invertInertia.e10 = e10 * r00 + e11 * r01 + e12 * r02;
invertInertia.e11 = e10 * r10 + e11 * r11 + e12 * r12;
invertInertia.e12 = e10 * r20 + e11 * r21 + e12 * r22;
invertInertia.e20 = e20 * r00 + e21 * r01 + e22 * r02;
invertInertia.e21 = e20 * r10 + e21 * r11 + e22 * r12;
invertInertia.e22 = e20 * r20 + e21 * r21 + e22 * r22;
for (var i:int = 0; i < numShapes; i++) {
var shape:Shape = shapes[i];
var relPos:Vec3 = shape.relativePosition;
var lRelPos:Vec3 = shape.localRelativePosition;
var relRot:Mat33 = shape.relativeRotation;
var rot:Mat33 = shape.rotation;
var lx:Number = lRelPos.x;
var ly:Number = lRelPos.y;
var lz:Number = lRelPos.z;
relPos.x = lx * r00 + ly * r01 + lz * r02;
relPos.y = lx * r10 + ly * r11 + lz * r12;
relPos.z = lx * r20 + ly * r21 + lz * r22;
shape.position.x = position.x + relPos.x;
shape.position.y = position.y + relPos.y;
shape.position.z = position.z + relPos.z;
e00 = relRot.e00;
e01 = relRot.e01;
e02 = relRot.e02;
e10 = relRot.e10;
e11 = relRot.e11;
e12 = relRot.e12;
e20 = relRot.e20;
e21 = relRot.e21;
e22 = relRot.e22;
rot.e00 = r00 * e00 + r01 * e10 + r02 * e20;
rot.e01 = r00 * e01 + r01 * e11 + r02 * e21;
rot.e02 = r00 * e02 + r01 * e12 + r02 * e22;
rot.e10 = r10 * e00 + r11 * e10 + r12 * e20;
rot.e11 = r10 * e01 + r11 * e11 + r12 * e21;
rot.e12 = r10 * e02 + r11 * e12 + r12 * e22;
rot.e20 = r20 * e00 + r21 * e10 + r22 * e20;
rot.e21 = r20 * e01 + r21 * e11 + r22 * e21;
rot.e22 = r20 * e02 + r21 * e12 + r22 * e22;
shape.updateProxy();
}
}
public function applyImpulse(position:Vec3, force:Vec3):void {
linearVelocity.x += force.x * invertMass;
linearVelocity.y += force.y * invertMass;
linearVelocity.z += force.z * invertMass;
var rel:Vec3 = new Vec3();
rel.sub(position, this.position).cross(rel, force).mulMat(invertInertia, rel);
angularVelocity.x += rel.x;
angularVelocity.y += rel.y;
angularVelocity.z += rel.z;
}
}
class World {
public static const MAX_BODIES:uint = 16384;
public static const MAX_SHAPES:uint = 32768;
public static const MAX_CONTACTS:uint = 65536;
public static const MAX_PAIRS:uint = 65536;
public var rigidBodies:Vector.<RigidBody>;
public var numRigidBodies:uint;
public var shapes:Vector.<Shape>;
public var numShapes:uint;
public var pairs:Vector.<Pair>;
public var numPairs:uint;
public var contacts:Vector.<Contact>;
private var contactsBuffer:Vector.<Contact>;
public var numContacts:uint;
public var timeStep:Number;
public var gravity:Vec3;
public var iteration:uint;
public var performance:Performance;
private var broadPhase:BroadPhase;
private var detectors:Vector.<Vector.<CollisionDetector>>;
private var contactInfos:Vector.<ContactInfo>;
private var numContactInfos:uint;
public function World(stepPerSecond:Number = 60) {
trace(OimoPhysics.DESCRIPTION);
timeStep = 1 / stepPerSecond;
iteration = 8;
gravity = new Vec3(0, -9.80665, 0);
rigidBodies = new Vector.<RigidBody>(MAX_BODIES, true);
shapes = new Vector.<Shape>(MAX_SHAPES, true);
pairs = new Vector.<Pair>(MAX_PAIRS, true);
performance = new Performance();
broadPhase = new SweepAndPruneBroadPhase();
// broadPhase = new BruteForceBroadPhase();
var numShapeTypes:uint = 3;
detectors = new Vector.<Vector.<CollisionDetector>>(numShapeTypes, true);
for (var i:int = 0; i < numShapeTypes; i++) {
detectors[i] = new Vector.<CollisionDetector>(numShapeTypes, true);
}
detectors[Shape.SHAPE_SPHERE][Shape.SHAPE_SPHERE] = new SphereSphereCollisionDetector();
detectors[Shape.SHAPE_SPHERE][Shape.SHAPE_BOX] = new SphereBoxCollisionDetector(false);
detectors[Shape.SHAPE_BOX][Shape.SHAPE_SPHERE] = new SphereBoxCollisionDetector(true);
detectors[Shape.SHAPE_BOX][Shape.SHAPE_BOX] = new BoxBoxCollisionDetector();
contactInfos = new Vector.<ContactInfo>(MAX_CONTACTS, true);
contacts = new Vector.<Contact>(MAX_CONTACTS, true);
contactsBuffer = new Vector.<Contact>(MAX_CONTACTS, true);
}
public function addRigidBody(rigidBody:RigidBody):void {
if (numRigidBodies == MAX_BODIES) {
throw new Error("これ以上ワールドに剛体を追加することはできません");
}
if (rigidBody.parent) {
throw new Error("一つの剛体を複数ワールドに追加することはできません");
}
rigidBodies[numRigidBodies++] = rigidBody;
var num:uint = rigidBody.numShapes;
for (var i:int = 0; i < num; i++) {
addShape(rigidBody.shapes[i]);
}
rigidBody.parent = this;
}
public function removeRigidBody(rigidBody:RigidBody, index:int = -1):void {
if (index < 0) {
for (var i:int = 0; i < numRigidBodies; i++) {
if (rigidBody == rigidBodies[i]) {
index = i;
break;
}
}
if (index == -1) {
return;
}
} else if (index >= numRigidBodies) {
throw new Error("削除する剛体のインデックスが範囲外です");
}
var remove:RigidBody = rigidBodies[index];
remove.parent = null;
var num:uint = rigidBody.numShapes;
for (var j:int = 0; j < num; j++) {
removeShape(rigidBody.shapes[j]);
}
for (var k:int = index; k < numRigidBodies - 1; k++) {
rigidBodies[k] = rigidBodies[k + 1];
}
rigidBodies[--numRigidBodies] = null;
}
public function addShape(shape:Shape):void {
if (numShapes == MAX_SHAPES) {
throw new Error("これ以上ワールドに形状を追加することはできません");
}
if (!shape.parent) {
throw new Error("ワールドに形状を単体で追加することはできません");
}
if (shape.parent.parent) {
throw new Error("一つの形状を複数ワールドに追加することはできません");
}
broadPhase.addProxy(shape.proxy);
shapes[numShapes++] = shape;
}
public function removeShape(shape:Shape, index:int = -1):void {
if (index < 0) {
for (var i:int = 0; i < numShapes; i++) {
if (shape == shapes[i]) {
index = i;
break;
}
}
if (index == -1) {
return;
}
} else if (index >= numShapes) {
throw new Error("削除する形状のインデックスが範囲外です");
}
var remove:Shape = shapes[index];
broadPhase.removeProxy(remove.proxy);
for (var j:int = index; j < numShapes - 1; j++) {
shapes[j] = shapes[j + 1];
}
shapes[--numShapes] = null;
}
public function step():void {
var start1:int = getTimer();
var tmp:Vector.<Contact> = contacts; // swap contacts
contacts = contactsBuffer;
contactsBuffer = tmp;
for (var i:int = 0; i < numRigidBodies; i++) {
rigidBodies[i].updateVelocity(timeStep, gravity);
}
performance.updateTime = getTimer() - start1;
collisionDetection();
collisionResponse();
var start2:int = getTimer();
for (var j:int = 0; j < numRigidBodies; j++) {
rigidBodies[j].updatePosition(timeStep);
}
performance.updateTime += getTimer() - start2;
performance.totalTime = getTimer() - start1;
}
private function collisionDetection():void {
collectContactInfos();
setupContacts();
}
private function collectContactInfos():void {
// broad phase
var start:int = getTimer();
numPairs = broadPhase.detectPairs(pairs);
performance.broadPhaseTime = getTimer() - start;
// narrow phase
performance.narrowPhaseTime = getTimer();
numContactInfos = 0;
for (var i:int = 0; i < numPairs; i++) {
var pair:Pair = pairs[i];
var s1:Shape = pair.shape1;
var s2:Shape = pair.shape2;
var detector:CollisionDetector = detectors[s1.type][s2.type];
if (detector) {
numContactInfos = detector.detectCollision(s1, s2, contactInfos, numContactInfos);
if (numContactInfos == MAX_CONTACTS) {
return;
}
}
}
}
private function setupContacts():void {
numContacts = numContactInfos;
for (var i:int = 0; i < numContacts; i++) {
if (!contacts[i]) {
contacts[i] = new Contact();
}
var c:Contact = contacts[i];
c.setupFromContactInfo(contactInfos[i]);
// search old contacts
var s1:Shape = c.shape1;
var s2:Shape = c.shape2;
var sc:Vector.<Contact>;
var numSc:uint;
if (s1.numContacts < s2.numContacts) {
sc = s1.contacts;
numSc = s1.numContacts;
} else {
sc = s2.contacts;
numSc = s2.numContacts;
}
for (var j:int = 0; j < numSc; j++) {
var oc:Contact = sc[j];
if (
(oc.shape1 == c.shape1 && oc.shape2 == c.shape2 ||
oc.shape1 == c.shape2 && oc.shape2 == c.shape1) &&
oc.id.equals(c.id)
) {
// warm starting
c.normalImpulse = oc.normalImpulse;
c.tangentImpulse = oc.tangentImpulse;
c.binormalImpulse = oc.binormalImpulse;
c.warmStarted = true;
break;
}
}
}
performance.narrowPhaseTime = getTimer() - performance.narrowPhaseTime;
}
private function collisionResponse():void {
var start:int = getTimer();
var invTimeStep:Number = 1 / timeStep;
// reset contact counts
for (var i:int = 0; i < numShapes; i++) {
shapes[i].numContacts = 0;
}
for (var j:int = 0; j < numContacts; j++) {
contacts[j].preSolve(timeStep, invTimeStep);
}
// solve system of equations
for (var k:int = 0; k < iteration; k++) {
for (var l:int = 0; l < numContacts; l++) {
contacts[l].solve();
}
}
for (var m:int = 0; m < numContacts; m++) {
contacts[m].postSolve();
}
performance.constraintsTime = getTimer() - start;
}
}
class DebugDraw {
private var w:uint;
private var h:uint;
private var wld:World;
private var c3d:Context3D;
private var gl:OimoGLMini;
private var m44:Mat44;
public function DebugDraw(width:uint, height:uint) {
w = width;
h = height;
m44 = new Mat44();
}
public function setContext3D(context3D:Context3D):void {
c3d = context3D;
gl = new OimoGLMini(c3d, w, h);
gl.material(1, 1, 0, 0.6, 32);
gl.registerSphere(0, 1, 10, 5);
gl.registerBox(1, 1, 1, 1);
gl.camera(0, 5, 10, 0, 0, 0, 0, 1, 0);
}
public function setWorld(world:World):void {
wld = world;
}
public function camera(
camX:Number, camY:Number, camZ:Number,
targetX:Number = 0, targetY:Number = 0, targetZ:Number = 0,
upX:Number = 0, upY:Number = 1, upZ:Number = 0
):void {
gl.camera(camX, camY, camZ, targetX, targetY, targetZ, upX, upY, upZ);
var dx:Number = targetX - camX;
var dy:Number = targetY - camY;
var dz:Number = targetZ - camZ;
var len:Number = Math.sqrt(dx * dx + dy * dy + dz * dz);
if (len > 0) len = 1 / len;
gl.directionalLightDirection(dx * len, dy * len, dz * len);
}
public function render():void {
if (!c3d) {
return;
}
gl.beginScene(0.1, 0.1, 0.1);
var alpha:Number = 1;
var drawContacts:Boolean = false;
var drawNormals:Boolean = true;
var drawForces:Boolean = true;
var cs:Vector.<Contact> = wld.contacts;
var num:uint = wld.numContacts;
if (drawContacts) {
for (var j:int = 0; j < num; j++) {
var c:Contact = cs[j];
gl.push();
gl.translate(c.position.x, c.position.y, c.position.z);
if (drawNormals) gl.push();
if (c.warmStarted) {
gl.scale(0.1, 0.1, 0.1);
gl.color(0.5, 0.5, 0.5);
} else {
gl.scale(0.15, 0.15, 0.15);
gl.color(1, 1, 0);
}
gl.drawTriangles(0);
if (drawNormals) {
gl.pop();
gl.push();
if (drawForces) gl.translate(c.normal.x * -c.normalImpulse * 0.3, c.normal.y * -c.normalImpulse * 0.3, c.normal.z * -c.normalImpulse * 0.3);
else gl.translate(c.normal.x * 0.3, c.normal.y * 0.3, c.normal.z * 0.3);
gl.scale(0.1, 0.1, 0.1);
gl.color(1, 0, 0);
gl.drawTriangles(0);
gl.pop();
if (!drawForces) {
gl.push();
gl.translate(c.tangent.x * 0.3, c.tangent.y * 0.3, c.tangent.z * 0.3);
gl.scale(0.1, 0.1, 0.1);
gl.color(0, 0.6, 0);
gl.drawTriangles(0);
gl.pop();
gl.push();
gl.translate(c.binormal.x * 0.3, c.binormal.y * 0.3, c.binormal.z * 0.3);
gl.scale(0.1, 0.1, 0.1);
gl.color(0, 0, 1);
gl.drawTriangles(0);
gl.pop();
}
if (drawForces) {
gl.push();
gl.translate(
(c.tangent.x * c.tangentImpulse + c.binormal.x * c.binormalImpulse) * 0.3,
(c.tangent.y * c.tangentImpulse + c.binormal.y * c.binormalImpulse) * 0.3,
(c.tangent.z * c.tangentImpulse + c.binormal.z * c.binormalImpulse) * 0.3
);
gl.scale(0.1, 0.1, 0.1);
gl.color(0, 1, 1);
gl.drawTriangles(0);
gl.pop();
}
}
gl.pop();
}
}
var ss:Vector.<Shape> = wld.shapes;
num = wld.numShapes;
for (var i:int = 0; i < num; i++) {
var s:Shape = ss[i];
gl.push();
m44.copyMat33(s.rotation);
m44.e03 = s.position.x;
m44.e13 = s.position.y;
m44.e23 = s.position.z;
gl.transform(m44);
switch(s.parent.type) {
case RigidBody.BODY_DYNAMIC:
if (s.id & 1) gl.color(1, 0.6, 0.1, alpha);
else gl.color(0.6, 0.1, 1, alpha);
break;
case RigidBody.BODY_STATIC:
gl.color(0.4, 0.4, 0.4, alpha);
break;
}
switch(s.type) {
case Shape.SHAPE_SPHERE:
var sph:SphereShape = s as SphereShape;
gl.scale(sph.radius, sph.radius, sph.radius);
gl.drawTriangles(0);
break;
case Shape.SHAPE_BOX:
var box:BoxShape = s as BoxShape;
gl.scale(box.width, box.height, box.depth);
gl.drawTriangles(1);
break;
}
gl.pop();
}
gl.endScene();
}
}
class Performance {
public var broadPhaseTime:uint;
public var narrowPhaseTime:uint;
public var constraintsTime:uint;
public var updateTime:uint;
public var totalTime:uint;
public function Performance() {
}
}
final class OimoPhysics {
public static const VERSION:String = "1.0.0";
public static const DESCRIPTION:String = "OimoPhysics " + VERSION + " (c) 2012 EL-EMENT saharan";
public function OimoPhysics() {
throw new Error("OimoPhysics オブジェクトを作成することはできません");
}
}