(function (global, factory) { typeof exports === 'object' && typeof module !== 'undefined' ? factory(exports) : typeof define === 'function' && define.amd ? define(['exports'], factory) : (factory((global.OIMO = global.OIMO || {}))); }(this, (function (exports) { 'use strict'; // Polyfills if ( Number.EPSILON === undefined ) { Number.EPSILON = Math.pow( 2, - 52 ); } // if ( Math.sign === undefined ) { // https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Math/sign Math.sign = function ( x ) { return ( x < 0 ) ? - 1 : ( x > 0 ) ? 1 : + x; }; } if ( Function.prototype.name === undefined ) { // Missing in IE9-11. // https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Function/name Object.defineProperty( Function.prototype, 'name', { get: function () { return this.toString().match( /^\s*function\s*([^\(\s]*)/ )[ 1 ]; } } ); } if ( Object.assign === undefined ) { // Missing in IE. // https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Object/assign ( function () { Object.assign = function ( target ) { 'use strict'; if ( target === undefined || target === null ) { throw new TypeError( 'Cannot convert undefined or null to object' ); } var output = Object( target ); for ( var index = 1; index < arguments.length; index ++ ) { var source = arguments[ index ]; if ( source !== undefined && source !== null ) { for ( var nextKey in source ) { if ( Object.prototype.hasOwnProperty.call( source, nextKey ) ) { output[ nextKey ] = source[ nextKey ]; } } } } return output; }; } )(); } /* * A list of constants built-in for * the physics engine. */ var REVISION = '1.0.9'; // BroadPhase var BR_NULL = 0; var BR_BRUTE_FORCE = 1; var BR_SWEEP_AND_PRUNE = 2; var BR_BOUNDING_VOLUME_TREE = 3; // Body type var BODY_NULL = 0; var BODY_DYNAMIC = 1; var BODY_STATIC = 2; var BODY_KINEMATIC = 3; var BODY_GHOST = 4; // Shape type var SHAPE_NULL = 0; var SHAPE_SPHERE = 1; var SHAPE_BOX = 2; var SHAPE_CYLINDER = 3; var SHAPE_PLANE = 4; var SHAPE_PARTICLE = 5; var SHAPE_TETRA = 6; // Joint type var JOINT_NULL = 0; var JOINT_DISTANCE = 1; var JOINT_BALL_AND_SOCKET = 2; var JOINT_HINGE = 3; var JOINT_WHEEL = 4; var JOINT_SLIDER = 5; var JOINT_PRISMATIC = 6; // AABB aproximation var AABB_PROX = 0.005; var _Math = { sqrt : Math.sqrt, abs : Math.abs, floor : Math.floor, cos : Math.cos, sin : Math.sin, acos : Math.acos, asin : Math.asin, atan2 : Math.atan2, round : Math.round, pow : Math.pow, max : Math.max, min : Math.min, random : Math.random, degtorad : 0.0174532925199432957, radtodeg : 57.295779513082320876, PI : 3.141592653589793, TwoPI : 6.283185307179586, PI90 : 1.570796326794896, PI270 : 4.712388980384689, INF : Infinity, EPZ : 0.00001, EPZ2 : 0.000001, lerp: function ( x, y, t ) { return ( 1 - t ) * x + t * y; }, randInt: function ( low, high ) { return low + _Math.floor( _Math.random() * ( high - low + 1 ) ); }, rand: function ( low, high ) { return low + _Math.random() * ( high - low ); }, generateUUID: function () { // http://www.broofa.com/Tools/Math.uuid.htm var chars = '0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz'.split( '' ); var uuid = new Array( 36 ); var rnd = 0, r; return function generateUUID() { for ( var i = 0; i < 36; i ++ ) { if ( i === 8 || i === 13 || i === 18 || i === 23 ) { uuid[ i ] = '-'; } else if ( i === 14 ) { uuid[ i ] = '4'; } else { if ( rnd <= 0x02 ) rnd = 0x2000000 + ( Math.random() * 0x1000000 ) | 0; r = rnd & 0xf; rnd = rnd >> 4; uuid[ i ] = chars[ ( i === 19 ) ? ( r & 0x3 ) | 0x8 : r ]; } } return uuid.join( '' ); }; }(), int: function( x ) { return _Math.floor(x); }, fix: function( x, n ) { return x.toFixed(n || 3, 10); }, clamp: function ( value, min, max ) { return _Math.max( min, _Math.min( max, value ) ); }, //clamp: function ( x, a, b ) { return ( x < a ) ? a : ( ( x > b ) ? b : x ); }, distance: function( p1, p2 ){ var xd = p2[0]-p1[0]; var yd = p2[1]-p1[1]; var zd = p2[2]-p1[2]; return _Math.sqrt(xd*xd + yd*yd + zd*zd); }, /*unwrapDegrees: function ( r ) { r = r % 360; if (r > 180) r -= 360; if (r < -180) r += 360; return r; }, unwrapRadian: function( r ){ r = r % _Math.TwoPI; if (r > _Math.PI) r -= _Math.TwoPI; if (r < -_Math.PI) r += _Math.TwoPI; return r; },*/ acosClamp: function ( cos ) { if(cos>1)return 0; else if(cos<-1)return _Math.PI; else return _Math.acos(cos); }, distanceVector: function( v1, v2 ){ var xd = v1.x - v2.x; var yd = v1.y - v2.y; var zd = v1.z - v2.z; return xd * xd + yd * yd + zd * zd; }, dotVectors: function ( a, b ) { return a.x * b.x + a.y * b.y + a.z * b.z; }, }; function printError( clazz, msg ){ console.error("[OIMO] " + clazz + ": " + msg); } // A performance evaluator function InfoDisplay(world){ this.parent = world; this.infos = new Float32Array( 13 ); this.f = [0,0,0]; this.times = [0,0,0,0]; this.broadPhase = this.parent.broadPhaseType; this.version = REVISION; this.fps = 0; this.tt = 0; this.broadPhaseTime = 0; this.narrowPhaseTime = 0; this.solvingTime = 0; this.totalTime = 0; this.updateTime = 0; this.MaxBroadPhaseTime = 0; this.MaxNarrowPhaseTime = 0; this.MaxSolvingTime = 0; this.MaxTotalTime = 0; this.MaxUpdateTime = 0; } Object.assign( InfoDisplay.prototype, { setTime: function(n){ this.times[ n || 0 ] = performance.now(); }, resetMax: function(){ this.MaxBroadPhaseTime = 0; this.MaxNarrowPhaseTime = 0; this.MaxSolvingTime = 0; this.MaxTotalTime = 0; this.MaxUpdateTime = 0; }, calcBroadPhase: function () { this.setTime( 2 ); this.broadPhaseTime = this.times[ 2 ] - this.times[ 1 ]; }, calcNarrowPhase: function () { this.setTime( 3 ); this.narrowPhaseTime = this.times[ 3 ] - this.times[ 2 ]; }, calcEnd: function () { this.setTime( 2 ); this.solvingTime = this.times[ 2 ] - this.times[ 1 ]; this.totalTime = this.times[ 2 ] - this.times[ 0 ]; this.updateTime = this.totalTime - ( this.broadPhaseTime + this.narrowPhaseTime + this.solvingTime ); if( this.tt === 100 ) this.resetMax(); if( this.tt > 100 ){ if( this.broadPhaseTime > this.MaxBroadPhaseTime ) this.MaxBroadPhaseTime = this.broadPhaseTime; if( this.narrowPhaseTime > this.MaxNarrowPhaseTime ) this.MaxNarrowPhaseTime = this.narrowPhaseTime; if( this.solvingTime > this.MaxSolvingTime ) this.MaxSolvingTime = this.solvingTime; if( this.totalTime > this.MaxTotalTime ) this.MaxTotalTime = this.totalTime; if( this.updateTime > this.MaxUpdateTime ) this.MaxUpdateTime = this.updateTime; } this.upfps(); this.tt ++; if(this.tt > 500) this.tt = 0; }, upfps : function(){ this.f[1] = Date.now(); if (this.f[1]-1000>this.f[0]){ this.f[0] = this.f[1]; this.fps = this.f[2]; this.f[2] = 0; } this.f[2]++; }, show: function(){ var info =[ "Oimo.js "+this.version+"
", this.broadPhase + "

", "FPS: " + this.fps +" fps

", "rigidbody "+this.parent.numRigidBodies+"
", "contact   "+this.parent.numContacts+"
", "ct-point  "+this.parent.numContactPoints+"
", "paircheck "+this.parent.broadPhase.numPairChecks+"
", "island    "+this.parent.numIslands +"

", "Time in milliseconds

", "broadphase  "+ _Math.fix(this.broadPhaseTime) + " | " + _Math.fix(this.MaxBroadPhaseTime) +"
", "narrowphase "+ _Math.fix(this.narrowPhaseTime) + " | " + _Math.fix(this.MaxNarrowPhaseTime) + "
", "solving     "+ _Math.fix(this.solvingTime)+ " | " + _Math.fix(this.MaxSolvingTime) + "
", "total       "+ _Math.fix(this.totalTime) + " | " + _Math.fix(this.MaxTotalTime) + "
", "updating    "+ _Math.fix(this.updateTime) + " | " + _Math.fix(this.MaxUpdateTime) + "
" ].join("\n"); return info; }, toArray: function(){ this.infos[0] = this.parent.broadPhase.types; this.infos[1] = this.parent.numRigidBodies; this.infos[2] = this.parent.numContacts; this.infos[3] = this.parent.broadPhase.numPairChecks; this.infos[4] = this.parent.numContactPoints; this.infos[5] = this.parent.numIslands; this.infos[6] = this.broadPhaseTime; this.infos[7] = this.narrowPhaseTime; this.infos[8] = this.solvingTime; this.infos[9] = this.updateTime; this.infos[10] = this.totalTime; this.infos[11] = this.fps; return this.infos; } }); function Vec3 ( x, y, z ) { this.x = x || 0; this.y = y || 0; this.z = z || 0; } Object.assign( Vec3.prototype, { Vec3: true, set: function( x, y, z ){ this.x = x; this.y = y; this.z = z; return this; }, add: function ( a, b ) { if ( b !== undefined ) return this.addVectors( a, b ); this.x += a.x; this.y += a.y; this.z += a.z; return this; }, addVectors: function ( a, b ) { this.x = a.x + b.x; this.y = a.y + b.y; this.z = a.z + b.z; return this; }, addEqual: function ( v ) { this.x += v.x; this.y += v.y; this.z += v.z; return this; }, sub: function ( a, b ) { if ( b !== undefined ) return this.subVectors( a, b ); this.x -= a.x; this.y -= a.y; this.z -= a.z; return this; }, subVectors: function ( a, b ) { this.x = a.x - b.x; this.y = a.y - b.y; this.z = a.z - b.z; return this; }, subEqual: function ( v ) { this.x -= v.x; this.y -= v.y; this.z -= v.z; return this; }, scale: function ( v, s ) { this.x = v.x * s; this.y = v.y * s; this.z = v.z * s; return this; }, scaleEqual: function( s ){ this.x *= s; this.y *= s; this.z *= s; return this; }, multiply: function( v ){ this.x *= v.x; this.y *= v.y; this.z *= v.z; return this; }, multiplyScalar: function( s ){ this.x *= s; this.y *= s; this.z *= s; return this; }, /*scaleV: function( v ){ this.x *= v.x; this.y *= v.y; this.z *= v.z; return this; }, scaleVectorEqual: function( v ){ this.x *= v.x; this.y *= v.y; this.z *= v.z; return this; },*/ addScaledVector: function ( v, s ) { this.x += v.x * s; this.y += v.y * s; this.z += v.z * s; return this; }, subScaledVector: function ( v, s ) { this.x -= v.x * s; this.y -= v.y * s; this.z -= v.z * s; return this; }, /*addTime: function ( v, t ) { this.x += v.x * t; this.y += v.y * t; this.z += v.z * t; return this; }, addScale: function ( v, s ) { this.x += v.x * s; this.y += v.y * s; this.z += v.z * s; return this; }, subScale: function ( v, s ) { this.x -= v.x * s; this.y -= v.y * s; this.z -= v.z * s; return this; },*/ cross: function( a, b ) { if ( b !== undefined ) return this.crossVectors( a, b ); var x = this.x, y = this.y, z = this.z; this.x = y * a.z - z * a.y; this.y = z * a.x - x * a.z; this.z = x * a.y - y * a.x; return this; }, crossVectors: function ( a, b ) { var ax = a.x, ay = a.y, az = a.z; var bx = b.x, by = b.y, bz = b.z; this.x = ay * bz - az * by; this.y = az * bx - ax * bz; this.z = ax * by - ay * bx; return this; }, tangent: function ( a ) { var ax = a.x, ay = a.y, az = a.z; this.x = ay * ax - az * az; this.y = - az * ay - ax * ax; this.z = ax * az + ay * ay; return this; }, invert: function ( v ) { this.x=-v.x; this.y=-v.y; this.z=-v.z; return this; }, negate: function () { this.x = - this.x; this.y = - this.y; this.z = - this.z; return this; }, dot: function ( v ) { return this.x * v.x + this.y * v.y + this.z * v.z; }, addition: function () { return this.x + this.y + this.z; }, lengthSq: function () { return this.x * this.x + this.y * this.y + this.z * this.z; }, length: function () { return _Math.sqrt( this.x * this.x + this.y * this.y + this.z * this.z ); }, copy: function( v ){ this.x = v.x; this.y = v.y; this.z = v.z; return this; }, /*mul: function( b, a, m ){ return this.mulMat( m, a ).add( b ); }, mulMat: function( m, a ){ var e = m.elements; var x = a.x, y = a.y, z = a.z; this.x = e[ 0 ] * x + e[ 1 ] * y + e[ 2 ] * z; this.y = e[ 3 ] * x + e[ 4 ] * y + e[ 5 ] * z; this.z = e[ 6 ] * x + e[ 7 ] * y + e[ 8 ] * z; return this; },*/ applyMatrix3: function ( m, transpose ) { //if( transpose ) m = m.clone().transpose(); var x = this.x, y = this.y, z = this.z; var e = m.elements; if( transpose ){ this.x = e[ 0 ] * x + e[ 1 ] * y + e[ 2 ] * z; this.y = e[ 3 ] * x + e[ 4 ] * y + e[ 5 ] * z; this.z = e[ 6 ] * x + e[ 7 ] * y + e[ 8 ] * z; } else { this.x = e[ 0 ] * x + e[ 3 ] * y + e[ 6 ] * z; this.y = e[ 1 ] * x + e[ 4 ] * y + e[ 7 ] * z; this.z = e[ 2 ] * x + e[ 5 ] * y + e[ 8 ] * z; } return this; }, applyQuaternion: function ( q ) { var x = this.x; var y = this.y; var z = this.z; var qx = q.x; var qy = q.y; var qz = q.z; var qw = q.w; // calculate quat * vector var ix = qw * x + qy * z - qz * y; var iy = qw * y + qz * x - qx * z; var iz = qw * z + qx * y - qy * x; var iw = - qx * x - qy * y - qz * z; // calculate result * inverse quat this.x = ix * qw + iw * - qx + iy * - qz - iz * - qy; this.y = iy * qw + iw * - qy + iz * - qx - ix * - qz; this.z = iz * qw + iw * - qz + ix * - qy - iy * - qx; return this; }, testZero: function () { if(this.x!==0 || this.y!==0 || this.z!==0) return true; else return false; }, testDiff: function( v ){ return this.equals( v ) ? false : true; }, equals: function ( v ) { return v.x === this.x && v.y === this.y && v.z === this.z; }, clone: function () { return new this.constructor( this.x, this.y, this.z ); }, toString: function(){ return"Vec3["+this.x.toFixed(4)+", "+this.y.toFixed(4)+", "+this.z.toFixed(4)+"]"; }, multiplyScalar: function ( scalar ) { if ( isFinite( scalar ) ) { this.x *= scalar; this.y *= scalar; this.z *= scalar; } else { this.x = 0; this.y = 0; this.z = 0; } return this; }, divideScalar: function ( scalar ) { return this.multiplyScalar( 1 / scalar ); }, normalize: function () { return this.divideScalar( this.length() ); }, toArray: function ( array, offset ) { if ( offset === undefined ) offset = 0; array[ offset ] = this.x; array[ offset + 1 ] = this.y; array[ offset + 2 ] = this.z; }, fromArray: function( array, offset ){ if ( offset === undefined ) offset = 0; this.x = array[ offset ]; this.y = array[ offset + 1 ]; this.z = array[ offset + 2 ]; return this; }, } ); function Quat ( x, y, z, w ){ this.x = x || 0; this.y = y || 0; this.z = z || 0; this.w = ( w !== undefined ) ? w : 1; } Object.assign( Quat.prototype, { Quat: true, set: function ( x, y, z, w ) { this.x = x; this.y = y; this.z = z; this.w = w; return this; }, addTime: function( v, t ){ var ax = v.x, ay = v.y, az = v.z; var qw = this.w, qx = this.x, qy = this.y, qz = this.z; t *= 0.5; this.x += t * ( ax*qw + ay*qz - az*qy ); this.y += t * ( ay*qw + az*qx - ax*qz ); this.z += t * ( az*qw + ax*qy - ay*qx ); this.w += t * ( -ax*qx - ay*qy - az*qz ); this.normalize(); return this; }, /*mul: function( q1, q2 ){ var ax = q1.x, ay = q1.y, az = q1.z, as = q1.w, bx = q2.x, by = q2.y, bz = q2.z, bs = q2.w; this.x = ax * bs + as * bx + ay * bz - az * by; this.y = ay * bs + as * by + az * bx - ax * bz; this.z = az * bs + as * bz + ax * by - ay * bx; this.w = as * bs - ax * bx - ay * by - az * bz; return this; },*/ multiply: function ( q, p ) { if ( p !== undefined ) return this.multiplyQuaternions( q, p ); return this.multiplyQuaternions( this, q ); }, multiplyQuaternions: function ( a, b ) { var qax = a.x, qay = a.y, qaz = a.z, qaw = a.w; var qbx = b.x, qby = b.y, qbz = b.z, qbw = b.w; this.x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby; this.y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz; this.z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx; this.w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz; return this; }, setFromUnitVectors: function( v1, v2 ) { var vx = new Vec3(); var r = v1.dot( v2 ) + 1; if ( r < _Math.EPS2 ) { r = 0; if ( _Math.abs( v1.x ) > _Math.abs( v1.z ) ) vx.set( - v1.y, v1.x, 0 ); else vx.set( 0, - v1.z, v1.y ); } else { vx.crossVectors( v1, v2 ); } this._x = vx.x; this._y = vx.y; this._z = vx.z; this._w = r; return this.normalize(); }, arc: function( v1, v2 ){ var x1 = v1.x; var y1 = v1.y; var z1 = v1.z; var x2 = v2.x; var y2 = v2.y; var z2 = v2.z; var d = x1*x2 + y1*y2 + z1*z2; if( d==-1 ){ x2 = y1*x1 - z1*z1; y2 = -z1*y1 - x1*x1; z2 = x1*z1 + y1*y1; d = 1 / _Math.sqrt( x2*x2 + y2*y2 + z2*z2 ); this.w = 0; this.x = x2*d; this.y = y2*d; this.z = z2*d; return this; } var cx = y1*z2 - z1*y2; var cy = z1*x2 - x1*z2; var cz = x1*y2 - y1*x2; this.w = _Math.sqrt( ( 1 + d) * 0.5 ); d = 0.5 / this.w; this.x = cx * d; this.y = cy * d; this.z = cz * d; return this; }, normalize: function(){ var l = this.length(); if ( l === 0 ) { this.set( 0, 0, 0, 1 ); } else { l = 1 / l; this.x = this.x * l; this.y = this.y * l; this.z = this.z * l; this.w = this.w * l; } return this; }, inverse: function () { return this.conjugate().normalize(); }, invert: function ( q ) { this.x = q.x; this.y = q.y; this.z = q.z; this.w = q.w; this.conjugate().normalize(); return this; }, conjugate: function () { this.x *= - 1; this.y *= - 1; this.z *= - 1; return this; }, length: function(){ return _Math.sqrt( this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w ); }, lengthSq: function () { return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w; }, copy: function( q ){ this.x = q.x; this.y = q.y; this.z = q.z; this.w = q.w; return this; }, clone: function( q ){ return new Quat( this.x, this.y, this.z, this.w ); }, testDiff: function ( q ) { return this.equals( q ) ? false : true; }, equals: function ( q ) { return this.x === q.x && this.y === q.y && this.z === q.z && this.w === q.w; }, toString: function(){ return"Quat["+this.x.toFixed(4)+", ("+this.y.toFixed(4)+", "+this.z.toFixed(4)+", "+this.w.toFixed(4)+")]"; }, setFromEuler: function ( x, y, z ){ var c1 = Math.cos( x * 0.5 ); var c2 = Math.cos( y * 0.5 ); var c3 = Math.cos( z * 0.5 ); var s1 = Math.sin( x * 0.5 ); var s2 = Math.sin( y * 0.5 ); var s3 = Math.sin( z * 0.5 ); // XYZ this.x = s1 * c2 * c3 + c1 * s2 * s3; this.y = c1 * s2 * c3 - s1 * c2 * s3; this.z = c1 * c2 * s3 + s1 * s2 * c3; this.w = c1 * c2 * c3 - s1 * s2 * s3; return this; }, setFromAxis: function ( axis, rad ) { axis.normalize(); rad = rad * 0.5; var s = _Math.sin( rad ); this.x = s * axis.x; this.y = s * axis.y; this.z = s * axis.z; this.w = _Math.cos( rad ); return this; }, setFromMat33: function ( m ) { var trace = m[0] + m[4] + m[8]; var s; if ( trace > 0 ) { s = _Math.sqrt( trace + 1.0 ); this.w = 0.5 / s; s = 0.5 / s; this.x = ( m[5] - m[7] ) * s; this.y = ( m[6] - m[2] ) * s; this.z = ( m[1] - m[3] ) * s; } else { var out = []; var i = 0; if ( m[4] > m[0] ) i = 1; if ( m[8] > m[i*3+i] ) i = 2; var j = (i+1)%3; var k = (i+2)%3; s = _Math.sqrt( m[i*3+i] - m[j*3+j] - m[k*3+k] + 1.0 ); out[i] = 0.5 * fRoot; s = 0.5 / fRoot; this.w = ( m[j*3+k] - m[k*3+j] ) * s; out[j] = ( m[j*3+i] + m[i*3+j] ) * s; out[k] = ( m[k*3+i] + m[i*3+k] ) * s; this.x = out[1]; this.y = out[2]; this.z = out[3]; } return this; }, toArray: function ( array, offset ) { offset = offset || 0; array[ offset ] = this.x; array[ offset + 1 ] = this.y; array[ offset + 2 ] = this.z; array[ offset + 3 ] = this.w; }, fromArray: function( array, offset ){ offset = offset || 0; this.set( array[ offset ], array[ offset + 1 ], array[ offset + 2 ], array[ offset + 3 ] ); return this; } }); function Mat33 ( e00, e01, e02, e10, e11, e12, e20, e21, e22 ){ this.elements = [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ]; if ( arguments.length > 0 ) { console.error( 'OIMO.Mat33: the constructor no longer reads arguments. use .set() instead.' ); } } Object.assign( Mat33.prototype, { Mat33: true, set: function ( e00, e01, e02, e10, e11, e12, e20, e21, e22 ){ var te = this.elements; te[0] = e00; te[1] = e01; te[2] = e02; te[3] = e10; te[4] = e11; te[5] = e12; te[6] = e20; te[7] = e21; te[8] = e22; return this; }, add: function ( a, b ) { if( b !== undefined ) return this.addMatrixs( a, b ); var e = this.elements, te = a.elements; e[0] += te[0]; e[1] += te[1]; e[2] += te[2]; e[3] += te[3]; e[4] += te[4]; e[5] += te[5]; e[6] += te[6]; e[7] += te[7]; e[8] += te[8]; return this; }, addMatrixs: function ( a, b ) { var te = this.elements, tem1 = a.elements, tem2 = b.elements; te[0] = tem1[0] + tem2[0]; te[1] = tem1[1] + tem2[1]; te[2] = tem1[2] + tem2[2]; te[3] = tem1[3] + tem2[3]; te[4] = tem1[4] + tem2[4]; te[5] = tem1[5] + tem2[5]; te[6] = tem1[6] + tem2[6]; te[7] = tem1[7] + tem2[7]; te[8] = tem1[8] + tem2[8]; return this; }, addEqual: function( m ){ var te = this.elements, tem = m.elements; te[0] += tem[0]; te[1] += tem[1]; te[2] += tem[2]; te[3] += tem[3]; te[4] += tem[4]; te[5] += tem[5]; te[6] += tem[6]; te[7] += tem[7]; te[8] += tem[8]; return this; }, sub: function ( a, b ) { if( b !== undefined ) return this.subMatrixs( a, b ); var e = this.elements, te = a.elements; e[0] -= te[0]; e[1] -= te[1]; e[2] -= te[2]; e[3] -= te[3]; e[4] -= te[4]; e[5] -= te[5]; e[6] -= te[6]; e[7] -= te[7]; e[8] -= te[8]; return this; }, subMatrixs: function ( a, b ) { var te = this.elements, tem1 = a.elements, tem2 = b.elements; te[0] = tem1[0] - tem2[0]; te[1] = tem1[1] - tem2[1]; te[2] = tem1[2] - tem2[2]; te[3] = tem1[3] - tem2[3]; te[4] = tem1[4] - tem2[4]; te[5] = tem1[5] - tem2[5]; te[6] = tem1[6] - tem2[6]; te[7] = tem1[7] - tem2[7]; te[8] = tem1[8] - tem2[8]; return this; }, subEqual: function ( m ) { var te = this.elements, tem = m.elements; te[0] -= tem[0]; te[1] -= tem[1]; te[2] -= tem[2]; te[3] -= tem[3]; te[4] -= tem[4]; te[5] -= tem[5]; te[6] -= tem[6]; te[7] -= tem[7]; te[8] -= tem[8]; return this; }, scale: function ( m, s ) { var te = this.elements, tm = m.elements; te[0] = tm[0] * s; te[1] = tm[1] * s; te[2] = tm[2] * s; te[3] = tm[3] * s; te[4] = tm[4] * s; te[5] = tm[5] * s; te[6] = tm[6] * s; te[7] = tm[7] * s; te[8] = tm[8] * s; return this; }, scaleEqual: function ( s ){// multiplyScalar var te = this.elements; te[0] *= s; te[1] *= s; te[2] *= s; te[3] *= s; te[4] *= s; te[5] *= s; te[6] *= s; te[7] *= s; te[8] *= s; return this; }, multiplyMatrices: function ( m1, m2, transpose ) { if( transpose ) m2 = m2.clone().transpose(); var te = this.elements; var tm1 = m1.elements; var tm2 = m2.elements; var a0 = tm1[0], a3 = tm1[3], a6 = tm1[6]; var a1 = tm1[1], a4 = tm1[4], a7 = tm1[7]; var a2 = tm1[2], a5 = tm1[5], a8 = tm1[8]; var b0 = tm2[0], b3 = tm2[3], b6 = tm2[6]; var b1 = tm2[1], b4 = tm2[4], b7 = tm2[7]; var b2 = tm2[2], b5 = tm2[5], b8 = tm2[8]; te[0] = a0*b0 + a1*b3 + a2*b6; te[1] = a0*b1 + a1*b4 + a2*b7; te[2] = a0*b2 + a1*b5 + a2*b8; te[3] = a3*b0 + a4*b3 + a5*b6; te[4] = a3*b1 + a4*b4 + a5*b7; te[5] = a3*b2 + a4*b5 + a5*b8; te[6] = a6*b0 + a7*b3 + a8*b6; te[7] = a6*b1 + a7*b4 + a8*b7; te[8] = a6*b2 + a7*b5 + a8*b8; return this; }, /*mul: function ( m1, m2, transpose ) { if( transpose ) m2 = m2.clone().transpose(); var te = this.elements; var tm1 = m1.elements; var tm2 = m2.elements; //var tmp; var a0 = tm1[0], a3 = tm1[3], a6 = tm1[6]; var a1 = tm1[1], a4 = tm1[4], a7 = tm1[7]; var a2 = tm1[2], a5 = tm1[5], a8 = tm1[8]; var b0 = tm2[0], b3 = tm2[3], b6 = tm2[6]; var b1 = tm2[1], b4 = tm2[4], b7 = tm2[7]; var b2 = tm2[2], b5 = tm2[5], b8 = tm2[8]; /*if( transpose ){ tmp = b1; b1 = b3; b3 = tmp; tmp = b2; b2 = b6; b6 = tmp; tmp = b5; b5 = b7; b7 = tmp; } te[0] = a0*b0 + a1*b3 + a2*b6; te[1] = a0*b1 + a1*b4 + a2*b7; te[2] = a0*b2 + a1*b5 + a2*b8; te[3] = a3*b0 + a4*b3 + a5*b6; te[4] = a3*b1 + a4*b4 + a5*b7; te[5] = a3*b2 + a4*b5 + a5*b8; te[6] = a6*b0 + a7*b3 + a8*b6; te[7] = a6*b1 + a7*b4 + a8*b7; te[8] = a6*b2 + a7*b5 + a8*b8; return this; },*/ transpose: function ( m ) { if( m !== undefined ){ var a = m.elements; this.set( a[0], a[3], a[6], a[1], a[4], a[7], a[2], a[5], a[8] ); return this; } var te = this.elements; var a01 = te[1], a02 = te[2], a12 = te[5]; te[1] = te[3]; te[2] = te[6]; te[3] = a01; te[5] = te[7]; te[6] = a02; te[7] = a12; return this; }, /*mulScale: function ( m, sx, sy, sz, Prepend ) { var prepend = Prepend || false; var te = this.elements, tm = m.elements; if(prepend){ te[0] = sx*tm[0]; te[1] = sx*tm[1]; te[2] = sx*tm[2]; te[3] = sy*tm[3]; te[4] = sy*tm[4]; te[5] = sy*tm[5]; te[6] = sz*tm[6]; te[7] = sz*tm[7]; te[8] = sz*tm[8]; }else{ te[0] = tm[0]*sx; te[1] = tm[1]*sy; te[2] = tm[2]*sz; te[3] = tm[3]*sx; te[4] = tm[4]*sy; te[5] = tm[5]*sz; te[6] = tm[6]*sx; te[7] = tm[7]*sy; te[8] = tm[8]*sz; } return this; }, transpose: function ( m ) { var te = this.elements, tm = m.elements; te[0] = tm[0]; te[1] = tm[3]; te[2] = tm[6]; te[3] = tm[1]; te[4] = tm[4]; te[5] = tm[7]; te[6] = tm[2]; te[7] = tm[5]; te[8] = tm[8]; return this; },*/ setQuat: function ( q ) { var te = this.elements; var x = q.x, y = q.y, z = q.z, w = q.w; var x2 = x + x, y2 = y + y, z2 = z + z; var xx = x * x2, xy = x * y2, xz = x * z2; var yy = y * y2, yz = y * z2, zz = z * z2; var wx = w * x2, wy = w * y2, wz = w * z2; te[0] = 1 - ( yy + zz ); te[1] = xy - wz; te[2] = xz + wy; te[3] = xy + wz; te[4] = 1 - ( xx + zz ); te[5] = yz - wx; te[6] = xz - wy; te[7] = yz + wx; te[8] = 1 - ( xx + yy ); return this; }, invert: function( m ) { var te = this.elements, tm = m.elements, a00 = tm[0], a10 = tm[3], a20 = tm[6], a01 = tm[1], a11 = tm[4], a21 = tm[7], a02 = tm[2], a12 = tm[5], a22 = tm[8], b01 = a22 * a11 - a12 * a21, b11 = -a22 * a10 + a12 * a20, b21 = a21 * a10 - a11 * a20, det = a00 * b01 + a01 * b11 + a02 * b21; if ( det === 0 ) { console.log( "can't invert matrix, determinant is 0"); return this.identity(); } det = 1.0 / det; te[0] = b01 * det; te[1] = (-a22 * a01 + a02 * a21) * det; te[2] = (a12 * a01 - a02 * a11) * det; te[3] = b11 * det; te[4] = (a22 * a00 - a02 * a20) * det; te[5] = (-a12 * a00 + a02 * a10) * det; te[6] = b21 * det; te[7] = (-a21 * a00 + a01 * a20) * det; te[8] = (a11 * a00 - a01 * a10) * det; return this; }, addOffset: function ( m, v ) { var relX = v.x; var relY = v.y; var relZ = v.z; var te = this.elements; te[0] += m * ( relY * relY + relZ * relZ ); te[4] += m * ( relX * relX + relZ * relZ ); te[8] += m * ( relX * relX + relY * relY ); var xy = m * relX * relY; var yz = m * relY * relZ; var zx = m * relZ * relX; te[1] -= xy; te[3] -= xy; te[2] -= yz; te[6] -= yz; te[5] -= zx; te[7] -= zx; return this; }, subOffset: function ( m, v ) { var relX = v.x; var relY = v.y; var relZ = v.z; var te = this.elements; te[0] -= m * ( relY * relY + relZ * relZ ); te[4] -= m * ( relX * relX + relZ * relZ ); te[8] -= m * ( relX * relX + relY * relY ); var xy = m * relX * relY; var yz = m * relY * relZ; var zx = m * relZ * relX; te[1] += xy; te[3] += xy; te[2] += yz; te[6] += yz; te[5] += zx; te[7] += zx; return this; }, // OK multiplyScalar: function ( s ) { var te = this.elements; te[ 0 ] *= s; te[ 3 ] *= s; te[ 6 ] *= s; te[ 1 ] *= s; te[ 4 ] *= s; te[ 7 ] *= s; te[ 2 ] *= s; te[ 5 ] *= s; te[ 8 ] *= s; return this; }, identity: function () { this.set( 1, 0, 0, 0, 1, 0, 0, 0, 1 ); return this; }, clone: function () { return new Mat33().fromArray( this.elements ); }, copy: function ( m ) { for ( var i = 0; i < 9; i ++ ) this.elements[ i ] = m.elements[ i ]; return this; }, determinant: function () { var te = this.elements; var a = te[ 0 ], b = te[ 1 ], c = te[ 2 ], d = te[ 3 ], e = te[ 4 ], f = te[ 5 ], g = te[ 6 ], h = te[ 7 ], i = te[ 8 ]; return a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g; }, fromArray: function ( array, offset ) { if ( offset === undefined ) offset = 0; for( var i = 0; i < 9; i ++ ) { this.elements[ i ] = array[ i + offset ]; } return this; }, toArray: function ( array, offset ) { if ( array === undefined ) array = []; if ( offset === undefined ) offset = 0; var te = this.elements; array[ offset ] = te[ 0 ]; array[ offset + 1 ] = te[ 1 ]; array[ offset + 2 ] = te[ 2 ]; array[ offset + 3 ] = te[ 3 ]; array[ offset + 4 ] = te[ 4 ]; array[ offset + 5 ] = te[ 5 ]; array[ offset + 6 ] = te[ 6 ]; array[ offset + 7 ] = te[ 7 ]; array[ offset + 8 ] = te[ 8 ]; return array; } } ); /** * An axis-aligned bounding box. * * @author saharan * @author lo-th */ function AABB( minX, maxX, minY, maxY, minZ, maxZ ){ this.elements = new Float32Array( 6 ); var te = this.elements; te[0] = minX || 0; te[1] = minY || 0; te[2] = minZ || 0; te[3] = maxX || 0; te[4] = maxY || 0; te[5] = maxZ || 0; } Object.assign( AABB.prototype, { AABB: true, set: function(minX, maxX, minY, maxY, minZ, maxZ){ var te = this.elements; te[0] = minX; te[3] = maxX; te[1] = minY; te[4] = maxY; te[2] = minZ; te[5] = maxZ; return this; }, intersectTest: function ( aabb ) { var te = this.elements; var ue = aabb.elements; return te[0] > ue[3] || te[1] > ue[4] || te[2] > ue[5] || te[3] < ue[0] || te[4] < ue[1] || te[5] < ue[2] ? true : false; }, intersectTestTwo: function ( aabb ) { var te = this.elements; var ue = aabb.elements; return te[0] < ue[0] || te[1] < ue[1] || te[2] < ue[2] || te[3] > ue[3] || te[4] > ue[4] || te[5] > ue[5] ? true : false; }, clone: function () { return new this.constructor().fromArray( this.elements ); }, copy: function ( aabb, margin ) { var m = margin || 0; var me = aabb.elements; this.set( me[ 0 ]-m, me[ 3 ]+m, me[ 1 ]-m, me[ 4 ]+m, me[ 2 ]-m, me[ 5 ]+m ); return this; }, fromArray: function ( array ) { this.elements.set( array ); return this; }, // Set this AABB to the combined AABB of aabb1 and aabb2. combine: function( aabb1, aabb2 ) { var a = aabb1.elements; var b = aabb2.elements; var te = this.elements; te[0] = a[0] < b[0] ? a[0] : b[0]; te[1] = a[1] < b[1] ? a[1] : b[1]; te[2] = a[2] < b[2] ? a[2] : b[2]; te[3] = a[3] > b[3] ? a[3] : b[3]; te[4] = a[4] > b[4] ? a[4] : b[4]; te[5] = a[5] > b[5] ? a[5] : b[5]; return this; }, // Get the surface area. surfaceArea: function () { var te = this.elements; var a = te[3] - te[0]; var h = te[4] - te[1]; var d = te[5] - te[2]; return 2 * (a * (h + d) + h * d ); }, // Get whether the AABB intersects with the point or not. intersectsWithPoint:function(x,y,z){ var te = this.elements; return x>=te[0] && x<=te[3] && y>=te[1] && y<=te[4] && z>=te[2] && z<=te[5]; }, /** * Set the AABB from an array * of vertices. From THREE. * @author WestLangley * @author xprogram */ setFromPoints: function(arr){ this.makeEmpty(); for(var i = 0; i < arr.length; i++){ this.expandByPoint(arr[i]); } }, makeEmpty: function(){ this.set(-Infinity, -Infinity, -Infinity, Infinity, Infinity, Infinity); }, expandByPoint: function(pt){ var te = this.elements; this.set( _Math.min(te[ 0 ], pt.x), _Math.min(te[ 1 ], pt.y), _Math.min(te[ 2 ], pt.z), _Math.max(te[ 3 ], pt.x), _Math.max(te[ 4 ], pt.y), _Math.max(te[ 5 ], pt.z) ); }, expandByScalar: function(s){ var te = this.elements; te[0] += -s; te[1] += -s; te[2] += -s; te[3] += s; te[4] += s; te[5] += s; } }); var count = 0; function ShapeIdCount() { return count++; } /** * A shape is used to detect collisions of rigid bodies. * * @author saharan * @author lo-th */ function Shape ( config ) { this.type = SHAPE_NULL; // global identification of the shape should be unique to the shape. this.id = ShapeIdCount(); // previous shape in parent rigid body. Used for fast interations. this.prev = null; // next shape in parent rigid body. Used for fast interations. this.next = null; // proxy of the shape used for broad-phase collision detection. this.proxy = null; // parent rigid body of the shape. this.parent = null; // linked list of the contacts with the shape. this.contactLink = null; // number of the contacts with the shape. this.numContacts = 0; // center of gravity of the shape in world coordinate system. this.position = new Vec3(); // rotation matrix of the shape in world coordinate system. this.rotation = new Mat33(); // position of the shape in parent's coordinate system. this.relativePosition = new Vec3().copy( config.relativePosition ); // rotation matrix of the shape in parent's coordinate system. this.relativeRotation = new Mat33().copy( config.relativeRotation ); // axis-aligned bounding box of the shape. this.aabb = new AABB(); // density of the shape. this.density = config.density; // coefficient of friction of the shape. this.friction = config.friction; // coefficient of restitution of the shape. this.restitution = config.restitution; // bits of the collision groups to which the shape belongs. this.belongsTo = config.belongsTo; // bits of the collision groups with which the shape collides. this.collidesWith = config.collidesWith; } Object.assign( Shape.prototype, { Shape: true, // Calculate the mass information of the shape. calculateMassInfo: function( out ){ printError("Shape", "Inheritance error."); }, // Update the proxy of the shape. updateProxy: function(){ printError("Shape", "Inheritance error."); } }); /** * Box shape. * @author saharan * @author lo-th */ function Box ( config, Width, Height, Depth ) { Shape.call( this, config ); this.type = SHAPE_BOX; this.width = Width; this.height = Height; this.depth = Depth; this.halfWidth = Width * 0.5; this.halfHeight = Height * 0.5; this.halfDepth = Depth * 0.5; this.dimentions = new Float32Array( 18 ); this.elements = new Float32Array( 24 ); } Box.prototype = Object.assign( Object.create( Shape.prototype ), { constructor: Box, calculateMassInfo: function ( out ) { var mass = this.width * this.height * this.depth * this.density; var divid = 1/12; out.mass = mass; out.inertia.set( mass * ( this.height * this.height + this.depth * this.depth ) * divid, 0, 0, 0, mass * ( this.width * this.width + this.depth * this.depth ) * divid, 0, 0, 0, mass * ( this.width * this.width + this.height * this.height ) * divid ); }, updateProxy: function () { var te = this.rotation.elements; var di = this.dimentions; // Width di[0] = te[0]; di[1] = te[3]; di[2] = te[6]; // Height di[3] = te[1]; di[4] = te[4]; di[5] = te[7]; // Depth di[6] = te[2]; di[7] = te[5]; di[8] = te[8]; // half Width di[9] = te[0] * this.halfWidth; di[10] = te[3] * this.halfWidth; di[11] = te[6] * this.halfWidth; // half Height di[12] = te[1] * this.halfHeight; di[13] = te[4] * this.halfHeight; di[14] = te[7] * this.halfHeight; // half Depth di[15] = te[2] * this.halfDepth; di[16] = te[5] * this.halfDepth; di[17] = te[8] * this.halfDepth; var wx = di[9]; var wy = di[10]; var wz = di[11]; var hx = di[12]; var hy = di[13]; var hz = di[14]; var dx = di[15]; var dy = di[16]; var dz = di[17]; var x = this.position.x; var y = this.position.y; var z = this.position.z; var v = this.elements; //v1 v[0] = x + wx + hx + dx; v[1] = y + wy + hy + dy; v[2] = z + wz + hz + dz; //v2 v[3] = x + wx + hx - dx; v[4] = y + wy + hy - dy; v[5] = z + wz + hz - dz; //v3 v[6] = x + wx - hx + dx; v[7] = y + wy - hy + dy; v[8] = z + wz - hz + dz; //v4 v[9] = x + wx - hx - dx; v[10] = y + wy - hy - dy; v[11] = z + wz - hz - dz; //v5 v[12] = x - wx + hx + dx; v[13] = y - wy + hy + dy; v[14] = z - wz + hz + dz; //v6 v[15] = x - wx + hx - dx; v[16] = y - wy + hy - dy; v[17] = z - wz + hz - dz; //v7 v[18] = x - wx - hx + dx; v[19] = y - wy - hy + dy; v[20] = z - wz - hz + dz; //v8 v[21] = x - wx - hx - dx; v[22] = y - wy - hy - dy; v[23] = z - wz - hz - dz; var w = di[9] < 0 ? -di[9] : di[9]; var h = di[10] < 0 ? -di[10] : di[10]; var d = di[11] < 0 ? -di[11] : di[11]; w = di[12] < 0 ? w - di[12] : w + di[12]; h = di[13] < 0 ? h - di[13] : h + di[13]; d = di[14] < 0 ? d - di[14] : d + di[14]; w = di[15] < 0 ? w - di[15] : w + di[15]; h = di[16] < 0 ? h - di[16] : h + di[16]; d = di[17] < 0 ? d - di[17] : d + di[17]; var p = AABB_PROX; this.aabb.set( this.position.x - w - p, this.position.x + w + p, this.position.y - h - p, this.position.y + h + p, this.position.z - d - p, this.position.z + d + p ); if ( this.proxy != null ) this.proxy.update(); } }); /** * Sphere shape * @author saharan * @author lo-th */ function Sphere( config, radius ) { Shape.call( this, config ); this.type = SHAPE_SPHERE; // radius of the shape. this.radius = radius; } Sphere.prototype = Object.assign( Object.create( Shape.prototype ), { constructor: Sphere, volume: function () { return _Math.PI * this.radius * 1.333333; }, calculateMassInfo: function ( out ) { var mass = this.volume() * this.radius * this.radius * this.density; //1.333 * _Math.PI * this.radius * this.radius * this.radius * this.density; out.mass = mass; var inertia = mass * this.radius * this.radius * 0.4; out.inertia.set( inertia, 0, 0, 0, inertia, 0, 0, 0, inertia ); }, updateProxy: function () { var p = AABB_PROX; this.aabb.set( this.position.x - this.radius - p, this.position.x + this.radius + p, this.position.y - this.radius - p, this.position.y + this.radius + p, this.position.z - this.radius - p, this.position.z + this.radius + p ); if ( this.proxy != null ) this.proxy.update(); } }); /** * Cylinder shape * @author saharan * @author lo-th */ function Cylinder ( config, radius, height ) { Shape.call( this, config ); this.type = SHAPE_CYLINDER; this.radius = radius; this.height = height; this.halfHeight = height * 0.5; this.normalDirection = new Vec3(); this.halfDirection = new Vec3(); } Cylinder.prototype = Object.assign( Object.create( Shape.prototype ), { constructor: Cylinder, calculateMassInfo: function ( out ) { var rsq = this.radius * this.radius; var mass = _Math.PI * rsq * this.height * this.density; var inertiaXZ = ( ( 0.25 * rsq ) + ( 0.0833 * this.height * this.height ) ) * mass; var inertiaY = 0.5 * rsq; out.mass = mass; out.inertia.set( inertiaXZ, 0, 0, 0, inertiaY, 0, 0, 0, inertiaXZ ); }, updateProxy: function () { var te = this.rotation.elements; var len, wx, hy, dz, xx, yy, zz, w, h, d, p; xx = te[1] * te[1]; yy = te[4] * te[4]; zz = te[7] * te[7]; this.normalDirection.set( te[1], te[4], te[7] ); this.halfDirection.scale( this.normalDirection, this.halfHeight ); wx = 1 - xx; len = _Math.sqrt(wx*wx + xx*yy + xx*zz); if(len>0) len = this.radius/len; wx *= len; hy = 1 - yy; len = _Math.sqrt(yy*xx + hy*hy + yy*zz); if(len>0) len = this.radius/len; hy *= len; dz = 1 - zz; len = _Math.sqrt(zz*xx + zz*yy + dz*dz); if(len>0) len = this.radius/len; dz *= len; w = this.halfDirection.x < 0 ? -this.halfDirection.x : this.halfDirection.x; h = this.halfDirection.y < 0 ? -this.halfDirection.y : this.halfDirection.y; d = this.halfDirection.z < 0 ? -this.halfDirection.z : this.halfDirection.z; w = wx < 0 ? w - wx : w + wx; h = hy < 0 ? h - hy : h + hy; d = dz < 0 ? d - dz : d + dz; p = AABB_PROX; this.aabb.set( this.position.x - w - p, this.position.x + w + p, this.position.y - h - p, this.position.y + h + p, this.position.z - d - p, this.position.z + d + p ); if ( this.proxy != null ) this.proxy.update(); } }); /** * Plane shape. * @author lo-th */ function Plane( config, normal ) { Shape.call( this, config ); this.type = SHAPE_PLANE; // radius of the shape. this.normal = new Vec3( 0, 1, 0 ); } Plane.prototype = Object.assign( Object.create( Shape.prototype ), { constructor: Plane, volume: function () { return Number.MAX_VALUE; }, calculateMassInfo: function ( out ) { out.mass = this.density;//0.0001; var inertia = 1; out.inertia.set( inertia, 0, 0, 0, inertia, 0, 0, 0, inertia ); }, updateProxy: function () { var p = AABB_PROX; var min = -_Math.INF; var max = _Math.INF; var n = this.normal; // The plane AABB is infinite, except if the normal is pointing along any axis this.aabb.set( n.x === -1 ? this.position.x - p : min, n.x === 1 ? this.position.x + p : max, n.y === -1 ? this.position.y - p : min, n.y === 1 ? this.position.y + p : max, n.z === -1 ? this.position.z - p : min, n.z === 1 ? this.position.z + p : max ); if ( this.proxy != null ) this.proxy.update(); } }); /** * A Particule shape * @author lo-th */ function Particle( config, normal ) { Shape.call( this, config ); this.type = SHAPE_PARTICLE; } Particle.prototype = Object.assign( Object.create( Shape.prototype ), { constructor: Particle, volume: function () { return Number.MAX_VALUE; }, calculateMassInfo: function ( out ) { var inertia = 0; out.inertia.set( inertia, 0, 0, 0, inertia, 0, 0, 0, inertia ); }, updateProxy: function () { var p = 0;//AABB_PROX; this.aabb.set( this.position.x - p, this.position.x + p, this.position.y - p, this.position.y + p, this.position.z - p, this.position.z + p ); if ( this.proxy != null ) this.proxy.update(); } }); /** * A shape configuration holds common configuration data for constructing a shape. * These configurations can be reused safely. * * @author saharan * @author lo-th */ function ShapeConfig(){ // position of the shape in parent's coordinate system. this.relativePosition = new Vec3(); // rotation matrix of the shape in parent's coordinate system. this.relativeRotation = new Mat33(); // coefficient of friction of the shape. this.friction = 0.2; // 0.4 // coefficient of restitution of the shape. this.restitution = 0.2; // density of the shape. this.density = 1; // bits of the collision groups to which the shape belongs. this.belongsTo = 1; // bits of the collision groups with which the shape collides. this.collidesWith = 0xffffffff; } /** * An information of limit and motor. * * @author saharan */ function LimitMotor ( axis, fixed ) { fixed = fixed || false; // The axis of the constraint. this.axis = axis; // The current angle for rotational constraints. this.angle = 0; // The lower limit. Set lower > upper to disable this.lowerLimit = fixed ? 0 : 1; // The upper limit. Set lower > upper to disable. this.upperLimit = 0; // The target motor speed. this.motorSpeed = 0; // The maximum motor force or torque. Set 0 to disable. this.maxMotorForce = 0; // The frequency of the spring. Set 0 to disable. this.frequency = 0; // The damping ratio of the spring. Set 0 for no damping, 1 for critical damping. this.dampingRatio = 0; } Object.assign( LimitMotor.prototype, { LimitMotor: true, // Set limit data into this constraint. setLimit:function ( lowerLimit, upperLimit ) { this.lowerLimit = lowerLimit; this.upperLimit = upperLimit; }, // Set motor data into this constraint. setMotor:function ( motorSpeed, maxMotorForce ) { this.motorSpeed = motorSpeed; this.maxMotorForce = maxMotorForce; }, // Set spring data into this constraint. setSpring:function ( frequency, dampingRatio ) { this.frequency = frequency; this.dampingRatio = dampingRatio; } }); /** * The base class of all type of the constraints. * * @author saharan * @author lo-th */ function Constraint(){ // parent world of the constraint. this.parent = null; // first body of the constraint. this.body1 = null; // second body of the constraint. this.body2 = null; // Internal this.addedToIsland = false; } Object.assign( Constraint.prototype, { Constraint: true, // Prepare for solving the constraint preSolve: function( timeStep, invTimeStep ){ printError("Constraint", "Inheritance error."); }, // Solve the constraint. This is usually called iteratively. solve: function(){ printError("Constraint", "Inheritance error."); }, // Do the post-processing. postSolve: function(){ printError("Constraint", "Inheritance error."); } }); function JointLink ( joint ){ // The previous joint link. this.prev = null; // The next joint link. this.next = null; // The other rigid body connected to the joint. this.body = null; // The joint of the link. this.joint = joint; } /** * Joints are used to constrain the motion between two rigid bodies. * * @author saharan * @author lo-th */ function Joint ( config ){ Constraint.call( this ); this.scale = 1; this.invScale = 1; // joint name this.name = ""; this.id = NaN; // The type of the joint. this.type = JOINT_NULL; // The previous joint in the world. this.prev = null; // The next joint in the world. this.next = null; this.body1 = config.body1; this.body2 = config.body2; // anchor point on the first rigid body in local coordinate system. this.localAnchorPoint1 = new Vec3().copy( config.localAnchorPoint1 ); // anchor point on the second rigid body in local coordinate system. this.localAnchorPoint2 = new Vec3().copy( config.localAnchorPoint2 ); // anchor point on the first rigid body in world coordinate system relative to the body's origin. this.relativeAnchorPoint1 = new Vec3(); // anchor point on the second rigid body in world coordinate system relative to the body's origin. this.relativeAnchorPoint2 = new Vec3(); // anchor point on the first rigid body in world coordinate system. this.anchorPoint1 = new Vec3(); // anchor point on the second rigid body in world coordinate system. this.anchorPoint2 = new Vec3(); // Whether allow collision between connected rigid bodies or not. this.allowCollision = config.allowCollision; this.b1Link = new JointLink( this ); this.b2Link = new JointLink( this ); } Joint.prototype = Object.assign( Object.create( Constraint.prototype ), { constructor: Joint, setId: function ( n ) { this.id = i; }, setParent: function ( world ) { this.parent = world; this.scale = this.parent.scale; this.invScale = this.parent.invScale; this.id = this.parent.numJoints; if( !this.name ) this.name = 'J' + this.id; }, // Update all the anchor points. updateAnchorPoints: function () { this.relativeAnchorPoint1.copy( this.localAnchorPoint1 ).applyMatrix3( this.body1.rotation, true ); this.relativeAnchorPoint2.copy( this.localAnchorPoint2 ).applyMatrix3( this.body2.rotation, true ); this.anchorPoint1.add( this.relativeAnchorPoint1, this.body1.position ); this.anchorPoint2.add( this.relativeAnchorPoint2, this.body2.position ); }, // Attach the joint from the bodies. attach: function ( isX ) { this.b1Link.body = this.body2; this.b2Link.body = this.body1; if(isX){ this.body1.jointLink.push( this.b1Link ); this.body2.jointLink.push( this.b2Link ); } else { if(this.body1.jointLink != null) (this.b1Link.next=this.body1.jointLink).prev = this.b1Link; else this.b1Link.next = null; this.body1.jointLink = this.b1Link; this.body1.numJoints++; if(this.body2.jointLink != null) (this.b2Link.next=this.body2.jointLink).prev = this.b2Link; else this.b2Link.next = null; this.body2.jointLink = this.b2Link; this.body2.numJoints++; } }, // Detach the joint from the bodies. detach: function ( isX ) { if( isX ){ this.body1.jointLink.splice( this.body1.jointLink.indexOf( this.b1Link ), 1 ); this.body2.jointLink.splice( this.body2.jointLink.indexOf( this.b2Link ), 1 ); } else { var prev = this.b1Link.prev; var next = this.b1Link.next; if(prev != null) prev.next = next; if(next != null) next.prev = prev; if(this.body1.jointLink == this.b1Link) this.body1.jointLink = next; this.b1Link.prev = null; this.b1Link.next = null; this.b1Link.body = null; this.body1.numJoints--; prev = this.b2Link.prev; next = this.b2Link.next; if(prev != null) prev.next = next; if(next != null) next.prev = prev; if(this.body2.jointLink==this.b2Link) this.body2.jointLink = next; this.b2Link.prev = null; this.b2Link.next = null; this.b2Link.body = null; this.body2.numJoints--; } this.b1Link.body = null; this.b2Link.body = null; }, // Awake the bodies. awake: function () { this.body1.awake(); this.body2.awake(); }, // calculation function preSolve: function ( timeStep, invTimeStep ) { }, solve: function () { }, postSolve: function () { }, // Delete process remove: function () { this.dispose(); }, dispose: function () { this.parent.removeJoint( this ); }, // Three js add getPosition: function () { var p1 = new Vec3().scale( this.anchorPoint1, this.scale ); var p2 = new Vec3().scale( this.anchorPoint2, this.scale ); return [ p1, p2 ]; } }); /** * A linear constraint for all axes for various joints. * @author saharan */ function LinearConstraint ( joint ){ this.m1=NaN; this.m2=NaN; this.ii1 = null; this.ii2 = null; this.dd = null; this.r1x=NaN; this.r1y=NaN; this.r1z=NaN; this.r2x=NaN; this.r2y=NaN; this.r2z=NaN; this.ax1x=NaN; this.ax1y=NaN; this.ax1z=NaN; this.ay1x=NaN; this.ay1y=NaN; this.ay1z=NaN; this.az1x=NaN; this.az1y=NaN; this.az1z=NaN; this.ax2x=NaN; this.ax2y=NaN; this.ax2z=NaN; this.ay2x=NaN; this.ay2y=NaN; this.ay2z=NaN; this.az2x=NaN; this.az2y=NaN; this.az2z=NaN; this.vel=NaN; this.velx=NaN; this.vely=NaN; this.velz=NaN; this.joint = joint; this.r1 = joint.relativeAnchorPoint1; this.r2 = joint.relativeAnchorPoint2; this.p1 = joint.anchorPoint1; this.p2 = joint.anchorPoint2; this.b1 = joint.body1; this.b2 = joint.body2; this.l1 = this.b1.linearVelocity; this.l2 = this.b2.linearVelocity; this.a1 = this.b1.angularVelocity; this.a2 = this.b2.angularVelocity; this.i1 = this.b1.inverseInertia; this.i2 = this.b2.inverseInertia; this.impx = 0; this.impy = 0; this.impz = 0; } Object.assign( LinearConstraint.prototype, { LinearConstraint: true, preSolve: function ( timeStep, invTimeStep ) { this.r1x = this.r1.x; this.r1y = this.r1.y; this.r1z = this.r1.z; this.r2x = this.r2.x; this.r2y = this.r2.y; this.r2z = this.r2.z; this.m1 = this.b1.inverseMass; this.m2 = this.b2.inverseMass; this.ii1 = this.i1.clone(); this.ii2 = this.i2.clone(); var ii1 = this.ii1.elements; var ii2 = this.ii2.elements; this.ax1x = this.r1z*ii1[1]+-this.r1y*ii1[2]; this.ax1y = this.r1z*ii1[4]+-this.r1y*ii1[5]; this.ax1z = this.r1z*ii1[7]+-this.r1y*ii1[8]; this.ay1x = -this.r1z*ii1[0]+this.r1x*ii1[2]; this.ay1y = -this.r1z*ii1[3]+this.r1x*ii1[5]; this.ay1z = -this.r1z*ii1[6]+this.r1x*ii1[8]; this.az1x = this.r1y*ii1[0]+-this.r1x*ii1[1]; this.az1y = this.r1y*ii1[3]+-this.r1x*ii1[4]; this.az1z = this.r1y*ii1[6]+-this.r1x*ii1[7]; this.ax2x = this.r2z*ii2[1]+-this.r2y*ii2[2]; this.ax2y = this.r2z*ii2[4]+-this.r2y*ii2[5]; this.ax2z = this.r2z*ii2[7]+-this.r2y*ii2[8]; this.ay2x = -this.r2z*ii2[0]+this.r2x*ii2[2]; this.ay2y = -this.r2z*ii2[3]+this.r2x*ii2[5]; this.ay2z = -this.r2z*ii2[6]+this.r2x*ii2[8]; this.az2x = this.r2y*ii2[0]+-this.r2x*ii2[1]; this.az2y = this.r2y*ii2[3]+-this.r2x*ii2[4]; this.az2z = this.r2y*ii2[6]+-this.r2x*ii2[7]; // calculate point-to-point mass matrix // from impulse equation // // M = ([/m] - [r^][/I][r^]) ^ -1 // // where // // [/m] = |1/m, 0, 0| // |0, 1/m, 0| // |0, 0, 1/m| // // [r^] = |0, -rz, ry| // |rz, 0, -rx| // |-ry, rx, 0| // // [/I] = Inverted moment inertia var rxx = this.m1+this.m2; var kk = new Mat33().set( rxx, 0, 0, 0, rxx, 0, 0, 0, rxx ); var k = kk.elements; k[0] += ii1[4]*this.r1z*this.r1z-(ii1[7]+ii1[5])*this.r1y*this.r1z+ii1[8]*this.r1y*this.r1y; k[1] += (ii1[6]*this.r1y+ii1[5]*this.r1x)*this.r1z-ii1[3]*this.r1z*this.r1z-ii1[8]*this.r1x*this.r1y; k[2] += (ii1[3]*this.r1y-ii1[4]*this.r1x)*this.r1z-ii1[6]*this.r1y*this.r1y+ii1[7]*this.r1x*this.r1y; k[3] += (ii1[2]*this.r1y+ii1[7]*this.r1x)*this.r1z-ii1[1]*this.r1z*this.r1z-ii1[8]*this.r1x*this.r1y; k[4] += ii1[0]*this.r1z*this.r1z-(ii1[6]+ii1[2])*this.r1x*this.r1z+ii1[8]*this.r1x*this.r1x; k[5] += (ii1[1]*this.r1x-ii1[0]*this.r1y)*this.r1z-ii1[7]*this.r1x*this.r1x+ii1[6]*this.r1x*this.r1y; k[6] += (ii1[1]*this.r1y-ii1[4]*this.r1x)*this.r1z-ii1[2]*this.r1y*this.r1y+ii1[5]*this.r1x*this.r1y; k[7] += (ii1[3]*this.r1x-ii1[0]*this.r1y)*this.r1z-ii1[5]*this.r1x*this.r1x+ii1[2]*this.r1x*this.r1y; k[8] += ii1[0]*this.r1y*this.r1y-(ii1[3]+ii1[1])*this.r1x*this.r1y+ii1[4]*this.r1x*this.r1x; k[0] += ii2[4]*this.r2z*this.r2z-(ii2[7]+ii2[5])*this.r2y*this.r2z+ii2[8]*this.r2y*this.r2y; k[1] += (ii2[6]*this.r2y+ii2[5]*this.r2x)*this.r2z-ii2[3]*this.r2z*this.r2z-ii2[8]*this.r2x*this.r2y; k[2] += (ii2[3]*this.r2y-ii2[4]*this.r2x)*this.r2z-ii2[6]*this.r2y*this.r2y+ii2[7]*this.r2x*this.r2y; k[3] += (ii2[2]*this.r2y+ii2[7]*this.r2x)*this.r2z-ii2[1]*this.r2z*this.r2z-ii2[8]*this.r2x*this.r2y; k[4] += ii2[0]*this.r2z*this.r2z-(ii2[6]+ii2[2])*this.r2x*this.r2z+ii2[8]*this.r2x*this.r2x; k[5] += (ii2[1]*this.r2x-ii2[0]*this.r2y)*this.r2z-ii2[7]*this.r2x*this.r2x+ii2[6]*this.r2x*this.r2y; k[6] += (ii2[1]*this.r2y-ii2[4]*this.r2x)*this.r2z-ii2[2]*this.r2y*this.r2y+ii2[5]*this.r2x*this.r2y; k[7] += (ii2[3]*this.r2x-ii2[0]*this.r2y)*this.r2z-ii2[5]*this.r2x*this.r2x+ii2[2]*this.r2x*this.r2y; k[8] += ii2[0]*this.r2y*this.r2y-(ii2[3]+ii2[1])*this.r2x*this.r2y+ii2[4]*this.r2x*this.r2x; var inv=1/( k[0]*(k[4]*k[8]-k[7]*k[5]) + k[3]*(k[7]*k[2]-k[1]*k[8]) + k[6]*(k[1]*k[5]-k[4]*k[2]) ); this.dd = new Mat33().set( k[4]*k[8]-k[5]*k[7], k[2]*k[7]-k[1]*k[8], k[1]*k[5]-k[2]*k[4], k[5]*k[6]-k[3]*k[8], k[0]*k[8]-k[2]*k[6], k[2]*k[3]-k[0]*k[5], k[3]*k[7]-k[4]*k[6], k[1]*k[6]-k[0]*k[7], k[0]*k[4]-k[1]*k[3] ).scaleEqual( inv ); this.velx = this.p2.x-this.p1.x; this.vely = this.p2.y-this.p1.y; this.velz = this.p2.z-this.p1.z; var len = _Math.sqrt(this.velx*this.velx+this.vely*this.vely+this.velz*this.velz); if(len>0.005){ len = (0.005-len)/len*invTimeStep*0.05; this.velx *= len; this.vely *= len; this.velz *= len; }else{ this.velx = 0; this.vely = 0; this.velz = 0; } this.impx *= 0.95; this.impy *= 0.95; this.impz *= 0.95; this.l1.x += this.impx*this.m1; this.l1.y += this.impy*this.m1; this.l1.z += this.impz*this.m1; this.a1.x += this.impx*this.ax1x+this.impy*this.ay1x+this.impz*this.az1x; this.a1.y += this.impx*this.ax1y+this.impy*this.ay1y+this.impz*this.az1y; this.a1.z += this.impx*this.ax1z+this.impy*this.ay1z+this.impz*this.az1z; this.l2.x -= this.impx*this.m2; this.l2.y -= this.impy*this.m2; this.l2.z -= this.impz*this.m2; this.a2.x -= this.impx*this.ax2x+this.impy*this.ay2x+this.impz*this.az2x; this.a2.y -= this.impx*this.ax2y+this.impy*this.ay2y+this.impz*this.az2y; this.a2.z -= this.impx*this.ax2z+this.impy*this.ay2z+this.impz*this.az2z; }, solve: function () { var d = this.dd.elements; var rvx = this.l2.x-this.l1.x+this.a2.y*this.r2z-this.a2.z*this.r2y-this.a1.y*this.r1z+this.a1.z*this.r1y-this.velx; var rvy = this.l2.y-this.l1.y+this.a2.z*this.r2x-this.a2.x*this.r2z-this.a1.z*this.r1x+this.a1.x*this.r1z-this.vely; var rvz = this.l2.z-this.l1.z+this.a2.x*this.r2y-this.a2.y*this.r2x-this.a1.x*this.r1y+this.a1.y*this.r1x-this.velz; var nimpx = rvx*d[0]+rvy*d[1]+rvz*d[2]; var nimpy = rvx*d[3]+rvy*d[4]+rvz*d[5]; var nimpz = rvx*d[6]+rvy*d[7]+rvz*d[8]; this.impx += nimpx; this.impy += nimpy; this.impz += nimpz; this.l1.x += nimpx*this.m1; this.l1.y += nimpy*this.m1; this.l1.z += nimpz*this.m1; this.a1.x += nimpx*this.ax1x+nimpy*this.ay1x+nimpz*this.az1x; this.a1.y += nimpx*this.ax1y+nimpy*this.ay1y+nimpz*this.az1y; this.a1.z += nimpx*this.ax1z+nimpy*this.ay1z+nimpz*this.az1z; this.l2.x -= nimpx*this.m2; this.l2.y -= nimpy*this.m2; this.l2.z -= nimpz*this.m2; this.a2.x -= nimpx*this.ax2x+nimpy*this.ay2x+nimpz*this.az2x; this.a2.y -= nimpx*this.ax2y+nimpy*this.ay2y+nimpz*this.az2y; this.a2.z -= nimpx*this.ax2z+nimpy*this.ay2z+nimpz*this.az2z; } } ); /** * A three-axis rotational constraint for various joints. * @author saharan */ function Rotational3Constraint ( joint, limitMotor1, limitMotor2, limitMotor3 ) { this.cfm1=NaN; this.cfm2=NaN; this.cfm3=NaN; this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.ax1=NaN; this.ay1=NaN; this.az1=NaN; this.ax2=NaN; this.ay2=NaN; this.az2=NaN; this.ax3=NaN; this.ay3=NaN; this.az3=NaN; this.a1x1=NaN; // jacoians this.a1y1=NaN; this.a1z1=NaN; this.a2x1=NaN; this.a2y1=NaN; this.a2z1=NaN; this.a1x2=NaN; this.a1y2=NaN; this.a1z2=NaN; this.a2x2=NaN; this.a2y2=NaN; this.a2z2=NaN; this.a1x3=NaN; this.a1y3=NaN; this.a1z3=NaN; this.a2x3=NaN; this.a2y3=NaN; this.a2z3=NaN; this.lowerLimit1=NaN; this.upperLimit1=NaN; this.limitVelocity1=NaN; this.limitState1=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor1=false; this.motorSpeed1=NaN; this.maxMotorForce1=NaN; this.maxMotorImpulse1=NaN; this.lowerLimit2=NaN; this.upperLimit2=NaN; this.limitVelocity2=NaN; this.limitState2=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor2=false; this.motorSpeed2=NaN; this.maxMotorForce2=NaN; this.maxMotorImpulse2=NaN; this.lowerLimit3=NaN; this.upperLimit3=NaN; this.limitVelocity3=NaN; this.limitState3=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor3=false; this.motorSpeed3=NaN; this.maxMotorForce3=NaN; this.maxMotorImpulse3=NaN; this.k00=NaN; // K = J*M*JT this.k01=NaN; this.k02=NaN; this.k10=NaN; this.k11=NaN; this.k12=NaN; this.k20=NaN; this.k21=NaN; this.k22=NaN; this.kv00=NaN; // diagonals without CFMs this.kv11=NaN; this.kv22=NaN; this.dv00=NaN; // ...inverted this.dv11=NaN; this.dv22=NaN; this.d00=NaN; // K^-1 this.d01=NaN; this.d02=NaN; this.d10=NaN; this.d11=NaN; this.d12=NaN; this.d20=NaN; this.d21=NaN; this.d22=NaN; this.limitMotor1=limitMotor1; this.limitMotor2=limitMotor2; this.limitMotor3=limitMotor3; this.b1=joint.body1; this.b2=joint.body2; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.limitImpulse1=0; this.motorImpulse1=0; this.limitImpulse2=0; this.motorImpulse2=0; this.limitImpulse3=0; this.motorImpulse3=0; } Object.assign( Rotational3Constraint.prototype, { Rotational3Constraint: true, preSolve: function( timeStep, invTimeStep ){ this.ax1=this.limitMotor1.axis.x; this.ay1=this.limitMotor1.axis.y; this.az1=this.limitMotor1.axis.z; this.ax2=this.limitMotor2.axis.x; this.ay2=this.limitMotor2.axis.y; this.az2=this.limitMotor2.axis.z; this.ax3=this.limitMotor3.axis.x; this.ay3=this.limitMotor3.axis.y; this.az3=this.limitMotor3.axis.z; this.lowerLimit1=this.limitMotor1.lowerLimit; this.upperLimit1=this.limitMotor1.upperLimit; this.motorSpeed1=this.limitMotor1.motorSpeed; this.maxMotorForce1=this.limitMotor1.maxMotorForce; this.enableMotor1=this.maxMotorForce1>0; this.lowerLimit2=this.limitMotor2.lowerLimit; this.upperLimit2=this.limitMotor2.upperLimit; this.motorSpeed2=this.limitMotor2.motorSpeed; this.maxMotorForce2=this.limitMotor2.maxMotorForce; this.enableMotor2=this.maxMotorForce2>0; this.lowerLimit3=this.limitMotor3.lowerLimit; this.upperLimit3=this.limitMotor3.upperLimit; this.motorSpeed3=this.limitMotor3.motorSpeed; this.maxMotorForce3=this.limitMotor3.maxMotorForce; this.enableMotor3=this.maxMotorForce3>0; var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; var frequency1=this.limitMotor1.frequency; var frequency2=this.limitMotor2.frequency; var frequency3=this.limitMotor3.frequency; var enableSpring1=frequency1>0; var enableSpring2=frequency2>0; var enableSpring3=frequency3>0; var enableLimit1=this.lowerLimit1<=this.upperLimit1; var enableLimit2=this.lowerLimit2<=this.upperLimit2; var enableLimit3=this.lowerLimit3<=this.upperLimit3; var angle1=this.limitMotor1.angle; if(enableLimit1){ if(this.lowerLimit1==this.upperLimit1){ if(this.limitState1!=0){ this.limitState1=0; this.limitImpulse1=0; } this.limitVelocity1=this.lowerLimit1-angle1; }else if(angle1this.upperLimit1){ if(this.limitState1!=1){ this.limitState1=1; this.limitImpulse1=0; } this.limitVelocity1=this.upperLimit1-angle1; }else{ this.limitState1=2; this.limitImpulse1=0; this.limitVelocity1=0; } if(!enableSpring1){ if(this.limitVelocity1>0.02)this.limitVelocity1-=0.02; else if(this.limitVelocity1<-0.02)this.limitVelocity1+=0.02; else this.limitVelocity1=0; } }else{ this.limitState1=2; this.limitImpulse1=0; } var angle2=this.limitMotor2.angle; if(enableLimit2){ if(this.lowerLimit2==this.upperLimit2){ if(this.limitState2!=0){ this.limitState2=0; this.limitImpulse2=0; } this.limitVelocity2=this.lowerLimit2-angle2; }else if(angle2this.upperLimit2){ if(this.limitState2!=1){ this.limitState2=1; this.limitImpulse2=0; } this.limitVelocity2=this.upperLimit2-angle2; }else{ this.limitState2=2; this.limitImpulse2=0; this.limitVelocity2=0; } if(!enableSpring2){ if(this.limitVelocity2>0.02)this.limitVelocity2-=0.02; else if(this.limitVelocity2<-0.02)this.limitVelocity2+=0.02; else this.limitVelocity2=0; } }else{ this.limitState2=2; this.limitImpulse2=0; } var angle3=this.limitMotor3.angle; if(enableLimit3){ if(this.lowerLimit3==this.upperLimit3){ if(this.limitState3!=0){ this.limitState3=0; this.limitImpulse3=0; } this.limitVelocity3=this.lowerLimit3-angle3; }else if(angle3this.upperLimit3){ if(this.limitState3!=1){ this.limitState3=1; this.limitImpulse3=0; } this.limitVelocity3=this.upperLimit3-angle3; }else{ this.limitState3=2; this.limitImpulse3=0; this.limitVelocity3=0; } if(!enableSpring3){ if(this.limitVelocity3>0.02)this.limitVelocity3-=0.02; else if(this.limitVelocity3<-0.02)this.limitVelocity3+=0.02; else this.limitVelocity3=0; } }else{ this.limitState3=2; this.limitImpulse3=0; } if(this.enableMotor1&&(this.limitState1!=0||enableSpring1)){ this.maxMotorImpulse1=this.maxMotorForce1*timeStep; }else{ this.motorImpulse1=0; this.maxMotorImpulse1=0; } if(this.enableMotor2&&(this.limitState2!=0||enableSpring2)){ this.maxMotorImpulse2=this.maxMotorForce2*timeStep; }else{ this.motorImpulse2=0; this.maxMotorImpulse2=0; } if(this.enableMotor3&&(this.limitState3!=0||enableSpring3)){ this.maxMotorImpulse3=this.maxMotorForce3*timeStep; }else{ this.motorImpulse3=0; this.maxMotorImpulse3=0; } // build jacobians this.a1x1=this.ax1*this.i1e00+this.ay1*this.i1e01+this.az1*this.i1e02; this.a1y1=this.ax1*this.i1e10+this.ay1*this.i1e11+this.az1*this.i1e12; this.a1z1=this.ax1*this.i1e20+this.ay1*this.i1e21+this.az1*this.i1e22; this.a2x1=this.ax1*this.i2e00+this.ay1*this.i2e01+this.az1*this.i2e02; this.a2y1=this.ax1*this.i2e10+this.ay1*this.i2e11+this.az1*this.i2e12; this.a2z1=this.ax1*this.i2e20+this.ay1*this.i2e21+this.az1*this.i2e22; this.a1x2=this.ax2*this.i1e00+this.ay2*this.i1e01+this.az2*this.i1e02; this.a1y2=this.ax2*this.i1e10+this.ay2*this.i1e11+this.az2*this.i1e12; this.a1z2=this.ax2*this.i1e20+this.ay2*this.i1e21+this.az2*this.i1e22; this.a2x2=this.ax2*this.i2e00+this.ay2*this.i2e01+this.az2*this.i2e02; this.a2y2=this.ax2*this.i2e10+this.ay2*this.i2e11+this.az2*this.i2e12; this.a2z2=this.ax2*this.i2e20+this.ay2*this.i2e21+this.az2*this.i2e22; this.a1x3=this.ax3*this.i1e00+this.ay3*this.i1e01+this.az3*this.i1e02; this.a1y3=this.ax3*this.i1e10+this.ay3*this.i1e11+this.az3*this.i1e12; this.a1z3=this.ax3*this.i1e20+this.ay3*this.i1e21+this.az3*this.i1e22; this.a2x3=this.ax3*this.i2e00+this.ay3*this.i2e01+this.az3*this.i2e02; this.a2y3=this.ax3*this.i2e10+this.ay3*this.i2e11+this.az3*this.i2e12; this.a2z3=this.ax3*this.i2e20+this.ay3*this.i2e21+this.az3*this.i2e22; // build an impulse matrix this.k00=this.ax1*(this.a1x1+this.a2x1)+this.ay1*(this.a1y1+this.a2y1)+this.az1*(this.a1z1+this.a2z1); this.k01=this.ax1*(this.a1x2+this.a2x2)+this.ay1*(this.a1y2+this.a2y2)+this.az1*(this.a1z2+this.a2z2); this.k02=this.ax1*(this.a1x3+this.a2x3)+this.ay1*(this.a1y3+this.a2y3)+this.az1*(this.a1z3+this.a2z3); this.k10=this.ax2*(this.a1x1+this.a2x1)+this.ay2*(this.a1y1+this.a2y1)+this.az2*(this.a1z1+this.a2z1); this.k11=this.ax2*(this.a1x2+this.a2x2)+this.ay2*(this.a1y2+this.a2y2)+this.az2*(this.a1z2+this.a2z2); this.k12=this.ax2*(this.a1x3+this.a2x3)+this.ay2*(this.a1y3+this.a2y3)+this.az2*(this.a1z3+this.a2z3); this.k20=this.ax3*(this.a1x1+this.a2x1)+this.ay3*(this.a1y1+this.a2y1)+this.az3*(this.a1z1+this.a2z1); this.k21=this.ax3*(this.a1x2+this.a2x2)+this.ay3*(this.a1y2+this.a2y2)+this.az3*(this.a1z2+this.a2z2); this.k22=this.ax3*(this.a1x3+this.a2x3)+this.ay3*(this.a1y3+this.a2y3)+this.az3*(this.a1z3+this.a2z3); this.kv00=this.k00; this.kv11=this.k11; this.kv22=this.k22; this.dv00=1/this.kv00; this.dv11=1/this.kv11; this.dv22=1/this.kv22; if(enableSpring1&&this.limitState1!=2){ var omega=6.2831853*frequency1; var k=omega*omega*timeStep; var dmp=invTimeStep/(k+2*this.limitMotor1.dampingRatio*omega); this.cfm1=this.kv00*dmp; this.limitVelocity1*=k*dmp; }else{ this.cfm1=0; this.limitVelocity1*=invTimeStep*0.05; } if(enableSpring2&&this.limitState2!=2){ omega=6.2831853*frequency2; k=omega*omega*timeStep; dmp=invTimeStep/(k+2*this.limitMotor2.dampingRatio*omega); this.cfm2=this.kv11*dmp; this.limitVelocity2*=k*dmp; }else{ this.cfm2=0; this.limitVelocity2*=invTimeStep*0.05; } if(enableSpring3&&this.limitState3!=2){ omega=6.2831853*frequency3; k=omega*omega*timeStep; dmp=invTimeStep/(k+2*this.limitMotor3.dampingRatio*omega); this.cfm3=this.kv22*dmp; this.limitVelocity3*=k*dmp; }else{ this.cfm3=0; this.limitVelocity3*=invTimeStep*0.05; } this.k00+=this.cfm1; this.k11+=this.cfm2; this.k22+=this.cfm3; var inv=1/( this.k00*(this.k11*this.k22-this.k21*this.k12)+ this.k10*(this.k21*this.k02-this.k01*this.k22)+ this.k20*(this.k01*this.k12-this.k11*this.k02) ); this.d00=(this.k11*this.k22-this.k12*this.k21)*inv; this.d01=(this.k02*this.k21-this.k01*this.k22)*inv; this.d02=(this.k01*this.k12-this.k02*this.k11)*inv; this.d10=(this.k12*this.k20-this.k10*this.k22)*inv; this.d11=(this.k00*this.k22-this.k02*this.k20)*inv; this.d12=(this.k02*this.k10-this.k00*this.k12)*inv; this.d20=(this.k10*this.k21-this.k11*this.k20)*inv; this.d21=(this.k01*this.k20-this.k00*this.k21)*inv; this.d22=(this.k00*this.k11-this.k01*this.k10)*inv; this.limitImpulse1*=0.95; this.motorImpulse1*=0.95; this.limitImpulse2*=0.95; this.motorImpulse2*=0.95; this.limitImpulse3*=0.95; this.motorImpulse3*=0.95; var totalImpulse1=this.limitImpulse1+this.motorImpulse1; var totalImpulse2=this.limitImpulse2+this.motorImpulse2; var totalImpulse3=this.limitImpulse3+this.motorImpulse3; this.a1.x+=totalImpulse1*this.a1x1+totalImpulse2*this.a1x2+totalImpulse3*this.a1x3; this.a1.y+=totalImpulse1*this.a1y1+totalImpulse2*this.a1y2+totalImpulse3*this.a1y3; this.a1.z+=totalImpulse1*this.a1z1+totalImpulse2*this.a1z2+totalImpulse3*this.a1z3; this.a2.x-=totalImpulse1*this.a2x1+totalImpulse2*this.a2x2+totalImpulse3*this.a2x3; this.a2.y-=totalImpulse1*this.a2y1+totalImpulse2*this.a2y2+totalImpulse3*this.a2y3; this.a2.z-=totalImpulse1*this.a2z1+totalImpulse2*this.a2z2+totalImpulse3*this.a2z3; }, solve_:function(){ var rvx=this.a2.x-this.a1.x; var rvy=this.a2.y-this.a1.y; var rvz=this.a2.z-this.a1.z; this.limitVelocity3=30; var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1-this.limitVelocity1; var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2-this.limitVelocity2; var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3-this.limitVelocity3; var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02; var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12; var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22; this.limitImpulse1+=dLimitImpulse1; this.limitImpulse2+=dLimitImpulse2; this.limitImpulse3+=dLimitImpulse3; this.a1.x+=dLimitImpulse1*this.a1x1+dLimitImpulse2*this.a1x2+dLimitImpulse3*this.a1x3; this.a1.y+=dLimitImpulse1*this.a1y1+dLimitImpulse2*this.a1y2+dLimitImpulse3*this.a1y3; this.a1.z+=dLimitImpulse1*this.a1z1+dLimitImpulse2*this.a1z2+dLimitImpulse3*this.a1z3; this.a2.x-=dLimitImpulse1*this.a2x1+dLimitImpulse2*this.a2x2+dLimitImpulse3*this.a2x3; this.a2.y-=dLimitImpulse1*this.a2y1+dLimitImpulse2*this.a2y2+dLimitImpulse3*this.a2y3; this.a2.z-=dLimitImpulse1*this.a2z1+dLimitImpulse2*this.a2z2+dLimitImpulse3*this.a2z3; }, solve:function(){ var rvx=this.a2.x-this.a1.x; var rvy=this.a2.y-this.a1.y; var rvz=this.a2.z-this.a1.z; var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1; var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2; var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3; var oldMotorImpulse1=this.motorImpulse1; var oldMotorImpulse2=this.motorImpulse2; var oldMotorImpulse3=this.motorImpulse3; var dMotorImpulse1=0; var dMotorImpulse2=0; var dMotorImpulse3=0; if(this.enableMotor1){ dMotorImpulse1=(rvn1-this.motorSpeed1)*this.dv00; this.motorImpulse1+=dMotorImpulse1; if(this.motorImpulse1>this.maxMotorImpulse1){ // clamp motor impulse this.motorImpulse1=this.maxMotorImpulse1; }else if(this.motorImpulse1<-this.maxMotorImpulse1){ this.motorImpulse1=-this.maxMotorImpulse1; } dMotorImpulse1=this.motorImpulse1-oldMotorImpulse1; } if(this.enableMotor2){ dMotorImpulse2=(rvn2-this.motorSpeed2)*this.dv11; this.motorImpulse2+=dMotorImpulse2; if(this.motorImpulse2>this.maxMotorImpulse2){ // clamp motor impulse this.motorImpulse2=this.maxMotorImpulse2; }else if(this.motorImpulse2<-this.maxMotorImpulse2){ this.motorImpulse2=-this.maxMotorImpulse2; } dMotorImpulse2=this.motorImpulse2-oldMotorImpulse2; } if(this.enableMotor3){ dMotorImpulse3=(rvn3-this.motorSpeed3)*this.dv22; this.motorImpulse3+=dMotorImpulse3; if(this.motorImpulse3>this.maxMotorImpulse3){ // clamp motor impulse this.motorImpulse3=this.maxMotorImpulse3; }else if(this.motorImpulse3<-this.maxMotorImpulse3){ this.motorImpulse3=-this.maxMotorImpulse3; } dMotorImpulse3=this.motorImpulse3-oldMotorImpulse3; } // apply motor impulse to relative velocity rvn1+=dMotorImpulse1*this.kv00+dMotorImpulse2*this.k01+dMotorImpulse3*this.k02; rvn2+=dMotorImpulse1*this.k10+dMotorImpulse2*this.kv11+dMotorImpulse3*this.k12; rvn3+=dMotorImpulse1*this.k20+dMotorImpulse2*this.k21+dMotorImpulse3*this.kv22; // subtract target velocity and applied impulse rvn1-=this.limitVelocity1+this.limitImpulse1*this.cfm1; rvn2-=this.limitVelocity2+this.limitImpulse2*this.cfm2; rvn3-=this.limitVelocity3+this.limitImpulse3*this.cfm3; var oldLimitImpulse1=this.limitImpulse1; var oldLimitImpulse2=this.limitImpulse2; var oldLimitImpulse3=this.limitImpulse3; var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02; var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12; var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22; this.limitImpulse1+=dLimitImpulse1; this.limitImpulse2+=dLimitImpulse2; this.limitImpulse3+=dLimitImpulse3; // clamp var clampState=0; if(this.limitState1==2||this.limitImpulse1*this.limitState1<0){ dLimitImpulse1=-oldLimitImpulse1; rvn2+=dLimitImpulse1*this.k10; rvn3+=dLimitImpulse1*this.k20; clampState|=1; } if(this.limitState2==2||this.limitImpulse2*this.limitState2<0){ dLimitImpulse2=-oldLimitImpulse2; rvn1+=dLimitImpulse2*this.k01; rvn3+=dLimitImpulse2*this.k21; clampState|=2; } if(this.limitState3==2||this.limitImpulse3*this.limitState3<0){ dLimitImpulse3=-oldLimitImpulse3; rvn1+=dLimitImpulse3*this.k02; rvn2+=dLimitImpulse3*this.k12; clampState|=4; } // update un-clamped impulse // TODO: isolate division var det; switch(clampState){ case 1: // update 2 3 det=1/(this.k11*this.k22-this.k12*this.k21); dLimitImpulse2=(this.k22*rvn2+-this.k12*rvn3)*det; dLimitImpulse3=(-this.k21*rvn2+this.k11*rvn3)*det; break; case 2: // update 1 3 det=1/(this.k00*this.k22-this.k02*this.k20); dLimitImpulse1=(this.k22*rvn1+-this.k02*rvn3)*det; dLimitImpulse3=(-this.k20*rvn1+this.k00*rvn3)*det; break; case 3: // update 3 dLimitImpulse3=rvn3/this.k22; break; case 4: // update 1 2 det=1/(this.k00*this.k11-this.k01*this.k10); dLimitImpulse1=(this.k11*rvn1+-this.k01*rvn2)*det; dLimitImpulse2=(-this.k10*rvn1+this.k00*rvn2)*det; break; case 5: // update 2 dLimitImpulse2=rvn2/this.k11; break; case 6: // update 1 dLimitImpulse1=rvn1/this.k00; break; } this.limitImpulse1=dLimitImpulse1+oldLimitImpulse1; this.limitImpulse2=dLimitImpulse2+oldLimitImpulse2; this.limitImpulse3=dLimitImpulse3+oldLimitImpulse3; var dImpulse1=dMotorImpulse1+dLimitImpulse1; var dImpulse2=dMotorImpulse2+dLimitImpulse2; var dImpulse3=dMotorImpulse3+dLimitImpulse3; // apply impulse this.a1.x+=dImpulse1*this.a1x1+dImpulse2*this.a1x2+dImpulse3*this.a1x3; this.a1.y+=dImpulse1*this.a1y1+dImpulse2*this.a1y2+dImpulse3*this.a1y3; this.a1.z+=dImpulse1*this.a1z1+dImpulse2*this.a1z2+dImpulse3*this.a1z3; this.a2.x-=dImpulse1*this.a2x1+dImpulse2*this.a2x2+dImpulse3*this.a2x3; this.a2.y-=dImpulse1*this.a2y1+dImpulse2*this.a2y2+dImpulse3*this.a2y3; this.a2.z-=dImpulse1*this.a2z1+dImpulse2*this.a2z2+dImpulse3*this.a2z3; rvx=this.a2.x-this.a1.x; rvy=this.a2.y-this.a1.y; rvz=this.a2.z-this.a1.z; rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2; } } ); /** * A hinge joint allows only for relative rotation of rigid bodies along the axis. * * @author saharan * @author lo-th */ function HingeJoint ( config, lowerAngleLimit, upperAngleLimit ) { Joint.call( this, config ); this.type = JOINT_HINGE; // The axis in the first body's coordinate system. this.localAxis1 = config.localAxis1.clone().normalize(); // The axis in the second body's coordinate system. this.localAxis2 = config.localAxis2.clone().normalize(); // make angle axis var arc = new Mat33().setQuat( new Quat().setFromUnitVectors( this.localAxis1, this.localAxis2 ) ); this.localAngle1 = new Vec3().tangent( this.localAxis1 ).normalize(); this.localAngle2 = this.localAngle1.clone().applyMatrix3( arc, true ); this.ax1 = new Vec3(); this.ax2 = new Vec3(); this.an1 = new Vec3(); this.an2 = new Vec3(); this.tmp = new Vec3(); this.nor = new Vec3(); this.tan = new Vec3(); this.bin = new Vec3(); // The rotational limit and motor information of the joint. this.limitMotor = new LimitMotor( this.nor, false ); this.limitMotor.lowerLimit = lowerAngleLimit; this.limitMotor.upperLimit = upperAngleLimit; this.lc = new LinearConstraint( this ); this.r3 = new Rotational3Constraint( this, this.limitMotor, new LimitMotor( this.tan, true ), new LimitMotor( this.bin, true ) ); } HingeJoint.prototype = Object.assign( Object.create( Joint.prototype ), { constructor: HingeJoint, preSolve: function ( timeStep, invTimeStep ) { this.updateAnchorPoints(); this.ax1.copy( this.localAxis1 ).applyMatrix3( this.body1.rotation, true ); this.ax2.copy( this.localAxis2 ).applyMatrix3( this.body2.rotation, true ); this.an1.copy( this.localAngle1 ).applyMatrix3( this.body1.rotation, true ); this.an2.copy( this.localAngle2 ).applyMatrix3( this.body2.rotation, true ); // normal tangent binormal this.nor.set( this.ax1.x*this.body2.inverseMass + this.ax2.x*this.body1.inverseMass, this.ax1.y*this.body2.inverseMass + this.ax2.y*this.body1.inverseMass, this.ax1.z*this.body2.inverseMass + this.ax2.z*this.body1.inverseMass ).normalize(); this.tan.tangent( this.nor ).normalize(); this.bin.crossVectors( this.nor, this.tan ); // calculate hinge angle var limite = _Math.acosClamp( _Math.dotVectors( this.an1, this.an2 ) ); this.tmp.crossVectors( this.an1, this.an2 ); if( _Math.dotVectors( this.nor, this.tmp ) < 0 ) this.limitMotor.angle = -limite; else this.limitMotor.angle = limite; this.tmp.crossVectors( this.ax1, this.ax2 ); this.r3.limitMotor2.angle = _Math.dotVectors( this.tan, this.tmp ); this.r3.limitMotor3.angle = _Math.dotVectors( this.bin, this.tmp ); // preSolve this.r3.preSolve( timeStep, invTimeStep ); this.lc.preSolve( timeStep, invTimeStep ); }, solve: function () { this.r3.solve(); this.lc.solve(); }, postSolve: function () { } }); /** * A ball-and-socket joint limits relative translation on two anchor points on rigid bodies. * * @author saharan * @author lo-th */ function BallAndSocketJoint ( config ){ Joint.call( this, config ); this.type = JOINT_BALL_AND_SOCKET; this.lc = new LinearConstraint( this ); } BallAndSocketJoint.prototype = Object.assign( Object.create( Joint.prototype ), { constructor: BallAndSocketJoint, preSolve: function ( timeStep, invTimeStep ) { this.updateAnchorPoints(); // preSolve this.lc.preSolve( timeStep, invTimeStep ); }, solve: function () { this.lc.solve(); }, postSolve: function () { } }); /** * A translational constraint for various joints. * @author saharan */ function TranslationalConstraint ( joint, limitMotor ){ this.cfm=NaN; this.m1=NaN; this.m2=NaN; this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.motorDenom=NaN; this.invMotorDenom=NaN; this.invDenom=NaN; this.ax=NaN; this.ay=NaN; this.az=NaN; this.r1x=NaN; this.r1y=NaN; this.r1z=NaN; this.r2x=NaN; this.r2y=NaN; this.r2z=NaN; this.t1x=NaN; this.t1y=NaN; this.t1z=NaN; this.t2x=NaN; this.t2y=NaN; this.t2z=NaN; this.l1x=NaN; this.l1y=NaN; this.l1z=NaN; this.l2x=NaN; this.l2y=NaN; this.l2z=NaN; this.a1x=NaN; this.a1y=NaN; this.a1z=NaN; this.a2x=NaN; this.a2y=NaN; this.a2z=NaN; this.lowerLimit=NaN; this.upperLimit=NaN; this.limitVelocity=NaN; this.limitState=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor=false; this.motorSpeed=NaN; this.maxMotorForce=NaN; this.maxMotorImpulse=NaN; this.limitMotor=limitMotor; this.b1=joint.body1; this.b2=joint.body2; this.p1=joint.anchorPoint1; this.p2=joint.anchorPoint2; this.r1=joint.relativeAnchorPoint1; this.r2=joint.relativeAnchorPoint2; this.l1=this.b1.linearVelocity; this.l2=this.b2.linearVelocity; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.limitImpulse=0; this.motorImpulse=0; } Object.assign( TranslationalConstraint.prototype, { TranslationalConstraint: true, preSolve:function(timeStep,invTimeStep){ this.ax=this.limitMotor.axis.x; this.ay=this.limitMotor.axis.y; this.az=this.limitMotor.axis.z; this.lowerLimit=this.limitMotor.lowerLimit; this.upperLimit=this.limitMotor.upperLimit; this.motorSpeed=this.limitMotor.motorSpeed; this.maxMotorForce=this.limitMotor.maxMotorForce; this.enableMotor=this.maxMotorForce>0; this.m1=this.b1.inverseMass; this.m2=this.b2.inverseMass; var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; var dx=this.p2.x-this.p1.x; var dy=this.p2.y-this.p1.y; var dz=this.p2.z-this.p1.z; var d=dx*this.ax+dy*this.ay+dz*this.az; var frequency=this.limitMotor.frequency; var enableSpring=frequency>0; var enableLimit=this.lowerLimit<=this.upperLimit; if(enableSpring&&d>20||d<-20){ enableSpring=false; } if(enableLimit){ if(this.lowerLimit==this.upperLimit){ if(this.limitState!=0){ this.limitState=0; this.limitImpulse=0; } this.limitVelocity=this.lowerLimit-d; if(!enableSpring)d=this.lowerLimit; }else if(dthis.upperLimit){ if(this.limitState!=1){ this.limitState=1; this.limitImpulse=0; } this.limitVelocity=this.upperLimit-d; if(!enableSpring)d=this.upperLimit; }else{ this.limitState=2; this.limitImpulse=0; this.limitVelocity=0; } if(!enableSpring){ if(this.limitVelocity>0.005)this.limitVelocity-=0.005; else if(this.limitVelocity<-0.005)this.limitVelocity+=0.005; else this.limitVelocity=0; } }else{ this.limitState=2; this.limitImpulse=0; } if(this.enableMotor&&(this.limitState!=0||enableSpring)){ this.maxMotorImpulse=this.maxMotorForce*timeStep; }else{ this.motorImpulse=0; this.maxMotorImpulse=0; } var rdx=d*this.ax; var rdy=d*this.ay; var rdz=d*this.az; var w1=this.m1/(this.m1+this.m2); var w2=1-w1; this.r1x=this.r1.x+rdx*w1; this.r1y=this.r1.y+rdy*w1; this.r1z=this.r1.z+rdz*w1; this.r2x=this.r2.x-rdx*w2; this.r2y=this.r2.y-rdy*w2; this.r2z=this.r2.z-rdz*w2; this.t1x=this.r1y*this.az-this.r1z*this.ay; this.t1y=this.r1z*this.ax-this.r1x*this.az; this.t1z=this.r1x*this.ay-this.r1y*this.ax; this.t2x=this.r2y*this.az-this.r2z*this.ay; this.t2y=this.r2z*this.ax-this.r2x*this.az; this.t2z=this.r2x*this.ay-this.r2y*this.ax; this.l1x=this.ax*this.m1; this.l1y=this.ay*this.m1; this.l1z=this.az*this.m1; this.l2x=this.ax*this.m2; this.l2y=this.ay*this.m2; this.l2z=this.az*this.m2; this.a1x=this.t1x*this.i1e00+this.t1y*this.i1e01+this.t1z*this.i1e02; this.a1y=this.t1x*this.i1e10+this.t1y*this.i1e11+this.t1z*this.i1e12; this.a1z=this.t1x*this.i1e20+this.t1y*this.i1e21+this.t1z*this.i1e22; this.a2x=this.t2x*this.i2e00+this.t2y*this.i2e01+this.t2z*this.i2e02; this.a2y=this.t2x*this.i2e10+this.t2y*this.i2e11+this.t2z*this.i2e12; this.a2z=this.t2x*this.i2e20+this.t2y*this.i2e21+this.t2z*this.i2e22; this.motorDenom= this.m1+this.m2+ this.ax*(this.a1y*this.r1z-this.a1z*this.r1y+this.a2y*this.r2z-this.a2z*this.r2y)+ this.ay*(this.a1z*this.r1x-this.a1x*this.r1z+this.a2z*this.r2x-this.a2x*this.r2z)+ this.az*(this.a1x*this.r1y-this.a1y*this.r1x+this.a2x*this.r2y-this.a2y*this.r2x); this.invMotorDenom=1/this.motorDenom; if(enableSpring&&this.limitState!=2){ var omega=6.2831853*frequency; var k=omega*omega*timeStep; var dmp=invTimeStep/(k+2*this.limitMotor.dampingRatio*omega); this.cfm=this.motorDenom*dmp; this.limitVelocity*=k*dmp; }else{ this.cfm=0; this.limitVelocity*=invTimeStep*0.05; } this.invDenom=1/(this.motorDenom+this.cfm); var totalImpulse=this.limitImpulse+this.motorImpulse; this.l1.x+=totalImpulse*this.l1x; this.l1.y+=totalImpulse*this.l1y; this.l1.z+=totalImpulse*this.l1z; this.a1.x+=totalImpulse*this.a1x; this.a1.y+=totalImpulse*this.a1y; this.a1.z+=totalImpulse*this.a1z; this.l2.x-=totalImpulse*this.l2x; this.l2.y-=totalImpulse*this.l2y; this.l2.z-=totalImpulse*this.l2z; this.a2.x-=totalImpulse*this.a2x; this.a2.y-=totalImpulse*this.a2y; this.a2.z-=totalImpulse*this.a2z; }, solve:function(){ var rvn= this.ax*(this.l2.x-this.l1.x)+this.ay*(this.l2.y-this.l1.y)+this.az*(this.l2.z-this.l1.z)+ this.t2x*this.a2.x-this.t1x*this.a1.x+this.t2y*this.a2.y-this.t1y*this.a1.y+this.t2z*this.a2.z-this.t1z*this.a1.z; // motor part var newMotorImpulse; if(this.enableMotor){ newMotorImpulse=(rvn-this.motorSpeed)*this.invMotorDenom; var oldMotorImpulse=this.motorImpulse; this.motorImpulse+=newMotorImpulse; if(this.motorImpulse>this.maxMotorImpulse)this.motorImpulse=this.maxMotorImpulse; else if(this.motorImpulse<-this.maxMotorImpulse)this.motorImpulse=-this.maxMotorImpulse; newMotorImpulse=this.motorImpulse-oldMotorImpulse; rvn-=newMotorImpulse*this.motorDenom; }else newMotorImpulse=0; // limit part var newLimitImpulse; if(this.limitState!=2){ newLimitImpulse=(rvn-this.limitVelocity-this.limitImpulse*this.cfm)*this.invDenom; var oldLimitImpulse=this.limitImpulse; this.limitImpulse+=newLimitImpulse; if(this.limitImpulse*this.limitState<0)this.limitImpulse=0; newLimitImpulse=this.limitImpulse-oldLimitImpulse; }else newLimitImpulse=0; var totalImpulse=newLimitImpulse+newMotorImpulse; this.l1.x+=totalImpulse*this.l1x; this.l1.y+=totalImpulse*this.l1y; this.l1.z+=totalImpulse*this.l1z; this.a1.x+=totalImpulse*this.a1x; this.a1.y+=totalImpulse*this.a1y; this.a1.z+=totalImpulse*this.a1z; this.l2.x-=totalImpulse*this.l2x; this.l2.y-=totalImpulse*this.l2y; this.l2.z-=totalImpulse*this.l2z; this.a2.x-=totalImpulse*this.a2x; this.a2.y-=totalImpulse*this.a2y; this.a2.z-=totalImpulse*this.a2z; } } ); /** * A distance joint limits the distance between two anchor points on rigid bodies. * * @author saharan * @author lo-th */ function DistanceJoint ( config, minDistance, maxDistance ){ Joint.call( this, config ); this.type = JOINT_DISTANCE; this.nor = new Vec3(); // The limit and motor information of the joint. this.limitMotor = new LimitMotor( this.nor, true ); this.limitMotor.lowerLimit = minDistance; this.limitMotor.upperLimit = maxDistance; this.t = new TranslationalConstraint( this, this.limitMotor ); } DistanceJoint.prototype = Object.assign( Object.create( Joint.prototype ), { constructor: DistanceJoint, preSolve: function ( timeStep, invTimeStep ) { this.updateAnchorPoints(); this.nor.sub( this.anchorPoint2, this.anchorPoint1 ).normalize(); // preSolve this.t.preSolve( timeStep, invTimeStep ); }, solve: function () { this.t.solve(); }, postSolve: function () { } }); /** * An angular constraint for all axes for various joints. * @author saharan */ function AngularConstraint( joint, targetOrientation ) { this.joint = joint; this.targetOrientation = new Quat().invert( targetOrientation ); this.relativeOrientation = new Quat(); this.ii1 = null; this.ii2 = null; this.dd = null; this.vel = new Vec3(); this.imp = new Vec3(); this.rn0 = new Vec3(); this.rn1 = new Vec3(); this.rn2 = new Vec3(); this.b1 = joint.body1; this.b2 = joint.body2; this.a1 = this.b1.angularVelocity; this.a2 = this.b2.angularVelocity; this.i1 = this.b1.inverseInertia; this.i2 = this.b2.inverseInertia; } Object.assign( AngularConstraint.prototype, { AngularConstraint: true, preSolve: function ( timeStep, invTimeStep ) { var inv, len, v; this.ii1 = this.i1.clone(); this.ii2 = this.i2.clone(); v = new Mat33().add(this.ii1, this.ii2).elements; inv = 1/( v[0]*(v[4]*v[8]-v[7]*v[5]) + v[3]*(v[7]*v[2]-v[1]*v[8]) + v[6]*(v[1]*v[5]-v[4]*v[2]) ); this.dd = new Mat33().set( v[4]*v[8]-v[5]*v[7], v[2]*v[7]-v[1]*v[8], v[1]*v[5]-v[2]*v[4], v[5]*v[6]-v[3]*v[8], v[0]*v[8]-v[2]*v[6], v[2]*v[3]-v[0]*v[5], v[3]*v[7]-v[4]*v[6], v[1]*v[6]-v[0]*v[7], v[0]*v[4]-v[1]*v[3] ).multiplyScalar( inv ); this.relativeOrientation.invert( this.b1.orientation ).multiply( this.targetOrientation ).multiply( this.b2.orientation ); inv = this.relativeOrientation.w*2; this.vel.copy( this.relativeOrientation ).multiplyScalar( inv ); len = this.vel.length(); if( len > 0.02 ) { len = (0.02-len)/len*invTimeStep*0.05; this.vel.multiplyScalar( len ); }else{ this.vel.set(0,0,0); } this.rn1.copy( this.imp ).applyMatrix3( this.ii1, true ); this.rn2.copy( this.imp ).applyMatrix3( this.ii2, true ); this.a1.add( this.rn1 ); this.a2.sub( this.rn2 ); }, solve: function () { var r = this.a2.clone().sub( this.a1 ).sub( this.vel ); this.rn0.copy( r ).applyMatrix3( this.dd, true ); this.rn1.copy( this.rn0 ).applyMatrix3( this.ii1, true ); this.rn2.copy( this.rn0 ).applyMatrix3( this.ii2, true ); this.imp.add( this.rn0 ); this.a1.add( this.rn1 ); this.a2.sub( this.rn2 ); } } ); /** * A three-axis translational constraint for various joints. * @author saharan */ function Translational3Constraint (joint,limitMotor1,limitMotor2,limitMotor3){ this.m1=NaN; this.m2=NaN; this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.ax1=NaN; this.ay1=NaN; this.az1=NaN; this.ax2=NaN; this.ay2=NaN; this.az2=NaN; this.ax3=NaN; this.ay3=NaN; this.az3=NaN; this.r1x=NaN; this.r1y=NaN; this.r1z=NaN; this.r2x=NaN; this.r2y=NaN; this.r2z=NaN; this.t1x1=NaN;// jacobians this.t1y1=NaN; this.t1z1=NaN; this.t2x1=NaN; this.t2y1=NaN; this.t2z1=NaN; this.l1x1=NaN; this.l1y1=NaN; this.l1z1=NaN; this.l2x1=NaN; this.l2y1=NaN; this.l2z1=NaN; this.a1x1=NaN; this.a1y1=NaN; this.a1z1=NaN; this.a2x1=NaN; this.a2y1=NaN; this.a2z1=NaN; this.t1x2=NaN; this.t1y2=NaN; this.t1z2=NaN; this.t2x2=NaN; this.t2y2=NaN; this.t2z2=NaN; this.l1x2=NaN; this.l1y2=NaN; this.l1z2=NaN; this.l2x2=NaN; this.l2y2=NaN; this.l2z2=NaN; this.a1x2=NaN; this.a1y2=NaN; this.a1z2=NaN; this.a2x2=NaN; this.a2y2=NaN; this.a2z2=NaN; this.t1x3=NaN; this.t1y3=NaN; this.t1z3=NaN; this.t2x3=NaN; this.t2y3=NaN; this.t2z3=NaN; this.l1x3=NaN; this.l1y3=NaN; this.l1z3=NaN; this.l2x3=NaN; this.l2y3=NaN; this.l2z3=NaN; this.a1x3=NaN; this.a1y3=NaN; this.a1z3=NaN; this.a2x3=NaN; this.a2y3=NaN; this.a2z3=NaN; this.lowerLimit1=NaN; this.upperLimit1=NaN; this.limitVelocity1=NaN; this.limitState1=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited this.enableMotor1=false; this.motorSpeed1=NaN; this.maxMotorForce1=NaN; this.maxMotorImpulse1=NaN; this.lowerLimit2=NaN; this.upperLimit2=NaN; this.limitVelocity2=NaN; this.limitState2=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited this.enableMotor2=false; this.motorSpeed2=NaN; this.maxMotorForce2=NaN; this.maxMotorImpulse2=NaN; this.lowerLimit3=NaN; this.upperLimit3=NaN; this.limitVelocity3=NaN; this.limitState3=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited this.enableMotor3=false; this.motorSpeed3=NaN; this.maxMotorForce3=NaN; this.maxMotorImpulse3=NaN; this.k00=NaN; // K = J*M*JT this.k01=NaN; this.k02=NaN; this.k10=NaN; this.k11=NaN; this.k12=NaN; this.k20=NaN; this.k21=NaN; this.k22=NaN; this.kv00=NaN; // diagonals without CFMs this.kv11=NaN; this.kv22=NaN; this.dv00=NaN; // ...inverted this.dv11=NaN; this.dv22=NaN; this.d00=NaN; // K^-1 this.d01=NaN; this.d02=NaN; this.d10=NaN; this.d11=NaN; this.d12=NaN; this.d20=NaN; this.d21=NaN; this.d22=NaN; this.limitMotor1=limitMotor1; this.limitMotor2=limitMotor2; this.limitMotor3=limitMotor3; this.b1=joint.body1; this.b2=joint.body2; this.p1=joint.anchorPoint1; this.p2=joint.anchorPoint2; this.r1=joint.relativeAnchorPoint1; this.r2=joint.relativeAnchorPoint2; this.l1=this.b1.linearVelocity; this.l2=this.b2.linearVelocity; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.limitImpulse1=0; this.motorImpulse1=0; this.limitImpulse2=0; this.motorImpulse2=0; this.limitImpulse3=0; this.motorImpulse3=0; this.cfm1=0;// Constraint Force Mixing this.cfm2=0; this.cfm3=0; this.weight=-1; } Object.assign( Translational3Constraint.prototype, { Translational3Constraint: true, preSolve:function(timeStep,invTimeStep){ this.ax1=this.limitMotor1.axis.x; this.ay1=this.limitMotor1.axis.y; this.az1=this.limitMotor1.axis.z; this.ax2=this.limitMotor2.axis.x; this.ay2=this.limitMotor2.axis.y; this.az2=this.limitMotor2.axis.z; this.ax3=this.limitMotor3.axis.x; this.ay3=this.limitMotor3.axis.y; this.az3=this.limitMotor3.axis.z; this.lowerLimit1=this.limitMotor1.lowerLimit; this.upperLimit1=this.limitMotor1.upperLimit; this.motorSpeed1=this.limitMotor1.motorSpeed; this.maxMotorForce1=this.limitMotor1.maxMotorForce; this.enableMotor1=this.maxMotorForce1>0; this.lowerLimit2=this.limitMotor2.lowerLimit; this.upperLimit2=this.limitMotor2.upperLimit; this.motorSpeed2=this.limitMotor2.motorSpeed; this.maxMotorForce2=this.limitMotor2.maxMotorForce; this.enableMotor2=this.maxMotorForce2>0; this.lowerLimit3=this.limitMotor3.lowerLimit; this.upperLimit3=this.limitMotor3.upperLimit; this.motorSpeed3=this.limitMotor3.motorSpeed; this.maxMotorForce3=this.limitMotor3.maxMotorForce; this.enableMotor3=this.maxMotorForce3>0; this.m1=this.b1.inverseMass; this.m2=this.b2.inverseMass; var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; var dx=this.p2.x-this.p1.x; var dy=this.p2.y-this.p1.y; var dz=this.p2.z-this.p1.z; var d1=dx*this.ax1+dy*this.ay1+dz*this.az1; var d2=dx*this.ax2+dy*this.ay2+dz*this.az2; var d3=dx*this.ax3+dy*this.ay3+dz*this.az3; var frequency1=this.limitMotor1.frequency; var frequency2=this.limitMotor2.frequency; var frequency3=this.limitMotor3.frequency; var enableSpring1=frequency1>0; var enableSpring2=frequency2>0; var enableSpring3=frequency3>0; var enableLimit1=this.lowerLimit1<=this.upperLimit1; var enableLimit2=this.lowerLimit2<=this.upperLimit2; var enableLimit3=this.lowerLimit3<=this.upperLimit3; // for stability if(enableSpring1&&d1>20||d1<-20){ enableSpring1=false; } if(enableSpring2&&d2>20||d2<-20){ enableSpring2=false; } if(enableSpring3&&d3>20||d3<-20){ enableSpring3=false; } if(enableLimit1){ if(this.lowerLimit1==this.upperLimit1){ if(this.limitState1!=0){ this.limitState1=0; this.limitImpulse1=0; } this.limitVelocity1=this.lowerLimit1-d1; if(!enableSpring1)d1=this.lowerLimit1; }else if(d1this.upperLimit1){ if(this.limitState1!=1){ this.limitState1=1; this.limitImpulse1=0; } this.limitVelocity1=this.upperLimit1-d1; if(!enableSpring1)d1=this.upperLimit1; }else{ this.limitState1=2; this.limitImpulse1=0; this.limitVelocity1=0; } if(!enableSpring1){ if(this.limitVelocity1>0.005)this.limitVelocity1-=0.005; else if(this.limitVelocity1<-0.005)this.limitVelocity1+=0.005; else this.limitVelocity1=0; } }else{ this.limitState1=2; this.limitImpulse1=0; } if(enableLimit2){ if(this.lowerLimit2==this.upperLimit2){ if(this.limitState2!=0){ this.limitState2=0; this.limitImpulse2=0; } this.limitVelocity2=this.lowerLimit2-d2; if(!enableSpring2)d2=this.lowerLimit2; }else if(d2this.upperLimit2){ if(this.limitState2!=1){ this.limitState2=1; this.limitImpulse2=0; } this.limitVelocity2=this.upperLimit2-d2; if(!enableSpring2)d2=this.upperLimit2; }else{ this.limitState2=2; this.limitImpulse2=0; this.limitVelocity2=0; } if(!enableSpring2){ if(this.limitVelocity2>0.005)this.limitVelocity2-=0.005; else if(this.limitVelocity2<-0.005)this.limitVelocity2+=0.005; else this.limitVelocity2=0; } }else{ this.limitState2=2; this.limitImpulse2=0; } if(enableLimit3){ if(this.lowerLimit3==this.upperLimit3){ if(this.limitState3!=0){ this.limitState3=0; this.limitImpulse3=0; } this.limitVelocity3=this.lowerLimit3-d3; if(!enableSpring3)d3=this.lowerLimit3; }else if(d3this.upperLimit3){ if(this.limitState3!=1){ this.limitState3=1; this.limitImpulse3=0; } this.limitVelocity3=this.upperLimit3-d3; if(!enableSpring3)d3=this.upperLimit3; }else{ this.limitState3=2; this.limitImpulse3=0; this.limitVelocity3=0; } if(!enableSpring3){ if(this.limitVelocity3>0.005)this.limitVelocity3-=0.005; else if(this.limitVelocity3<-0.005)this.limitVelocity3+=0.005; else this.limitVelocity3=0; } }else{ this.limitState3=2; this.limitImpulse3=0; } if(this.enableMotor1&&(this.limitState1!=0||enableSpring1)){ this.maxMotorImpulse1=this.maxMotorForce1*timeStep; }else{ this.motorImpulse1=0; this.maxMotorImpulse1=0; } if(this.enableMotor2&&(this.limitState2!=0||enableSpring2)){ this.maxMotorImpulse2=this.maxMotorForce2*timeStep; }else{ this.motorImpulse2=0; this.maxMotorImpulse2=0; } if(this.enableMotor3&&(this.limitState3!=0||enableSpring3)){ this.maxMotorImpulse3=this.maxMotorForce3*timeStep; }else{ this.motorImpulse3=0; this.maxMotorImpulse3=0; } var rdx=d1*this.ax1+d2*this.ax2+d3*this.ax2; var rdy=d1*this.ay1+d2*this.ay2+d3*this.ay2; var rdz=d1*this.az1+d2*this.az2+d3*this.az2; var w1=this.m2/(this.m1+this.m2); if(this.weight>=0)w1=this.weight; // use given weight var w2=1-w1; this.r1x=this.r1.x+rdx*w1; this.r1y=this.r1.y+rdy*w1; this.r1z=this.r1.z+rdz*w1; this.r2x=this.r2.x-rdx*w2; this.r2y=this.r2.y-rdy*w2; this.r2z=this.r2.z-rdz*w2; // build jacobians this.t1x1=this.r1y*this.az1-this.r1z*this.ay1; this.t1y1=this.r1z*this.ax1-this.r1x*this.az1; this.t1z1=this.r1x*this.ay1-this.r1y*this.ax1; this.t2x1=this.r2y*this.az1-this.r2z*this.ay1; this.t2y1=this.r2z*this.ax1-this.r2x*this.az1; this.t2z1=this.r2x*this.ay1-this.r2y*this.ax1; this.l1x1=this.ax1*this.m1; this.l1y1=this.ay1*this.m1; this.l1z1=this.az1*this.m1; this.l2x1=this.ax1*this.m2; this.l2y1=this.ay1*this.m2; this.l2z1=this.az1*this.m2; this.a1x1=this.t1x1*this.i1e00+this.t1y1*this.i1e01+this.t1z1*this.i1e02; this.a1y1=this.t1x1*this.i1e10+this.t1y1*this.i1e11+this.t1z1*this.i1e12; this.a1z1=this.t1x1*this.i1e20+this.t1y1*this.i1e21+this.t1z1*this.i1e22; this.a2x1=this.t2x1*this.i2e00+this.t2y1*this.i2e01+this.t2z1*this.i2e02; this.a2y1=this.t2x1*this.i2e10+this.t2y1*this.i2e11+this.t2z1*this.i2e12; this.a2z1=this.t2x1*this.i2e20+this.t2y1*this.i2e21+this.t2z1*this.i2e22; this.t1x2=this.r1y*this.az2-this.r1z*this.ay2; this.t1y2=this.r1z*this.ax2-this.r1x*this.az2; this.t1z2=this.r1x*this.ay2-this.r1y*this.ax2; this.t2x2=this.r2y*this.az2-this.r2z*this.ay2; this.t2y2=this.r2z*this.ax2-this.r2x*this.az2; this.t2z2=this.r2x*this.ay2-this.r2y*this.ax2; this.l1x2=this.ax2*this.m1; this.l1y2=this.ay2*this.m1; this.l1z2=this.az2*this.m1; this.l2x2=this.ax2*this.m2; this.l2y2=this.ay2*this.m2; this.l2z2=this.az2*this.m2; this.a1x2=this.t1x2*this.i1e00+this.t1y2*this.i1e01+this.t1z2*this.i1e02; this.a1y2=this.t1x2*this.i1e10+this.t1y2*this.i1e11+this.t1z2*this.i1e12; this.a1z2=this.t1x2*this.i1e20+this.t1y2*this.i1e21+this.t1z2*this.i1e22; this.a2x2=this.t2x2*this.i2e00+this.t2y2*this.i2e01+this.t2z2*this.i2e02; this.a2y2=this.t2x2*this.i2e10+this.t2y2*this.i2e11+this.t2z2*this.i2e12; this.a2z2=this.t2x2*this.i2e20+this.t2y2*this.i2e21+this.t2z2*this.i2e22; this.t1x3=this.r1y*this.az3-this.r1z*this.ay3; this.t1y3=this.r1z*this.ax3-this.r1x*this.az3; this.t1z3=this.r1x*this.ay3-this.r1y*this.ax3; this.t2x3=this.r2y*this.az3-this.r2z*this.ay3; this.t2y3=this.r2z*this.ax3-this.r2x*this.az3; this.t2z3=this.r2x*this.ay3-this.r2y*this.ax3; this.l1x3=this.ax3*this.m1; this.l1y3=this.ay3*this.m1; this.l1z3=this.az3*this.m1; this.l2x3=this.ax3*this.m2; this.l2y3=this.ay3*this.m2; this.l2z3=this.az3*this.m2; this.a1x3=this.t1x3*this.i1e00+this.t1y3*this.i1e01+this.t1z3*this.i1e02; this.a1y3=this.t1x3*this.i1e10+this.t1y3*this.i1e11+this.t1z3*this.i1e12; this.a1z3=this.t1x3*this.i1e20+this.t1y3*this.i1e21+this.t1z3*this.i1e22; this.a2x3=this.t2x3*this.i2e00+this.t2y3*this.i2e01+this.t2z3*this.i2e02; this.a2y3=this.t2x3*this.i2e10+this.t2y3*this.i2e11+this.t2z3*this.i2e12; this.a2z3=this.t2x3*this.i2e20+this.t2y3*this.i2e21+this.t2z3*this.i2e22; // build an impulse matrix var m12=this.m1+this.m2; this.k00=(this.ax1*this.ax1+this.ay1*this.ay1+this.az1*this.az1)*m12; this.k01=(this.ax1*this.ax2+this.ay1*this.ay2+this.az1*this.az2)*m12; this.k02=(this.ax1*this.ax3+this.ay1*this.ay3+this.az1*this.az3)*m12; this.k10=(this.ax2*this.ax1+this.ay2*this.ay1+this.az2*this.az1)*m12; this.k11=(this.ax2*this.ax2+this.ay2*this.ay2+this.az2*this.az2)*m12; this.k12=(this.ax2*this.ax3+this.ay2*this.ay3+this.az2*this.az3)*m12; this.k20=(this.ax3*this.ax1+this.ay3*this.ay1+this.az3*this.az1)*m12; this.k21=(this.ax3*this.ax2+this.ay3*this.ay2+this.az3*this.az2)*m12; this.k22=(this.ax3*this.ax3+this.ay3*this.ay3+this.az3*this.az3)*m12; this.k00+=this.t1x1*this.a1x1+this.t1y1*this.a1y1+this.t1z1*this.a1z1; this.k01+=this.t1x1*this.a1x2+this.t1y1*this.a1y2+this.t1z1*this.a1z2; this.k02+=this.t1x1*this.a1x3+this.t1y1*this.a1y3+this.t1z1*this.a1z3; this.k10+=this.t1x2*this.a1x1+this.t1y2*this.a1y1+this.t1z2*this.a1z1; this.k11+=this.t1x2*this.a1x2+this.t1y2*this.a1y2+this.t1z2*this.a1z2; this.k12+=this.t1x2*this.a1x3+this.t1y2*this.a1y3+this.t1z2*this.a1z3; this.k20+=this.t1x3*this.a1x1+this.t1y3*this.a1y1+this.t1z3*this.a1z1; this.k21+=this.t1x3*this.a1x2+this.t1y3*this.a1y2+this.t1z3*this.a1z2; this.k22+=this.t1x3*this.a1x3+this.t1y3*this.a1y3+this.t1z3*this.a1z3; this.k00+=this.t2x1*this.a2x1+this.t2y1*this.a2y1+this.t2z1*this.a2z1; this.k01+=this.t2x1*this.a2x2+this.t2y1*this.a2y2+this.t2z1*this.a2z2; this.k02+=this.t2x1*this.a2x3+this.t2y1*this.a2y3+this.t2z1*this.a2z3; this.k10+=this.t2x2*this.a2x1+this.t2y2*this.a2y1+this.t2z2*this.a2z1; this.k11+=this.t2x2*this.a2x2+this.t2y2*this.a2y2+this.t2z2*this.a2z2; this.k12+=this.t2x2*this.a2x3+this.t2y2*this.a2y3+this.t2z2*this.a2z3; this.k20+=this.t2x3*this.a2x1+this.t2y3*this.a2y1+this.t2z3*this.a2z1; this.k21+=this.t2x3*this.a2x2+this.t2y3*this.a2y2+this.t2z3*this.a2z2; this.k22+=this.t2x3*this.a2x3+this.t2y3*this.a2y3+this.t2z3*this.a2z3; this.kv00=this.k00; this.kv11=this.k11; this.kv22=this.k22; this.dv00=1/this.kv00; this.dv11=1/this.kv11; this.dv22=1/this.kv22; if(enableSpring1&&this.limitState1!=2){ var omega=6.2831853*frequency1; var k=omega*omega*timeStep; var dmp=invTimeStep/(k+2*this.limitMotor1.dampingRatio*omega); this.cfm1=this.kv00*dmp; this.limitVelocity1*=k*dmp; }else{ this.cfm1=0; this.limitVelocity1*=invTimeStep*0.05; } if(enableSpring2&&this.limitState2!=2){ omega=6.2831853*frequency2; k=omega*omega*timeStep; dmp=invTimeStep/(k+2*this.limitMotor2.dampingRatio*omega); this.cfm2=this.kv11*dmp; this.limitVelocity2*=k*dmp; }else{ this.cfm2=0; this.limitVelocity2*=invTimeStep*0.05; } if(enableSpring3&&this.limitState3!=2){ omega=6.2831853*frequency3; k=omega*omega*timeStep; dmp=invTimeStep/(k+2*this.limitMotor3.dampingRatio*omega); this.cfm3=this.kv22*dmp; this.limitVelocity3*=k*dmp; }else{ this.cfm3=0; this.limitVelocity3*=invTimeStep*0.05; } this.k00+=this.cfm1; this.k11+=this.cfm2; this.k22+=this.cfm3; var inv=1/( this.k00*(this.k11*this.k22-this.k21*this.k12)+ this.k10*(this.k21*this.k02-this.k01*this.k22)+ this.k20*(this.k01*this.k12-this.k11*this.k02) ); this.d00=(this.k11*this.k22-this.k12*this.k21)*inv; this.d01=(this.k02*this.k21-this.k01*this.k22)*inv; this.d02=(this.k01*this.k12-this.k02*this.k11)*inv; this.d10=(this.k12*this.k20-this.k10*this.k22)*inv; this.d11=(this.k00*this.k22-this.k02*this.k20)*inv; this.d12=(this.k02*this.k10-this.k00*this.k12)*inv; this.d20=(this.k10*this.k21-this.k11*this.k20)*inv; this.d21=(this.k01*this.k20-this.k00*this.k21)*inv; this.d22=(this.k00*this.k11-this.k01*this.k10)*inv; // warm starting var totalImpulse1=this.limitImpulse1+this.motorImpulse1; var totalImpulse2=this.limitImpulse2+this.motorImpulse2; var totalImpulse3=this.limitImpulse3+this.motorImpulse3; this.l1.x+=totalImpulse1*this.l1x1+totalImpulse2*this.l1x2+totalImpulse3*this.l1x3; this.l1.y+=totalImpulse1*this.l1y1+totalImpulse2*this.l1y2+totalImpulse3*this.l1y3; this.l1.z+=totalImpulse1*this.l1z1+totalImpulse2*this.l1z2+totalImpulse3*this.l1z3; this.a1.x+=totalImpulse1*this.a1x1+totalImpulse2*this.a1x2+totalImpulse3*this.a1x3; this.a1.y+=totalImpulse1*this.a1y1+totalImpulse2*this.a1y2+totalImpulse3*this.a1y3; this.a1.z+=totalImpulse1*this.a1z1+totalImpulse2*this.a1z2+totalImpulse3*this.a1z3; this.l2.x-=totalImpulse1*this.l2x1+totalImpulse2*this.l2x2+totalImpulse3*this.l2x3; this.l2.y-=totalImpulse1*this.l2y1+totalImpulse2*this.l2y2+totalImpulse3*this.l2y3; this.l2.z-=totalImpulse1*this.l2z1+totalImpulse2*this.l2z2+totalImpulse3*this.l2z3; this.a2.x-=totalImpulse1*this.a2x1+totalImpulse2*this.a2x2+totalImpulse3*this.a2x3; this.a2.y-=totalImpulse1*this.a2y1+totalImpulse2*this.a2y2+totalImpulse3*this.a2y3; this.a2.z-=totalImpulse1*this.a2z1+totalImpulse2*this.a2z2+totalImpulse3*this.a2z3; }, solve:function(){ var rvx=this.l2.x-this.l1.x+this.a2.y*this.r2z-this.a2.z*this.r2y-this.a1.y*this.r1z+this.a1.z*this.r1y; var rvy=this.l2.y-this.l1.y+this.a2.z*this.r2x-this.a2.x*this.r2z-this.a1.z*this.r1x+this.a1.x*this.r1z; var rvz=this.l2.z-this.l1.z+this.a2.x*this.r2y-this.a2.y*this.r2x-this.a1.x*this.r1y+this.a1.y*this.r1x; var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1; var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2; var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3; var oldMotorImpulse1=this.motorImpulse1; var oldMotorImpulse2=this.motorImpulse2; var oldMotorImpulse3=this.motorImpulse3; var dMotorImpulse1=0; var dMotorImpulse2=0; var dMotorImpulse3=0; if(this.enableMotor1){ dMotorImpulse1=(rvn1-this.motorSpeed1)*this.dv00; this.motorImpulse1+=dMotorImpulse1; if(this.motorImpulse1>this.maxMotorImpulse1){ // clamp motor impulse this.motorImpulse1=this.maxMotorImpulse1; }else if(this.motorImpulse1<-this.maxMotorImpulse1){ this.motorImpulse1=-this.maxMotorImpulse1; } dMotorImpulse1=this.motorImpulse1-oldMotorImpulse1; } if(this.enableMotor2){ dMotorImpulse2=(rvn2-this.motorSpeed2)*this.dv11; this.motorImpulse2+=dMotorImpulse2; if(this.motorImpulse2>this.maxMotorImpulse2){ // clamp motor impulse this.motorImpulse2=this.maxMotorImpulse2; }else if(this.motorImpulse2<-this.maxMotorImpulse2){ this.motorImpulse2=-this.maxMotorImpulse2; } dMotorImpulse2=this.motorImpulse2-oldMotorImpulse2; } if(this.enableMotor3){ dMotorImpulse3=(rvn3-this.motorSpeed3)*this.dv22; this.motorImpulse3+=dMotorImpulse3; if(this.motorImpulse3>this.maxMotorImpulse3){ // clamp motor impulse this.motorImpulse3=this.maxMotorImpulse3; }else if(this.motorImpulse3<-this.maxMotorImpulse3){ this.motorImpulse3=-this.maxMotorImpulse3; } dMotorImpulse3=this.motorImpulse3-oldMotorImpulse3; } // apply motor impulse to relative velocity rvn1+=dMotorImpulse1*this.kv00+dMotorImpulse2*this.k01+dMotorImpulse3*this.k02; rvn2+=dMotorImpulse1*this.k10+dMotorImpulse2*this.kv11+dMotorImpulse3*this.k12; rvn3+=dMotorImpulse1*this.k20+dMotorImpulse2*this.k21+dMotorImpulse3*this.kv22; // subtract target velocity and applied impulse rvn1-=this.limitVelocity1+this.limitImpulse1*this.cfm1; rvn2-=this.limitVelocity2+this.limitImpulse2*this.cfm2; rvn3-=this.limitVelocity3+this.limitImpulse3*this.cfm3; var oldLimitImpulse1=this.limitImpulse1; var oldLimitImpulse2=this.limitImpulse2; var oldLimitImpulse3=this.limitImpulse3; var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02; var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12; var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22; this.limitImpulse1+=dLimitImpulse1; this.limitImpulse2+=dLimitImpulse2; this.limitImpulse3+=dLimitImpulse3; // clamp var clampState=0; if(this.limitState1==2||this.limitImpulse1*this.limitState1<0){ dLimitImpulse1=-oldLimitImpulse1; rvn2+=dLimitImpulse1*this.k10; rvn3+=dLimitImpulse1*this.k20; clampState|=1; } if(this.limitState2==2||this.limitImpulse2*this.limitState2<0){ dLimitImpulse2=-oldLimitImpulse2; rvn1+=dLimitImpulse2*this.k01; rvn3+=dLimitImpulse2*this.k21; clampState|=2; } if(this.limitState3==2||this.limitImpulse3*this.limitState3<0){ dLimitImpulse3=-oldLimitImpulse3; rvn1+=dLimitImpulse3*this.k02; rvn2+=dLimitImpulse3*this.k12; clampState|=4; } // update un-clamped impulse // TODO: isolate division var det; switch(clampState){ case 1:// update 2 3 det=1/(this.k11*this.k22-this.k12*this.k21); dLimitImpulse2=(this.k22*rvn2+-this.k12*rvn3)*det; dLimitImpulse3=(-this.k21*rvn2+this.k11*rvn3)*det; break; case 2:// update 1 3 det=1/(this.k00*this.k22-this.k02*this.k20); dLimitImpulse1=(this.k22*rvn1+-this.k02*rvn3)*det; dLimitImpulse3=(-this.k20*rvn1+this.k00*rvn3)*det; break; case 3:// update 3 dLimitImpulse3=rvn3/this.k22; break; case 4:// update 1 2 det=1/(this.k00*this.k11-this.k01*this.k10); dLimitImpulse1=(this.k11*rvn1+-this.k01*rvn2)*det; dLimitImpulse2=(-this.k10*rvn1+this.k00*rvn2)*det; break; case 5:// update 2 dLimitImpulse2=rvn2/this.k11; break; case 6:// update 1 dLimitImpulse1=rvn1/this.k00; break; } this.limitImpulse1=oldLimitImpulse1+dLimitImpulse1; this.limitImpulse2=oldLimitImpulse2+dLimitImpulse2; this.limitImpulse3=oldLimitImpulse3+dLimitImpulse3; var dImpulse1=dMotorImpulse1+dLimitImpulse1; var dImpulse2=dMotorImpulse2+dLimitImpulse2; var dImpulse3=dMotorImpulse3+dLimitImpulse3; // apply impulse this.l1.x+=dImpulse1*this.l1x1+dImpulse2*this.l1x2+dImpulse3*this.l1x3; this.l1.y+=dImpulse1*this.l1y1+dImpulse2*this.l1y2+dImpulse3*this.l1y3; this.l1.z+=dImpulse1*this.l1z1+dImpulse2*this.l1z2+dImpulse3*this.l1z3; this.a1.x+=dImpulse1*this.a1x1+dImpulse2*this.a1x2+dImpulse3*this.a1x3; this.a1.y+=dImpulse1*this.a1y1+dImpulse2*this.a1y2+dImpulse3*this.a1y3; this.a1.z+=dImpulse1*this.a1z1+dImpulse2*this.a1z2+dImpulse3*this.a1z3; this.l2.x-=dImpulse1*this.l2x1+dImpulse2*this.l2x2+dImpulse3*this.l2x3; this.l2.y-=dImpulse1*this.l2y1+dImpulse2*this.l2y2+dImpulse3*this.l2y3; this.l2.z-=dImpulse1*this.l2z1+dImpulse2*this.l2z2+dImpulse3*this.l2z3; this.a2.x-=dImpulse1*this.a2x1+dImpulse2*this.a2x2+dImpulse3*this.a2x3; this.a2.y-=dImpulse1*this.a2y1+dImpulse2*this.a2y2+dImpulse3*this.a2y3; this.a2.z-=dImpulse1*this.a2z1+dImpulse2*this.a2z2+dImpulse3*this.a2z3; } } ); /** * A prismatic joint allows only for relative translation of rigid bodies along the axis. * * @author saharan * @author lo-th */ function PrismaticJoint( config, lowerTranslation, upperTranslation ){ Joint.call( this, config ); this.type = JOINT_PRISMATIC; // The axis in the first body's coordinate system. this.localAxis1 = config.localAxis1.clone().normalize(); // The axis in the second body's coordinate system. this.localAxis2 = config.localAxis2.clone().normalize(); this.ax1 = new Vec3(); this.ax2 = new Vec3(); this.nor = new Vec3(); this.tan = new Vec3(); this.bin = new Vec3(); this.ac = new AngularConstraint( this, new Quat().setFromUnitVectors( this.localAxis1, this.localAxis2 ) ); // The translational limit and motor information of the joint. this.limitMotor = new LimitMotor( this.nor, true ); this.limitMotor.lowerLimit = lowerTranslation; this.limitMotor.upperLimit = upperTranslation; this.t3 = new Translational3Constraint( this, this.limitMotor, new LimitMotor( this.tan, true ), new LimitMotor( this.bin, true ) ); } PrismaticJoint.prototype = Object.assign( Object.create( Joint.prototype ), { constructor: PrismaticJoint, preSolve: function ( timeStep, invTimeStep ) { this.updateAnchorPoints(); this.ax1.copy( this.localAxis1 ).applyMatrix3( this.body1.rotation, true ); this.ax2.copy( this.localAxis2 ).applyMatrix3( this.body2.rotation, true ); // normal tangent binormal this.nor.set( this.ax1.x*this.body2.inverseMass + this.ax2.x*this.body1.inverseMass, this.ax1.y*this.body2.inverseMass + this.ax2.y*this.body1.inverseMass, this.ax1.z*this.body2.inverseMass + this.ax2.z*this.body1.inverseMass ).normalize(); this.tan.tangent( this.nor ).normalize(); this.bin.crossVectors( this.nor, this.tan ); // preSolve this.ac.preSolve( timeStep, invTimeStep ); this.t3.preSolve( timeStep, invTimeStep ); }, solve: function () { this.ac.solve(); this.t3.solve(); }, postSolve: function () { } }); /** * A slider joint allows for relative translation and relative rotation between two rigid bodies along the axis. * * @author saharan * @author lo-th */ function SliderJoint( config, lowerTranslation, upperTranslation ){ Joint.call( this, config ); this.type = JOINT_SLIDER; // The axis in the first body's coordinate system. this.localAxis1 = config.localAxis1.clone().normalize(); // The axis in the second body's coordinate system. this.localAxis2 = config.localAxis2.clone().normalize(); // make angle axis var arc = new Mat33().setQuat( new Quat().setFromUnitVectors( this.localAxis1, this.localAxis2 ) ); this.localAngle1 = new Vec3().tangent( this.localAxis1 ).normalize(); this.localAngle2 = this.localAngle1.clone().applyMatrix3( arc, true ); this.ax1 = new Vec3(); this.ax2 = new Vec3(); this.an1 = new Vec3(); this.an2 = new Vec3(); this.tmp = new Vec3(); this.nor = new Vec3(); this.tan = new Vec3(); this.bin = new Vec3(); // The limit and motor for the rotation this.rotationalLimitMotor = new LimitMotor( this.nor, false ); this.r3 = new Rotational3Constraint( this, this.rotationalLimitMotor, new LimitMotor( this.tan, true ), new LimitMotor( this.bin, true ) ); // The limit and motor for the translation. this.translationalLimitMotor = new LimitMotor( this.nor, true ); this.translationalLimitMotor.lowerLimit = lowerTranslation; this.translationalLimitMotor.upperLimit = upperTranslation; this.t3 = new Translational3Constraint( this, this.translationalLimitMotor, new LimitMotor( this.tan, true ), new LimitMotor( this.bin, true ) ); } SliderJoint.prototype = Object.assign( Object.create( Joint.prototype ), { constructor: SliderJoint, preSolve: function ( timeStep, invTimeStep ) { this.updateAnchorPoints(); this.ax1.copy( this.localAxis1 ).applyMatrix3( this.body1.rotation, true ); this.an1.copy( this.localAngle1 ).applyMatrix3( this.body1.rotation, true ); this.ax2.copy( this.localAxis2 ).applyMatrix3( this.body2.rotation, true ); this.an2.copy( this.localAngle2 ).applyMatrix3( this.body2.rotation, true ); // normal tangent binormal this.nor.set( this.ax1.x*this.body2.inverseMass + this.ax2.x*this.body1.inverseMass, this.ax1.y*this.body2.inverseMass + this.ax2.y*this.body1.inverseMass, this.ax1.z*this.body2.inverseMass + this.ax2.z*this.body1.inverseMass ).normalize(); this.tan.tangent( this.nor ).normalize(); this.bin.crossVectors( this.nor, this.tan ); // calculate hinge angle this.tmp.crossVectors( this.an1, this.an2 ); var limite = _Math.acosClamp( _Math.dotVectors( this.an1, this.an2 ) ); if( _Math.dotVectors( this.nor, this.tmp ) < 0 ) this.rotationalLimitMotor.angle = -limite; else this.rotationalLimitMotor.angle = limite; // angular error this.tmp.crossVectors( this.ax1, this.ax2 ); this.r3.limitMotor2.angle = _Math.dotVectors( this.tan, this.tmp ); this.r3.limitMotor3.angle = _Math.dotVectors( this.bin, this.tmp ); // preSolve this.r3.preSolve( timeStep, invTimeStep ); this.t3.preSolve( timeStep, invTimeStep ); }, solve: function () { this.r3.solve(); this.t3.solve(); }, postSolve: function () { } }); /** * A wheel joint allows for relative rotation between two rigid bodies along two axes. * The wheel joint also allows for relative translation for the suspension. * * @author saharan * @author lo-th */ function WheelJoint ( config ){ Joint.call( this, config ); this.type = JOINT_WHEEL; // The axis in the first body's coordinate system. this.localAxis1 = config.localAxis1.clone().normalize(); // The axis in the second body's coordinate system. this.localAxis2 = config.localAxis2.clone().normalize(); this.localAngle1 = new Vec3(); this.localAngle2 = new Vec3(); var dot = _Math.dotVectors( this.localAxis1, this.localAxis2 ); if( dot > -1 && dot < 1 ){ this.localAngle1.set( this.localAxis2.x - dot*this.localAxis1.x, this.localAxis2.y - dot*this.localAxis1.y, this.localAxis2.z - dot*this.localAxis1.z ).normalize(); this.localAngle2.set( this.localAxis1.x - dot*this.localAxis2.x, this.localAxis1.y - dot*this.localAxis2.y, this.localAxis1.z - dot*this.localAxis2.z ).normalize(); } else { var arc = new Mat33().setQuat( new Quat().setFromUnitVectors( this.localAxis1, this.localAxis2 ) ); this.localAngle1.tangent( this.localAxis1 ).normalize(); this.localAngle2 = this.localAngle1.clone().applyMatrix3( arc, true ); } this.ax1 = new Vec3(); this.ax2 = new Vec3(); this.an1 = new Vec3(); this.an2 = new Vec3(); this.tmp = new Vec3(); this.nor = new Vec3(); this.tan = new Vec3(); this.bin = new Vec3(); // The translational limit and motor information of the joint. this.translationalLimitMotor = new LimitMotor( this.tan,true ); this.translationalLimitMotor.frequency = 8; this.translationalLimitMotor.dampingRatio = 1; // The first rotational limit and motor information of the joint. this.rotationalLimitMotor1 = new LimitMotor( this.tan, false ); // The second rotational limit and motor information of the joint. this.rotationalLimitMotor2 = new LimitMotor( this.bin, false ); this.t3 = new Translational3Constraint( this, new LimitMotor( this.nor, true ),this.translationalLimitMotor,new LimitMotor( this.bin, true )); this.t3.weight = 1; this.r3 = new Rotational3Constraint(this,new LimitMotor( this.nor, true ),this.rotationalLimitMotor1,this.rotationalLimitMotor2); } WheelJoint.prototype = Object.assign( Object.create( Joint.prototype ), { constructor: WheelJoint, preSolve: function ( timeStep, invTimeStep ) { this.updateAnchorPoints(); this.ax1.copy( this.localAxis1 ).applyMatrix3( this.body1.rotation, true ); this.an1.copy( this.localAngle1 ).applyMatrix3( this.body1.rotation, true ); this.ax2.copy( this.localAxis2 ).applyMatrix3( this.body2.rotation, true ); this.an2.copy( this.localAngle2 ).applyMatrix3( this.body2.rotation, true ); this.r3.limitMotor1.angle = _Math.dotVectors( this.ax1, this.ax2 ); var limite = _Math.dotVectors( this.an1, this.ax2 ); if( _Math.dotVectors( this.ax1, this.tmp.crossVectors( this.an1, this.ax2 ) ) < 0 ) this.rotationalLimitMotor1.angle = -limite; else this.rotationalLimitMotor1.angle = limite; limite = _Math.dotVectors( this.an2, this.ax1 ); if( _Math.dotVectors( this.ax2, this.tmp.crossVectors( this.an2, this.ax1 ) ) < 0 ) this.rotationalLimitMotor2.angle = -limite; else this.rotationalLimitMotor2.angle = limite; this.nor.crossVectors( this.ax1, this.ax2 ).normalize(); this.tan.crossVectors( this.nor, this.ax2 ).normalize(); this.bin.crossVectors( this.nor, this.ax1 ).normalize(); this.r3.preSolve(timeStep,invTimeStep); this.t3.preSolve(timeStep,invTimeStep); }, solve: function () { this.r3.solve(); this.t3.solve(); }, postSolve: function () { } }); function JointConfig(){ this.scale = 1; this.invScale = 1; // The first rigid body of the joint. this.body1 = null; // The second rigid body of the joint. this.body2 = null; // The anchor point on the first rigid body in local coordinate system. this.localAnchorPoint1 = new Vec3(); // The anchor point on the second rigid body in local coordinate system. this.localAnchorPoint2 = new Vec3(); // The axis in the first body's coordinate system. // his property is available in some joints. this.localAxis1 = new Vec3(); // The axis in the second body's coordinate system. // This property is available in some joints. this.localAxis2 = new Vec3(); // Whether allow collision between connected rigid bodies or not. this.allowCollision = false; } /** * This class holds mass information of a shape. * @author lo-th * @author saharan */ function MassInfo (){ // Mass of the shape. this.mass = 0; // The moment inertia of the shape. this.inertia = new Mat33(); } /** * A link list of contacts. * @author saharan */ function ContactLink ( contact ){ // The previous contact link. this.prev = null; // The next contact link. this.next = null; // The shape of the contact. this.shape = null; // The other rigid body. this.body = null; // The contact of the link. this.contact = contact; } function ImpulseDataBuffer (){ this.lp1X = NaN; this.lp1Y = NaN; this.lp1Z = NaN; this.lp2X = NaN; this.lp2Y = NaN; this.lp2Z = NaN; this.impulse = NaN; } /** * The class holds details of the contact point. * @author saharan */ function ManifoldPoint(){ // Whether this manifold point is persisting or not. this.warmStarted = false; // The position of this manifold point. this.position = new Vec3(); // The position in the first shape's coordinate. this.localPoint1 = new Vec3(); // The position in the second shape's coordinate. this.localPoint2 = new Vec3(); // The normal vector of this manifold point. this.normal = new Vec3(); // The tangent vector of this manifold point. this.tangent = new Vec3(); // The binormal vector of this manifold point. this.binormal = new Vec3(); // The impulse in normal direction. this.normalImpulse = 0; // The impulse in tangent direction. this.tangentImpulse = 0; // The impulse in binormal direction. this.binormalImpulse = 0; // The denominator in normal direction. this.normalDenominator = 0; // The denominator in tangent direction. this.tangentDenominator = 0; // The denominator in binormal direction. this.binormalDenominator = 0; // The depth of penetration. this.penetration = 0; } /** * A contact manifold between two shapes. * @author saharan * @author lo-th */ function ContactManifold () { // The first rigid body. this.body1 = null; // The second rigid body. this.body2 = null; // The number of manifold points. this.numPoints = 0; // The manifold points. this.points = [ new ManifoldPoint(), new ManifoldPoint(), new ManifoldPoint(), new ManifoldPoint() ]; } ContactManifold.prototype = { constructor: ContactManifold, //Reset the manifold. reset:function( shape1, shape2 ){ this.body1 = shape1.parent; this.body2 = shape2.parent; this.numPoints = 0; }, // Add a point into this manifold. addPointVec: function ( pos, norm, penetration, flip ) { var p = this.points[ this.numPoints++ ]; p.position.copy( pos ); p.localPoint1.sub( pos, this.body1.position ).applyMatrix3( this.body1.rotation ); p.localPoint2.sub( pos, this.body2.position ).applyMatrix3( this.body2.rotation ); p.normal.copy( norm ); if( flip ) p.normal.negate(); p.normalImpulse = 0; p.penetration = penetration; p.warmStarted = false; }, // Add a point into this manifold. addPoint: function ( x, y, z, nx, ny, nz, penetration, flip ) { var p = this.points[ this.numPoints++ ]; p.position.set( x, y, z ); p.localPoint1.sub( p.position, this.body1.position ).applyMatrix3( this.body1.rotation ); p.localPoint2.sub( p.position, this.body2.position ).applyMatrix3( this.body2.rotation ); p.normalImpulse = 0; p.normal.set( nx, ny, nz ); if( flip ) p.normal.negate(); p.penetration = penetration; p.warmStarted = false; } }; function ContactPointDataBuffer (){ this.nor = new Vec3(); this.tan = new Vec3(); this.bin = new Vec3(); this.norU1 = new Vec3(); this.tanU1 = new Vec3(); this.binU1 = new Vec3(); this.norU2 = new Vec3(); this.tanU2 = new Vec3(); this.binU2 = new Vec3(); this.norT1 = new Vec3(); this.tanT1 = new Vec3(); this.binT1 = new Vec3(); this.norT2 = new Vec3(); this.tanT2 = new Vec3(); this.binT2 = new Vec3(); this.norTU1 = new Vec3(); this.tanTU1 = new Vec3(); this.binTU1 = new Vec3(); this.norTU2 = new Vec3(); this.tanTU2 = new Vec3(); this.binTU2 = new Vec3(); this.norImp = 0; this.tanImp = 0; this.binImp = 0; this.norDen = 0; this.tanDen = 0; this.binDen = 0; this.norTar = 0; this.next = null; this.last = false; } /** * ... * @author saharan */ function ContactConstraint ( manifold ){ Constraint.call( this ); // The contact manifold of the constraint. this.manifold = manifold; // The coefficient of restitution of the constraint. this.restitution=NaN; // The coefficient of friction of the constraint. this.friction=NaN; this.p1=null; this.p2=null; this.lv1=null; this.lv2=null; this.av1=null; this.av2=null; this.i1=null; this.i2=null; //this.ii1 = null; //this.ii2 = null; this.tmp = new Vec3(); this.tmpC1 = new Vec3(); this.tmpC2 = new Vec3(); this.tmpP1 = new Vec3(); this.tmpP2 = new Vec3(); this.tmplv1 = new Vec3(); this.tmplv2 = new Vec3(); this.tmpav1 = new Vec3(); this.tmpav2 = new Vec3(); this.m1=NaN; this.m2=NaN; this.num=0; this.ps = manifold.points; this.cs = new ContactPointDataBuffer(); this.cs.next = new ContactPointDataBuffer(); this.cs.next.next = new ContactPointDataBuffer(); this.cs.next.next.next = new ContactPointDataBuffer(); } ContactConstraint.prototype = Object.assign( Object.create( Constraint.prototype ), { constructor: ContactConstraint, // Attach the constraint to the bodies. attach: function(){ this.p1=this.body1.position; this.p2=this.body2.position; this.lv1=this.body1.linearVelocity; this.av1=this.body1.angularVelocity; this.lv2=this.body2.linearVelocity; this.av2=this.body2.angularVelocity; this.i1=this.body1.inverseInertia; this.i2=this.body2.inverseInertia; }, // Detach the constraint from the bodies. detach: function(){ this.p1=null; this.p2=null; this.lv1=null; this.lv2=null; this.av1=null; this.av2=null; this.i1=null; this.i2=null; }, preSolve: function( timeStep, invTimeStep ){ this.m1 = this.body1.inverseMass; this.m2 = this.body2.inverseMass; var m1m2 = this.m1 + this.m2; this.num = this.manifold.numPoints; var c = this.cs; var p, rvn, len, norImp, norTar, sepV, i1, i2; for( var i=0; i < this.num; i++ ){ p = this.ps[i]; this.tmpP1.sub( p.position, this.p1 ); this.tmpP2.sub( p.position, this.p2 ); this.tmpC1.crossVectors( this.av1, this.tmpP1 ); this.tmpC2.crossVectors( this.av2, this.tmpP2 ); c.norImp = p.normalImpulse; c.tanImp = p.tangentImpulse; c.binImp = p.binormalImpulse; c.nor.copy( p.normal ); this.tmp.set( ( this.lv2.x + this.tmpC2.x ) - ( this.lv1.x + this.tmpC1.x ), ( this.lv2.y + this.tmpC2.y ) - ( this.lv1.y + this.tmpC1.y ), ( this.lv2.z + this.tmpC2.z ) - ( this.lv1.z + this.tmpC1.z ) ); rvn = _Math.dotVectors( c.nor, this.tmp ); c.tan.set( this.tmp.x - rvn * c.nor.x, this.tmp.y - rvn * c.nor.y, this.tmp.z - rvn * c.nor.z ); len = _Math.dotVectors( c.tan, c.tan ); if( len <= 0.04 ) { c.tan.tangent( c.nor ); } c.tan.normalize(); c.bin.crossVectors( c.nor, c.tan ); c.norU1.scale( c.nor, this.m1 ); c.norU2.scale( c.nor, this.m2 ); c.tanU1.scale( c.tan, this.m1 ); c.tanU2.scale( c.tan, this.m2 ); c.binU1.scale( c.bin, this.m1 ); c.binU2.scale( c.bin, this.m2 ); c.norT1.crossVectors( this.tmpP1, c.nor ); c.tanT1.crossVectors( this.tmpP1, c.tan ); c.binT1.crossVectors( this.tmpP1, c.bin ); c.norT2.crossVectors( this.tmpP2, c.nor ); c.tanT2.crossVectors( this.tmpP2, c.tan ); c.binT2.crossVectors( this.tmpP2, c.bin ); i1 = this.i1; i2 = this.i2; c.norTU1.copy( c.norT1 ).applyMatrix3( i1, true ); c.tanTU1.copy( c.tanT1 ).applyMatrix3( i1, true ); c.binTU1.copy( c.binT1 ).applyMatrix3( i1, true ); c.norTU2.copy( c.norT2 ).applyMatrix3( i2, true ); c.tanTU2.copy( c.tanT2 ).applyMatrix3( i2, true ); c.binTU2.copy( c.binT2 ).applyMatrix3( i2, true ); /*c.norTU1.mulMat( this.i1, c.norT1 ); c.tanTU1.mulMat( this.i1, c.tanT1 ); c.binTU1.mulMat( this.i1, c.binT1 ); c.norTU2.mulMat( this.i2, c.norT2 ); c.tanTU2.mulMat( this.i2, c.tanT2 ); c.binTU2.mulMat( this.i2, c.binT2 );*/ this.tmpC1.crossVectors( c.norTU1, this.tmpP1 ); this.tmpC2.crossVectors( c.norTU2, this.tmpP2 ); this.tmp.add( this.tmpC1, this.tmpC2 ); c.norDen = 1 / ( m1m2 +_Math.dotVectors( c.nor, this.tmp )); this.tmpC1.crossVectors( c.tanTU1, this.tmpP1 ); this.tmpC2.crossVectors( c.tanTU2, this.tmpP2 ); this.tmp.add( this.tmpC1, this.tmpC2 ); c.tanDen = 1 / ( m1m2 +_Math.dotVectors( c.tan, this.tmp )); this.tmpC1.crossVectors( c.binTU1, this.tmpP1 ); this.tmpC2.crossVectors( c.binTU2, this.tmpP2 ); this.tmp.add( this.tmpC1, this.tmpC2 ); c.binDen = 1 / ( m1m2 +_Math.dotVectors( c.bin, this.tmp )); if( p.warmStarted ){ norImp = p.normalImpulse; this.lv1.addScaledVector( c.norU1, norImp ); this.av1.addScaledVector( c.norTU1, norImp ); this.lv2.subScaledVector( c.norU2, norImp ); this.av2.subScaledVector( c.norTU2, norImp ); c.norImp = norImp; c.tanImp = 0; c.binImp = 0; rvn = 0; // disable bouncing } else { c.norImp=0; c.tanImp=0; c.binImp=0; } if(rvn>-1) rvn=0; // disable bouncing norTar = this.restitution*-rvn; sepV = -(p.penetration+0.005)*invTimeStep*0.05; // allow 0.5cm error if(norTar max * max ){ len = max/_Math.sqrt(len); tanImp *= len; binImp *= len; } newImp1 = tanImp-oldImp1; newImp2 = binImp-oldImp2; // this.tmp.set( c.tanU1.x*newImp1 + c.binU1.x*newImp2, c.tanU1.y*newImp1 + c.binU1.y*newImp2, c.tanU1.z*newImp1 + c.binU1.z*newImp2 ); this.tmplv1.addEqual( this.tmp ); this.tmp.set( c.tanTU1.x*newImp1 + c.binTU1.x*newImp2, c.tanTU1.y*newImp1 + c.binTU1.y*newImp2, c.tanTU1.z*newImp1 + c.binTU1.z*newImp2 ); this.tmpav1.addEqual( this.tmp ); this.tmp.set( c.tanU2.x*newImp1 + c.binU2.x*newImp2, c.tanU2.y*newImp1 + c.binU2.y*newImp2, c.tanU2.z*newImp1 + c.binU2.z*newImp2 ); this.tmplv2.subEqual( this.tmp ); this.tmp.set( c.tanTU2.x*newImp1 + c.binTU2.x*newImp2, c.tanTU2.y*newImp1 + c.binTU2.y*newImp2, c.tanTU2.z*newImp1 + c.binTU2.z*newImp2 ); this.tmpav2.subEqual( this.tmp ); // restitution part this.tmp.sub( this.tmplv2, this.tmplv1 ); rvn = _Math.dotVectors( this.tmp, c.nor ) + _Math.dotVectors( this.tmpav2, c.norT2 ) - _Math.dotVectors( this.tmpav1, c.norT1 ); oldImp1 = norImp; newImp1 = (rvn-c.norTar)*c.norDen; norImp += newImp1; if( norImp > 0 ) norImp = 0; newImp1 = norImp - oldImp1; this.tmplv1.addScaledVector( c.norU1, newImp1 ); this.tmpav1.addScaledVector( c.norTU1, newImp1 ); this.tmplv2.subScaledVector( c.norU2, newImp1 ); this.tmpav2.subScaledVector( c.norTU2, newImp1 ); c.norImp = norImp; c.tanImp = tanImp; c.binImp = binImp; if(c.last)break; c = c.next; } this.lv1.copy( this.tmplv1 ); this.lv2.copy( this.tmplv2 ); this.av1.copy( this.tmpav1 ); this.av2.copy( this.tmpav2 ); }, postSolve: function(){ var c = this.cs, p; var i = this.num; while(i--){ //for(var i=0;i -1 ){ this.proxies.splice( n, 1 ); //this.numProxies--; } /*var i = this.numProxies; while(i--){ //for(var i=0, l=this.numProxies;i>1; //this.numPairChecks=this.numProxies*(this.numProxies-1)*0.5; while( i < l ){ p1 = px[i++]; j = i + 1; while( j < l ){ p2 = px[j++]; if ( p1.aabb.intersectTest( p2.aabb ) || !this.isAvailablePair( p1.shape, p2.shape ) ) continue; this.addPair( p1.shape, p2.shape ); } } } }); /** * A projection axis for sweep and prune broad-phase. * @author saharan */ function SAPAxis (){ this.numElements = 0; this.bufferSize = 256; this.elements = []; this.elements.length = this.bufferSize; this.stack = new Float32Array( 64 ); } Object.assign( SAPAxis.prototype, { SAPAxis: true, addElements: function ( min, max ) { if(this.numElements+2>=this.bufferSize){ //this.bufferSize<<=1; this.bufferSize*=2; var newElements=[]; var i = this.numElements; while(i--){ //for(var i=0, l=this.numElements; i> threshold) != 0 ) threshold++; threshold = threshold * this.numElements >> 2; count = 0; var giveup = false; var elements = this.elements; for( var i = 1, l = this.numElements; i < l; i++){ // try insertion sort var tmp=elements[i]; var pivot=tmp.value; var tmp2=elements[i-1]; if(tmp2.value>pivot){ var j=i; do{ elements[j]=tmp2; if(--j==0)break; tmp2=elements[j-1]; }while(tmp2.value>pivot); elements[j]=tmp; count+=i-j; if(count>threshold){ giveup=true; // stop and use quick sort break; } } } if(!giveup)return; count=2;var stack=this.stack; stack[0]=0; stack[1]=this.numElements-1; while(count>0){ var right=stack[--count]; var left=stack[--count]; var diff=right-left; if(diff>16){ // quick sort //var mid=left+(diff>>1); var mid = left + (_Math.floor(diff*0.5)); tmp = elements[mid]; elements[mid] = elements[right]; elements[right] = tmp; pivot = tmp.value; i = left-1; j = right; while( true ){ var ei; var ej; do{ ei = elements[++i]; } while( ei.value < pivot); do{ ej = elements[--j]; } while( pivot < ej.value && j != left ); if( i >= j ) break; elements[i] = ej; elements[j] = ei; } elements[right] = elements[i]; elements[i] = tmp; if( i - left > right - i ) { stack[count++] = left; stack[count++] = i - 1; stack[count++] = i + 1; stack[count++] = right; }else{ stack[count++] = i + 1; stack[count++] = right; stack[count++] = left; stack[count++] = i - 1; } }else{ for( i = left + 1; i <= right; i++ ) { tmp = elements[i]; pivot = tmp.value; tmp2 = elements[i-1]; if( tmp2.value > pivot ) { j = i; do{ elements[j] = tmp2; if( --j == 0 ) break; tmp2 = elements[j-1]; }while( tmp2.value > pivot ); elements[j] = tmp; } } } } }, calculateTestCount: function () { var num = 1; var sum = 0; for(var i = 1, l = this.numElements; i e2.max1.value || max1 < e2.min1.value || min2 > e2.max2.value || max2 < e2.min2.value || !this.isAvailablePair( s1, s2 ) ) continue; this.addPair( s1, s2 ); } if( dyn ){ for( e2 = activeS; e2 != null; e2 = e2.pair ) {// test for static s2 = e2.proxy.shape; this.numPairChecks++; if( min1 > e2.max1.value || max1 < e2.min1.value|| min2 > e2.max2.value || max2 < e2.min2.value || !this.isAvailablePair(s1,s2) ) continue; this.addPair( s1, s2 ); } e1.pair = activeD; activeD = e1; }else{ e1.pair = activeS; activeS = e1; } }else{ var min = e1.pair; if( dyn ){ if( min == activeD ){ activeD = activeD.pair; continue; }else{ e1 = activeD; } }else{ if( min == activeS ){ activeS = activeS.pair; continue; }else{ e1 = activeS; } } do{ e2 = e1.pair; if( e2 == min ){ e1.pair = e2.pair; break; } e1 = e2; }while( e1 != null ); } } this.index2 = (this.index1|this.index2)^3; } }); /** * A node of the dynamic bounding volume tree. * @author saharan */ function DBVTNode(){ // The first child node of this node. this.child1 = null; // The second child node of this node. this.child2 = null; // The parent node of this tree. this.parent = null; // The proxy of this node. This has no value if this node is not leaf. this.proxy = null; // The maximum distance from leaf nodes. this.height = 0; // The AABB of this node. this.aabb = new AABB(); } /** * A dynamic bounding volume tree for the broad-phase algorithm. * * @author saharan * @author lo-th */ function DBVT(){ // The root of the tree. this.root = null; this.freeNodes = []; this.freeNodes.length = 16384; this.numFreeNodes = 0; this.aabb = new AABB(); } Object.assign( DBVT.prototype, { DBVT: true, moveLeaf: function( leaf ) { this.deleteLeaf( leaf ); this.insertLeaf( leaf ); }, insertLeaf: function ( leaf ) { if(this.root == null){ this.root = leaf; return; } var lb = leaf.aabb; var sibling = this.root; var oldArea; var newArea; while(sibling.proxy == null){ // descend the node to search the best pair var c1 = sibling.child1; var c2 = sibling.child2; var b = sibling.aabb; var c1b = c1.aabb; var c2b = c2.aabb; oldArea = b.surfaceArea(); this.aabb.combine(lb,b); newArea = this.aabb.surfaceArea(); var creatingCost = newArea*2; var incrementalCost = (newArea-oldArea)*2; // cost of creating a new pair with the node var discendingCost1 = incrementalCost; this.aabb.combine(lb,c1b); if(c1.proxy!=null){ // leaf cost = area(combined aabb) discendingCost1+=this.aabb.surfaceArea(); }else{ // node cost = area(combined aabb) - area(old aabb) discendingCost1+=this.aabb.surfaceArea()-c1b.surfaceArea(); } var discendingCost2=incrementalCost; this.aabb.combine(lb,c2b); if(c2.proxy!=null){ // leaf cost = area(combined aabb) discendingCost2+=this.aabb.surfaceArea(); }else{ // node cost = area(combined aabb) - area(old aabb) discendingCost2+=this.aabb.surfaceArea()-c2b.surfaceArea(); } if(discendingCost10){ newParent = this.freeNodes[--this.numFreeNodes]; }else{ newParent = new DBVTNode(); } newParent.parent = oldParent; newParent.child1 = leaf; newParent.child2 = sibling; newParent.aabb.combine(leaf.aabb,sibling.aabb); newParent.height = sibling.height+1; sibling.parent = newParent; leaf.parent = newParent; if(sibling == this.root){ // replace root this.root = newParent; }else{ // replace child if(oldParent.child1 == sibling){ oldParent.child1 = newParent; }else{ oldParent.child2 = newParent; } } // update whole tree do{ newParent = this.balance(newParent); this.fix(newParent); newParent = newParent.parent; }while(newParent != null); }, getBalance: function( node ) { if(node.proxy!=null)return 0; return node.child1.height-node.child2.height; }, deleteLeaf: function( leaf ) { if(leaf == this.root){ this.root = null; return; } var parent = leaf.parent; var sibling; if(parent.child1==leaf){ sibling=parent.child2; }else{ sibling=parent.child1; } if(parent==this.root){ this.root=sibling; sibling.parent=null; return; } var grandParent = parent.parent; sibling.parent = grandParent; if(grandParent.child1 == parent ) { grandParent.child1 = sibling; }else{ grandParent.child2 = sibling; } if(this.numFreeNodes<16384){ this.freeNodes[this.numFreeNodes++] = parent; } do{ grandParent = this.balance(grandParent); this.fix(grandParent); grandParent = grandParent.parent; }while( grandParent != null ); }, balance: function( node ) { var nh = node.height; if(nh<2){ return node; } var p = node.parent; var l = node.child1; var r = node.child2; var lh = l.height; var rh = r.height; var balance = lh-rh; var t;// for bit operation // [ N ] // / \ // [ L ] [ R ] // / \ / \ // [L-L] [L-R] [R-L] [R-R] // Is the tree balanced? if(balance>1){ var ll = l.child1; var lr = l.child2; var llh = ll.height; var lrh = lr.height; // Is L-L higher than L-R? if(llh>lrh){ // set N to L-R l.child2 = node; node.parent = l; // [ L ] // / \ // [L-L] [ N ] // / \ / \ // [...] [...] [ L ] [ R ] // set L-R node.child1 = lr; lr.parent = node; // [ L ] // / \ // [L-L] [ N ] // / \ / \ // [...] [...] [L-R] [ R ] // fix bounds and heights node.aabb.combine( lr.aabb, r.aabb ); t = lrh-rh; node.height=lrh-(t&t>>31)+1; l.aabb.combine(ll.aabb,node.aabb); t=llh-nh; l.height=llh-(t&t>>31)+1; }else{ // set N to L-L l.child1=node; node.parent=l; // [ L ] // / \ // [ N ] [L-R] // / \ / \ // [ L ] [ R ] [...] [...] // set L-L node.child1 = ll; ll.parent = node; // [ L ] // / \ // [ N ] [L-R] // / \ / \ // [L-L] [ R ] [...] [...] // fix bounds and heights node.aabb.combine(ll.aabb,r.aabb); t = llh - rh; node.height=llh-(t&t>>31)+1; l.aabb.combine(node.aabb,lr.aabb); t=nh-lrh; l.height=nh-(t&t>>31)+1; } // set new parent of L if(p!=null){ if(p.child1==node){ p.child1=l; }else{ p.child2=l; } }else{ this.root=l; } l.parent=p; return l; }else if(balance<-1){ var rl = r.child1; var rr = r.child2; var rlh = rl.height; var rrh = rr.height; // Is R-L higher than R-R? if( rlh > rrh ) { // set N to R-R r.child2 = node; node.parent = r; // [ R ] // / \ // [R-L] [ N ] // / \ / \ // [...] [...] [ L ] [ R ] // set R-R node.child2 = rr; rr.parent = node; // [ R ] // / \ // [R-L] [ N ] // / \ / \ // [...] [...] [ L ] [R-R] // fix bounds and heights node.aabb.combine(l.aabb,rr.aabb); t = lh-rrh; node.height = lh-(t&t>>31)+1; r.aabb.combine(rl.aabb,node.aabb); t = rlh-nh; r.height = rlh-(t&t>>31)+1; }else{ // set N to R-L r.child1 = node; node.parent = r; // [ R ] // / \ // [ N ] [R-R] // / \ / \ // [ L ] [ R ] [...] [...] // set R-L node.child2 = rl; rl.parent = node; // [ R ] // / \ // [ N ] [R-R] // / \ / \ // [ L ] [R-L] [...] [...] // fix bounds and heights node.aabb.combine(l.aabb,rl.aabb); t=lh-rlh; node.height=lh-(t&t>>31)+1; r.aabb.combine(node.aabb,rr.aabb); t=nh-rrh; r.height=nh-(t&t>>31)+1; } // set new parent of R if(p!=null){ if(p.child1==node){ p.child1=r; }else{ p.child2=r; } }else{ this.root=r; } r.parent=p; return r; } return node; }, fix: function ( node ) { var c1 = node.child1; var c2 = node.child2; node.aabb.combine( c1.aabb, c2.aabb ); node.height = c1.height < c2.height ? c2.height+1 : c1.height+1; } }); /** * A proxy for dynamic bounding volume tree broad-phase. * @author saharan */ function DBVTProxy ( shape ) { Proxy.call( this, shape); // The leaf of the proxy. this.leaf = new DBVTNode(); this.leaf.proxy = this; } DBVTProxy.prototype = Object.assign( Object.create( Proxy.prototype ), { constructor: DBVTProxy, update: function () { } }); /** * A broad-phase algorithm using dynamic bounding volume tree. * * @author saharan * @author lo-th */ function DBVTBroadPhase(){ BroadPhase.call( this ); this.types = BR_BOUNDING_VOLUME_TREE; this.tree = new DBVT(); this.stack = []; this.leaves = []; this.numLeaves = 0; } DBVTBroadPhase.prototype = Object.assign( Object.create( BroadPhase.prototype ), { constructor: DBVTBroadPhase, createProxy: function ( shape ) { return new DBVTProxy( shape ); }, addProxy: function ( proxy ) { this.tree.insertLeaf( proxy.leaf ); this.leaves.push( proxy.leaf ); this.numLeaves++; }, removeProxy: function ( proxy ) { this.tree.deleteLeaf( proxy.leaf ); var n = this.leaves.indexOf( proxy.leaf ); if ( n > -1 ) { this.leaves.splice(n,1); this.numLeaves--; } }, collectPairs: function () { if ( this.numLeaves < 2 ) return; var leaf, margin = 0.1, i = this.numLeaves; while(i--){ leaf = this.leaves[i]; if ( leaf.proxy.aabb.intersectTestTwo( leaf.aabb ) ){ leaf.aabb.copy( leaf.proxy.aabb, margin ); this.tree.deleteLeaf( leaf ); this.tree.insertLeaf( leaf ); this.collide( leaf, this.tree.root ); } } }, collide: function ( node1, node2 ) { var stackCount = 2; var s1, s2, n1, n2, l1, l2; this.stack[0] = node1; this.stack[1] = node2; while( stackCount > 0 ){ n1 = this.stack[--stackCount]; n2 = this.stack[--stackCount]; l1 = n1.proxy != null; l2 = n2.proxy != null; this.numPairChecks++; if( l1 && l2 ){ s1 = n1.proxy.shape; s2 = n2.proxy.shape; if ( s1 == s2 || s1.aabb.intersectTest( s2.aabb ) || !this.isAvailablePair( s1, s2 ) ) continue; this.addPair(s1,s2); }else{ if ( n1.aabb.intersectTest( n2.aabb ) ) continue; /*if(stackCount+4>=this.maxStack){// expand the stack //this.maxStack<<=1; this.maxStack*=2; var newStack = [];// vector newStack.length = this.maxStack; for(var i=0;i n2.aabb.surfaceArea()) ){ this.stack[stackCount++] = n1.child1; this.stack[stackCount++] = n2; this.stack[stackCount++] = n1.child2; this.stack[stackCount++] = n2; }else{ this.stack[stackCount++] = n1; this.stack[stackCount++] = n2.child1; this.stack[stackCount++] = n1; this.stack[stackCount++] = n2.child2; } } } } }); function CollisionDetector (){ this.flip = false; } Object.assign( CollisionDetector.prototype, { CollisionDetector: true, detectCollision: function ( shape1, shape2, manifold ) { printError("CollisionDetector", "Inheritance error."); } } ); /** * A collision detector which detects collisions between two boxes. * @author saharan */ function BoxBoxCollisionDetector() { CollisionDetector.call( this ); this.clipVertices1 = new Float32Array( 24 ); // 8 vertices x,y,z this.clipVertices2 = new Float32Array( 24 ); this.used = new Float32Array( 8 ); this.INF = 1/0; } BoxBoxCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), { constructor: BoxBoxCollisionDetector, detectCollision: function ( shape1, shape2, manifold ) { // What you are doing // · I to prepare a separate axis of the fifteen //-Six in each of three normal vectors of the xyz direction of the box both // · Remaining nine 3x3 a vector perpendicular to the side of the box 2 and the side of the box 1 // · Calculate the depth to the separation axis // Calculates the distance using the inner product and put the amount of embedment // · However a vertical separation axis and side to weight a little to avoid vibration // And end when there is a separate axis that is remote even one // · I look for separation axis with little to dent most // Men and if separation axis of the first six - end collision // Heng If it separate axis of nine other - side collision // Heng - case of a side collision // · Find points of two sides on which you made ​​the separation axis // Calculates the point of closest approach of a straight line consisting of separate axis points obtained, and the collision point //-Surface - the case of the plane crash //-Box A, box B and the other a box of better made ​​a separate axis // • The surface A and the plane that made the separation axis of the box A, and B to the surface the face of the box B close in the opposite direction to the most isolated axis // When viewed from the front surface A, and the cut part exceeding the area of the surface A is a surface B //-Plane B becomes the 3-8 triangle, I a candidate for the collision point the vertex of surface B // • If more than one candidate 5 exists, scraping up to four // For potential collision points of all, to examine the distance between the surface A // • If you were on the inside surface of A, and the collision point var b1; var b2; if(shape1.id0; if(!right1)len=-len; len1=w1; dot1=a1x*a4x+a1y*a4y+a1z*a4z; dot2=a1x*a5x+a1y*a5y+a1z*a5z; dot3=a1x*a6x+a1y*a6y+a1z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len2=dot1*w2+dot2*h2+dot3*d2; overlap1=len-len1-len2; if(overlap1>0)return; // try axis 2 len=a2x*dx+a2y*dy+a2z*dz; right2=len>0; if(!right2)len=-len; len1=h1; dot1=a2x*a4x+a2y*a4y+a2z*a4z; dot2=a2x*a5x+a2y*a5y+a2z*a5z; dot3=a2x*a6x+a2y*a6y+a2z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len2=dot1*w2+dot2*h2+dot3*d2; overlap2=len-len1-len2; if(overlap2>0)return; // try axis 3 len=a3x*dx+a3y*dy+a3z*dz; right3=len>0; if(!right3)len=-len; len1=d1; dot1=a3x*a4x+a3y*a4y+a3z*a4z; dot2=a3x*a5x+a3y*a5y+a3z*a5z; dot3=a3x*a6x+a3y*a6y+a3z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len2=dot1*w2+dot2*h2+dot3*d2; overlap3=len-len1-len2; if(overlap3>0)return; // try axis 4 len=a4x*dx+a4y*dy+a4z*dz; right4=len>0; if(!right4)len=-len; dot1=a4x*a1x+a4y*a1y+a4z*a1z; dot2=a4x*a2x+a4y*a2y+a4z*a2z; dot3=a4x*a3x+a4y*a3y+a4z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len1=dot1*w1+dot2*h1+dot3*d1; len2=w2; overlap4=(len-len1-len2)*1.0; if(overlap4>0)return; // try axis 5 len=a5x*dx+a5y*dy+a5z*dz; right5=len>0; if(!right5)len=-len; dot1=a5x*a1x+a5y*a1y+a5z*a1z; dot2=a5x*a2x+a5y*a2y+a5z*a2z; dot3=a5x*a3x+a5y*a3y+a5z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len1=dot1*w1+dot2*h1+dot3*d1; len2=h2; overlap5=(len-len1-len2)*1.0; if(overlap5>0)return; // try axis 6 len=a6x*dx+a6y*dy+a6z*dz; right6=len>0; if(!right6)len=-len; dot1=a6x*a1x+a6y*a1y+a6z*a1z; dot2=a6x*a2x+a6y*a2y+a6z*a2z; dot3=a6x*a3x+a6y*a3y+a6z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len1=dot1*w1+dot2*h1+dot3*d1; len2=d2; overlap6=(len-len1-len2)*1.0; if(overlap6>0)return; // try axis 7 len=a7x*a7x+a7y*a7y+a7z*a7z; if(len>1e-5){ len=1/_Math.sqrt(len); a7x*=len; a7y*=len; a7z*=len; len=a7x*dx+a7y*dy+a7z*dz; right7=len>0; if(!right7)len=-len; dot1=a7x*a2x+a7y*a2y+a7z*a2z; dot2=a7x*a3x+a7y*a3y+a7z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*h1+dot2*d1; dot1=a7x*a5x+a7y*a5y+a7z*a5z; dot2=a7x*a6x+a7y*a6y+a7z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*h2+dot2*d2; overlap7=len-len1-len2; if(overlap7>0)return; }else{ right7=false; overlap7=0; invalid7=true; } // try axis 8 len=a8x*a8x+a8y*a8y+a8z*a8z; if(len>1e-5){ len=1/_Math.sqrt(len); a8x*=len; a8y*=len; a8z*=len; len=a8x*dx+a8y*dy+a8z*dz; right8=len>0; if(!right8)len=-len; dot1=a8x*a2x+a8y*a2y+a8z*a2z; dot2=a8x*a3x+a8y*a3y+a8z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*h1+dot2*d1; dot1=a8x*a4x+a8y*a4y+a8z*a4z; dot2=a8x*a6x+a8y*a6y+a8z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*d2; overlap8=len-len1-len2; if(overlap8>0)return; }else{ right8=false; overlap8=0; invalid8=true; } // try axis 9 len=a9x*a9x+a9y*a9y+a9z*a9z; if(len>1e-5){ len=1/_Math.sqrt(len); a9x*=len; a9y*=len; a9z*=len; len=a9x*dx+a9y*dy+a9z*dz; right9=len>0; if(!right9)len=-len; dot1=a9x*a2x+a9y*a2y+a9z*a2z; dot2=a9x*a3x+a9y*a3y+a9z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*h1+dot2*d1; dot1=a9x*a4x+a9y*a4y+a9z*a4z; dot2=a9x*a5x+a9y*a5y+a9z*a5z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*h2; overlap9=len-len1-len2; if(overlap9>0)return; }else{ right9=false; overlap9=0; invalid9=true; } // try axis 10 len=aax*aax+aay*aay+aaz*aaz; if(len>1e-5){ len=1/_Math.sqrt(len); aax*=len; aay*=len; aaz*=len; len=aax*dx+aay*dy+aaz*dz; righta=len>0; if(!righta)len=-len; dot1=aax*a1x+aay*a1y+aaz*a1z; dot2=aax*a3x+aay*a3y+aaz*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*d1; dot1=aax*a5x+aay*a5y+aaz*a5z; dot2=aax*a6x+aay*a6y+aaz*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*h2+dot2*d2; overlapa=len-len1-len2; if(overlapa>0)return; }else{ righta=false; overlapa=0; invalida=true; } // try axis 11 len=abx*abx+aby*aby+abz*abz; if(len>1e-5){ len=1/_Math.sqrt(len); abx*=len; aby*=len; abz*=len; len=abx*dx+aby*dy+abz*dz; rightb=len>0; if(!rightb)len=-len; dot1=abx*a1x+aby*a1y+abz*a1z; dot2=abx*a3x+aby*a3y+abz*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*d1; dot1=abx*a4x+aby*a4y+abz*a4z; dot2=abx*a6x+aby*a6y+abz*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*d2; overlapb=len-len1-len2; if(overlapb>0)return; }else{ rightb=false; overlapb=0; invalidb=true; } // try axis 12 len=acx*acx+acy*acy+acz*acz; if(len>1e-5){ len=1/_Math.sqrt(len); acx*=len; acy*=len; acz*=len; len=acx*dx+acy*dy+acz*dz; rightc=len>0; if(!rightc)len=-len; dot1=acx*a1x+acy*a1y+acz*a1z; dot2=acx*a3x+acy*a3y+acz*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*d1; dot1=acx*a4x+acy*a4y+acz*a4z; dot2=acx*a5x+acy*a5y+acz*a5z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*h2; overlapc=len-len1-len2; if(overlapc>0)return; }else{ rightc=false; overlapc=0; invalidc=true; } // try axis 13 len=adx*adx+ady*ady+adz*adz; if(len>1e-5){ len=1/_Math.sqrt(len); adx*=len; ady*=len; adz*=len; len=adx*dx+ady*dy+adz*dz; rightd=len>0; if(!rightd)len=-len; dot1=adx*a1x+ady*a1y+adz*a1z; dot2=adx*a2x+ady*a2y+adz*a2z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*h1; dot1=adx*a5x+ady*a5y+adz*a5z; dot2=adx*a6x+ady*a6y+adz*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*h2+dot2*d2; overlapd=len-len1-len2; if(overlapd>0)return; }else{ rightd=false; overlapd=0; invalidd=true; } // try axis 14 len=aex*aex+aey*aey+aez*aez; if(len>1e-5){ len=1/_Math.sqrt(len); aex*=len; aey*=len; aez*=len; len=aex*dx+aey*dy+aez*dz; righte=len>0; if(!righte)len=-len; dot1=aex*a1x+aey*a1y+aez*a1z; dot2=aex*a2x+aey*a2y+aez*a2z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*h1; dot1=aex*a4x+aey*a4y+aez*a4z; dot2=aex*a6x+aey*a6y+aez*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*d2; overlape=len-len1-len2; if(overlape>0)return; }else{ righte=false; overlape=0; invalide=true; } // try axis 15 len=afx*afx+afy*afy+afz*afz; if(len>1e-5){ len=1/_Math.sqrt(len); afx*=len; afy*=len; afz*=len; len=afx*dx+afy*dy+afz*dz; rightf=len>0; if(!rightf)len=-len; dot1=afx*a1x+afy*a1y+afz*a1z; dot2=afx*a2x+afy*a2y+afz*a2z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*h1; dot1=afx*a4x+afy*a4y+afz*a4z; dot2=afx*a5x+afy*a5y+afz*a5z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*h2; overlapf=len-len1-len2; if(overlapf>0)return; }else{ rightf=false; overlapf=0; invalidf=true; } // boxes are overlapping var depth=overlap1; var depth2=overlap1; var minIndex=0; var right=right1; if(overlap2>depth2){ depth=overlap2; depth2=overlap2; minIndex=1; right=right2; } if(overlap3>depth2){ depth=overlap3; depth2=overlap3; minIndex=2; right=right3; } if(overlap4>depth2){ depth=overlap4; depth2=overlap4; minIndex=3; right=right4; } if(overlap5>depth2){ depth=overlap5; depth2=overlap5; minIndex=4; right=right5; } if(overlap6>depth2){ depth=overlap6; depth2=overlap6; minIndex=5; right=right6; } if(overlap7-0.01>depth2&&!invalid7){ depth=overlap7; depth2=overlap7-0.01; minIndex=6; right=right7; } if(overlap8-0.01>depth2&&!invalid8){ depth=overlap8; depth2=overlap8-0.01; minIndex=7; right=right8; } if(overlap9-0.01>depth2&&!invalid9){ depth=overlap9; depth2=overlap9-0.01; minIndex=8; right=right9; } if(overlapa-0.01>depth2&&!invalida){ depth=overlapa; depth2=overlapa-0.01; minIndex=9; right=righta; } if(overlapb-0.01>depth2&&!invalidb){ depth=overlapb; depth2=overlapb-0.01; minIndex=10; right=rightb; } if(overlapc-0.01>depth2&&!invalidc){ depth=overlapc; depth2=overlapc-0.01; minIndex=11; right=rightc; } if(overlapd-0.01>depth2&&!invalidd){ depth=overlapd; depth2=overlapd-0.01; minIndex=12; right=rightd; } if(overlape-0.01>depth2&&!invalide){ depth=overlape; depth2=overlape-0.01; minIndex=13; right=righte; } if(overlapf-0.01>depth2&&!invalidf){ depth=overlapf; minIndex=14; right=rightf; } // normal var nx=0; var ny=0; var nz=0; // edge line or face side normal var n1x=0; var n1y=0; var n1z=0; var n2x=0; var n2y=0; var n2z=0; // center of current face var cx=0; var cy=0; var cz=0; // face side var s1x=0; var s1y=0; var s1z=0; var s2x=0; var s2y=0; var s2z=0; // swap b1 b2 var swap=false; //_______________________________________ if(minIndex==0){// b1.x * b2 if(right){ cx=p1x+d1x; cy=p1y+d1y; cz=p1z+d1z; nx=a1x; ny=a1y; nz=a1z; }else{ cx=p1x-d1x; cy=p1y-d1y; cz=p1z-d1z; nx=-a1x; ny=-a1y; nz=-a1z; } s1x=d2x; s1y=d2y; s1z=d2z; n1x=-a2x; n1y=-a2y; n1z=-a2z; s2x=d3x; s2y=d3y; s2z=d3z; n2x=-a3x; n2y=-a3y; n2z=-a3z; } else if(minIndex==1){// b1.y * b2 if(right){ cx=p1x+d2x; cy=p1y+d2y; cz=p1z+d2z; nx=a2x; ny=a2y; nz=a2z; }else{ cx=p1x-d2x; cy=p1y-d2y; cz=p1z-d2z; nx=-a2x; ny=-a2y; nz=-a2z; } s1x=d1x; s1y=d1y; s1z=d1z; n1x=-a1x; n1y=-a1y; n1z=-a1z; s2x=d3x; s2y=d3y; s2z=d3z; n2x=-a3x; n2y=-a3y; n2z=-a3z; } else if(minIndex==2){// b1.z * b2 if(right){ cx=p1x+d3x; cy=p1y+d3y; cz=p1z+d3z; nx=a3x; ny=a3y; nz=a3z; }else{ cx=p1x-d3x; cy=p1y-d3y; cz=p1z-d3z; nx=-a3x; ny=-a3y; nz=-a3z; } s1x=d1x; s1y=d1y; s1z=d1z; n1x=-a1x; n1y=-a1y; n1z=-a1z; s2x=d2x; s2y=d2y; s2z=d2z; n2x=-a2x; n2y=-a2y; n2z=-a2z; } else if(minIndex==3){// b2.x * b1 swap=true; if(!right){ cx=p2x+d4x; cy=p2y+d4y; cz=p2z+d4z; nx=a4x; ny=a4y; nz=a4z; }else{ cx=p2x-d4x; cy=p2y-d4y; cz=p2z-d4z; nx=-a4x; ny=-a4y; nz=-a4z; } s1x=d5x; s1y=d5y; s1z=d5z; n1x=-a5x; n1y=-a5y; n1z=-a5z; s2x=d6x; s2y=d6y; s2z=d6z; n2x=-a6x; n2y=-a6y; n2z=-a6z; } else if(minIndex==4){// b2.y * b1 swap=true; if(!right){ cx=p2x+d5x; cy=p2y+d5y; cz=p2z+d5z; nx=a5x; ny=a5y; nz=a5z; }else{ cx=p2x-d5x; cy=p2y-d5y; cz=p2z-d5z; nx=-a5x; ny=-a5y; nz=-a5z; } s1x=d4x; s1y=d4y; s1z=d4z; n1x=-a4x; n1y=-a4y; n1z=-a4z; s2x=d6x; s2y=d6y; s2z=d6z; n2x=-a6x; n2y=-a6y; n2z=-a6z; } else if(minIndex==5){// b2.z * b1 swap=true; if(!right){ cx=p2x+d6x; cy=p2y+d6y; cz=p2z+d6z; nx=a6x; ny=a6y; nz=a6z; }else{ cx=p2x-d6x; cy=p2y-d6y; cz=p2z-d6z; nx=-a6x; ny=-a6y; nz=-a6z; } s1x=d4x; s1y=d4y; s1z=d4z; n1x=-a4x; n1y=-a4y; n1z=-a4z; s2x=d5x; s2y=d5y; s2z=d5z; n2x=-a5x; n2y=-a5y; n2z=-a5z; } else if(minIndex==6){// b1.x * b2.x nx=a7x; ny=a7y; nz=a7z; n1x=a1x; n1y=a1y; n1z=a1z; n2x=a4x; n2y=a4y; n2z=a4z; } else if(minIndex==7){// b1.x * b2.y nx=a8x; ny=a8y; nz=a8z; n1x=a1x; n1y=a1y; n1z=a1z; n2x=a5x; n2y=a5y; n2z=a5z; } else if(minIndex==8){// b1.x * b2.z nx=a9x; ny=a9y; nz=a9z; n1x=a1x; n1y=a1y; n1z=a1z; n2x=a6x; n2y=a6y; n2z=a6z; } else if(minIndex==9){// b1.y * b2.x nx=aax; ny=aay; nz=aaz; n1x=a2x; n1y=a2y; n1z=a2z; n2x=a4x; n2y=a4y; n2z=a4z; } else if(minIndex==10){// b1.y * b2.y nx=abx; ny=aby; nz=abz; n1x=a2x; n1y=a2y; n1z=a2z; n2x=a5x; n2y=a5y; n2z=a5z; } else if(minIndex==11){// b1.y * b2.z nx=acx; ny=acy; nz=acz; n1x=a2x; n1y=a2y; n1z=a2z; n2x=a6x; n2y=a6y; n2z=a6z; } else if(minIndex==12){// b1.z * b2.x nx=adx; ny=ady; nz=adz; n1x=a3x; n1y=a3y; n1z=a3z; n2x=a4x; n2y=a4y; n2z=a4z; } else if(minIndex==13){// b1.z * b2.y nx=aex; ny=aey; nz=aez; n1x=a3x; n1y=a3y; n1z=a3z; n2x=a5x; n2y=a5y; n2z=a5z; } else if(minIndex==14){// b1.z * b2.z nx=afx; ny=afy; nz=afz; n1x=a3x; n1y=a3y; n1z=a3z; n2x=a6x; n2y=a6y; n2z=a6z; } //__________________________________________ //var v; if(minIndex>5){ if(!right){ nx=-nx; ny=-ny; nz=-nz; } var distance; var maxDistance; var vx; var vy; var vz; var v1x; var v1y; var v1z; var v2x; var v2y; var v2z; //vertex1; v1x=V1[0]; v1y=V1[1]; v1z=V1[2]; maxDistance=nx*v1x+ny*v1y+nz*v1z; //vertex2; vx=V1[3]; vy=V1[4]; vz=V1[5]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex3; vx=V1[6]; vy=V1[7]; vz=V1[8]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex4; vx=V1[9]; vy=V1[10]; vz=V1[11]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex5; vx=V1[12]; vy=V1[13]; vz=V1[14]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex6; vx=V1[15]; vy=V1[16]; vz=V1[17]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex7; vx=V1[18]; vy=V1[19]; vz=V1[20]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex8; vx=V1[21]; vy=V1[22]; vz=V1[23]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex1; v2x=V2[0]; v2y=V2[1]; v2z=V2[2]; maxDistance=nx*v2x+ny*v2y+nz*v2z; //vertex2; vx=V2[3]; vy=V2[4]; vz=V2[5]; distance=nx*vx+ny*vy+nz*vz; if(distance0){ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices2[index]=x2; this.clipVertices2[index+1]=y2; this.clipVertices2[index+2]=z2; }else{ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices2[index]=x1+(x2-x1)*t; this.clipVertices2[index+1]=y1+(y2-y1)*t; this.clipVertices2[index+2]=z1+(z2-z1)*t; } }else{ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices2[index]=x1+(x2-x1)*t; this.clipVertices2[index+1]=y1+(y2-y1)*t; this.clipVertices2[index+2]=z1+(z2-z1)*t; index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices2[index]=x2; this.clipVertices2[index+1]=y2; this.clipVertices2[index+2]=z2; } } x1=x2; y1=y2; z1=z2; dot1=dot2; } numClipVertices=numAddedClipVertices; if(numClipVertices==0)return; numAddedClipVertices=0; index=(numClipVertices-1)*3; x1=this.clipVertices2[index]; y1=this.clipVertices2[index+1]; z1=this.clipVertices2[index+2]; dot1=(x1-cx-s2x)*n2x+(y1-cy-s2y)*n2y+(z1-cz-s2z)*n2z; //i = numClipVertices; //while(i--){ for(i=0;i0){ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices1[index]=x2; this.clipVertices1[index+1]=y2; this.clipVertices1[index+2]=z2; }else{ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices1[index]=x1+(x2-x1)*t; this.clipVertices1[index+1]=y1+(y2-y1)*t; this.clipVertices1[index+2]=z1+(z2-z1)*t; } }else{ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices1[index]=x1+(x2-x1)*t; this.clipVertices1[index+1]=y1+(y2-y1)*t; this.clipVertices1[index+2]=z1+(z2-z1)*t; index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices1[index]=x2; this.clipVertices1[index+1]=y2; this.clipVertices1[index+2]=z2; } } x1=x2; y1=y2; z1=z2; dot1=dot2; } numClipVertices=numAddedClipVertices; if(numClipVertices==0)return; numAddedClipVertices=0; index=(numClipVertices-1)*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot1=(x1-cx+s1x)*-n1x+(y1-cy+s1y)*-n1y+(z1-cz+s1z)*-n1z; //i = numClipVertices; //while(i--){ for(i=0;i0){ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices2[index]=x2; this.clipVertices2[index+1]=y2; this.clipVertices2[index+2]=z2; }else{ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices2[index]=x1+(x2-x1)*t; this.clipVertices2[index+1]=y1+(y2-y1)*t; this.clipVertices2[index+2]=z1+(z2-z1)*t; } }else{ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices2[index]=x1+(x2-x1)*t; this.clipVertices2[index+1]=y1+(y2-y1)*t; this.clipVertices2[index+2]=z1+(z2-z1)*t; index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices2[index]=x2; this.clipVertices2[index+1]=y2; this.clipVertices2[index+2]=z2; } } x1=x2; y1=y2; z1=z2; dot1=dot2; } numClipVertices=numAddedClipVertices; if(numClipVertices==0)return; numAddedClipVertices=0; index=(numClipVertices-1)*3; x1=this.clipVertices2[index]; y1=this.clipVertices2[index+1]; z1=this.clipVertices2[index+2]; dot1=(x1-cx+s2x)*-n2x+(y1-cy+s2y)*-n2y+(z1-cz+s2z)*-n2z; //i = numClipVertices; //while(i--){ for(i=0;i0){ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices1[index]=x2; this.clipVertices1[index+1]=y2; this.clipVertices1[index+2]=z2; }else{ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices1[index]=x1+(x2-x1)*t; this.clipVertices1[index+1]=y1+(y2-y1)*t; this.clipVertices1[index+2]=z1+(z2-z1)*t; } }else{ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices1[index]=x1+(x2-x1)*t; this.clipVertices1[index+1]=y1+(y2-y1)*t; this.clipVertices1[index+2]=z1+(z2-z1)*t; index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices1[index]=x2; this.clipVertices1[index+1]=y2; this.clipVertices1[index+2]=z2; } } x1=x2; y1=y2; z1=z2; dot1=dot2; } numClipVertices=numAddedClipVertices; if(swap){ var tb=b1; b1=b2; b2=tb; } if(numClipVertices==0)return; var flipped=b1!=shape1; if(numClipVertices>4){ x1=(q1x+q2x+q3x+q4x)*0.25; y1=(q1y+q2y+q3y+q4y)*0.25; z1=(q1z+q2z+q3z+q4z)*0.25; n1x=q1x-x1; n1y=q1y-y1; n1z=q1z-z1; n2x=q2x-x1; n2y=q2y-y1; n2z=q2z-z1; var index1=0; var index2=0; var index3=0; var index4=0; var maxDot=-this.INF; minDot=this.INF; //i = numClipVertices; //while(i--){ for(i=0;imaxDot){ maxDot=dot; index3=i; } } this.used[index1]=true; this.used[index3]=true; maxDot=-this.INF; minDot=this.INF; //i = numClipVertices; //while(i--){ for(i=0;imaxDot){ maxDot=dot; index4=i; } } index=index1*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz; if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped); index=index2*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz; if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped); index=index3*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz; if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped); index=index4*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz; if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped); }else{ //i = numClipVertices; //while(i--){ for(i=0;i0){ t1x=v1x; t1y=v1y; t1z=v1z; v1x=v2x; v1y=v2y; v1z=v2z; v2x=t1x; v2y=t1y; v2z=t1z; t1x=v11x; t1y=v11y; t1z=v11z; v11x=v21x; v11y=v21y; v11z=v21z; v21x=t1x; v21y=t1y; v21z=t1z; t1x=v12x; t1y=v12y; t1z=v12z; v12x=v22x; v12y=v22y; v12z=v22z; v22x=t1x; v22y=t1y; v22z=t1z; nx=-nx; ny=-ny; nz=-nz; } var iterations=0; while(true){ if(++iterations>100){ return false; } this.supportPointB(c1,-nx,-ny,-nz,sup); var v31x=sup.x; var v31y=sup.y; var v31z=sup.z; this.supportPointC(c2,nx,ny,nz,sup); var v32x=sup.x; var v32y=sup.y; var v32z=sup.z; var v3x=v32x-v31x; var v3y=v32y-v31y; var v3z=v32z-v31z; if(v3x*nx+v3y*ny+v3z*nz<=0){ return false; } if((v1y*v3z-v1z*v3y)*v0x+(v1z*v3x-v1x*v3z)*v0y+(v1x*v3y-v1y*v3x)*v0z<0){ v2x=v3x; v2y=v3y; v2z=v3z; v21x=v31x; v21y=v31y; v21z=v31z; v22x=v32x; v22y=v32y; v22z=v32z; t1x=v1x-v0x; t1y=v1y-v0y; t1z=v1z-v0z; t2x=v3x-v0x; t2y=v3y-v0y; t2z=v3z-v0z; nx=t1y*t2z-t1z*t2y; ny=t1z*t2x-t1x*t2z; nz=t1x*t2y-t1y*t2x; continue; } if((v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z<0){ v1x=v3x; v1y=v3y; v1z=v3z; v11x=v31x; v11y=v31y; v11z=v31z; v12x=v32x; v12y=v32y; v12z=v32z; t1x=v3x-v0x; t1y=v3y-v0y; t1z=v3z-v0z; t2x=v2x-v0x; t2y=v2y-v0y; t2z=v2z-v0z; nx=t1y*t2z-t1z*t2y; ny=t1z*t2x-t1x*t2z; nz=t1x*t2y-t1y*t2x; continue; } var hit=false; while(true){ t1x=v2x-v1x; t1y=v2y-v1y; t1z=v2z-v1z; t2x=v3x-v1x; t2y=v3y-v1y; t2z=v3z-v1z; nx=t1y*t2z-t1z*t2y; ny=t1z*t2x-t1x*t2z; nz=t1x*t2y-t1y*t2x; len=1/_Math.sqrt(nx*nx+ny*ny+nz*nz); nx*=len; ny*=len; nz*=len; if(nx*v1x+ny*v1y+nz*v1z>=0&&!hit){ var b0=(v1y*v2z-v1z*v2y)*v3x+(v1z*v2x-v1x*v2z)*v3y+(v1x*v2y-v1y*v2x)*v3z; var b1=(v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z; var b2=(v0y*v1z-v0z*v1y)*v3x+(v0z*v1x-v0x*v1z)*v3y+(v0x*v1y-v0y*v1x)*v3z; var b3=(v2y*v1z-v2z*v1y)*v0x+(v2z*v1x-v2x*v1z)*v0y+(v2x*v1y-v2y*v1x)*v0z; var sum=b0+b1+b2+b3; if(sum<=0){ b0=0; b1=(v2y*v3z-v2z*v3y)*nx+(v2z*v3x-v2x*v3z)*ny+(v2x*v3y-v2y*v3x)*nz; b2=(v3y*v2z-v3z*v2y)*nx+(v3z*v2x-v3x*v2z)*ny+(v3x*v2y-v3y*v2x)*nz; b3=(v1y*v2z-v1z*v2y)*nx+(v1z*v2x-v1x*v2z)*ny+(v1x*v2y-v1y*v2x)*nz; sum=b1+b2+b3; } var inv=1/sum; p1x=(v01x*b0+v11x*b1+v21x*b2+v31x*b3)*inv; p1y=(v01y*b0+v11y*b1+v21y*b2+v31y*b3)*inv; p1z=(v01z*b0+v11z*b1+v21z*b2+v31z*b3)*inv; p2x=(v02x*b0+v12x*b1+v22x*b2+v32x*b3)*inv; p2y=(v02y*b0+v12y*b1+v22y*b2+v32y*b3)*inv; p2z=(v02z*b0+v12z*b1+v22z*b2+v32z*b3)*inv; hit=true; } this.supportPointB(c1,-nx,-ny,-nz,sup); var v41x=sup.x; var v41y=sup.y; var v41z=sup.z; this.supportPointC(c2,nx,ny,nz,sup); var v42x=sup.x; var v42y=sup.y; var v42z=sup.z; var v4x=v42x-v41x; var v4y=v42y-v41y; var v4z=v42z-v41z; var separation=-(v4x*nx+v4y*ny+v4z*nz); if((v4x-v3x)*nx+(v4y-v3y)*ny+(v4z-v3z)*nz<=0.01||separation>=0){ if(hit){ sep.set( -nx, -ny, -nz ); pos.set( (p1x+p2x)*0.5, (p1y+p2y)*0.5, (p1z+p2z)*0.5 ); dep.x=separation; return true; } return false; } if( (v4y*v1z-v4z*v1y)*v0x+ (v4z*v1x-v4x*v1z)*v0y+ (v4x*v1y-v4y*v1x)*v0z<0 ){ if( (v4y*v2z-v4z*v2y)*v0x+ (v4z*v2x-v4x*v2z)*v0y+ (v4x*v2y-v4y*v2x)*v0z<0 ){ v1x=v4x; v1y=v4y; v1z=v4z; v11x=v41x; v11y=v41y; v11z=v41z; v12x=v42x; v12y=v42y; v12z=v42z; }else{ v3x=v4x; v3y=v4y; v3z=v4z; v31x=v41x; v31y=v41y; v31z=v41z; v32x=v42x; v32y=v42y; v32z=v42z; } }else{ if( (v4y*v3z-v4z*v3y)*v0x+ (v4z*v3x-v4x*v3z)*v0y+ (v4x*v3y-v4y*v3x)*v0z<0 ){ v2x=v4x; v2y=v4y; v2z=v4z; v21x=v41x; v21y=v41y; v21z=v41z; v22x=v42x; v22y=v42y; v22z=v42z; }else{ v1x=v4x; v1y=v4y; v1z=v4z; v11x=v41x; v11y=v41y; v11z=v41z; v12x=v42x; v12y=v42y; v12z=v42z; } } } } //return false; }, supportPointB: function( c, dx, dy, dz, out ) { var rot=c.rotation.elements; var ldx=rot[0]*dx+rot[3]*dy+rot[6]*dz; var ldy=rot[1]*dx+rot[4]*dy+rot[7]*dz; var ldz=rot[2]*dx+rot[5]*dy+rot[8]*dz; var w=c.halfWidth; var h=c.halfHeight; var d=c.halfDepth; var ox; var oy; var oz; if(ldx<0)ox=-w; else ox=w; if(ldy<0)oy=-h; else oy=h; if(ldz<0)oz=-d; else oz=d; ldx=rot[0]*ox+rot[1]*oy+rot[2]*oz+c.position.x; ldy=rot[3]*ox+rot[4]*oy+rot[5]*oz+c.position.y; ldz=rot[6]*ox+rot[7]*oy+rot[8]*oz+c.position.z; out.set( ldx, ldy, ldz ); }, supportPointC: function ( c, dx, dy, dz, out ) { var rot=c.rotation.elements; var ldx=rot[0]*dx+rot[3]*dy+rot[6]*dz; var ldy=rot[1]*dx+rot[4]*dy+rot[7]*dz; var ldz=rot[2]*dx+rot[5]*dy+rot[8]*dz; var radx=ldx; var radz=ldz; var len=radx*radx+radz*radz; var rad=c.radius; var hh=c.halfHeight; var ox; var oy; var oz; if(len==0){ if(ldy<0){ ox=rad; oy=-hh; oz=0; }else{ ox=rad; oy=hh; oz=0; } }else{ len=c.radius/_Math.sqrt(len); if(ldy<0){ ox=radx*len; oy=-hh; oz=radz*len; }else{ ox=radx*len; oy=hh; oz=radz*len; } } ldx=rot[0]*ox+rot[1]*oy+rot[2]*oz+c.position.x; ldy=rot[3]*ox+rot[4]*oy+rot[5]*oz+c.position.y; ldz=rot[6]*ox+rot[7]*oy+rot[8]*oz+c.position.z; out.set( ldx, ldy, ldz ); }, detectCollision: function( shape1, shape2, manifold ) { var b; var c; if(this.flip){ b=shape2; c=shape1; }else{ b=shape1; c=shape2; } var sep=new Vec3(); var pos=new Vec3(); var dep=new Vec3(); if(!this.getSep(b,c,sep,pos,dep))return; var pbx=b.position.x; var pby=b.position.y; var pbz=b.position.z; var pcx=c.position.x; var pcy=c.position.y; var pcz=c.position.z; var bw=b.halfWidth; var bh=b.halfHeight; var bd=b.halfDepth; var ch=c.halfHeight; var r=c.radius; var D = b.dimentions; var nwx=D[0];//b.normalDirectionWidth.x; var nwy=D[1];//b.normalDirectionWidth.y; var nwz=D[2];//b.normalDirectionWidth.z; var nhx=D[3];//b.normalDirectionHeight.x; var nhy=D[4];//b.normalDirectionHeight.y; var nhz=D[5];//b.normalDirectionHeight.z; var ndx=D[6];//b.normalDirectionDepth.x; var ndy=D[7];//b.normalDirectionDepth.y; var ndz=D[8];//b.normalDirectionDepth.z; var dwx=D[9];//b.halfDirectionWidth.x; var dwy=D[10];//b.halfDirectionWidth.y; var dwz=D[11];//b.halfDirectionWidth.z; var dhx=D[12];//b.halfDirectionHeight.x; var dhy=D[13];//b.halfDirectionHeight.y; var dhz=D[14];//b.halfDirectionHeight.z; var ddx=D[15];//b.halfDirectionDepth.x; var ddy=D[16];//b.halfDirectionDepth.y; var ddz=D[17];//b.halfDirectionDepth.z; var ncx=c.normalDirection.x; var ncy=c.normalDirection.y; var ncz=c.normalDirection.z; var dcx=c.halfDirection.x; var dcy=c.halfDirection.y; var dcz=c.halfDirection.z; var nx=sep.x; var ny=sep.y; var nz=sep.z; var dotw=nx*nwx+ny*nwy+nz*nwz; var doth=nx*nhx+ny*nhy+nz*nhz; var dotd=nx*ndx+ny*ndy+nz*ndz; var dotc=nx*ncx+ny*ncy+nz*ncz; var right1=dotw>0; var right2=doth>0; var right3=dotd>0; var right4=dotc>0; if(!right1)dotw=-dotw; if(!right2)doth=-doth; if(!right3)dotd=-dotd; if(!right4)dotc=-dotc; var state=0; if(dotc>0.999){ if(dotw>0.999){ if(dotw>dotc)state=1; else state=4; }else if(doth>0.999){ if(doth>dotc)state=2; else state=4; }else if(dotd>0.999){ if(dotd>dotc)state=3; else state=4; }else state=4; }else{ if(dotw>0.999)state=1; else if(doth>0.999)state=2; else if(dotd>0.999)state=3; } var cbx; var cby; var cbz; var ccx; var ccy; var ccz; var r00; var r01; var r02; var r10; var r11; var r12; var r20; var r21; var r22; var px; var py; var pz; var pd; var dot; var len; var tx; var ty; var tz; var td; var dx; var dy; var dz; var d1x; var d1y; var d1z; var d2x; var d2y; var d2z; var sx; var sy; var sz; var sd; var ex; var ey; var ez; var ed; var dot1; var dot2; var t1; var dir1x; var dir1y; var dir1z; var dir2x; var dir2y; var dir2z; var dir1l; var dir2l; if(state==0){ //manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,dep.x,b,c,0,0,false); manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,dep.x,this.flip); }else if(state==4){ if(right4){ ccx=pcx-dcx; ccy=pcy-dcy; ccz=pcz-dcz; nx=-ncx; ny=-ncy; nz=-ncz; }else{ ccx=pcx+dcx; ccy=pcy+dcy; ccz=pcz+dcz; nx=ncx; ny=ncy; nz=ncz; } var v1x; var v1y; var v1z; var v2x; var v2y; var v2z; var v3x; var v3y; var v3z; var v4x; var v4y; var v4z; dot=1; state=0; dot1=nwx*nx+nwy*ny+nwz*nz; if(dot1=0.999999){ tx=-ny; ty=nz; tz=nx; }else{ tx=nx; ty=ny; tz=nz; } len=tx*ncx+ty*ncy+tz*ncz; dx=len*ncx-tx; dy=len*ncy-ty; dz=len*ncz-tz; len=_Math.sqrt(dx*dx+dy*dy+dz*dz); if(len==0)return; len=r/len; dx*=len; dy*=len; dz*=len; tx=ccx+dx; ty=ccy+dy; tz=ccz+dz; if(dot<-0.96||dot>0.96){ r00=ncx*ncx*1.5-0.5; r01=ncx*ncy*1.5-ncz*0.866025403; r02=ncx*ncz*1.5+ncy*0.866025403; r10=ncy*ncx*1.5+ncz*0.866025403; r11=ncy*ncy*1.5-0.5; r12=ncy*ncz*1.5-ncx*0.866025403; r20=ncz*ncx*1.5-ncy*0.866025403; r21=ncz*ncy*1.5+ncx*0.866025403; r22=ncz*ncz*1.5-0.5; px=tx; py=ty; pz=tz; pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz); tx=px-pd*nx-cbx; ty=py-pd*ny-cby; tz=pz-pd*nz-cbz; sd=dir1x*tx+dir1y*ty+dir1z*tz; ed=dir2x*tx+dir2y*ty+dir2z*tz; if(sd<-dir1l)sd=-dir1l; else if(sd>dir1l)sd=dir1l; if(ed<-dir2l)ed=-dir2l; else if(ed>dir2l)ed=dir2l; tx=sd*dir1x+ed*dir2x; ty=sd*dir1y+ed*dir2y; tz=sd*dir1z+ed*dir2z; px=cbx+tx; py=cby+ty; pz=cbz+tz; manifold.addPoint(px,py,pz,nx,ny,nz,pd,this.flip); px=dx*r00+dy*r01+dz*r02; py=dx*r10+dy*r11+dz*r12; pz=dx*r20+dy*r21+dz*r22; px=(dx=px)+ccx; py=(dy=py)+ccy; pz=(dz=pz)+ccz; pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz); if(pd<=0){ tx=px-pd*nx-cbx; ty=py-pd*ny-cby; tz=pz-pd*nz-cbz; sd=dir1x*tx+dir1y*ty+dir1z*tz; ed=dir2x*tx+dir2y*ty+dir2z*tz; if(sd<-dir1l)sd=-dir1l; else if(sd>dir1l)sd=dir1l; if(ed<-dir2l)ed=-dir2l; else if(ed>dir2l)ed=dir2l; tx=sd*dir1x+ed*dir2x; ty=sd*dir1y+ed*dir2y; tz=sd*dir1z+ed*dir2z; px=cbx+tx; py=cby+ty; pz=cbz+tz; //manifold.addPoint(px,py,pz,nx,ny,nz,pd,b,c,2,0,false); manifold.addPoint(px,py,pz,nx,ny,nz,pd,this.flip); } px=dx*r00+dy*r01+dz*r02; py=dx*r10+dy*r11+dz*r12; pz=dx*r20+dy*r21+dz*r22; px=(dx=px)+ccx; py=(dy=py)+ccy; pz=(dz=pz)+ccz; pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz); if(pd<=0){ tx=px-pd*nx-cbx; ty=py-pd*ny-cby; tz=pz-pd*nz-cbz; sd=dir1x*tx+dir1y*ty+dir1z*tz; ed=dir2x*tx+dir2y*ty+dir2z*tz; if(sd<-dir1l)sd=-dir1l; else if(sd>dir1l)sd=dir1l; if(ed<-dir2l)ed=-dir2l; else if(ed>dir2l)ed=dir2l; tx=sd*dir1x+ed*dir2x; ty=sd*dir1y+ed*dir2y; tz=sd*dir1z+ed*dir2z; px=cbx+tx; py=cby+ty; pz=cbz+tz; //manifold.addPoint(px,py,pz,nx,ny,nz,pd,b,c,3,0,false); manifold.addPoint(px,py,pz,nx,ny,nz,pd,this.flip); } }else{ sx=tx; sy=ty; sz=tz; sd=nx*(sx-cbx)+ny*(sy-cby)+nz*(sz-cbz); sx-=sd*nx; sy-=sd*ny; sz-=sd*nz; if(dot>0){ ex=tx+dcx*2; ey=ty+dcy*2; ez=tz+dcz*2; }else{ ex=tx-dcx*2; ey=ty-dcy*2; ez=tz-dcz*2; } ed=nx*(ex-cbx)+ny*(ey-cby)+nz*(ez-cbz); ex-=ed*nx; ey-=ed*ny; ez-=ed*nz; d1x=sx-cbx; d1y=sy-cby; d1z=sz-cbz; d2x=ex-cbx; d2y=ey-cby; d2z=ez-cbz; tx=ex-sx; ty=ey-sy; tz=ez-sz; td=ed-sd; dotw=d1x*dir1x+d1y*dir1y+d1z*dir1z; doth=d2x*dir1x+d2y*dir1y+d2z*dir1z; dot1=dotw-dir1l; dot2=doth-dir1l; if(dot1>0){ if(dot2>0)return; t1=dot1/(dot1-dot2); sx=sx+tx*t1; sy=sy+ty*t1; sz=sz+tz*t1; sd=sd+td*t1; d1x=sx-cbx; d1y=sy-cby; d1z=sz-cbz; dotw=d1x*dir1x+d1y*dir1y+d1z*dir1z; tx=ex-sx; ty=ey-sy; tz=ez-sz; td=ed-sd; }else if(dot2>0){ t1=dot1/(dot1-dot2); ex=sx+tx*t1; ey=sy+ty*t1; ez=sz+tz*t1; ed=sd+td*t1; d2x=ex-cbx; d2y=ey-cby; d2z=ez-cbz; doth=d2x*dir1x+d2y*dir1y+d2z*dir1z; tx=ex-sx; ty=ey-sy; tz=ez-sz; td=ed-sd; } dot1=dotw+dir1l; dot2=doth+dir1l; if(dot1<0){ if(dot2<0)return; t1=dot1/(dot1-dot2); sx=sx+tx*t1; sy=sy+ty*t1; sz=sz+tz*t1; sd=sd+td*t1; d1x=sx-cbx; d1y=sy-cby; d1z=sz-cbz; tx=ex-sx; ty=ey-sy; tz=ez-sz; td=ed-sd; }else if(dot2<0){ t1=dot1/(dot1-dot2); ex=sx+tx*t1; ey=sy+ty*t1; ez=sz+tz*t1; ed=sd+td*t1; d2x=ex-cbx; d2y=ey-cby; d2z=ez-cbz; tx=ex-sx; ty=ey-sy; tz=ez-sz; td=ed-sd; } dotw=d1x*dir2x+d1y*dir2y+d1z*dir2z; doth=d2x*dir2x+d2y*dir2y+d2z*dir2z; dot1=dotw-dir2l; dot2=doth-dir2l; if(dot1>0){ if(dot2>0)return; t1=dot1/(dot1-dot2); sx=sx+tx*t1; sy=sy+ty*t1; sz=sz+tz*t1; sd=sd+td*t1; d1x=sx-cbx; d1y=sy-cby; d1z=sz-cbz; dotw=d1x*dir2x+d1y*dir2y+d1z*dir2z; tx=ex-sx; ty=ey-sy; tz=ez-sz; td=ed-sd; }else if(dot2>0){ t1=dot1/(dot1-dot2); ex=sx+tx*t1; ey=sy+ty*t1; ez=sz+tz*t1; ed=sd+td*t1; d2x=ex-cbx; d2y=ey-cby; d2z=ez-cbz; doth=d2x*dir2x+d2y*dir2y+d2z*dir2z; tx=ex-sx; ty=ey-sy; tz=ez-sz; td=ed-sd; } dot1=dotw+dir2l; dot2=doth+dir2l; if(dot1<0){ if(dot2<0)return; t1=dot1/(dot1-dot2); sx=sx+tx*t1; sy=sy+ty*t1; sz=sz+tz*t1; sd=sd+td*t1; }else if(dot2<0){ t1=dot1/(dot1-dot2); ex=sx+tx*t1; ey=sy+ty*t1; ez=sz+tz*t1; ed=sd+td*t1; } if(sd<0){ //manifold.addPoint(sx,sy,sz,nx,ny,nz,sd,b,c,1,0,false); manifold.addPoint(sx,sy,sz,nx,ny,nz,sd,this.flip); } if(ed<0){ //manifold.addPoint(ex,ey,ez,nx,ny,nz,ed,b,c,4,0,false); manifold.addPoint(ex,ey,ez,nx,ny,nz,ed,this.flip); } } } } }); function CylinderCylinderCollisionDetector() { CollisionDetector.call( this ); } CylinderCylinderCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), { constructor: CylinderCylinderCollisionDetector, getSep: function ( c1, c2, sep, pos, dep ) { var t1x; var t1y; var t1z; var t2x; var t2y; var t2z; var sup=new Vec3(); var len; var p1x; var p1y; var p1z; var p2x; var p2y; var p2z; var v01x=c1.position.x; var v01y=c1.position.y; var v01z=c1.position.z; var v02x=c2.position.x; var v02y=c2.position.y; var v02z=c2.position.z; var v0x=v02x-v01x; var v0y=v02y-v01y; var v0z=v02z-v01z; if(v0x*v0x+v0y*v0y+v0z*v0z==0)v0y=0.001; var nx=-v0x; var ny=-v0y; var nz=-v0z; this.supportPoint(c1,-nx,-ny,-nz,sup); var v11x=sup.x; var v11y=sup.y; var v11z=sup.z; this.supportPoint(c2,nx,ny,nz,sup); var v12x=sup.x; var v12y=sup.y; var v12z=sup.z; var v1x=v12x-v11x; var v1y=v12y-v11y; var v1z=v12z-v11z; if(v1x*nx+v1y*ny+v1z*nz<=0){ return false; } nx=v1y*v0z-v1z*v0y; ny=v1z*v0x-v1x*v0z; nz=v1x*v0y-v1y*v0x; if(nx*nx+ny*ny+nz*nz==0){ sep.set( v1x-v0x, v1y-v0y, v1z-v0z ).normalize(); pos.set( (v11x+v12x)*0.5, (v11y+v12y)*0.5, (v11z+v12z)*0.5 ); return true; } this.supportPoint(c1,-nx,-ny,-nz,sup); var v21x=sup.x; var v21y=sup.y; var v21z=sup.z; this.supportPoint(c2,nx,ny,nz,sup); var v22x=sup.x; var v22y=sup.y; var v22z=sup.z; var v2x=v22x-v21x; var v2y=v22y-v21y; var v2z=v22z-v21z; if(v2x*nx+v2y*ny+v2z*nz<=0){ return false; } t1x=v1x-v0x; t1y=v1y-v0y; t1z=v1z-v0z; t2x=v2x-v0x; t2y=v2y-v0y; t2z=v2z-v0z; nx=t1y*t2z-t1z*t2y; ny=t1z*t2x-t1x*t2z; nz=t1x*t2y-t1y*t2x; if(nx*v0x+ny*v0y+nz*v0z>0){ t1x=v1x; t1y=v1y; t1z=v1z; v1x=v2x; v1y=v2y; v1z=v2z; v2x=t1x; v2y=t1y; v2z=t1z; t1x=v11x; t1y=v11y; t1z=v11z; v11x=v21x; v11y=v21y; v11z=v21z; v21x=t1x; v21y=t1y; v21z=t1z; t1x=v12x; t1y=v12y; t1z=v12z; v12x=v22x; v12y=v22y; v12z=v22z; v22x=t1x; v22y=t1y; v22z=t1z; nx=-nx; ny=-ny; nz=-nz; } var iterations=0; while(true){ if(++iterations>100){ return false; } this.supportPoint(c1,-nx,-ny,-nz,sup); var v31x=sup.x; var v31y=sup.y; var v31z=sup.z; this.supportPoint(c2,nx,ny,nz,sup); var v32x=sup.x; var v32y=sup.y; var v32z=sup.z; var v3x=v32x-v31x; var v3y=v32y-v31y; var v3z=v32z-v31z; if(v3x*nx+v3y*ny+v3z*nz<=0){ return false; } if((v1y*v3z-v1z*v3y)*v0x+(v1z*v3x-v1x*v3z)*v0y+(v1x*v3y-v1y*v3x)*v0z<0){ v2x=v3x; v2y=v3y; v2z=v3z; v21x=v31x; v21y=v31y; v21z=v31z; v22x=v32x; v22y=v32y; v22z=v32z; t1x=v1x-v0x; t1y=v1y-v0y; t1z=v1z-v0z; t2x=v3x-v0x; t2y=v3y-v0y; t2z=v3z-v0z; nx=t1y*t2z-t1z*t2y; ny=t1z*t2x-t1x*t2z; nz=t1x*t2y-t1y*t2x; continue; } if((v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z<0){ v1x=v3x; v1y=v3y; v1z=v3z; v11x=v31x; v11y=v31y; v11z=v31z; v12x=v32x; v12y=v32y; v12z=v32z; t1x=v3x-v0x; t1y=v3y-v0y; t1z=v3z-v0z; t2x=v2x-v0x; t2y=v2y-v0y; t2z=v2z-v0z; nx=t1y*t2z-t1z*t2y; ny=t1z*t2x-t1x*t2z; nz=t1x*t2y-t1y*t2x; continue; } var hit=false; while(true){ t1x=v2x-v1x; t1y=v2y-v1y; t1z=v2z-v1z; t2x=v3x-v1x; t2y=v3y-v1y; t2z=v3z-v1z; nx=t1y*t2z-t1z*t2y; ny=t1z*t2x-t1x*t2z; nz=t1x*t2y-t1y*t2x; len=1/_Math.sqrt(nx*nx+ny*ny+nz*nz); nx*=len; ny*=len; nz*=len; if(nx*v1x+ny*v1y+nz*v1z>=0&&!hit){ var b0=(v1y*v2z-v1z*v2y)*v3x+(v1z*v2x-v1x*v2z)*v3y+(v1x*v2y-v1y*v2x)*v3z; var b1=(v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z; var b2=(v0y*v1z-v0z*v1y)*v3x+(v0z*v1x-v0x*v1z)*v3y+(v0x*v1y-v0y*v1x)*v3z; var b3=(v2y*v1z-v2z*v1y)*v0x+(v2z*v1x-v2x*v1z)*v0y+(v2x*v1y-v2y*v1x)*v0z; var sum=b0+b1+b2+b3; if(sum<=0){ b0=0; b1=(v2y*v3z-v2z*v3y)*nx+(v2z*v3x-v2x*v3z)*ny+(v2x*v3y-v2y*v3x)*nz; b2=(v3y*v2z-v3z*v2y)*nx+(v3z*v2x-v3x*v2z)*ny+(v3x*v2y-v3y*v2x)*nz; b3=(v1y*v2z-v1z*v2y)*nx+(v1z*v2x-v1x*v2z)*ny+(v1x*v2y-v1y*v2x)*nz; sum=b1+b2+b3; } var inv=1/sum; p1x=(v01x*b0+v11x*b1+v21x*b2+v31x*b3)*inv; p1y=(v01y*b0+v11y*b1+v21y*b2+v31y*b3)*inv; p1z=(v01z*b0+v11z*b1+v21z*b2+v31z*b3)*inv; p2x=(v02x*b0+v12x*b1+v22x*b2+v32x*b3)*inv; p2y=(v02y*b0+v12y*b1+v22y*b2+v32y*b3)*inv; p2z=(v02z*b0+v12z*b1+v22z*b2+v32z*b3)*inv; hit=true; } this.supportPoint(c1,-nx,-ny,-nz,sup); var v41x=sup.x; var v41y=sup.y; var v41z=sup.z; this.supportPoint(c2,nx,ny,nz,sup); var v42x=sup.x; var v42y=sup.y; var v42z=sup.z; var v4x=v42x-v41x; var v4y=v42y-v41y; var v4z=v42z-v41z; var separation=-(v4x*nx+v4y*ny+v4z*nz); if((v4x-v3x)*nx+(v4y-v3y)*ny+(v4z-v3z)*nz<=0.01||separation>=0){ if(hit){ sep.set( -nx, -ny, -nz ); pos.set( (p1x+p2x)*0.5, (p1y+p2y)*0.5, (p1z+p2z)*0.5 ); dep.x=separation; return true; } return false; } if( (v4y*v1z-v4z*v1y)*v0x+ (v4z*v1x-v4x*v1z)*v0y+ (v4x*v1y-v4y*v1x)*v0z<0 ){ if( (v4y*v2z-v4z*v2y)*v0x+ (v4z*v2x-v4x*v2z)*v0y+ (v4x*v2y-v4y*v2x)*v0z<0 ){ v1x=v4x; v1y=v4y; v1z=v4z; v11x=v41x; v11y=v41y; v11z=v41z; v12x=v42x; v12y=v42y; v12z=v42z; }else{ v3x=v4x; v3y=v4y; v3z=v4z; v31x=v41x; v31y=v41y; v31z=v41z; v32x=v42x; v32y=v42y; v32z=v42z; } }else{ if( (v4y*v3z-v4z*v3y)*v0x+ (v4z*v3x-v4x*v3z)*v0y+ (v4x*v3y-v4y*v3x)*v0z<0 ){ v2x=v4x; v2y=v4y; v2z=v4z; v21x=v41x; v21y=v41y; v21z=v41z; v22x=v42x; v22y=v42y; v22z=v42z; }else{ v1x=v4x; v1y=v4y; v1z=v4z; v11x=v41x; v11y=v41y; v11z=v41z; v12x=v42x; v12y=v42y; v12z=v42z; } } } } //return false; }, supportPoint: function ( c, dx, dy, dz, out ) { var rot=c.rotation.elements; var ldx=rot[0]*dx+rot[3]*dy+rot[6]*dz; var ldy=rot[1]*dx+rot[4]*dy+rot[7]*dz; var ldz=rot[2]*dx+rot[5]*dy+rot[8]*dz; var radx=ldx; var radz=ldz; var len=radx*radx+radz*radz; var rad=c.radius; var hh=c.halfHeight; var ox; var oy; var oz; if(len==0){ if(ldy<0){ ox=rad; oy=-hh; oz=0; }else{ ox=rad; oy=hh; oz=0; } }else{ len=c.radius/_Math.sqrt(len); if(ldy<0){ ox=radx*len; oy=-hh; oz=radz*len; }else{ ox=radx*len; oy=hh; oz=radz*len; } } ldx=rot[0]*ox+rot[1]*oy+rot[2]*oz+c.position.x; ldy=rot[3]*ox+rot[4]*oy+rot[5]*oz+c.position.y; ldz=rot[6]*ox+rot[7]*oy+rot[8]*oz+c.position.z; out.set( ldx, ldy, ldz ); }, detectCollision: function ( shape1, shape2, manifold ) { var c1; var c2; if(shape1.id0; var right2=dot2>0; if(!right1)dot1=-dot1; if(!right2)dot2=-dot2; var state=0; if(dot1>0.999||dot2>0.999){ if(dot1>dot2)state=1; else state=2; } var nx; var ny; var nz; var depth=dep.x; var r00; var r01; var r02; var r10; var r11; var r12; var r20; var r21; var r22; var px; var py; var pz; var pd; var a; var b; var e; var f; nx=sep.x; ny=sep.y; nz=sep.z; switch(state){ case 0: manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,depth,false); break; case 1: if(right1){ c1x=p1x+d1x; c1y=p1y+d1y; c1z=p1z+d1z; nx=n1x; ny=n1y; nz=n1z; }else{ c1x=p1x-d1x; c1y=p1y-d1y; c1z=p1z-d1z; nx=-n1x; ny=-n1y; nz=-n1z; } dot=nx*n2x+ny*n2y+nz*n2z; if(dot<0)len=h2; else len=-h2; c2x=p2x+len*n2x; c2y=p2y+len*n2y; c2z=p2z+len*n2z; if(dot2>=0.999999){ tx=-ny; ty=nz; tz=nx; }else{ tx=nx; ty=ny; tz=nz; } len=tx*n2x+ty*n2y+tz*n2z; dx=len*n2x-tx; dy=len*n2y-ty; dz=len*n2z-tz; len=_Math.sqrt(dx*dx+dy*dy+dz*dz); if(len==0)break; len=r2/len; dx*=len; dy*=len; dz*=len; tx=c2x+dx; ty=c2y+dy; tz=c2z+dz; if(dot<-0.96||dot>0.96){ r00=n2x*n2x*1.5-0.5; r01=n2x*n2y*1.5-n2z*0.866025403; r02=n2x*n2z*1.5+n2y*0.866025403; r10=n2y*n2x*1.5+n2z*0.866025403; r11=n2y*n2y*1.5-0.5; r12=n2y*n2z*1.5-n2x*0.866025403; r20=n2z*n2x*1.5-n2y*0.866025403; r21=n2z*n2y*1.5+n2x*0.866025403; r22=n2z*n2z*1.5-0.5; px=tx; py=ty; pz=tz; pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z); tx=px-pd*nx-c1x; ty=py-pd*ny-c1y; tz=pz-pd*nz-c1z; len=tx*tx+ty*ty+tz*tz; if(len>r1*r1){ len=r1/_Math.sqrt(len); tx*=len; ty*=len; tz*=len; } px=c1x+tx; py=c1y+ty; pz=c1z+tz; manifold.addPoint(px,py,pz,nx,ny,nz,pd,false); px=dx*r00+dy*r01+dz*r02; py=dx*r10+dy*r11+dz*r12; pz=dx*r20+dy*r21+dz*r22; px=(dx=px)+c2x; py=(dy=py)+c2y; pz=(dz=pz)+c2z; pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z); if(pd<=0){ tx=px-pd*nx-c1x; ty=py-pd*ny-c1y; tz=pz-pd*nz-c1z; len=tx*tx+ty*ty+tz*tz; if(len>r1*r1){ len=r1/_Math.sqrt(len); tx*=len; ty*=len; tz*=len; } px=c1x+tx; py=c1y+ty; pz=c1z+tz; manifold.addPoint(px,py,pz,nx,ny,nz,pd,false); } px=dx*r00+dy*r01+dz*r02; py=dx*r10+dy*r11+dz*r12; pz=dx*r20+dy*r21+dz*r22; px=(dx=px)+c2x; py=(dy=py)+c2y; pz=(dz=pz)+c2z; pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z); if(pd<=0){ tx=px-pd*nx-c1x; ty=py-pd*ny-c1y; tz=pz-pd*nz-c1z; len=tx*tx+ty*ty+tz*tz; if(len>r1*r1){ len=r1/_Math.sqrt(len); tx*=len; ty*=len; tz*=len; } px=c1x+tx; py=c1y+ty; pz=c1z+tz; manifold.addPoint(px,py,pz,nx,ny,nz,pd,false); } }else{ sx=tx; sy=ty; sz=tz; depth1=nx*(sx-c1x)+ny*(sy-c1y)+nz*(sz-c1z); sx-=depth1*nx; sy-=depth1*ny; sz-=depth1*nz; if(dot>0){ ex=tx+n2x*h2*2; ey=ty+n2y*h2*2; ez=tz+n2z*h2*2; }else{ ex=tx-n2x*h2*2; ey=ty-n2y*h2*2; ez=tz-n2z*h2*2; } depth2=nx*(ex-c1x)+ny*(ey-c1y)+nz*(ez-c1z); ex-=depth2*nx; ey-=depth2*ny; ez-=depth2*nz; dx=c1x-sx; dy=c1y-sy; dz=c1z-sz; tx=ex-sx; ty=ey-sy; tz=ez-sz; a=dx*dx+dy*dy+dz*dz; b=dx*tx+dy*ty+dz*tz; e=tx*tx+ty*ty+tz*tz; f=b*b-e*(a-r1*r1); if(f<0)break; f=_Math.sqrt(f); t1=(b+f)/e; t2=(b-f)/e; if(t21)t2=1; if(t1<0)t1=0; tx=sx+(ex-sx)*t1; ty=sy+(ey-sy)*t1; tz=sz+(ez-sz)*t1; ex=sx+(ex-sx)*t2; ey=sy+(ey-sy)*t2; ez=sz+(ez-sz)*t2; sx=tx; sy=ty; sz=tz; len=depth1+(depth2-depth1)*t1; depth2=depth1+(depth2-depth1)*t2; depth1=len; if(depth1<0) manifold.addPoint(sx,sy,sz,nx,ny,nz,pd,false); if(depth2<0) manifold.addPoint(ex,ey,ez,nx,ny,nz,pd,false); } break; case 2: if(right2){ c2x=p2x-d2x; c2y=p2y-d2y; c2z=p2z-d2z; nx=-n2x; ny=-n2y; nz=-n2z; }else{ c2x=p2x+d2x; c2y=p2y+d2y; c2z=p2z+d2z; nx=n2x; ny=n2y; nz=n2z; } dot=nx*n1x+ny*n1y+nz*n1z; if(dot<0)len=h1; else len=-h1; c1x=p1x+len*n1x; c1y=p1y+len*n1y; c1z=p1z+len*n1z; if(dot1>=0.999999){ tx=-ny; ty=nz; tz=nx; }else{ tx=nx; ty=ny; tz=nz; } len=tx*n1x+ty*n1y+tz*n1z; dx=len*n1x-tx; dy=len*n1y-ty; dz=len*n1z-tz; len=_Math.sqrt(dx*dx+dy*dy+dz*dz); if(len==0)break; len=r1/len; dx*=len; dy*=len; dz*=len; tx=c1x+dx; ty=c1y+dy; tz=c1z+dz; if(dot<-0.96||dot>0.96){ r00=n1x*n1x*1.5-0.5; r01=n1x*n1y*1.5-n1z*0.866025403; r02=n1x*n1z*1.5+n1y*0.866025403; r10=n1y*n1x*1.5+n1z*0.866025403; r11=n1y*n1y*1.5-0.5; r12=n1y*n1z*1.5-n1x*0.866025403; r20=n1z*n1x*1.5-n1y*0.866025403; r21=n1z*n1y*1.5+n1x*0.866025403; r22=n1z*n1z*1.5-0.5; px=tx; py=ty; pz=tz; pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z); tx=px-pd*nx-c2x; ty=py-pd*ny-c2y; tz=pz-pd*nz-c2z; len=tx*tx+ty*ty+tz*tz; if(len>r2*r2){ len=r2/_Math.sqrt(len); tx*=len; ty*=len; tz*=len; } px=c2x+tx; py=c2y+ty; pz=c2z+tz; manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false); px=dx*r00+dy*r01+dz*r02; py=dx*r10+dy*r11+dz*r12; pz=dx*r20+dy*r21+dz*r22; px=(dx=px)+c1x; py=(dy=py)+c1y; pz=(dz=pz)+c1z; pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z); if(pd<=0){ tx=px-pd*nx-c2x; ty=py-pd*ny-c2y; tz=pz-pd*nz-c2z; len=tx*tx+ty*ty+tz*tz; if(len>r2*r2){ len=r2/_Math.sqrt(len); tx*=len; ty*=len; tz*=len; } px=c2x+tx; py=c2y+ty; pz=c2z+tz; manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false); } px=dx*r00+dy*r01+dz*r02; py=dx*r10+dy*r11+dz*r12; pz=dx*r20+dy*r21+dz*r22; px=(dx=px)+c1x; py=(dy=py)+c1y; pz=(dz=pz)+c1z; pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z); if(pd<=0){ tx=px-pd*nx-c2x; ty=py-pd*ny-c2y; tz=pz-pd*nz-c2z; len=tx*tx+ty*ty+tz*tz; if(len>r2*r2){ len=r2/_Math.sqrt(len); tx*=len; ty*=len; tz*=len; } px=c2x+tx; py=c2y+ty; pz=c2z+tz; manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false); } }else{ sx=tx; sy=ty; sz=tz; depth1=nx*(sx-c2x)+ny*(sy-c2y)+nz*(sz-c2z); sx-=depth1*nx; sy-=depth1*ny; sz-=depth1*nz; if(dot>0){ ex=tx+n1x*h1*2; ey=ty+n1y*h1*2; ez=tz+n1z*h1*2; }else{ ex=tx-n1x*h1*2; ey=ty-n1y*h1*2; ez=tz-n1z*h1*2; } depth2=nx*(ex-c2x)+ny*(ey-c2y)+nz*(ez-c2z); ex-=depth2*nx; ey-=depth2*ny; ez-=depth2*nz; dx=c2x-sx; dy=c2y-sy; dz=c2z-sz; tx=ex-sx; ty=ey-sy; tz=ez-sz; a=dx*dx+dy*dy+dz*dz; b=dx*tx+dy*ty+dz*tz; e=tx*tx+ty*ty+tz*tz; f=b*b-e*(a-r2*r2); if(f<0)break; f=_Math.sqrt(f); t1=(b+f)/e; t2=(b-f)/e; if(t21)t2=1; if(t1<0)t1=0; tx=sx+(ex-sx)*t1; ty=sy+(ey-sy)*t1; tz=sz+(ez-sz)*t1; ex=sx+(ex-sx)*t2; ey=sy+(ey-sy)*t2; ez=sz+(ez-sz)*t2; sx=tx; sy=ty; sz=tz; len=depth1+(depth2-depth1)*t1; depth2=depth1+(depth2-depth1)*t2; depth1=len; if(depth1<0){ manifold.addPoint(sx,sy,sz,-nx,-ny,-nz,depth1,false); } if(depth2<0){ manifold.addPoint(ex,ey,ez,-nx,-ny,-nz,depth2,false); } } break; } } }); /** * A collision detector which detects collisions between sphere and box. * @author saharan */ function SphereBoxCollisionDetector ( flip ) { CollisionDetector.call( this ); this.flip = flip; } SphereBoxCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), { constructor: SphereBoxCollisionDetector, detectCollision: function ( shape1, shape2, manifold ) { var s; var b; if(this.flip){ s=(shape2); b=(shape1); }else{ s=(shape1); b=(shape2); } var D = b.dimentions; var ps=s.position; var psx=ps.x; var psy=ps.y; var psz=ps.z; var pb=b.position; var pbx=pb.x; var pby=pb.y; var pbz=pb.z; var rad=s.radius; var hw=b.halfWidth; var hh=b.halfHeight; var hd=b.halfDepth; var dx=psx-pbx; var dy=psy-pby; var dz=psz-pbz; var sx=D[0]*dx+D[1]*dy+D[2]*dz; var sy=D[3]*dx+D[4]*dy+D[5]*dz; var sz=D[6]*dx+D[7]*dy+D[8]*dz; var cx; var cy; var cz; var len; var invLen; var overlap=0; if(sx>hw){ sx=hw; }else if(sx<-hw){ sx=-hw; }else{ overlap=1; } if(sy>hh){ sy=hh; }else if(sy<-hh){ sy=-hh; }else{ overlap|=2; } if(sz>hd){ sz=hd; }else if(sz<-hd){ sz=-hd; }else{ overlap|=4; } if(overlap==7){ // center of sphere is in the box if(sx<0){ dx=hw+sx; }else{ dx=hw-sx; } if(sy<0){ dy=hh+sy; }else{ dy=hh-sy; } if(sz<0){ dz=hd+sz; }else{ dz=hd-sz; } if(dx0&&len halfh + rads ) return; var cx = pcx + dot * dirx; var cy = pcy + dot * diry; var cz = pcz + dot * dirz; var d2x = psx - cx; var d2y = psy - cy; var d2z = psz - cz; var len = d2x * d2x + d2y * d2y + d2z * d2z; if ( len > rad2 * rad2 ) return; if ( len > radc * radc ) { len = radc / _Math.sqrt( len ); d2x *= len; d2y *= len; d2z *= len; } if( dot < -halfh ) dot = -halfh; else if( dot > halfh ) dot = halfh; cx = pcx + dot * dirx + d2x; cy = pcy + dot * diry + d2y; cz = pcz + dot * dirz + d2z; dx = cx - psx; dy = cy - psy; dz = cz - psz; len = dx * dx + dy * dy + dz * dz; var invLen; if ( len > 0 && len < rads * rads ) { len = _Math.sqrt(len); invLen = 1 / len; dx *= invLen; dy *= invLen; dz *= invLen; ///result.addContactInfo(psx+dx*rads,psy+dy*rads,psz+dz*rads,dx,dy,dz,len-rads,s,c,0,0,false); manifold.addPoint( psx + dx * rads, psy + dy * rads, psz + dz * rads, dx, dy, dz, len - rads, this.flip ); } } }); /** * A collision detector which detects collisions between two spheres. * @author saharan */ function SphereSphereCollisionDetector (){ CollisionDetector.call( this ); } SphereSphereCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), { constructor: SphereSphereCollisionDetector, detectCollision: function ( shape1, shape2, manifold ) { var s1 = shape1; var s2 = shape2; var p1 = s1.position; var p2 = s2.position; var dx = p2.x - p1.x; var dy = p2.y - p1.y; var dz = p2.z - p1.z; var len = dx * dx + dy * dy + dz * dz; var r1 = s1.radius; var r2 = s2.radius; var rad = r1 + r2; if ( len > 0 && len < rad * rad ){ len = _Math.sqrt( len ); var invLen = 1 / len; dx *= invLen; dy *= invLen; dz *= invLen; manifold.addPoint( p1.x + dx * r1, p1.y + dy * r1, p1.z + dz * r1, dx, dy, dz, len - rad, false ); } } }); /** * A collision detector which detects collisions between two spheres. * @author saharan * @author lo-th */ function SpherePlaneCollisionDetector ( flip ){ CollisionDetector.call( this ); this.flip = flip; this.n = new Vec3(); this.p = new Vec3(); } SpherePlaneCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), { constructor: SpherePlaneCollisionDetector, detectCollision: function ( shape1, shape2, manifold ) { var n = this.n; var p = this.p; var s = this.flip ? shape2 : shape1; var pn = this.flip ? shape1 : shape2; var rad = s.radius; var len; n.sub( s.position, pn.position ); //var h = _Math.dotVectors( pn.normal, n ); n.x *= pn.normal.x;//+ rad; n.y *= pn.normal.y; n.z *= pn.normal.z;//+ rad; var len = n.lengthSq(); if( len > 0 && len < rad * rad){//&& h > rad*rad ){ len = _Math.sqrt( len ); //len = _Math.sqrt( h ); n.copy(pn.normal).negate(); //n.scaleEqual( 1/len ); //(0, -1, 0) //n.normalize(); p.copy( s.position ).addScaledVector( n, rad ); manifold.addPointVec( p, n, len - rad, this.flip ); } } }); /** * A collision detector which detects collisions between two spheres. * @author saharan * @author lo-th */ function BoxPlaneCollisionDetector ( flip ){ CollisionDetector.call( this ); this.flip = flip; this.n = new Vec3(); this.p = new Vec3(); this.dix = new Vec3(); this.diy = new Vec3(); this.diz = new Vec3(); this.cc = new Vec3(); this.cc2 = new Vec3(); } BoxPlaneCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), { constructor: BoxPlaneCollisionDetector, detectCollision: function ( shape1, shape2, manifold ) { var n = this.n; var p = this.p; var cc = this.cc; var b = this.flip ? shape2 : shape1; var pn = this.flip ? shape1 : shape2; var D = b.dimentions; var hw = b.halfWidth; var hh = b.halfHeight; var hd = b.halfDepth; var len; var overlap = 0; this.dix.set( D[0], D[1], D[2] ); this.diy.set( D[3], D[4], D[5] ); this.diz.set( D[6], D[7], D[8] ); n.sub( b.position, pn.position ); n.x *= pn.normal.x;//+ rad; n.y *= pn.normal.y; n.z *= pn.normal.z;//+ rad; cc.set( _Math.dotVectors( this.dix, n ), _Math.dotVectors( this.diy, n ), _Math.dotVectors( this.diz, n ) ); if( cc.x > hw ) cc.x = hw; else if( cc.x < -hw ) cc.x = -hw; else overlap = 1; if( cc.y > hh ) cc.y = hh; else if( cc.y < -hh ) cc.y = -hh; else overlap |= 2; if( cc.z > hd ) cc.z = hd; else if( cc.z < -hd ) cc.z = -hd; else overlap |= 4; if( overlap === 7 ){ // center of sphere is in the box n.set( cc.x < 0 ? hw + cc.x : hw - cc.x, cc.y < 0 ? hh + cc.y : hh - cc.y, cc.z < 0 ? hd + cc.z : hd - cc.z ); if( n.x < n.y ){ if( n.x < n.z ){ len = n.x - hw; if( cc.x < 0 ){ cc.x = -hw; n.copy( this.dix ); }else{ cc.x = hw; n.subEqual( this.dix ); } }else{ len = n.z - hd; if( cc.z < 0 ){ cc.z = -hd; n.copy( this.diz ); }else{ cc.z = hd; n.subEqual( this.diz ); } } }else{ if( n.y < n.z ){ len = n.y - hh; if( cc.y < 0 ){ cc.y = -hh; n.copy( this.diy ); }else{ cc.y = hh; n.subEqual( this.diy ); } }else{ len = n.z - hd; if( cc.z < 0 ){ cc.z = -hd; n.copy( this.diz ); }else{ cc.z = hd; n.subEqual( this.diz ); } } } p.copy( pn.position ).addScaledVector( n, 1 ); manifold.addPointVec( p, n, len, this.flip ); } } }); //import { TetraShape } from '../collision/shape/TetraShape'; /** * The class of physical computing world. * You must be added to the world physical all computing objects * * @author saharan * @author lo-th */ // timestep, broadphase, iterations, worldscale, random, stat function World ( o ) { if( !(o instanceof Object) ) o = {}; // this world scale defaut is 0.1 to 10 meters max for dynamique body this.scale = o.worldscale || 1; this.invScale = 1/this.scale; // The time between each step this.timeStep = o.timestep || 0.01666; // 1/60; this.timerate = this.timeStep * 1000; this.timer = null; this.preLoop = null;//function(){}; this.postLoop = null;//function(){}; // The number of iterations for constraint solvers. this.numIterations = o.iterations || 8; // It is a wide-area collision judgment that is used in order to reduce as much as possible a detailed collision judgment. switch( o.broadphase || 2 ){ case 1: this.broadPhase = new BruteForceBroadPhase(); break; case 2: default: this.broadPhase = new SAPBroadPhase(); break; case 3: this.broadPhase = new DBVTBroadPhase(); break; } this.Btypes = ['None','BruteForce','Sweep & Prune', 'Bounding Volume Tree' ]; this.broadPhaseType = this.Btypes[ o.broadphase || 2 ]; // This is the detailed information of the performance. this.performance = null; this.isStat = o.info === undefined ? false : o.info; if( this.isStat ) this.performance = new InfoDisplay( this ); /** * Whether the constraints randomizer is enabled or not. * * @property enableRandomizer * @type {Boolean} */ this.enableRandomizer = o.random !== undefined ? o.random : true; // The rigid body list this.rigidBodies=null; // number of rigid body this.numRigidBodies=0; // The contact list this.contacts=null; this.unusedContacts=null; // The number of contact this.numContacts=0; // The number of contact points this.numContactPoints=0; // The joint list this.joints=null; // The number of joints. this.numJoints=0; // The number of simulation islands. this.numIslands=0; // The gravity in the world. this.gravity = new Vec3(0,-9.8,0); if( o.gravity !== undefined ) this.gravity.fromArray( o.gravity ); var numShapeTypes = 5;//4;//3; this.detectors=[]; this.detectors.length = numShapeTypes; var i = numShapeTypes; while(i--){ this.detectors[i]=[]; this.detectors[i].length = numShapeTypes; } this.detectors[SHAPE_SPHERE][SHAPE_SPHERE] = new SphereSphereCollisionDetector(); this.detectors[SHAPE_SPHERE][SHAPE_BOX] = new SphereBoxCollisionDetector(false); this.detectors[SHAPE_BOX][SHAPE_SPHERE] = new SphereBoxCollisionDetector(true); this.detectors[SHAPE_BOX][SHAPE_BOX] = new BoxBoxCollisionDetector(); // CYLINDER add this.detectors[SHAPE_CYLINDER][SHAPE_CYLINDER] = new CylinderCylinderCollisionDetector(); this.detectors[SHAPE_CYLINDER][SHAPE_BOX] = new BoxCylinderCollisionDetector(true); this.detectors[SHAPE_BOX][SHAPE_CYLINDER] = new BoxCylinderCollisionDetector(false); this.detectors[SHAPE_CYLINDER][SHAPE_SPHERE] = new SphereCylinderCollisionDetector(true); this.detectors[SHAPE_SPHERE][SHAPE_CYLINDER] = new SphereCylinderCollisionDetector(false); // PLANE add this.detectors[SHAPE_PLANE][SHAPE_SPHERE] = new SpherePlaneCollisionDetector(true); this.detectors[SHAPE_SPHERE][SHAPE_PLANE] = new SpherePlaneCollisionDetector(false); this.detectors[SHAPE_PLANE][SHAPE_BOX] = new BoxPlaneCollisionDetector(true); this.detectors[SHAPE_BOX][SHAPE_PLANE] = new BoxPlaneCollisionDetector(false); // TETRA add //this.detectors[SHAPE_TETRA][SHAPE_TETRA] = new TetraTetraCollisionDetector(); this.randX = 65535; this.randA = 98765; this.randB = 123456789; this.islandRigidBodies = []; this.islandStack = []; this.islandConstraints = []; } Object.assign( World.prototype, { World: true, play: function(){ if( this.timer !== null ) return; var _this = this; this.timer = setInterval( function(){ _this.step(); } , this.timerate ); //this.timer = setInterval( this.loop.bind(this) , this.timerate ); }, stop: function(){ if( this.timer === null ) return; clearInterval( this.timer ); this.timer = null; }, setGravity: function ( ar ) { this.gravity.fromArray( ar ); }, getInfo: function () { return this.isStat ? this.performance.show() : ''; }, // Reset the world and remove all rigid bodies, shapes, joints and any object from the world. clear:function(){ this.stop(); this.preLoop = null; this.postLoop = null; this.randX = 65535; while(this.joints!==null){ this.removeJoint( this.joints ); } while(this.contacts!==null){ this.removeContact( this.contacts ); } while(this.rigidBodies!==null){ this.removeRigidBody( this.rigidBodies ); } }, /** * I'll add a rigid body to the world. * Rigid body that has been added will be the operands of each step. * @param rigidBody Rigid body that you want to add */ addRigidBody:function( rigidBody ){ if(rigidBody.parent){ printError("World", "It is not possible to be added to more than one world one of the rigid body"); } rigidBody.setParent( this ); //rigidBody.awake(); for(var shape = rigidBody.shapes; shape !== null; shape = shape.next){ this.addShape( shape ); } if(this.rigidBodies!==null)(this.rigidBodies.prev=rigidBody).next=this.rigidBodies; this.rigidBodies = rigidBody; this.numRigidBodies++; }, /** * I will remove the rigid body from the world. * Rigid body that has been deleted is excluded from the calculation on a step-by-step basis. * @param rigidBody Rigid body to be removed */ removeRigidBody:function( rigidBody ){ var remove=rigidBody; if(remove.parent!==this)return; remove.awake(); var js=remove.jointLink; while(js!=null){ var joint=js.joint; js=js.next; this.removeJoint(joint); } for(var shape=rigidBody.shapes; shape!==null; shape=shape.next){ this.removeShape(shape); } var prev=remove.prev; var next=remove.next; if(prev!==null) prev.next=next; if(next!==null) next.prev=prev; if(this.rigidBodies==remove) this.rigidBodies=next; remove.prev=null; remove.next=null; remove.parent=null; this.numRigidBodies--; }, getByName: function( name ){ var body = this.rigidBodies; while( body !== null ){ if( body.name === name ) return body; body=body.next; } var joint = this.joints; while( joint !== null ){ if( joint.name === name ) return joint; joint = joint.next; } return null; }, /** * I'll add a shape to the world.. * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world, * Shape will be added to the world automatically, please do not call from outside this method. * @param shape Shape you want to add */ addShape:function ( shape ){ if(!shape.parent || !shape.parent.parent){ printError("World", "It is not possible to be added alone to shape world"); } shape.proxy = this.broadPhase.createProxy(shape); shape.updateProxy(); this.broadPhase.addProxy(shape.proxy); }, /** * I will remove the shape from the world. * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world, * Shape will be added to the world automatically, please do not call from outside this method. * @param shape Shape you want to delete */ removeShape: function ( shape ){ this.broadPhase.removeProxy(shape.proxy); shape.proxy = null; }, /** * I'll add a joint to the world. * Joint that has been added will be the operands of each step. * @param shape Joint to be added */ addJoint: function ( joint ) { if(joint.parent){ printError("World", "It is not possible to be added to more than one world one of the joint"); } if(this.joints!=null)(this.joints.prev=joint).next=this.joints; this.joints=joint; joint.setParent( this ); this.numJoints++; joint.awake(); joint.attach(); }, /** * I will remove the joint from the world. * Joint that has been added will be the operands of each step. * @param shape Joint to be deleted */ removeJoint: function ( joint ) { var remove=joint; var prev=remove.prev; var next=remove.next; if(prev!==null)prev.next=next; if(next!==null)next.prev=prev; if(this.joints==remove)this.joints=next; remove.prev=null; remove.next=null; this.numJoints--; remove.awake(); remove.detach(); remove.parent=null; }, addContact: function ( s1, s2 ) { var newContact; if(this.unusedContacts!==null){ newContact=this.unusedContacts; this.unusedContacts=this.unusedContacts.next; }else{ newContact = new Contact(); } newContact.attach(s1,s2); newContact.detector = this.detectors[s1.type][s2.type]; if(this.contacts)(this.contacts.prev = newContact).next = this.contacts; this.contacts = newContact; this.numContacts++; }, removeContact: function ( contact ) { var prev = contact.prev; var next = contact.next; if(next) next.prev = prev; if(prev) prev.next = next; if(this.contacts == contact) this.contacts = next; contact.prev = null; contact.next = null; contact.detach(); contact.next = this.unusedContacts; this.unusedContacts = contact; this.numContacts--; }, getContact: function ( b1, b2 ) { b1 = b1.constructor === RigidBody ? b1.name : b1; b2 = b2.constructor === RigidBody ? b2.name : b2; var n1, n2; var contact = this.contacts; while(contact!==null){ n1 = contact.body1.name; n2 = contact.body2.name; if((n1===b1 && n2===b2) || (n2===b1 && n1===b2)){ if(contact.touching) return contact; else return null;} else contact = contact.next; } return null; }, checkContact: function ( name1, name2 ) { var n1, n2; var contact = this.contacts; while(contact!==null){ n1 = contact.body1.name || ' '; n2 = contact.body2.name || ' '; if((n1==name1 && n2==name2) || (n2==name1 && n1==name2)){ if(contact.touching) return true; else return false;} else contact = contact.next; } //return false; }, callSleep: function( body ) { if( !body.allowSleep ) return false; if( body.linearVelocity.lengthSq() > 0.04 ) return false; if( body.angularVelocity.lengthSq() > 0.25 ) return false; return true; }, /** * I will proceed only time step seconds time of World. */ step: function () { var stat = this.isStat; if( stat ) this.performance.setTime( 0 ); var body = this.rigidBodies; while( body !== null ){ body.addedToIsland = false; if( body.sleeping ) body.testWakeUp(); body = body.next; } //------------------------------------------------------ // UPDATE BROADPHASE CONTACT //------------------------------------------------------ if( stat ) this.performance.setTime( 1 ); this.broadPhase.detectPairs(); var pairs = this.broadPhase.pairs; var i = this.broadPhase.numPairs; //do{ while(i--){ //for(var i=0, l=numPairs; i0); if( stat ) this.performance.calcBroadPhase(); //------------------------------------------------------ // UPDATE NARROWPHASE CONTACT //------------------------------------------------------ // update & narrow phase this.numContactPoints = 0; contact = this.contacts; while( contact!==null ){ if(!contact.persisting){ if ( contact.shape1.aabb.intersectTest( contact.shape2.aabb ) ) { /*var aabb1=contact.shape1.aabb; var aabb2=contact.shape2.aabb; if( aabb1.minX>aabb2.maxX || aabb1.maxXaabb2.maxY || aabb1.maxYaabb2.maxZ || aabb1.maxZ 0.5 ) base.sleep(); else base.updatePosition( this.timeStep ); }else{ base.sleepTime = 0; base.updatePosition( this.timeStep ); } this.numIslands++; continue; } var islandNumRigidBodies = 0; var islandNumConstraints = 0; var stackCount = 1; // add rigid body to stack this.islandStack[0] = base; base.addedToIsland = true; // build an island do{ // get rigid body from stack body = this.islandStack[--stackCount]; this.islandStack[stackCount] = null; body.sleeping = false; // add rigid body to the island this.islandRigidBodies[islandNumRigidBodies++] = body; if(body.isStatic) continue; // search connections for( var cs = body.contactLink; cs !== null; cs = cs.next ) { var contact = cs.contact; constraint = contact.constraint; if( constraint.addedToIsland || !contact.touching ) continue;// ignore // add constraint to the island this.islandConstraints[islandNumConstraints++] = constraint; constraint.addedToIsland = true; var next = cs.body; if(next.addedToIsland) continue; // add rigid body to stack this.islandStack[stackCount++] = next; next.addedToIsland = true; } for( var js = body.jointLink; js !== null; js = js.next ) { constraint = js.joint; if(constraint.addedToIsland) continue;// ignore // add constraint to the island this.islandConstraints[islandNumConstraints++] = constraint; constraint.addedToIsland = true; next = js.body; if( next.addedToIsland || !next.isDynamic ) continue; // add rigid body to stack this.islandStack[stackCount++] = next; next.addedToIsland = true; } } while( stackCount != 0 ); // update velocities var gVel = new Vec3().addScaledVector( this.gravity, this.timeStep ); /*var gx=this.gravity.x*this.timeStep; var gy=this.gravity.y*this.timeStep; var gz=this.gravity.z*this.timeStep;*/ var j = islandNumRigidBodies; while (j--){ //or(var j=0, l=islandNumRigidBodies; j 0.5){ // sleep the island j = islandNumRigidBodies; while(j--){ //for(j=0, l=islandNumRigidBodies;j