p2s

<!DOCTYPE html> <html lang="en"> <head> <meta charset="utf-8"> <title>src/equations/Equation.js - p2.js</title> <link rel="stylesheet" href="http://yui.yahooapis.com/3.9.1/build/cssgrids/cssgrids-min.css"> <link rel="stylesheet" href="../assets/vendor/prettify/prettify-min.css"> <link rel="stylesheet" href="../assets/css/main.css" id="site_styles"> <link rel="icon" href="../assets/favicon.ico"> <script src="http://yui.yahooapis.com/combo?3.9.1/build/yui/yui-min.js"></script> </head> <body class="yui3-skin-sam"> <div id="doc"> <div id="hd" class="yui3-g header"> <div class="yui3-u-3-4"> <h1><img src="../assets/css/logo.png" title="p2.js" width="117" height="52"></h1> </div> <div class="yui3-u-1-4 version"> <em>API Docs for: 0.7.1</em> </div> </div> <div id="bd" class="yui3-g"> <div class="yui3-u-1-4"> <div id="docs-sidebar" class="sidebar apidocs"> <div id="api-list"> <h2 class="off-left">APIs</h2> <div id="api-tabview" class="tabview"> <ul class="tabs"> <li><a href="#api-classes">Classes</a></li> <li><a href="#api-modules">Modules</a></li> </ul> <div id="api-tabview-filter"> <input type="search" id="api-filter" placeholder="Type to filter APIs"> </div> <div id="api-tabview-panel"> <ul id="api-classes" class="apis classes"> <li><a href="../classes/AABB.html">AABB</a></li> <li><a href="../classes/AngleLockEquation.html">AngleLockEquation</a></li> <li><a href="../classes/Body.html">Body</a></li> <li><a href="../classes/Box.html">Box</a></li> <li><a href="../classes/Broadphase.html">Broadphase</a></li> <li><a href="../classes/Capsule.html">Capsule</a></li> <li><a href="../classes/Circle.html">Circle</a></li> <li><a href="../classes/Constraint.html">Constraint</a></li> <li><a href="../classes/ContactEquation.html">ContactEquation</a></li> <li><a href="../classes/ContactMaterial.html">ContactMaterial</a></li> <li><a href="../classes/Convex.html">Convex</a></li> <li><a href="../classes/DistanceConstraint.html">DistanceConstraint</a></li> <li><a href="../classes/Equation.html">Equation</a></li> <li><a href="../classes/EventEmitter.html">EventEmitter</a></li> <li><a href="../classes/FrictionEquation.html">FrictionEquation</a></li> <li><a href="../classes/GearConstraint.html">GearConstraint</a></li> <li><a href="../classes/GSSolver.html">GSSolver</a></li> <li><a href="../classes/Heightfield.html">Heightfield</a></li> <li><a href="../classes/Island.html">Island</a></li> <li><a href="../classes/IslandManager.html">IslandManager</a></li> <li><a href="../classes/IslandNode.html">IslandNode</a></li> <li><a href="../classes/Line.html">Line</a></li> <li><a href="../classes/LinearSpring.html">LinearSpring</a></li> <li><a href="../classes/LockConstraint.html">LockConstraint</a></li> <li><a href="../classes/Material.html">Material</a></li> <li><a href="../classes/NaiveBroadphase.html">NaiveBroadphase</a></li> <li><a href="../classes/Narrowphase.html">Narrowphase</a></li> <li><a href="../classes/Object pooling utility..html">Object pooling utility.</a></li> <li><a href="../classes/OverlapKeeper.html">OverlapKeeper</a></li> <li><a href="../classes/OverlapKeeperRecord.html">OverlapKeeperRecord</a></li> <li><a href="../classes/Particle.html">Particle</a></li> <li><a href="../classes/Plane.html">Plane</a></li> <li><a href="../classes/PrismaticConstraint.html">PrismaticConstraint</a></li> <li><a href="../classes/Ray.html">Ray</a></li> <li><a href="../classes/RaycastResult.html">RaycastResult</a></li> <li><a href="../classes/RevoluteConstraint.html">RevoluteConstraint</a></li> <li><a href="../classes/RotationalLockEquation.html">RotationalLockEquation</a></li> <li><a href="../classes/.html"></a></li> <li><a href="../classes/RotationalSpring.html">RotationalSpring</a></li> <li><a href="../classes/RotationalVelocityEquation.html">RotationalVelocityEquation</a></li> <li><a href="../classes/SAPBroadphase.html">SAPBroadphase</a></li> <li><a href="../classes/Shape.html">Shape</a></li> <li><a href="../classes/Solver.html">Solver</a></li> <li><a href="../classes/Spring.html">Spring</a></li> <li><a href="../classes/TopDownVehicle.html">TopDownVehicle</a></li> <li><a href="../classes/TupleDictionary.html">TupleDictionary</a></li> <li><a href="../classes/Utils.html">Utils</a></li> <li><a href="../classes/vec2.html">vec2</a></li> <li><a href="../classes/WheelConstraint.html">WheelConstraint</a></li> <li><a href="../classes/World.html">World</a></li> </ul> <ul id="api-modules" class="apis modules"> </ul> </div> </div> </div> </div> </div> <div class="yui3-u-3-4"> <div id="api-options"> Show: <label for="api-show-inherited"> <input type="checkbox" id="api-show-inherited" checked> Inherited </label> <label for="api-show-protected"> <input type="checkbox" id="api-show-protected"> Protected </label> <label for="api-show-private"> <input type="checkbox" id="api-show-private"> Private </label> <label for="api-show-deprecated"> <input type="checkbox" id="api-show-deprecated"> Deprecated </label> </div> <div class="apidocs"> <div id="docs-main"> <div class="content"> <h1 class="file-heading">File: src/equations/Equation.js</h1> <div class="file"> <pre class="code prettyprint linenums"> module.exports = Equation; var vec2 = require('../math/vec2'), Utils = require('../utils/Utils'), Body = require('../objects/Body'); /** * Base class for constraint equations. * @class Equation * @constructor * @param {Body} bodyA First body participating in the equation * @param {Body} bodyB Second body participating in the equation * @param {number} minForce Minimum force to apply. Default: -Number.MAX_VALUE * @param {number} maxForce Maximum force to apply. Default: Number.MAX_VALUE */ function Equation(bodyA, bodyB, minForce, maxForce){ /** * Minimum force to apply when solving. * @property minForce * @type {Number} */ this.minForce = typeof(minForce)==="undefined" ? -Number.MAX_VALUE : minForce; /** * Max force to apply when solving. * @property maxForce * @type {Number} */ this.maxForce = typeof(maxForce)==="undefined" ? Number.MAX_VALUE : maxForce; /** * First body participating in the constraint * @property bodyA * @type {Body} */ this.bodyA = bodyA; /** * Second body participating in the constraint * @property bodyB * @type {Body} */ this.bodyB = bodyB; /** * The stiffness of this equation. Typically chosen to a large number (~1e7), but can be chosen somewhat freely to get a stable simulation. * @property stiffness * @type {Number} */ this.stiffness = Equation.DEFAULT_STIFFNESS; /** * The number of time steps needed to stabilize the constraint equation. Typically between 3 and 5 time steps. * @property relaxation * @type {Number} */ this.relaxation = Equation.DEFAULT_RELAXATION; /** * The Jacobian entry of this equation. 6 numbers, 3 per body (x,y,angle). * @property G * @type {Array} */ this.G = new Utils.ARRAY_TYPE(6); for(var i=0; i<6; i++){ this.G[i]=0; } this.offset = 0; this.a = 0; this.b = 0; this.epsilon = 0; this.timeStep = 1/60; /** * Indicates if stiffness or relaxation was changed. * @property {Boolean} needsUpdate */ this.needsUpdate = true; /** * The resulting constraint multiplier from the last solve. This is mostly equivalent to the force produced by the constraint. * @property multiplier * @type {Number} */ this.multiplier = 0; /** * Relative velocity. * @property {Number} relativeVelocity */ this.relativeVelocity = 0; /** * Whether this equation is enabled or not. If true, it will be added to the solver. * @property {Boolean} enabled */ this.enabled = true; } Equation.prototype.constructor = Equation; /** * The default stiffness when creating a new Equation. * @static * @property {Number} DEFAULT_STIFFNESS * @default 1e6 */ Equation.DEFAULT_STIFFNESS = 1e6; /** * The default relaxation when creating a new Equation. * @static * @property {Number} DEFAULT_RELAXATION * @default 4 */ Equation.DEFAULT_RELAXATION = 4; /** * Compute SPOOK parameters .a, .b and .epsilon according to the current parameters. See equations 9, 10 and 11 in the <a href="http://www8.cs.umu.se/kurser/5DV058/VT09/lectures/spooknotes.pdf">SPOOK notes</a>. * @method update */ Equation.prototype.update = function(){ var k = this.stiffness, d = this.relaxation, h = this.timeStep; this.a = 4.0 / (h * (1 + 4 * d)); this.b = (4.0 * d) / (1 + 4 * d); this.epsilon = 4.0 / (h * h * k * (1 + 4 * d)); this.needsUpdate = false; }; /** * Multiply a jacobian entry with corresponding positions or velocities * @method gmult * @return {Number} */ Equation.prototype.gmult = function(G,vi,wi,vj,wj){ return G[0] * vi[0] + G[1] * vi[1] + G[2] * wi + G[3] * vj[0] + G[4] * vj[1] + G[5] * wj; }; /** * Computes the RHS of the SPOOK equation * @method computeB * @return {Number} */ Equation.prototype.computeB = function(a,b,h){ var GW = this.computeGW(); var Gq = this.computeGq(); var GiMf = this.computeGiMf(); return - Gq * a - GW * b - GiMf*h; }; /** * Computes G\*q, where q are the generalized body coordinates * @method computeGq * @return {Number} */ var qi = vec2.create(), qj = vec2.create(); Equation.prototype.computeGq = function(){ var G = this.G, bi = this.bodyA, bj = this.bodyB, xi = bi.position, xj = bj.position, ai = bi.angle, aj = bj.angle; return this.gmult(G, qi, ai, qj, aj) + this.offset; }; /** * Computes G\*W, where W are the body velocities * @method computeGW * @return {Number} */ Equation.prototype.computeGW = function(){ var G = this.G, bi = this.bodyA, bj = this.bodyB, vi = bi.velocity, vj = bj.velocity, wi = bi.angularVelocity, wj = bj.angularVelocity; return this.gmult(G,vi,wi,vj,wj) + this.relativeVelocity; }; /** * Computes G\*Wlambda, where W are the body velocities * @method computeGWlambda * @return {Number} */ Equation.prototype.computeGWlambda = function(){ var G = this.G, bi = this.bodyA, bj = this.bodyB, vi = bi.vlambda, vj = bj.vlambda, wi = bi.wlambda, wj = bj.wlambda; return this.gmult(G,vi,wi,vj,wj); }; /** * Computes G\*inv(M)\*f, where M is the mass matrix with diagonal blocks for each body, and f are the forces on the bodies. * @method computeGiMf * @return {Number} */ var iMfi = vec2.create(), iMfj = vec2.create(); Equation.prototype.computeGiMf = function(){ var bi = this.bodyA, bj = this.bodyB, fi = bi.force, ti = bi.angularForce, fj = bj.force, tj = bj.angularForce, invMassi = bi.invMassSolve, invMassj = bj.invMassSolve, invIi = bi.invInertiaSolve, invIj = bj.invInertiaSolve, G = this.G; vec2.scale(iMfi, fi, invMassi); vec2.multiply(iMfi, bi.massMultiplier, iMfi); vec2.scale(iMfj, fj,invMassj); vec2.multiply(iMfj, bj.massMultiplier, iMfj); return this.gmult(G,iMfi,ti*invIi,iMfj,tj*invIj); }; /** * Computes G\*inv(M)\*G' * @method computeGiMGt * @return {Number} */ Equation.prototype.computeGiMGt = function(){ var bi = this.bodyA, bj = this.bodyB, invMassi = bi.invMassSolve, invMassj = bj.invMassSolve, invIi = bi.invInertiaSolve, invIj = bj.invInertiaSolve, G = this.G; return G[0] * G[0] * invMassi * bi.massMultiplier[0] + G[1] * G[1] * invMassi * bi.massMultiplier[1] + G[2] * G[2] * invIi + G[3] * G[3] * invMassj * bj.massMultiplier[0] + G[4] * G[4] * invMassj * bj.massMultiplier[1] + G[5] * G[5] * invIj; }; var addToWlambda_temp = vec2.create(), addToWlambda_Gi = vec2.create(), addToWlambda_Gj = vec2.create(), addToWlambda_ri = vec2.create(), addToWlambda_rj = vec2.create(), addToWlambda_Mdiag = vec2.create(); /** * Add constraint velocity to the bodies. * @method addToWlambda * @param {Number} deltalambda */ Equation.prototype.addToWlambda = function(deltalambda){ var bi = this.bodyA, bj = this.bodyB, temp = addToWlambda_temp, Gi = addToWlambda_Gi, Gj = addToWlambda_Gj, ri = addToWlambda_ri, rj = addToWlambda_rj, invMassi = bi.invMassSolve, invMassj = bj.invMassSolve, invIi = bi.invInertiaSolve, invIj = bj.invInertiaSolve, Mdiag = addToWlambda_Mdiag, G = this.G; Gi[0] = G[0]; Gi[1] = G[1]; Gj[0] = G[3]; Gj[1] = G[4]; // Add to linear velocity // v_lambda += inv(M) * delta_lamba * G vec2.scale(temp, Gi, invMassi*deltalambda); vec2.multiply(temp, temp, bi.massMultiplier); vec2.add( bi.vlambda, bi.vlambda, temp); // This impulse is in the offset frame // Also add contribution to angular //bi.wlambda -= vec2.crossLength(temp,ri); bi.wlambda += invIi * G[2] * deltalambda; vec2.scale(temp, Gj, invMassj*deltalambda); vec2.multiply(temp, temp, bj.massMultiplier); vec2.add( bj.vlambda, bj.vlambda, temp); //bj.wlambda -= vec2.crossLength(temp,rj); bj.wlambda += invIj * G[5] * deltalambda; }; /** * Compute the denominator part of the SPOOK equation: C = G\*inv(M)\*G' + eps * @method computeInvC * @param {Number} eps * @return {Number} */ Equation.prototype.computeInvC = function(eps){ return 1.0 / (this.computeGiMGt() + eps); }; </pre> </div> </div> </div> </div> </div> </div> </div> <script src="../assets/vendor/prettify/prettify-min.js"></script> <script>prettyPrint();</script> <script src="../assets/js/yui-prettify.js"></script> <script src="../assets/../api.js"></script> <script src="../assets/js/api-filter.js"></script> <script src="../assets/js/api-list.js"></script> <script src="../assets/js/api-search.js"></script> <script src="../assets/js/apidocs.js"></script> </body> </html>