wassname/phaser
1
0
Fork 0
mirror of https://github.com/wassname/phaser.git synced 2026-07-05 17:30:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sorted out the bounds for when sprites are in trimmed texture atlases to stop the physics checks going insane. Also bundled in Advanced Physics lib, although not hooked up yet.

Browse Source
This commit is contained in:
Richard Davey
2013-09-03 15:35:40 +01:00
parent 923c2504ba
commit bdc1c2ceb9
34 changed files with 7600 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Phaser.sublime-workspace
+1301 -53
View File
File diff suppressed because it is too large Load Diff
examples/body1.php
+77
View File
@@ -0,0 +1,77 @@
<!DOCTYPE HTML>
<html>
<head>
<title>phaser.js - a new beginning</title>
<?php
require('js.php');
?>
</head>
<body>
<script type="text/javascript">
(function () {
var game = new Phaser.Game(800, 600, Phaser.CANVAS, '', { preload: preload, create: create, update: update, render: render });
function preload() {
game.load.atlasJSONHash('bot', 'assets/sprites/running_bot.png', 'assets/sprites/running_bot.json');
}
var s;
function create() {
game.world._stage.backgroundColorString = '#182d3b';
s = game.add.sprite(game.world.centerX, game.world.centerY, 'bot');
// s.anchor.setTo(0.5, 0.5);
// s.body.offset.setTo(0, 0);
// s.scale.setTo(2, 2);
s.animations.add('run');
s.animations.play('run', 10, true);
}
function update() {
s.rotation += 0.01;
if (game.input.keyboard.isDown(Phaser.Keyboard.LEFT))
{
s.x -= 4;
}
else if (game.input.keyboard.isDown(Phaser.Keyboard.RIGHT))
{
s.x += 4;
}
if (game.input.keyboard.isDown(Phaser.Keyboard.UP))
{
s.y -= 4;
}
else if (game.input.keyboard.isDown(Phaser.Keyboard.DOWN))
{
s.y += 4;
}
}
function render() {
game.debug.renderSpriteCorners(s, true, true);
// game.debug.renderRectangle(s.body.bounds, 'rgba(255,0,0,0.3)');
game.debug.renderSpriteInfo(s, 20, 32);
}
})();
</script>
</body>
</html>
examples/js.php
+25 -13
View File
@@ -38,7 +38,6 @@
<script src="../src/pixi/textures/BaseTexture.js"></script>
<script src="../src/pixi/textures/Texture.js"></script>
<script src="../src/pixi/textures/RenderTexture.js"></script>
<!--<script src="../src/pixi/utils/Detector.js"></script>-->
<script src="../src/pixi/utils/EventTarget.js"></script>
<script src="../src/pixi/utils/Polyk.js"></script>
<script src="../src/pixi/utils/Utils.js"></script>
@@ -53,51 +52,64 @@
<script src="../src/core/Stage.js"></script>
<script src="../src/core/World.js"></script>
<script src="../src/core/Game.js"></script>
<script src="../src/input/Input.js"></script>
<script src="../src/input/Keyboard.js"></script>
<script src="../src/input/Mouse.js"></script>
<script src="../src/input/MSPointer.js"></script>
<script src="../src/input/Pointer.js"></script>
<script src="../src/input/Touch.js"></script>
<script src="../src/system/Canvas.js"></script>
<script src="../src/gameobjects/GameObjectFactory.js"></script>
<script src="../src/gameobjects/Sprite.js"></script>
<script src="../src/gameobjects/TileSprite.js"></script>
<script src="../src/gameobjects/Text.js"></script>
<script src="../src/system/Canvas.js"></script>
<script src="../src/system/Device.js"></script>
<script src="../src/system/RequestAnimationFrame.js"></script>
<script src="../src/math/RandomDataGenerator.js"></script>
<script src="../src/math/Math.js"></script>
<script src="../src/geom/Circle.js"></script>
<script src="../src/geom/Point.js"></script>
<script src="../src/geom/Rectangle.js"></script>
<script src="../src/net/Net.js"></script>
<script src="../src/tween/TweenManager.js"></script>
<script src="../src/tween/Tween.js"></script>
<script src="../src/tween/Easing.js"></script>
<script src="../src/time/Time.js"></script>
<script src="../src/animation/AnimationManager.js"></script>
<script src="../src/animation/Animation.js"></script>
<script src="../src/animation/Frame.js"></script>
<script src="../src/animation/FrameData.js"></script>
<script src="../src/animation/Parser.js"></script>
<script src="../src/loader/Cache.js"></script>
<script src="../src/loader/Loader.js"></script>
<script src="../src/sound/Sound.js"></script>
<script src="../src/sound/SoundManager.js"></script>
<script src="../src/utils/Debug.js"></script>
<script src="../src/physics/arcade/ArcadePhysics.js"></script>
<script src="../src/physics/arcade/Body.js"></script>
<script src="../src/physics/advanced/Math.js"></script>
<script src="../src/physics/advanced/Util.js"></script>
<script src="../src/physics/advanced/Collision.js"></script>
<script src="../src/physics/advanced/Body.js"></script>
<script src="../src/physics/advanced/Joint.js"></script>
<script src="../src/physics/advanced/Shape.js"></script>
<script src="../src/physics/advanced/Contact.js"></script>
<script src="../src/physics/advanced/ContactSolver.js"></script>
<script src="../src/physics/advanced/Space.js"></script>
<script src="../src/physics/advanced/joints/Angle.js"></script>
<script src="../src/physics/advanced/joints/Revolute.js"></script>
<script src="../src/physics/advanced/joints/Weld.js"></script>
<script src="../src/physics/advanced/joints/Wheel.js"></script>
<script src="../src/physics/advanced/joints/Prismatic.js"></script>
<script src="../src/physics/advanced/joints/Distance.js"></script>
<script src="../src/physics/advanced/joints/Rope.js"></script>
<script src="../src/physics/advanced/joints/Mouse.js"></script>
<script src="../src/physics/advanced/shapes/Circle.js"></script>
<script src="../src/physics/advanced/shapes/Segment.js"></script>
<script src="../src/physics/advanced/shapes/Poly.js"></script>
<script src="../src/physics/advanced/shapes/Triangle.js"></script>
<script src="../src/physics/advanced/shapes/Box.js"></script>
examples/sprite4.php
+70
View File
@@ -0,0 +1,70 @@
<!DOCTYPE HTML>
<html>
<head>
<title>phaser.js - a new beginning</title>
<?php
require('js.php');
?>
</head>
<body>
<script type="text/javascript">
(function () {
var game = new Phaser.Game(800, 600, Phaser.CANVAS, '', { preload: preload, create: create, update: update, render: render });
function preload() {
game.load.atlasJSONHash('bot', 'assets/sprites/running_bot.png', 'assets/sprites/running_bot.json');
}
var s;
function create() {
// game.world._stage.backgroundColorString = '#182d3b';
s = game.add.sprite(game.world.centerX, game.world.centerY, 'bot');
// s.anchor.setTo(0.5, 0.5);
s.scale.setTo(2, 2);
s.animations.add('run');
s.animations.play('run', 10, true);
}
function update() {
if (game.input.keyboard.isDown(Phaser.Keyboard.LEFT))
{
s.x -= 4;
}
else if (game.input.keyboard.isDown(Phaser.Keyboard.RIGHT))
{
s.x += 4;
}
if (game.input.keyboard.isDown(Phaser.Keyboard.UP))
{
s.y -= 4;
}
else if (game.input.keyboard.isDown(Phaser.Keyboard.DOWN))
{
s.y += 4;
}
}
function render() {
game.debug.renderSpriteCorners(s, false, false);
game.debug.renderSpriteInfo(s, 20, 32);
}
})();
</script>
</body>
</html>
src/animation/Animation.js
+3 -2
View File
@@ -41,8 +41,8 @@ Phaser.Animation.prototype = {
*/
play: function (frameRate, loop) {
if (typeof frameRate === "undefined") { frameRate = null; }
if (typeof loop === "undefined") { loop = null; }
frameRate = frameRate || null;
loop = loop || null;
if (frameRate !== null)
{
@@ -113,6 +113,7 @@ Phaser.Animation.prototype = {
{
this._frameIndex = 0;
this.currentFrame = this._frameData.getFrame(this._frames[this._frameIndex]);
this._parent.setTexture(PIXI.TextureCache[this.currentFrame.uuid]);
// this._parent.events.onAnimationLoop.dispatch(this._parent, this);
}
else
src/animation/AnimationManager.js
+8 -1
View File
@@ -177,19 +177,24 @@ Phaser.AnimationManager.prototype = {
/**
* Update animation and parent sprite's bounds.
* Returns true if a new frame has been set, otherwise false.
*/
update: function () {
if (this.updateIfVisible && this._parent.visible == false)
{
return;
return false;
}
if (this.currentAnim && this.currentAnim.update() == true)
{
this.currentFrame = this.currentAnim.currentFrame;
this._parent.currentFrame = this.currentFrame;
return true;
}
return false;
},
/**
@@ -256,6 +261,7 @@ Object.defineProperty(Phaser.AnimationManager.prototype, "frame", {
{
this.currentFrame = this._frameData.getFrame(value);
this._frameIndex = value;
this._parent.currentFrame = this.currentFrame;
this._parent.setTexture(PIXI.TextureCache[this.currentFrame.uuid]);
}
@@ -282,6 +288,7 @@ Object.defineProperty(Phaser.AnimationManager.prototype, "frameName", {
{
this.currentFrame = this._frameData.getFrameByName(value);
this._frameIndex = this.currentFrame.index;
this._parent.currentFrame = this.currentFrame;
this._parent.setTexture(PIXI.TextureCache[this.currentFrame.uuid]);
}
else
src/animation/Frame.js
+20 -1
View File
@@ -14,6 +14,10 @@ Phaser.Animation.Frame = function (x, y, width, height, name, uuid) {
this.y = y;
this.width = width;
this.height = height;
this.sourceSizeW = width;
this.sourceSizeH = height;
this.centerX = Math.floor(width / 2);
this.centerY = Math.floor(height / 2);
this.name = name;
this.uuid = uuid;
this.distance = Phaser.Math.distance(0, 0, width, height);
@@ -51,6 +55,18 @@ Phaser.Animation.Frame.prototype = {
*/
height: 0,
/**
* center X position within the image to cut from.
* @type {number}
*/
centerX: 0,
/**
* center Y position within the image to cut from.
* @type {number}
*/
centerY: 0,
/**
* The distance from the top left to the bottom-right of this Frame.
* @type {number}
@@ -136,11 +152,14 @@ Phaser.Animation.Frame.prototype = {
this.trimmed = trimmed;
if (trimmed) {
if (trimmed)
{
this.width = actualWidth;
this.height = actualHeight;
this.sourceSizeW = actualWidth;
this.sourceSizeH = actualHeight;
this.centerX = Math.floor(actualWidth / 2);
this.centerY = Math.floor(actualHeight / 2);
this.spriteSourceSizeX = destX;
this.spriteSourceSizeY = destY;
this.spriteSourceSizeW = destWidth;
src/gameobjects/Sprite.js
+66 -54
View File
@@ -24,7 +24,6 @@ Phaser.Sprite = function (game, x, y, key, frame) {
else
{
// No texture yet
console.log('no texture yet');
PIXI.Sprite.call(this);
}
@@ -36,43 +35,37 @@ Phaser.Sprite = function (game, x, y, key, frame) {
*/
this.animations = new Phaser.AnimationManager(this);
// PIXI.DisplayObjectContainer.call(this);
/**
* The anchor sets the origin point of the texture.
* The default is 0,0 this means the textures origin is the top left
* Setting than anchor to 0.5,0.5 means the textures origin is centered
* Setting the anchor to 1,1 would mean the textures origin points will be the bottom right
*
* @property anchor
* @type Point
*/
this.anchor = new Phaser.Point();
/**
* The texture that the sprite is using
*
* @property texture
* @type Texture
*/
// this.texture = PIXI.TextureCache[key];
if (this.game.cache.isSpriteSheet(key))
{
this.animations.loadFrameData(this.game.cache.getFrameData(key));
if (frame !== null)
{
if (typeof frame === 'string')
{
this.frameName = frame;
}
else
{
this.frame = frame;
}
}
}
else
{
this.currentFrame = new Phaser.Animation.Frame(x, y, width, height, '', '');
}
if (frame !== null)
{
if (typeof frame === 'string')
{
this.frameName = frame;
}
else
{
this.frame = frame;
}
}
/**
* The anchor sets the origin point of the texture.
* The default is 0,0 this means the textures origin is the top left
* Setting than anchor to 0.5,0.5 means the textures origin is centered
* Setting the anchor to 1,1 would mean the textures origin points will be the bottom right
*
* @property anchor
* @type Point
*/
this.anchor = new Phaser.Point();
this.x = x;
this.y = y;
@@ -115,10 +108,19 @@ Phaser.Sprite = function (game, x, y, key, frame) {
scaleX: 1, scaleY: 1,
// The width/height of the image, based on the un-modified frame size multiplied by the final calculated scale size
width: 0, height: 0,
width: this.currentFrame.sourceSizeW, height: this.currentFrame.sourceSizeH,
// The current frame index, used to check for bounds updates
frameWidth: 0, frameHeight: 0,
// The actual width/height of the image if from a trimmed atlas, multiplied by the final calculated scale size
// actualWidth: 0, actualHeight: 0,
// The actual width/height of the image if from a trimmed atlas, multiplied by the final calculated scale size
halfWidth: Math.floor(this.currentFrame.sourceSizeW), halfHeight: Math.floor(this.currentFrame.sourceSizeH),
// The actual width/height of the image if from a trimmed atlas, multiplied by the final calculated scale size
// centerX: 0, centerY: 0,
// The current frame details
frameID: this.currentFrame.uuid, frameWidth: this.currentFrame.width, frameHeight: this.currentFrame.height,
boundsX: 0, boundsY: 0,
@@ -127,18 +129,17 @@ Phaser.Sprite = function (game, x, y, key, frame) {
};
// Corner points
this.offset = new Phaser.Point();
this.topLeft = new Phaser.Point();
this.topRight = new Phaser.Point();
this.bottomRight = new Phaser.Point();
this.bottomLeft = new Phaser.Point();
this.bounds = new Phaser.Rectangle(x, y, this.width, this.height);
// Corner point defaults
this.offset = new Phaser.Point;
this.center = new Phaser.Point(Math.floor(this._cache.width / 2), Math.floor(this._cache.height / 2));
this.topLeft = new Phaser.Point(x, y);
this.topRight = new Phaser.Point(x + this._cache.width, y);
this.bottomRight = new Phaser.Point(x + this._cache.width, y + this._cache.height);
this.bottomLeft = new Phaser.Point(x, y + this._cache.height);
this.bounds = new Phaser.Rectangle(x, y, this._cache.width, this._cache.height);
this.getLocalPosition(this.topLeft, this.offset.x, this.offset.y);
this.getLocalPosition(this.topRight, this.offset.x + this.width, this.offset.y);
this.getLocalPosition(this.bottomLeft, this.offset.x, this.offset.y + this.height);
this.getLocalPosition(this.bottomRight, this.offset.x + this.width, this.offset.y + this.height);
// Set-up the physics body
this.body = new Phaser.Physics.Arcade.Body(this);
};
@@ -153,7 +154,10 @@ Phaser.Sprite.prototype.update = function() {
this._cache.dirty = false;
this.animations.update();
if (this.animations.update())
{
this._cache.dirty = true;
}
this._cache.x = this.x - (this.game.world.camera.x * this.scrollFactor.x);
this._cache.y = this.y - (this.game.world.camera.y * this.scrollFactor.y);
@@ -200,20 +204,22 @@ Phaser.Sprite.prototype.update = function() {
this._cache.dirty = true;
}
if (this._cache.dirty || this.texture.frame.width != this._cache.frameWidth || this.texture.frame.height != this._cache.frameHeight)
// Frame updated?
if (this.currentFrame.uuid != this._cache.frameID)
{
this._cache.frameWidth = this.texture.frame.width;
this._cache.frameHeight = this.texture.frame.height;
this._cache.width = this.texture.frame.width * this._cache.scaleX;
this._cache.height = this.texture.frame.height * this._cache.scaleY;
this._cache.dirty = true;
}
if (this._cache.dirty)
{
this._cache.width = this.currentFrame.sourceSizeW * this._cache.scaleX;
this._cache.height = this.currentFrame.sourceSizeH * this._cache.scaleY;
// this.getLocalPosition(this.center, this.x - (this.anchor.x * this._cache.width), this.y - (this.anchor.y * this._cache.height));
this._cache.id = 1 / (this._cache.a00 * this._cache.a11 + this._cache.a01 * -this._cache.a10);
this.updateBounds();
}
}
@@ -241,6 +247,9 @@ Phaser.Sprite.prototype.update = function() {
{
this.visible = this._cache.cameraVisible;
}
// Update our physics bounds
this.body.update();
}
// Check our bounds
@@ -253,6 +262,9 @@ Phaser.Sprite.prototype.updateBounds = function() {
this.offset.setTo(this._cache.a02 - (this.anchor.x * this._cache.width), this._cache.a12 - (this.anchor.y * this._cache.height));
// this.getLocalPosition(this.center, this.x - (this.anchor.x * this._cache.width), this.y - (this.anchor.y * this._cache.height));
this.getLocalPosition(this.center, this.offset.x + this._cache.halfWidth, this.offset.y + this._cache.halfHeight);
this.getLocalPosition(this.topLeft, this.offset.x, this.offset.y);
this.getLocalPosition(this.topRight, this.offset.x + this._cache.width, this.offset.y);
this.getLocalPosition(this.bottomLeft, this.offset.x, this.offset.y + this._cache.height);
src/geom/Rectangle.js
+50 -4
View File
@@ -11,10 +11,10 @@
**/
Phaser.Rectangle = function (x, y, width, height) {
if (typeof x === "undefined") { x = 0; }
if (typeof y === "undefined") { y = 0; }
if (typeof width === "undefined") { width = 0; }
if (typeof height === "undefined") { height = 0; }
x = x || 0;
y = y || 0;
width = width || 0;
height = height || 0;
/**
* @property x
@@ -449,6 +449,52 @@ Object.defineProperty(Phaser.Rectangle.prototype, "perimeter", {
configurable: true
});
Object.defineProperty(Phaser.Rectangle.prototype, "centerX", {
/**
* The x coordinate of the center of the Rectangle.
* @method centerX
* @return {Number}
**/
get: function () {
return this.x + this.halfWidth;
},
/**
* The x coordinate of the center of the Rectangle.
* @method centerX
* @param {Number} value
**/
set: function (value) {
this.x = value - this.halfWidth;
},
enumerable: true,
configurable: true
});
Object.defineProperty(Phaser.Rectangle.prototype, "centerY", {
/**
* The y coordinate of the center of the Rectangle.
* @method centerY
* @return {Number}
**/
get: function () {
return this.y + this.halfHeight;
},
/**
* The y coordinate of the center of the Rectangle.
* @method centerY
* @param {Number} value
**/
set: function (value) {
this.y = value - this.halfHeight;
},
enumerable: true,
configurable: true
});
Object.defineProperty(Phaser.Rectangle.prototype, "top", {
/**
src/physics/advanced/Body.js
+402
View File
@@ -0,0 +1,402 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
Body = function(type, pos, angle) {
if (Body.id_counter == undefined) {
Body.id_counter = 0;
}
this.id = Body.id_counter++;
// Identifier
this.name = "body" + this.id;
// STATIC or DYNAMIC
this.type = type;
// Default values
pos = pos || new vec2(0, 0);
angle = angle || 0;
// Local to world transform
this.xf = new Transform(pos, angle);
// Local center of mass
this.centroid = new vec2(0, 0);
// World position of centroid
this.p = new vec2(pos.x, pos.y);
// Velocity
this.v = new vec2(0, 0);
// Force
this.f = new vec2(0, 0);
// Orientation (angle)
this.a = angle;
// Angular velocity
this.w = 0;
// Torque
this.t = 0;
// Linear damping
this.linearDamping = 0;
// Angular damping
this.angularDamping = 0;
// Sleep time
this.sleepTime = 0;
// Awaked flag
this.awaked = false;
// Shape list for this body
this.shapeArr = [];
// Joint hash for this body
this.jointArr = [];
this.jointHash = {};
// Bounds of all shapes
this.bounds = new Bounds;
this.fixedRotation = false;
this.categoryBits = 0x0001;
this.maskBits = 0xFFFF;
this.stepCount = 0;
}
Body.STATIC = 0;
Body.KINETIC = 1;
Body.DYNAMIC = 2;
Body.prototype.duplicate = function() {
var body = new Body(this.type, this.xf.t, this.a);
for (var i = 0; i < this.shapeArr.length; i++) {
body.addShape(this.shapeArr[i].duplicate());
}
body.resetMassData();
return body;
}
Body.prototype.serialize = function() {
var shapes = [];
for (var i = 0; i < this.shapeArr.length; i++) {
var obj = this.shapeArr[i].serialize();
shapes.push(obj);
}
return {
"type": ["static", "kinetic", "dynamic"][this.type],
"name": this.name,
"position": this.xf.t,
"angle": this.xf.a,
"shapes": shapes
};
}
Body.prototype.toString = function() {
return "[{Body (name=" + this.name + " velocity=" + this.v.toString() + " angularVelocity: " + this.w + ")}]";
}
Body.prototype.isStatic = function() {
return this.type == Body.STATIC ? true : false;
}
Body.prototype.isDynamic = function() {
return this.type == Body.DYNAMIC ? true : false;
}
Body.prototype.isKinetic = function() {
return this.type == Body.KINETIC ? true : false;
}
Body.prototype.setType = function(type) {
if (type == this.type) {
return;
}
this.f.set(0, 0);
this.v.set(0, 0);
this.t = 0;
this.w = 0;
this.type = type;
this.awake(true);
}
Body.prototype.addShape = function(shape) {
shape.body = this;
this.shapeArr.push(shape);
}
Body.prototype.removeShape = function(shape) {
var index = this.shapeArr.indexOf(shape);
if (index != -1) {
this.shapeArr.splice(index, 1);
shape.body = undefined;
}
}
// Internal function
Body.prototype.setMass = function(mass) {
this.m = mass;
this.m_inv = mass > 0 ? 1 / mass : 0;
}
// Internal function
Body.prototype.setInertia = function(inertia) {
this.i = inertia;
this.i_inv = inertia > 0 ? 1 / inertia : 0;
}
Body.prototype.setTransform = function(pos, angle) {
this.xf.set(pos, angle);
this.p = this.xf.transform(this.centroid);
this.a = angle;
}
Body.prototype.syncTransform = function() {
this.xf.setRotation(this.a);
this.xf.setPosition(vec2.sub(this.p, this.xf.rotate(this.centroid)));
}
Body.prototype.getWorldPoint = function(p) {
return this.xf.transform(p);
}
Body.prototype.getWorldVector = function(v) {
return this.xf.rotate(v);
}
Body.prototype.getLocalPoint = function(p) {
return this.xf.untransform(p);
}
Body.prototype.getLocalVector = function(v) {
return this.xf.unrotate(v);
}
Body.prototype.setFixedRotation = function(flag) {
this.fixedRotation = flag;
this.resetMassData();
}
Body.prototype.resetMassData = function() {
this.centroid.set(0, 0);
this.m = 0;
this.m_inv = 0;
this.i = 0;
this.i_inv = 0;
if (!this.isDynamic()) {
this.p = this.xf.transform(this.centroid);
return;
}
var totalMassCentroid = new vec2(0, 0);
var totalMass = 0;
var totalInertia = 0;
for (var i = 0; i < this.shapeArr.length; i++) {
var shape = this.shapeArr[i];
var centroid = shape.centroid();
var mass = shape.area() * shape.density;
var inertia = shape.inertia(mass);
totalMassCentroid.mad(centroid, mass);
totalMass += mass;
totalInertia += inertia;
}
this.centroid.copy(vec2.scale(totalMassCentroid, 1 / totalMass));
this.setMass(totalMass);
if (!this.fixedRotation) {
this.setInertia(totalInertia - totalMass * vec2.dot(this.centroid, this.centroid));
}
// Move center of mass
var old_p = this.p;
this.p = this.xf.transform(this.centroid);
// Update center of mass velocity ??
this.v.mad(vec2.perp(vec2.sub(this.p, old_p)), this.w);
}
Body.prototype.resetJointAnchors = function() {
for (var i = 0; i < this.jointArr.length; i++) {
var joint = this.jointArr[i];
if (!joint) {
continue;
}
var anchor1 = joint.getWorldAnchor1();
var anchor2 = joint.getWorldAnchor2();
joint.setWorldAnchor1(anchor1);
joint.setWorldAnchor2(anchor2);
}
}
Body.prototype.cacheData = function() {
this.bounds.clear();
for (var i = 0; i < this.shapeArr.length; i++) {
var shape = this.shapeArr[i];
shape.cacheData(this.xf);
this.bounds.addBounds(shape.bounds);
}
}
Body.prototype.updateVelocity = function(gravity, dt, damping) {
this.v = vec2.mad(this.v, vec2.mad(gravity, this.f, this.m_inv), dt);
this.w = this.w + this.t * this.i_inv * dt;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
this.v.scale(Math.clamp(1 - dt * (damping + this.linearDamping), 0, 1));
this.w *= Math.clamp(1 - dt * (damping + this.angularDamping), 0, 1);
this.f.set(0, 0);
this.t = 0;
}
Body.prototype.updatePosition = function(dt) {
this.p.addself(vec2.scale(this.v, dt));
this.a += this.w * dt;
}
Body.prototype.resetForce = function() {
this.f.set(0, 0);
this.t = 0;
}
Body.prototype.applyForce = function(force, p) {
if (!this.isDynamic())
return;
if (!this.isAwake())
this.awake(true);
this.f.addself(force);
this.t += vec2.cross(vec2.sub(p, this.p), force);
}
Body.prototype.applyForceToCenter = function(force) {
if (!this.isDynamic())
return;
if (!this.isAwake())
this.awake(true);
this.f.addself(force);
}
Body.prototype.applyTorque = function(torque) {
if (!this.isDynamic())
return;
if (!this.isAwake())
this.awake(true);
this.t += torque;
}
Body.prototype.applyLinearImpulse = function(impulse, p) {
if (!this.isDynamic())
return;
if (!this.isAwake())
this.awake(true);
this.v.mad(impulse, this.m_inv);
this.w += vec2.cross(vec2.sub(p, this.p), impulse) * this.i_inv;
}
Body.prototype.applyAngularImpulse = function(impulse) {
if (!this.isDynamic())
return;
if (!this.isAwake())
this.awake(true);
this.w += impulse * this.i_inv;
}
Body.prototype.kineticEnergy = function() {
var vsq = this.v.dot(this.v);
var wsq = this.w * this.w;
return 0.5 * (this.m * vsq + this.i * wsq);
}
Body.prototype.isAwake = function() {
return this.awaked;
}
Body.prototype.awake = function(flag) {
this.awaked = flag;
if (flag) {
this.sleepTime = 0;
}
else {
this.v.set(0, 0);
this.w = 0;
this.f.set(0, 0);
this.t = 0;
}
}
Body.prototype.isCollidable = function(other) {
if (this == other)
return false;
if (!this.isDynamic() && !other.isDynamic())
return false;
if (!(this.maskBits & other.categoryBits) || !(other.maskBits & this.categoryBits))
return false;
for (var i = 0; i < this.jointArr.length; i++) {
var joint = this.jointArr[i];
if (!joint) {
continue;
}
if (!joint.collideConnected && other.jointHash[joint.id] != undefined) {
return false;
}
}
return true;
}
src/physics/advanced/Collision.js
+382
View File
@@ -0,0 +1,382 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
var collision = {};
(function() {
var colFuncs = [];
function addCollideFunc(a, b, func) {
colFuncs[a * Shape.NUM_TYPES + b] = func;
}
function _circle2Circle(c1, r1, c2, r2, contactArr) {
var rmax = r1 + r2;
var t = vec2.sub(c2, c1);
var distsq = t.lengthsq();
if (distsq > rmax * rmax) {
return 0;
}
var dist = Math.sqrt(distsq);
var p = vec2.mad(c1, t, 0.5 + (r1 - r2) * 0.5 / dist);
var n = (dist != 0) ? vec2.scale(t, 1 / dist) : vec2.zero;
var d = dist - rmax;
contactArr.push(new Contact(p, n, d, 0));
return 1;
}
function circle2Circle(circ1, circ2, contactArr) {
return _circle2Circle(circ1.tc, circ1.r, circ2.tc, circ2.r, contactArr);
}
function circle2Segment(circ, seg, contactArr) {
var rsum = circ.r + seg.r;
// Normal distance from segment
var dn = vec2.dot(circ.tc, seg.tn) - vec2.dot(seg.ta, seg.tn);
var dist = (dn < 0 ? dn * -1 : dn) - rsum;
if (dist > 0) {
return 0;
}
// Tangential distance along segment
var dt = vec2.cross(circ.tc, seg.tn);
var dtMin = vec2.cross(seg.ta, seg.tn);
var dtMax = vec2.cross(seg.tb, seg.tn);
if (dt < dtMin) {
if (dt < dtMin - rsum) {
return 0;
}
return _circle2Circle(circ.tc, circ.r, seg.ta, seg.r, contactArr);
}
else if (dt > dtMax) {
if (dt > dtMax + rsum) {
return 0;
}
return _circle2Circle(circ.tc, circ.r, seg.tb, seg.r, contactArr);
}
var n = (dn > 0) ? seg.tn : vec2.neg(seg.tn);
contactArr.push(new Contact(vec2.mad(circ.tc, n, -(circ.r + dist * 0.5)), vec2.neg(n), dist, 0));
return 1;
}
function circle2Poly(circ, poly, contactArr) {
var minDist = -999999;
var minIdx = -1;
for (var i = 0; i < poly.verts.length; i++) {
var plane = poly.tplanes[i];
var dist = vec2.dot(circ.tc, plane.n) - plane.d - circ.r;
if (dist > 0) {
return 0;
}
else if (dist > minDist) {
minDist = dist;
minIdx = i;
}
}
var n = poly.tplanes[minIdx].n;
var a = poly.tverts[minIdx];
var b = poly.tverts[(minIdx + 1) % poly.verts.length];
var dta = vec2.cross(a, n);
var dtb = vec2.cross(b, n);
var dt = vec2.cross(circ.tc, n);
if (dt > dta) {
return _circle2Circle(circ.tc, circ.r, a, 0, contactArr);
}
else if (dt < dtb) {
return _circle2Circle(circ.tc, circ.r, b, 0, contactArr);
}
contactArr.push(new Contact(vec2.mad(circ.tc, n, -(circ.r + minDist * 0.5)), vec2.neg(n), minDist, 0));
return 1;
}
function segmentPointDistanceSq(seg, p) {
var w = vec2.sub(p, seg.ta);
var d = vec2.sub(seg.tb, seg.ta);
var proj = w.dot(d);
if (proj <= 0) {
return w.dot(w);
}
var vsq = d.dot(d)
if (proj >= vsq) {
return w.dot(w) - 2 * proj + vsq;
}
return w.dot(w) - proj * proj / vsq;
}
// FIXME !!
function segment2Segment(seg1, seg2, contactArr) {
var d = [];
d[0] = segmentPointDistanceSq(seg1, seg2.ta);
d[1] = segmentPointDistanceSq(seg1, seg2.tb);
d[2] = segmentPointDistanceSq(seg2, seg1.ta);
d[3] = segmentPointDistanceSq(seg2, seg1.tb);
var idx1 = d[0] < d[1] ? 0 : 1;
var idx2 = d[2] < d[3] ? 2 : 3;
var idxm = d[idx1] < d[idx2] ? idx1 : idx2;
var s, t;
var u = vec2.sub(seg1.tb, seg1.ta);
var v = vec2.sub(seg2.tb, seg2.ta);
switch (idxm) {
case 0:
s = vec2.dot(vec2.sub(seg2.ta, seg1.ta), u) / vec2.dot(u, u);
s = s < 0 ? 0 : (s > 1 ? 1 : s);
t = 0;
break;
case 1:
s = vec2.dot(vec2.sub(seg2.tb, seg1.ta), u) / vec2.dot(u, u);
s = s < 0 ? 0 : (s > 1 ? 1 : s);
t = 1;
break;
case 2:
s = 0;
t = vec2.dot(vec2.sub(seg1.ta, seg2.ta), v) / vec2.dot(v, v);
t = t < 0 ? 0 : (t > 1 ? 1 : t);
break;
case 3:
s = 1;
t = vec2.dot(vec2.sub(seg1.tb, seg2.ta), v) / vec2.dot(v, v);
t = t < 0 ? 0 : (t > 1 ? 1 : t);
break;
}
var minp1 = vec2.mad(seg1.ta, u, s);
var minp2 = vec2.mad(seg2.ta, v, t);
return _circle2Circle(minp1, seg1.r, minp2, seg2.r, contactArr);
}
// Identify vertexes that have penetrated the segment.
function findPointsBehindSeg(contactArr, seg, poly, dist, coef) {
var dta = vec2.cross(seg.tn, seg.ta);
var dtb = vec2.cross(seg.tn, seg.tb);
var n = vec2.scale(seg.tn, coef);
for (var i = 0; i < poly.verts.length; i++) {
var v = poly.tverts[i];
if (vec2.dot(v, n) < vec2.dot(seg.tn, seg.ta) * coef + seg.r) {
var dt = vec2.cross(seg.tn, v);
if (dta >= dt && dt >= dtb) {
contactArr.push(new Contact(v, n, dist, (poly.id << 16) | i));
}
}
}
}
function segment2Poly(seg, poly, contactArr) {
var seg_td = vec2.dot(seg.tn, seg.ta);
var seg_d1 = poly.distanceOnPlane(seg.tn, seg_td) - seg.r;
if (seg_d1 > 0) {
return 0;
}
var seg_d2 = poly.distanceOnPlane(vec2.neg(seg.tn), -seg_td) - seg.r;
if (seg_d2 > 0) {
return 0;
}
var poly_d = -999999;
var poly_i = -1;
for (var i = 0; i < poly.verts.length; i++) {
var plane = poly.tplanes[i];
var dist = seg.distanceOnPlane(plane.n, plane.d);
if (dist > 0) {
return 0;
}
if (dist > poly_d) {
poly_d = dist;
poly_i = i;
}
}
var poly_n = vec2.neg(poly.tplanes[poly_i].n);
var va = vec2.mad(seg.ta, poly_n, seg.r);
var vb = vec2.mad(seg.tb, poly_n, seg.r);
if (poly.containPoint(va)) {
contactArr.push(new Contact(va, poly_n, poly_d, (seg.id << 16) | 0));
}
if (poly.containPoint(vb)) {
contactArr.push(new Contact(vb, poly_n, poly_d, (seg.id << 16) | 1));
}
// Floating point precision problems here.
// This will have to do for now.
poly_d -= 0.1
if (seg_d1 >= poly_d || seg_d2 >= poly_d) {
if (seg_d1 > seg_d2) {
findPointsBehindSeg(contactArr, seg, poly, seg_d1, 1);
}
else {
findPointsBehindSeg(contactArr, seg, poly, seg_d2, -1);
}
}
// If no other collision points are found, try colliding endpoints.
if (contactArr.length == 0) {
var poly_a = poly.tverts[poly_i];
var poly_b = poly.tverts[(poly_i + 1) % poly.verts.length];
if (_circle2Circle(seg.ta, seg.r, poly_a, 0, contactArr))
return 1;
if (_circle2Circle(seg.tb, seg.r, poly_a, 0, contactArr))
return 1;
if (_circle2Circle(seg.ta, seg.r, poly_b, 0, contactArr))
return 1;
if (_circle2Circle(seg.tb, seg.r, poly_b, 0, contactArr))
return 1;
}
return contactArr.length;
}
// Find the minimum separating axis for the given poly and plane list.
function findMSA(poly, planes, num) {
var min_dist = -999999;
var min_index = -1;
for (var i = 0; i < num; i++) {
var dist = poly.distanceOnPlane(planes[i].n, planes[i].d);
if (dist > 0) { // no collision
return { dist: 0, index: -1 };
}
else if (dist > min_dist) {
min_dist = dist;
min_index = i;
}
}
return { dist: min_dist, index: min_index };
}
function findVertsFallback(contactArr, poly1, poly2, n, dist) {
var num = 0;
for (var i = 0; i < poly1.verts.length; i++) {
var v = poly1.tverts[i];
if (poly2.containPointPartial(v, n)) {
contactArr.push(new Contact(v, n, dist, (poly1.id << 16) | i));
num++;
}
}
for (var i = 0; i < poly2.verts.length; i++) {
var v = poly2.tverts[i];
if (poly1.containPointPartial(v, n)) {
contactArr.push(new Contact(v, n, dist, (poly2.id << 16) | i));
num++;
}
}
return num;
}
// Find the overlapped vertices.
function findVerts(contactArr, poly1, poly2, n, dist) {
var num = 0;
for (var i = 0; i < poly1.verts.length; i++) {
var v = poly1.tverts[i];
if (poly2.containPoint(v)) {
contactArr.push(new Contact(v, n, dist, (poly1.id << 16) | i));
num++;
}
}
for (var i = 0; i < poly2.verts.length; i++) {
var v = poly2.tverts[i];
if (poly1.containPoint(v)) {
contactArr.push(new Contact(v, n, dist, (poly2.id << 16) | i));
num++;
}
}
return num > 0 ? num : findVertsFallback(contactArr, poly1, poly2, n, dist);
}
function poly2Poly(poly1, poly2, contactArr) {
var msa1 = findMSA(poly2, poly1.tplanes, poly1.verts.length);
if (msa1.index == -1) {
return 0;
}
var msa2 = findMSA(poly1, poly2.tplanes, poly2.verts.length);
if (msa2.index == -1) {
return 0;
}
// Penetration normal direction shoud be from poly1 to poly2
if (msa1.dist > msa2.dist) {
return findVerts(contactArr, poly1, poly2, poly1.tplanes[msa1.index].n, msa1.dist);
}
return findVerts(contactArr, poly1, poly2, vec2.neg(poly2.tplanes[msa2.index].n), msa2.dist);
}
collision.init = function() {
addCollideFunc(Shape.TYPE_CIRCLE, Shape.TYPE_CIRCLE, circle2Circle);
addCollideFunc(Shape.TYPE_CIRCLE, Shape.TYPE_SEGMENT, circle2Segment);
addCollideFunc(Shape.TYPE_CIRCLE, Shape.TYPE_POLY, circle2Poly);
addCollideFunc(Shape.TYPE_SEGMENT, Shape.TYPE_SEGMENT, segment2Segment);
addCollideFunc(Shape.TYPE_SEGMENT, Shape.TYPE_POLY, segment2Poly);
addCollideFunc(Shape.TYPE_POLY, Shape.TYPE_POLY, poly2Poly);
};
collision.collide = function(a, b, contactArr) {
if (a.type > b.type) {
var c = a;
a = b;
b = c;
}
return colFuncs[a.type * Shape.NUM_TYPES + b.type](a, b, contactArr);
};
})();
src/physics/advanced/Contact.js
+37
View File
@@ -0,0 +1,37 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
function Contact(p, n, d, hash) {
this.hash = hash;
// Contact point
this.p = p;
// Contact normal (toward shape2)
this.n = n;
// Penetration depth (d < 0)
this.d = d;
// Accumulated normal constraint impulse
this.lambda_n_acc = 0;
// Accumulated tangential constraint impulse
this.lambda_t_acc = 0;
}
src/physics/advanced/ContactSolver.js
+267
View File
@@ -0,0 +1,267 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Contact Constraint
//
// Non-penetration constraint:
// C = dot(p2 - p1, n)
// Cdot = dot(v2 - v1, n)
// J = [ -n, -cross(r1, n), n, cross(r2, n) ]
//
// impulse = JT * lambda = [ -n * lambda, -cross(r1, n) * lambda, n * lambda, cross(r1, n) * lambda ]
//
// Friction constraint:
// C = dot(p2 - p1, t)
// Cdot = dot(v2 - v1, t)
// J = [ -t, -cross(r1, t), t, cross(r2, t) ]
//
// impulse = JT * lambda = [ -t * lambda, -cross(r1, t) * lambda, t * lambda, cross(r1, t) * lambda ]
//
// NOTE: lambda is an impulse in constraint space.
//-------------------------------------------------------------------------------------------------
function ContactSolver(shape1, shape2) {
// Contact shapes
this.shape1 = shape1;
this.shape2 = shape2;
// Contact list
this.contactArr = [];
// Coefficient of restitution (elasticity)
this.e = 1;
// Frictional coefficient
this.u = 1;
}
ContactSolver.COLLISION_SLOP = 0.0008;
ContactSolver.BAUMGARTE = 0.28;
ContactSolver.MAX_LINEAR_CORRECTION = 1;//Infinity;
ContactSolver.prototype.update = function(newContactArr) {
for (var i = 0; i < newContactArr.length; i++) {
var newContact = newContactArr[i];
var k = -1;
for (var j = 0; j < this.contactArr.length; j++) {
if (newContact.hash == this.contactArr[j].hash) {
k = j;
break;
}
}
if (k > -1) {
newContact.lambda_n_acc = this.contactArr[k].lambda_n_acc;
newContact.lambda_t_acc = this.contactArr[k].lambda_t_acc;
}
}
this.contactArr = newContactArr;
}
ContactSolver.prototype.initSolver = function(dt_inv) {
var body1 = this.shape1.body;
var body2 = this.shape2.body;
var sum_m_inv = body1.m_inv + body2.m_inv;
for (var i = 0; i < this.contactArr.length; i++) {
var con = this.contactArr[i];
// Transformed r1, r2
con.r1 = vec2.sub(con.p, body1.p);
con.r2 = vec2.sub(con.p, body2.p);
// Local r1, r2
con.r1_local = body1.xf.unrotate(con.r1);
con.r2_local = body2.xf.unrotate(con.r2);
var n = con.n;
var t = vec2.perp(con.n);
// invEMn = J * invM * JT
// J = [ -n, -cross(r1, n), n, cross(r2, n) ]
var sn1 = vec2.cross(con.r1, n);
var sn2 = vec2.cross(con.r2, n);
var emn_inv = sum_m_inv + body1.i_inv * sn1 * sn1 + body2.i_inv * sn2 * sn2;
con.emn = emn_inv == 0 ? 0 : 1 / emn_inv;
// invEMt = J * invM * JT
// J = [ -t, -cross(r1, t), t, cross(r2, t) ]
var st1 = vec2.cross(con.r1, t);
var st2 = vec2.cross(con.r2, t);
var emt_inv = sum_m_inv + body1.i_inv * st1 * st1 + body2.i_inv * st2 * st2;
con.emt = emt_inv == 0 ? 0 : 1 / emt_inv;
// Linear velocities at contact point
// in 2D: cross(w, r) = perp(r) * w
var v1 = vec2.mad(body1.v, vec2.perp(con.r1), body1.w);
var v2 = vec2.mad(body2.v, vec2.perp(con.r2), body2.w);
// relative velocity at contact point
var rv = vec2.sub(v2, v1);
// bounce velocity dot n
con.bounce = vec2.dot(rv, con.n) * this.e;
}
}
ContactSolver.prototype.warmStart = function() {
var body1 = this.shape1.body;
var body2 = this.shape2.body;
for (var i = 0; i < this.contactArr.length; i++) {
var con = this.contactArr[i];
var n = con.n;
var lambda_n = con.lambda_n_acc;
var lambda_t = con.lambda_t_acc;
// Apply accumulated impulses
//var impulse = vec2.rotate_vec(new vec2(lambda_n, lambda_t), n);
var impulse = new vec2(lambda_n * n.x - lambda_t * n.y, lambda_t * n.x + lambda_n * n.y);
body1.v.mad(impulse, -body1.m_inv);
body1.w -= vec2.cross(con.r1, impulse) * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += vec2.cross(con.r2, impulse) * body2.i_inv;
}
}
ContactSolver.prototype.solveVelocityConstraints = function() {
var body1 = this.shape1.body;
var body2 = this.shape2.body;
var m1_inv = body1.m_inv;
var i1_inv = body1.i_inv;
var m2_inv = body2.m_inv;
var i2_inv = body2.i_inv;
for (var i = 0; i < this.contactArr.length; i++) {
var con = this.contactArr[i];
var n = con.n;
var t = vec2.perp(n);
var r1 = con.r1;
var r2 = con.r2;
// Linear velocities at contact point
// in 2D: cross(w, r) = perp(r) * w
var v1 = vec2.mad(body1.v, vec2.perp(r1), body1.w);
var v2 = vec2.mad(body2.v, vec2.perp(r2), body2.w);
// Relative velocity at contact point
var rv = vec2.sub(v2, v1);
// Compute normal constraint impulse + adding bounce as a velocity bias
// lambda_n = -EMn * J * V
var lambda_n = -con.emn * (vec2.dot(n, rv) + con.bounce);
// Accumulate and clamp
var lambda_n_old = con.lambda_n_acc;
con.lambda_n_acc = Math.max(lambda_n_old + lambda_n, 0);
lambda_n = con.lambda_n_acc - lambda_n_old;
// Compute frictional constraint impulse
// lambda_t = -EMt * J * V
var lambda_t = -con.emt * vec2.dot(t, rv);
// Max friction constraint impulse (Coulomb's Law)
var lambda_t_max = con.lambda_n_acc * this.u;
// Accumulate and clamp
var lambda_t_old = con.lambda_t_acc;
con.lambda_t_acc = Math.clamp(lambda_t_old + lambda_t, -lambda_t_max, lambda_t_max);
lambda_t = con.lambda_t_acc - lambda_t_old;
// Apply the final impulses
//var impulse = vec2.rotate_vec(new vec2(lambda_n, lambda_t), n);
var impulse = new vec2(lambda_n * n.x - lambda_t * n.y, lambda_t * n.x + lambda_n * n.y);
body1.v.mad(impulse, -m1_inv);
body1.w -= vec2.cross(r1, impulse) * i1_inv;
body2.v.mad(impulse, m2_inv);
body2.w += vec2.cross(r2, impulse) * i2_inv;
}
}
ContactSolver.prototype.solvePositionConstraints = function() {
var body1 = this.shape1.body;
var body2 = this.shape2.body;
var m1_inv = body1.m_inv;
var i1_inv = body1.i_inv;
var m2_inv = body2.m_inv;
var i2_inv = body2.i_inv;
var sum_m_inv = m1_inv + m2_inv;
var max_penetration = 0;
for (var i = 0; i < this.contactArr.length; i++) {
var con = this.contactArr[i];
var n = con.n;
// Transformed r1, r2
var r1 = vec2.rotate(con.r1_local, body1.a);
var r2 = vec2.rotate(con.r2_local, body2.a);
// Contact points (corrected)
var p1 = vec2.add(body1.p, r1);
var p2 = vec2.add(body2.p, r2);
// Corrected delta vector
var dp = vec2.sub(p2, p1);
// Position constraint
var c = vec2.dot(dp, n) + con.d;
var correction = Math.clamp(ContactSolver.BAUMGARTE * (c + ContactSolver.COLLISION_SLOP), -ContactSolver.MAX_LINEAR_CORRECTION, 0);
if (correction == 0) {
continue;
}
// We don't need max_penetration less than or equal slop
max_penetration = Math.max(max_penetration, -c);
// Compute lambda for position constraint
// Solve (J * invM * JT) * lambda = -C / dt
var sn1 = vec2.cross(r1, n);
var sn2 = vec2.cross(r2, n);
var em_inv = sum_m_inv + body1.i_inv * sn1 * sn1 + body2.i_inv * sn2 * sn2;
var lambda_dt = em_inv == 0 ? 0 : -correction / em_inv;
// Apply correction impulses
var impulse_dt = vec2.scale(n, lambda_dt);
body1.p.mad(impulse_dt, -m1_inv);
body1.a -= sn1 * lambda_dt * i1_inv;
body2.p.mad(impulse_dt, m2_inv);
body2.a += sn2 * lambda_dt * i2_inv;
}
return max_penetration <= ContactSolver.COLLISION_SLOP * 3;
}
src/physics/advanced/Joint.js
+76
View File
@@ -0,0 +1,76 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
Joint = function(type, body1, body2, collideConnected) {
if (arguments.length == 0)
return;
if (Joint.id_counter == undefined)
Joint.id_counter = 0;
this.id = Joint.id_counter++;
this.type = type;
this.body1 = body1;
this.body2 = body2;
// Allow collision between to cennected body
this.collideConnected = collideConnected;
// Constraint force limit
this.maxForce = 9999999999;
// Is breakable ?
this.breakable = false;
}
Joint.TYPE_ANGLE = 0;
Joint.TYPE_REVOLUTE = 1;
Joint.TYPE_WELD = 2;
Joint.TYPE_WHEEL = 3;
Joint.TYPE_PRISMATIC = 4;
Joint.TYPE_DISTANCE = 5;
Joint.TYPE_ROPE = 6;
Joint.TYPE_MOUSE = 7;
Joint.LINEAR_SLOP = 0.0008;
Joint.ANGULAR_SLOP = deg2rad(2);
Joint.MAX_LINEAR_CORRECTION = 0.5;
Joint.MAX_ANGULAR_CORRECTION = deg2rad(8);
Joint.LIMIT_STATE_INACTIVE = 0;
Joint.LIMIT_STATE_AT_LOWER = 1;
Joint.LIMIT_STATE_AT_UPPER = 2;
Joint.LIMIT_STATE_EQUAL_LIMITS = 3;
Joint.prototype.getWorldAnchor1 = function() {
return this.body1.getWorldPoint(this.anchor1);
}
Joint.prototype.getWorldAnchor2 = function() {
return this.body2.getWorldPoint(this.anchor2);
}
Joint.prototype.setWorldAnchor1 = function(anchor1) {
this.anchor1 = this.body1.getLocalPoint(anchor1);
}
Joint.prototype.setWorldAnchor2 = function(anchor2) {
this.anchor2 = this.body2.getLocalPoint(anchor2);
}
src/physics/advanced/Math.js
+817
View File
@@ -0,0 +1,817 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
Math.clamp = function(v, min, max) { return v < min ? min : (v > max ? max : v); }
Math.log2 = function(a) { return Math.log(a) / Math.log(2); }
function deg2rad(deg) { return (deg / 180) * Math.PI; }
function rad2deg(rad) { return (rad / Math.PI) * 180; }
function pixel2meter(px) { return px * 0.02; }
function meter2pixel(mt) { return mt * 50; }
//-----------------------------------
// 2D Vector
//-----------------------------------
function vec2(x, y) {
this.x = x || 0;
this.y = y || 0;
}
vec2.zero = new vec2(0, 0);
vec2.prototype.toString = function() {
//return ["x:", this.x, "y:", this.y].join(" ");
return "x=" + this.x + " y=" + this.y;
}
vec2.prototype.set = function(x, y) {
this.x = x;
this.y = y;
return this;
}
vec2.prototype.copy = function(v) {
this.x = v.x;
this.y = v.y;
return this;
}
vec2.prototype.duplicate = function() {
return new vec2(this.x, this.y);
}
vec2.prototype.equal = function(v) {
return (this.x != v.x || this.y != v.y) ? false : true;
}
vec2.prototype.add = function(v1, v2) {
this.x = v1.x + v2.x;
this.y = v1.y + v2.y;
return this;
}
vec2.prototype.addself = function(v) {
this.x += v.x;
this.y += v.y;
return this;
}
vec2.prototype.sub = function(v1, v2) {
this.x = v1.x - v2.x;
this.y = v1.y - v2.y;
return this;
}
vec2.prototype.subself = function(v) {
this.x -= v.x;
this.y -= v.y;
return this;
}
vec2.prototype.scale = function(s) {
this.x *= s;
this.y *= s;
return this;
}
vec2.prototype.scale2 = function(s) {
this.x *= s.x;
this.y *= s.y;
return this;
}
vec2.prototype.mad = function(v, s) {
this.x += v.x * s;
this.y += v.y * s;
}
vec2.prototype.neg = function() {
this.x *= -1;
this.y *= -1;
return this;
}
vec2.prototype.rcp = function() {
this.x = 1 / this.x;
this.y = 1 / this.y;
return this;
}
vec2.prototype.lengthsq = function() {
return this.x * this.x + this.y * this.y;
}
vec2.prototype.length = function() {
return Math.sqrt(this.x * this.x + this.y * this.y);
}
vec2.prototype.normalize = function() {
var inv = (this.x != 0 || this.y != 0) ? 1 / Math.sqrt(this.x * this.x + this.y * this.y) : 0;
this.x *= inv;
this.y *= inv;
return this;
}
vec2.prototype.dot = function(v) {
return this.x * v.x + this.y * v.y;
}
// Z-component of 3d cross product (ax, ay, 0) x (bx, by, 0)
vec2.prototype.cross = function(v) {
return this.x * v.y - this.y * v.x;
}
vec2.prototype.toAngle = function() {
return Math.atan2(this.y, this.x);
}
vec2.prototype.rotation = function(angle) {
this.x = Math.cos(angle);
this.y = Math.sin(angle);
return this;
}
vec2.prototype.rotate = function(angle) {
var c = Math.cos(angle);
var s = Math.sin(angle);
return this.set(this.x * c - this.y * s, this.x * s + this.y * c);
}
vec2.prototype.lerp = function(v1, v2, t) {
return this.add(vec2.scale(v1, 1 - t), vec2.scale(v2, t));
}
vec2.add = function(v1, v2) {
return new vec2(v1.x + v2.x, v1.y + v2.y);
}
vec2.sub = function(v1, v2) {
return new vec2(v1.x - v2.x, v1.y - v2.y);
}
vec2.scale = function(v, s) {
return new vec2(v.x * s, v.y * s);
}
vec2.scale2 = function(v, s) {
return new vec2(v.x * s.x, v.y * s.y);
}
vec2.mad = function(v1, v2, s) {
return new vec2(v1.x + v2.x * s, v1.y + v2.y * s);
}
vec2.neg = function(v) {
return new vec2(-v.x, -v.y);
}
vec2.rcp = function(v) {
return new vec2(1 / v.x, 1 / v.y);
}
vec2.normalize = function(v) {
var inv = (v.x != 0 || v.y != 0) ? 1 / Math.sqrt(v.x * v.x + v.y * v.y) : 0;
return new vec2(v.x * inv, v.y * inv);
}
vec2.dot = function(v1, v2) {
return v1.x * v2.x + v1.y * v2.y;
}
vec2.cross = function(v1, v2) {
return v1.x * v2.y - v1.y * v2.x;
}
vec2.toAngle = function(v) {
return Math.atan2(v.y, v.x);
}
vec2.rotation = function(angle) {
return new vec2(Math.cos(angle), Math.sin(angle));
}
vec2.rotate = function(v, angle) {
var c = Math.cos(angle);
var s = Math.sin(angle);
return new vec2(v.x * c - v.y * s, v.x * s + v.y * c);
}
// Return perpendicular vector (90 degree rotation)
vec2.perp = function(v) {
return new vec2(-v.y, v.x);
}
// Return perpendicular vector (-90 degree rotation)
vec2.rperp = function(v) {
return new vec2(v.y, -v.x);
}
vec2.dist = function(v1, v2) {
var dx = v2.x - v1.x;
var dy = v2.y - v1.y;
return Math.sqrt(dx * dx + dy * dy);
}
vec2.distsq = function(v1, v2) {
var dx = v2.x - v1.x;
var dy = v2.y - v1.y;
return dx * dx + dy * dy;
}
vec2.lerp = function(v1, v2, t) {
return vec2.add(vec2.scale(v1, 1 - t), vec2.scale(v2, t));
}
vec2.truncate = function(v, length) {
var ret = v.duplicate();
var length_sq = v.x * v.x + v.y * v.y;
if (length_sq > length * length) {
ret.scale(length / Math.sqrt(length_sq));
}
return ret;
}
//-----------------------------------
// 3D Vector
//-----------------------------------
function vec3(x, y, z) {
this.x = x || 0;
this.y = y || 0;
this.z = z || 0;
}
vec3.zero = new vec3(0, 0, 0);
vec3.prototype.toString = function() {
return ["x:", this.x, "y:", this.y, "z:", this.z].join(" ");
}
vec3.prototype.set = function(x, y, z) {
this.x = x;
this.y = y;
this.z = z;
return this;
}
vec3.prototype.copy = function(v) {
this.x = v.x;
this.y = v.y;
this.z = v.z;
return this;
}
vec3.prototype.duplicate = function() {
return new vec3(this.x, this.y, this.z);
}
vec3.prototype.equal = function(v) {
return this.x != v.x || this.y != v.y || this.z != v.z ? false : true;
}
vec3.prototype.add = function(v1, v2) {
this.x = v1.x + v2.x;
this.y = v1.y + v2.y;
this.z = v1.z + v2.z;
return this;
}
vec3.prototype.addself = function(v) {
this.x += v.x;
this.y += v.y;
this.z += v.z;
return this;
}
vec3.prototype.sub = function(v1, v2) {
this.x = v1.x - v2.x;
this.y = v1.y - v2.y;
this.z = v1.z - v2.z;
return this;
}
vec3.prototype.subself = function(v) {
this.x -= v.x;
this.y -= v.y;
this.z -= v.z;
return this;
}
vec3.prototype.scale = function(s) {
this.x *= s;
this.y *= s;
this.z *= s;
return this;
}
vec3.prototype.mad = function(v, s) {
this.x += v.x * s;
this.y += v.y * s;
this.z += v.z * s;
}
vec3.prototype.neg = function() {
this.x *= -1;
this.y *= -1;
this.z *= -1;
return this;
}
vec3.prototype.rcp = function() {
this.x = 1 / this.x;
this.y = 1 / this.y;
this.z = 1 / this.z;
return this;
}
vec3.prototype.lengthsq = function() {
return this.x * this.x + this.y * this.y + this.z * this.z;
}
vec3.prototype.length = function() {
return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
}
vec3.prototype.normalize = function() {
var inv = (this.x != 0 || this.y != 0 || this.z != 0) ? 1 / Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z) : 0;
this.x *= inv;
this.y *= inv;
this.z *= inv;
return this;
}
vec3.prototype.dot = function(v) {
return this.x * v.x + this.y * v.y + this.z * v.z;
}
vec3.prototype.toVec2 = function() {
return new vec2(this.x, this.y);
}
vec3.fromVec2 = function(v, z) {
return new vec3(v.x, v.y, z);
}
vec3.truncate = function(v, length) {
var ret = v.duplicate();
var length_sq = v.x * v.x + v.y * v.y + v.z * v.z;
if (length_sq > length * length) {
ret.scale(length / Math.sqrt(length_sq));
}
return ret;
}
//-----------------------------------
// 2x2 Matrix (row major)
//-----------------------------------
function mat2(_11, _12, _21, _22) {
this._11 = _11 || 0;
this._12 = _12 || 0;
this._21 = _21 || 0;
this._22 = _22 || 0;
}
mat2.zero = new mat2(0, 0, 0, 0);
mat2.prototype.toString = function() {
return ["[", this._11, this._12, this_21, this._22, "]"].join(" ");
}
mat2.prototype.set = function(_11, _12, _21, _22) {
this._11 = _11;
this._12 = _12;
this._21 = _21;
this._22 = _22;
return this;
}
mat2.prototype.copy = function(m) {
this._11 = m._11;
this._12 = m._12;
this._21 = m._21;
this._22 = m._22;
return this;
}
mat2.prototype.duplicate = function() {
return new mat2(this._11, this._12, this._21, this._22);
}
mat2.prototype.scale = function(s) {
this._11 *= s;
this._12 *= s;
this._21 *= s;
this._22 *= s;
return this;
}
mat2.prototype.mul = function(m) {
return this.set(
this._11 * m2._11 + this._12 * m2._21,
this._11 * m2._12 + this._12 * m2._22,
this._21 * m2._11 + this._22 * m2._21,
this._21 * m2._12 + this._22 * m2._22);
}
mat2.prototype.mulvec = function(v) {
return new vec2(
this._11 * v.x + this._12 * v.y,
this._21 * v.x + this._22 * v.y);
}
mat2.prototype.invert = function() {
var det = this._11 * this._22 - this._12 * this._21;
if (det != 0)
det = 1 / det;
return this.set(
this._22 * det, -this._12 * det,
-this._21 * det, this._11 * det);
}
// Solve A * x = b
mat2.prototype.solve = function(b) {
var det = this._11 * this._22 - this._12 * this._21;
if (det != 0)
det = 1 / det;
return new vec2(
det * (this._22 * b.x - this._12 * b.y),
det * (this._11 * b.y - this._21 * b.x));
}
mat2.mul = function(m1, m2) {
return new mat2(
m1._11 * m2._11 + m1._12 * m2._21,
m1._11 * m2._12 + m1._12 * m2._22,
m1._21 * m2._11 + m1._22 * m2._21,
m1._21 * m2._12 + m1._22 * m2._22);
}
//-----------------------------------
// 3x3 Matrix (row major)
//-----------------------------------
function mat3(_11, _12, _13, _21, _22, _23, _31, _32, _33) {
this._11 = _11 || 0;
this._12 = _12 || 0;
this._13 = _13 || 0;
this._21 = _21 || 0;
this._22 = _22 || 0;
this._23 = _23 || 0;
this._31 = _31 || 0;
this._32 = _32 || 0;
this._33 = _33 || 0;
}
mat3.zero = new mat3(0, 0, 0, 0, 0, 0, 0, 0, 0);
mat3.prototype.toString = function() {
return ["[", this._11, this._12, this._13, this_21, this._22, this._23, this._31, this._32, this._33, "]"].join(" ");
}
mat3.prototype.set = function(_11, _12, _13, _21, _22, _23, _31, _32, _33) {
this._11 = _11;
this._12 = _12;
this._13 = _13;
this._21 = _21;
this._22 = _22;
this._23 = _23;
this._31 = _31;
this._32 = _32;
this._33 = _33;
return this;
}
mat3.prototype.copy = function(m) {
this._11 = m._11;
this._12 = m._12;
this._13 = m._13;
this._21 = m._21;
this._22 = m._22;
this._23 = m._23;
this._31 = m._31;
this._32 = m._32;
this._33 = m._33;
return this;
}
mat3.prototype.duplicate = function() {
return new mat3(this._11, this._12, this._13, this._21, this._22, this._23, this._31, this._32, this._33);
}
mat3.prototype.scale = function(s) {
this._11 *= s;
this._12 *= s;
this._13 *= s;
this._21 *= s;
this._22 *= s;
this._23 *= s;
this._31 *= s;
this._32 *= s;
this._33 *= s;
return this;
}
mat3.prototype.mul = function(m) {
return this.set(
this._11 * m2._11 + this._12 * m2._21 + this._13 * m2._31,
this._11 * m2._12 + this._12 * m2._22 + this._13 * m2._32,
this._11 * m2._13 + this._12 * m2._23 + this._13 * m2._33,
this._21 * m2._11 + this._22 * m2._21 + this._23 * m2._31,
this._21 * m2._12 + this._22 * m2._22 + this._23 * m2._32,
this._21 * m2._13 + this._22 * m2._23 + this._23 * m2._33,
this._31 * m2._11 + this._32 * m2._21 + this._33 * m2._31,
this._31 * m2._12 + this._32 * m2._22 + this._33 * m2._32,
this._31 * m2._13 + this._32 * m2._23 + this._33 * m2._33);
}
mat3.prototype.mulvec = function(v) {
return new vec2(
this._11 * v.x + this._12 * v.y + this._13 * v.z,
this._21 * v.x + this._22 * v.y + this._23 * v.z,
this._31 * v.x + this._32 * v.y + this._33 * v.z);
}
mat3.prototype.invert = function() {
var det2_11 = this._22 * this._33 - this._23 * this._32;
var det2_12 = this._23 * this._31 - this._21 * this._33;
var det2_13 = this._21 * this._32 - this._22 * this._31;
var det = this._11 * det2_11 + this._12 * det2_12 + this._13 * det2_13;
if (det != 0)
det = 1 / det;
var det2_21 = this._13 * this._32 - this._12 * this._33;
var det2_22 = this._11 * this._33 - this._13 * this._31;
var det2_23 = this._12 * this._31 - this._11 * this._32;
var det2_31 = this._12 * this._23 - this._13 * this._22;
var det2_32 = this._13 * this._21 - this._11 * this._23;
var det2_33 = this._11 * this._22 - this._12 * this._21;
return this.set(
det2_11 * det, det2_12 * det, det2_13 * det,
det2_21 * det, det2_22 * det, det2_23 * det,
det2_31 * det, det2_32 * det, det2_33 * det);
}
// Solve A(2x2) * x = b
mat3.prototype.solve2x2 = function(b) {
var det = this._11 * this._22 - this._12 * this._21;
if (det != 0)
det = 1 / det;
return new vec2(
det * (this._22 * b.x - this._12 * b.y),
det * (this._11 * b.y - this._21 * b.x));
}
// Solve A(3x3) * x = b
mat3.prototype.solve = function(b) {
var det2_11 = this._22 * this._33 - this._23 * this._32;
var det2_12 = this._23 * this._31 - this._21 * this._33;
var det2_13 = this._21 * this._32 - this._22 * this._31;
var det = this._11 * det2_11 + this._12 * det2_12 + this._13 * det2_13;
if (det != 0)
det = 1 / det;
var det2_21 = this._13 * this._32 - this._12 * this._33;
var det2_22 = this._11 * this._33 - this._13 * this._31;
var det2_23 = this._12 * this._31 - this._11 * this._32;
var det2_31 = this._12 * this._23 - this._13 * this._22;
var det2_32 = this._13 * this._21 - this._11 * this._23;
var det2_33 = this._11 * this._22 - this._12 * this._21;
return new vec3(
det * (det2_11 * b.x + det2_12 * b.y + det2_13 * b.z),
det * (det2_21 * b.x + det2_22 * b.y + det2_23 * b.z),
det * (det2_31 * b.x + det2_32 * b.y + det2_33 * b.z));
}
mat3.mul = function(m1, m2) {
return new mat3(
m1._11 * m2._11 + m1._12 * m2._21 + m1._13 * m2._31,
m1._11 * m2._12 + m1._12 * m2._22 + m1._13 * m2._32,
m1._11 * m2._13 + m1._12 * m2._23 + m1._13 * m2._33,
m1._21 * m2._11 + m1._22 * m2._21 + m1._23 * m2._31,
m1._21 * m2._12 + m1._22 * m2._22 + m1._23 * m2._32,
m1._21 * m2._13 + m1._22 * m2._23 + m1._23 * m2._33,
m1._31 * m2._11 + m1._32 * m2._21 + m1._33 * m2._31,
m1._31 * m2._12 + m1._32 * m2._22 + m1._33 * m2._32,
m1._31 * m2._13 + m1._32 * m2._23 + m1._33 * m2._33);
}
//-----------------------------------
// 2D Transform
//-----------------------------------
Transform = function(pos, angle) {
this.t = pos.duplicate();
this.c = Math.cos(angle);
this.s = Math.sin(angle);
this.a = angle;
}
Transform.prototype.toString = function() {
return 't=' + this.t.toString() + ' c=' + this.c + ' s=' + this.s + ' a=' + this.a;
}
Transform.prototype.set = function(pos, angle) {
this.t.copy(pos);
this.c = Math.cos(angle);
this.s = Math.sin(angle);
this.a = angle;
return this;
}
Transform.prototype.setRotation = function(angle) {
this.c = Math.cos(angle);
this.s = Math.sin(angle);
this.a = angle;
return this;
}
Transform.prototype.setPosition = function(p) {
this.t.copy(p);
return this;
}
Transform.prototype.identity = function() {
this.t.set(0, 0);
this.c = 1;
this.s = 0;
this.a = 0;
return this;
}
Transform.prototype.rotate = function(v) {
return new vec2(v.x * this.c - v.y * this.s, v.x * this.s + v.y * this.c);
}
Transform.prototype.unrotate = function(v) {
return new vec2(v.x * this.c + v.y * this.s, -v.x * this.s + v.y * this.c);
}
Transform.prototype.transform = function(v) {
return new vec2(v.x * this.c - v.y * this.s + this.t.x, v.x * this.s + v.y * this.c + this.t.y);
}
Transform.prototype.untransform = function(v) {
var px = v.x - this.t.x;
var py = v.y - this.t.y;
return new vec2(px * this.c + py * this.s, -px * this.s + py * this.c);
}
//-----------------------------------
// 2D AABB
//-----------------------------------
Bounds = function(mins, maxs) {
this.mins = mins ? new vec2(mins.x, mins.y) : new vec2(999999, 999999);
this.maxs = maxs ? new vec2(maxs.x, maxs.y) : new vec2(-999999, -999999);
}
Bounds.prototype.toString = function() {
return ["mins:", this.mins.toString(), "maxs:", this.maxs.toString()].join(" ");
}
Bounds.prototype.set = function(mins, maxs) {
this.mins.set(mins.x, mins.y);
this.maxs.set(maxs.x, maxs.y);
}
Bounds.prototype.copy = function(b) {
this.mins.copy(b.mins);
this.maxs.copy(b.maxs);
return this;
}
Bounds.prototype.clear = function() {
this.mins.set(999999, 999999);
this.maxs.set(-999999, -999999);
return this;
}
Bounds.prototype.isEmpty = function() {
if (this.mins.x > this.maxs.x || this.mins.y > this.maxs.y)
return true;
}
Bounds.prototype.getCenter = function() {
return vec2.scale(vec2.add(this.mins, this.maxs), 0.5);
}
Bounds.prototype.getExtent = function() {
return vec2.scale(vec2.sub(this.maxs, this.mins), 0.5);
}
Bounds.prototype.getPerimeter = function() {
return (maxs.x - mins.x + maxs.y - mins.y) * 2;
}
Bounds.prototype.addPoint = function(p) {
if (this.mins.x > p.x) this.mins.x = p.x;
if (this.maxs.x < p.x) this.maxs.x = p.x;
if (this.mins.y > p.y) this.mins.y = p.y;
if (this.maxs.y < p.y) this.maxs.y = p.y;
return this;
}
Bounds.prototype.addBounds = function(b) {
if (this.mins.x > b.mins.x) this.mins.x = b.mins.x;
if (this.maxs.x < b.maxs.x) this.maxs.x = b.maxs.x;
if (this.mins.y > b.mins.y) this.mins.y = b.mins.y;
if (this.maxs.y < b.maxs.y) this.maxs.y = b.maxs.y;
return this;
}
Bounds.prototype.addBounds2 = function(mins, maxs) {
if (this.mins.x > mins.x) this.mins.x = mins.x;
if (this.maxs.x < maxs.x) this.maxs.x = maxs.x;
if (this.mins.y > mins.y) this.mins.y = mins.y;
if (this.maxs.y < maxs.y) this.maxs.y = maxs.y;
return this;
}
Bounds.prototype.addExtents = function(center, extent_x, extent_y) {
if (this.mins.x > center.x - extent_x) this.mins.x = center.x - extent_x;
if (this.maxs.x < center.x + extent_x) this.maxs.x = center.x + extent_x;
if (this.mins.y > center.y - extent_y) this.mins.y = center.y - extent_y;
if (this.maxs.y < center.y + extent_y) this.maxs.y = center.y + extent_y;
return this;
}
Bounds.prototype.expand = function(ax, ay) {
this.mins.x -= ax;
this.mins.y -= ay;
this.maxs.x += ax;
this.maxs.y += ay;
return this;
}
Bounds.prototype.containPoint = function(p) {
if (p.x < this.mins.x || p.x > this.maxs.x || p.y < this.mins.y || p.y > this.maxs.y)
return false;
return true;
}
Bounds.prototype.intersectsBounds = function(b) {
if (this.mins.x > b.maxs.x || this.maxs.x < b.mins.x || this.mins.y > b.maxs.y || this.maxs.y < b.mins.y)
return false;
return true;
}
Bounds.expand = function(b, ax, ay) {
var b = new Bounds(b.mins, b.maxs);
b.expand(ax, ay);
return b;
}
src/physics/advanced/Shape.js
+47
View File
@@ -0,0 +1,47 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
Shape = function(type) {
if (arguments.length == 0)
return;
if (Shape.id_counter == undefined)
Shape.id_counter = 0;
this.id = Shape.id_counter++;
this.type = type;
// Coefficient of restitution (elasticity)
this.e = 0.0;
// Frictional coefficient
this.u = 1.0;
// Mass density
this.density = 1;
// Axis-aligned bounding box
this.bounds = new Bounds;
}
Shape.TYPE_CIRCLE = 0;
Shape.TYPE_SEGMENT = 1;
Shape.TYPE_POLY = 2;
Shape.NUM_TYPES = 3;
src/physics/advanced/Space.js
+797
View File
@@ -0,0 +1,797 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
function Space() {
this.bodyArr = [];
this.bodyHash = {};
this.jointArr = [];
this.jointHash = {};
this.numContacts = 0;
this.contactSolverArr = [];
this.postSolve = function(arb) {};
this.gravity = new vec2(0, 0);
this.damping = 0;
this.log = [];
}
Space.TIME_TO_SLEEP = 0.5;
Space.SLEEP_LINEAR_TOLERANCE = 0.5;
Space.SLEEP_ANGULAR_TOLERANCE = deg2rad(2);
Space.prototype.clear = function() {
Shape.id_counter = 0;
Body.id_counter = 0;
Joint.id_counter = 0;
for (var i = 0; i < this.bodyArr.length; i++) {
if (this.bodyArr[i]) {
this.removeBody(this.bodyArr[i]);
}
}
this.bodyArr = [];
this.bodyHash = {};
this.jointArr = [];
this.jointHash = {};
this.contactSolverArr = [];
this.stepCount = 0;
}
Space.prototype.toJSON = function(key) {
var o_bodies = [];
for (var i = 0; i < this.bodyArr.length; i++) {
if (this.bodyArr[i]) {
o_bodies.push(this.bodyArr[i].serialize());
}
}
var o_joints = [];
for (var i = 0; i < this.jointArr.length; i++) {
if (this.jointArr[i]) {
o_joints.push(this.jointHash[i].serialize());
}
}
return {
bodies: o_bodies,
joints: o_joints
};
}
Space.prototype.create = function(text) {
var config = JSON.parse(text);
this.clear();
for (var i = 0; i < config.bodies.length; i++) {
var config_body = config.bodies[i];
var type = {"static": Body.Static, "kinetic": Body.KINETIC, "dynamic": Body.DYNAMIC}[config_body.type];
var body = new Body(type, config_body.position.x, config_body.position.y, config_body.angle);
for (var j = 0; j < config_body.shapes.length; j++) {
var config_shape = config_body.shapes[j];
var shape;
switch (config_shape.type) {
case "ShapeCircle":
shape = new ShapeCircle(config_shape.center.x, config_shape.center.y, config_shape.radius);
break;
case "ShapeSegment":
shape = new ShapeSegment(config_shape.a, config_shape.b, config_shape.radius);
break;
case "ShapePoly":
shape = new ShapePoly(config_shape.verts);
break;
}
shape.e = config_shape.e;
shape.u = config_shape.u;
shape.density = config_shape.density;
body.addShape(shape);
}
body.resetMassData();
this.addBody(body);
}
for (var i = 0; i < config.joints.length; i++) {
var config_joint = config.joints[i];
var body1 = this.bodyArr[this.bodyHash[config_joint.body1]];
var body2 = this.bodyArr[this.bodyHash[config_joint.body2]];
var joint;
switch (config_joint.type) {
case "AngleJoint":
joint = new AngleJoint(body1, body2);
break;
case "RevoluteJoint":
joint = new RevoluteJoint(body1, body2, config_joint.anchor);
joint.enableLimit(config_joint.limitEnabled);
joint.setLimits(config_joint.limitLowerAngle, config_joint.limitUpperAngle);
joint.enableMotor(config_joint.motorEnabled);
joint.setMotorSpeed(config_joint.motorSpeed);
joint.setMaxMotorTorque(config_joint.maxMotorTorque);
break;
case "WeldJoint":
joint = new WeldJoint(body1, body2, config_joint.anchor);
joint.setSpringFrequencyHz(config_joint.frequencyHz);
joint.setSpringDampingRatio(config_joint.dampingRatio);
break;
case "WheelJoint":
joint = new WheelJoint(body1, body2, config_joint.anchor1, config_joint.anchor2);
joint.enableMotor(config_joint.motorEnabled);
joint.setMotorSpeed(config_joint.motorSpeed);
joint.setMaxMotorTorque(config_joint.maxMotorTorque);
break;
case "PrismaticJoint":
joint = new PrismaticJoint(body1, body2, config_joint.anchor1, config_joint.anchor2);
break;
case "DistanceJoint":
joint = new DistanceJoint(body1, body2, config_joint.anchor1, config_joint.anchor2);
joint.setSpringFrequencyHz(config_joint.frequencyHz);
joint.setSpringDampingRatio(config_joint.dampingRatio);
break;
case "RopeJoint":
joint = new RopeJoint(body1, body2, config_joint.anchor1, config_joint.anchor2);
break;
}
joint.collideConnected = config_joint.collideConnected;
joint.maxForce = config_joint.maxForce;
joint.breakable = config_joint.breakable;
this.addJoint(joint);
}
}
Space.prototype.addBody = function(body) {
if (this.bodyHash[body.id] != undefined) {
return;
}
var index = this.bodyArr.push(body) - 1;
this.bodyHash[body.id] = index;
body.awake(true);
body.space = this;
body.cacheData();
}
Space.prototype.removeBody = function(body) {
if (this.bodyHash[body.id] == undefined) {
return;
}
// Remove linked joint
for (var i = 0; i < body.jointArr.length; i++) {
if (body.jointArr[i]) {
this.removeJoint(body.jointArr[i]);
}
}
body.space = null;
var index = this.bodyHash[body.id];
delete this.bodyHash[body.id];
delete this.bodyArr[index];
}
Space.prototype.addJoint = function(joint) {
if (this.jointHash[joint.id] != undefined) {
return;
}
joint.body1.awake(true);
joint.body2.awake(true);
var index = this.jointArr.push(joint) - 1;
this.jointHash[joint.id] = index;
var index = joint.body1.jointArr.push(joint) - 1;
joint.body1.jointHash[joint.id] = index;
var index = joint.body2.jointArr.push(joint) - 1;
joint.body2.jointHash[joint.id] = index;
}
Space.prototype.removeJoint = function(joint) {
if (this.jointHash[joint.id] == undefined) {
return;
}
joint.body1.awake(true);
joint.body2.awake(true);
var index = joint.body1.jointHash[joint.id];
delete joint.body1.jointHash[joint.id];
delete joint.body1.jointArr[index];
var index = joint.body2.jointHash[joint.id];
delete joint.body2.jointHash[joint.id];
delete joint.body2.jointArr[index];
var index = this.jointHash[joint.id];
delete this.jointHash[joint.id];
delete this.jointArr[index];
}
Space.prototype.findShapeByPoint = function(p, refShape) {
var firstShape;
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
for (var j = 0; j < body.shapeArr.length; j++) {
var shape = body.shapeArr[j];
if (shape.pointQuery(p)) {
if (!refShape) {
return shape;
}
if (!firstShape) {
firstShape = shape;
}
if (shape == refShape) {
refShape = null;
}
}
}
}
return firstShape;
}
Space.prototype.findBodyByPoint = function(p, refBody) {
var firstBody;
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
for (var j = 0; j < body.shapeArr.length; j++) {
var shape = body.shapeArr[j];
if (shape.pointQuery(p)) {
if (!refBody) {
return shape.body;
}
if (!firstBody) {
firstBody = shape.body;
}
if (shape.body == refBody) {
refBody = null;
}
break;
}
}
}
return firstBody;
}
// TODO: Replace this function to shape hashing
Space.prototype.shapeById = function(id) {
var shape;
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
for (var j = 0; j < body.shapeArr.length; j++) {
if (body.shapeArr[j].id == id) {
return body.shapeArr[j];
}
}
}
return null;
}
Space.prototype.jointById = function(id) {
var index = this.jointHash[id];
if (index != undefined) {
return this.jointArr[index];
}
return null;
}
Space.prototype.findVertexByPoint = function(p, minDist, refVertexId) {
var firstVertexId = -1;
refVertexId = refVertexId || -1;
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
for (var j = 0; j < body.shapeArr.length; j++) {
var shape = body.shapeArr[j];
var index = shape.findVertexByPoint(p, minDist);
if (index != -1) {
var vertex = (shape.id << 16) | index;
if (refVertexId == -1) {
return vertex;
}
if (firstVertexId == -1) {
firstVertexId = vertex;
}
if (vertex == refVertexId) {
refVertexId = -1;
}
}
}
}
return firstVertexId;
}
Space.prototype.findEdgeByPoint = function(p, minDist, refEdgeId) {
var firstEdgeId = -1;
refEdgeId = refEdgeId || -1;
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
for (var j = 0; j < body.shapeArr.length; j++) {
var shape = body.shapeArr[j];
if (shape.type != Shape.TYPE_POLY) {
continue;
}
var index = shape.findEdgeByPoint(p, minDist);
if (index != -1) {
var edge = (shape.id << 16) | index;
if (refEdgeId == -1) {
return edge;
}
if (firstEdgeId == -1) {
firstEdgeId = edge;
}
if (edge == refEdgeId) {
refEdgeId = -1;
}
}
}
}
return firstEdgeId;
}
Space.prototype.findJointByPoint = function(p, minDist, refJointId) {
var firstJointId = -1;
var dsq = minDist * minDist;
refJointId = refJointId || -1;
for (var i = 0; i < this.jointArr.length; i++) {
var joint = this.jointArr[i];
if (!joint) {
continue;
}
var jointId = -1;
if (vec2.distsq(p, joint.getWorldAnchor1()) < dsq) {
jointId = (joint.id << 16 | 0);
}
else if (vec2.distsq(p, joint.getWorldAnchor2()) < dsq) {
jointId = (joint.id << 16 | 1);
}
if (jointId != -1) {
if (refJointId == -1) {
return jointId;
}
if (firstJointId == -1) {
firstJointId = jointId;
}
if (jointId == refJointId) {
refJointId = -1;
}
}
}
return firstJointId;
}
Space.prototype.findContactSolver = function(shape1, shape2) {
for (var i = 0; i < this.contactSolverArr.length; i++) {
var contactSolver = this.contactSolverArr[i];
if (shape1 == contactSolver.shape1 && shape2 == contactSolver.shape2) {
return contactSolver;
}
}
return null;
}
Space.dump = function (phase, body) {
var s = "\n\nPhase: " + phase + "\n";
s += "Position: " + body.p.toString() + "\n";
s += "Velocity: " + body.v.toString() + "\n";
s += "Angle: " + body.a + "\n";
s += "Force: " + body.f.toString() + "\n";
s += "Torque: " + body.t + "\n";
s += "Bounds: " + body.bounds.toString() + "\n";
s += "Shape ***\n";
s += "Vert 0: " + body.shapeArr[0].verts[0].toString() + "\n";
s += "Vert 1: " + body.shapeArr[0].verts[1].toString() + "\n";
s += "Vert 2: " + body.shapeArr[0].verts[2].toString() + "\n";
s += "Vert 3: " + body.shapeArr[0].verts[3].toString() + "\n";
s += "TVert 0: " + body.shapeArr[0].tverts[0].toString() + "\n";
s += "TVert 1: " + body.shapeArr[0].tverts[1].toString() + "\n";
s += "TVert 2: " + body.shapeArr[0].tverts[2].toString() + "\n";
s += "TVert 3: " + body.shapeArr[0].tverts[3].toString() + "\n";
s += "Plane 0: " + body.shapeArr[0].planes[0].n.toString() + "\n";
s += "Plane 1: " + body.shapeArr[0].planes[1].n.toString() + "\n";
s += "Plane 2: " + body.shapeArr[0].planes[2].n.toString() + "\n";
s += "Plane 3: " + body.shapeArr[0].planes[3].n.toString() + "\n";
s += "TPlane 0: " + body.shapeArr[0].tplanes[0].n.toString() + "\n";
s += "TPlane 1: " + body.shapeArr[0].tplanes[1].n.toString() + "\n";
s += "TPlane 2: " + body.shapeArr[0].tplanes[2].n.toString() + "\n";
s += "TPlane 3: " + body.shapeArr[0].tplanes[3].n.toString() + "\n";
this.log.push(s);
}
Space.prototype.genTemporalContactSolvers = function() {
var t0 = Date.now();
var newContactSolverArr = [];
this.numContacts = 0;
for (var body1_index = 0; body1_index < this.bodyArr.length; body1_index++) {
var body1 = this.bodyArr[body1_index];
if (!body1) {
continue;
}
body1.stepCount = this.stepCount;
for (var body2_index = 0; body2_index < this.bodyArr.length; body2_index++) {
var body2 = this.bodyArr[body2_index];
if (!body2) {
continue;
}
if (body1.stepCount == body2.stepCount) {
continue;
}
var active1 = body1.isAwake() && !body1.isStatic();
var active2 = body2.isAwake() && !body2.isStatic();
if (!active1 && !active2) {
continue;
}
if (!body1.isCollidable(body2)) {
continue;
}
if (!body1.bounds.intersectsBounds(body2.bounds)) {
continue;
}
for (var i = 0; i < body1.shapeArr.length; i++) {
for (var j = 0; j < body2.shapeArr.length; j++) {
var shape1 = body1.shapeArr[i];
var shape2 = body2.shapeArr[j];
var contactArr = [];
if (!collision.collide(shape1, shape2, contactArr)) {
continue;
}
if (shape1.type > shape2.type) {
var temp = shape1;
shape1 = shape2;
shape2 = temp;
}
this.numContacts += contactArr.length;
var contactSolver = this.findContactSolver(shape1, shape2);
if (contactSolver) {
contactSolver.update(contactArr);
newContactSolverArr.push(contactSolver);
}
else {
body1.awake(true);
body2.awake(true);
var newContactSolver = new ContactSolver(shape1, shape2);
newContactSolver.contactArr = contactArr;
newContactSolver.e = Math.max(shape1.e, shape2.e);
newContactSolver.u = Math.sqrt(shape1.u * shape2.u);
newContactSolverArr.push(newContactSolver);
}
}
}
}
}
stats.timeCollision = Date.now() - t0;
return newContactSolverArr;
}
Space.prototype.initSolver = function(dt, dt_inv, warmStarting) {
var t0 = Date.now();
// Initialize contact solvers
for (var i = 0; i < this.contactSolverArr.length; i++) {
this.contactSolverArr[i].initSolver(dt_inv);
}
// Initialize joint solver
for (var i = 0; i < this.jointArr.length; i++) {
if (this.jointArr[i]) {
this.jointArr[i].initSolver(dt, warmStarting);
}
}
// Warm starting (apply cached impulse)
if (warmStarting) {
for (var i = 0; i < this.contactSolverArr.length; i++) {
this.contactSolverArr[i].warmStart();
}
}
stats.timeInitSolver = Date.now() - t0;
}
Space.prototype.velocitySolver = function(iteration) {
var t0 = Date.now();
for (var i = 0; i < iteration; i++) {
for (var j = 0; j < this.jointArr.length; j++) {
if (this.jointArr[j]) {
this.jointArr[j].solveVelocityConstraints();
}
}
for (var j = 0; j < this.contactSolverArr.length; j++) {
this.contactSolverArr[j].solveVelocityConstraints();
}
}
stats.timeVelocitySolver = Date.now() - t0;
}
Space.prototype.positionSolver = function(iteration) {
var t0 = Date.now();
var positionSolved = false;
stats.positionIterations = 0;
for (var i = 0; i < iteration; i++) {
var contactsOk = true;
var jointsOk = true;
for (var j = 0; j < this.contactSolverArr.length; j++) {
var contactOk = this.contactSolverArr[j].solvePositionConstraints();
contactsOk = contactOk && contactsOk;
}
for (var j = 0; j < this.jointArr.length; j++) {
if (this.jointArr[j]) {
var jointOk = this.jointArr[j].solvePositionConstraints();
jointsOk = jointOk && jointsOk;
}
}
if (contactsOk && jointsOk) {
// exit early if the position errors are small
positionSolved = true;
break;
}
stats.positionIterations++;
}
stats.timePositionSolver = Date.now() - t0;
return positionSolved;
}
Space.prototype.step = function(dt, vel_iteration, pos_iteration, warmStarting, allowSleep) {
var dt_inv = 1 / dt;
this.stepCount++;
// Generate contact & contactSolver
this.contactSolverArr = this.genTemporalContactSolvers();
// Initialize contacts & joints solver
this.initSolver(dt, dt_inv, warmStarting);
// Intergrate velocity
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
if (body.isDynamic() && body.isAwake()) {
body.updateVelocity(this.gravity, dt, this.damping);
}
}
for (var i = 0; i < this.jointArr.length; i++) {
var joint = this.jointArr[i];
if (!joint) {
continue;
}
var body1 = joint.body1;
var body2 = joint.body2;
var awake1 = body1.isAwake() && !body1.isStatic();
var awake2 = body2.isAwake() && !body2.isStatic();
if (awake1 ^ awake2) {
if (!awake1)
body1.awake(true);
if (!awake2)
body2.awake(true);
}
}
// Iterative velocity constraints solver
this.velocitySolver(vel_iteration);
// Intergrate position
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue
}
if (body.isDynamic() && body.isAwake()) {
body.updatePosition(dt);
}
}
// Process breakable joint
for (var i = 0; i < this.jointArr.length; i++) {
var joint = this.jointArr[i];
if (!joint) {
continue;
}
if (joint.breakable) {
if (joint.getReactionForce(dt_inv).lengthsq() >= joint.maxForce * joint.maxForce)
this.removeJoint(joint);
}
}
// Iterative position constraints solver
var positionSolved = this.positionSolver(pos_iteration);
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
body.syncTransform();
}
// Post solve collision callback
for (var i = 0; i < this.contactSolverArr.length; i++) {
var arb = this.contactSolverArr[i];
//this.postSolve(arb);
}
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
if (body.isDynamic() && body.isAwake()) {
body.cacheData();
}
}
// Process sleeping
if (allowSleep) {
var minSleepTime = 999999;
var linTolSqr = Space.SLEEP_LINEAR_TOLERANCE * Space.SLEEP_LINEAR_TOLERANCE;
var angTolSqr = Space.SLEEP_ANGULAR_TOLERANCE * Space.SLEEP_ANGULAR_TOLERANCE;
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
if (!body.isDynamic()) {
continue;
}
if (body.w * body.w > angTolSqr || body.v.dot(body.v) > linTolSqr) {
body.sleepTime = 0;
minSleepTime = 0;
}
else {
body.sleepTime += dt;
minSleepTime = Math.min(minSleepTime, body.sleepTime);
}
}
if (positionSolved && minSleepTime >= Space.TIME_TO_SLEEP) {
for (var i = 0; i < this.bodyArr.length; i++) {
var body = this.bodyArr[i];
if (!body) {
continue;
}
body.awake(false);
}
}
}
}
src/physics/advanced/Util.js
+147
View File
@@ -0,0 +1,147 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
function areaForCircle(radius_outer, radius_inner) {
return Math.PI * (radius_outer * radius_outer - radius_inner * radius_inner);
}
function inertiaForCircle(mass, center, radius_outer, radius_inner) {
return mass * ((radius_outer * radius_outer + radius_inner * radius_inner) * 0.5 + center.lengthsq());
}
function areaForSegment(a, b, radius) {
return radius * (Math.PI * radius + 2 * vec2.dist(a, b));
}
function centroidForSegment(a, b) {
return vec2.scale(vec2.add(a, b), 0.5);
}
function inertiaForSegment(mass, a, b) {
var distsq = vec2.distsq(b, a);
var offset = vec2.scale(vec2.add(a, b), 0.5);
return mass * (distsq / 12 + offset.lengthsq());
}
function areaForPoly(verts) {
var area = 0;
for (var i = 0; i < verts.length; i++) {
area += vec2.cross(verts[i], verts[(i + 1) % verts.length]);
}
return area / 2;
}
function centroidForPoly(verts) {
var area = 0;
var vsum = new vec2(0, 0);
for (var i = 0; i < verts.length; i++) {
var v1 = verts[i];
var v2 = verts[(i + 1) % verts.length];
var cross = vec2.cross(v1, v2);
area += cross;
vsum.addself(vec2.scale(vec2.add(v1, v2), cross));
}
return vec2.scale(vsum, 1 / (3 * area));
}
function inertiaForPoly(mass, verts, offset) {
var sum1 = 0;
var sum2 = 0;
for (var i = 0; i < verts.length; i++) {
var v1 = vec2.add(verts[i], offset);
var v2 = vec2.add(verts[(i+1) % verts.length], offset);
var a = vec2.cross(v2, v1);
var b = vec2.dot(v1, v1) + vec2.dot(v1, v2) + vec2.dot(v2, v2);
sum1 += a * b;
sum2 += a;
}
return (mass * sum1) / (6 * sum2);
}
function inertiaForBox(mass, w, h) {
return mass * (w * w + h * h) / 12;
}
// Create the convex hull using the Gift wrapping algorithm
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
function createConvexHull(points) {
// Find the right most point on the hull
var i0 = 0;
var x0 = points[0].x;
for (var i = 1; i < points.length; i++) {
var x = points[i].x;
if (x > x0 || (x == x0 && points[i].y < points[i0].y)) {
i0 = i;
x0 = x;
}
}
var n = points.length;
var hull = [];
var m = 0;
var ih = i0;
while (1) {
hull[m] = ih;
var ie = 0;
for (var j = 1; j < n; j++) {
if (ie == ih) {
ie = j;
continue;
}
var r = vec2.sub(points[ie], points[hull[m]]);
var v = vec2.sub(points[j], points[hull[m]]);
var c = vec2.cross(r, v);
if (c < 0) {
ie = j;
}
// Collinearity check
if (c == 0 && v.lengthsq() > r.lengthsq()) {
ie = j;
}
}
m++;
ih = ie;
if (ie == i0) {
break;
}
}
// Copy vertices
var newPoints = [];
for (var i = 0; i < m; ++i) {
newPoints.push(points[hull[i]]);
}
return newPoints;
}
src/physics/advanced/joints/Angle.js
+130
View File
@@ -0,0 +1,130 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Angle Joint
//
// C = a2 - a1 - refAngle
// Cdot = w2 - w1
// J = [0, -1, 0, 1]
//
// impulse = JT * lambda = [ 0, -lambda, 0, lambda ]
//-------------------------------------------------------------------------------------------------
AngleJoint = function(body1, body2) {
Joint.call(this, Joint.TYPE_ANGLE, body1, body2, true);
this.anchor1 = new vec2(0, 0);
this.anchor2 = new vec2(0, 0);
// Initial angle difference
this.refAngle = body2.a - body1.a;
// Accumulated lambda for angular velocity constraint
this.lambda_acc = 0;
}
AngleJoint.prototype = new Joint;
AngleJoint.prototype.constructor = AngleJoint;
AngleJoint.prototype.setWorldAnchor1 = function(anchor1) {
this.anchor1 = new vec2(0, 0);
}
AngleJoint.prototype.setWorldAnchor2 = function(anchor2) {
this.anchor2 = new vec2(0, 0);
}
AngleJoint.prototype.serialize = function() {
return {
"type": "AngleJoint",
"body1": this.body1.id,
"body2": this.body2.id,
"collideConnected": this.collideConnected
};
}
AngleJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// Max impulse
this.maxImpulse = this.maxForce * dt;
// invEM = J * invM * JT
var em_inv = body1.i_inv + body2.i_inv;
this.em = em_inv == 0 ? 0 : 1 / em_inv;
if (warmStarting) {
// Apply cached constraint impulses
// V += JT * lambda * invM
body1.w -= this.lambda_acc * body1.i_inv;
body2.w += this.lambda_acc * body2.i_inv;
}
else {
this.lambda_acc = 0;
}
}
AngleJoint.prototype.solveVelocityConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -J * V
var cdot = body2.w - body1.w;
var lambda = -this.em * cdot;
// Accumulate lambda
this.lambda_acc += lambda;
// Apply constraint impulses
// V += JT * lambda * invM
body1.w -= lambda * body1.i_inv;
body2.w += lambda * body2.i_inv;
}
AngleJoint.prototype.solvePositionConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Position (angle) constraint
var c = body2.a - body1.a - this.refAngle;
var correction = Math.clamp(c, -Joint.MAX_ANGULAR_CORRECTION, Joint.MAX_ANGULAR_CORRECTION);
// Compute lambda for position (angle) constraint
// Solve J * invM * JT * lambda = -C / dt
var lambda_dt = this.em * (-correction);
// Apply constraint impulses
// impulse = JT * lambda
// X += impulse * invM * dt
body1.a -= lambda_dt * body1.i_inv;
body2.a += lambda_dt * body2.i_inv;
return Math.abs(c) < Joint.ANGULAR_SLOP;
}
AngleJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.zero;
}
AngleJoint.prototype.getReactionTorque = function(dt_inv) {
return this.lambda_acc * dt_inv;
}
src/physics/advanced/joints/Distance.js
+522
View File
@@ -0,0 +1,522 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Distance Joint
//
// d = p2 - p1
// u = d / norm(d)
// C = norm(d) - l
// C = sqrt(dot(d, d)) - l
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = -dot(u, v1) - dot(w1, cross(r1, u)) + dot(u, v2) + dot(w2, cross(r2, u))
// J = [ -u, -cross(r1, u), u, cross(r2, u) ]
//
// impulse = JT * lambda = [ -u * lambda, -cross(r1, u) * lambda, u * lambda, cross(r1, u) * lambda ]
//-------------------------------------------------------------------------------------------------
DistanceJoint = function(body1, body2, anchor1, anchor2) {
Joint.call(this, Joint.TYPE_DISTANCE, body1, body2, true);
// Local anchor points
this.anchor1 = this.body1.getLocalPoint(anchor1);
this.anchor2 = this.body2.getLocalPoint(anchor2);
// Rest length
this.restLength = vec2.dist(anchor1, anchor2);
// Soft constraint coefficients
this.gamma = 0;
this.beta_c = 0;
// Spring coefficients
this.frequencyHz = 0;
this.dampingRatio = 0;
// Accumulated impulse
this.lambda_acc = 0;
}
DistanceJoint.prototype = new Joint;
DistanceJoint.prototype.constructor = DistanceJoint;
DistanceJoint.prototype.setWorldAnchor1 = function(anchor1) {
this.anchor1 = this.body1.getLocalPoint(anchor1);
this.restLength = vec2.dist(anchor1, this.getWorldAnchor2());
}
DistanceJoint.prototype.setWorldAnchor2 = function(anchor2) {
this.anchor2 = this.body2.getLocalPoint(anchor2);
this.restLength = vec2.dist(anchor2, this.getWorldAnchor1());
}
DistanceJoint.prototype.serialize = function() {
return {
"type": "DistanceJoint",
"body1": this.body1.id,
"body2": this.body2.id,
"anchor1": this.body1.getWorldPoint(this.anchor1),
"anchor2": this.body2.getWorldPoint(this.anchor2),
"collideConnected": this.collideConnected,
"maxForce": this.maxForce,
"breakable": this.breakable,
"frequencyHz": this.frequencyHz,
"dampingRatio": this.dampingRatio
};
}
DistanceJoint.prototype.setSpringFrequencyHz = function(frequencyHz) {
// NOTE: frequencyHz should be limited to under 4 times time steps
this.frequencyHz = frequencyHz;
}
DistanceJoint.prototype.setSpringDampingRatio = function(dampingRatio) {
this.dampingRatio = dampingRatio;
}
DistanceJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// Max impulse
this.maxImpulse = this.maxForce * dt;
// Transformed r1, r2
this.r1 = body1.xf.rotate(vec2.sub(this.anchor1, body1.centroid));
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
// Delta vector between two world anchors
var d = vec2.sub(vec2.add(body2.p, this.r2), vec2.add(body1.p, this.r1));
// Distance between two anchors
var dist = d.length();
// Unit delta vector
if (dist > Joint.LINEAR_SLOP) {
this.u = vec2.scale(d, 1 / dist);
}
else {
this.u = vec2.zero;
}
// s1, s2
this.s1 = vec2.cross(this.r1, this.u);
this.s2 = vec2.cross(this.r2, this.u);
// invEM = J * invM * JT
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * this.s1 * this.s1 + body2.i_inv * this.s2 * this.s2;
this.em = em_inv == 0 ? 0 : 1 / em_inv;
// Compute soft constraint parameters
if (this.frequencyHz > 0) {
// Frequency
var omega = 2 * Math.PI * this.frequencyHz;
// Spring stiffness
var k = this.em * omega * omega;
// Damping coefficient
var c = this.em * 2 * this.dampingRatio * omega;
// Soft constraint formulas
// gamma and beta are divided by dt to reduce computation
this.gamma = (c + k * dt) * dt;
this.gamma = this.gamma == 0 ? 0 : 1 / this.gamma;
var beta = dt * k * this.gamma;
// Position constraint
var pc = dist - this.restLength;
this.beta_c = beta * pc;
// invEM = invEM + gamma * I (to reduce calculation)
em_inv = em_inv + this.gamma;
this.em = em_inv == 0 ? 0 : 1 / em_inv;
}
else {
this.gamma = 0;
this.beta_c = 0;
}
if (warmStarting) {
// linearImpulse = JT * lambda
var impulse = vec2.scale(this.u, this.lambda_acc);
// Apply cached constraint impulses
// V += JT * lambda * invM
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.s1 * this.lambda_acc * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.s2 * this.lambda_acc * body2.i_inv;
}
else {
this.lambda_acc = 0;
}
}
DistanceJoint.prototype.solveVelocityConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -(J * V + beta * C + gamma * (lambda_acc + lambda))
var cdot = this.u.dot(vec2.sub(body2.v, body1.v)) + this.s2 * body2.w - this.s1 * body1.w;
var soft = this.beta_c + this.gamma * this.lambda_acc;
var lambda = -this.em * (cdot + soft);
// Accumulate lambda
this.lambda_acc += lambda;
// linearImpulse = JT * lambda
var impulse = vec2.scale(this.u, lambda);
// Apply constraint impulses
// V += JT * lambda * invM
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.s1 * lambda * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.s2 * lambda * body2.i_inv;
}
DistanceJoint.prototype.solvePositionConstraints = function() {
// There is no position correction for soft constraints
if (this.frequencyHz > 0) {
return true;
}
var body1 = this.body1;
var body2 = this.body2;
// Transformed r1, r2
var r1 = vec2.rotate(vec2.sub(this.anchor1, body1.centroid), body1.a);
var r2 = vec2.rotate(vec2.sub(this.anchor2, body2.centroid), body2.a);
// Delta vector between two anchors
var d = vec2.sub(vec2.add(body2.p, r2), vec2.add(body1.p, r1));
// Distance between two anchors
var dist = d.length();
// Unit delta vector
var u = vec2.scale(d, 1 / dist);
// Position constraint
var c = dist - this.restLength;
var correction = Math.clamp(c, -Joint.MAX_LINEAR_CORRECTION, Joint.MAX_LINEAR_CORRECTION);
// Compute lambda for correction
// Solve J * invM * JT * lambda = -C / dt
var s1 = vec2.cross(r1, u);
var s2 = vec2.cross(r2, u);
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * s1 * s1 + body2.i_inv * s2 * s2;
var lambda_dt = em_inv == 0 ? 0 : -correction / em_inv;
// Apply constraint impulses
// impulse = JT * lambda
// X += impulse * invM * dt
var impulse_dt = vec2.scale(u, lambda_dt);
body1.p.mad(impulse_dt, -body1.m_inv);
body1.a -= s1 * lambda_dt * body1.i_inv;
body2.p.mad(impulse_dt, body2.m_inv);
body2.a += s2 * lambda_dt * body2.i_inv;
return Math.abs(c) < Joint.LINEAR_SLOP;
}
DistanceJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.u, this.lambda_acc * dt_inv);
}
DistanceJoint.prototype.getReactionTorque = function(dt_inv) {
return 0;
}
/*
//------------------------------------------
// MaxDistance Joint
//------------------------------------------
MaxDistanceJoint = function(body1, body2, anchor1, anchor2, minDist, maxDist) {
Joint.call(this, body1, body2, true);
// Local anchor points
this.anchor1 = body1.getLocalPoint(anchor1);
this.anchor2 = body2.getLocalPoint(anchor2);
this.minDist = minDist || 0;
this.maxDist = maxDist;
if (maxDist == undefined) {
var p1 = vec2.add(vec2.rotate(vec2.sub(this.anchor1, body1.centroid), body1.a), body1.p);
var p2 = vec2.add(vec2.rotate(vec2.sub(this.anchor2, body2.centroid), body2.a), body2.p);
this.maxDist = vec2.dist(p1, p2);
}
// accumulated impulse
this.lambda_acc = 0;
}
MaxDistanceJoint.prototype = new Joint;
MaxDistanceJoint.prototype.constructor = MaxDistanceJoint;
MaxDistanceJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// max impulse
this.maxImpulse = this.maxForce * dt;
// transformed r1, r2
this.r1 = body1.xf.rotate(vec2.sub(this.anchor1, body1.centroid));
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
// delta vector between two anchors
var d = vec2.sub(vec2.add(body2.p, this.r2), vec2.add(body1.p, this.r1));
// distance between two anchors
var dist = d.length();
// unit delta vector
this.u = vec2.scale(d, 1 / dist);
// s1, s2
this.s1 = vec2.cross(this.r1, this.u);
this.s2 = vec2.cross(this.r2, this.u);
// invEM = J * invM * JT
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * this.s1 * this.s1 + body2.i_inv * this.s2 * this.s2;
this.em = em_inv == 0 ? 0 : 1 / em_inv;
// initial error
this.initial_err = 0;
if (dist < this.minDist) {
this.initial_err = dist - this.minDist;
}
else if (dist > this.maxDist) {
this.initial_err = dist - this.maxDist;
}
if (this.initial_err == 0) {
this.lambda_acc = 0;
}
if (warmStarting) {
// apply cached impulses
// V += JT * lambda
var impulse = vec2.scale(this.u, this.lambda_acc);
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.s1 * this.lambda_acc * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.s2 * this.lambda_acc * body2.i_inv;
}
else {
this.lambda_acc = 0;
}
}
MaxDistanceJoint.prototype.solveVelocityConstraints = function() {
if (this.initial_err == 0)
return;
var body1 = this.body1;
var body2 = this.body2;
// compute lambda for velocity constraint
// solve J * invM * JT * lambda = -J * V
var cdot = this.u.dot(vec2.sub(body2.v, body1.v)) + this.s2 * body2.w - this.s1 * body1.w;
var lambda = -this.em * cdot;
// accumulate lambda for velocity constraint
this.lambda_acc += lambda;
// apply impulses
// V += JT * lambda
var impulse = vec2.scale(this.u, lambda);
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.s1 * lambda * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.s2 * lambda * body2.i_inv;
}
MaxDistanceJoint.prototype.solvePositionConstraints = function() {
if (this.initial_err == 0)
return;
var body1 = this.body1;
var body2 = this.body2;
// transformed r1, r2
var r1 = vec2.rotate(vec2.sub(this.anchor1, body1.centroid), body1.a);
var r2 = vec2.rotate(vec2.sub(this.anchor2, body2.centroid), body2.a);
// World Center points
var pc1 = vec2.add(body1.p, body1.centroid);
var pc2 = vec2.add(body2.p, body2.centroid);
// delta vector between two anchors
var d = vec2.sub(vec2.add(pc2, r2), vec2.add(pc1, r1));
// distance between two anchors
var dist = d.length();
// unit delta vector
u = vec2.scale(d, 1 / dist);
// position constraint
var c = 0;
if (dist < this.minDist) {
c = dist - this.minDist;
}
else if (dist > this.maxDist) {
c = dist - this.maxDist;
}
var correction = Math.clamp(c, -Joint.MAX_LINEAR_CORRECTION, Joint.MAX_LINEAR_CORRECTION);
// compute lambda for position constraint
// solve J * invM * JT * lambda = -C / dt
var s1 = vec2.cross(r1, u);
var s2 = vec2.cross(r2, u);
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * s1 * s1 + body2.i_inv * s2 * s2;
var lambda_dt = em_inv == 0 ? 0 : -correction / em_inv;
// apply impulses
// X += JT * lambda * dt
var impulse_dt = vec2.scale(u, lambda_dt);
body1.p.mad(impulse_dt, -body1.m_inv);
body1.a -= s1 * lambda_dt * body1.i_inv;
body2.p.mad(impulse_dt, body2.m_inv);
body2.a += s2 * lambda_dt * body2.i_inv;
return Math.abs(c) < Joint.LINEAR_SLOP;
}
MaxDistanceJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.u, this.lambda_acc * dt_inv);
}
MaxDistanceJoint.prototype.getReactionTorque = function(dt_inv) {
return 0;
}
//------------------------------------------
// Damped Spring (Deprecated)
//------------------------------------------
SpringJoint = function(body1, body2, anchor1, anchor2, restLength, stiffness, damping) {
Joint.call(this, body1, body2, true);
// local anchor points
this.anchor1 = anchor1;
this.anchor2 = anchor2;
this.restLength = restLength;
this.stiffness = stiffness;
this.damping = damping;
}
SpringJoint.prototype = new Joint;
SpringJoint.prototype.constructor = SpringJoint;
SpringJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// transformed r1, r2
this.r1 = body1.xf.rotate(vec2.sub(this.anchor1, body2.centroid));
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
var d = vec2.sub(vec2.add(body2.p, this.r2), vec2.add(body1.p, this.r1));
var dist = d.length();
this.u = vec2.scale(d, 1 / dist);
// s1, s2
this.s1 = vec2.cross(this.r1, this.u);
this.s2 = vec2.cross(this.r2, this.u);
// invEM = J * invM * JT
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * this.s1 * this.s1 + body2.i_inv * this.s2 * this.s2;
this.em = em_inv == 0 ? 0 : 1 / em_inv;
//
this.target_rnv = 0;
this.v_coeff = 1.0 - Math.exp(-this.damping * dt * em_inv);
// apply spring force
var spring_f = (this.restLength - dist) * this.stiffness;
this.spring_impulse = spring_f * dt;
// apply impulses
// V += JT * lambda
var impulse = vec2.scale(this.u, this.spring_impulse);
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.s1 * this.spring_impulse * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.s2 * this.spring_impulse * body2.i_inv;
}
SpringJoint.prototype.solveVelocityConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// compute lambda for velocity constraint
// solve J * invM * JT * lambda = -J * V
var cdot = this.u.dot(vec2.sub(body2.v, body1.v)) + this.s2 * body2.w - this.s1 * body1.w;
var rnv = cdot + this.target_rnv;
// compute velocity loss from drag
var v_damp = rnv * this.v_coeff;
this.target_rnv = -rnv + v_damp;
var lambda = -this.em * v_damp;
// apply impulses
// V += JT * lambda
var impulse = vec2.scale(this.u, lambda);
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.s1 * lambda * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.s2 * lambda * body2.i_inv;
}
SpringJoint.prototype.solvePositionConstraints = function() {
return true;
}
SpringJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.u, this.spring_impulse * dt_inv);
}
SpringJoint.prototype.getReactionTorque = function(dt_inv) {
return 0;
}*/
src/physics/advanced/joints/Mouse.js
+150
View File
@@ -0,0 +1,150 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Mouse Joint
//
// p = attached point, m = mouse point (constant)
// C = p - m
// Cdot = v + cross(w, r)
// J = [ I, -skew(r) ]
//
// impulse = JT * lambda = [ lambda, cross(r2, lambda) ]
//-------------------------------------------------------------------------------------------------
MouseJoint = function(mouseBody, body, anchor) {
if (arguments.length == 0)
return;
Joint.call(this, Joint.TYPE_MOUSE, mouseBody, body, true);
// Local anchor points
this.anchor1 = this.body1.getLocalPoint(anchor);
this.anchor2 = this.body2.getLocalPoint(anchor);
// Soft constraint coefficients
this.gamma = 0;
this.beta_c = 0;
// Spring coefficients
this.frequencyHz = 5;
this.dampingRatio = 0.9;
// Accumulated impulse
this.lambda_acc = new vec2(0, 0);
}
MouseJoint.prototype = new Joint;
MouseJoint.prototype.constructor = MouseJoint;
MouseJoint.prototype.setSpringFrequencyHz = function(frequencyHz) {
this.frequencyHz = frequencyHz;
}
MouseJoint.prototype.setSpringDampingRatio = function(dampingRatio) {
this.dampingRatio = dampingRatio;
}
MouseJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// Max impulse
this.maxImpulse = this.maxForce * dt;
// Frequency
var omega = 2 * Math.PI * this.frequencyHz;
// Spring stiffness
var k = body2.m * (omega * omega);
// Damping coefficient
var d = body2.m * 2 * this.dampingRatio * omega;
// Soft constraint formulas
// gamma and beta are divided by dt to reduce computation
this.gamma = (d + k * dt) * dt;
this.gamma = this.gamma == 0 ? 0 : 1 / this.gamma;
var beta = dt * k * this.gamma;
// Transformed r
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
// invEM = J * invM * JT
var r2 = this.r2;
var r2y_i = r2.y * body2.i_inv;
var k11 = body2.m_inv + r2.y * r2y_i + this.gamma;
var k12 = -r2.x * r2y_i;
var k22 = body2.m_inv + r2.x * r2.x * body2.i_inv + this.gamma;
this.em_inv = new mat2(k11, k12, k12, k22);
// Position constraint
var c = vec2.sub(vec2.add(body2.p, this.r2), body1.p);
this.beta_c = vec2.scale(c, beta);
body2.w *= 0.98;
if (warmStarting) {
// Apply cached constraint impulse
// V += JT * lambda * invM
body2.v.mad(this.lambda_acc, body2.m_inv);
body2.w += vec2.cross(this.r2, this.lambda_acc) * body2.i_inv;
}
else {
this.lambda_acc.set(0, 0);
}
}
MouseJoint.prototype.solveVelocityConstraints = function() {
var body2 = this.body2;
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -(J * V + beta * C + gamma * (lambda_acc + lambda))
// in 2D: cross(w, r) = perp(r) * w
var cdot = vec2.mad(body2.v, vec2.perp(this.r2), body2.w);
var soft = vec2.mad(this.beta_c, this.lambda_acc, this.gamma);
var lambda = this.em_inv.solve(vec2.add(cdot, soft).neg());
// Accumulate lambda
var lambda_old = this.lambda_acc.duplicate();
this.lambda_acc.addself(lambda);
var lsq = this.lambda_acc.lengthsq();
if (lsq > this.maxImpulse * this.maxImpulse) {
this.lambda_acc.scale(this.maxImpulse / Math.sqrt(lsq));
}
lambda = vec2.sub(this.lambda_acc, lambda_old);
// Apply constraint impulse
// V += JT * lambda * invM
body2.v.mad(lambda, body2.m_inv);
body2.w += vec2.cross(this.r2, lambda) * body2.i_inv;
}
MouseJoint.prototype.solvePositionConstraints = function() {
return true;
}
MouseJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.lambda_acc, dt_inv);
}
MouseJoint.prototype.getReactionTorque = function(dt_inv) {
return 0;
}
src/physics/advanced/joints/Prismatic.js
+241
View File
@@ -0,0 +1,241 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Prismatic Joint
//
// Linear Constraint:
// d = p2 - p1
// n = normalize(perp(d))
// C1 = dot(n, d)
// C1dot = dot(d, dn/dt) + dot(n dd/dt)
// = dot(d, cross(w1, n)) + dot(n, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = dot(d, cross(w1, n)) + dot(n, v2) + dot(n, cross(w2, r2)) - dot(n, v1) - dot(n, cross(w1, r1))
// = -dot(n, v1) - dot(cross(d + r1, n), w1) + dot(n, v2) + dot(cross(r2, n), w2)
// J1 = [ -n, -s1, n, s2 ]
// s1 = cross(r1 + d, n)
// s2 = cross(r2, n)
//
// Angular Constraint:
// C2 = a2 - a1 - initial_da
// C2dot = w2 - w1
// J2 = [ 0, -1, 0, 1 ]
//
// Block Jacobian Matrix:
// J = [ -n, -s1, n, s2 ]
// [ 0, -1, 0, 1 ]
//
// impulse = JT * lambda = [ -n * lambda_x, -(s1 * lambda_x + lambda_y), n * lambda_x, s2 * lambda_x + lambda_y ]
//-------------------------------------------------------------------------------------------------
PrismaticJoint = function(body1, body2, anchor1, anchor2) {
Joint.call(this, Joint.TYPE_PRISMATIC, body1, body2, true);
// Local anchor points
this.anchor1 = this.body1.getLocalPoint(anchor1);
this.anchor2 = this.body2.getLocalPoint(anchor2);
var d = vec2.sub(anchor2, anchor1);
// Body1's local line normal
this.n_local = this.body1.getLocalVector(vec2.normalize(vec2.perp(d)));
this.da = body2.a - body1.a;
// Accumulated lambda
this.lambda_acc = new vec2(0, 0);
}
PrismaticJoint.prototype = new Joint;
PrismaticJoint.prototype.constructor = PrismaticJoint;
PrismaticJoint.prototype.setWorldAnchor1 = function(anchor1) {
// Local anchor points
this.anchor1 = this.body1.getLocalPoint(anchor1);
var d = vec2.sub(this.getWorldAnchor2(), anchor1);
// Body1's local line normal
this.n_local = this.body1.getLocalVector(vec2.normalize(vec2.perp(d)));
}
PrismaticJoint.prototype.setWorldAnchor2 = function(anchor2) {
// Local anchor points
this.anchor2 = this.body2.getLocalPoint(anchor2);
var d = vec2.sub(anchor2, this.getWorldAnchor1());
// Body1's local line normal
this.n_local = this.body1.getLocalVector(vec2.normalize(vec2.perp(d)));
}
PrismaticJoint.prototype.serialize = function() {
return {
"type": "PrismaticJoint",
"body1": this.body1.id,
"body2": this.body2.id,
"anchor1": this.body1.getWorldPoint(this.anchor1),
"anchor2": this.body2.getWorldPoint(this.anchor2),
"collideConnected": this.collideConnected,
"maxForce": this.maxForce,
"breakable": this.breakable
};
}
PrismaticJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// Max impulse
this.maxImpulse = this.maxForce * dt;
// Transformed r1, r2
this.r1 = body1.xf.rotate(vec2.sub(this.anchor1, body1.centroid));
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
// World anchor points
var p1 = vec2.add(body1.p, this.r1);
var p2 = vec2.add(body2.p, this.r2);
// Delta vector between world anchor points
var d = vec2.sub(p2, p1);
// r1 + d
this.r1_d = vec2.add(this.r1, d);
// World line normal
this.n = vec2.normalize(vec2.perp(d));
// s1, s2
this.s1 = vec2.cross(this.r1_d, this.n);
this.s2 = vec2.cross(this.r2, this.n);
// invEM = J * invM * JT
var s1 = this.s1;
var s2 = this.s2;
var s1_i = s1 * body1.i_inv;
var s2_i = s2 * body2.i_inv;
var k11 = body1.m_inv + body2.m_inv + s1 * s1_i + s2 * s2_i;
var k12 = s1_i + s2_i;
var k22 = body1.i_inv + body2.i_inv;
this.em_inv = new mat2(k11, k12, k12, k22);
if (warmStarting) {
// linearImpulse = JT * lambda
var impulse = vec2.scale(this.n, this.lambda_acc.x);
// Apply cached constraint impulses
// V += JT * lambda * invM
body1.v.mad(impulse, -body1.m_inv);
body1.w -= (this.s1 * this.lambda_acc.x + this.lambda_acc.y) * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += (this.s2 * this.lambda_acc.x + this.lambda_acc.y) * body2.i_inv;
}
else {
this.lambda_acc.set(0, 0);
}
}
PrismaticJoint.prototype.solveVelocityConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -J * V
var cdot1 = this.n.dot(vec2.sub(body2.v, body1.v)) + this.s2 * body2.w - this.s1 * body1.w;
var cdot2 = body2.w - body1.w;
var lambda = this.em_inv.solve(new vec2(-cdot1, -cdot2));
// Accumulate lambda
this.lambda_acc.addself(lambda);
// linearImpulse = JT * lambda
var impulse = vec2.scale(this.n, lambda.x);
// Apply constraint impulses
// V += JT * lambda * invM
body1.v.mad(impulse, -body1.m_inv);
body1.w -= (this.s1 * lambda.x + lambda.y) * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += (this.s2 * lambda.x + lambda.y) * body2.i_inv;
}
PrismaticJoint.prototype.solvePositionConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Transformed r1, r2
var r1 = vec2.rotate(vec2.sub(this.anchor1, body1.centroid), body1.a);
var r2 = vec2.rotate(vec2.sub(this.anchor2, body2.centroid), body2.a);
// World anchor points
var p1 = vec2.add(body1.p, r1);
var p2 = vec2.add(body2.p, r2);
// Delta vector between world anchor points
var d = vec2.sub(p2, p1);
// r1 + d
var r1_d = vec2.add(r1, d);
// World line normal
var n = vec2.rotate(this.n_local, body1.a);
// Position constraint
var c1 = vec2.dot(n, d);
var c2 = body2.a - body1.a - this.da;
var correction = new vec2;
correction.x = Math.clamp(c1, -Joint.MAX_LINEAR_CORRECTION, Joint.MAX_LINEAR_CORRECTION);
correction.y = Math.clamp(c2, -Joint.MAX_ANGULAR_CORRECTION, Joint.MAX_ANGULAR_CORRECTION);
// Compute impulse for position constraint
// Solve J * invM * JT * lambda = -C / dt
var s1 = vec2.cross(r1_d, n);
var s2 = vec2.cross(r2, n);
var s1_i = s1 * body1.i_inv;
var s2_i = s2 * body2.i_inv;
var k11 = body1.m_inv + body2.m_inv + s1 * s1_i + s2 * s2_i;
var k12 = s1_i + s2_i;
var k22 = body1.i_inv + body2.i_inv;
var em_inv = new mat2(k11, k12, k12, k22);
var lambda_dt = em_inv.solve(correction.neg());
// Apply constarint impulses
// impulse = JT * lambda
// X += impulse * invM * dt
var impulse_dt = vec2.scale(n, lambda_dt.x);
body1.p.mad(impulse_dt, -body1.m_inv);
body1.a -= (vec2.cross(r1_d, impulse_dt) + lambda_dt.y) * body1.i_inv;
body2.p.mad(impulse_dt, body2.m_inv);
body2.a += (vec2.cross(r2, impulse_dt) + lambda_dt.y) * body2.i_inv;
return Math.abs(c1) <= Joint.LINEAR_SLOP && Math.abs(c2) <= Joint.ANGULAR_SLOP;
}
PrismaticJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.n, this.lambda_acc.x * dt_inv);
}
PrismaticJoint.prototype.getReactionTorque = function(dt_inv) {
return this.lambda_acc.y * dt_inv;
}
src/physics/advanced/joints/Revolute.js
+378
View File
@@ -0,0 +1,378 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Revolute Joint
//
// Point-to-Point Constraint:
// C1 = p2 - p1
// C1dot = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// = -v1 + cross(r1, w1) + v2 - cross(r2, w1)
// J1 = [ -I, skew(r1), I, -skew(r2) ]
//
// Angular Constraint (for angle limit):
// C2 = a2 - a1 - refAngle
// C2dot = w2 - w1
// J2 = [ 0, -1, 0, 1 ]
//
// Block Jacobian Matrix:
// J = [ -I, skew(r1), I, -skew(r2) ]
// [ 0, -1, 0, 1 ]
//
// impulse = JT * lambda = [ -lambda_xy, -(cross(r1, lambda_xy) + lambda_z), lambda_xy, cross(r1, lambda_xy) + lambda_z ]
//-------------------------------------------------------------------------------------------------
RevoluteJoint = function(body1, body2, anchor) {
Joint.call(this, Joint.TYPE_REVOLUTE, body1, body2, false);
this.anchor1 = this.body1.getLocalPoint(anchor);
this.anchor2 = this.body2.getLocalPoint(anchor);
// Initial angle difference
this.refAngle = body2.a - body1.a;
// Accumulated lambda
this.lambda_acc = new vec3(0, 0, 0);
this.motorLambda_acc = 0;
// Angle limit
this.limitEnabled = false;
this.limitLowerAngle = 0;
this.limitUpperAngle = 0;
this.limitState = Joint.LIMIT_STATE_INACTIVE;
// Motor
this.motorEnabled = false;
this.motorSpeed = 0;
this.maxMotorTorque = 0;
}
RevoluteJoint.prototype = new Joint;
RevoluteJoint.prototype.constructor = RevoluteJoint;
RevoluteJoint.prototype.setWorldAnchor1 = function(anchor1) {
this.anchor1 = this.body1.getLocalPoint(anchor1);
this.anchor2 = this.body2.getLocalPoint(anchor1);
}
RevoluteJoint.prototype.setWorldAnchor2 = function(anchor2) {
this.anchor1 = this.body1.getLocalPoint(anchor2);
this.anchor2 = this.body2.getLocalPoint(anchor2);
}
RevoluteJoint.prototype.serialize = function() {
return {
"type": "RevoluteJoint",
"body1": this.body1.id,
"body2": this.body2.id,
"anchor": this.body1.getWorldPoint(this.anchor1),
"collideConnected": this.collideConnected,
"maxForce": this.maxForce,
"breakable": this.breakable,
"limitEnabled": this.limitEnabled,
"limitLowerAngle": this.limitLowerAngle,
"limitUpperAngle": this.limitUpperAngle,
"motorEnabled": this.motorEnabled,
"motorSpeed": this.motorSpeed,
"maxMotorTorque": this.maxMotorTorque
};
}
RevoluteJoint.prototype.enableMotor = function(flag) {
this.motorEnabled = flag;
}
RevoluteJoint.prototype.setMotorSpeed = function(speed) {
this.motorSpeed = speed;
}
RevoluteJoint.prototype.setMaxMotorTorque = function(torque) {
this.maxMotorTorque = torque;
}
RevoluteJoint.prototype.enableLimit = function(flag) {
this.limitEnabled = flag;
}
RevoluteJoint.prototype.setLimits = function(lower, upper) {
this.limitLowerAngle = lower;
this.limitUpperAngle = upper;
}
RevoluteJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// Max impulse
this.maxImpulse = this.maxForce * dt;
if (!this.motorEnabled) {
this.motorLambda_acc = 0;
}
else {
this.maxMotorImpulse = this.maxMotorTorque * dt;
}
if (this.limitEnabled) {
var da = body2.a - body1.a - this.refAngle;
if (Math.abs(this.limitUpperAngle - this.limitLowerAngle) < Joint.ANGULAR_SLOP) {
this.limitState = Joint.LIMIT_STATE_EQUAL_LIMITS;
}
else if (da <= this.limitLowerAngle) {
if (this.limitState != Joint.LIMIT_STATE_AT_LOWER) {
this.lambda_acc.z = 0;
}
this.limitState = Joint.LIMIT_STATE_AT_LOWER;
}
else if (da >= this.limitUpperAngle) {
if (this.limitState != Joint.LIMIT_STATE_AT_UPPER) {
this.lambda_acc.z = 0;
}
this.limitState = Joint.LIMIT_STATE_AT_UPPER;
}
else {
this.limitState = Joint.LIMIT_STATE_INACTIVE;
this.lambda_acc.z = 0;
}
}
else {
this.limitState = Joint.LIMIT_STATE_INACTIVE;
}
// Transformed r1, r2
this.r1 = body1.xf.rotate(vec2.sub(this.anchor1, body1.centroid));
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
// invEM = J * invM * JT
var sum_m_inv = body1.m_inv + body2.m_inv;
var r1 = this.r1;
var r2 = this.r2;
var r1x_i = r1.x * body1.i_inv;
var r1y_i = r1.y * body1.i_inv;
var r2x_i = r2.x * body2.i_inv;
var r2y_i = r2.y * body2.i_inv;
var k11 = sum_m_inv + r1.y * r1y_i + r2.y * r2y_i;
var k12 = -r1.x * r1y_i - r2.x * r2y_i;
var k13 = -r1y_i - r2y_i;
var k22 = sum_m_inv + r1.x * r1x_i + r2.x * r2x_i;
var k23 = r1x_i + r2x_i;
var k33 = body1.i_inv + body2.i_inv;
this.em_inv = new mat3(k11, k12, k13, k12, k22, k23, k13, k23, k33);
// K2 = J2 * invM * J2T
if (k33 != 0) {
this.em2 = 1 / k33;
}
if (warmStarting) {
// Apply cached constraint impulses
// V += JT * lambda
var lambda_xy = new vec2(this.lambda_acc.x, this.lambda_acc.y);
var lambda_z = this.lambda_acc.z + this.motorLambda_acc;
body1.v.mad(lambda_xy, -body1.m_inv);
body1.w -= (vec2.cross(this.r1, lambda_xy) + lambda_z) * body1.i_inv;
body2.v.mad(lambda_xy, body2.m_inv);
body2.w += (vec2.cross(this.r2, lambda_xy) + lambda_z) * body2.i_inv;
}
else {
this.lambda_acc.set(0, 0, 0);
this.motorLambda_acc = 0;
}
}
RevoluteJoint.prototype.solveVelocityConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Solve motor constraint
if (this.motorEnabled && this.limitState != Joint.LIMIT_STATE_EQUAL_LIMITS) {
// Compute motor impulse
var cdot = body2.w - body1.w - this.motorSpeed;
var lambda = -this.em2 * cdot;
var motorLambdaOld = this.motorLambda_acc;
this.motorLambda_acc = Math.clamp(this.motorLambda_acc + lambda, -this.maxMotorImpulse, this.maxMotorImpulse);
lambda = this.motorLambda_acc - motorLambdaOld;
// Apply motor constraint impulses
body1.w -= lambda * body1.i_inv;
body2.w += lambda * body2.i_inv;
}
// Solve point-to-point constraint with angular limit
if (this.limitEnabled && this.limitState != Joint.LIMIT_STATE_INACTIVE) {
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -J * V
// in 2D: cross(w, r) = perp(r) * w
var v1 = vec2.mad(body1.v, vec2.perp(this.r1), body1.w);
var v2 = vec2.mad(body2.v, vec2.perp(this.r2), body2.w);
var cdot1 = vec2.sub(v2, v1);
var cdot2 = body2.w - body1.w;
var cdot = vec3.fromVec2(cdot1, cdot2);
var lambda = this.em_inv.solve(cdot.neg());
if (this.limitState == Joint.LIMIT_STATE_EQUAL_LIMITS) {
// Accumulate lambda
this.lambda_acc.addself(lambda);
}
else if (this.limitState == Joint.LIMIT_STATE_AT_LOWER || this.limitState == Joint.LIMIT_STATE_AT_UPPER) {
// Accumulated new lambda.z
var newLambda_z = this.lambda_acc.z + lambda.z;
var lowerLimited = this.limitState == Joint.LIMIT_STATE_AT_LOWER && newLambda_z < 0;
var upperLimited = this.limitState == Joint.LIMIT_STATE_AT_UPPER && newLambda_z > 0;
if (lowerLimited || upperLimited) {
// Modify last equation to get lambda_acc.z to 0
// That is, lambda.z have to be equal -lambda_acc.z
// rhs = -J * V - (K_13, K_23, K_33) * (lambda.z + lambda_acc.z)
// Solve J * invM * JT * reduced_lambda = rhs
var rhs = vec2.add(cdot1, vec2.scale(new vec2(this.em_inv._13, this.em_inv._23), newLambda_z));
var reduced = this.em_inv.solve2x2(rhs.neg());
lambda.x = reduced.x;
lambda.y = reduced.y;
lambda.z = -this.lambda_acc.z;
// Accumulate lambda
this.lambda_acc.x += lambda.x;
this.lambda_acc.y += lambda.y;
this.lambda_acc.z = 0;
}
else {
// Accumulate lambda
this.lambda_acc.addself(lambda);
}
}
// Apply constraint impulses
// V += JT * lambda * invM
var lambda_xy = new vec2(lambda.x, lambda.y);
body1.v.mad(lambda_xy, -body1.m_inv);
body1.w -= (vec2.cross(this.r1, lambda_xy) + lambda.z) * body1.i_inv;
body2.v.mad(lambda_xy, body2.m_inv);
body2.w += (vec2.cross(this.r2, lambda_xy) + lambda.z) * body2.i_inv;
}
// Solve point-to-point constraint
else {
// Compute lambda for velocity constraint
// Solve J1 * invM * J1T * lambda = -J1 * V
// in 2D: cross(w, r) = perp(r) * w
var v1 = vec2.mad(body1.v, vec2.perp(this.r1), body1.w);
var v2 = vec2.mad(body2.v, vec2.perp(this.r2), body2.w);
var cdot = vec2.sub(v2, v1);
var lambda = this.em_inv.solve2x2(cdot.neg());
// Accumulate lambda
this.lambda_acc.addself(vec3.fromVec2(lambda, 0));
// Apply constraint impulses
// V += J1T * lambda * invM
body1.v.mad(lambda, -body1.m_inv);
body1.w -= vec2.cross(this.r1, lambda) * body1.i_inv;
body2.v.mad(lambda, body2.m_inv);
body2.w += vec2.cross(this.r2, lambda) * body2.i_inv;
}
}
RevoluteJoint.prototype.solvePositionConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
var angularError = 0;
var positionError = 0;
// Solve limit constraint
if (this.limitEnabled && this.limitState != Joint.LIMIT_STATE_INACTIVE) {
var da = body2.a - body1.a - this.refAngle;
// angular lambda = -EM * C / dt
var angularImpulseDt = 0;
if (this.limitState == Joint.LIMIT_STATE_EQUAL_LIMITS) {
var c = Math.clamp(da - this.limitLowerAngle, -Joint.MAX_ANGULAR_CORRECTION, Joint.MAX_ANGULAR_CORRECTION);
angularError = Math.abs(c);
angularImpulseDt = -this.em2 * c;
}
else if (this.limitState == Joint.LIMIT_STATE_AT_LOWER) {
var c = da - this.limitLowerAngle;
angularError = -c;
c = Math.clamp(c + Joint.ANGULAR_SLOP, -Joint.MAX_ANGULAR_CORRECTION, 0);
angularImpulseDt = -this.em2 * c;
}
else if (this.limitState == Joint.LIMIT_STATE_AT_UPPER) {
var c = da - this.limitUpperAngle;
angularError = c;
c = Math.clamp(c - Joint.ANGULAR_SLOP, 0, Joint.MAX_ANGULAR_CORRECTION);
angularImpulseDt = -this.em2 * c;
}
body1.a -= angularImpulseDt * body1.i_inv;
body2.a += angularImpulseDt * body2.i_inv;
}
// Solve point-to-point constraint
{
// Transformed r1, r2
var r1 = vec2.rotate(vec2.sub(this.anchor1, body1.centroid), body1.a);
var r2 = vec2.rotate(vec2.sub(this.anchor2, body2.centroid), body2.a);
// Position constraint
var c = vec2.sub(vec2.add(body2.p, r2), vec2.add(body1.p, r1));
var correction = vec2.truncate(c, Joint.MAX_LINEAR_CORRECTION);
positionError = correction.length();
// Compute lambda for position constraint
// Solve J1 * invM * J1T * lambda = -C / dt
var sum_m_inv = body1.m_inv + body2.m_inv;
var r1y_i = r1.y * body1.i_inv;
var r2y_i = r2.y * body2.i_inv;
var k11 = sum_m_inv + r1.y * r1y_i + r2.y * r2y_i;
var k12 = -r1.x * r1y_i - r2.x * r2y_i;
var k22 = sum_m_inv + r1.x * r1.x * body1.i_inv + r2.x * r2.x * body2.i_inv;
var em_inv = new mat2(k11, k12, k12, k22);
var lambda_dt = em_inv.solve(correction.neg());
// Apply constraint impulses
// impulse = J1T * lambda
// X += impulse * invM * dt
body1.p.mad(lambda_dt, -body1.m_inv);
body1.a -= vec2.cross(r1, lambda_dt) * body1.i_inv;
body2.p.mad(lambda_dt, body2.m_inv);
body2.a += vec2.cross(r2, lambda_dt) * body2.i_inv;
}
return positionError < Joint.LINEAR_SLOP && angularError < Joint.ANGULAR_SLOP;
}
RevoluteJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.lambda_acc, dt_inv);
}
RevoluteJoint.prototype.getReactionTorque = function(dt_inv) {
return 0;
}
src/physics/advanced/joints/Rope.js
+211
View File
@@ -0,0 +1,211 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Rope Joint
//
// d = p2 - p1
// u = d / norm(d)
// C = norm(d) - l
// C = sqrt(dot(d, d)) - l
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = -dot(u, v1) - dot(w1, cross(r1, u)) + dot(u, v2) + dot(w2, cross(r2, u))
// J = [ -u, -cross(r1, u), u, cross(r2, u) ]
//
// impulse = JT * lambda = [ -u * lambda, -cross(r1, u) * lambda, u * lambda, cross(r1, u) * lambda ]
//-------------------------------------------------------------------------------------------------
RopeJoint = function(body1, body2, anchor1, anchor2) {
Joint.call(this, Joint.TYPE_ROPE, body1, body2, true);
// Local anchor points
this.anchor1 = this.body1.getLocalPoint(anchor1);
this.anchor2 = this.body2.getLocalPoint(anchor2);
// Max distance
this.maxDistance = vec2.dist(anchor1, anchor2);
// Accumulated impulse
this.lambda_acc = 0;
}
RopeJoint.prototype = new Joint;
RopeJoint.prototype.constructor = RopeJoint;
RopeJoint.prototype.setWorldAnchor1 = function(anchor1) {
this.anchor1 = this.body1.getLocalPoint(anchor1);
this.maxDistance = vec2.dist(anchor1, this.getWorldAnchor2());
}
RopeJoint.prototype.setWorldAnchor2 = function(anchor2) {
this.anchor2 = this.body2.getLocalPoint(anchor2);
this.maxDistance = vec2.dist(anchor2, this.getWorldAnchor1());
}
RopeJoint.prototype.serialize = function() {
return {
"type": "RopeJoint",
"body1": this.body1.id,
"body2": this.body2.id,
"anchor1": this.body1.getWorldPoint(this.anchor1),
"anchor2": this.body2.getWorldPoint(this.anchor2),
"collideConnected": this.collideConnected,
"maxForce": this.maxForce,
"breakable": this.breakable,
};
}
RopeJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// Max impulse
this.maxImpulse = this.maxForce * dt;
// Transformed r1, r2
this.r1 = body1.xf.rotate(vec2.sub(this.anchor1, body1.centroid));
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
// Delta vector between two world anchors
var d = vec2.sub(vec2.add(body2.p, this.r2), vec2.add(body1.p, this.r1));
// Distance between two anchors
this.distance = d.length();
//
var c = this.distance - this.maxDistance;
if (c > 0) {
this.cdt = 0;
this.limitState = Joint.LIMIT_STATE_AT_UPPER;
}
else {
this.cdt = c / dt;
this.limitState = Joint.LIMIT_STATE_INACTIVE;
}
// Unit delta vector
if (this.distance > Joint.LINEAR_SLOP) {
this.u = vec2.scale(d, 1 / this.distance);
}
else {
this.u = vec2.zero;
}
// s1, s2
this.s1 = vec2.cross(this.r1, this.u);
this.s2 = vec2.cross(this.r2, this.u);
// invEM = J * invM * JT
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * this.s1 * this.s1 + body2.i_inv * this.s2 * this.s2;
this.em = em_inv == 0 ? 0 : 1 / em_inv;
if (warmStarting) {
// linearImpulse = JT * lambda
var impulse = vec2.scale(this.u, this.lambda_acc);
// Apply cached constraint impulses
// V += JT * lambda * invM
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.s1 * this.lambda_acc * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.s2 * this.lambda_acc * body2.i_inv;
}
else {
this.lambda_acc = 0;
}
}
RopeJoint.prototype.solveVelocityConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -(J * V)
var cdot = this.u.dot(vec2.sub(body2.v, body1.v)) + this.s2 * body2.w - this.s1 * body1.w;
var lambda = -this.em * (cdot + this.cdt);
// Accumulate lambda and clamp it to zero
var lambda_old = this.lambda_acc;
this.lambda_acc = Math.min(lambda_old + lambda, 0);
lambda = this.lambda_acc - lambda_old;
// linearImpulse = JT * lambda
var impulse = vec2.scale(this.u, lambda);
// Apply constraint impulses
// V += JT * lambda * invM
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.s1 * lambda * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.s2 * lambda * body2.i_inv;
}
RopeJoint.prototype.solvePositionConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Transformed r1, r2
var r1 = vec2.rotate(vec2.sub(this.anchor1, body1.centroid), body1.a);
var r2 = vec2.rotate(vec2.sub(this.anchor2, body2.centroid), body2.a);
// Delta vector between two anchors
var d = vec2.sub(vec2.add(body2.p, r2), vec2.add(body1.p, r1));
// Distance between two anchors
var dist = d.length();
// Unit delta vector
var u = vec2.scale(d, 1 / dist);
// Position constraint
var c = dist - this.maxDistance;
var correction = Math.clamp(c, 0, Joint.MAX_LINEAR_CORRECTION);
// Compute lambda for correction
// Solve J * invM * JT * lambda = -C / dt
var s1 = vec2.cross(r1, u);
var s2 = vec2.cross(r2, u);
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * s1 * s1 + body2.i_inv * s2 * s2;
var lambda_dt = em_inv == 0 ? 0 : -correction / em_inv;
// Apply constraint impulses
// impulse = JT * lambda
// X += impulse * invM * dt
var impulse_dt = vec2.scale(u, lambda_dt);
body1.p.mad(impulse_dt, -body1.m_inv);
body1.a -= s1 * lambda_dt * body1.i_inv;
body2.p.mad(impulse_dt, body2.m_inv);
body2.a += s2 * lambda_dt * body2.i_inv;
return c < Joint.LINEAR_SLOP;
}
RopeJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.u, this.lambda_acc * dt_inv);
}
RopeJoint.prototype.getReactionTorque = function(dt_inv) {
return 0;
}
src/physics/advanced/joints/Weld.js
+306
View File
@@ -0,0 +1,306 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Weld Joint
//
// Point-to-Point Constraint:
// C1 = p2 - p1
// Cdot1 = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// = -v1 + cross(r1, w1) + v2 - cross(r2, w1)
// J1 = [ -I, skew(r1), I, -skew(r2) ]
//
// Angular Constraint:
// C2 = a2 - a1
// C2dot = w2 - w1
// J2 = [ 0, -1, 0, 1 ]
//
// Block Jacobian Matrix:
// J = [ -I, skew(r1), I, -skew(r2) ]
// [ 0, -1, 0, 1 ]
//
// impulse = JT * lambda = [ -lambda_xy, -(cross(r1, lambda_xy) + lambda_z), lambda_xy, cross(r1, lambda_xy) + lambda_z ]
//-------------------------------------------------------------------------------------------------
WeldJoint = function(body1, body2, anchor) {
Joint.call(this, Joint.TYPE_WELD, body1, body2, false);
this.anchor1 = this.body1.getLocalPoint(anchor);
this.anchor2 = this.body2.getLocalPoint(anchor);
// Soft constraint coefficients
this.gamma = 0;
this.beta_c = 0;
// Spring coefficients
this.frequencyHz = 0;
this.dampingRatio = 0;
// Accumulated lambda
this.lambda_acc = new vec3(0, 0, 0);
}
WeldJoint.prototype = new Joint;
WeldJoint.prototype.constructor = WeldJoint;
WeldJoint.prototype.setWorldAnchor1 = function(anchor1) {
this.anchor1 = this.body1.getLocalPoint(anchor1);
this.anchor2 = this.body2.getLocalPoint(anchor1);
}
WeldJoint.prototype.setWorldAnchor2 = function(anchor2) {
this.anchor1 = this.body1.getLocalPoint(anchor2);
this.anchor2 = this.body2.getLocalPoint(anchor2);
}
WeldJoint.prototype.serialize = function() {
return {
"type": "WeldJoint",
"body1": this.body1.id,
"body2": this.body2.id,
"anchor1": this.body1.getWorldPoint(this.anchor1),
"anchor2": this.body2.getWorldPoint(this.anchor2),
"collideConnected": this.collideConnected,
"maxForce": this.maxForce,
"breakable": this.breakable,
"frequencyHz": this.frequencyHz,
"dampingRatio": this.dampingRatio
};
}
WeldJoint.prototype.setSpringFrequencyHz = function(frequencyHz) {
// NOTE: frequencyHz should be limited to under 4 times time steps
this.frequencyHz = frequencyHz;
}
WeldJoint.prototype.setSpringDampingRatio = function(dampingRatio) {
this.dampingRatio = dampingRatio;
}
WeldJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// Max impulse
this.maxImpulse = this.maxForce * dt;
// Transformed r1, r2
this.r1 = body1.xf.rotate(vec2.sub(this.anchor1, body1.centroid));
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
// invEM = J * invM * JT
var sum_m_inv = body1.m_inv + body2.m_inv;
var r1 = this.r1;
var r2 = this.r2;
var r1x_i = r1.x * body1.i_inv;
var r1y_i = r1.y * body1.i_inv;
var r2x_i = r2.x * body2.i_inv;
var r2y_i = r2.y * body2.i_inv;
var k11 = sum_m_inv + r1.y * r1y_i + r2.y * r2y_i;
var k12 = -r1.x * r1y_i - r2.x * r2y_i;
var k13 = -r1y_i - r2y_i;
var k22 = sum_m_inv + r1.x * r1x_i + r2.x * r2x_i;
var k23 = r1x_i + r2x_i;
var k33 = body1.i_inv + body2.i_inv;
this.em_inv = new mat3(k11, k12, k13, k12, k22, k23, k13, k23, k33);
// Compute soft constraint parameters
if (this.frequencyHz > 0) {
var m = k33 > 0 ? 1 / k33 : 0;
// Frequency
var omega = 2 * Math.PI * this.frequencyHz;
// Spring stiffness
var k = m * omega * omega;
// Damping coefficient
var c = m * 2 * this.dampingRatio * omega;
// Soft constraint formulas
// gamma and beta are divided by dt to reduce computation
this.gamma = (c + k * dt) * dt;
this.gamma = this.gamma == 0 ? 0 : 1 / this.gamma;
var beta = dt * k * this.gamma;
// Position constraint
var pc = body2.a - body1.a;
this.beta_c = beta * pc;
// invEM = invEM + gamma * I (to reduce calculation)
this.em_inv._33 += this.gamma;
}
else {
this.gamma = 0;
this.beta_c = 0;
}
if (warmStarting) {
// Apply cached constraint impulses
// V += JT * lambda * invM
var lambda_xy = new vec2(this.lambda_acc.x, this.lambda_acc.y);
var lambda_z = this.lambda_acc.z;
body1.v.mad(lambda_xy, -body1.m_inv);
body1.w -= (vec2.cross(this.r1, lambda_xy) + lambda_z) * body1.i_inv;
body2.v.mad(lambda_xy, body2.m_inv);
body2.w += (vec2.cross(this.r2, lambda_xy) + lambda_z) * body2.i_inv;
}
else {
this.lambda_acc.set(0, 0, 0);
}
}
WeldJoint.prototype.solveVelocityConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
if (this.frequencyHz > 0) {
// Compute lambda for angular velocity constraint
// Solve J2 * invM * J2T * lambda = -(J2 * V + beta * C + gamma * (lambda_acc + lambda))
var cdot2 = body2.w - body1.w;
lambda_z = -(cdot2 + this.beta_c + this.gamma * this.lambda_acc.z) / this.em_inv._33;
// Apply angular constraint impulses
// V += J2T * lambda * invM
body1.w -= lambda_z * body1.i_inv;
body2.w += lambda_z * body2.i_inv;
// Compute lambda for velocity constraint
// Solve J1 * invM * J1T * lambda = -J1 * V
var v1 = vec2.mad(body1.v, vec2.perp(this.r1), body1.w);
var v2 = vec2.mad(body2.v, vec2.perp(this.r2), body2.w);
var cdot1 = vec2.sub(v2, v1);
var lambda_xy = this.em_inv.solve2x2(cdot1.neg());
// Accumulate lambda
this.lambda_acc.x += lambda_xy.x;
this.lambda_acc.y += lambda_xy.y;
this.lambda_acc.z += lambda_z;
// Apply constraint impulses
// V += J1T * lambda * invM
body1.v.mad(lambda_xy, -body1.m_inv);
body1.w -= vec2.cross(this.r1, lambda_xy) * body1.i_inv;
body2.v.mad(lambda_xy, body2.m_inv);
body2.w += vec2.cross(this.r2, lambda_xy) * body2.i_inv;
}
else {
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -J * V
// in 2D: cross(w, r) = perp(r) * w
var v1 = vec2.mad(body1.v, vec2.perp(this.r1), body1.w);
var v2 = vec2.mad(body2.v, vec2.perp(this.r2), body2.w);
var cdot1 = vec2.sub(v2, v1);
var cdot2 = body2.w - body1.w;
var cdot = vec3.fromVec2(cdot1, cdot2);
var lambda = this.em_inv.solve(cdot.neg());
// Accumulate lambda
this.lambda_acc.addself(lambda);
// Apply constraint impulses
// V += JT * lambda * invM
var lambda_xy = new vec2(lambda.x, lambda.y);
body1.v.mad(lambda_xy, -body1.m_inv);
body1.w -= (vec2.cross(this.r1, lambda_xy) + lambda.z) * body1.i_inv;
body2.v.mad(lambda_xy, body2.m_inv);
body2.w += (vec2.cross(this.r2, lambda_xy) + lambda.z) * body2.i_inv;
}
}
WeldJoint.prototype.solvePositionConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Transformed r1, r2
var r1 = vec2.rotate(vec2.sub(this.anchor1, body1.centroid), body1.a);
var r2 = vec2.rotate(vec2.sub(this.anchor2, body2.centroid), body2.a);
// Compute J * invM * JT
var sum_m_inv = body1.m_inv + body2.m_inv;
var r1x_i = r1.x * body1.i_inv;
var r1y_i = r1.y * body1.i_inv;
var r2x_i = r2.x * body2.i_inv;
var r2y_i = r2.y * body2.i_inv;
var k11 = sum_m_inv + r1.y * r1y_i + r2.y * r2y_i;
var k12 = -r1.x * r1y_i - r2.x * r2y_i;
var k13 = -r1y_i - r2y_i;
var k22 = sum_m_inv + r1.x * r1x_i + r2.x * r2x_i;
var k23 = r1x_i + r2x_i;
var k33 = body1.i_inv + body2.i_inv;
var em_inv = new mat3(k11, k12, k13, k12, k22, k23, k13, k23, k33);
if (this.frequencyHz > 0) {
// Position constraint
var c1 = vec2.sub(vec2.add(body2.p, r2), vec2.add(body1.p, r1));
var c2 = 0;
var correction = vec2.truncate(c1, Joint.MAX_LINEAR_CORRECTION);
// Compute lambda for position constraint
// Solve J1 * invM * J1T * lambda = -C / dt
var lambda_dt_xy = em_inv.solve2x2(correction.neg());
// Apply constraint impulses
// impulse = J1T * lambda
// X += impulse * invM * dt
body1.p.mad(lambda_dt_xy, -body1.m_inv);
body1.a -= vec2.cross(r1, lambda_dt_xy) * body1.i_inv;
body2.p.mad(lambda_dt_xy, body2.m_inv);
body2.a += vec2.cross(r2, lambda_dt_xy) * body2.i_inv;
}
else {
// Position constraint
var c1 = vec2.sub(vec2.add(body2.p, r2), vec2.add(body1.p, r1));
var c2 = body2.a - body1.a;
var correction = vec3.fromVec2(
vec2.truncate(c1, Joint.MAX_LINEAR_CORRECTION),
Math.clamp(c2, -Joint.MAX_ANGULAR_CORRECTION, Joint.MAX_ANGULAR_CORRECTION));
// Compute lambda for position constraint
// Solve J * invM * JT * lambda = -C / dt
var lambda_dt = em_inv.solve(correction.neg());
// Apply constraint impulses
// impulse = JT * lambda
// X += impulse * invM * dt
var lambda_dt_xy = new vec2(lambda_dt.x, lambda_dt.y);
body1.p.mad(lambda_dt_xy, -body1.m_inv);
body1.a -= (vec2.cross(r1, lambda_dt_xy) + lambda_dt.z) * body1.i_inv;
body2.p.mad(lambda_dt_xy, body2.m_inv);
body2.a += (vec2.cross(r2, lambda_dt_xy) + lambda_dt.z) * body2.i_inv;
}
return c1.length() < Joint.LINEAR_SLOP && Math.abs(c2) <= Joint.ANGULAR_SLOP;
}
WeldJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.lambda_acc.toVec2(), dt_inv);
}
WeldJoint.prototype.getReactionTorque = function(dt_inv) {
return this.lambda_acc.z * dt_inv;
}
src/physics/advanced/joints/Wheel.js
+376
View File
@@ -0,0 +1,376 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//-------------------------------------------------------------------------------------------------
// Wheel Joint
//
// Point-to-Line constraint:
// d = p2 - p1
// n = normalize(perp(d))
// C = dot(n, d)
// Cdot = dot(d, dn/dt) + dot(n dd/dt)
// = dot(d, cross(w1, n)) + dot(n, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = dot(d, cross(w1, n)) + dot(n, v2) + dot(n, cross(w2, r2)) - dot(n, v1) - dot(n, cross(w1, r1))
// = -dot(n, v1) - dot(cross(d + r1, n), w1) + dot(n, v2) + dot(cross(r2, n), w2)
// J = [ -n, -sn1, n, sn2 ]
// sn1 = cross(r1 + d, n)
// sn2 = cross(r2, n)
//
// impulse = JT * lambda = [ -n * lambda, -(sn1 * lambda), n * lambda, sn2 * lambda ]
//
// Spring constraint:
// u = normalize(d)
// C = dot(u, d)
// Cdot = -dot(u, v1) - dot(cross(d + r1, u), w1) + dot(u, v2) + dot(cross(r2, u), w2)
// J = [ -u, -su1, u, su2 ]
// su1 = cross(r1 + d, u)
// su2 = cross(r2, u)
//
// impulse = JT * lambda = [ -u * lambda, -(su1 * lambda), u * lambda, su2 * lambda ]
//
// Motor rotational constraint:
// Cdot = w2 - w1
// J = [ 0, -1, 0, 1 ]
//-------------------------------------------------------------------------------------------------
WheelJoint = function(body1, body2, anchor1, anchor2) {
Joint.call(this, Joint.TYPE_WHEEL, body1, body2, true);
// Local anchor points
this.anchor1 = this.body1.getLocalPoint(anchor1);
this.anchor2 = this.body2.getLocalPoint(anchor2);
var d = vec2.sub(anchor2, anchor1);
// Rest length
this.restLength = d.length();
// Body1's local axis
this.u_local = this.body1.getLocalVector(vec2.normalize(d));
this.n_local = vec2.perp(this.u_local);
// Accumulated impulse
this.lambda_acc = 0;
this.motorLambda_acc = 0;
this.springLambda_acc = 0;
// Motor
this.motorEnabled = false;
this.motorSpeed = 0;
this.maxMotorTorque = 0;
// Soft constraint coefficients
this.gamma = 0;
this.beta_c = 0;
// Spring coefficients
this.frequencyHz = 0;
this.dampingRatio = 0;
}
WheelJoint.prototype = new Joint;
WheelJoint.prototype.constructor = WheelJoint;
WheelJoint.prototype.setWorldAnchor1 = function(anchor1) {
this.anchor1 = this.body1.getLocalPoint(anchor1);
var d = vec2.sub(this.getWorldAnchor2(), anchor1);
this.u_local = this.body1.getLocalVector(vec2.normalize(d));
this.n_local = vec2.perp(this.u_local);
}
WheelJoint.prototype.setWorldAnchor2 = function(anchor2) {
this.anchor2 = this.body2.getLocalPoint(anchor2);
var d = vec2.sub(anchor2, this.getWorldAnchor1());
this.u_local = this.body1.getLocalVector(vec2.normalize(d));
this.n_local = vec2.perp(this.u_local);
}
WheelJoint.prototype.serialize = function() {
return {
"type": "WheelJoint",
"body1": this.body1.id,
"body2": this.body2.id,
"anchor1": this.body1.getWorldPoint(this.anchor1),
"anchor2": this.body2.getWorldPoint(this.anchor2),
"collideConnected": this.collideConnected,
"maxForce": this.maxForce,
"breakable": this.breakable,
"motorEnabled": this.motorEnabled,
"motorSpeed": this.motorSpeed,
"maxMotorTorque": this.maxMotorTorque,
"frequencyHz": this.frequencyHz,
"dampingRatio": this.dampingRatio
};
}
WheelJoint.prototype.setSpringFrequencyHz = function(frequencyHz) {
// NOTE: frequencyHz should be limited to under 4 times time steps
this.frequencyHz = frequencyHz;
}
WheelJoint.prototype.setSpringDampingRatio = function(dampingRatio) {
this.dampingRatio = dampingRatio;
}
WheelJoint.prototype.enableMotor = function(flag) {
this.motorEnabled = flag;
}
WheelJoint.prototype.setMotorSpeed = function(speed) {
this.motorSpeed = speed;
}
WheelJoint.prototype.setMaxMotorTorque = function(torque) {
this.maxMotorTorque = torque;
}
WheelJoint.prototype.initSolver = function(dt, warmStarting) {
var body1 = this.body1;
var body2 = this.body2;
// Max impulse
this.maxImpulse = this.maxForce * dt;
// Transformed r1, r2
this.r1 = body1.xf.rotate(vec2.sub(this.anchor1, body1.centroid));
this.r2 = body2.xf.rotate(vec2.sub(this.anchor2, body2.centroid));
// World anchor points
var p1 = vec2.add(body1.p, this.r1);
var p2 = vec2.add(body2.p, this.r2);
// Delta vector between world anchor points
var d = vec2.sub(p2, p1);
// r1 + d
this.r1_d = vec2.add(this.r1, d);
// World line normal
this.n = vec2.rotate(this.n_local, body1.a);
// sn1, sn2
this.sn1 = vec2.cross(this.r1_d, this.n);
this.sn2 = vec2.cross(this.r2, this.n);
// invEM = J * invM * JT
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * this.sn1 * this.sn1 + body2.i_inv * this.sn2 * this.sn2;
this.em = em_inv > 0 ? 1 / em_inv : em_inv;
// Compute soft constraint parameters
if (this.frequencyHz > 0) {
// World delta axis
this.u = vec2.rotate(this.u_local, body1.a);
// su1, su2
this.su1 = vec2.cross(this.r1_d, this.u);
this.su2 = vec2.cross(this.r2, this.u);
// invEM = J * invM * JT
var springEm_inv = body1.m_inv + body2.m_inv + body1.i_inv * this.su1 * this.su1 + body2.i_inv * this.su2 * this.su2;
springEm = springEm_inv == 0 ? 0 : 1 / springEm_inv;
// Frequency
var omega = 2 * Math.PI * this.frequencyHz;
// Spring stiffness
var k = springEm * omega * omega;
// Damping coefficient
var c = springEm * 2 * this.dampingRatio * omega;
// Soft constraint formulas
// gamma and beta are divided by dt to reduce computation
this.gamma = (c + k * dt) * dt;
this.gamma = this.gamma == 0 ? 0 : 1 / this.gamma;
var beta = dt * k * this.gamma;
// Position constraint
var pc = vec2.dot(d, this.u) - this.restLength;
this.beta_c = beta * pc;
// invEM = invEM + gamma * I (to reduce calculation)
springEm_inv = springEm_inv + this.gamma;
this.springEm = springEm_inv == 0 ? 0 : 1 / springEm_inv;
}
else {
this.gamma = 0;
this.beta_c = 0;
this.springLambda_acc = 0;
}
if (this.motorEnabled) {
this.maxMotorImpulse = this.maxMotorTorque * dt;
// invEM2 = J2 * invM * J2T
var motorEm_inv = body1.i_inv + body2.i_inv;
this.motorEm = motorEm_inv > 0 ? 1 / motorEm_inv : motorEm_inv;
}
else {
this.motorEm = 0;
this.motorLambda_acc = 0;
}
if (warmStarting) {
// impulse = JT * lambda
var linearImpulse = vec2.scale(this.n, this.lambda_acc);
var angularImpulse1 = this.sn1 * this.lambda_acc + this.motorLambda_acc;
var angularImpulse2 = this.sn2 * this.lambda_acc + this.motorLambda_acc;
if (this.frequencyHz > 0) {
linearImpulse.addself(vec2.scale(this.u, this.springLambda_acc));
angularImpulse1 += this.su1 * this.springLambda_acc;
angularImpulse2 += this.su2 * this.springLambda_acc;
}
// Apply cached constraint impulses
// V += JT * lambda * invM
body1.v.mad(linearImpulse, -body1.m_inv);
body1.w -= angularImpulse1 * body1.i_inv;
body2.v.mad(linearImpulse, body2.m_inv);
body2.w += angularImpulse2 * body2.i_inv;
}
else {
this.lambda_acc = 0;
this.springLambda_acc = 0;
this.motorLambda_acc = 0;
}
}
WheelJoint.prototype.solveVelocityConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Solve spring constraint
if (this.frequencyHz > 0) {
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -(J * V + beta * C + gamma * (lambda_acc + lambda))
var cdot = this.u.dot(vec2.sub(body2.v, body1.v)) + this.su2 * body2.w - this.su1 * body1.w;
var soft = this.beta_c + this.gamma * this.springLambda_acc;
var lambda = -this.springEm * (cdot + soft);
// Accumulate lambda
this.springLambda_acc += lambda;
// linearImpulse = JT * lambda
var impulse = vec2.scale(this.u, lambda);
// Apply constraint impulses
// V += JT * lambda * invM
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.su1 * lambda * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.su2 * lambda * body2.i_inv;
}
// Solve motor constraint
if (this.motorEnabled) {
// Compute motor impulse
var cdot = body2.w - body1.w - this.motorSpeed;
var lambda = -this.motorEm * cdot;
var motorLambdaOld = this.motorLambda_acc;
this.motorLambda_acc = Math.clamp(this.motorLambda_acc + lambda, -this.maxMotorImpulse, this.maxMotorImpulse);
lambda = this.motorLambda_acc - motorLambdaOld;
// Apply motor impulses
body1.w -= lambda * body1.i_inv;
body2.w += lambda * body2.i_inv;
}
// Compute lambda for velocity constraint
// Solve J * invM * JT * lambda = -J * V
var cdot = this.n.dot(vec2.sub(body2.v, body1.v)) + this.sn2 * body2.w - this.sn1 * body1.w;
var lambda = -this.em * cdot;
// Accumulate lambda
this.lambda_acc += lambda;
// linearImpulse = JT * lambda
var impulse = vec2.scale(this.n, lambda);
// Apply constraint impulses
// V += JT * lambda * invM
body1.v.mad(impulse, -body1.m_inv);
body1.w -= this.sn1 * lambda * body1.i_inv;
body2.v.mad(impulse, body2.m_inv);
body2.w += this.sn2 * lambda * body2.i_inv;
}
WheelJoint.prototype.solvePositionConstraints = function() {
var body1 = this.body1;
var body2 = this.body2;
// Transformed r1, r2
var r1 = vec2.rotate(vec2.sub(this.anchor1, body1.centroid), body1.a);
var r2 = vec2.rotate(vec2.sub(this.anchor2, body2.centroid), body2.a);
// World anchor points
var p1 = vec2.add(body1.p, r1);
var p2 = vec2.add(body2.p, r2);
// Delta vector between world anchor points
var d = vec2.sub(p2, p1);
// r1 + d
var r1_d = vec2.add(r1, d);
// World line normal
var n = vec2.rotate(this.n_local, body1.a);
// Position constraint
var c = vec2.dot(n, d);
var correction = Math.clamp(c, -Joint.MAX_LINEAR_CORRECTION, Joint.MAX_LINEAR_CORRECTION);
// Compute lambda for position constraint
// Solve J * invM * JT * lambda = -C / dt
var s1 = vec2.cross(r1_d, n);
var s2 = vec2.cross(r2, n);
var em_inv = body1.m_inv + body2.m_inv + body1.i_inv * s1 * s1 + body2.i_inv * s2 * s2;
var k_inv = em_inv == 0 ? 0 : 1 / em_inv;
var lambda_dt = k_inv * (-correction);
// Apply constraint impulses
// impulse = JT * lambda
// X += impulse * invM * dt
var impulse_dt = vec2.scale(n, lambda_dt);
body1.p.mad(impulse_dt, -body1.m_inv);
body1.a -= s1 * lambda_dt * body1.i_inv;
body2.p.mad(impulse_dt, body2.m_inv);
body2.a += s2 * lambda_dt * body2.i_inv;
return Math.abs(c) < Joint.LINEAR_SLOP;
}
WheelJoint.prototype.getReactionForce = function(dt_inv) {
return vec2.scale(this.n, this.lambda_acc * dt_inv);
}
WheelJoint.prototype.getReactionTorque = function(dt_inv) {
return 0;
}
src/physics/advanced/shapes/Box.js
+19
View File
@@ -0,0 +1,19 @@
//--------------------------------
// Box
//--------------------------------
ShapeBox = function(local_x, local_y, w, h) {
local_x = local_x || 0;
local_y = local_y || 0;
var hw = w * 0.5;
var hh = h * 0.5;
var verts = [
new vec2(-hw + local_x, +hh + local_y),
new vec2(-hw + local_x, -hh + local_y),
new vec2(+hw + local_x, -hh + local_y),
new vec2(+hw + local_x, +hh + local_y)
];
return new ShapePoly(verts);
}
src/physics/advanced/shapes/Circle.js
+102
View File
@@ -0,0 +1,102 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//------------------------------------------
// ShapeCircle
//------------------------------------------
ShapeCircle = function(local_x, local_y, radius) {
Shape.call(this, Shape.TYPE_CIRCLE);
this.c = new vec2(local_x || 0, local_y || 0);
this.r = radius;
this.tc = vec2.zero;
this.finishVerts();
}
ShapeCircle.prototype = new Shape;
ShapeCircle.prototype.constructor = ShapeCircle;
ShapeCircle.prototype.finishVerts = function() {
this.r = Math.abs(this.r);
}
ShapeCircle.prototype.duplicate = function() {
return new ShapeCircle(this.c.x, this.c.y, this.r);
}
ShapeCircle.prototype.serialize = function() {
return {
"type": "ShapeCircle",
"e": this.e,
"u": this.u,
"density": this.density,
"center": this.c,
"radius": this.r
};
}
ShapeCircle.prototype.recenter = function(c) {
this.c.subself(c);
}
ShapeCircle.prototype.transform = function(xf) {
this.c = xf.transform(this.c);
}
ShapeCircle.prototype.untransform = function(xf) {
this.c = xf.untransform(this.c);
}
ShapeCircle.prototype.area = function() {
return areaForCircle(this.r, 0);
}
ShapeCircle.prototype.centroid = function() {
return this.c.duplicate();
}
ShapeCircle.prototype.inertia = function(mass) {
return inertiaForCircle(mass, this.c, this.r, 0);
}
ShapeCircle.prototype.cacheData = function(xf) {
this.tc = xf.transform(this.c);
this.bounds.mins.set(this.tc.x - this.r, this.tc.y - this.r);
this.bounds.maxs.set(this.tc.x + this.r, this.tc.y + this.r);
}
ShapeCircle.prototype.pointQuery = function(p) {
return vec2.distsq(this.tc, p) < (this.r * this.r);
}
ShapeCircle.prototype.findVertexByPoint = function(p, minDist) {
var dsq = minDist * minDist;
if (vec2.distsq(this.tc, p) < dsq) {
return 0;
}
return -1;
}
ShapeCircle.prototype.distanceOnPlane = function(n, d) {
return vec2.dot(n, this.tc) - this.r - d;
}
src/physics/advanced/shapes/Poly.js
+254
View File
@@ -0,0 +1,254 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//--------------------------------
// ShapePoly (convex only)
//--------------------------------
ShapePoly = function(verts) {
Shape.call(this, Shape.TYPE_POLY);
this.verts = [];
this.planes = [];
this.tverts = [];
this.tplanes = [];
if (verts) {
for (var i = 0; i < verts.length; i++) {
this.verts[i] = verts[i].duplicate();
this.tverts[i] = this.verts[i];
this.tplanes[i] = {};
this.tplanes[i].n = vec2.zero;
this.tplanes[i].d = 0;
}
}
this.finishVerts();
}
ShapePoly.prototype = new Shape;
ShapePoly.prototype.constructor = ShapePoly;
ShapePoly.prototype.finishVerts = function() {
if (this.verts.length < 2) {
this.convexity = false;
this.planes = [];
return;
}
this.convexity = true;
this.tverts = [];
this.tplanes = [];
// Must be counter-clockwise verts
for (var i = 0; i < this.verts.length; i++) {
var a = this.verts[i];
var b = this.verts[(i + 1) % this.verts.length];
var n = vec2.normalize(vec2.perp(vec2.sub(a, b)));
this.planes[i] = {};
this.planes[i].n = n;
this.planes[i].d = vec2.dot(n, a);
this.tverts[i] = this.verts[i];
this.tplanes[i] = {};
this.tplanes[i].n = vec2.zero;
this.tplanes[i].d = 0;
}
for (var i = 0; i < this.verts.length; i++) {
var b = this.verts[(i + 2) % this.verts.length];
var n = this.planes[i].n;
var d = this.planes[i].d;
if (vec2.dot(n, b) - d > 0) {
this.convexity = false;
}
}
}
ShapePoly.prototype.duplicate = function() {
return new ShapePoly(this.verts);
}
ShapePoly.prototype.serialize = function() {
return {
"type": "ShapePoly",
"e": this.e,
"u": this.u,
"density": this.density,
"verts": this.verts
};
}
ShapePoly.prototype.recenter = function(c) {
for (var i = 0; i < this.verts.length; i++) {
this.verts[i].subself(c);
}
}
ShapePoly.prototype.transform = function(xf) {
for (var i = 0; i < this.verts.length; i++) {
this.verts[i] = xf.transform(this.verts[i]);
}
}
ShapePoly.prototype.untransform = function(xf) {
for (var i = 0; i < this.verts.length; i++) {
this.verts[i] = xf.untransform(this.verts[i]);
}
}
ShapePoly.prototype.area = function() {
return areaForPoly(this.verts);
}
ShapePoly.prototype.centroid = function() {
return centroidForPoly(this.verts);
}
ShapePoly.prototype.inertia = function(mass) {
return inertiaForPoly(mass, this.verts, vec2.zero);
}
ShapePoly.prototype.cacheData = function(xf) {
this.bounds.clear();
var numVerts = this.verts.length;
if (numVerts == 0) {
return;
}
for (var i = 0; i < numVerts; i++) {
this.tverts[i] = xf.transform(this.verts[i]);
}
if (numVerts < 2) {
this.bounds.addPoint(this.tverts[0]);
return;
}
for (var i = 0; i < numVerts; i++) {
var a = this.tverts[i];
var b = this.tverts[(i + 1) % numVerts];
var n = vec2.normalize(vec2.perp(vec2.sub(a, b)));
this.tplanes[i].n = n;
this.tplanes[i].d = vec2.dot(n, a);
this.bounds.addPoint(a);
}
}
ShapePoly.prototype.pointQuery = function(p) {
if (!this.bounds.containPoint(p)) {
return false;
}
return this.containPoint(p);
}
ShapePoly.prototype.findVertexByPoint = function(p, minDist) {
var dsq = minDist * minDist;
for (var i = 0; i < this.tverts.length; i++) {
if (vec2.distsq(this.tverts[i], p) < dsq) {
return i;
}
}
return -1;
}
ShapePoly.prototype.findEdgeByPoint = function(p, minDist) {
var dsq = minDist * minDist;
var numVerts = this.tverts.length;
for (var i = 0; i < this.tverts.length; i++) {
var v1 = this.tverts[i];
var v2 = this.tverts[(i + 1) % numVerts];
var n = this.tplanes[i].n;
var dtv1 = vec2.cross(v1, n);
var dtv2 = vec2.cross(v2, n);
var dt = vec2.cross(p, n);
if (dt > dtv1) {
if (vec2.distsq(v1, p) < dsq) {
return i;
}
}
else if (dt < dtv2) {
if (vec2.distsq(v2, p) < dsq) {
return i;
}
}
else {
var dist = vec2.dot(n, p) - vec2.dot(n, v1);
if (dist * dist < dsq) {
return i;
}
}
}
return -1;
}
ShapePoly.prototype.distanceOnPlane = function(n, d) {
var min = 999999;
for (var i = 0; i < this.verts.length; i++) {
min = Math.min(min, vec2.dot(n, this.tverts[i]));
}
return min - d;
}
ShapePoly.prototype.containPoint = function(p) {
for (var i = 0; i < this.verts.length; i++) {
var plane = this.tplanes[i];
if (vec2.dot(plane.n, p) - plane.d > 0) {
return false;
}
}
return true;
}
ShapePoly.prototype.containPointPartial = function(p, n) {
for (var i = 0; i < this.verts.length; i++) {
var plane = this.tplanes[i];
if (vec2.dot(plane.n, n) < 0.0001) {
continue;
}
if (vec2.dot(plane.n, p) - plane.d > 0) {
return false;
}
}
return true;
}
src/physics/advanced/shapes/Segment.js
+170
View File
@@ -0,0 +1,170 @@
/*
* Copyright (c) 2012 Ju Hyung Lee
*
* 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.
*/
//------------------------------------------
// ShapeSegment (thick rounded line segment)
//------------------------------------------
ShapeSegment = function(a, b, radius) {
Shape.call(this, Shape.TYPE_SEGMENT);
this.a = a.duplicate();
this.b = b.duplicate();
this.r = radius;
this.n = vec2.perp(vec2.sub(b, a));
this.n.normalize();
this.ta = vec2.zero;
this.tb = vec2.zero;
this.tn = vec2.zero;
this.finishVerts();
}
ShapeSegment.prototype = new Shape;
ShapeSegment.prototype.constructor = ShapeSegment;
ShapeSegment.prototype.finishVerts = function() {
this.n = vec2.perp(vec2.sub(this.b, this.a));
this.n.normalize();
this.r = Math.abs(this.r);
}
ShapeSegment.prototype.duplicate = function() {
return new ShapeSegment(this.a, this.b, this.r);
}
ShapeSegment.prototype.serialize = function() {
return {
"type": "ShapeSegment",
"e": this.e,
"u": this.u,
"density": this.density,
"a": this.a,
"b": this.b,
"radius": this.r
};
}
ShapeSegment.prototype.recenter = function(c) {
this.a.subself(c);
this.b.subself(c);
}
ShapeSegment.prototype.transform = function(xf) {
this.a = xf.transform(this.a);
this.b = xf.transform(this.b);
}
ShapeSegment.prototype.untransform = function(xf) {
this.a = xf.untransform(this.a);
this.b = xf.untransform(this.b);
}
ShapeSegment.prototype.area = function() {
return areaForSegment(this.a, this.b, this.r);
}
ShapeSegment.prototype.centroid = function() {
return centroidForSegment(this.a, this.b);
}
ShapeSegment.prototype.inertia = function(mass) {
return inertiaForSegment(mass, this.a, this.b);
}
ShapeSegment.prototype.cacheData = function(xf) {
this.ta = xf.transform(this.a);
this.tb = xf.transform(this.b);
this.tn = vec2.perp(vec2.sub(this.tb, this.ta)).normalize();
if (this.ta.x < this.tb.x) {
l = this.ta.x;
r = this.tb.x;
}
else {
l = this.tb.x;
r = this.ta.x;
}
if (this.ta.y < this.tb.y) {
b = this.ta.y;
t = this.tb.y;
} else {
b = this.tb.y;
t = this.ta.y;
}
this.bounds.mins.set(l - this.r, b - this.r);
this.bounds.maxs.set(r + this.r, t + this.r);
}
ShapeSegment.prototype.pointQuery = function(p) {
if (!this.bounds.containPoint(p)) {
return false;
}
var dn = vec2.dot(this.tn, p) - vec2.dot(this.ta, this.tn);
var dist = Math.abs(dn);
if (dist > this.r) {
return false;
}
var dt = vec2.cross(p, this.tn);
var dta = vec2.cross(this.ta, this.tn);
var dtb = vec2.cross(this.tb, this.tn);
if (dt <= dta) {
if (dt < dta - this.r) {
return false;
}
return vec2.distsq(this.ta, p) < (this.r * this.r);
}
else if (dt > dtb) {
if (dt > dtb + this.r) {
return false;
}
return vec2.distsq(this.tb, p) < (this.r * this.r);
}
return true;
}
ShapeSegment.prototype.findVertexByPoint = function(p, minDist) {
var dsq = minDist * minDist;
if (vec2.distsq(this.ta, p) < dsq) {
return 0;
}
if (vec2.distsq(this.tb, p) < dsq) {
return 1;
}
return -1;
}
ShapeSegment.prototype.distanceOnPlane = function(n, d) {
var a = vec2.dot(n, this.ta) - this.r;
var b = vec2.dot(n, this.tb) - this.r;
return Math.min(a, b) - d;
}
src/physics/advanced/shapes/Triangle.js
+12
View File
@@ -0,0 +1,12 @@
//--------------------------------
// Triangle
//--------------------------------
ShapeTriangle = function(p1, p2, p3) {
var verts = [
new vec2(p1.x, p1.y),
new vec2(p2.x, p2.y),
new vec2(p3.x, p3.y)
];
return new ShapePoly(verts);
}
src/physics/arcade/ArcadePhysics.js
+107
View File
@@ -0,0 +1,107 @@
Phaser.Physics = {};
Phaser.Physics.Arcade = function (game) {
this.game = game;
this._length = 0;
/**
* @type {number}
*/
this.worldDivisions = 6;
// this.angularDrag = 0;
// this.gravity = new Phaser.Point;
// this.drag = new Phaser.Point;
// this.bounce = new Phaser.Point;
// this.bounds = new Phaser.Rectangle(0, 0, game.world.width, game.world.height);
// this._distance = new Phaser.Point;
// this._tangent = new Phaser.Point;
};
Phaser.Physics.Arcade.prototype = {
updateMotion: function (body) {
this._velocityDelta = (this.computeVelocity(body.angularVelocity, body.gravity.x, body.angularAcceleration, body.angularDrag, body.maxAngular) - body.angularVelocity) / 2;
body.angularVelocity += this._velocityDelta;
body.sprite.rotation += body.angularVelocity * this.game.time.elapsed;
body.angularVelocity += this._velocityDelta;
this._velocityDelta = (this.computeVelocity(body.velocity.x, body.gravity.x, body.acceleration.x, body.drag.x) - body.velocity.x) / 2;
body.velocity.x += this._velocityDelta;
this._delta = body.velocity.x * this.game.time.elapsed;
body.velocity.x += this._velocityDelta;
body.sprite.x += this._delta;
this._velocityDelta = (this.computeVelocity(body.velocity.y, body.gravity.y, body.acceleration.y, body.drag.y) - body.velocity.y) / 2;
body.velocity.y += this._velocityDelta;
this._delta = body.velocity.y * this.game.time.elapsed;
body.velocity.y += this._velocityDelta;
body.sprite.y += this._delta;
},
/**
* A tween-like function that takes a starting velocity and some other factors and returns an altered velocity.
*
* @param {number} Velocity Any component of velocity (e.g. 20).
* @param {number} Acceleration Rate at which the velocity is changing.
* @param {number} Drag Really kind of a deceleration, this is how much the velocity changes if Acceleration is not set.
* @param {number} Max An absolute value cap for the velocity.
*
* @return {number} The altered Velocity value.
*/
computeVelocity: function (velocity, gravity, acceleration, drag, max) {
gravity = gravity || 0;
acceleration = acceleration || 0;
drag = drag || 0;
max = max || 10000;
if (acceleration !== 0)
{
velocity += (acceleration + gravity) * this.game.time.elapsed;
}
else if (drag !== 0)
{
this._drag = drag * this.game.time.elapsed;
if (velocity - this._drag > 0)
{
velocity = velocity - this._drag;
}
else if (velocity + this._drag < 0)
{
velocity += this._drag;
}
else
{
velocity = 0;
}
velocity += gravity;
}
if ((velocity != 0) && (max != 10000))
{
if (velocity > max)
{
velocity = max;
}
else if (velocity < -max)
{
velocity = -max;
}
}
return velocity;
},
};
Phaser.Physics.Arcade.prototype.OVERLAP_BIAS = 4;
Phaser.Physics.Arcade.prototype.TILE_OVERLAP = false;
src/physics/arcade/Body.js
+29
View File
@@ -0,0 +1,29 @@
Phaser.Physics.Arcade.Body = function (sprite) {
this.sprite = sprite;
this.game = sprite.game;
this.bounds = new Phaser.Rectangle(sprite.x, sprite.y, sprite.currentFrame.sourceSizeW, sprite.currentFrame.sourceSizeH);
this.offset = new Phaser.Point;
this._w = sprite.width;
this._h = sprite.height;
};
Phaser.Physics.Arcade.Body.prototype = {
sprite: null,
game: null,
bounds: null,
update: function () {
this.bounds.x = this.sprite.x - (this.sprite.anchor.x * (this.offset.x * this.sprite.scale.x));
this.bounds.y = this.sprite.y - (this.sprite.anchor.y * (this.offset.y * this.sprite.scale.y));
this.bounds.width = this._w * this.sprite.scale.x;
this.bounds.height = this._h * this.sprite.scale.y;
},
};
src/utils/Debug.js
+1
View File
@@ -139,6 +139,7 @@ Phaser.Utils.Debug.prototype = {
this.context.strokeStyle = 'rgba(0,0,255,0.8)';
this.context.stroke();
this.renderPoint(sprite.center);
this.renderPoint(sprite.topLeft);
this.renderPoint(sprite.topRight);
this.renderPoint(sprite.bottomLeft);