From bdc1c2ceb9e9825284d3d49caa4755ae9b81cb67 Mon Sep 17 00:00:00 2001 From: Richard Davey Date: Tue, 3 Sep 2013 15:35:40 +0100 Subject: [PATCH] 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. --- Phaser.sublime-workspace | 1354 +++++++++++++++++++++- examples/body1.php | 77 ++ examples/js.php | 38 +- examples/sprite4.php | 70 ++ src/animation/Animation.js | 5 +- src/animation/AnimationManager.js | 9 +- src/animation/Frame.js | 21 +- src/gameobjects/Sprite.js | 120 +- src/geom/Rectangle.js | 54 +- src/physics/advanced/Body.js | 402 +++++++ src/physics/advanced/Collision.js | 382 ++++++ src/physics/advanced/Contact.js | 37 + src/physics/advanced/ContactSolver.js | 267 +++++ src/physics/advanced/Joint.js | 76 ++ src/physics/advanced/Math.js | 817 +++++++++++++ src/physics/advanced/Shape.js | 47 + src/physics/advanced/Space.js | 797 +++++++++++++ src/physics/advanced/Util.js | 147 +++ src/physics/advanced/joints/Angle.js | 130 +++ src/physics/advanced/joints/Distance.js | 522 +++++++++ src/physics/advanced/joints/Mouse.js | 150 +++ src/physics/advanced/joints/Prismatic.js | 241 ++++ src/physics/advanced/joints/Revolute.js | 378 ++++++ src/physics/advanced/joints/Rope.js | 211 ++++ src/physics/advanced/joints/Weld.js | 306 +++++ src/physics/advanced/joints/Wheel.js | 376 ++++++ src/physics/advanced/shapes/Box.js | 19 + src/physics/advanced/shapes/Circle.js | 102 ++ src/physics/advanced/shapes/Poly.js | 254 ++++ src/physics/advanced/shapes/Segment.js | 170 +++ src/physics/advanced/shapes/Triangle.js | 12 + src/physics/arcade/ArcadePhysics.js | 107 ++ src/physics/arcade/Body.js | 29 + src/utils/Debug.js | 1 + 34 files changed, 7600 insertions(+), 128 deletions(-) create mode 100644 examples/body1.php create mode 100644 examples/sprite4.php create mode 100644 src/physics/advanced/Body.js create mode 100644 src/physics/advanced/Collision.js create mode 100644 src/physics/advanced/Contact.js create mode 100644 src/physics/advanced/ContactSolver.js create mode 100644 src/physics/advanced/Joint.js create mode 100644 src/physics/advanced/Math.js create mode 100644 src/physics/advanced/Shape.js create mode 100644 src/physics/advanced/Space.js create mode 100644 src/physics/advanced/Util.js create mode 100644 src/physics/advanced/joints/Angle.js create mode 100644 src/physics/advanced/joints/Distance.js create mode 100644 src/physics/advanced/joints/Mouse.js create mode 100644 src/physics/advanced/joints/Prismatic.js create mode 100644 src/physics/advanced/joints/Revolute.js create mode 100644 src/physics/advanced/joints/Rope.js create mode 100644 src/physics/advanced/joints/Weld.js create mode 100644 src/physics/advanced/joints/Wheel.js create mode 100644 src/physics/advanced/shapes/Box.js create mode 100644 src/physics/advanced/shapes/Circle.js create mode 100644 src/physics/advanced/shapes/Poly.js create mode 100644 src/physics/advanced/shapes/Segment.js create mode 100644 src/physics/advanced/shapes/Triangle.js create mode 100644 src/physics/arcade/ArcadePhysics.js create mode 100644 src/physics/arcade/Body.js diff --git a/Phaser.sublime-workspace b/Phaser.sublime-workspace index 5c8d6953..5e45011f 100644 --- a/Phaser.sublime-workspace +++ b/Phaser.sublime-workspace @@ -3,6 +3,14 @@ { "selected_items": [ + [ + "til", + "tilingSprite" + ], + [ + "for", + "forceRestart" + ], [ "bot", "bottomLeft" @@ -135,10 +143,6 @@ "child", "children" ], - [ - "for", - "for for (…) {…}" - ], [ "fra", "frameHeight" @@ -510,15 +514,84 @@ [ "round", "roundComplete" - ], - [ - "scr", - "scrollUp" ] ] }, "buffers": [ + { + "file": "/D/wamp/www/pixi.js/examples/example 10 - Text/index.html", + "settings": + { + "buffer_size": 3882, + "line_ending": "Windows" + } + }, + { + "file": "src/gameobjects/TileSprite.js", + "settings": + { + "buffer_size": 882, + "line_ending": "Windows" + } + }, + { + "file": "src/pixi/text/Text.js", + "settings": + { + "buffer_size": 7827, + "line_ending": "Windows" + } + }, + { + "file": "src/gameobjects/Text.js", + "settings": + { + "buffer_size": 689, + "line_ending": "Windows", + "name": "Phaser.Text = function (game) {" + } + }, + { + "file": "examples/text3.php", + "settings": + { + "buffer_size": 1354, + "line_ending": "Windows" + } + }, + { + "file": "src/gameobjects/GameObjectFactory.js", + "settings": + { + "buffer_size": 1813, + "line_ending": "Windows" + } + }, + { + "file": "src/gameobjects/Sprite.js", + "settings": + { + "buffer_size": 11637, + "line_ending": "Windows" + } + }, + { + "file": "src/pixi/textures/Texture.js", + "settings": + { + "buffer_size": 5130, + "line_ending": "Windows" + } + }, + { + "file": "src/utils/Debug.js", + "settings": + { + "buffer_size": 15358, + "line_ending": "Windows" + } + } ], "build_system": "", "command_palette": @@ -548,25 +621,46 @@ }, "file_history": [ + "/D/wamp/www/phaser/examples/get_bounds.php", + "/D/wamp/www/phaser/src/pixi/renderers/canvas/CanvasRenderer.js", + "/D/wamp/www/phaser/src/pixi/display/Sprite.js", + "/D/wamp/www/phaser/examples/js.php", + "/D/wamp/www/phaser/examples/tilesprite1.php", + "/D/wamp/www/phaser/src/utils/Utils.js", + "/D/wamp/www/phaser/src/Phaser.js", + "/D/wamp/www/phaser/examples/inherit.php", + "/D/wamp/www/bbc-wolfblood/wolfblood.js", + "/D/wamp/www/phaser/src/gameobjects/Sprite.js", + "/D/wamp/www/phaser/src/gameobjects/TileSprite.js", + "/D/wamp/www/phaser/examples/tilesprite2.php", + "/D/wamp/www/phaser/src/pixi/extras/TilingSprite.js", + "/D/wamp/www/phaser/src/animation/AnimationManager.js", + "/D/wamp/www/phaser/src/gameobjects/GameObjectFactory.js", + "/D/wamp/www/pixi.js/examples/example 9 - Tiling Texture/index.html", + "/D/wamp/www/phaser/src/loader/Loader.js", + "/D/wamp/www/phaser/TS Tests/audio/play sound 1.ts", + "/D/wamp/www/phaser/src/sound/SoundManager.js", + "/D/wamp/www/phaser/examples/sound1.php", + "/D/wamp/www/phaser/src/utils/Debug.js", + "/D/wamp/www/phaser/TS Source/gameobjects/GameObjectFactory.ts", + "/D/wamp/www/phaser/TS Source/sound/Sound.js", + "/D/wamp/www/phaser/src/sound/Sound.js", + "/D/wamp/www/phaser/TS Source/sound/Sound.ts", + "/D/wamp/www/phaser/src/core/Game.js", + "/D/wamp/www/phaser/TS Source/sound/SoundManager.js", "/D/wamp/www/phaser/src/math/QuadTree.js", "/D/wamp/www/phaser/TS Source/math/QuadTree.js", - "/D/wamp/www/phaser/src/animation/AnimationManager.js", - "/D/wamp/www/phaser/src/gameobjects/Sprite.js", - "/D/wamp/www/phaser/src/utils/Debug.js", "/D/wamp/www/phaser/src/core/World.js", "/D/wamp/www/phaser/src/pixi/display/Stage.js", "/D/wamp/www/phaser/examples/camera_cull1.php", - "/D/wamp/www/phaser/src/pixi/renderers/canvas/CanvasRenderer.js", "/D/wamp/www/phaser/src/pixi/display/DisplayObjectContainer.js", "/D/wamp/www/phaser/src/pixi/display/DisplayObject.js", - "/D/wamp/www/phaser/src/pixi/display/Sprite.js", "/D/wamp/www/bbc-wolfblood/assets/1024/help/help_titles.json", "/D/wamp/www/phaser/src/math/LinkedList.js", "/D/wamp/www/phaser/TS Source/math/LinkedList.js", "/D/wamp/www/phaser/src/animation/Frame.js", "/D/wamp/www/phaser/src/animation/Parser.js", "/D/wamp/www/phaser/examples/camera4.php", - "/D/wamp/www/phaser/src/core/Game.js", "/D/wamp/www/phaser/examples/sprite3.php", "/D/wamp/www/phaser/examples/camera5.php", "/D/wamp/www/phaser/src/pixi/InteractionManager.js", @@ -581,7 +675,6 @@ "/D/wamp/www/phaser/src/input/MSPointer.js", "/D/wamp/www/phaser/src/core/StateManager.js", "/D/wamp/www/phaser/TS Source/utils/DebugUtils.js", - "/D/wamp/www/phaser/examples/js.php", "/D/wamp/www/phaser/TS Source/renderers/canvas/SpriteRenderer.ts", "/D/wamp/www/phaser/examples/input1.php", "/D/wamp/www/phaser/src/input/Pointer.js", @@ -590,7 +683,6 @@ "/D/wamp/www/phaser/src/input/Keyboard.js", "/D/wamp/www/phaser/TS Source/gameobjects/Sprite.ts", "/D/wamp/www/phaser/src/core/Stage.js", - "/D/wamp/www/phaser/src/loader/Loader.js", "/D/wamp/www/phaser/TS Source/input/Pointer.js", "/D/wamp/www/phaser/TS Source/input/Touch.js", "/D/wamp/www/phaser/TS Source/input/MSPointer.js", @@ -612,12 +704,10 @@ "/D/wamp/www/phaser/src/core/State.js", "/D/wamp/www/bbc-wolfblood/game.js", "/D/wamp/www/phaser/TS Source/gameobjects/GameObjectFactory.js", - "/D/wamp/www/phaser/src/gameobjects/GameObjectFactory.js", "/D/wamp/www/phaser/src/tween/Tween.js", "/D/wamp/www/phaser/TS Source/tweens/Tween.ts", "/D/wamp/www/phaser/TS Source/tweens/TweenManager.ts", "/D/wamp/www/phaser/src/tween/TweenManager.js", - "/D/wamp/www/phaser/TS Source/gameobjects/GameObjectFactory.ts", "/C/Users/rich/Desktop/game.js", "/D/wamp/www/phaser/src/animation/Animation.js", "/D/wamp/www/phaser/TS Source/animation/Animation.js", @@ -643,7 +733,6 @@ "/D/wamp/www/phaser/src/Stage.js", "/D/wamp/www/phaser/examples/pixi 1.html", "/D/wamp/www/phaser/src/system/Canvas.js", - "/D/wamp/www/phaser/src/Phaser.js", "/D/wamp/www/phaser/TS Source/Stage.js", "/D/wamp/www/phaser/examples/rect1.php", "/D/wamp/www/phaser/src/geom/Rectangle.js", @@ -659,23 +748,7 @@ "/D/wamp/www/phaser/TS Tests/tilemaps/csv tilemap.js", "/D/wamp/www/phaser/TS Source/system/RequestAnimationFrame.js", "/D/wamp/www/phaser/TS Source/Game.js", - "/D/wamp/www/phaser/TS Source/State.js", - "/D/wamp/www/phaser/TS Tests/audio/audio sprites 1.ts", - "/D/wamp/www/phaser/examples/loader 2.html", - "/D/wamp/www/phaser/src/core/Plugin.js", - "/D/wamp/www/phaser/src/core/PluginManager.js", - "/D/wamp/www/index.php", - "/D/wamp/www/phaser/TS Source/core/Plugin.js", - "/D/wamp/www/phaser/TS Source/core/PluginManager.js", - "/D/wamp/www/phaser/TS Source/Game.ts", - "/D/wamp/www/pixi.js/examples/example 1 - Basics/index.html", - "/D/wamp/www/phaser/examples/loader.html", - "/D/wamp/www/phaser/examples/math sincos.html", - "/D/wamp/www/phaser/examples/raf.html", - "/D/wamp/www/phaser/src/time/Time.js", - "/D/wamp/www/phaser/src/pixi/utils/Utils.js", - "/D/wamp/www/phaser/TS Source/system/RequestAnimationFrame.ts", - "/D/wamp/www/phaser/todo/Tween.js" + "/D/wamp/www/phaser/TS Source/State.js" ], "find": { @@ -683,7 +756,7 @@ }, "find_in_files": { - "height": 90.0, + "height": 0.0, "where_history": [ "D:\\wamp\\www\\phaser\\src\\pixi", @@ -711,6 +784,21 @@ "case_sensitive": false, "find_history": [ + "text", + "TileSprite", + "4", + "256", + "Phaser.Sprite", + "\n});\n", + "Phaser.Sprite", + "tilingSprite", + "()", + "this.", + "Sound.prototype.", + "Sound.", + "sound", + "SoundManager.prototype.", + "this.", "game.camera", "\n ", "moveTo", @@ -823,22 +911,7 @@ "addState", "this._pendingState", "game", - "this", - "this.current", - "this.state", - "Phaser.GAMES[this.id].boot(parent, width, height)", - "this.boot", - "game", - "State.prototype.", - "preload", - "Callback", - "Plugin.prototype.", - "this._parent", - "PluginManager.prototype.", - "public ", - "startSt", - "_this", - "Phaser.Math" + "this" ], "highlight": true, "in_selection": false, @@ -910,8 +983,1183 @@ "groups": [ { + "selected": 4, "sheets": [ + { + "buffer": 0, + "file": "/D/wamp/www/pixi.js/examples/example 10 - Text/index.html", + "settings": + { + "buffer_size": 3882, + "regions": + { + }, + "selection": + [ + [ + 2445, + 2569 + ] + ], + "settings": + { + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/HTML/HTML.tmLanguage", + "translate_tabs_to_spaces": false + }, + "translation.x": 0.0, + "translation.y": 1367.0, + "zoom_level": 1.0 + }, + "type": "text" + }, + { + "buffer": 1, + "file": "src/gameobjects/TileSprite.js", + "settings": + { + "buffer_size": 882, + "regions": + { + }, + "selection": + [ + [ + 850, + 850 + ] + ], + "settings": + { + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/JavaScript/JavaScript.tmLanguage", + "tab_size": 4, + "translate_tabs_to_spaces": true + }, + "translation.x": 0.0, + "translation.y": 0.0, + "zoom_level": 1.0 + }, + "type": "text" + }, + { + "buffer": 2, + "file": "src/pixi/text/Text.js", + "settings": + { + "buffer_size": 7827, + "regions": + { + }, + "selection": + [ + [ + 2621, + 2621 + ] + ], + "settings": + { + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/JavaScript/JavaScript.tmLanguage", + "translate_tabs_to_spaces": false + }, + "translation.x": 0.0, + "translation.y": 584.0, + "zoom_level": 1.0 + }, + "type": "text" + }, + { + "buffer": 3, + "file": "src/gameobjects/Text.js", + "settings": + { + "buffer_size": 689, + "regions": + { + }, + "selection": + [ + [ + 402, + 402 + ] + ], + "settings": + { + "auto_name": "Phaser.Text = function (game) {", + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/JavaScript/JavaScript.tmLanguage" + }, + "translation.x": 0.0, + "translation.y": 0.0, + "zoom_level": 1.0 + }, + "type": "text" + }, + { + "buffer": 4, + "file": "examples/text3.php", + "settings": + { + "buffer_size": 1354, + "regions": + { + }, + "selection": + [ + [ + 281, + 281 + ] + ], + "settings": + { + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/PHP/PHP.tmLanguage", + "translate_tabs_to_spaces": false + }, + "translation.x": 0.0, + "translation.y": 0.0, + "zoom_level": 1.0 + }, + "type": "text" + }, + { + "buffer": 5, + "file": "src/gameobjects/GameObjectFactory.js", + "settings": + { + "buffer_size": 1813, + "regions": + { + }, + "selection": + [ + [ + 1722, + 1722 + ] + ], + "settings": + { + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/JavaScript/JavaScript.tmLanguage", + "tab_size": 4, + "translate_tabs_to_spaces": true + }, + "translation.x": 0.0, + "translation.y": 195.0, + "zoom_level": 1.0 + }, + "type": "text" + }, + { + "buffer": 6, + "file": "src/gameobjects/Sprite.js", + "settings": + { + "buffer_size": 11637, + "regions": + { + }, + "selection": + [ + [ + 344, + 344 + ] + ], + "settings": + { + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/JavaScript/JavaScript.tmLanguage", + "tab_size": 4, + "translate_tabs_to_spaces": true + }, + "translation.x": 0.0, + "translation.y": 0.0, + "zoom_level": 1.0 + }, + "type": "text" + }, + { + "buffer": 7, + "file": "src/pixi/textures/Texture.js", + "settings": + { + "buffer_size": 5130, + "regions": + { + }, + "selection": + [ + [ + 4317, + 4405 + ] + ], + "settings": + { + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/JavaScript/JavaScript.tmLanguage", + "translate_tabs_to_spaces": false + }, + "translation.x": 0.0, + "translation.y": 2635.0, + "zoom_level": 1.0 + }, + "type": "text" + }, + { + "buffer": 8, + "file": "src/utils/Debug.js", + "settings": + { + "buffer_size": 15358, + "regions": + { + }, + "selection": + [ + [ + 4016, + 4016 + ] + ], + "settings": + { + "codeintel": true, + "codeintel_config": + { + "JavaScript": + { + "codeintel_max_recursive_dir_depth": 2, + "codeintel_scan_files_in_project": false, + "javascriptExtraPaths": + [ + ] + }, + "PHP": + { + "codeintel_max_recursive_dir_depth": 5, + "codeintel_scan_files_in_project": false, + "phpExtraPaths": + [ + ] + }, + "Python": + { + "env": + { + } + } + }, + "codeintel_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_live": true, + "codeintel_live_enabled_languages": + [ + "JavaScript", + "Mason", + "XBL", + "XUL", + "RHTML", + "SCSS", + "Python", + "HTML", + "Ruby", + "Python3", + "XML", + "Sass", + "XSLT", + "Django", + "HTML5", + "Perl", + "CSS", + "Twig", + "Less", + "Smarty", + "Node.js", + "Tcl", + "TemplateToolkit", + "PHP" + ], + "codeintel_max_recursive_dir_depth": 10, + "codeintel_scan_exclude_dir": + { + "JavaScript": + [ + "/build/", + "/min/" + ] + }, + "codeintel_scan_files_in_project": true, + "codeintel_selected_catalogs": + [ + "PyWin32", + "jQuery", + "Rails" + ], + "codeintel_snippets": true, + "codeintel_syntax_map": + { + "Python Django": "Python" + }, + "sublime_auto_complete": true, + "syntax": "Packages/JavaScript/JavaScript.tmLanguage", + "tab_size": 4, + "translate_tabs_to_spaces": true + }, + "translation.x": 0.0, + "translation.y": 1939.0, + "zoom_level": 1.0 + }, + "type": "text" + } ] } ], diff --git a/examples/body1.php b/examples/body1.php new file mode 100644 index 00000000..ff276dc3 --- /dev/null +++ b/examples/body1.php @@ -0,0 +1,77 @@ + + + + phaser.js - a new beginning + + + + + + + + \ No newline at end of file diff --git a/examples/js.php b/examples/js.php index 5804575e..7181313f 100644 --- a/examples/js.php +++ b/examples/js.php @@ -38,7 +38,6 @@ - @@ -53,51 +52,64 @@ - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/sprite4.php b/examples/sprite4.php new file mode 100644 index 00000000..295eed61 --- /dev/null +++ b/examples/sprite4.php @@ -0,0 +1,70 @@ + + + + phaser.js - a new beginning + + + + + + + + \ No newline at end of file diff --git a/src/animation/Animation.js b/src/animation/Animation.js index 3be0181a..29340a78 100644 --- a/src/animation/Animation.js +++ b/src/animation/Animation.js @@ -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 diff --git a/src/animation/AnimationManager.js b/src/animation/AnimationManager.js index 5ff1f238..ab0a163b 100644 --- a/src/animation/AnimationManager.js +++ b/src/animation/AnimationManager.js @@ -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 diff --git a/src/animation/Frame.js b/src/animation/Frame.js index 30ec2f7a..40e4df93 100644 --- a/src/animation/Frame.js +++ b/src/animation/Frame.js @@ -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; diff --git a/src/gameobjects/Sprite.js b/src/gameobjects/Sprite.js index 8591500c..f77fd1bc 100644 --- a/src/gameobjects/Sprite.js +++ b/src/gameobjects/Sprite.js @@ -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); diff --git a/src/geom/Rectangle.js b/src/geom/Rectangle.js index a6b0895c..7d0238ed 100644 --- a/src/geom/Rectangle.js +++ b/src/geom/Rectangle.js @@ -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", { /** diff --git a/src/physics/advanced/Body.js b/src/physics/advanced/Body.js new file mode 100644 index 00000000..03dbcba0 --- /dev/null +++ b/src/physics/advanced/Body.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/Collision.js b/src/physics/advanced/Collision.js new file mode 100644 index 00000000..848a07aa --- /dev/null +++ b/src/physics/advanced/Collision.js @@ -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); + }; +})(); diff --git a/src/physics/advanced/Contact.js b/src/physics/advanced/Contact.js new file mode 100644 index 00000000..f4ab7423 --- /dev/null +++ b/src/physics/advanced/Contact.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/ContactSolver.js b/src/physics/advanced/ContactSolver.js new file mode 100644 index 00000000..b5f2b31b --- /dev/null +++ b/src/physics/advanced/ContactSolver.js @@ -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; +} diff --git a/src/physics/advanced/Joint.js b/src/physics/advanced/Joint.js new file mode 100644 index 00000000..1dc01968 --- /dev/null +++ b/src/physics/advanced/Joint.js @@ -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); +} \ No newline at end of file diff --git a/src/physics/advanced/Math.js b/src/physics/advanced/Math.js new file mode 100644 index 00000000..b144f8b0 --- /dev/null +++ b/src/physics/advanced/Math.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/Shape.js b/src/physics/advanced/Shape.js new file mode 100644 index 00000000..0e31f21f --- /dev/null +++ b/src/physics/advanced/Shape.js @@ -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; + diff --git a/src/physics/advanced/Space.js b/src/physics/advanced/Space.js new file mode 100644 index 00000000..e869af2e --- /dev/null +++ b/src/physics/advanced/Space.js @@ -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); + } + } + } + +} + diff --git a/src/physics/advanced/Util.js b/src/physics/advanced/Util.js new file mode 100644 index 00000000..93b0e793 --- /dev/null +++ b/src/physics/advanced/Util.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/joints/Angle.js b/src/physics/advanced/joints/Angle.js new file mode 100644 index 00000000..10b09c8d --- /dev/null +++ b/src/physics/advanced/joints/Angle.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/joints/Distance.js b/src/physics/advanced/joints/Distance.js new file mode 100644 index 00000000..e016d80f --- /dev/null +++ b/src/physics/advanced/joints/Distance.js @@ -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; +}*/ diff --git a/src/physics/advanced/joints/Mouse.js b/src/physics/advanced/joints/Mouse.js new file mode 100644 index 00000000..f900afe5 --- /dev/null +++ b/src/physics/advanced/joints/Mouse.js @@ -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; +} + diff --git a/src/physics/advanced/joints/Prismatic.js b/src/physics/advanced/joints/Prismatic.js new file mode 100644 index 00000000..38d4a57c --- /dev/null +++ b/src/physics/advanced/joints/Prismatic.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/joints/Revolute.js b/src/physics/advanced/joints/Revolute.js new file mode 100644 index 00000000..0708da54 --- /dev/null +++ b/src/physics/advanced/joints/Revolute.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/joints/Rope.js b/src/physics/advanced/joints/Rope.js new file mode 100644 index 00000000..5453e459 --- /dev/null +++ b/src/physics/advanced/joints/Rope.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/joints/Weld.js b/src/physics/advanced/joints/Weld.js new file mode 100644 index 00000000..90922893 --- /dev/null +++ b/src/physics/advanced/joints/Weld.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/joints/Wheel.js b/src/physics/advanced/joints/Wheel.js new file mode 100644 index 00000000..9036821b --- /dev/null +++ b/src/physics/advanced/joints/Wheel.js @@ -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; +} + diff --git a/src/physics/advanced/shapes/Box.js b/src/physics/advanced/shapes/Box.js new file mode 100644 index 00000000..6e6e948d --- /dev/null +++ b/src/physics/advanced/shapes/Box.js @@ -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); +} \ No newline at end of file diff --git a/src/physics/advanced/shapes/Circle.js b/src/physics/advanced/shapes/Circle.js new file mode 100644 index 00000000..d2c7f18c --- /dev/null +++ b/src/physics/advanced/shapes/Circle.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/shapes/Poly.js b/src/physics/advanced/shapes/Poly.js new file mode 100644 index 00000000..8b84fa26 --- /dev/null +++ b/src/physics/advanced/shapes/Poly.js @@ -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; +} + diff --git a/src/physics/advanced/shapes/Segment.js b/src/physics/advanced/shapes/Segment.js new file mode 100644 index 00000000..3509e87e --- /dev/null +++ b/src/physics/advanced/shapes/Segment.js @@ -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; +} \ No newline at end of file diff --git a/src/physics/advanced/shapes/Triangle.js b/src/physics/advanced/shapes/Triangle.js new file mode 100644 index 00000000..a8150386 --- /dev/null +++ b/src/physics/advanced/shapes/Triangle.js @@ -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); +} diff --git a/src/physics/arcade/ArcadePhysics.js b/src/physics/arcade/ArcadePhysics.js new file mode 100644 index 00000000..3b9f6e42 --- /dev/null +++ b/src/physics/arcade/ArcadePhysics.js @@ -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; diff --git a/src/physics/arcade/Body.js b/src/physics/arcade/Body.js new file mode 100644 index 00000000..ceadb1dc --- /dev/null +++ b/src/physics/arcade/Body.js @@ -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; + + }, + +}; \ No newline at end of file diff --git a/src/utils/Debug.js b/src/utils/Debug.js index e279b0fc..6adf4169 100644 --- a/src/utils/Debug.js +++ b/src/utils/Debug.js @@ -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);