{"id":"node_modules\\oimo\\build\\oimo.module.js","dependencies":[{"name":"C:\\Users\\florianmaxim\\work\\meta\\examples\\1\\node_modules\\oimo\\package.json","includedInParent":true,"mtime":1531760494354},{"name":"C:\\Users\\florianmaxim\\work\\meta\\examples\\1\\package.json","includedInParent":true,"mtime":1531761174062},{"name":"C:\\Users\\florianmaxim\\work\\meta\\.babelrc","includedInParent":true,"mtime":1531746256699}],"generated":{"js":"'use strict';\n\nObject.defineProperty(exports, \"__esModule\", {\n value: true\n});\n// Polyfills\n\nif (Number.EPSILON === undefined) {\n\n Number.EPSILON = Math.pow(2, -52);\n}\n\n//\n\nif (Math.sign === undefined) {\n\n // https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Math/sign\n\n Math.sign = function (x) {\n\n return x < 0 ? -1 : x > 0 ? 1 : +x;\n };\n}\n\nif (Function.prototype.name === undefined) {\n\n // Missing in IE9-11.\n // https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Function/name\n\n Object.defineProperty(Function.prototype, 'name', {\n\n get: function () {\n\n return this.toString().match(/^\\s*function\\s*([^\\(\\s]*)/)[1];\n }\n\n });\n}\n\nif (Object.assign === undefined) {\n\n // Missing in IE.\n // https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Object/assign\n\n (function () {\n\n Object.assign = function (target) {\n\n 'use strict';\n\n if (target === undefined || target === null) {\n\n throw new TypeError('Cannot convert undefined or null to object');\n }\n\n var output = Object(target);\n\n for (var index = 1; index < arguments.length; index++) {\n\n var source = arguments[index];\n\n if (source !== undefined && source !== null) {\n\n for (var nextKey in source) {\n\n if (Object.prototype.hasOwnProperty.call(source, nextKey)) {\n\n output[nextKey] = source[nextKey];\n }\n }\n }\n }\n\n return output;\n };\n })();\n}\n\n/*\r\n * A list of constants built-in for\r\n * the physics engine.\r\n */\n\nvar REVISION = '1.0.9';\n\n// BroadPhase\nvar BR_NULL = 0;\nvar BR_BRUTE_FORCE = 1;\nvar BR_SWEEP_AND_PRUNE = 2;\nvar BR_BOUNDING_VOLUME_TREE = 3;\n\n// Body type\nvar BODY_NULL = 0;\nvar BODY_DYNAMIC = 1;\nvar BODY_STATIC = 2;\nvar BODY_KINEMATIC = 3;\nvar BODY_GHOST = 4;\n\n// Shape type\nvar SHAPE_NULL = 0;\nvar SHAPE_SPHERE = 1;\nvar SHAPE_BOX = 2;\nvar SHAPE_CYLINDER = 3;\nvar SHAPE_PLANE = 4;\nvar SHAPE_PARTICLE = 5;\nvar SHAPE_TETRA = 6;\n\n// Joint type\nvar JOINT_NULL = 0;\nvar JOINT_DISTANCE = 1;\nvar JOINT_BALL_AND_SOCKET = 2;\nvar JOINT_HINGE = 3;\nvar JOINT_WHEEL = 4;\nvar JOINT_SLIDER = 5;\nvar JOINT_PRISMATIC = 6;\n\n// AABB aproximation\nvar AABB_PROX = 0.005;\n\nvar _Math = {\n\n sqrt: Math.sqrt,\n abs: Math.abs,\n floor: Math.floor,\n cos: Math.cos,\n sin: Math.sin,\n acos: Math.acos,\n asin: Math.asin,\n atan2: Math.atan2,\n round: Math.round,\n pow: Math.pow,\n max: Math.max,\n min: Math.min,\n random: Math.random,\n\n degtorad: 0.0174532925199432957,\n radtodeg: 57.295779513082320876,\n PI: 3.141592653589793,\n TwoPI: 6.283185307179586,\n PI90: 1.570796326794896,\n PI270: 4.712388980384689,\n\n INF: Infinity,\n EPZ: 0.00001,\n EPZ2: 0.000001,\n\n lerp: function (x, y, t) {\n\n return (1 - t) * x + t * y;\n },\n\n randInt: function (low, high) {\n\n return low + _Math.floor(_Math.random() * (high - low + 1));\n },\n\n rand: function (low, high) {\n\n return low + _Math.random() * (high - low);\n },\n\n generateUUID: function () {\n\n // http://www.broofa.com/Tools/Math.uuid.htm\n\n var chars = '0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz'.split('');\n var uuid = new Array(36);\n var rnd = 0,\n r;\n\n return function generateUUID() {\n\n for (var i = 0; i < 36; i++) {\n\n if (i === 8 || i === 13 || i === 18 || i === 23) {\n\n uuid[i] = '-';\n } else if (i === 14) {\n\n uuid[i] = '4';\n } else {\n\n if (rnd <= 0x02) rnd = 0x2000000 + Math.random() * 0x1000000 | 0;\n r = rnd & 0xf;\n rnd = rnd >> 4;\n uuid[i] = chars[i === 19 ? r & 0x3 | 0x8 : r];\n }\n }\n\n return uuid.join('');\n };\n }(),\n\n int: function (x) {\n\n return _Math.floor(x);\n },\n\n fix: function (x, n) {\n\n return x.toFixed(n || 3, 10);\n },\n\n clamp: function (value, min, max) {\n\n return _Math.max(min, _Math.min(max, value));\n },\n\n //clamp: function ( x, a, b ) { return ( x < a ) ? a : ( ( x > b ) ? b : x ); },\n\n\n distance: function (p1, p2) {\n\n var xd = p2[0] - p1[0];\n var yd = p2[1] - p1[1];\n var zd = p2[2] - p1[2];\n return _Math.sqrt(xd * xd + yd * yd + zd * zd);\n },\n\n /*unwrapDegrees: function ( r ) {\r\n r = r % 360;\r\n if (r > 180) r -= 360;\r\n if (r < -180) r += 360;\r\n return r;\r\n },\r\n unwrapRadian: function( r ){\r\n r = r % _Math.TwoPI;\r\n if (r > _Math.PI) r -= _Math.TwoPI;\r\n if (r < -_Math.PI) r += _Math.TwoPI;\r\n return r;\r\n },*/\n\n acosClamp: function (cos) {\n\n if (cos > 1) return 0;else if (cos < -1) return _Math.PI;else return _Math.acos(cos);\n },\n\n distanceVector: function (v1, v2) {\n\n var xd = v1.x - v2.x;\n var yd = v1.y - v2.y;\n var zd = v1.z - v2.z;\n return xd * xd + yd * yd + zd * zd;\n },\n\n dotVectors: function (a, b) {\n\n return a.x * b.x + a.y * b.y + a.z * b.z;\n }\n\n};\n\nfunction printError(clazz, msg) {\n console.error(\"[OIMO] \" + clazz + \": \" + msg);\n}\n\n// A performance evaluator\n\nfunction InfoDisplay(world) {\n\n this.parent = world;\n\n this.infos = new Float32Array(13);\n this.f = [0, 0, 0];\n\n this.times = [0, 0, 0, 0];\n\n this.broadPhase = this.parent.broadPhaseType;\n\n this.version = REVISION;\n\n this.fps = 0;\n\n this.tt = 0;\n\n this.broadPhaseTime = 0;\n this.narrowPhaseTime = 0;\n this.solvingTime = 0;\n this.totalTime = 0;\n this.updateTime = 0;\n\n this.MaxBroadPhaseTime = 0;\n this.MaxNarrowPhaseTime = 0;\n this.MaxSolvingTime = 0;\n this.MaxTotalTime = 0;\n this.MaxUpdateTime = 0;\n}\n\nObject.assign(InfoDisplay.prototype, {\n\n setTime: function (n) {\n this.times[n || 0] = performance.now();\n },\n\n resetMax: function () {\n\n this.MaxBroadPhaseTime = 0;\n this.MaxNarrowPhaseTime = 0;\n this.MaxSolvingTime = 0;\n this.MaxTotalTime = 0;\n this.MaxUpdateTime = 0;\n },\n\n calcBroadPhase: function () {\n\n this.setTime(2);\n this.broadPhaseTime = this.times[2] - this.times[1];\n },\n\n calcNarrowPhase: function () {\n\n this.setTime(3);\n this.narrowPhaseTime = this.times[3] - this.times[2];\n },\n\n calcEnd: function () {\n\n this.setTime(2);\n this.solvingTime = this.times[2] - this.times[1];\n this.totalTime = this.times[2] - this.times[0];\n this.updateTime = this.totalTime - (this.broadPhaseTime + this.narrowPhaseTime + this.solvingTime);\n\n if (this.tt === 100) this.resetMax();\n\n if (this.tt > 100) {\n if (this.broadPhaseTime > this.MaxBroadPhaseTime) this.MaxBroadPhaseTime = this.broadPhaseTime;\n if (this.narrowPhaseTime > this.MaxNarrowPhaseTime) this.MaxNarrowPhaseTime = this.narrowPhaseTime;\n if (this.solvingTime > this.MaxSolvingTime) this.MaxSolvingTime = this.solvingTime;\n if (this.totalTime > this.MaxTotalTime) this.MaxTotalTime = this.totalTime;\n if (this.updateTime > this.MaxUpdateTime) this.MaxUpdateTime = this.updateTime;\n }\n\n this.upfps();\n\n this.tt++;\n if (this.tt > 500) this.tt = 0;\n },\n\n upfps: function () {\n this.f[1] = Date.now();\n if (this.f[1] - 1000 > this.f[0]) {\n this.f[0] = this.f[1];this.fps = this.f[2];this.f[2] = 0;\n }this.f[2]++;\n },\n\n show: function () {\n 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\");\n return info;\n },\n\n toArray: function () {\n this.infos[0] = this.parent.broadPhase.types;\n this.infos[1] = this.parent.numRigidBodies;\n this.infos[2] = this.parent.numContacts;\n this.infos[3] = this.parent.broadPhase.numPairChecks;\n this.infos[4] = this.parent.numContactPoints;\n this.infos[5] = this.parent.numIslands;\n this.infos[6] = this.broadPhaseTime;\n this.infos[7] = this.narrowPhaseTime;\n this.infos[8] = this.solvingTime;\n this.infos[9] = this.updateTime;\n this.infos[10] = this.totalTime;\n this.infos[11] = this.fps;\n return this.infos;\n }\n\n});\n\nfunction Vec3(x, y, z) {\n\n this.x = x || 0;\n this.y = y || 0;\n this.z = z || 0;\n}\n\nObject.assign(Vec3.prototype, {\n\n Vec3: true,\n\n set: function (x, y, z) {\n\n this.x = x;\n this.y = y;\n this.z = z;\n return this;\n },\n\n add: function (a, b) {\n\n if (b !== undefined) return this.addVectors(a, b);\n\n this.x += a.x;\n this.y += a.y;\n this.z += a.z;\n return this;\n },\n\n addVectors: function (a, b) {\n\n this.x = a.x + b.x;\n this.y = a.y + b.y;\n this.z = a.z + b.z;\n return this;\n },\n\n addEqual: function (v) {\n\n this.x += v.x;\n this.y += v.y;\n this.z += v.z;\n return this;\n },\n\n sub: function (a, b) {\n\n if (b !== undefined) return this.subVectors(a, b);\n\n this.x -= a.x;\n this.y -= a.y;\n this.z -= a.z;\n return this;\n },\n\n subVectors: function (a, b) {\n\n this.x = a.x - b.x;\n this.y = a.y - b.y;\n this.z = a.z - b.z;\n return this;\n },\n\n subEqual: function (v) {\n\n this.x -= v.x;\n this.y -= v.y;\n this.z -= v.z;\n return this;\n },\n\n scale: function (v, s) {\n\n this.x = v.x * s;\n this.y = v.y * s;\n this.z = v.z * s;\n return this;\n },\n\n scaleEqual: function (s) {\n\n this.x *= s;\n this.y *= s;\n this.z *= s;\n return this;\n },\n\n multiply: function (v) {\n\n this.x *= v.x;\n this.y *= v.y;\n this.z *= v.z;\n return this;\n },\n\n multiplyScalar: function (s) {\n\n this.x *= s;\n this.y *= s;\n this.z *= s;\n return this;\n },\n\n /*scaleV: function( v ){\n this.x *= v.x;\n this.y *= v.y;\n this.z *= v.z;\n return this;\n },\n scaleVectorEqual: function( v ){\n this.x *= v.x;\n this.y *= v.y;\n this.z *= v.z;\n return this;\n },*/\n\n addScaledVector: function (v, s) {\n\n this.x += v.x * s;\n this.y += v.y * s;\n this.z += v.z * s;\n\n return this;\n },\n\n subScaledVector: function (v, s) {\n\n this.x -= v.x * s;\n this.y -= v.y * s;\n this.z -= v.z * s;\n\n return this;\n },\n\n /*addTime: function ( v, t ) {\n this.x += v.x * t;\n this.y += v.y * t;\n this.z += v.z * t;\n return this;\n },\n \n addScale: function ( v, s ) {\n this.x += v.x * s;\n this.y += v.y * s;\n this.z += v.z * s;\n return this;\n },\n subScale: function ( v, s ) {\n this.x -= v.x * s;\n this.y -= v.y * s;\n this.z -= v.z * s;\n return this;\n },*/\n\n cross: function (a, b) {\n\n if (b !== undefined) return this.crossVectors(a, b);\n\n var x = this.x,\n y = this.y,\n z = this.z;\n\n this.x = y * a.z - z * a.y;\n this.y = z * a.x - x * a.z;\n this.z = x * a.y - y * a.x;\n\n return this;\n },\n\n crossVectors: function (a, b) {\n\n var ax = a.x,\n ay = a.y,\n az = a.z;\n var bx = b.x,\n by = b.y,\n bz = b.z;\n\n this.x = ay * bz - az * by;\n this.y = az * bx - ax * bz;\n this.z = ax * by - ay * bx;\n\n return this;\n },\n\n tangent: function (a) {\n\n var ax = a.x,\n ay = a.y,\n az = a.z;\n\n this.x = ay * ax - az * az;\n this.y = -az * ay - ax * ax;\n this.z = ax * az + ay * ay;\n\n return this;\n },\n\n invert: function (v) {\n\n this.x = -v.x;\n this.y = -v.y;\n this.z = -v.z;\n return this;\n },\n\n negate: function () {\n\n this.x = -this.x;\n this.y = -this.y;\n this.z = -this.z;\n\n return this;\n },\n\n dot: function (v) {\n\n return this.x * v.x + this.y * v.y + this.z * v.z;\n },\n\n addition: function () {\n\n return this.x + this.y + this.z;\n },\n\n lengthSq: function () {\n\n return this.x * this.x + this.y * this.y + this.z * this.z;\n },\n\n length: function () {\n\n return _Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);\n },\n\n copy: function (v) {\n\n this.x = v.x;\n this.y = v.y;\n this.z = v.z;\n return this;\n },\n\n /*mul: function( b, a, m ){\n return this.mulMat( m, a ).add( b );\n },\n mulMat: function( m, a ){\n var e = m.elements;\n var x = a.x, y = a.y, z = a.z;\n this.x = e[ 0 ] * x + e[ 1 ] * y + e[ 2 ] * z;\n this.y = e[ 3 ] * x + e[ 4 ] * y + e[ 5 ] * z;\n this.z = e[ 6 ] * x + e[ 7 ] * y + e[ 8 ] * z;\n return this;\n },*/\n\n applyMatrix3: function (m, transpose) {\n\n //if( transpose ) m = m.clone().transpose();\n var x = this.x,\n y = this.y,\n z = this.z;\n var e = m.elements;\n\n if (transpose) {\n\n this.x = e[0] * x + e[1] * y + e[2] * z;\n this.y = e[3] * x + e[4] * y + e[5] * z;\n this.z = e[6] * x + e[7] * y + e[8] * z;\n } else {\n\n this.x = e[0] * x + e[3] * y + e[6] * z;\n this.y = e[1] * x + e[4] * y + e[7] * z;\n this.z = e[2] * x + e[5] * y + e[8] * z;\n }\n\n return this;\n },\n\n applyQuaternion: function (q) {\n\n var x = this.x;\n var y = this.y;\n var z = this.z;\n\n var qx = q.x;\n var qy = q.y;\n var qz = q.z;\n var qw = q.w;\n\n // calculate quat * vector\n\n var ix = qw * x + qy * z - qz * y;\n var iy = qw * y + qz * x - qx * z;\n var iz = qw * z + qx * y - qy * x;\n var iw = -qx * x - qy * y - qz * z;\n\n // calculate result * inverse quat\n\n this.x = ix * qw + iw * -qx + iy * -qz - iz * -qy;\n this.y = iy * qw + iw * -qy + iz * -qx - ix * -qz;\n this.z = iz * qw + iw * -qz + ix * -qy - iy * -qx;\n\n return this;\n },\n\n testZero: function () {\n\n if (this.x !== 0 || this.y !== 0 || this.z !== 0) return true;else return false;\n },\n\n testDiff: function (v) {\n\n return this.equals(v) ? false : true;\n },\n\n equals: function (v) {\n\n return v.x === this.x && v.y === this.y && v.z === this.z;\n },\n\n clone: function () {\n\n return new this.constructor(this.x, this.y, this.z);\n },\n\n toString: function () {\n\n return \"Vec3[\" + this.x.toFixed(4) + \", \" + this.y.toFixed(4) + \", \" + this.z.toFixed(4) + \"]\";\n },\n\n multiplyScalar: function (scalar) {\n\n if (isFinite(scalar)) {\n this.x *= scalar;\n this.y *= scalar;\n this.z *= scalar;\n } else {\n this.x = 0;\n this.y = 0;\n this.z = 0;\n }\n\n return this;\n },\n\n divideScalar: function (scalar) {\n\n return this.multiplyScalar(1 / scalar);\n },\n\n normalize: function () {\n\n return this.divideScalar(this.length());\n },\n\n toArray: function (array, offset) {\n\n if (offset === undefined) offset = 0;\n\n array[offset] = this.x;\n array[offset + 1] = this.y;\n array[offset + 2] = this.z;\n },\n\n fromArray: function (array, offset) {\n\n if (offset === undefined) offset = 0;\n\n this.x = array[offset];\n this.y = array[offset + 1];\n this.z = array[offset + 2];\n return this;\n }\n\n});\n\nfunction Quat(x, y, z, w) {\n\n this.x = x || 0;\n this.y = y || 0;\n this.z = z || 0;\n this.w = w !== undefined ? w : 1;\n}\n\nObject.assign(Quat.prototype, {\n\n Quat: true,\n\n set: function (x, y, z, w) {\n\n this.x = x;\n this.y = y;\n this.z = z;\n this.w = w;\n\n return this;\n },\n\n addTime: function (v, t) {\n\n var ax = v.x,\n ay = v.y,\n az = v.z;\n var qw = this.w,\n qx = this.x,\n qy = this.y,\n qz = this.z;\n t *= 0.5;\n this.x += t * (ax * qw + ay * qz - az * qy);\n this.y += t * (ay * qw + az * qx - ax * qz);\n this.z += t * (az * qw + ax * qy - ay * qx);\n this.w += t * (-ax * qx - ay * qy - az * qz);\n this.normalize();\n return this;\n },\n\n /*mul: function( q1, q2 ){\n var ax = q1.x, ay = q1.y, az = q1.z, as = q1.w,\n bx = q2.x, by = q2.y, bz = q2.z, bs = q2.w;\n this.x = ax * bs + as * bx + ay * bz - az * by;\n this.y = ay * bs + as * by + az * bx - ax * bz;\n this.z = az * bs + as * bz + ax * by - ay * bx;\n this.w = as * bs - ax * bx - ay * by - az * bz;\n return this;\n },*/\n\n multiply: function (q, p) {\n\n if (p !== undefined) return this.multiplyQuaternions(q, p);\n return this.multiplyQuaternions(this, q);\n },\n\n multiplyQuaternions: function (a, b) {\n\n var qax = a.x,\n qay = a.y,\n qaz = a.z,\n qaw = a.w;\n var qbx = b.x,\n qby = b.y,\n qbz = b.z,\n qbw = b.w;\n\n this.x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;\n this.y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;\n this.z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;\n this.w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;\n return this;\n },\n\n setFromUnitVectors: function (v1, v2) {\n\n var vx = new Vec3();\n var r = v1.dot(v2) + 1;\n\n if (r < _Math.EPS2) {\n\n r = 0;\n if (_Math.abs(v1.x) > _Math.abs(v1.z)) vx.set(-v1.y, v1.x, 0);else vx.set(0, -v1.z, v1.y);\n } else {\n\n vx.crossVectors(v1, v2);\n }\n\n this._x = vx.x;\n this._y = vx.y;\n this._z = vx.z;\n this._w = r;\n\n return this.normalize();\n },\n\n arc: function (v1, v2) {\n\n var x1 = v1.x;\n var y1 = v1.y;\n var z1 = v1.z;\n var x2 = v2.x;\n var y2 = v2.y;\n var z2 = v2.z;\n var d = x1 * x2 + y1 * y2 + z1 * z2;\n if (d == -1) {\n x2 = y1 * x1 - z1 * z1;\n y2 = -z1 * y1 - x1 * x1;\n z2 = x1 * z1 + y1 * y1;\n d = 1 / _Math.sqrt(x2 * x2 + y2 * y2 + z2 * z2);\n this.w = 0;\n this.x = x2 * d;\n this.y = y2 * d;\n this.z = z2 * d;\n return this;\n }\n var cx = y1 * z2 - z1 * y2;\n var cy = z1 * x2 - x1 * z2;\n var cz = x1 * y2 - y1 * x2;\n this.w = _Math.sqrt((1 + d) * 0.5);\n d = 0.5 / this.w;\n this.x = cx * d;\n this.y = cy * d;\n this.z = cz * d;\n return this;\n },\n\n normalize: function () {\n\n var l = this.length();\n if (l === 0) {\n this.set(0, 0, 0, 1);\n } else {\n l = 1 / l;\n this.x = this.x * l;\n this.y = this.y * l;\n this.z = this.z * l;\n this.w = this.w * l;\n }\n return this;\n },\n\n inverse: function () {\n\n return this.conjugate().normalize();\n },\n\n invert: function (q) {\n\n this.x = q.x;\n this.y = q.y;\n this.z = q.z;\n this.w = q.w;\n this.conjugate().normalize();\n return this;\n },\n\n conjugate: function () {\n\n this.x *= -1;\n this.y *= -1;\n this.z *= -1;\n return this;\n },\n\n length: function () {\n\n return _Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);\n },\n\n lengthSq: function () {\n\n return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;\n },\n\n copy: function (q) {\n\n this.x = q.x;\n this.y = q.y;\n this.z = q.z;\n this.w = q.w;\n return this;\n },\n\n clone: function (q) {\n\n return new Quat(this.x, this.y, this.z, this.w);\n },\n\n testDiff: function (q) {\n\n return this.equals(q) ? false : true;\n },\n\n equals: function (q) {\n\n return this.x === q.x && this.y === q.y && this.z === q.z && this.w === q.w;\n },\n\n toString: function () {\n\n return \"Quat[\" + this.x.toFixed(4) + \", (\" + this.y.toFixed(4) + \", \" + this.z.toFixed(4) + \", \" + this.w.toFixed(4) + \")]\";\n },\n\n setFromEuler: function (x, y, z) {\n\n var c1 = Math.cos(x * 0.5);\n var c2 = Math.cos(y * 0.5);\n var c3 = Math.cos(z * 0.5);\n var s1 = Math.sin(x * 0.5);\n var s2 = Math.sin(y * 0.5);\n var s3 = Math.sin(z * 0.5);\n\n // XYZ\n this.x = s1 * c2 * c3 + c1 * s2 * s3;\n this.y = c1 * s2 * c3 - s1 * c2 * s3;\n this.z = c1 * c2 * s3 + s1 * s2 * c3;\n this.w = c1 * c2 * c3 - s1 * s2 * s3;\n\n return this;\n },\n\n setFromAxis: function (axis, rad) {\n\n axis.normalize();\n rad = rad * 0.5;\n var s = _Math.sin(rad);\n this.x = s * axis.x;\n this.y = s * axis.y;\n this.z = s * axis.z;\n this.w = _Math.cos(rad);\n return this;\n },\n\n setFromMat33: function (m) {\n\n var trace = m[0] + m[4] + m[8];\n var s;\n\n if (trace > 0) {\n\n s = _Math.sqrt(trace + 1.0);\n this.w = 0.5 / s;\n s = 0.5 / s;\n this.x = (m[5] - m[7]) * s;\n this.y = (m[6] - m[2]) * s;\n this.z = (m[1] - m[3]) * s;\n } else {\n\n var out = [];\n var i = 0;\n if (m[4] > m[0]) i = 1;\n if (m[8] > m[i * 3 + i]) i = 2;\n\n var j = (i + 1) % 3;\n var k = (i + 2) % 3;\n\n s = _Math.sqrt(m[i * 3 + i] - m[j * 3 + j] - m[k * 3 + k] + 1.0);\n out[i] = 0.5 * fRoot;\n s = 0.5 / fRoot;\n this.w = (m[j * 3 + k] - m[k * 3 + j]) * s;\n out[j] = (m[j * 3 + i] + m[i * 3 + j]) * s;\n out[k] = (m[k * 3 + i] + m[i * 3 + k]) * s;\n\n this.x = out[1];\n this.y = out[2];\n this.z = out[3];\n }\n\n return this;\n },\n\n toArray: function (array, offset) {\n\n offset = offset || 0;\n\n array[offset] = this.x;\n array[offset + 1] = this.y;\n array[offset + 2] = this.z;\n array[offset + 3] = this.w;\n },\n\n fromArray: function (array, offset) {\n\n offset = offset || 0;\n this.set(array[offset], array[offset + 1], array[offset + 2], array[offset + 3]);\n return this;\n }\n\n});\n\nfunction Mat33(e00, e01, e02, e10, e11, e12, e20, e21, e22) {\n\n this.elements = [1, 0, 0, 0, 1, 0, 0, 0, 1];\n\n if (arguments.length > 0) {\n\n console.error('OIMO.Mat33: the constructor no longer reads arguments. use .set() instead.');\n }\n}\n\nObject.assign(Mat33.prototype, {\n\n Mat33: true,\n\n set: function (e00, e01, e02, e10, e11, e12, e20, e21, e22) {\n\n var te = this.elements;\n te[0] = e00;te[1] = e01;te[2] = e02;\n te[3] = e10;te[4] = e11;te[5] = e12;\n te[6] = e20;te[7] = e21;te[8] = e22;\n return this;\n },\n\n add: function (a, b) {\n\n if (b !== undefined) return this.addMatrixs(a, b);\n\n var e = this.elements,\n te = a.elements;\n e[0] += te[0];e[1] += te[1];e[2] += te[2];\n e[3] += te[3];e[4] += te[4];e[5] += te[5];\n e[6] += te[6];e[7] += te[7];e[8] += te[8];\n return this;\n },\n\n addMatrixs: function (a, b) {\n\n var te = this.elements,\n tem1 = a.elements,\n tem2 = b.elements;\n te[0] = tem1[0] + tem2[0];te[1] = tem1[1] + tem2[1];te[2] = tem1[2] + tem2[2];\n te[3] = tem1[3] + tem2[3];te[4] = tem1[4] + tem2[4];te[5] = tem1[5] + tem2[5];\n te[6] = tem1[6] + tem2[6];te[7] = tem1[7] + tem2[7];te[8] = tem1[8] + tem2[8];\n return this;\n },\n\n addEqual: function (m) {\n\n var te = this.elements,\n tem = m.elements;\n te[0] += tem[0];te[1] += tem[1];te[2] += tem[2];\n te[3] += tem[3];te[4] += tem[4];te[5] += tem[5];\n te[6] += tem[6];te[7] += tem[7];te[8] += tem[8];\n return this;\n },\n\n sub: function (a, b) {\n\n if (b !== undefined) return this.subMatrixs(a, b);\n\n var e = this.elements,\n te = a.elements;\n e[0] -= te[0];e[1] -= te[1];e[2] -= te[2];\n e[3] -= te[3];e[4] -= te[4];e[5] -= te[5];\n e[6] -= te[6];e[7] -= te[7];e[8] -= te[8];\n return this;\n },\n\n subMatrixs: function (a, b) {\n\n var te = this.elements,\n tem1 = a.elements,\n tem2 = b.elements;\n te[0] = tem1[0] - tem2[0];te[1] = tem1[1] - tem2[1];te[2] = tem1[2] - tem2[2];\n te[3] = tem1[3] - tem2[3];te[4] = tem1[4] - tem2[4];te[5] = tem1[5] - tem2[5];\n te[6] = tem1[6] - tem2[6];te[7] = tem1[7] - tem2[7];te[8] = tem1[8] - tem2[8];\n return this;\n },\n\n subEqual: function (m) {\n\n var te = this.elements,\n tem = m.elements;\n te[0] -= tem[0];te[1] -= tem[1];te[2] -= tem[2];\n te[3] -= tem[3];te[4] -= tem[4];te[5] -= tem[5];\n te[6] -= tem[6];te[7] -= tem[7];te[8] -= tem[8];\n return this;\n },\n\n scale: function (m, s) {\n\n var te = this.elements,\n tm = m.elements;\n te[0] = tm[0] * s;te[1] = tm[1] * s;te[2] = tm[2] * s;\n te[3] = tm[3] * s;te[4] = tm[4] * s;te[5] = tm[5] * s;\n te[6] = tm[6] * s;te[7] = tm[7] * s;te[8] = tm[8] * s;\n return this;\n },\n\n scaleEqual: function (s) {\n // multiplyScalar\n\n var te = this.elements;\n te[0] *= s;te[1] *= s;te[2] *= s;\n te[3] *= s;te[4] *= s;te[5] *= s;\n te[6] *= s;te[7] *= s;te[8] *= s;\n return this;\n },\n\n multiplyMatrices: function (m1, m2, transpose) {\n\n if (transpose) m2 = m2.clone().transpose();\n\n var te = this.elements;\n var tm1 = m1.elements;\n var tm2 = m2.elements;\n\n var a0 = tm1[0],\n a3 = tm1[3],\n a6 = tm1[6];\n var a1 = tm1[1],\n a4 = tm1[4],\n a7 = tm1[7];\n var a2 = tm1[2],\n a5 = tm1[5],\n a8 = tm1[8];\n\n var b0 = tm2[0],\n b3 = tm2[3],\n b6 = tm2[6];\n var b1 = tm2[1],\n b4 = tm2[4],\n b7 = tm2[7];\n var b2 = tm2[2],\n b5 = tm2[5],\n b8 = tm2[8];\n\n te[0] = a0 * b0 + a1 * b3 + a2 * b6;\n te[1] = a0 * b1 + a1 * b4 + a2 * b7;\n te[2] = a0 * b2 + a1 * b5 + a2 * b8;\n te[3] = a3 * b0 + a4 * b3 + a5 * b6;\n te[4] = a3 * b1 + a4 * b4 + a5 * b7;\n te[5] = a3 * b2 + a4 * b5 + a5 * b8;\n te[6] = a6 * b0 + a7 * b3 + a8 * b6;\n te[7] = a6 * b1 + a7 * b4 + a8 * b7;\n te[8] = a6 * b2 + a7 * b5 + a8 * b8;\n\n return this;\n },\n\n /*mul: function ( m1, m2, transpose ) {\r\n if( transpose ) m2 = m2.clone().transpose();\r\n var te = this.elements;\r\n var tm1 = m1.elements;\r\n var tm2 = m2.elements;\r\n //var tmp;\r\n var a0 = tm1[0], a3 = tm1[3], a6 = tm1[6];\r\n var a1 = tm1[1], a4 = tm1[4], a7 = tm1[7];\r\n var a2 = tm1[2], a5 = tm1[5], a8 = tm1[8];\r\n var b0 = tm2[0], b3 = tm2[3], b6 = tm2[6];\r\n var b1 = tm2[1], b4 = tm2[4], b7 = tm2[7];\r\n var b2 = tm2[2], b5 = tm2[5], b8 = tm2[8];\r\n /*if( transpose ){\r\n tmp = b1; b1 = b3; b3 = tmp;\r\n tmp = b2; b2 = b6; b6 = tmp;\r\n tmp = b5; b5 = b7; b7 = tmp;\r\n }\r\n te[0] = a0*b0 + a1*b3 + a2*b6;\r\n te[1] = a0*b1 + a1*b4 + a2*b7;\r\n te[2] = a0*b2 + a1*b5 + a2*b8;\r\n te[3] = a3*b0 + a4*b3 + a5*b6;\r\n te[4] = a3*b1 + a4*b4 + a5*b7;\r\n te[5] = a3*b2 + a4*b5 + a5*b8;\r\n te[6] = a6*b0 + a7*b3 + a8*b6;\r\n te[7] = a6*b1 + a7*b4 + a8*b7;\r\n te[8] = a6*b2 + a7*b5 + a8*b8;\r\n return this;\r\n },*/\n\n transpose: function (m) {\n\n if (m !== undefined) {\n var a = m.elements;\n this.set(a[0], a[3], a[6], a[1], a[4], a[7], a[2], a[5], a[8]);\n return this;\n }\n\n var te = this.elements;\n var a01 = te[1],\n a02 = te[2],\n a12 = te[5];\n te[1] = te[3];\n te[2] = te[6];\n te[3] = a01;\n te[5] = te[7];\n te[6] = a02;\n te[7] = a12;\n return this;\n },\n\n /*mulScale: function ( m, sx, sy, sz, Prepend ) {\r\n var prepend = Prepend || false;\r\n var te = this.elements, tm = m.elements;\r\n if(prepend){\r\n te[0] = sx*tm[0]; te[1] = sx*tm[1]; te[2] = sx*tm[2];\r\n te[3] = sy*tm[3]; te[4] = sy*tm[4]; te[5] = sy*tm[5];\r\n te[6] = sz*tm[6]; te[7] = sz*tm[7]; te[8] = sz*tm[8];\r\n }else{\r\n te[0] = tm[0]*sx; te[1] = tm[1]*sy; te[2] = tm[2]*sz;\r\n te[3] = tm[3]*sx; te[4] = tm[4]*sy; te[5] = tm[5]*sz;\r\n te[6] = tm[6]*sx; te[7] = tm[7]*sy; te[8] = tm[8]*sz;\r\n }\r\n return this;\r\n },\r\n transpose: function ( m ) {\r\n var te = this.elements, tm = m.elements;\r\n te[0] = tm[0]; te[1] = tm[3]; te[2] = tm[6];\r\n te[3] = tm[1]; te[4] = tm[4]; te[5] = tm[7];\r\n te[6] = tm[2]; te[7] = tm[5]; te[8] = tm[8];\r\n return this;\r\n },*/\n\n setQuat: function (q) {\n\n var te = this.elements;\n var x = q.x,\n y = q.y,\n z = q.z,\n w = q.w;\n var x2 = x + x,\n y2 = y + y,\n z2 = z + z;\n var xx = x * x2,\n xy = x * y2,\n xz = x * z2;\n var yy = y * y2,\n yz = y * z2,\n zz = z * z2;\n var wx = w * x2,\n wy = w * y2,\n wz = w * z2;\n\n te[0] = 1 - (yy + zz);\n te[1] = xy - wz;\n te[2] = xz + wy;\n\n te[3] = xy + wz;\n te[4] = 1 - (xx + zz);\n te[5] = yz - wx;\n\n te[6] = xz - wy;\n te[7] = yz + wx;\n te[8] = 1 - (xx + yy);\n\n return this;\n },\n\n invert: function (m) {\n\n var te = this.elements,\n tm = m.elements,\n a00 = tm[0],\n a10 = tm[3],\n a20 = tm[6],\n a01 = tm[1],\n a11 = tm[4],\n a21 = tm[7],\n a02 = tm[2],\n a12 = tm[5],\n a22 = tm[8],\n b01 = a22 * a11 - a12 * a21,\n b11 = -a22 * a10 + a12 * a20,\n b21 = a21 * a10 - a11 * a20,\n det = a00 * b01 + a01 * b11 + a02 * b21;\n\n if (det === 0) {\n console.log(\"can't invert matrix, determinant is 0\");\n return this.identity();\n }\n\n det = 1.0 / det;\n te[0] = b01 * det;\n te[1] = (-a22 * a01 + a02 * a21) * det;\n te[2] = (a12 * a01 - a02 * a11) * det;\n te[3] = b11 * det;\n te[4] = (a22 * a00 - a02 * a20) * det;\n te[5] = (-a12 * a00 + a02 * a10) * det;\n te[6] = b21 * det;\n te[7] = (-a21 * a00 + a01 * a20) * det;\n te[8] = (a11 * a00 - a01 * a10) * det;\n return this;\n },\n\n addOffset: function (m, v) {\n\n var relX = v.x;\n var relY = v.y;\n var relZ = v.z;\n\n var te = this.elements;\n te[0] += m * (relY * relY + relZ * relZ);\n te[4] += m * (relX * relX + relZ * relZ);\n te[8] += m * (relX * relX + relY * relY);\n var xy = m * relX * relY;\n var yz = m * relY * relZ;\n var zx = m * relZ * relX;\n te[1] -= xy;\n te[3] -= xy;\n te[2] -= yz;\n te[6] -= yz;\n te[5] -= zx;\n te[7] -= zx;\n return this;\n },\n\n subOffset: function (m, v) {\n\n var relX = v.x;\n var relY = v.y;\n var relZ = v.z;\n\n var te = this.elements;\n te[0] -= m * (relY * relY + relZ * relZ);\n te[4] -= m * (relX * relX + relZ * relZ);\n te[8] -= m * (relX * relX + relY * relY);\n var xy = m * relX * relY;\n var yz = m * relY * relZ;\n var zx = m * relZ * relX;\n te[1] += xy;\n te[3] += xy;\n te[2] += yz;\n te[6] += yz;\n te[5] += zx;\n te[7] += zx;\n return this;\n },\n\n // OK \n\n multiplyScalar: function (s) {\n\n var te = this.elements;\n\n te[0] *= s;te[3] *= s;te[6] *= s;\n te[1] *= s;te[4] *= s;te[7] *= s;\n te[2] *= s;te[5] *= s;te[8] *= s;\n\n return this;\n },\n\n identity: function () {\n\n this.set(1, 0, 0, 0, 1, 0, 0, 0, 1);\n return this;\n },\n\n clone: function () {\n\n return new Mat33().fromArray(this.elements);\n },\n\n copy: function (m) {\n\n for (var i = 0; i < 9; i++) this.elements[i] = m.elements[i];\n return this;\n },\n\n determinant: function () {\n\n var te = this.elements;\n var a = te[0],\n b = te[1],\n c = te[2],\n d = te[3],\n e = te[4],\n f = te[5],\n g = te[6],\n h = te[7],\n i = te[8];\n\n return a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g;\n },\n\n fromArray: function (array, offset) {\n\n if (offset === undefined) offset = 0;\n\n for (var i = 0; i < 9; i++) {\n\n this.elements[i] = array[i + offset];\n }\n\n return this;\n },\n\n toArray: function (array, offset) {\n\n if (array === undefined) array = [];\n if (offset === undefined) offset = 0;\n\n var te = this.elements;\n\n array[offset] = te[0];\n array[offset + 1] = te[1];\n array[offset + 2] = te[2];\n\n array[offset + 3] = te[3];\n array[offset + 4] = te[4];\n array[offset + 5] = te[5];\n\n array[offset + 6] = te[6];\n array[offset + 7] = te[7];\n array[offset + 8] = te[8];\n\n return array;\n }\n\n});\n\n/**\r\n * An axis-aligned bounding box.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction AABB(minX, maxX, minY, maxY, minZ, maxZ) {\n\n this.elements = new Float32Array(6);\n var te = this.elements;\n\n te[0] = minX || 0;te[1] = minY || 0;te[2] = minZ || 0;\n te[3] = maxX || 0;te[4] = maxY || 0;te[5] = maxZ || 0;\n}\n\nObject.assign(AABB.prototype, {\n\n AABB: true,\n\n set: function (minX, maxX, minY, maxY, minZ, maxZ) {\n\n var te = this.elements;\n te[0] = minX;\n te[3] = maxX;\n te[1] = minY;\n te[4] = maxY;\n te[2] = minZ;\n te[5] = maxZ;\n return this;\n },\n\n intersectTest: function (aabb) {\n\n var te = this.elements;\n var ue = aabb.elements;\n 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;\n },\n\n intersectTestTwo: function (aabb) {\n\n var te = this.elements;\n var ue = aabb.elements;\n 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;\n },\n\n clone: function () {\n\n return new this.constructor().fromArray(this.elements);\n },\n\n copy: function (aabb, margin) {\n\n var m = margin || 0;\n var me = aabb.elements;\n this.set(me[0] - m, me[3] + m, me[1] - m, me[4] + m, me[2] - m, me[5] + m);\n return this;\n },\n\n fromArray: function (array) {\n\n this.elements.set(array);\n return this;\n },\n\n // Set this AABB to the combined AABB of aabb1 and aabb2.\n\n combine: function (aabb1, aabb2) {\n\n var a = aabb1.elements;\n var b = aabb2.elements;\n var te = this.elements;\n\n te[0] = a[0] < b[0] ? a[0] : b[0];\n te[1] = a[1] < b[1] ? a[1] : b[1];\n te[2] = a[2] < b[2] ? a[2] : b[2];\n\n te[3] = a[3] > b[3] ? a[3] : b[3];\n te[4] = a[4] > b[4] ? a[4] : b[4];\n te[5] = a[5] > b[5] ? a[5] : b[5];\n\n return this;\n },\n\n // Get the surface area.\n\n surfaceArea: function () {\n\n var te = this.elements;\n var a = te[3] - te[0];\n var h = te[4] - te[1];\n var d = te[5] - te[2];\n return 2 * (a * (h + d) + h * d);\n },\n\n // Get whether the AABB intersects with the point or not.\n\n intersectsWithPoint: function (x, y, z) {\n\n var te = this.elements;\n return x >= te[0] && x <= te[3] && y >= te[1] && y <= te[4] && z >= te[2] && z <= te[5];\n },\n\n /**\r\n * Set the AABB from an array\r\n * of vertices. From THREE.\r\n * @author WestLangley\r\n * @author xprogram\r\n */\n\n setFromPoints: function (arr) {\n this.makeEmpty();\n for (var i = 0; i < arr.length; i++) {\n this.expandByPoint(arr[i]);\n }\n },\n\n makeEmpty: function () {\n this.set(-Infinity, -Infinity, -Infinity, Infinity, Infinity, Infinity);\n },\n\n expandByPoint: function (pt) {\n var te = this.elements;\n 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));\n },\n\n expandByScalar: function (s) {\n\n var te = this.elements;\n te[0] += -s;\n te[1] += -s;\n te[2] += -s;\n te[3] += s;\n te[4] += s;\n te[5] += s;\n }\n\n});\n\nvar count = 0;\nfunction ShapeIdCount() {\n return count++;\n}\n\n/**\r\n * A shape is used to detect collisions of rigid bodies.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction Shape(config) {\n\n this.type = SHAPE_NULL;\n\n // global identification of the shape should be unique to the shape.\n this.id = ShapeIdCount();\n\n // previous shape in parent rigid body. Used for fast interations.\n this.prev = null;\n\n // next shape in parent rigid body. Used for fast interations.\n this.next = null;\n\n // proxy of the shape used for broad-phase collision detection.\n this.proxy = null;\n\n // parent rigid body of the shape.\n this.parent = null;\n\n // linked list of the contacts with the shape.\n this.contactLink = null;\n\n // number of the contacts with the shape.\n this.numContacts = 0;\n\n // center of gravity of the shape in world coordinate system.\n this.position = new Vec3();\n\n // rotation matrix of the shape in world coordinate system.\n this.rotation = new Mat33();\n\n // position of the shape in parent's coordinate system.\n this.relativePosition = new Vec3().copy(config.relativePosition);\n\n // rotation matrix of the shape in parent's coordinate system.\n this.relativeRotation = new Mat33().copy(config.relativeRotation);\n\n // axis-aligned bounding box of the shape.\n this.aabb = new AABB();\n\n // density of the shape.\n this.density = config.density;\n\n // coefficient of friction of the shape.\n this.friction = config.friction;\n\n // coefficient of restitution of the shape.\n this.restitution = config.restitution;\n\n // bits of the collision groups to which the shape belongs.\n this.belongsTo = config.belongsTo;\n\n // bits of the collision groups with which the shape collides.\n this.collidesWith = config.collidesWith;\n}\n\nObject.assign(Shape.prototype, {\n\n Shape: true,\n\n // Calculate the mass information of the shape.\n\n calculateMassInfo: function (out) {\n\n printError(\"Shape\", \"Inheritance error.\");\n },\n\n // Update the proxy of the shape.\n\n updateProxy: function () {\n\n printError(\"Shape\", \"Inheritance error.\");\n }\n\n});\n\n/**\r\n * Box shape.\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction Box(config, Width, Height, Depth) {\n\n Shape.call(this, config);\n\n this.type = SHAPE_BOX;\n\n this.width = Width;\n this.height = Height;\n this.depth = Depth;\n\n this.halfWidth = Width * 0.5;\n this.halfHeight = Height * 0.5;\n this.halfDepth = Depth * 0.5;\n\n this.dimentions = new Float32Array(18);\n this.elements = new Float32Array(24);\n}\n\nBox.prototype = Object.assign(Object.create(Shape.prototype), {\n\n constructor: Box,\n\n calculateMassInfo: function (out) {\n\n var mass = this.width * this.height * this.depth * this.density;\n var divid = 1 / 12;\n out.mass = mass;\n 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);\n },\n\n updateProxy: function () {\n\n var te = this.rotation.elements;\n var di = this.dimentions;\n // Width\n di[0] = te[0];\n di[1] = te[3];\n di[2] = te[6];\n // Height\n di[3] = te[1];\n di[4] = te[4];\n di[5] = te[7];\n // Depth\n di[6] = te[2];\n di[7] = te[5];\n di[8] = te[8];\n // half Width\n di[9] = te[0] * this.halfWidth;\n di[10] = te[3] * this.halfWidth;\n di[11] = te[6] * this.halfWidth;\n // half Height\n di[12] = te[1] * this.halfHeight;\n di[13] = te[4] * this.halfHeight;\n di[14] = te[7] * this.halfHeight;\n // half Depth\n di[15] = te[2] * this.halfDepth;\n di[16] = te[5] * this.halfDepth;\n di[17] = te[8] * this.halfDepth;\n\n var wx = di[9];\n var wy = di[10];\n var wz = di[11];\n var hx = di[12];\n var hy = di[13];\n var hz = di[14];\n var dx = di[15];\n var dy = di[16];\n var dz = di[17];\n\n var x = this.position.x;\n var y = this.position.y;\n var z = this.position.z;\n\n var v = this.elements;\n //v1\n v[0] = x + wx + hx + dx;\n v[1] = y + wy + hy + dy;\n v[2] = z + wz + hz + dz;\n //v2\n v[3] = x + wx + hx - dx;\n v[4] = y + wy + hy - dy;\n v[5] = z + wz + hz - dz;\n //v3\n v[6] = x + wx - hx + dx;\n v[7] = y + wy - hy + dy;\n v[8] = z + wz - hz + dz;\n //v4\n v[9] = x + wx - hx - dx;\n v[10] = y + wy - hy - dy;\n v[11] = z + wz - hz - dz;\n //v5\n v[12] = x - wx + hx + dx;\n v[13] = y - wy + hy + dy;\n v[14] = z - wz + hz + dz;\n //v6\n v[15] = x - wx + hx - dx;\n v[16] = y - wy + hy - dy;\n v[17] = z - wz + hz - dz;\n //v7\n v[18] = x - wx - hx + dx;\n v[19] = y - wy - hy + dy;\n v[20] = z - wz - hz + dz;\n //v8\n v[21] = x - wx - hx - dx;\n v[22] = y - wy - hy - dy;\n v[23] = z - wz - hz - dz;\n\n var w = di[9] < 0 ? -di[9] : di[9];\n var h = di[10] < 0 ? -di[10] : di[10];\n var d = di[11] < 0 ? -di[11] : di[11];\n\n w = di[12] < 0 ? w - di[12] : w + di[12];\n h = di[13] < 0 ? h - di[13] : h + di[13];\n d = di[14] < 0 ? d - di[14] : d + di[14];\n\n w = di[15] < 0 ? w - di[15] : w + di[15];\n h = di[16] < 0 ? h - di[16] : h + di[16];\n d = di[17] < 0 ? d - di[17] : d + di[17];\n\n var p = AABB_PROX;\n\n 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);\n\n if (this.proxy != null) this.proxy.update();\n }\n});\n\n/**\r\n * Sphere shape\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction Sphere(config, radius) {\n\n Shape.call(this, config);\n\n this.type = SHAPE_SPHERE;\n\n // radius of the shape.\n this.radius = radius;\n}\n\nSphere.prototype = Object.assign(Object.create(Shape.prototype), {\n\n constructor: Sphere,\n\n volume: function () {\n\n return _Math.PI * this.radius * 1.333333;\n },\n\n calculateMassInfo: function (out) {\n\n var mass = this.volume() * this.radius * this.radius * this.density; //1.333 * _Math.PI * this.radius * this.radius * this.radius * this.density;\n out.mass = mass;\n var inertia = mass * this.radius * this.radius * 0.4;\n out.inertia.set(inertia, 0, 0, 0, inertia, 0, 0, 0, inertia);\n },\n\n updateProxy: function () {\n\n var p = AABB_PROX;\n\n 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);\n\n if (this.proxy != null) this.proxy.update();\n }\n\n});\n\n/**\r\n * Cylinder shape\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction Cylinder(config, radius, height) {\n\n Shape.call(this, config);\n\n this.type = SHAPE_CYLINDER;\n\n this.radius = radius;\n this.height = height;\n this.halfHeight = height * 0.5;\n\n this.normalDirection = new Vec3();\n this.halfDirection = new Vec3();\n}\n\nCylinder.prototype = Object.assign(Object.create(Shape.prototype), {\n\n constructor: Cylinder,\n\n calculateMassInfo: function (out) {\n\n var rsq = this.radius * this.radius;\n var mass = _Math.PI * rsq * this.height * this.density;\n var inertiaXZ = (0.25 * rsq + 0.0833 * this.height * this.height) * mass;\n var inertiaY = 0.5 * rsq;\n out.mass = mass;\n out.inertia.set(inertiaXZ, 0, 0, 0, inertiaY, 0, 0, 0, inertiaXZ);\n },\n\n updateProxy: function () {\n\n var te = this.rotation.elements;\n var len, wx, hy, dz, xx, yy, zz, w, h, d, p;\n\n xx = te[1] * te[1];\n yy = te[4] * te[4];\n zz = te[7] * te[7];\n\n this.normalDirection.set(te[1], te[4], te[7]);\n this.halfDirection.scale(this.normalDirection, this.halfHeight);\n\n wx = 1 - xx;\n len = _Math.sqrt(wx * wx + xx * yy + xx * zz);\n if (len > 0) len = this.radius / len;\n wx *= len;\n hy = 1 - yy;\n len = _Math.sqrt(yy * xx + hy * hy + yy * zz);\n if (len > 0) len = this.radius / len;\n hy *= len;\n dz = 1 - zz;\n len = _Math.sqrt(zz * xx + zz * yy + dz * dz);\n if (len > 0) len = this.radius / len;\n dz *= len;\n\n w = this.halfDirection.x < 0 ? -this.halfDirection.x : this.halfDirection.x;\n h = this.halfDirection.y < 0 ? -this.halfDirection.y : this.halfDirection.y;\n d = this.halfDirection.z < 0 ? -this.halfDirection.z : this.halfDirection.z;\n\n w = wx < 0 ? w - wx : w + wx;\n h = hy < 0 ? h - hy : h + hy;\n d = dz < 0 ? d - dz : d + dz;\n\n p = AABB_PROX;\n\n 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);\n\n if (this.proxy != null) this.proxy.update();\n }\n\n});\n\n/**\r\n * Plane shape.\r\n * @author lo-th\r\n */\n\nfunction Plane(config, normal) {\n\n Shape.call(this, config);\n\n this.type = SHAPE_PLANE;\n\n // radius of the shape.\n this.normal = new Vec3(0, 1, 0);\n}\n\nPlane.prototype = Object.assign(Object.create(Shape.prototype), {\n\n constructor: Plane,\n\n volume: function () {\n\n return Number.MAX_VALUE;\n },\n\n calculateMassInfo: function (out) {\n\n out.mass = this.density; //0.0001;\n var inertia = 1;\n out.inertia.set(inertia, 0, 0, 0, inertia, 0, 0, 0, inertia);\n },\n\n updateProxy: function () {\n\n var p = AABB_PROX;\n\n var min = -_Math.INF;\n var max = _Math.INF;\n var n = this.normal;\n // The plane AABB is infinite, except if the normal is pointing along any axis\n 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);\n\n if (this.proxy != null) this.proxy.update();\n }\n\n});\n\n/**\r\n * A Particule shape\r\n * @author lo-th\r\n */\n\nfunction Particle(config, normal) {\n\n Shape.call(this, config);\n\n this.type = SHAPE_PARTICLE;\n}\n\nParticle.prototype = Object.assign(Object.create(Shape.prototype), {\n\n constructor: Particle,\n\n volume: function () {\n\n return Number.MAX_VALUE;\n },\n\n calculateMassInfo: function (out) {\n\n var inertia = 0;\n out.inertia.set(inertia, 0, 0, 0, inertia, 0, 0, 0, inertia);\n },\n\n updateProxy: function () {\n\n var p = 0; //AABB_PROX;\n\n 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);\n\n if (this.proxy != null) this.proxy.update();\n }\n\n});\n\n/**\r\n * A shape configuration holds common configuration data for constructing a shape.\r\n * These configurations can be reused safely.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction ShapeConfig() {\n\n // position of the shape in parent's coordinate system.\n this.relativePosition = new Vec3();\n // rotation matrix of the shape in parent's coordinate system.\n this.relativeRotation = new Mat33();\n // coefficient of friction of the shape.\n this.friction = 0.2; // 0.4\n // coefficient of restitution of the shape.\n this.restitution = 0.2;\n // density of the shape.\n this.density = 1;\n // bits of the collision groups to which the shape belongs.\n this.belongsTo = 1;\n // bits of the collision groups with which the shape collides.\n this.collidesWith = 0xffffffff;\n}\n\n/**\n* An information of limit and motor.\n*\n* @author saharan\n*/\n\nfunction LimitMotor(axis, fixed) {\n\n fixed = fixed || false;\n // The axis of the constraint.\n this.axis = axis;\n // The current angle for rotational constraints.\n this.angle = 0;\n // The lower limit. Set lower > upper to disable\n this.lowerLimit = fixed ? 0 : 1;\n\n // The upper limit. Set lower > upper to disable.\n this.upperLimit = 0;\n // The target motor speed.\n this.motorSpeed = 0;\n // The maximum motor force or torque. Set 0 to disable.\n this.maxMotorForce = 0;\n // The frequency of the spring. Set 0 to disable.\n this.frequency = 0;\n // The damping ratio of the spring. Set 0 for no damping, 1 for critical damping.\n this.dampingRatio = 0;\n}\n\nObject.assign(LimitMotor.prototype, {\n\n LimitMotor: true,\n\n // Set limit data into this constraint.\n setLimit: function (lowerLimit, upperLimit) {\n\n this.lowerLimit = lowerLimit;\n this.upperLimit = upperLimit;\n },\n\n // Set motor data into this constraint.\n setMotor: function (motorSpeed, maxMotorForce) {\n\n this.motorSpeed = motorSpeed;\n this.maxMotorForce = maxMotorForce;\n },\n\n // Set spring data into this constraint.\n setSpring: function (frequency, dampingRatio) {\n\n this.frequency = frequency;\n this.dampingRatio = dampingRatio;\n }\n\n});\n\n/**\r\n * The base class of all type of the constraints.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction Constraint() {\n\n // parent world of the constraint.\n this.parent = null;\n\n // first body of the constraint.\n this.body1 = null;\n\n // second body of the constraint.\n this.body2 = null;\n\n // Internal\n this.addedToIsland = false;\n}\n\nObject.assign(Constraint.prototype, {\n\n Constraint: true,\n\n // Prepare for solving the constraint\n preSolve: function (timeStep, invTimeStep) {\n\n printError(\"Constraint\", \"Inheritance error.\");\n },\n\n // Solve the constraint. This is usually called iteratively.\n solve: function () {\n\n printError(\"Constraint\", \"Inheritance error.\");\n },\n\n // Do the post-processing.\n postSolve: function () {\n\n printError(\"Constraint\", \"Inheritance error.\");\n }\n\n});\n\nfunction JointLink(joint) {\n\n // The previous joint link.\n this.prev = null;\n // The next joint link.\n this.next = null;\n // The other rigid body connected to the joint.\n this.body = null;\n // The joint of the link.\n this.joint = joint;\n}\n\n/**\n * Joints are used to constrain the motion between two rigid bodies.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction Joint(config) {\n\n Constraint.call(this);\n\n this.scale = 1;\n this.invScale = 1;\n\n // joint name\n this.name = \"\";\n this.id = NaN;\n\n // The type of the joint.\n this.type = JOINT_NULL;\n // The previous joint in the world.\n this.prev = null;\n // The next joint in the world.\n this.next = null;\n\n this.body1 = config.body1;\n this.body2 = config.body2;\n\n // anchor point on the first rigid body in local coordinate system.\n this.localAnchorPoint1 = new Vec3().copy(config.localAnchorPoint1);\n // anchor point on the second rigid body in local coordinate system.\n this.localAnchorPoint2 = new Vec3().copy(config.localAnchorPoint2);\n // anchor point on the first rigid body in world coordinate system relative to the body's origin.\n this.relativeAnchorPoint1 = new Vec3();\n // anchor point on the second rigid body in world coordinate system relative to the body's origin.\n this.relativeAnchorPoint2 = new Vec3();\n // anchor point on the first rigid body in world coordinate system.\n this.anchorPoint1 = new Vec3();\n // anchor point on the second rigid body in world coordinate system.\n this.anchorPoint2 = new Vec3();\n // Whether allow collision between connected rigid bodies or not.\n this.allowCollision = config.allowCollision;\n\n this.b1Link = new JointLink(this);\n this.b2Link = new JointLink(this);\n}\n\nJoint.prototype = Object.assign(Object.create(Constraint.prototype), {\n\n constructor: Joint,\n\n setId: function (n) {\n\n this.id = i;\n },\n\n setParent: function (world) {\n\n this.parent = world;\n this.scale = this.parent.scale;\n this.invScale = this.parent.invScale;\n this.id = this.parent.numJoints;\n if (!this.name) this.name = 'J' + this.id;\n },\n\n // Update all the anchor points.\n\n updateAnchorPoints: function () {\n\n this.relativeAnchorPoint1.copy(this.localAnchorPoint1).applyMatrix3(this.body1.rotation, true);\n this.relativeAnchorPoint2.copy(this.localAnchorPoint2).applyMatrix3(this.body2.rotation, true);\n\n this.anchorPoint1.add(this.relativeAnchorPoint1, this.body1.position);\n this.anchorPoint2.add(this.relativeAnchorPoint2, this.body2.position);\n },\n\n // Attach the joint from the bodies.\n\n attach: function (isX) {\n\n this.b1Link.body = this.body2;\n this.b2Link.body = this.body1;\n\n if (isX) {\n\n this.body1.jointLink.push(this.b1Link);\n this.body2.jointLink.push(this.b2Link);\n } else {\n\n if (this.body1.jointLink != null) (this.b1Link.next = this.body1.jointLink).prev = this.b1Link;else this.b1Link.next = null;\n this.body1.jointLink = this.b1Link;\n this.body1.numJoints++;\n if (this.body2.jointLink != null) (this.b2Link.next = this.body2.jointLink).prev = this.b2Link;else this.b2Link.next = null;\n this.body2.jointLink = this.b2Link;\n this.body2.numJoints++;\n }\n },\n\n // Detach the joint from the bodies.\n\n detach: function (isX) {\n\n if (isX) {\n\n this.body1.jointLink.splice(this.body1.jointLink.indexOf(this.b1Link), 1);\n this.body2.jointLink.splice(this.body2.jointLink.indexOf(this.b2Link), 1);\n } else {\n\n var prev = this.b1Link.prev;\n var next = this.b1Link.next;\n if (prev != null) prev.next = next;\n if (next != null) next.prev = prev;\n if (this.body1.jointLink == this.b1Link) this.body1.jointLink = next;\n this.b1Link.prev = null;\n this.b1Link.next = null;\n this.b1Link.body = null;\n this.body1.numJoints--;\n\n prev = this.b2Link.prev;\n next = this.b2Link.next;\n if (prev != null) prev.next = next;\n if (next != null) next.prev = prev;\n if (this.body2.jointLink == this.b2Link) this.body2.jointLink = next;\n this.b2Link.prev = null;\n this.b2Link.next = null;\n this.b2Link.body = null;\n this.body2.numJoints--;\n }\n\n this.b1Link.body = null;\n this.b2Link.body = null;\n },\n\n // Awake the bodies.\n\n awake: function () {\n\n this.body1.awake();\n this.body2.awake();\n },\n\n // calculation function\n\n preSolve: function (timeStep, invTimeStep) {},\n\n solve: function () {},\n\n postSolve: function () {},\n\n // Delete process\n\n remove: function () {\n\n this.dispose();\n },\n\n dispose: function () {\n\n this.parent.removeJoint(this);\n },\n\n // Three js add\n\n getPosition: function () {\n\n var p1 = new Vec3().scale(this.anchorPoint1, this.scale);\n var p2 = new Vec3().scale(this.anchorPoint2, this.scale);\n return [p1, p2];\n }\n\n});\n\n/**\n* A linear constraint for all axes for various joints.\n* @author saharan\n*/\nfunction LinearConstraint(joint) {\n\n this.m1 = NaN;\n this.m2 = NaN;\n\n this.ii1 = null;\n this.ii2 = null;\n this.dd = null;\n\n this.r1x = NaN;\n this.r1y = NaN;\n this.r1z = NaN;\n\n this.r2x = NaN;\n this.r2y = NaN;\n this.r2z = NaN;\n\n this.ax1x = NaN;\n this.ax1y = NaN;\n this.ax1z = NaN;\n this.ay1x = NaN;\n this.ay1y = NaN;\n this.ay1z = NaN;\n this.az1x = NaN;\n this.az1y = NaN;\n this.az1z = NaN;\n\n this.ax2x = NaN;\n this.ax2y = NaN;\n this.ax2z = NaN;\n this.ay2x = NaN;\n this.ay2y = NaN;\n this.ay2z = NaN;\n this.az2x = NaN;\n this.az2y = NaN;\n this.az2z = NaN;\n\n this.vel = NaN;\n this.velx = NaN;\n this.vely = NaN;\n this.velz = NaN;\n\n this.joint = joint;\n this.r1 = joint.relativeAnchorPoint1;\n this.r2 = joint.relativeAnchorPoint2;\n this.p1 = joint.anchorPoint1;\n this.p2 = joint.anchorPoint2;\n this.b1 = joint.body1;\n this.b2 = joint.body2;\n this.l1 = this.b1.linearVelocity;\n this.l2 = this.b2.linearVelocity;\n this.a1 = this.b1.angularVelocity;\n this.a2 = this.b2.angularVelocity;\n this.i1 = this.b1.inverseInertia;\n this.i2 = this.b2.inverseInertia;\n this.impx = 0;\n this.impy = 0;\n this.impz = 0;\n}\n\nObject.assign(LinearConstraint.prototype, {\n\n LinearConstraint: true,\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.r1x = this.r1.x;\n this.r1y = this.r1.y;\n this.r1z = this.r1.z;\n\n this.r2x = this.r2.x;\n this.r2y = this.r2.y;\n this.r2z = this.r2.z;\n\n this.m1 = this.b1.inverseMass;\n this.m2 = this.b2.inverseMass;\n\n this.ii1 = this.i1.clone();\n this.ii2 = this.i2.clone();\n\n var ii1 = this.ii1.elements;\n var ii2 = this.ii2.elements;\n\n this.ax1x = this.r1z * ii1[1] + -this.r1y * ii1[2];\n this.ax1y = this.r1z * ii1[4] + -this.r1y * ii1[5];\n this.ax1z = this.r1z * ii1[7] + -this.r1y * ii1[8];\n this.ay1x = -this.r1z * ii1[0] + this.r1x * ii1[2];\n this.ay1y = -this.r1z * ii1[3] + this.r1x * ii1[5];\n this.ay1z = -this.r1z * ii1[6] + this.r1x * ii1[8];\n this.az1x = this.r1y * ii1[0] + -this.r1x * ii1[1];\n this.az1y = this.r1y * ii1[3] + -this.r1x * ii1[4];\n this.az1z = this.r1y * ii1[6] + -this.r1x * ii1[7];\n this.ax2x = this.r2z * ii2[1] + -this.r2y * ii2[2];\n this.ax2y = this.r2z * ii2[4] + -this.r2y * ii2[5];\n this.ax2z = this.r2z * ii2[7] + -this.r2y * ii2[8];\n this.ay2x = -this.r2z * ii2[0] + this.r2x * ii2[2];\n this.ay2y = -this.r2z * ii2[3] + this.r2x * ii2[5];\n this.ay2z = -this.r2z * ii2[6] + this.r2x * ii2[8];\n this.az2x = this.r2y * ii2[0] + -this.r2x * ii2[1];\n this.az2y = this.r2y * ii2[3] + -this.r2x * ii2[4];\n this.az2z = this.r2y * ii2[6] + -this.r2x * ii2[7];\n\n // calculate point-to-point mass matrix\n // from impulse equation\n // \n // M = ([/m] - [r^][/I][r^]) ^ -1\n // \n // where\n // \n // [/m] = |1/m, 0, 0|\n // |0, 1/m, 0|\n // |0, 0, 1/m|\n // \n // [r^] = |0, -rz, ry|\n // |rz, 0, -rx|\n // |-ry, rx, 0|\n // \n // [/I] = Inverted moment inertia\n\n var rxx = this.m1 + this.m2;\n\n var kk = new Mat33().set(rxx, 0, 0, 0, rxx, 0, 0, 0, rxx);\n var k = kk.elements;\n\n k[0] += ii1[4] * this.r1z * this.r1z - (ii1[7] + ii1[5]) * this.r1y * this.r1z + ii1[8] * this.r1y * this.r1y;\n k[1] += (ii1[6] * this.r1y + ii1[5] * this.r1x) * this.r1z - ii1[3] * this.r1z * this.r1z - ii1[8] * this.r1x * this.r1y;\n k[2] += (ii1[3] * this.r1y - ii1[4] * this.r1x) * this.r1z - ii1[6] * this.r1y * this.r1y + ii1[7] * this.r1x * this.r1y;\n k[3] += (ii1[2] * this.r1y + ii1[7] * this.r1x) * this.r1z - ii1[1] * this.r1z * this.r1z - ii1[8] * this.r1x * this.r1y;\n k[4] += ii1[0] * this.r1z * this.r1z - (ii1[6] + ii1[2]) * this.r1x * this.r1z + ii1[8] * this.r1x * this.r1x;\n k[5] += (ii1[1] * this.r1x - ii1[0] * this.r1y) * this.r1z - ii1[7] * this.r1x * this.r1x + ii1[6] * this.r1x * this.r1y;\n k[6] += (ii1[1] * this.r1y - ii1[4] * this.r1x) * this.r1z - ii1[2] * this.r1y * this.r1y + ii1[5] * this.r1x * this.r1y;\n k[7] += (ii1[3] * this.r1x - ii1[0] * this.r1y) * this.r1z - ii1[5] * this.r1x * this.r1x + ii1[2] * this.r1x * this.r1y;\n k[8] += ii1[0] * this.r1y * this.r1y - (ii1[3] + ii1[1]) * this.r1x * this.r1y + ii1[4] * this.r1x * this.r1x;\n\n k[0] += ii2[4] * this.r2z * this.r2z - (ii2[7] + ii2[5]) * this.r2y * this.r2z + ii2[8] * this.r2y * this.r2y;\n k[1] += (ii2[6] * this.r2y + ii2[5] * this.r2x) * this.r2z - ii2[3] * this.r2z * this.r2z - ii2[8] * this.r2x * this.r2y;\n k[2] += (ii2[3] * this.r2y - ii2[4] * this.r2x) * this.r2z - ii2[6] * this.r2y * this.r2y + ii2[7] * this.r2x * this.r2y;\n k[3] += (ii2[2] * this.r2y + ii2[7] * this.r2x) * this.r2z - ii2[1] * this.r2z * this.r2z - ii2[8] * this.r2x * this.r2y;\n k[4] += ii2[0] * this.r2z * this.r2z - (ii2[6] + ii2[2]) * this.r2x * this.r2z + ii2[8] * this.r2x * this.r2x;\n k[5] += (ii2[1] * this.r2x - ii2[0] * this.r2y) * this.r2z - ii2[7] * this.r2x * this.r2x + ii2[6] * this.r2x * this.r2y;\n k[6] += (ii2[1] * this.r2y - ii2[4] * this.r2x) * this.r2z - ii2[2] * this.r2y * this.r2y + ii2[5] * this.r2x * this.r2y;\n k[7] += (ii2[3] * this.r2x - ii2[0] * this.r2y) * this.r2z - ii2[5] * this.r2x * this.r2x + ii2[2] * this.r2x * this.r2y;\n k[8] += ii2[0] * this.r2y * this.r2y - (ii2[3] + ii2[1]) * this.r2x * this.r2y + ii2[4] * this.r2x * this.r2x;\n\n 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]));\n 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);\n\n this.velx = this.p2.x - this.p1.x;\n this.vely = this.p2.y - this.p1.y;\n this.velz = this.p2.z - this.p1.z;\n var len = _Math.sqrt(this.velx * this.velx + this.vely * this.vely + this.velz * this.velz);\n if (len > 0.005) {\n len = (0.005 - len) / len * invTimeStep * 0.05;\n this.velx *= len;\n this.vely *= len;\n this.velz *= len;\n } else {\n this.velx = 0;\n this.vely = 0;\n this.velz = 0;\n }\n\n this.impx *= 0.95;\n this.impy *= 0.95;\n this.impz *= 0.95;\n\n this.l1.x += this.impx * this.m1;\n this.l1.y += this.impy * this.m1;\n this.l1.z += this.impz * this.m1;\n this.a1.x += this.impx * this.ax1x + this.impy * this.ay1x + this.impz * this.az1x;\n this.a1.y += this.impx * this.ax1y + this.impy * this.ay1y + this.impz * this.az1y;\n this.a1.z += this.impx * this.ax1z + this.impy * this.ay1z + this.impz * this.az1z;\n this.l2.x -= this.impx * this.m2;\n this.l2.y -= this.impy * this.m2;\n this.l2.z -= this.impz * this.m2;\n this.a2.x -= this.impx * this.ax2x + this.impy * this.ay2x + this.impz * this.az2x;\n this.a2.y -= this.impx * this.ax2y + this.impy * this.ay2y + this.impz * this.az2y;\n this.a2.z -= this.impx * this.ax2z + this.impy * this.ay2z + this.impz * this.az2z;\n },\n\n solve: function () {\n\n var d = this.dd.elements;\n 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;\n 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;\n 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;\n var nimpx = rvx * d[0] + rvy * d[1] + rvz * d[2];\n var nimpy = rvx * d[3] + rvy * d[4] + rvz * d[5];\n var nimpz = rvx * d[6] + rvy * d[7] + rvz * d[8];\n this.impx += nimpx;\n this.impy += nimpy;\n this.impz += nimpz;\n this.l1.x += nimpx * this.m1;\n this.l1.y += nimpy * this.m1;\n this.l1.z += nimpz * this.m1;\n this.a1.x += nimpx * this.ax1x + nimpy * this.ay1x + nimpz * this.az1x;\n this.a1.y += nimpx * this.ax1y + nimpy * this.ay1y + nimpz * this.az1y;\n this.a1.z += nimpx * this.ax1z + nimpy * this.ay1z + nimpz * this.az1z;\n this.l2.x -= nimpx * this.m2;\n this.l2.y -= nimpy * this.m2;\n this.l2.z -= nimpz * this.m2;\n this.a2.x -= nimpx * this.ax2x + nimpy * this.ay2x + nimpz * this.az2x;\n this.a2.y -= nimpx * this.ax2y + nimpy * this.ay2y + nimpz * this.az2y;\n this.a2.z -= nimpx * this.ax2z + nimpy * this.ay2z + nimpz * this.az2z;\n }\n\n});\n\n/**\n* A three-axis rotational constraint for various joints.\n* @author saharan\n*/\n\nfunction Rotational3Constraint(joint, limitMotor1, limitMotor2, limitMotor3) {\n\n this.cfm1 = NaN;\n this.cfm2 = NaN;\n this.cfm3 = NaN;\n this.i1e00 = NaN;\n this.i1e01 = NaN;\n this.i1e02 = NaN;\n this.i1e10 = NaN;\n this.i1e11 = NaN;\n this.i1e12 = NaN;\n this.i1e20 = NaN;\n this.i1e21 = NaN;\n this.i1e22 = NaN;\n this.i2e00 = NaN;\n this.i2e01 = NaN;\n this.i2e02 = NaN;\n this.i2e10 = NaN;\n this.i2e11 = NaN;\n this.i2e12 = NaN;\n this.i2e20 = NaN;\n this.i2e21 = NaN;\n this.i2e22 = NaN;\n this.ax1 = NaN;\n this.ay1 = NaN;\n this.az1 = NaN;\n this.ax2 = NaN;\n this.ay2 = NaN;\n this.az2 = NaN;\n this.ax3 = NaN;\n this.ay3 = NaN;\n this.az3 = NaN;\n\n this.a1x1 = NaN; // jacoians\n this.a1y1 = NaN;\n this.a1z1 = NaN;\n this.a2x1 = NaN;\n this.a2y1 = NaN;\n this.a2z1 = NaN;\n this.a1x2 = NaN;\n this.a1y2 = NaN;\n this.a1z2 = NaN;\n this.a2x2 = NaN;\n this.a2y2 = NaN;\n this.a2z2 = NaN;\n this.a1x3 = NaN;\n this.a1y3 = NaN;\n this.a1z3 = NaN;\n this.a2x3 = NaN;\n this.a2y3 = NaN;\n this.a2z3 = NaN;\n\n this.lowerLimit1 = NaN;\n this.upperLimit1 = NaN;\n this.limitVelocity1 = NaN;\n this.limitState1 = 0; // -1: at lower, 0: locked, 1: at upper, 2: free\n this.enableMotor1 = false;\n this.motorSpeed1 = NaN;\n this.maxMotorForce1 = NaN;\n this.maxMotorImpulse1 = NaN;\n this.lowerLimit2 = NaN;\n this.upperLimit2 = NaN;\n this.limitVelocity2 = NaN;\n this.limitState2 = 0; // -1: at lower, 0: locked, 1: at upper, 2: free\n this.enableMotor2 = false;\n this.motorSpeed2 = NaN;\n this.maxMotorForce2 = NaN;\n this.maxMotorImpulse2 = NaN;\n this.lowerLimit3 = NaN;\n this.upperLimit3 = NaN;\n this.limitVelocity3 = NaN;\n this.limitState3 = 0; // -1: at lower, 0: locked, 1: at upper, 2: free\n this.enableMotor3 = false;\n this.motorSpeed3 = NaN;\n this.maxMotorForce3 = NaN;\n this.maxMotorImpulse3 = NaN;\n\n this.k00 = NaN; // K = J*M*JT\n this.k01 = NaN;\n this.k02 = NaN;\n this.k10 = NaN;\n this.k11 = NaN;\n this.k12 = NaN;\n this.k20 = NaN;\n this.k21 = NaN;\n this.k22 = NaN;\n\n this.kv00 = NaN; // diagonals without CFMs\n this.kv11 = NaN;\n this.kv22 = NaN;\n\n this.dv00 = NaN; // ...inverted\n this.dv11 = NaN;\n this.dv22 = NaN;\n\n this.d00 = NaN; // K^-1\n this.d01 = NaN;\n this.d02 = NaN;\n this.d10 = NaN;\n this.d11 = NaN;\n this.d12 = NaN;\n this.d20 = NaN;\n this.d21 = NaN;\n this.d22 = NaN;\n\n this.limitMotor1 = limitMotor1;\n this.limitMotor2 = limitMotor2;\n this.limitMotor3 = limitMotor3;\n this.b1 = joint.body1;\n this.b2 = joint.body2;\n this.a1 = this.b1.angularVelocity;\n this.a2 = this.b2.angularVelocity;\n this.i1 = this.b1.inverseInertia;\n this.i2 = this.b2.inverseInertia;\n this.limitImpulse1 = 0;\n this.motorImpulse1 = 0;\n this.limitImpulse2 = 0;\n this.motorImpulse2 = 0;\n this.limitImpulse3 = 0;\n this.motorImpulse3 = 0;\n}\n\nObject.assign(Rotational3Constraint.prototype, {\n\n Rotational3Constraint: true,\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.ax1 = this.limitMotor1.axis.x;\n this.ay1 = this.limitMotor1.axis.y;\n this.az1 = this.limitMotor1.axis.z;\n this.ax2 = this.limitMotor2.axis.x;\n this.ay2 = this.limitMotor2.axis.y;\n this.az2 = this.limitMotor2.axis.z;\n this.ax3 = this.limitMotor3.axis.x;\n this.ay3 = this.limitMotor3.axis.y;\n this.az3 = this.limitMotor3.axis.z;\n this.lowerLimit1 = this.limitMotor1.lowerLimit;\n this.upperLimit1 = this.limitMotor1.upperLimit;\n this.motorSpeed1 = this.limitMotor1.motorSpeed;\n this.maxMotorForce1 = this.limitMotor1.maxMotorForce;\n this.enableMotor1 = this.maxMotorForce1 > 0;\n this.lowerLimit2 = this.limitMotor2.lowerLimit;\n this.upperLimit2 = this.limitMotor2.upperLimit;\n this.motorSpeed2 = this.limitMotor2.motorSpeed;\n this.maxMotorForce2 = this.limitMotor2.maxMotorForce;\n this.enableMotor2 = this.maxMotorForce2 > 0;\n this.lowerLimit3 = this.limitMotor3.lowerLimit;\n this.upperLimit3 = this.limitMotor3.upperLimit;\n this.motorSpeed3 = this.limitMotor3.motorSpeed;\n this.maxMotorForce3 = this.limitMotor3.maxMotorForce;\n this.enableMotor3 = this.maxMotorForce3 > 0;\n\n var ti1 = this.i1.elements;\n var ti2 = this.i2.elements;\n this.i1e00 = ti1[0];\n this.i1e01 = ti1[1];\n this.i1e02 = ti1[2];\n this.i1e10 = ti1[3];\n this.i1e11 = ti1[4];\n this.i1e12 = ti1[5];\n this.i1e20 = ti1[6];\n this.i1e21 = ti1[7];\n this.i1e22 = ti1[8];\n\n this.i2e00 = ti2[0];\n this.i2e01 = ti2[1];\n this.i2e02 = ti2[2];\n this.i2e10 = ti2[3];\n this.i2e11 = ti2[4];\n this.i2e12 = ti2[5];\n this.i2e20 = ti2[6];\n this.i2e21 = ti2[7];\n this.i2e22 = ti2[8];\n\n var frequency1 = this.limitMotor1.frequency;\n var frequency2 = this.limitMotor2.frequency;\n var frequency3 = this.limitMotor3.frequency;\n var enableSpring1 = frequency1 > 0;\n var enableSpring2 = frequency2 > 0;\n var enableSpring3 = frequency3 > 0;\n var enableLimit1 = this.lowerLimit1 <= this.upperLimit1;\n var enableLimit2 = this.lowerLimit2 <= this.upperLimit2;\n var enableLimit3 = this.lowerLimit3 <= this.upperLimit3;\n var angle1 = this.limitMotor1.angle;\n if (enableLimit1) {\n if (this.lowerLimit1 == this.upperLimit1) {\n if (this.limitState1 != 0) {\n this.limitState1 = 0;\n this.limitImpulse1 = 0;\n }\n this.limitVelocity1 = this.lowerLimit1 - angle1;\n } else if (angle1 < this.lowerLimit1) {\n if (this.limitState1 != -1) {\n this.limitState1 = -1;\n this.limitImpulse1 = 0;\n }\n this.limitVelocity1 = this.lowerLimit1 - angle1;\n } else if (angle1 > this.upperLimit1) {\n if (this.limitState1 != 1) {\n this.limitState1 = 1;\n this.limitImpulse1 = 0;\n }\n this.limitVelocity1 = this.upperLimit1 - angle1;\n } else {\n this.limitState1 = 2;\n this.limitImpulse1 = 0;\n this.limitVelocity1 = 0;\n }\n if (!enableSpring1) {\n if (this.limitVelocity1 > 0.02) this.limitVelocity1 -= 0.02;else if (this.limitVelocity1 < -0.02) this.limitVelocity1 += 0.02;else this.limitVelocity1 = 0;\n }\n } else {\n this.limitState1 = 2;\n this.limitImpulse1 = 0;\n }\n\n var angle2 = this.limitMotor2.angle;\n if (enableLimit2) {\n if (this.lowerLimit2 == this.upperLimit2) {\n if (this.limitState2 != 0) {\n this.limitState2 = 0;\n this.limitImpulse2 = 0;\n }\n this.limitVelocity2 = this.lowerLimit2 - angle2;\n } else if (angle2 < this.lowerLimit2) {\n if (this.limitState2 != -1) {\n this.limitState2 = -1;\n this.limitImpulse2 = 0;\n }\n this.limitVelocity2 = this.lowerLimit2 - angle2;\n } else if (angle2 > this.upperLimit2) {\n if (this.limitState2 != 1) {\n this.limitState2 = 1;\n this.limitImpulse2 = 0;\n }\n this.limitVelocity2 = this.upperLimit2 - angle2;\n } else {\n this.limitState2 = 2;\n this.limitImpulse2 = 0;\n this.limitVelocity2 = 0;\n }\n if (!enableSpring2) {\n if (this.limitVelocity2 > 0.02) this.limitVelocity2 -= 0.02;else if (this.limitVelocity2 < -0.02) this.limitVelocity2 += 0.02;else this.limitVelocity2 = 0;\n }\n } else {\n this.limitState2 = 2;\n this.limitImpulse2 = 0;\n }\n\n var angle3 = this.limitMotor3.angle;\n if (enableLimit3) {\n if (this.lowerLimit3 == this.upperLimit3) {\n if (this.limitState3 != 0) {\n this.limitState3 = 0;\n this.limitImpulse3 = 0;\n }\n this.limitVelocity3 = this.lowerLimit3 - angle3;\n } else if (angle3 < this.lowerLimit3) {\n if (this.limitState3 != -1) {\n this.limitState3 = -1;\n this.limitImpulse3 = 0;\n }\n this.limitVelocity3 = this.lowerLimit3 - angle3;\n } else if (angle3 > this.upperLimit3) {\n if (this.limitState3 != 1) {\n this.limitState3 = 1;\n this.limitImpulse3 = 0;\n }\n this.limitVelocity3 = this.upperLimit3 - angle3;\n } else {\n this.limitState3 = 2;\n this.limitImpulse3 = 0;\n this.limitVelocity3 = 0;\n }\n if (!enableSpring3) {\n if (this.limitVelocity3 > 0.02) this.limitVelocity3 -= 0.02;else if (this.limitVelocity3 < -0.02) this.limitVelocity3 += 0.02;else this.limitVelocity3 = 0;\n }\n } else {\n this.limitState3 = 2;\n this.limitImpulse3 = 0;\n }\n\n if (this.enableMotor1 && (this.limitState1 != 0 || enableSpring1)) {\n this.maxMotorImpulse1 = this.maxMotorForce1 * timeStep;\n } else {\n this.motorImpulse1 = 0;\n this.maxMotorImpulse1 = 0;\n }\n if (this.enableMotor2 && (this.limitState2 != 0 || enableSpring2)) {\n this.maxMotorImpulse2 = this.maxMotorForce2 * timeStep;\n } else {\n this.motorImpulse2 = 0;\n this.maxMotorImpulse2 = 0;\n }\n if (this.enableMotor3 && (this.limitState3 != 0 || enableSpring3)) {\n this.maxMotorImpulse3 = this.maxMotorForce3 * timeStep;\n } else {\n this.motorImpulse3 = 0;\n this.maxMotorImpulse3 = 0;\n }\n\n // build jacobians\n this.a1x1 = this.ax1 * this.i1e00 + this.ay1 * this.i1e01 + this.az1 * this.i1e02;\n this.a1y1 = this.ax1 * this.i1e10 + this.ay1 * this.i1e11 + this.az1 * this.i1e12;\n this.a1z1 = this.ax1 * this.i1e20 + this.ay1 * this.i1e21 + this.az1 * this.i1e22;\n this.a2x1 = this.ax1 * this.i2e00 + this.ay1 * this.i2e01 + this.az1 * this.i2e02;\n this.a2y1 = this.ax1 * this.i2e10 + this.ay1 * this.i2e11 + this.az1 * this.i2e12;\n this.a2z1 = this.ax1 * this.i2e20 + this.ay1 * this.i2e21 + this.az1 * this.i2e22;\n\n this.a1x2 = this.ax2 * this.i1e00 + this.ay2 * this.i1e01 + this.az2 * this.i1e02;\n this.a1y2 = this.ax2 * this.i1e10 + this.ay2 * this.i1e11 + this.az2 * this.i1e12;\n this.a1z2 = this.ax2 * this.i1e20 + this.ay2 * this.i1e21 + this.az2 * this.i1e22;\n this.a2x2 = this.ax2 * this.i2e00 + this.ay2 * this.i2e01 + this.az2 * this.i2e02;\n this.a2y2 = this.ax2 * this.i2e10 + this.ay2 * this.i2e11 + this.az2 * this.i2e12;\n this.a2z2 = this.ax2 * this.i2e20 + this.ay2 * this.i2e21 + this.az2 * this.i2e22;\n\n this.a1x3 = this.ax3 * this.i1e00 + this.ay3 * this.i1e01 + this.az3 * this.i1e02;\n this.a1y3 = this.ax3 * this.i1e10 + this.ay3 * this.i1e11 + this.az3 * this.i1e12;\n this.a1z3 = this.ax3 * this.i1e20 + this.ay3 * this.i1e21 + this.az3 * this.i1e22;\n this.a2x3 = this.ax3 * this.i2e00 + this.ay3 * this.i2e01 + this.az3 * this.i2e02;\n this.a2y3 = this.ax3 * this.i2e10 + this.ay3 * this.i2e11 + this.az3 * this.i2e12;\n this.a2z3 = this.ax3 * this.i2e20 + this.ay3 * this.i2e21 + this.az3 * this.i2e22;\n\n // build an impulse matrix\n this.k00 = this.ax1 * (this.a1x1 + this.a2x1) + this.ay1 * (this.a1y1 + this.a2y1) + this.az1 * (this.a1z1 + this.a2z1);\n this.k01 = this.ax1 * (this.a1x2 + this.a2x2) + this.ay1 * (this.a1y2 + this.a2y2) + this.az1 * (this.a1z2 + this.a2z2);\n this.k02 = this.ax1 * (this.a1x3 + this.a2x3) + this.ay1 * (this.a1y3 + this.a2y3) + this.az1 * (this.a1z3 + this.a2z3);\n this.k10 = this.ax2 * (this.a1x1 + this.a2x1) + this.ay2 * (this.a1y1 + this.a2y1) + this.az2 * (this.a1z1 + this.a2z1);\n this.k11 = this.ax2 * (this.a1x2 + this.a2x2) + this.ay2 * (this.a1y2 + this.a2y2) + this.az2 * (this.a1z2 + this.a2z2);\n this.k12 = this.ax2 * (this.a1x3 + this.a2x3) + this.ay2 * (this.a1y3 + this.a2y3) + this.az2 * (this.a1z3 + this.a2z3);\n this.k20 = this.ax3 * (this.a1x1 + this.a2x1) + this.ay3 * (this.a1y1 + this.a2y1) + this.az3 * (this.a1z1 + this.a2z1);\n this.k21 = this.ax3 * (this.a1x2 + this.a2x2) + this.ay3 * (this.a1y2 + this.a2y2) + this.az3 * (this.a1z2 + this.a2z2);\n this.k22 = this.ax3 * (this.a1x3 + this.a2x3) + this.ay3 * (this.a1y3 + this.a2y3) + this.az3 * (this.a1z3 + this.a2z3);\n\n this.kv00 = this.k00;\n this.kv11 = this.k11;\n this.kv22 = this.k22;\n this.dv00 = 1 / this.kv00;\n this.dv11 = 1 / this.kv11;\n this.dv22 = 1 / this.kv22;\n\n if (enableSpring1 && this.limitState1 != 2) {\n var omega = 6.2831853 * frequency1;\n var k = omega * omega * timeStep;\n var dmp = invTimeStep / (k + 2 * this.limitMotor1.dampingRatio * omega);\n this.cfm1 = this.kv00 * dmp;\n this.limitVelocity1 *= k * dmp;\n } else {\n this.cfm1 = 0;\n this.limitVelocity1 *= invTimeStep * 0.05;\n }\n\n if (enableSpring2 && this.limitState2 != 2) {\n omega = 6.2831853 * frequency2;\n k = omega * omega * timeStep;\n dmp = invTimeStep / (k + 2 * this.limitMotor2.dampingRatio * omega);\n this.cfm2 = this.kv11 * dmp;\n this.limitVelocity2 *= k * dmp;\n } else {\n this.cfm2 = 0;\n this.limitVelocity2 *= invTimeStep * 0.05;\n }\n\n if (enableSpring3 && this.limitState3 != 2) {\n omega = 6.2831853 * frequency3;\n k = omega * omega * timeStep;\n dmp = invTimeStep / (k + 2 * this.limitMotor3.dampingRatio * omega);\n this.cfm3 = this.kv22 * dmp;\n this.limitVelocity3 *= k * dmp;\n } else {\n this.cfm3 = 0;\n this.limitVelocity3 *= invTimeStep * 0.05;\n }\n\n this.k00 += this.cfm1;\n this.k11 += this.cfm2;\n this.k22 += this.cfm3;\n\n 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));\n this.d00 = (this.k11 * this.k22 - this.k12 * this.k21) * inv;\n this.d01 = (this.k02 * this.k21 - this.k01 * this.k22) * inv;\n this.d02 = (this.k01 * this.k12 - this.k02 * this.k11) * inv;\n this.d10 = (this.k12 * this.k20 - this.k10 * this.k22) * inv;\n this.d11 = (this.k00 * this.k22 - this.k02 * this.k20) * inv;\n this.d12 = (this.k02 * this.k10 - this.k00 * this.k12) * inv;\n this.d20 = (this.k10 * this.k21 - this.k11 * this.k20) * inv;\n this.d21 = (this.k01 * this.k20 - this.k00 * this.k21) * inv;\n this.d22 = (this.k00 * this.k11 - this.k01 * this.k10) * inv;\n\n this.limitImpulse1 *= 0.95;\n this.motorImpulse1 *= 0.95;\n this.limitImpulse2 *= 0.95;\n this.motorImpulse2 *= 0.95;\n this.limitImpulse3 *= 0.95;\n this.motorImpulse3 *= 0.95;\n var totalImpulse1 = this.limitImpulse1 + this.motorImpulse1;\n var totalImpulse2 = this.limitImpulse2 + this.motorImpulse2;\n var totalImpulse3 = this.limitImpulse3 + this.motorImpulse3;\n this.a1.x += totalImpulse1 * this.a1x1 + totalImpulse2 * this.a1x2 + totalImpulse3 * this.a1x3;\n this.a1.y += totalImpulse1 * this.a1y1 + totalImpulse2 * this.a1y2 + totalImpulse3 * this.a1y3;\n this.a1.z += totalImpulse1 * this.a1z1 + totalImpulse2 * this.a1z2 + totalImpulse3 * this.a1z3;\n this.a2.x -= totalImpulse1 * this.a2x1 + totalImpulse2 * this.a2x2 + totalImpulse3 * this.a2x3;\n this.a2.y -= totalImpulse1 * this.a2y1 + totalImpulse2 * this.a2y2 + totalImpulse3 * this.a2y3;\n this.a2.z -= totalImpulse1 * this.a2z1 + totalImpulse2 * this.a2z2 + totalImpulse3 * this.a2z3;\n },\n solve_: function () {\n\n var rvx = this.a2.x - this.a1.x;\n var rvy = this.a2.y - this.a1.y;\n var rvz = this.a2.z - this.a1.z;\n\n this.limitVelocity3 = 30;\n var rvn1 = rvx * this.ax1 + rvy * this.ay1 + rvz * this.az1 - this.limitVelocity1;\n var rvn2 = rvx * this.ax2 + rvy * this.ay2 + rvz * this.az2 - this.limitVelocity2;\n var rvn3 = rvx * this.ax3 + rvy * this.ay3 + rvz * this.az3 - this.limitVelocity3;\n\n var dLimitImpulse1 = rvn1 * this.d00 + rvn2 * this.d01 + rvn3 * this.d02;\n var dLimitImpulse2 = rvn1 * this.d10 + rvn2 * this.d11 + rvn3 * this.d12;\n var dLimitImpulse3 = rvn1 * this.d20 + rvn2 * this.d21 + rvn3 * this.d22;\n\n this.limitImpulse1 += dLimitImpulse1;\n this.limitImpulse2 += dLimitImpulse2;\n this.limitImpulse3 += dLimitImpulse3;\n\n this.a1.x += dLimitImpulse1 * this.a1x1 + dLimitImpulse2 * this.a1x2 + dLimitImpulse3 * this.a1x3;\n this.a1.y += dLimitImpulse1 * this.a1y1 + dLimitImpulse2 * this.a1y2 + dLimitImpulse3 * this.a1y3;\n this.a1.z += dLimitImpulse1 * this.a1z1 + dLimitImpulse2 * this.a1z2 + dLimitImpulse3 * this.a1z3;\n this.a2.x -= dLimitImpulse1 * this.a2x1 + dLimitImpulse2 * this.a2x2 + dLimitImpulse3 * this.a2x3;\n this.a2.y -= dLimitImpulse1 * this.a2y1 + dLimitImpulse2 * this.a2y2 + dLimitImpulse3 * this.a2y3;\n this.a2.z -= dLimitImpulse1 * this.a2z1 + dLimitImpulse2 * this.a2z2 + dLimitImpulse3 * this.a2z3;\n },\n solve: function () {\n\n var rvx = this.a2.x - this.a1.x;\n var rvy = this.a2.y - this.a1.y;\n var rvz = this.a2.z - this.a1.z;\n\n var rvn1 = rvx * this.ax1 + rvy * this.ay1 + rvz * this.az1;\n var rvn2 = rvx * this.ax2 + rvy * this.ay2 + rvz * this.az2;\n var rvn3 = rvx * this.ax3 + rvy * this.ay3 + rvz * this.az3;\n\n var oldMotorImpulse1 = this.motorImpulse1;\n var oldMotorImpulse2 = this.motorImpulse2;\n var oldMotorImpulse3 = this.motorImpulse3;\n\n var dMotorImpulse1 = 0;\n var dMotorImpulse2 = 0;\n var dMotorImpulse3 = 0;\n\n if (this.enableMotor1) {\n dMotorImpulse1 = (rvn1 - this.motorSpeed1) * this.dv00;\n this.motorImpulse1 += dMotorImpulse1;\n if (this.motorImpulse1 > this.maxMotorImpulse1) {\n // clamp motor impulse\n this.motorImpulse1 = this.maxMotorImpulse1;\n } else if (this.motorImpulse1 < -this.maxMotorImpulse1) {\n this.motorImpulse1 = -this.maxMotorImpulse1;\n }\n dMotorImpulse1 = this.motorImpulse1 - oldMotorImpulse1;\n }\n if (this.enableMotor2) {\n dMotorImpulse2 = (rvn2 - this.motorSpeed2) * this.dv11;\n this.motorImpulse2 += dMotorImpulse2;\n if (this.motorImpulse2 > this.maxMotorImpulse2) {\n // clamp motor impulse\n this.motorImpulse2 = this.maxMotorImpulse2;\n } else if (this.motorImpulse2 < -this.maxMotorImpulse2) {\n this.motorImpulse2 = -this.maxMotorImpulse2;\n }\n dMotorImpulse2 = this.motorImpulse2 - oldMotorImpulse2;\n }\n if (this.enableMotor3) {\n dMotorImpulse3 = (rvn3 - this.motorSpeed3) * this.dv22;\n this.motorImpulse3 += dMotorImpulse3;\n if (this.motorImpulse3 > this.maxMotorImpulse3) {\n // clamp motor impulse\n this.motorImpulse3 = this.maxMotorImpulse3;\n } else if (this.motorImpulse3 < -this.maxMotorImpulse3) {\n this.motorImpulse3 = -this.maxMotorImpulse3;\n }\n dMotorImpulse3 = this.motorImpulse3 - oldMotorImpulse3;\n }\n\n // apply motor impulse to relative velocity\n rvn1 += dMotorImpulse1 * this.kv00 + dMotorImpulse2 * this.k01 + dMotorImpulse3 * this.k02;\n rvn2 += dMotorImpulse1 * this.k10 + dMotorImpulse2 * this.kv11 + dMotorImpulse3 * this.k12;\n rvn3 += dMotorImpulse1 * this.k20 + dMotorImpulse2 * this.k21 + dMotorImpulse3 * this.kv22;\n\n // subtract target velocity and applied impulse\n rvn1 -= this.limitVelocity1 + this.limitImpulse1 * this.cfm1;\n rvn2 -= this.limitVelocity2 + this.limitImpulse2 * this.cfm2;\n rvn3 -= this.limitVelocity3 + this.limitImpulse3 * this.cfm3;\n\n var oldLimitImpulse1 = this.limitImpulse1;\n var oldLimitImpulse2 = this.limitImpulse2;\n var oldLimitImpulse3 = this.limitImpulse3;\n\n var dLimitImpulse1 = rvn1 * this.d00 + rvn2 * this.d01 + rvn3 * this.d02;\n var dLimitImpulse2 = rvn1 * this.d10 + rvn2 * this.d11 + rvn3 * this.d12;\n var dLimitImpulse3 = rvn1 * this.d20 + rvn2 * this.d21 + rvn3 * this.d22;\n\n this.limitImpulse1 += dLimitImpulse1;\n this.limitImpulse2 += dLimitImpulse2;\n this.limitImpulse3 += dLimitImpulse3;\n\n // clamp\n var clampState = 0;\n if (this.limitState1 == 2 || this.limitImpulse1 * this.limitState1 < 0) {\n dLimitImpulse1 = -oldLimitImpulse1;\n rvn2 += dLimitImpulse1 * this.k10;\n rvn3 += dLimitImpulse1 * this.k20;\n clampState |= 1;\n }\n if (this.limitState2 == 2 || this.limitImpulse2 * this.limitState2 < 0) {\n dLimitImpulse2 = -oldLimitImpulse2;\n rvn1 += dLimitImpulse2 * this.k01;\n rvn3 += dLimitImpulse2 * this.k21;\n clampState |= 2;\n }\n if (this.limitState3 == 2 || this.limitImpulse3 * this.limitState3 < 0) {\n dLimitImpulse3 = -oldLimitImpulse3;\n rvn1 += dLimitImpulse3 * this.k02;\n rvn2 += dLimitImpulse3 * this.k12;\n clampState |= 4;\n }\n\n // update un-clamped impulse\n // TODO: isolate division\n var det;\n switch (clampState) {\n case 1:\n // update 2 3\n det = 1 / (this.k11 * this.k22 - this.k12 * this.k21);\n dLimitImpulse2 = (this.k22 * rvn2 + -this.k12 * rvn3) * det;\n dLimitImpulse3 = (-this.k21 * rvn2 + this.k11 * rvn3) * det;\n break;\n case 2:\n // update 1 3\n det = 1 / (this.k00 * this.k22 - this.k02 * this.k20);\n dLimitImpulse1 = (this.k22 * rvn1 + -this.k02 * rvn3) * det;\n dLimitImpulse3 = (-this.k20 * rvn1 + this.k00 * rvn3) * det;\n break;\n case 3:\n // update 3\n dLimitImpulse3 = rvn3 / this.k22;\n break;\n case 4:\n // update 1 2\n det = 1 / (this.k00 * this.k11 - this.k01 * this.k10);\n dLimitImpulse1 = (this.k11 * rvn1 + -this.k01 * rvn2) * det;\n dLimitImpulse2 = (-this.k10 * rvn1 + this.k00 * rvn2) * det;\n break;\n case 5:\n // update 2\n dLimitImpulse2 = rvn2 / this.k11;\n break;\n case 6:\n // update 1\n dLimitImpulse1 = rvn1 / this.k00;\n break;\n }\n\n this.limitImpulse1 = dLimitImpulse1 + oldLimitImpulse1;\n this.limitImpulse2 = dLimitImpulse2 + oldLimitImpulse2;\n this.limitImpulse3 = dLimitImpulse3 + oldLimitImpulse3;\n\n var dImpulse1 = dMotorImpulse1 + dLimitImpulse1;\n var dImpulse2 = dMotorImpulse2 + dLimitImpulse2;\n var dImpulse3 = dMotorImpulse3 + dLimitImpulse3;\n\n // apply impulse\n this.a1.x += dImpulse1 * this.a1x1 + dImpulse2 * this.a1x2 + dImpulse3 * this.a1x3;\n this.a1.y += dImpulse1 * this.a1y1 + dImpulse2 * this.a1y2 + dImpulse3 * this.a1y3;\n this.a1.z += dImpulse1 * this.a1z1 + dImpulse2 * this.a1z2 + dImpulse3 * this.a1z3;\n this.a2.x -= dImpulse1 * this.a2x1 + dImpulse2 * this.a2x2 + dImpulse3 * this.a2x3;\n this.a2.y -= dImpulse1 * this.a2y1 + dImpulse2 * this.a2y2 + dImpulse3 * this.a2y3;\n this.a2.z -= dImpulse1 * this.a2z1 + dImpulse2 * this.a2z2 + dImpulse3 * this.a2z3;\n rvx = this.a2.x - this.a1.x;\n rvy = this.a2.y - this.a1.y;\n rvz = this.a2.z - this.a1.z;\n\n rvn2 = rvx * this.ax2 + rvy * this.ay2 + rvz * this.az2;\n }\n\n});\n\n/**\n * A hinge joint allows only for relative rotation of rigid bodies along the axis.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction HingeJoint(config, lowerAngleLimit, upperAngleLimit) {\n\n Joint.call(this, config);\n\n this.type = JOINT_HINGE;\n\n // The axis in the first body's coordinate system.\n this.localAxis1 = config.localAxis1.clone().normalize();\n // The axis in the second body's coordinate system.\n this.localAxis2 = config.localAxis2.clone().normalize();\n\n // make angle axis\n var arc = new Mat33().setQuat(new Quat().setFromUnitVectors(this.localAxis1, this.localAxis2));\n this.localAngle1 = new Vec3().tangent(this.localAxis1).normalize();\n this.localAngle2 = this.localAngle1.clone().applyMatrix3(arc, true);\n\n this.ax1 = new Vec3();\n this.ax2 = new Vec3();\n this.an1 = new Vec3();\n this.an2 = new Vec3();\n\n this.tmp = new Vec3();\n\n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n // The rotational limit and motor information of the joint.\n this.limitMotor = new LimitMotor(this.nor, false);\n this.limitMotor.lowerLimit = lowerAngleLimit;\n this.limitMotor.upperLimit = upperAngleLimit;\n\n this.lc = new LinearConstraint(this);\n this.r3 = new Rotational3Constraint(this, this.limitMotor, new LimitMotor(this.tan, true), new LimitMotor(this.bin, true));\n}\n\nHingeJoint.prototype = Object.assign(Object.create(Joint.prototype), {\n\n constructor: HingeJoint,\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.updateAnchorPoints();\n\n this.ax1.copy(this.localAxis1).applyMatrix3(this.body1.rotation, true);\n this.ax2.copy(this.localAxis2).applyMatrix3(this.body2.rotation, true);\n\n this.an1.copy(this.localAngle1).applyMatrix3(this.body1.rotation, true);\n this.an2.copy(this.localAngle2).applyMatrix3(this.body2.rotation, true);\n\n // normal tangent binormal\n\n 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();\n\n this.tan.tangent(this.nor).normalize();\n\n this.bin.crossVectors(this.nor, this.tan);\n\n // calculate hinge angle\n\n var limite = _Math.acosClamp(_Math.dotVectors(this.an1, this.an2));\n\n this.tmp.crossVectors(this.an1, this.an2);\n\n if (_Math.dotVectors(this.nor, this.tmp) < 0) this.limitMotor.angle = -limite;else this.limitMotor.angle = limite;\n\n this.tmp.crossVectors(this.ax1, this.ax2);\n\n this.r3.limitMotor2.angle = _Math.dotVectors(this.tan, this.tmp);\n this.r3.limitMotor3.angle = _Math.dotVectors(this.bin, this.tmp);\n\n // preSolve\n\n this.r3.preSolve(timeStep, invTimeStep);\n this.lc.preSolve(timeStep, invTimeStep);\n },\n\n solve: function () {\n\n this.r3.solve();\n this.lc.solve();\n },\n\n postSolve: function () {}\n\n});\n\n/**\n * A ball-and-socket joint limits relative translation on two anchor points on rigid bodies.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction BallAndSocketJoint(config) {\n\n Joint.call(this, config);\n\n this.type = JOINT_BALL_AND_SOCKET;\n\n this.lc = new LinearConstraint(this);\n}\n\nBallAndSocketJoint.prototype = Object.assign(Object.create(Joint.prototype), {\n\n constructor: BallAndSocketJoint,\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.updateAnchorPoints();\n\n // preSolve\n\n this.lc.preSolve(timeStep, invTimeStep);\n },\n\n solve: function () {\n\n this.lc.solve();\n },\n\n postSolve: function () {}\n\n});\n\n/**\n* A translational constraint for various joints.\n* @author saharan\n*/\nfunction TranslationalConstraint(joint, limitMotor) {\n this.cfm = NaN;\n this.m1 = NaN;\n this.m2 = NaN;\n this.i1e00 = NaN;\n this.i1e01 = NaN;\n this.i1e02 = NaN;\n this.i1e10 = NaN;\n this.i1e11 = NaN;\n this.i1e12 = NaN;\n this.i1e20 = NaN;\n this.i1e21 = NaN;\n this.i1e22 = NaN;\n this.i2e00 = NaN;\n this.i2e01 = NaN;\n this.i2e02 = NaN;\n this.i2e10 = NaN;\n this.i2e11 = NaN;\n this.i2e12 = NaN;\n this.i2e20 = NaN;\n this.i2e21 = NaN;\n this.i2e22 = NaN;\n this.motorDenom = NaN;\n this.invMotorDenom = NaN;\n this.invDenom = NaN;\n this.ax = NaN;\n this.ay = NaN;\n this.az = NaN;\n this.r1x = NaN;\n this.r1y = NaN;\n this.r1z = NaN;\n this.r2x = NaN;\n this.r2y = NaN;\n this.r2z = NaN;\n this.t1x = NaN;\n this.t1y = NaN;\n this.t1z = NaN;\n this.t2x = NaN;\n this.t2y = NaN;\n this.t2z = NaN;\n this.l1x = NaN;\n this.l1y = NaN;\n this.l1z = NaN;\n this.l2x = NaN;\n this.l2y = NaN;\n this.l2z = NaN;\n this.a1x = NaN;\n this.a1y = NaN;\n this.a1z = NaN;\n this.a2x = NaN;\n this.a2y = NaN;\n this.a2z = NaN;\n this.lowerLimit = NaN;\n this.upperLimit = NaN;\n this.limitVelocity = NaN;\n this.limitState = 0; // -1: at lower, 0: locked, 1: at upper, 2: free\n this.enableMotor = false;\n this.motorSpeed = NaN;\n this.maxMotorForce = NaN;\n this.maxMotorImpulse = NaN;\n\n this.limitMotor = limitMotor;\n this.b1 = joint.body1;\n this.b2 = joint.body2;\n this.p1 = joint.anchorPoint1;\n this.p2 = joint.anchorPoint2;\n this.r1 = joint.relativeAnchorPoint1;\n this.r2 = joint.relativeAnchorPoint2;\n this.l1 = this.b1.linearVelocity;\n this.l2 = this.b2.linearVelocity;\n this.a1 = this.b1.angularVelocity;\n this.a2 = this.b2.angularVelocity;\n this.i1 = this.b1.inverseInertia;\n this.i2 = this.b2.inverseInertia;\n this.limitImpulse = 0;\n this.motorImpulse = 0;\n}\n\nObject.assign(TranslationalConstraint.prototype, {\n\n TranslationalConstraint: true,\n\n preSolve: function (timeStep, invTimeStep) {\n this.ax = this.limitMotor.axis.x;\n this.ay = this.limitMotor.axis.y;\n this.az = this.limitMotor.axis.z;\n this.lowerLimit = this.limitMotor.lowerLimit;\n this.upperLimit = this.limitMotor.upperLimit;\n this.motorSpeed = this.limitMotor.motorSpeed;\n this.maxMotorForce = this.limitMotor.maxMotorForce;\n this.enableMotor = this.maxMotorForce > 0;\n this.m1 = this.b1.inverseMass;\n this.m2 = this.b2.inverseMass;\n\n var ti1 = this.i1.elements;\n var ti2 = this.i2.elements;\n this.i1e00 = ti1[0];\n this.i1e01 = ti1[1];\n this.i1e02 = ti1[2];\n this.i1e10 = ti1[3];\n this.i1e11 = ti1[4];\n this.i1e12 = ti1[5];\n this.i1e20 = ti1[6];\n this.i1e21 = ti1[7];\n this.i1e22 = ti1[8];\n\n this.i2e00 = ti2[0];\n this.i2e01 = ti2[1];\n this.i2e02 = ti2[2];\n this.i2e10 = ti2[3];\n this.i2e11 = ti2[4];\n this.i2e12 = ti2[5];\n this.i2e20 = ti2[6];\n this.i2e21 = ti2[7];\n this.i2e22 = ti2[8];\n\n var dx = this.p2.x - this.p1.x;\n var dy = this.p2.y - this.p1.y;\n var dz = this.p2.z - this.p1.z;\n var d = dx * this.ax + dy * this.ay + dz * this.az;\n var frequency = this.limitMotor.frequency;\n var enableSpring = frequency > 0;\n var enableLimit = this.lowerLimit <= this.upperLimit;\n if (enableSpring && d > 20 || d < -20) {\n enableSpring = false;\n }\n\n if (enableLimit) {\n if (this.lowerLimit == this.upperLimit) {\n if (this.limitState != 0) {\n this.limitState = 0;\n this.limitImpulse = 0;\n }\n this.limitVelocity = this.lowerLimit - d;\n if (!enableSpring) d = this.lowerLimit;\n } else if (d < this.lowerLimit) {\n if (this.limitState != -1) {\n this.limitState = -1;\n this.limitImpulse = 0;\n }\n this.limitVelocity = this.lowerLimit - d;\n if (!enableSpring) d = this.lowerLimit;\n } else if (d > this.upperLimit) {\n if (this.limitState != 1) {\n this.limitState = 1;\n this.limitImpulse = 0;\n }\n this.limitVelocity = this.upperLimit - d;\n if (!enableSpring) d = this.upperLimit;\n } else {\n this.limitState = 2;\n this.limitImpulse = 0;\n this.limitVelocity = 0;\n }\n if (!enableSpring) {\n if (this.limitVelocity > 0.005) this.limitVelocity -= 0.005;else if (this.limitVelocity < -0.005) this.limitVelocity += 0.005;else this.limitVelocity = 0;\n }\n } else {\n this.limitState = 2;\n this.limitImpulse = 0;\n }\n\n if (this.enableMotor && (this.limitState != 0 || enableSpring)) {\n this.maxMotorImpulse = this.maxMotorForce * timeStep;\n } else {\n this.motorImpulse = 0;\n this.maxMotorImpulse = 0;\n }\n\n var rdx = d * this.ax;\n var rdy = d * this.ay;\n var rdz = d * this.az;\n var w1 = this.m1 / (this.m1 + this.m2);\n var w2 = 1 - w1;\n this.r1x = this.r1.x + rdx * w1;\n this.r1y = this.r1.y + rdy * w1;\n this.r1z = this.r1.z + rdz * w1;\n this.r2x = this.r2.x - rdx * w2;\n this.r2y = this.r2.y - rdy * w2;\n this.r2z = this.r2.z - rdz * w2;\n\n this.t1x = this.r1y * this.az - this.r1z * this.ay;\n this.t1y = this.r1z * this.ax - this.r1x * this.az;\n this.t1z = this.r1x * this.ay - this.r1y * this.ax;\n this.t2x = this.r2y * this.az - this.r2z * this.ay;\n this.t2y = this.r2z * this.ax - this.r2x * this.az;\n this.t2z = this.r2x * this.ay - this.r2y * this.ax;\n this.l1x = this.ax * this.m1;\n this.l1y = this.ay * this.m1;\n this.l1z = this.az * this.m1;\n this.l2x = this.ax * this.m2;\n this.l2y = this.ay * this.m2;\n this.l2z = this.az * this.m2;\n this.a1x = this.t1x * this.i1e00 + this.t1y * this.i1e01 + this.t1z * this.i1e02;\n this.a1y = this.t1x * this.i1e10 + this.t1y * this.i1e11 + this.t1z * this.i1e12;\n this.a1z = this.t1x * this.i1e20 + this.t1y * this.i1e21 + this.t1z * this.i1e22;\n this.a2x = this.t2x * this.i2e00 + this.t2y * this.i2e01 + this.t2z * this.i2e02;\n this.a2y = this.t2x * this.i2e10 + this.t2y * this.i2e11 + this.t2z * this.i2e12;\n this.a2z = this.t2x * this.i2e20 + this.t2y * this.i2e21 + this.t2z * this.i2e22;\n 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);\n\n this.invMotorDenom = 1 / this.motorDenom;\n\n if (enableSpring && this.limitState != 2) {\n var omega = 6.2831853 * frequency;\n var k = omega * omega * timeStep;\n var dmp = invTimeStep / (k + 2 * this.limitMotor.dampingRatio * omega);\n this.cfm = this.motorDenom * dmp;\n this.limitVelocity *= k * dmp;\n } else {\n this.cfm = 0;\n this.limitVelocity *= invTimeStep * 0.05;\n }\n\n this.invDenom = 1 / (this.motorDenom + this.cfm);\n\n var totalImpulse = this.limitImpulse + this.motorImpulse;\n this.l1.x += totalImpulse * this.l1x;\n this.l1.y += totalImpulse * this.l1y;\n this.l1.z += totalImpulse * this.l1z;\n this.a1.x += totalImpulse * this.a1x;\n this.a1.y += totalImpulse * this.a1y;\n this.a1.z += totalImpulse * this.a1z;\n this.l2.x -= totalImpulse * this.l2x;\n this.l2.y -= totalImpulse * this.l2y;\n this.l2.z -= totalImpulse * this.l2z;\n this.a2.x -= totalImpulse * this.a2x;\n this.a2.y -= totalImpulse * this.a2y;\n this.a2.z -= totalImpulse * this.a2z;\n },\n solve: function () {\n 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;\n\n // motor part\n var newMotorImpulse;\n if (this.enableMotor) {\n newMotorImpulse = (rvn - this.motorSpeed) * this.invMotorDenom;\n var oldMotorImpulse = this.motorImpulse;\n this.motorImpulse += newMotorImpulse;\n if (this.motorImpulse > this.maxMotorImpulse) this.motorImpulse = this.maxMotorImpulse;else if (this.motorImpulse < -this.maxMotorImpulse) this.motorImpulse = -this.maxMotorImpulse;\n newMotorImpulse = this.motorImpulse - oldMotorImpulse;\n rvn -= newMotorImpulse * this.motorDenom;\n } else newMotorImpulse = 0;\n\n // limit part\n var newLimitImpulse;\n if (this.limitState != 2) {\n newLimitImpulse = (rvn - this.limitVelocity - this.limitImpulse * this.cfm) * this.invDenom;\n var oldLimitImpulse = this.limitImpulse;\n this.limitImpulse += newLimitImpulse;\n if (this.limitImpulse * this.limitState < 0) this.limitImpulse = 0;\n newLimitImpulse = this.limitImpulse - oldLimitImpulse;\n } else newLimitImpulse = 0;\n\n var totalImpulse = newLimitImpulse + newMotorImpulse;\n this.l1.x += totalImpulse * this.l1x;\n this.l1.y += totalImpulse * this.l1y;\n this.l1.z += totalImpulse * this.l1z;\n this.a1.x += totalImpulse * this.a1x;\n this.a1.y += totalImpulse * this.a1y;\n this.a1.z += totalImpulse * this.a1z;\n this.l2.x -= totalImpulse * this.l2x;\n this.l2.y -= totalImpulse * this.l2y;\n this.l2.z -= totalImpulse * this.l2z;\n this.a2.x -= totalImpulse * this.a2x;\n this.a2.y -= totalImpulse * this.a2y;\n this.a2.z -= totalImpulse * this.a2z;\n }\n});\n\n/**\n * A distance joint limits the distance between two anchor points on rigid bodies.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction DistanceJoint(config, minDistance, maxDistance) {\n\n Joint.call(this, config);\n\n this.type = JOINT_DISTANCE;\n\n this.nor = new Vec3();\n\n // The limit and motor information of the joint.\n this.limitMotor = new LimitMotor(this.nor, true);\n this.limitMotor.lowerLimit = minDistance;\n this.limitMotor.upperLimit = maxDistance;\n\n this.t = new TranslationalConstraint(this, this.limitMotor);\n}\n\nDistanceJoint.prototype = Object.assign(Object.create(Joint.prototype), {\n\n constructor: DistanceJoint,\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.updateAnchorPoints();\n\n this.nor.sub(this.anchorPoint2, this.anchorPoint1).normalize();\n\n // preSolve\n\n this.t.preSolve(timeStep, invTimeStep);\n },\n\n solve: function () {\n\n this.t.solve();\n },\n\n postSolve: function () {}\n\n});\n\n/**\n* An angular constraint for all axes for various joints.\n* @author saharan\n*/\n\nfunction AngularConstraint(joint, targetOrientation) {\n\n this.joint = joint;\n\n this.targetOrientation = new Quat().invert(targetOrientation);\n\n this.relativeOrientation = new Quat();\n\n this.ii1 = null;\n this.ii2 = null;\n this.dd = null;\n\n this.vel = new Vec3();\n this.imp = new Vec3();\n\n this.rn0 = new Vec3();\n this.rn1 = new Vec3();\n this.rn2 = new Vec3();\n\n this.b1 = joint.body1;\n this.b2 = joint.body2;\n this.a1 = this.b1.angularVelocity;\n this.a2 = this.b2.angularVelocity;\n this.i1 = this.b1.inverseInertia;\n this.i2 = this.b2.inverseInertia;\n}\n\nObject.assign(AngularConstraint.prototype, {\n\n AngularConstraint: true,\n\n preSolve: function (timeStep, invTimeStep) {\n\n var inv, len, v;\n\n this.ii1 = this.i1.clone();\n this.ii2 = this.i2.clone();\n\n v = new Mat33().add(this.ii1, this.ii2).elements;\n 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]));\n 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);\n\n this.relativeOrientation.invert(this.b1.orientation).multiply(this.targetOrientation).multiply(this.b2.orientation);\n\n inv = this.relativeOrientation.w * 2;\n\n this.vel.copy(this.relativeOrientation).multiplyScalar(inv);\n\n len = this.vel.length();\n\n if (len > 0.02) {\n len = (0.02 - len) / len * invTimeStep * 0.05;\n this.vel.multiplyScalar(len);\n } else {\n this.vel.set(0, 0, 0);\n }\n\n this.rn1.copy(this.imp).applyMatrix3(this.ii1, true);\n this.rn2.copy(this.imp).applyMatrix3(this.ii2, true);\n\n this.a1.add(this.rn1);\n this.a2.sub(this.rn2);\n },\n\n solve: function () {\n\n var r = this.a2.clone().sub(this.a1).sub(this.vel);\n\n this.rn0.copy(r).applyMatrix3(this.dd, true);\n this.rn1.copy(this.rn0).applyMatrix3(this.ii1, true);\n this.rn2.copy(this.rn0).applyMatrix3(this.ii2, true);\n\n this.imp.add(this.rn0);\n this.a1.add(this.rn1);\n this.a2.sub(this.rn2);\n }\n\n});\n\n/**\n* A three-axis translational constraint for various joints.\n* @author saharan\n*/\nfunction Translational3Constraint(joint, limitMotor1, limitMotor2, limitMotor3) {\n\n this.m1 = NaN;\n this.m2 = NaN;\n this.i1e00 = NaN;\n this.i1e01 = NaN;\n this.i1e02 = NaN;\n this.i1e10 = NaN;\n this.i1e11 = NaN;\n this.i1e12 = NaN;\n this.i1e20 = NaN;\n this.i1e21 = NaN;\n this.i1e22 = NaN;\n this.i2e00 = NaN;\n this.i2e01 = NaN;\n this.i2e02 = NaN;\n this.i2e10 = NaN;\n this.i2e11 = NaN;\n this.i2e12 = NaN;\n this.i2e20 = NaN;\n this.i2e21 = NaN;\n this.i2e22 = NaN;\n this.ax1 = NaN;\n this.ay1 = NaN;\n this.az1 = NaN;\n this.ax2 = NaN;\n this.ay2 = NaN;\n this.az2 = NaN;\n this.ax3 = NaN;\n this.ay3 = NaN;\n this.az3 = NaN;\n this.r1x = NaN;\n this.r1y = NaN;\n this.r1z = NaN;\n this.r2x = NaN;\n this.r2y = NaN;\n this.r2z = NaN;\n this.t1x1 = NaN; // jacobians\n this.t1y1 = NaN;\n this.t1z1 = NaN;\n this.t2x1 = NaN;\n this.t2y1 = NaN;\n this.t2z1 = NaN;\n this.l1x1 = NaN;\n this.l1y1 = NaN;\n this.l1z1 = NaN;\n this.l2x1 = NaN;\n this.l2y1 = NaN;\n this.l2z1 = NaN;\n this.a1x1 = NaN;\n this.a1y1 = NaN;\n this.a1z1 = NaN;\n this.a2x1 = NaN;\n this.a2y1 = NaN;\n this.a2z1 = NaN;\n this.t1x2 = NaN;\n this.t1y2 = NaN;\n this.t1z2 = NaN;\n this.t2x2 = NaN;\n this.t2y2 = NaN;\n this.t2z2 = NaN;\n this.l1x2 = NaN;\n this.l1y2 = NaN;\n this.l1z2 = NaN;\n this.l2x2 = NaN;\n this.l2y2 = NaN;\n this.l2z2 = NaN;\n this.a1x2 = NaN;\n this.a1y2 = NaN;\n this.a1z2 = NaN;\n this.a2x2 = NaN;\n this.a2y2 = NaN;\n this.a2z2 = NaN;\n this.t1x3 = NaN;\n this.t1y3 = NaN;\n this.t1z3 = NaN;\n this.t2x3 = NaN;\n this.t2y3 = NaN;\n this.t2z3 = NaN;\n this.l1x3 = NaN;\n this.l1y3 = NaN;\n this.l1z3 = NaN;\n this.l2x3 = NaN;\n this.l2y3 = NaN;\n this.l2z3 = NaN;\n this.a1x3 = NaN;\n this.a1y3 = NaN;\n this.a1z3 = NaN;\n this.a2x3 = NaN;\n this.a2y3 = NaN;\n this.a2z3 = NaN;\n this.lowerLimit1 = NaN;\n this.upperLimit1 = NaN;\n this.limitVelocity1 = NaN;\n this.limitState1 = 0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited\n this.enableMotor1 = false;\n this.motorSpeed1 = NaN;\n this.maxMotorForce1 = NaN;\n this.maxMotorImpulse1 = NaN;\n this.lowerLimit2 = NaN;\n this.upperLimit2 = NaN;\n this.limitVelocity2 = NaN;\n this.limitState2 = 0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited\n this.enableMotor2 = false;\n this.motorSpeed2 = NaN;\n this.maxMotorForce2 = NaN;\n this.maxMotorImpulse2 = NaN;\n this.lowerLimit3 = NaN;\n this.upperLimit3 = NaN;\n this.limitVelocity3 = NaN;\n this.limitState3 = 0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited\n this.enableMotor3 = false;\n this.motorSpeed3 = NaN;\n this.maxMotorForce3 = NaN;\n this.maxMotorImpulse3 = NaN;\n this.k00 = NaN; // K = J*M*JT\n this.k01 = NaN;\n this.k02 = NaN;\n this.k10 = NaN;\n this.k11 = NaN;\n this.k12 = NaN;\n this.k20 = NaN;\n this.k21 = NaN;\n this.k22 = NaN;\n this.kv00 = NaN; // diagonals without CFMs\n this.kv11 = NaN;\n this.kv22 = NaN;\n this.dv00 = NaN; // ...inverted\n this.dv11 = NaN;\n this.dv22 = NaN;\n this.d00 = NaN; // K^-1\n this.d01 = NaN;\n this.d02 = NaN;\n this.d10 = NaN;\n this.d11 = NaN;\n this.d12 = NaN;\n this.d20 = NaN;\n this.d21 = NaN;\n this.d22 = NaN;\n\n this.limitMotor1 = limitMotor1;\n this.limitMotor2 = limitMotor2;\n this.limitMotor3 = limitMotor3;\n this.b1 = joint.body1;\n this.b2 = joint.body2;\n this.p1 = joint.anchorPoint1;\n this.p2 = joint.anchorPoint2;\n this.r1 = joint.relativeAnchorPoint1;\n this.r2 = joint.relativeAnchorPoint2;\n this.l1 = this.b1.linearVelocity;\n this.l2 = this.b2.linearVelocity;\n this.a1 = this.b1.angularVelocity;\n this.a2 = this.b2.angularVelocity;\n this.i1 = this.b1.inverseInertia;\n this.i2 = this.b2.inverseInertia;\n this.limitImpulse1 = 0;\n this.motorImpulse1 = 0;\n this.limitImpulse2 = 0;\n this.motorImpulse2 = 0;\n this.limitImpulse3 = 0;\n this.motorImpulse3 = 0;\n this.cfm1 = 0; // Constraint Force Mixing\n this.cfm2 = 0;\n this.cfm3 = 0;\n this.weight = -1;\n}\n\nObject.assign(Translational3Constraint.prototype, {\n\n Translational3Constraint: true,\n\n preSolve: function (timeStep, invTimeStep) {\n this.ax1 = this.limitMotor1.axis.x;\n this.ay1 = this.limitMotor1.axis.y;\n this.az1 = this.limitMotor1.axis.z;\n this.ax2 = this.limitMotor2.axis.x;\n this.ay2 = this.limitMotor2.axis.y;\n this.az2 = this.limitMotor2.axis.z;\n this.ax3 = this.limitMotor3.axis.x;\n this.ay3 = this.limitMotor3.axis.y;\n this.az3 = this.limitMotor3.axis.z;\n this.lowerLimit1 = this.limitMotor1.lowerLimit;\n this.upperLimit1 = this.limitMotor1.upperLimit;\n this.motorSpeed1 = this.limitMotor1.motorSpeed;\n this.maxMotorForce1 = this.limitMotor1.maxMotorForce;\n this.enableMotor1 = this.maxMotorForce1 > 0;\n this.lowerLimit2 = this.limitMotor2.lowerLimit;\n this.upperLimit2 = this.limitMotor2.upperLimit;\n this.motorSpeed2 = this.limitMotor2.motorSpeed;\n this.maxMotorForce2 = this.limitMotor2.maxMotorForce;\n this.enableMotor2 = this.maxMotorForce2 > 0;\n this.lowerLimit3 = this.limitMotor3.lowerLimit;\n this.upperLimit3 = this.limitMotor3.upperLimit;\n this.motorSpeed3 = this.limitMotor3.motorSpeed;\n this.maxMotorForce3 = this.limitMotor3.maxMotorForce;\n this.enableMotor3 = this.maxMotorForce3 > 0;\n this.m1 = this.b1.inverseMass;\n this.m2 = this.b2.inverseMass;\n\n var ti1 = this.i1.elements;\n var ti2 = this.i2.elements;\n this.i1e00 = ti1[0];\n this.i1e01 = ti1[1];\n this.i1e02 = ti1[2];\n this.i1e10 = ti1[3];\n this.i1e11 = ti1[4];\n this.i1e12 = ti1[5];\n this.i1e20 = ti1[6];\n this.i1e21 = ti1[7];\n this.i1e22 = ti1[8];\n\n this.i2e00 = ti2[0];\n this.i2e01 = ti2[1];\n this.i2e02 = ti2[2];\n this.i2e10 = ti2[3];\n this.i2e11 = ti2[4];\n this.i2e12 = ti2[5];\n this.i2e20 = ti2[6];\n this.i2e21 = ti2[7];\n this.i2e22 = ti2[8];\n\n var dx = this.p2.x - this.p1.x;\n var dy = this.p2.y - this.p1.y;\n var dz = this.p2.z - this.p1.z;\n var d1 = dx * this.ax1 + dy * this.ay1 + dz * this.az1;\n var d2 = dx * this.ax2 + dy * this.ay2 + dz * this.az2;\n var d3 = dx * this.ax3 + dy * this.ay3 + dz * this.az3;\n var frequency1 = this.limitMotor1.frequency;\n var frequency2 = this.limitMotor2.frequency;\n var frequency3 = this.limitMotor3.frequency;\n var enableSpring1 = frequency1 > 0;\n var enableSpring2 = frequency2 > 0;\n var enableSpring3 = frequency3 > 0;\n var enableLimit1 = this.lowerLimit1 <= this.upperLimit1;\n var enableLimit2 = this.lowerLimit2 <= this.upperLimit2;\n var enableLimit3 = this.lowerLimit3 <= this.upperLimit3;\n\n // for stability\n if (enableSpring1 && d1 > 20 || d1 < -20) {\n enableSpring1 = false;\n }\n if (enableSpring2 && d2 > 20 || d2 < -20) {\n enableSpring2 = false;\n }\n if (enableSpring3 && d3 > 20 || d3 < -20) {\n enableSpring3 = false;\n }\n\n if (enableLimit1) {\n if (this.lowerLimit1 == this.upperLimit1) {\n if (this.limitState1 != 0) {\n this.limitState1 = 0;\n this.limitImpulse1 = 0;\n }\n this.limitVelocity1 = this.lowerLimit1 - d1;\n if (!enableSpring1) d1 = this.lowerLimit1;\n } else if (d1 < this.lowerLimit1) {\n if (this.limitState1 != -1) {\n this.limitState1 = -1;\n this.limitImpulse1 = 0;\n }\n this.limitVelocity1 = this.lowerLimit1 - d1;\n if (!enableSpring1) d1 = this.lowerLimit1;\n } else if (d1 > this.upperLimit1) {\n if (this.limitState1 != 1) {\n this.limitState1 = 1;\n this.limitImpulse1 = 0;\n }\n this.limitVelocity1 = this.upperLimit1 - d1;\n if (!enableSpring1) d1 = this.upperLimit1;\n } else {\n this.limitState1 = 2;\n this.limitImpulse1 = 0;\n this.limitVelocity1 = 0;\n }\n if (!enableSpring1) {\n if (this.limitVelocity1 > 0.005) this.limitVelocity1 -= 0.005;else if (this.limitVelocity1 < -0.005) this.limitVelocity1 += 0.005;else this.limitVelocity1 = 0;\n }\n } else {\n this.limitState1 = 2;\n this.limitImpulse1 = 0;\n }\n\n if (enableLimit2) {\n if (this.lowerLimit2 == this.upperLimit2) {\n if (this.limitState2 != 0) {\n this.limitState2 = 0;\n this.limitImpulse2 = 0;\n }\n this.limitVelocity2 = this.lowerLimit2 - d2;\n if (!enableSpring2) d2 = this.lowerLimit2;\n } else if (d2 < this.lowerLimit2) {\n if (this.limitState2 != -1) {\n this.limitState2 = -1;\n this.limitImpulse2 = 0;\n }\n this.limitVelocity2 = this.lowerLimit2 - d2;\n if (!enableSpring2) d2 = this.lowerLimit2;\n } else if (d2 > this.upperLimit2) {\n if (this.limitState2 != 1) {\n this.limitState2 = 1;\n this.limitImpulse2 = 0;\n }\n this.limitVelocity2 = this.upperLimit2 - d2;\n if (!enableSpring2) d2 = this.upperLimit2;\n } else {\n this.limitState2 = 2;\n this.limitImpulse2 = 0;\n this.limitVelocity2 = 0;\n }\n if (!enableSpring2) {\n if (this.limitVelocity2 > 0.005) this.limitVelocity2 -= 0.005;else if (this.limitVelocity2 < -0.005) this.limitVelocity2 += 0.005;else this.limitVelocity2 = 0;\n }\n } else {\n this.limitState2 = 2;\n this.limitImpulse2 = 0;\n }\n\n if (enableLimit3) {\n if (this.lowerLimit3 == this.upperLimit3) {\n if (this.limitState3 != 0) {\n this.limitState3 = 0;\n this.limitImpulse3 = 0;\n }\n this.limitVelocity3 = this.lowerLimit3 - d3;\n if (!enableSpring3) d3 = this.lowerLimit3;\n } else if (d3 < this.lowerLimit3) {\n if (this.limitState3 != -1) {\n this.limitState3 = -1;\n this.limitImpulse3 = 0;\n }\n this.limitVelocity3 = this.lowerLimit3 - d3;\n if (!enableSpring3) d3 = this.lowerLimit3;\n } else if (d3 > this.upperLimit3) {\n if (this.limitState3 != 1) {\n this.limitState3 = 1;\n this.limitImpulse3 = 0;\n }\n this.limitVelocity3 = this.upperLimit3 - d3;\n if (!enableSpring3) d3 = this.upperLimit3;\n } else {\n this.limitState3 = 2;\n this.limitImpulse3 = 0;\n this.limitVelocity3 = 0;\n }\n if (!enableSpring3) {\n if (this.limitVelocity3 > 0.005) this.limitVelocity3 -= 0.005;else if (this.limitVelocity3 < -0.005) this.limitVelocity3 += 0.005;else this.limitVelocity3 = 0;\n }\n } else {\n this.limitState3 = 2;\n this.limitImpulse3 = 0;\n }\n\n if (this.enableMotor1 && (this.limitState1 != 0 || enableSpring1)) {\n this.maxMotorImpulse1 = this.maxMotorForce1 * timeStep;\n } else {\n this.motorImpulse1 = 0;\n this.maxMotorImpulse1 = 0;\n }\n\n if (this.enableMotor2 && (this.limitState2 != 0 || enableSpring2)) {\n this.maxMotorImpulse2 = this.maxMotorForce2 * timeStep;\n } else {\n this.motorImpulse2 = 0;\n this.maxMotorImpulse2 = 0;\n }\n\n if (this.enableMotor3 && (this.limitState3 != 0 || enableSpring3)) {\n this.maxMotorImpulse3 = this.maxMotorForce3 * timeStep;\n } else {\n this.motorImpulse3 = 0;\n this.maxMotorImpulse3 = 0;\n }\n\n var rdx = d1 * this.ax1 + d2 * this.ax2 + d3 * this.ax2;\n var rdy = d1 * this.ay1 + d2 * this.ay2 + d3 * this.ay2;\n var rdz = d1 * this.az1 + d2 * this.az2 + d3 * this.az2;\n var w1 = this.m2 / (this.m1 + this.m2);\n if (this.weight >= 0) w1 = this.weight; // use given weight\n var w2 = 1 - w1;\n this.r1x = this.r1.x + rdx * w1;\n this.r1y = this.r1.y + rdy * w1;\n this.r1z = this.r1.z + rdz * w1;\n this.r2x = this.r2.x - rdx * w2;\n this.r2y = this.r2.y - rdy * w2;\n this.r2z = this.r2.z - rdz * w2;\n\n // build jacobians\n this.t1x1 = this.r1y * this.az1 - this.r1z * this.ay1;\n this.t1y1 = this.r1z * this.ax1 - this.r1x * this.az1;\n this.t1z1 = this.r1x * this.ay1 - this.r1y * this.ax1;\n this.t2x1 = this.r2y * this.az1 - this.r2z * this.ay1;\n this.t2y1 = this.r2z * this.ax1 - this.r2x * this.az1;\n this.t2z1 = this.r2x * this.ay1 - this.r2y * this.ax1;\n this.l1x1 = this.ax1 * this.m1;\n this.l1y1 = this.ay1 * this.m1;\n this.l1z1 = this.az1 * this.m1;\n this.l2x1 = this.ax1 * this.m2;\n this.l2y1 = this.ay1 * this.m2;\n this.l2z1 = this.az1 * this.m2;\n this.a1x1 = this.t1x1 * this.i1e00 + this.t1y1 * this.i1e01 + this.t1z1 * this.i1e02;\n this.a1y1 = this.t1x1 * this.i1e10 + this.t1y1 * this.i1e11 + this.t1z1 * this.i1e12;\n this.a1z1 = this.t1x1 * this.i1e20 + this.t1y1 * this.i1e21 + this.t1z1 * this.i1e22;\n this.a2x1 = this.t2x1 * this.i2e00 + this.t2y1 * this.i2e01 + this.t2z1 * this.i2e02;\n this.a2y1 = this.t2x1 * this.i2e10 + this.t2y1 * this.i2e11 + this.t2z1 * this.i2e12;\n this.a2z1 = this.t2x1 * this.i2e20 + this.t2y1 * this.i2e21 + this.t2z1 * this.i2e22;\n\n this.t1x2 = this.r1y * this.az2 - this.r1z * this.ay2;\n this.t1y2 = this.r1z * this.ax2 - this.r1x * this.az2;\n this.t1z2 = this.r1x * this.ay2 - this.r1y * this.ax2;\n this.t2x2 = this.r2y * this.az2 - this.r2z * this.ay2;\n this.t2y2 = this.r2z * this.ax2 - this.r2x * this.az2;\n this.t2z2 = this.r2x * this.ay2 - this.r2y * this.ax2;\n this.l1x2 = this.ax2 * this.m1;\n this.l1y2 = this.ay2 * this.m1;\n this.l1z2 = this.az2 * this.m1;\n this.l2x2 = this.ax2 * this.m2;\n this.l2y2 = this.ay2 * this.m2;\n this.l2z2 = this.az2 * this.m2;\n this.a1x2 = this.t1x2 * this.i1e00 + this.t1y2 * this.i1e01 + this.t1z2 * this.i1e02;\n this.a1y2 = this.t1x2 * this.i1e10 + this.t1y2 * this.i1e11 + this.t1z2 * this.i1e12;\n this.a1z2 = this.t1x2 * this.i1e20 + this.t1y2 * this.i1e21 + this.t1z2 * this.i1e22;\n this.a2x2 = this.t2x2 * this.i2e00 + this.t2y2 * this.i2e01 + this.t2z2 * this.i2e02;\n this.a2y2 = this.t2x2 * this.i2e10 + this.t2y2 * this.i2e11 + this.t2z2 * this.i2e12;\n this.a2z2 = this.t2x2 * this.i2e20 + this.t2y2 * this.i2e21 + this.t2z2 * this.i2e22;\n\n this.t1x3 = this.r1y * this.az3 - this.r1z * this.ay3;\n this.t1y3 = this.r1z * this.ax3 - this.r1x * this.az3;\n this.t1z3 = this.r1x * this.ay3 - this.r1y * this.ax3;\n this.t2x3 = this.r2y * this.az3 - this.r2z * this.ay3;\n this.t2y3 = this.r2z * this.ax3 - this.r2x * this.az3;\n this.t2z3 = this.r2x * this.ay3 - this.r2y * this.ax3;\n this.l1x3 = this.ax3 * this.m1;\n this.l1y3 = this.ay3 * this.m1;\n this.l1z3 = this.az3 * this.m1;\n this.l2x3 = this.ax3 * this.m2;\n this.l2y3 = this.ay3 * this.m2;\n this.l2z3 = this.az3 * this.m2;\n this.a1x3 = this.t1x3 * this.i1e00 + this.t1y3 * this.i1e01 + this.t1z3 * this.i1e02;\n this.a1y3 = this.t1x3 * this.i1e10 + this.t1y3 * this.i1e11 + this.t1z3 * this.i1e12;\n this.a1z3 = this.t1x3 * this.i1e20 + this.t1y3 * this.i1e21 + this.t1z3 * this.i1e22;\n this.a2x3 = this.t2x3 * this.i2e00 + this.t2y3 * this.i2e01 + this.t2z3 * this.i2e02;\n this.a2y3 = this.t2x3 * this.i2e10 + this.t2y3 * this.i2e11 + this.t2z3 * this.i2e12;\n this.a2z3 = this.t2x3 * this.i2e20 + this.t2y3 * this.i2e21 + this.t2z3 * this.i2e22;\n\n // build an impulse matrix\n var m12 = this.m1 + this.m2;\n this.k00 = (this.ax1 * this.ax1 + this.ay1 * this.ay1 + this.az1 * this.az1) * m12;\n this.k01 = (this.ax1 * this.ax2 + this.ay1 * this.ay2 + this.az1 * this.az2) * m12;\n this.k02 = (this.ax1 * this.ax3 + this.ay1 * this.ay3 + this.az1 * this.az3) * m12;\n this.k10 = (this.ax2 * this.ax1 + this.ay2 * this.ay1 + this.az2 * this.az1) * m12;\n this.k11 = (this.ax2 * this.ax2 + this.ay2 * this.ay2 + this.az2 * this.az2) * m12;\n this.k12 = (this.ax2 * this.ax3 + this.ay2 * this.ay3 + this.az2 * this.az3) * m12;\n this.k20 = (this.ax3 * this.ax1 + this.ay3 * this.ay1 + this.az3 * this.az1) * m12;\n this.k21 = (this.ax3 * this.ax2 + this.ay3 * this.ay2 + this.az3 * this.az2) * m12;\n this.k22 = (this.ax3 * this.ax3 + this.ay3 * this.ay3 + this.az3 * this.az3) * m12;\n\n this.k00 += this.t1x1 * this.a1x1 + this.t1y1 * this.a1y1 + this.t1z1 * this.a1z1;\n this.k01 += this.t1x1 * this.a1x2 + this.t1y1 * this.a1y2 + this.t1z1 * this.a1z2;\n this.k02 += this.t1x1 * this.a1x3 + this.t1y1 * this.a1y3 + this.t1z1 * this.a1z3;\n this.k10 += this.t1x2 * this.a1x1 + this.t1y2 * this.a1y1 + this.t1z2 * this.a1z1;\n this.k11 += this.t1x2 * this.a1x2 + this.t1y2 * this.a1y2 + this.t1z2 * this.a1z2;\n this.k12 += this.t1x2 * this.a1x3 + this.t1y2 * this.a1y3 + this.t1z2 * this.a1z3;\n this.k20 += this.t1x3 * this.a1x1 + this.t1y3 * this.a1y1 + this.t1z3 * this.a1z1;\n this.k21 += this.t1x3 * this.a1x2 + this.t1y3 * this.a1y2 + this.t1z3 * this.a1z2;\n this.k22 += this.t1x3 * this.a1x3 + this.t1y3 * this.a1y3 + this.t1z3 * this.a1z3;\n\n this.k00 += this.t2x1 * this.a2x1 + this.t2y1 * this.a2y1 + this.t2z1 * this.a2z1;\n this.k01 += this.t2x1 * this.a2x2 + this.t2y1 * this.a2y2 + this.t2z1 * this.a2z2;\n this.k02 += this.t2x1 * this.a2x3 + this.t2y1 * this.a2y3 + this.t2z1 * this.a2z3;\n this.k10 += this.t2x2 * this.a2x1 + this.t2y2 * this.a2y1 + this.t2z2 * this.a2z1;\n this.k11 += this.t2x2 * this.a2x2 + this.t2y2 * this.a2y2 + this.t2z2 * this.a2z2;\n this.k12 += this.t2x2 * this.a2x3 + this.t2y2 * this.a2y3 + this.t2z2 * this.a2z3;\n this.k20 += this.t2x3 * this.a2x1 + this.t2y3 * this.a2y1 + this.t2z3 * this.a2z1;\n this.k21 += this.t2x3 * this.a2x2 + this.t2y3 * this.a2y2 + this.t2z3 * this.a2z2;\n this.k22 += this.t2x3 * this.a2x3 + this.t2y3 * this.a2y3 + this.t2z3 * this.a2z3;\n\n this.kv00 = this.k00;\n this.kv11 = this.k11;\n this.kv22 = this.k22;\n\n this.dv00 = 1 / this.kv00;\n this.dv11 = 1 / this.kv11;\n this.dv22 = 1 / this.kv22;\n\n if (enableSpring1 && this.limitState1 != 2) {\n var omega = 6.2831853 * frequency1;\n var k = omega * omega * timeStep;\n var dmp = invTimeStep / (k + 2 * this.limitMotor1.dampingRatio * omega);\n this.cfm1 = this.kv00 * dmp;\n this.limitVelocity1 *= k * dmp;\n } else {\n this.cfm1 = 0;\n this.limitVelocity1 *= invTimeStep * 0.05;\n }\n if (enableSpring2 && this.limitState2 != 2) {\n omega = 6.2831853 * frequency2;\n k = omega * omega * timeStep;\n dmp = invTimeStep / (k + 2 * this.limitMotor2.dampingRatio * omega);\n this.cfm2 = this.kv11 * dmp;\n this.limitVelocity2 *= k * dmp;\n } else {\n this.cfm2 = 0;\n this.limitVelocity2 *= invTimeStep * 0.05;\n }\n if (enableSpring3 && this.limitState3 != 2) {\n omega = 6.2831853 * frequency3;\n k = omega * omega * timeStep;\n dmp = invTimeStep / (k + 2 * this.limitMotor3.dampingRatio * omega);\n this.cfm3 = this.kv22 * dmp;\n this.limitVelocity3 *= k * dmp;\n } else {\n this.cfm3 = 0;\n this.limitVelocity3 *= invTimeStep * 0.05;\n }\n this.k00 += this.cfm1;\n this.k11 += this.cfm2;\n this.k22 += this.cfm3;\n\n 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));\n this.d00 = (this.k11 * this.k22 - this.k12 * this.k21) * inv;\n this.d01 = (this.k02 * this.k21 - this.k01 * this.k22) * inv;\n this.d02 = (this.k01 * this.k12 - this.k02 * this.k11) * inv;\n this.d10 = (this.k12 * this.k20 - this.k10 * this.k22) * inv;\n this.d11 = (this.k00 * this.k22 - this.k02 * this.k20) * inv;\n this.d12 = (this.k02 * this.k10 - this.k00 * this.k12) * inv;\n this.d20 = (this.k10 * this.k21 - this.k11 * this.k20) * inv;\n this.d21 = (this.k01 * this.k20 - this.k00 * this.k21) * inv;\n this.d22 = (this.k00 * this.k11 - this.k01 * this.k10) * inv;\n\n // warm starting\n var totalImpulse1 = this.limitImpulse1 + this.motorImpulse1;\n var totalImpulse2 = this.limitImpulse2 + this.motorImpulse2;\n var totalImpulse3 = this.limitImpulse3 + this.motorImpulse3;\n this.l1.x += totalImpulse1 * this.l1x1 + totalImpulse2 * this.l1x2 + totalImpulse3 * this.l1x3;\n this.l1.y += totalImpulse1 * this.l1y1 + totalImpulse2 * this.l1y2 + totalImpulse3 * this.l1y3;\n this.l1.z += totalImpulse1 * this.l1z1 + totalImpulse2 * this.l1z2 + totalImpulse3 * this.l1z3;\n this.a1.x += totalImpulse1 * this.a1x1 + totalImpulse2 * this.a1x2 + totalImpulse3 * this.a1x3;\n this.a1.y += totalImpulse1 * this.a1y1 + totalImpulse2 * this.a1y2 + totalImpulse3 * this.a1y3;\n this.a1.z += totalImpulse1 * this.a1z1 + totalImpulse2 * this.a1z2 + totalImpulse3 * this.a1z3;\n this.l2.x -= totalImpulse1 * this.l2x1 + totalImpulse2 * this.l2x2 + totalImpulse3 * this.l2x3;\n this.l2.y -= totalImpulse1 * this.l2y1 + totalImpulse2 * this.l2y2 + totalImpulse3 * this.l2y3;\n this.l2.z -= totalImpulse1 * this.l2z1 + totalImpulse2 * this.l2z2 + totalImpulse3 * this.l2z3;\n this.a2.x -= totalImpulse1 * this.a2x1 + totalImpulse2 * this.a2x2 + totalImpulse3 * this.a2x3;\n this.a2.y -= totalImpulse1 * this.a2y1 + totalImpulse2 * this.a2y2 + totalImpulse3 * this.a2y3;\n this.a2.z -= totalImpulse1 * this.a2z1 + totalImpulse2 * this.a2z2 + totalImpulse3 * this.a2z3;\n },\n\n solve: function () {\n 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;\n 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;\n 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;\n var rvn1 = rvx * this.ax1 + rvy * this.ay1 + rvz * this.az1;\n var rvn2 = rvx * this.ax2 + rvy * this.ay2 + rvz * this.az2;\n var rvn3 = rvx * this.ax3 + rvy * this.ay3 + rvz * this.az3;\n var oldMotorImpulse1 = this.motorImpulse1;\n var oldMotorImpulse2 = this.motorImpulse2;\n var oldMotorImpulse3 = this.motorImpulse3;\n var dMotorImpulse1 = 0;\n var dMotorImpulse2 = 0;\n var dMotorImpulse3 = 0;\n if (this.enableMotor1) {\n dMotorImpulse1 = (rvn1 - this.motorSpeed1) * this.dv00;\n this.motorImpulse1 += dMotorImpulse1;\n if (this.motorImpulse1 > this.maxMotorImpulse1) {\n // clamp motor impulse\n this.motorImpulse1 = this.maxMotorImpulse1;\n } else if (this.motorImpulse1 < -this.maxMotorImpulse1) {\n this.motorImpulse1 = -this.maxMotorImpulse1;\n }\n dMotorImpulse1 = this.motorImpulse1 - oldMotorImpulse1;\n }\n if (this.enableMotor2) {\n dMotorImpulse2 = (rvn2 - this.motorSpeed2) * this.dv11;\n this.motorImpulse2 += dMotorImpulse2;\n if (this.motorImpulse2 > this.maxMotorImpulse2) {\n // clamp motor impulse\n this.motorImpulse2 = this.maxMotorImpulse2;\n } else if (this.motorImpulse2 < -this.maxMotorImpulse2) {\n this.motorImpulse2 = -this.maxMotorImpulse2;\n }\n dMotorImpulse2 = this.motorImpulse2 - oldMotorImpulse2;\n }\n if (this.enableMotor3) {\n dMotorImpulse3 = (rvn3 - this.motorSpeed3) * this.dv22;\n this.motorImpulse3 += dMotorImpulse3;\n if (this.motorImpulse3 > this.maxMotorImpulse3) {\n // clamp motor impulse\n this.motorImpulse3 = this.maxMotorImpulse3;\n } else if (this.motorImpulse3 < -this.maxMotorImpulse3) {\n this.motorImpulse3 = -this.maxMotorImpulse3;\n }\n dMotorImpulse3 = this.motorImpulse3 - oldMotorImpulse3;\n }\n\n // apply motor impulse to relative velocity\n rvn1 += dMotorImpulse1 * this.kv00 + dMotorImpulse2 * this.k01 + dMotorImpulse3 * this.k02;\n rvn2 += dMotorImpulse1 * this.k10 + dMotorImpulse2 * this.kv11 + dMotorImpulse3 * this.k12;\n rvn3 += dMotorImpulse1 * this.k20 + dMotorImpulse2 * this.k21 + dMotorImpulse3 * this.kv22;\n\n // subtract target velocity and applied impulse\n rvn1 -= this.limitVelocity1 + this.limitImpulse1 * this.cfm1;\n rvn2 -= this.limitVelocity2 + this.limitImpulse2 * this.cfm2;\n rvn3 -= this.limitVelocity3 + this.limitImpulse3 * this.cfm3;\n\n var oldLimitImpulse1 = this.limitImpulse1;\n var oldLimitImpulse2 = this.limitImpulse2;\n var oldLimitImpulse3 = this.limitImpulse3;\n\n var dLimitImpulse1 = rvn1 * this.d00 + rvn2 * this.d01 + rvn3 * this.d02;\n var dLimitImpulse2 = rvn1 * this.d10 + rvn2 * this.d11 + rvn3 * this.d12;\n var dLimitImpulse3 = rvn1 * this.d20 + rvn2 * this.d21 + rvn3 * this.d22;\n\n this.limitImpulse1 += dLimitImpulse1;\n this.limitImpulse2 += dLimitImpulse2;\n this.limitImpulse3 += dLimitImpulse3;\n\n // clamp\n var clampState = 0;\n if (this.limitState1 == 2 || this.limitImpulse1 * this.limitState1 < 0) {\n dLimitImpulse1 = -oldLimitImpulse1;\n rvn2 += dLimitImpulse1 * this.k10;\n rvn3 += dLimitImpulse1 * this.k20;\n clampState |= 1;\n }\n if (this.limitState2 == 2 || this.limitImpulse2 * this.limitState2 < 0) {\n dLimitImpulse2 = -oldLimitImpulse2;\n rvn1 += dLimitImpulse2 * this.k01;\n rvn3 += dLimitImpulse2 * this.k21;\n clampState |= 2;\n }\n if (this.limitState3 == 2 || this.limitImpulse3 * this.limitState3 < 0) {\n dLimitImpulse3 = -oldLimitImpulse3;\n rvn1 += dLimitImpulse3 * this.k02;\n rvn2 += dLimitImpulse3 * this.k12;\n clampState |= 4;\n }\n\n // update un-clamped impulse\n // TODO: isolate division\n var det;\n switch (clampState) {\n case 1:\n // update 2 3\n det = 1 / (this.k11 * this.k22 - this.k12 * this.k21);\n dLimitImpulse2 = (this.k22 * rvn2 + -this.k12 * rvn3) * det;\n dLimitImpulse3 = (-this.k21 * rvn2 + this.k11 * rvn3) * det;\n break;\n case 2:\n // update 1 3\n det = 1 / (this.k00 * this.k22 - this.k02 * this.k20);\n dLimitImpulse1 = (this.k22 * rvn1 + -this.k02 * rvn3) * det;\n dLimitImpulse3 = (-this.k20 * rvn1 + this.k00 * rvn3) * det;\n break;\n case 3:\n // update 3\n dLimitImpulse3 = rvn3 / this.k22;\n break;\n case 4:\n // update 1 2\n det = 1 / (this.k00 * this.k11 - this.k01 * this.k10);\n dLimitImpulse1 = (this.k11 * rvn1 + -this.k01 * rvn2) * det;\n dLimitImpulse2 = (-this.k10 * rvn1 + this.k00 * rvn2) * det;\n break;\n case 5:\n // update 2\n dLimitImpulse2 = rvn2 / this.k11;\n break;\n case 6:\n // update 1\n dLimitImpulse1 = rvn1 / this.k00;\n break;\n }\n\n this.limitImpulse1 = oldLimitImpulse1 + dLimitImpulse1;\n this.limitImpulse2 = oldLimitImpulse2 + dLimitImpulse2;\n this.limitImpulse3 = oldLimitImpulse3 + dLimitImpulse3;\n\n var dImpulse1 = dMotorImpulse1 + dLimitImpulse1;\n var dImpulse2 = dMotorImpulse2 + dLimitImpulse2;\n var dImpulse3 = dMotorImpulse3 + dLimitImpulse3;\n\n // apply impulse\n this.l1.x += dImpulse1 * this.l1x1 + dImpulse2 * this.l1x2 + dImpulse3 * this.l1x3;\n this.l1.y += dImpulse1 * this.l1y1 + dImpulse2 * this.l1y2 + dImpulse3 * this.l1y3;\n this.l1.z += dImpulse1 * this.l1z1 + dImpulse2 * this.l1z2 + dImpulse3 * this.l1z3;\n this.a1.x += dImpulse1 * this.a1x1 + dImpulse2 * this.a1x2 + dImpulse3 * this.a1x3;\n this.a1.y += dImpulse1 * this.a1y1 + dImpulse2 * this.a1y2 + dImpulse3 * this.a1y3;\n this.a1.z += dImpulse1 * this.a1z1 + dImpulse2 * this.a1z2 + dImpulse3 * this.a1z3;\n this.l2.x -= dImpulse1 * this.l2x1 + dImpulse2 * this.l2x2 + dImpulse3 * this.l2x3;\n this.l2.y -= dImpulse1 * this.l2y1 + dImpulse2 * this.l2y2 + dImpulse3 * this.l2y3;\n this.l2.z -= dImpulse1 * this.l2z1 + dImpulse2 * this.l2z2 + dImpulse3 * this.l2z3;\n this.a2.x -= dImpulse1 * this.a2x1 + dImpulse2 * this.a2x2 + dImpulse3 * this.a2x3;\n this.a2.y -= dImpulse1 * this.a2y1 + dImpulse2 * this.a2y2 + dImpulse3 * this.a2y3;\n this.a2.z -= dImpulse1 * this.a2z1 + dImpulse2 * this.a2z2 + dImpulse3 * this.a2z3;\n }\n\n});\n\n/**\n * A prismatic joint allows only for relative translation of rigid bodies along the axis.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction PrismaticJoint(config, lowerTranslation, upperTranslation) {\n\n Joint.call(this, config);\n\n this.type = JOINT_PRISMATIC;\n\n // The axis in the first body's coordinate system.\n this.localAxis1 = config.localAxis1.clone().normalize();\n // The axis in the second body's coordinate system.\n this.localAxis2 = config.localAxis2.clone().normalize();\n\n this.ax1 = new Vec3();\n this.ax2 = new Vec3();\n\n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n this.ac = new AngularConstraint(this, new Quat().setFromUnitVectors(this.localAxis1, this.localAxis2));\n\n // The translational limit and motor information of the joint.\n this.limitMotor = new LimitMotor(this.nor, true);\n this.limitMotor.lowerLimit = lowerTranslation;\n this.limitMotor.upperLimit = upperTranslation;\n this.t3 = new Translational3Constraint(this, this.limitMotor, new LimitMotor(this.tan, true), new LimitMotor(this.bin, true));\n}\n\nPrismaticJoint.prototype = Object.assign(Object.create(Joint.prototype), {\n\n constructor: PrismaticJoint,\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.updateAnchorPoints();\n\n this.ax1.copy(this.localAxis1).applyMatrix3(this.body1.rotation, true);\n this.ax2.copy(this.localAxis2).applyMatrix3(this.body2.rotation, true);\n\n // normal tangent binormal\n\n 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();\n this.tan.tangent(this.nor).normalize();\n this.bin.crossVectors(this.nor, this.tan);\n\n // preSolve\n\n this.ac.preSolve(timeStep, invTimeStep);\n this.t3.preSolve(timeStep, invTimeStep);\n },\n\n solve: function () {\n\n this.ac.solve();\n this.t3.solve();\n },\n\n postSolve: function () {}\n\n});\n\n/**\n * A slider joint allows for relative translation and relative rotation between two rigid bodies along the axis.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction SliderJoint(config, lowerTranslation, upperTranslation) {\n\n Joint.call(this, config);\n\n this.type = JOINT_SLIDER;\n\n // The axis in the first body's coordinate system.\n this.localAxis1 = config.localAxis1.clone().normalize();\n // The axis in the second body's coordinate system.\n this.localAxis2 = config.localAxis2.clone().normalize();\n\n // make angle axis\n var arc = new Mat33().setQuat(new Quat().setFromUnitVectors(this.localAxis1, this.localAxis2));\n this.localAngle1 = new Vec3().tangent(this.localAxis1).normalize();\n this.localAngle2 = this.localAngle1.clone().applyMatrix3(arc, true);\n\n this.ax1 = new Vec3();\n this.ax2 = new Vec3();\n this.an1 = new Vec3();\n this.an2 = new Vec3();\n\n this.tmp = new Vec3();\n\n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n // The limit and motor for the rotation\n this.rotationalLimitMotor = new LimitMotor(this.nor, false);\n this.r3 = new Rotational3Constraint(this, this.rotationalLimitMotor, new LimitMotor(this.tan, true), new LimitMotor(this.bin, true));\n\n // The limit and motor for the translation.\n this.translationalLimitMotor = new LimitMotor(this.nor, true);\n this.translationalLimitMotor.lowerLimit = lowerTranslation;\n this.translationalLimitMotor.upperLimit = upperTranslation;\n this.t3 = new Translational3Constraint(this, this.translationalLimitMotor, new LimitMotor(this.tan, true), new LimitMotor(this.bin, true));\n}\n\nSliderJoint.prototype = Object.assign(Object.create(Joint.prototype), {\n\n constructor: SliderJoint,\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.updateAnchorPoints();\n\n this.ax1.copy(this.localAxis1).applyMatrix3(this.body1.rotation, true);\n this.an1.copy(this.localAngle1).applyMatrix3(this.body1.rotation, true);\n\n this.ax2.copy(this.localAxis2).applyMatrix3(this.body2.rotation, true);\n this.an2.copy(this.localAngle2).applyMatrix3(this.body2.rotation, true);\n\n // normal tangent binormal\n\n 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();\n this.tan.tangent(this.nor).normalize();\n this.bin.crossVectors(this.nor, this.tan);\n\n // calculate hinge angle\n\n this.tmp.crossVectors(this.an1, this.an2);\n\n var limite = _Math.acosClamp(_Math.dotVectors(this.an1, this.an2));\n\n if (_Math.dotVectors(this.nor, this.tmp) < 0) this.rotationalLimitMotor.angle = -limite;else this.rotationalLimitMotor.angle = limite;\n\n // angular error\n\n this.tmp.crossVectors(this.ax1, this.ax2);\n this.r3.limitMotor2.angle = _Math.dotVectors(this.tan, this.tmp);\n this.r3.limitMotor3.angle = _Math.dotVectors(this.bin, this.tmp);\n\n // preSolve\n\n this.r3.preSolve(timeStep, invTimeStep);\n this.t3.preSolve(timeStep, invTimeStep);\n },\n\n solve: function () {\n\n this.r3.solve();\n this.t3.solve();\n },\n\n postSolve: function () {}\n\n});\n\n/**\n * A wheel joint allows for relative rotation between two rigid bodies along two axes.\n * The wheel joint also allows for relative translation for the suspension.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction WheelJoint(config) {\n\n Joint.call(this, config);\n\n this.type = JOINT_WHEEL;\n\n // The axis in the first body's coordinate system.\n this.localAxis1 = config.localAxis1.clone().normalize();\n // The axis in the second body's coordinate system.\n this.localAxis2 = config.localAxis2.clone().normalize();\n\n this.localAngle1 = new Vec3();\n this.localAngle2 = new Vec3();\n\n var dot = _Math.dotVectors(this.localAxis1, this.localAxis2);\n\n if (dot > -1 && dot < 1) {\n\n 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();\n\n 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();\n } else {\n\n var arc = new Mat33().setQuat(new Quat().setFromUnitVectors(this.localAxis1, this.localAxis2));\n this.localAngle1.tangent(this.localAxis1).normalize();\n this.localAngle2 = this.localAngle1.clone().applyMatrix3(arc, true);\n }\n\n this.ax1 = new Vec3();\n this.ax2 = new Vec3();\n this.an1 = new Vec3();\n this.an2 = new Vec3();\n\n this.tmp = new Vec3();\n\n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n // The translational limit and motor information of the joint.\n this.translationalLimitMotor = new LimitMotor(this.tan, true);\n this.translationalLimitMotor.frequency = 8;\n this.translationalLimitMotor.dampingRatio = 1;\n // The first rotational limit and motor information of the joint.\n this.rotationalLimitMotor1 = new LimitMotor(this.tan, false);\n // The second rotational limit and motor information of the joint.\n this.rotationalLimitMotor2 = new LimitMotor(this.bin, false);\n\n this.t3 = new Translational3Constraint(this, new LimitMotor(this.nor, true), this.translationalLimitMotor, new LimitMotor(this.bin, true));\n this.t3.weight = 1;\n this.r3 = new Rotational3Constraint(this, new LimitMotor(this.nor, true), this.rotationalLimitMotor1, this.rotationalLimitMotor2);\n}\n\nWheelJoint.prototype = Object.assign(Object.create(Joint.prototype), {\n\n constructor: WheelJoint,\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.updateAnchorPoints();\n\n this.ax1.copy(this.localAxis1).applyMatrix3(this.body1.rotation, true);\n this.an1.copy(this.localAngle1).applyMatrix3(this.body1.rotation, true);\n\n this.ax2.copy(this.localAxis2).applyMatrix3(this.body2.rotation, true);\n this.an2.copy(this.localAngle2).applyMatrix3(this.body2.rotation, true);\n\n this.r3.limitMotor1.angle = _Math.dotVectors(this.ax1, this.ax2);\n\n var limite = _Math.dotVectors(this.an1, this.ax2);\n\n if (_Math.dotVectors(this.ax1, this.tmp.crossVectors(this.an1, this.ax2)) < 0) this.rotationalLimitMotor1.angle = -limite;else this.rotationalLimitMotor1.angle = limite;\n\n limite = _Math.dotVectors(this.an2, this.ax1);\n\n if (_Math.dotVectors(this.ax2, this.tmp.crossVectors(this.an2, this.ax1)) < 0) this.rotationalLimitMotor2.angle = -limite;else this.rotationalLimitMotor2.angle = limite;\n\n this.nor.crossVectors(this.ax1, this.ax2).normalize();\n this.tan.crossVectors(this.nor, this.ax2).normalize();\n this.bin.crossVectors(this.nor, this.ax1).normalize();\n\n this.r3.preSolve(timeStep, invTimeStep);\n this.t3.preSolve(timeStep, invTimeStep);\n },\n\n solve: function () {\n\n this.r3.solve();\n this.t3.solve();\n },\n\n postSolve: function () {}\n\n});\n\nfunction JointConfig() {\n\n this.scale = 1;\n this.invScale = 1;\n\n // The first rigid body of the joint.\n this.body1 = null;\n // The second rigid body of the joint.\n this.body2 = null;\n // The anchor point on the first rigid body in local coordinate system.\n this.localAnchorPoint1 = new Vec3();\n // The anchor point on the second rigid body in local coordinate system.\n this.localAnchorPoint2 = new Vec3();\n // The axis in the first body's coordinate system.\n // his property is available in some joints.\n this.localAxis1 = new Vec3();\n // The axis in the second body's coordinate system.\n // This property is available in some joints.\n this.localAxis2 = new Vec3();\n // Whether allow collision between connected rigid bodies or not.\n this.allowCollision = false;\n}\n\n/**\r\n * This class holds mass information of a shape.\r\n * @author lo-th\r\n * @author saharan\r\n */\n\nfunction MassInfo() {\n\n // Mass of the shape.\n this.mass = 0;\n\n // The moment inertia of the shape.\n this.inertia = new Mat33();\n}\n\n/**\n* A link list of contacts.\n* @author saharan\n*/\nfunction ContactLink(contact) {\n\n // The previous contact link.\n this.prev = null;\n // The next contact link.\n this.next = null;\n // The shape of the contact.\n this.shape = null;\n // The other rigid body.\n this.body = null;\n // The contact of the link.\n this.contact = contact;\n}\n\nfunction ImpulseDataBuffer() {\n\n this.lp1X = NaN;\n this.lp1Y = NaN;\n this.lp1Z = NaN;\n this.lp2X = NaN;\n this.lp2Y = NaN;\n this.lp2Z = NaN;\n this.impulse = NaN;\n}\n\n/**\n* The class holds details of the contact point.\n* @author saharan\n*/\n\nfunction ManifoldPoint() {\n\n // Whether this manifold point is persisting or not.\n this.warmStarted = false;\n // The position of this manifold point.\n this.position = new Vec3();\n // The position in the first shape's coordinate.\n this.localPoint1 = new Vec3();\n // The position in the second shape's coordinate.\n this.localPoint2 = new Vec3();\n // The normal vector of this manifold point.\n this.normal = new Vec3();\n // The tangent vector of this manifold point.\n this.tangent = new Vec3();\n // The binormal vector of this manifold point.\n this.binormal = new Vec3();\n // The impulse in normal direction.\n this.normalImpulse = 0;\n // The impulse in tangent direction.\n this.tangentImpulse = 0;\n // The impulse in binormal direction.\n this.binormalImpulse = 0;\n // The denominator in normal direction.\n this.normalDenominator = 0;\n // The denominator in tangent direction.\n this.tangentDenominator = 0;\n // The denominator in binormal direction.\n this.binormalDenominator = 0;\n // The depth of penetration.\n this.penetration = 0;\n}\n\n/**\n* A contact manifold between two shapes.\n* @author saharan\n* @author lo-th\n*/\n\nfunction ContactManifold() {\n\n // The first rigid body.\n this.body1 = null;\n // The second rigid body.\n this.body2 = null;\n // The number of manifold points.\n this.numPoints = 0;\n // The manifold points.\n this.points = [new ManifoldPoint(), new ManifoldPoint(), new ManifoldPoint(), new ManifoldPoint()];\n}\n\nContactManifold.prototype = {\n\n constructor: ContactManifold,\n\n //Reset the manifold.\n reset: function (shape1, shape2) {\n\n this.body1 = shape1.parent;\n this.body2 = shape2.parent;\n this.numPoints = 0;\n },\n\n // Add a point into this manifold.\n addPointVec: function (pos, norm, penetration, flip) {\n\n var p = this.points[this.numPoints++];\n\n p.position.copy(pos);\n p.localPoint1.sub(pos, this.body1.position).applyMatrix3(this.body1.rotation);\n p.localPoint2.sub(pos, this.body2.position).applyMatrix3(this.body2.rotation);\n\n p.normal.copy(norm);\n if (flip) p.normal.negate();\n\n p.normalImpulse = 0;\n p.penetration = penetration;\n p.warmStarted = false;\n },\n\n // Add a point into this manifold.\n addPoint: function (x, y, z, nx, ny, nz, penetration, flip) {\n\n var p = this.points[this.numPoints++];\n\n p.position.set(x, y, z);\n p.localPoint1.sub(p.position, this.body1.position).applyMatrix3(this.body1.rotation);\n p.localPoint2.sub(p.position, this.body2.position).applyMatrix3(this.body2.rotation);\n\n p.normalImpulse = 0;\n\n p.normal.set(nx, ny, nz);\n if (flip) p.normal.negate();\n\n p.penetration = penetration;\n p.warmStarted = false;\n }\n};\n\nfunction ContactPointDataBuffer() {\n\n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n this.norU1 = new Vec3();\n this.tanU1 = new Vec3();\n this.binU1 = new Vec3();\n\n this.norU2 = new Vec3();\n this.tanU2 = new Vec3();\n this.binU2 = new Vec3();\n\n this.norT1 = new Vec3();\n this.tanT1 = new Vec3();\n this.binT1 = new Vec3();\n\n this.norT2 = new Vec3();\n this.tanT2 = new Vec3();\n this.binT2 = new Vec3();\n\n this.norTU1 = new Vec3();\n this.tanTU1 = new Vec3();\n this.binTU1 = new Vec3();\n\n this.norTU2 = new Vec3();\n this.tanTU2 = new Vec3();\n this.binTU2 = new Vec3();\n\n this.norImp = 0;\n this.tanImp = 0;\n this.binImp = 0;\n\n this.norDen = 0;\n this.tanDen = 0;\n this.binDen = 0;\n\n this.norTar = 0;\n\n this.next = null;\n this.last = false;\n}\n\n/**\n* ...\n* @author saharan\n*/\nfunction ContactConstraint(manifold) {\n\n Constraint.call(this);\n // The contact manifold of the constraint.\n this.manifold = manifold;\n // The coefficient of restitution of the constraint.\n this.restitution = NaN;\n // The coefficient of friction of the constraint.\n this.friction = NaN;\n this.p1 = null;\n this.p2 = null;\n this.lv1 = null;\n this.lv2 = null;\n this.av1 = null;\n this.av2 = null;\n this.i1 = null;\n this.i2 = null;\n\n //this.ii1 = null;\n //this.ii2 = null;\n\n this.tmp = new Vec3();\n this.tmpC1 = new Vec3();\n this.tmpC2 = new Vec3();\n\n this.tmpP1 = new Vec3();\n this.tmpP2 = new Vec3();\n\n this.tmplv1 = new Vec3();\n this.tmplv2 = new Vec3();\n this.tmpav1 = new Vec3();\n this.tmpav2 = new Vec3();\n\n this.m1 = NaN;\n this.m2 = NaN;\n this.num = 0;\n\n this.ps = manifold.points;\n this.cs = new ContactPointDataBuffer();\n this.cs.next = new ContactPointDataBuffer();\n this.cs.next.next = new ContactPointDataBuffer();\n this.cs.next.next.next = new ContactPointDataBuffer();\n}\n\nContactConstraint.prototype = Object.assign(Object.create(Constraint.prototype), {\n\n constructor: ContactConstraint,\n\n // Attach the constraint to the bodies.\n attach: function () {\n\n this.p1 = this.body1.position;\n this.p2 = this.body2.position;\n this.lv1 = this.body1.linearVelocity;\n this.av1 = this.body1.angularVelocity;\n this.lv2 = this.body2.linearVelocity;\n this.av2 = this.body2.angularVelocity;\n this.i1 = this.body1.inverseInertia;\n this.i2 = this.body2.inverseInertia;\n },\n\n // Detach the constraint from the bodies.\n detach: function () {\n\n this.p1 = null;\n this.p2 = null;\n this.lv1 = null;\n this.lv2 = null;\n this.av1 = null;\n this.av2 = null;\n this.i1 = null;\n this.i2 = null;\n },\n\n preSolve: function (timeStep, invTimeStep) {\n\n this.m1 = this.body1.inverseMass;\n this.m2 = this.body2.inverseMass;\n\n var m1m2 = this.m1 + this.m2;\n\n this.num = this.manifold.numPoints;\n\n var c = this.cs;\n var p, rvn, len, norImp, norTar, sepV, i1, i2;\n\n for (var i = 0; i < this.num; i++) {\n\n p = this.ps[i];\n\n this.tmpP1.sub(p.position, this.p1);\n this.tmpP2.sub(p.position, this.p2);\n\n this.tmpC1.crossVectors(this.av1, this.tmpP1);\n this.tmpC2.crossVectors(this.av2, this.tmpP2);\n\n c.norImp = p.normalImpulse;\n c.tanImp = p.tangentImpulse;\n c.binImp = p.binormalImpulse;\n\n c.nor.copy(p.normal);\n\n 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));\n\n rvn = _Math.dotVectors(c.nor, this.tmp);\n\n c.tan.set(this.tmp.x - rvn * c.nor.x, this.tmp.y - rvn * c.nor.y, this.tmp.z - rvn * c.nor.z);\n\n len = _Math.dotVectors(c.tan, c.tan);\n\n if (len <= 0.04) {\n c.tan.tangent(c.nor);\n }\n\n c.tan.normalize();\n\n c.bin.crossVectors(c.nor, c.tan);\n\n c.norU1.scale(c.nor, this.m1);\n c.norU2.scale(c.nor, this.m2);\n\n c.tanU1.scale(c.tan, this.m1);\n c.tanU2.scale(c.tan, this.m2);\n\n c.binU1.scale(c.bin, this.m1);\n c.binU2.scale(c.bin, this.m2);\n\n c.norT1.crossVectors(this.tmpP1, c.nor);\n c.tanT1.crossVectors(this.tmpP1, c.tan);\n c.binT1.crossVectors(this.tmpP1, c.bin);\n\n c.norT2.crossVectors(this.tmpP2, c.nor);\n c.tanT2.crossVectors(this.tmpP2, c.tan);\n c.binT2.crossVectors(this.tmpP2, c.bin);\n\n i1 = this.i1;\n i2 = this.i2;\n\n c.norTU1.copy(c.norT1).applyMatrix3(i1, true);\n c.tanTU1.copy(c.tanT1).applyMatrix3(i1, true);\n c.binTU1.copy(c.binT1).applyMatrix3(i1, true);\n\n c.norTU2.copy(c.norT2).applyMatrix3(i2, true);\n c.tanTU2.copy(c.tanT2).applyMatrix3(i2, true);\n c.binTU2.copy(c.binT2).applyMatrix3(i2, true);\n\n /*c.norTU1.mulMat( this.i1, c.norT1 );\n c.tanTU1.mulMat( this.i1, c.tanT1 );\n c.binTU1.mulMat( this.i1, c.binT1 );\n c.norTU2.mulMat( this.i2, c.norT2 );\n c.tanTU2.mulMat( this.i2, c.tanT2 );\n c.binTU2.mulMat( this.i2, c.binT2 );*/\n\n this.tmpC1.crossVectors(c.norTU1, this.tmpP1);\n this.tmpC2.crossVectors(c.norTU2, this.tmpP2);\n this.tmp.add(this.tmpC1, this.tmpC2);\n c.norDen = 1 / (m1m2 + _Math.dotVectors(c.nor, this.tmp));\n\n this.tmpC1.crossVectors(c.tanTU1, this.tmpP1);\n this.tmpC2.crossVectors(c.tanTU2, this.tmpP2);\n this.tmp.add(this.tmpC1, this.tmpC2);\n c.tanDen = 1 / (m1m2 + _Math.dotVectors(c.tan, this.tmp));\n\n this.tmpC1.crossVectors(c.binTU1, this.tmpP1);\n this.tmpC2.crossVectors(c.binTU2, this.tmpP2);\n this.tmp.add(this.tmpC1, this.tmpC2);\n c.binDen = 1 / (m1m2 + _Math.dotVectors(c.bin, this.tmp));\n\n if (p.warmStarted) {\n\n norImp = p.normalImpulse;\n\n this.lv1.addScaledVector(c.norU1, norImp);\n this.av1.addScaledVector(c.norTU1, norImp);\n\n this.lv2.subScaledVector(c.norU2, norImp);\n this.av2.subScaledVector(c.norTU2, norImp);\n\n c.norImp = norImp;\n c.tanImp = 0;\n c.binImp = 0;\n rvn = 0; // disable bouncing\n } else {\n\n c.norImp = 0;\n c.tanImp = 0;\n c.binImp = 0;\n }\n\n if (rvn > -1) rvn = 0; // disable bouncing\n\n norTar = this.restitution * -rvn;\n sepV = -(p.penetration + 0.005) * invTimeStep * 0.05; // allow 0.5cm error\n if (norTar < sepV) norTar = sepV;\n c.norTar = norTar;\n c.last = i == this.num - 1;\n c = c.next;\n }\n },\n\n solve: function () {\n\n this.tmplv1.copy(this.lv1);\n this.tmplv2.copy(this.lv2);\n this.tmpav1.copy(this.av1);\n this.tmpav2.copy(this.av2);\n\n var oldImp1, newImp1, oldImp2, newImp2, rvn, norImp, tanImp, binImp, max, len;\n\n var c = this.cs;\n\n while (true) {\n\n norImp = c.norImp;\n tanImp = c.tanImp;\n binImp = c.binImp;\n max = -norImp * this.friction;\n\n this.tmp.sub(this.tmplv2, this.tmplv1);\n\n rvn = _Math.dotVectors(this.tmp, c.tan) + _Math.dotVectors(this.tmpav2, c.tanT2) - _Math.dotVectors(this.tmpav1, c.tanT1);\n\n oldImp1 = tanImp;\n newImp1 = rvn * c.tanDen;\n tanImp += newImp1;\n\n rvn = _Math.dotVectors(this.tmp, c.bin) + _Math.dotVectors(this.tmpav2, c.binT2) - _Math.dotVectors(this.tmpav1, c.binT1);\n\n oldImp2 = binImp;\n newImp2 = rvn * c.binDen;\n binImp += newImp2;\n\n // cone friction clamp\n len = tanImp * tanImp + binImp * binImp;\n if (len > max * max) {\n len = max / _Math.sqrt(len);\n tanImp *= len;\n binImp *= len;\n }\n\n newImp1 = tanImp - oldImp1;\n newImp2 = binImp - oldImp2;\n\n //\n\n 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);\n\n this.tmplv1.addEqual(this.tmp);\n\n 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);\n\n this.tmpav1.addEqual(this.tmp);\n\n 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);\n\n this.tmplv2.subEqual(this.tmp);\n\n 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);\n\n this.tmpav2.subEqual(this.tmp);\n\n // restitution part\n\n this.tmp.sub(this.tmplv2, this.tmplv1);\n\n rvn = _Math.dotVectors(this.tmp, c.nor) + _Math.dotVectors(this.tmpav2, c.norT2) - _Math.dotVectors(this.tmpav1, c.norT1);\n\n oldImp1 = norImp;\n newImp1 = (rvn - c.norTar) * c.norDen;\n norImp += newImp1;\n if (norImp > 0) norImp = 0;\n\n newImp1 = norImp - oldImp1;\n\n this.tmplv1.addScaledVector(c.norU1, newImp1);\n this.tmpav1.addScaledVector(c.norTU1, newImp1);\n this.tmplv2.subScaledVector(c.norU2, newImp1);\n this.tmpav2.subScaledVector(c.norTU2, newImp1);\n\n c.norImp = norImp;\n c.tanImp = tanImp;\n c.binImp = binImp;\n\n if (c.last) break;\n c = c.next;\n }\n\n this.lv1.copy(this.tmplv1);\n this.lv2.copy(this.tmplv2);\n this.av1.copy(this.tmpav1);\n this.av2.copy(this.tmpav2);\n },\n\n postSolve: function () {\n\n var c = this.cs,\n p;\n var i = this.num;\n while (i--) {\n //for(var i=0;i -1) {\n this.proxies.splice(n, 1);\n //this.numProxies--;\n }\n\n /*var i = this.numProxies;\r\n while(i--){\r\n //for(var i=0, l=this.numProxies;i> 1;\n //this.numPairChecks=this.numProxies*(this.numProxies-1)*0.5;\n\n while (i < l) {\n p1 = px[i++];\n j = i + 1;\n while (j < l) {\n p2 = px[j++];\n if (p1.aabb.intersectTest(p2.aabb) || !this.isAvailablePair(p1.shape, p2.shape)) continue;\n this.addPair(p1.shape, p2.shape);\n }\n }\n }\n\n});\n\n/**\r\n * A projection axis for sweep and prune broad-phase.\r\n * @author saharan\r\n */\n\nfunction SAPAxis() {\n\n this.numElements = 0;\n this.bufferSize = 256;\n this.elements = [];\n this.elements.length = this.bufferSize;\n this.stack = new Float32Array(64);\n}\n\nObject.assign(SAPAxis.prototype, {\n\n SAPAxis: true,\n\n addElements: function (min, max) {\n\n if (this.numElements + 2 >= this.bufferSize) {\n //this.bufferSize<<=1;\n this.bufferSize *= 2;\n var newElements = [];\n var i = this.numElements;\n while (i--) {\n //for(var i=0, l=this.numElements; i> threshold != 0) threshold++;\n threshold = threshold * this.numElements >> 2;\n count = 0;\n\n var giveup = false;\n var elements = this.elements;\n for (var i = 1, l = this.numElements; i < l; i++) {\n // try insertion sort\n var tmp = elements[i];\n var pivot = tmp.value;\n var tmp2 = elements[i - 1];\n if (tmp2.value > pivot) {\n var j = i;\n do {\n elements[j] = tmp2;\n if (--j == 0) break;\n tmp2 = elements[j - 1];\n } while (tmp2.value > pivot);\n elements[j] = tmp;\n count += i - j;\n if (count > threshold) {\n giveup = true; // stop and use quick sort\n break;\n }\n }\n }\n if (!giveup) return;\n count = 2;var stack = this.stack;\n stack[0] = 0;\n stack[1] = this.numElements - 1;\n while (count > 0) {\n var right = stack[--count];\n var left = stack[--count];\n var diff = right - left;\n if (diff > 16) {\n // quick sort\n //var mid=left+(diff>>1);\n var mid = left + _Math.floor(diff * 0.5);\n tmp = elements[mid];\n elements[mid] = elements[right];\n elements[right] = tmp;\n pivot = tmp.value;\n i = left - 1;\n j = right;\n while (true) {\n var ei;\n var ej;\n do {\n ei = elements[++i];\n } while (ei.value < pivot);\n do {\n ej = elements[--j];\n } while (pivot < ej.value && j != left);\n if (i >= j) break;\n elements[i] = ej;\n elements[j] = ei;\n }\n\n elements[right] = elements[i];\n elements[i] = tmp;\n if (i - left > right - i) {\n stack[count++] = left;\n stack[count++] = i - 1;\n stack[count++] = i + 1;\n stack[count++] = right;\n } else {\n stack[count++] = i + 1;\n stack[count++] = right;\n stack[count++] = left;\n stack[count++] = i - 1;\n }\n } else {\n for (i = left + 1; i <= right; i++) {\n tmp = elements[i];\n pivot = tmp.value;\n tmp2 = elements[i - 1];\n if (tmp2.value > pivot) {\n j = i;\n do {\n elements[j] = tmp2;\n if (--j == 0) break;\n tmp2 = elements[j - 1];\n } while (tmp2.value > pivot);\n elements[j] = tmp;\n }\n }\n }\n }\n },\n\n calculateTestCount: function () {\n\n var num = 1;\n var sum = 0;\n for (var i = 1, l = this.numElements; i < l; i++) {\n if (this.elements[i].max) {\n num--;\n } else {\n sum += num;\n num++;\n }\n }\n return sum;\n }\n\n});\n\n/**\r\n * An element of proxies.\r\n * @author saharan\r\n */\n\nfunction SAPElement(proxy, max) {\n\n // The parent proxy\n this.proxy = proxy;\n // The pair element.\n this.pair = null;\n // The minimum element on other axis.\n this.min1 = null;\n // The maximum element on other axis.\n this.max1 = null;\n // The minimum element on other axis.\n this.min2 = null;\n // The maximum element on other axis.\n this.max2 = null;\n // Whether the element has maximum value or not.\n this.max = max;\n // The value of the element.\n this.value = 0;\n}\n\n/**\r\n * A proxy for sweep and prune broad-phase.\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction SAPProxy(sap, shape) {\n\n Proxy.call(this, shape);\n // Type of the axis to which the proxy belongs to. [0:none, 1:dynamic, 2:static]\n this.belongsTo = 0;\n // The maximum elements on each axis.\n this.max = [];\n // The minimum elements on each axis.\n this.min = [];\n\n this.sap = sap;\n this.min[0] = new SAPElement(this, false);\n this.max[0] = new SAPElement(this, true);\n this.min[1] = new SAPElement(this, false);\n this.max[1] = new SAPElement(this, true);\n this.min[2] = new SAPElement(this, false);\n this.max[2] = new SAPElement(this, true);\n this.max[0].pair = this.min[0];\n this.max[1].pair = this.min[1];\n this.max[2].pair = this.min[2];\n this.min[0].min1 = this.min[1];\n this.min[0].max1 = this.max[1];\n this.min[0].min2 = this.min[2];\n this.min[0].max2 = this.max[2];\n this.min[1].min1 = this.min[0];\n this.min[1].max1 = this.max[0];\n this.min[1].min2 = this.min[2];\n this.min[1].max2 = this.max[2];\n this.min[2].min1 = this.min[0];\n this.min[2].max1 = this.max[0];\n this.min[2].min2 = this.min[1];\n this.min[2].max2 = this.max[1];\n}\n\nSAPProxy.prototype = Object.assign(Object.create(Proxy.prototype), {\n\n constructor: SAPProxy,\n\n // Returns whether the proxy is dynamic or not.\n isDynamic: function () {\n\n var body = this.shape.parent;\n return body.isDynamic && !body.sleeping;\n },\n\n update: function () {\n\n var te = this.aabb.elements;\n this.min[0].value = te[0];\n this.min[1].value = te[1];\n this.min[2].value = te[2];\n this.max[0].value = te[3];\n this.max[1].value = te[4];\n this.max[2].value = te[5];\n\n if (this.belongsTo == 1 && !this.isDynamic() || this.belongsTo == 2 && this.isDynamic()) {\n this.sap.removeProxy(this);\n this.sap.addProxy(this);\n }\n }\n\n});\n\n/**\r\n * A broad-phase collision detection algorithm using sweep and prune.\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction SAPBroadPhase() {\n\n BroadPhase.call(this);\n this.types = BR_SWEEP_AND_PRUNE;\n\n this.numElementsD = 0;\n this.numElementsS = 0;\n // dynamic proxies\n this.axesD = [new SAPAxis(), new SAPAxis(), new SAPAxis()];\n // static or sleeping proxies\n this.axesS = [new SAPAxis(), new SAPAxis(), new SAPAxis()];\n\n this.index1 = 0;\n this.index2 = 1;\n}\n\nSAPBroadPhase.prototype = Object.assign(Object.create(BroadPhase.prototype), {\n\n constructor: SAPBroadPhase,\n\n createProxy: function (shape) {\n\n return new SAPProxy(this, shape);\n },\n\n addProxy: function (proxy) {\n\n var p = proxy;\n if (p.isDynamic()) {\n this.axesD[0].addElements(p.min[0], p.max[0]);\n this.axesD[1].addElements(p.min[1], p.max[1]);\n this.axesD[2].addElements(p.min[2], p.max[2]);\n p.belongsTo = 1;\n this.numElementsD += 2;\n } else {\n this.axesS[0].addElements(p.min[0], p.max[0]);\n this.axesS[1].addElements(p.min[1], p.max[1]);\n this.axesS[2].addElements(p.min[2], p.max[2]);\n p.belongsTo = 2;\n this.numElementsS += 2;\n }\n },\n\n removeProxy: function (proxy) {\n\n var p = proxy;\n if (p.belongsTo == 0) return;\n\n /*else if ( p.belongsTo == 1 ) {\r\n this.axesD[0].removeElements( p.min[0], p.max[0] );\r\n this.axesD[1].removeElements( p.min[1], p.max[1] );\r\n this.axesD[2].removeElements( p.min[2], p.max[2] );\r\n this.numElementsD -= 2;\r\n } else if ( p.belongsTo == 2 ) {\r\n this.axesS[0].removeElements( p.min[0], p.max[0] );\r\n this.axesS[1].removeElements( p.min[1], p.max[1] );\r\n this.axesS[2].removeElements( p.min[2], p.max[2] );\r\n this.numElementsS -= 2;\r\n }*/\n\n switch (p.belongsTo) {\n case 1:\n this.axesD[0].removeElements(p.min[0], p.max[0]);\n this.axesD[1].removeElements(p.min[1], p.max[1]);\n this.axesD[2].removeElements(p.min[2], p.max[2]);\n this.numElementsD -= 2;\n break;\n case 2:\n this.axesS[0].removeElements(p.min[0], p.max[0]);\n this.axesS[1].removeElements(p.min[1], p.max[1]);\n this.axesS[2].removeElements(p.min[2], p.max[2]);\n this.numElementsS -= 2;\n break;\n }\n\n p.belongsTo = 0;\n },\n\n collectPairs: function () {\n\n if (this.numElementsD == 0) return;\n\n var axis1 = this.axesD[this.index1];\n var axis2 = this.axesD[this.index2];\n\n axis1.sort();\n axis2.sort();\n\n var count1 = axis1.calculateTestCount();\n var count2 = axis2.calculateTestCount();\n var elementsD;\n var elementsS;\n if (count1 <= count2) {\n // select the best axis\n axis2 = this.axesS[this.index1];\n axis2.sort();\n elementsD = axis1.elements;\n elementsS = axis2.elements;\n } else {\n axis1 = this.axesS[this.index2];\n axis1.sort();\n elementsD = axis2.elements;\n elementsS = axis1.elements;\n this.index1 ^= this.index2;\n this.index2 ^= this.index1;\n this.index1 ^= this.index2;\n }\n var activeD;\n var activeS;\n var p = 0;\n var q = 0;\n while (p < this.numElementsD) {\n var e1;\n var dyn;\n if (q == this.numElementsS) {\n e1 = elementsD[p];\n dyn = true;\n p++;\n } else {\n var d = elementsD[p];\n var s = elementsS[q];\n if (d.value < s.value) {\n e1 = d;\n dyn = true;\n p++;\n } else {\n e1 = s;\n dyn = false;\n q++;\n }\n }\n if (!e1.max) {\n var s1 = e1.proxy.shape;\n var min1 = e1.min1.value;\n var max1 = e1.max1.value;\n var min2 = e1.min2.value;\n var max2 = e1.max2.value;\n\n for (var e2 = activeD; e2 != null; e2 = e2.pair) {\n // test for dynamic\n var s2 = e2.proxy.shape;\n\n this.numPairChecks++;\n if (min1 > e2.max1.value || max1 < e2.min1.value || min2 > e2.max2.value || max2 < e2.min2.value || !this.isAvailablePair(s1, s2)) continue;\n this.addPair(s1, s2);\n }\n if (dyn) {\n for (e2 = activeS; e2 != null; e2 = e2.pair) {\n // test for static\n s2 = e2.proxy.shape;\n\n this.numPairChecks++;\n\n if (min1 > e2.max1.value || max1 < e2.min1.value || min2 > e2.max2.value || max2 < e2.min2.value || !this.isAvailablePair(s1, s2)) continue;\n this.addPair(s1, s2);\n }\n e1.pair = activeD;\n activeD = e1;\n } else {\n e1.pair = activeS;\n activeS = e1;\n }\n } else {\n var min = e1.pair;\n if (dyn) {\n if (min == activeD) {\n activeD = activeD.pair;\n continue;\n } else {\n e1 = activeD;\n }\n } else {\n if (min == activeS) {\n activeS = activeS.pair;\n continue;\n } else {\n e1 = activeS;\n }\n }\n do {\n e2 = e1.pair;\n if (e2 == min) {\n e1.pair = e2.pair;\n break;\n }\n e1 = e2;\n } while (e1 != null);\n }\n }\n this.index2 = (this.index1 | this.index2) ^ 3;\n }\n\n});\n\n/**\r\n* A node of the dynamic bounding volume tree.\r\n* @author saharan\r\n*/\n\nfunction DBVTNode() {\n\n // The first child node of this node.\n this.child1 = null;\n // The second child node of this node.\n this.child2 = null;\n // The parent node of this tree.\n this.parent = null;\n // The proxy of this node. This has no value if this node is not leaf.\n this.proxy = null;\n // The maximum distance from leaf nodes.\n this.height = 0;\n // The AABB of this node.\n this.aabb = new AABB();\n}\n\n/**\r\n * A dynamic bounding volume tree for the broad-phase algorithm.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction DBVT() {\n\n // The root of the tree.\n this.root = null;\n this.freeNodes = [];\n this.freeNodes.length = 16384;\n this.numFreeNodes = 0;\n this.aabb = new AABB();\n}\n\nObject.assign(DBVT.prototype, {\n\n DBVT: true,\n\n moveLeaf: function (leaf) {\n\n this.deleteLeaf(leaf);\n this.insertLeaf(leaf);\n },\n\n insertLeaf: function (leaf) {\n\n if (this.root == null) {\n this.root = leaf;\n return;\n }\n var lb = leaf.aabb;\n var sibling = this.root;\n var oldArea;\n var newArea;\n while (sibling.proxy == null) {\n // descend the node to search the best pair\n var c1 = sibling.child1;\n var c2 = sibling.child2;\n var b = sibling.aabb;\n var c1b = c1.aabb;\n var c2b = c2.aabb;\n oldArea = b.surfaceArea();\n this.aabb.combine(lb, b);\n newArea = this.aabb.surfaceArea();\n var creatingCost = newArea * 2;\n var incrementalCost = (newArea - oldArea) * 2; // cost of creating a new pair with the node\n var discendingCost1 = incrementalCost;\n this.aabb.combine(lb, c1b);\n if (c1.proxy != null) {\n // leaf cost = area(combined aabb)\n discendingCost1 += this.aabb.surfaceArea();\n } else {\n // node cost = area(combined aabb) - area(old aabb)\n discendingCost1 += this.aabb.surfaceArea() - c1b.surfaceArea();\n }\n var discendingCost2 = incrementalCost;\n this.aabb.combine(lb, c2b);\n if (c2.proxy != null) {\n // leaf cost = area(combined aabb)\n discendingCost2 += this.aabb.surfaceArea();\n } else {\n // node cost = area(combined aabb) - area(old aabb)\n discendingCost2 += this.aabb.surfaceArea() - c2b.surfaceArea();\n }\n if (discendingCost1 < discendingCost2) {\n if (creatingCost < discendingCost1) {\n break; // stop descending\n } else {\n sibling = c1; // descend into first child\n }\n } else {\n if (creatingCost < discendingCost2) {\n break; // stop descending\n } else {\n sibling = c2; // descend into second child\n }\n }\n }\n var oldParent = sibling.parent;\n var newParent;\n if (this.numFreeNodes > 0) {\n newParent = this.freeNodes[--this.numFreeNodes];\n } else {\n newParent = new DBVTNode();\n }\n\n newParent.parent = oldParent;\n newParent.child1 = leaf;\n newParent.child2 = sibling;\n newParent.aabb.combine(leaf.aabb, sibling.aabb);\n newParent.height = sibling.height + 1;\n sibling.parent = newParent;\n leaf.parent = newParent;\n if (sibling == this.root) {\n // replace root\n this.root = newParent;\n } else {\n // replace child\n if (oldParent.child1 == sibling) {\n oldParent.child1 = newParent;\n } else {\n oldParent.child2 = newParent;\n }\n }\n // update whole tree\n do {\n newParent = this.balance(newParent);\n this.fix(newParent);\n newParent = newParent.parent;\n } while (newParent != null);\n },\n\n getBalance: function (node) {\n\n if (node.proxy != null) return 0;\n return node.child1.height - node.child2.height;\n },\n\n deleteLeaf: function (leaf) {\n\n if (leaf == this.root) {\n this.root = null;\n return;\n }\n var parent = leaf.parent;\n var sibling;\n if (parent.child1 == leaf) {\n sibling = parent.child2;\n } else {\n sibling = parent.child1;\n }\n if (parent == this.root) {\n this.root = sibling;\n sibling.parent = null;\n return;\n }\n var grandParent = parent.parent;\n sibling.parent = grandParent;\n if (grandParent.child1 == parent) {\n grandParent.child1 = sibling;\n } else {\n grandParent.child2 = sibling;\n }\n if (this.numFreeNodes < 16384) {\n this.freeNodes[this.numFreeNodes++] = parent;\n }\n do {\n grandParent = this.balance(grandParent);\n this.fix(grandParent);\n grandParent = grandParent.parent;\n } while (grandParent != null);\n },\n\n balance: function (node) {\n\n var nh = node.height;\n if (nh < 2) {\n return node;\n }\n var p = node.parent;\n var l = node.child1;\n var r = node.child2;\n var lh = l.height;\n var rh = r.height;\n var balance = lh - rh;\n var t; // for bit operation\n\n // [ N ]\n // / \\\n // [ L ] [ R ]\n // / \\ / \\\n // [L-L] [L-R] [R-L] [R-R]\n\n // Is the tree balanced?\n if (balance > 1) {\n var ll = l.child1;\n var lr = l.child2;\n var llh = ll.height;\n var lrh = lr.height;\n\n // Is L-L higher than L-R?\n if (llh > lrh) {\n // set N to L-R\n l.child2 = node;\n node.parent = l;\n\n // [ L ]\n // / \\\n // [L-L] [ N ]\n // / \\ / \\\n // [...] [...] [ L ] [ R ]\n\n // set L-R\n node.child1 = lr;\n lr.parent = node;\n\n // [ L ]\n // / \\\n // [L-L] [ N ]\n // / \\ / \\\n // [...] [...] [L-R] [ R ]\n\n // fix bounds and heights\n node.aabb.combine(lr.aabb, r.aabb);\n t = lrh - rh;\n node.height = lrh - (t & t >> 31) + 1;\n l.aabb.combine(ll.aabb, node.aabb);\n t = llh - nh;\n l.height = llh - (t & t >> 31) + 1;\n } else {\n // set N to L-L\n l.child1 = node;\n node.parent = l;\n\n // [ L ]\n // / \\\n // [ N ] [L-R]\n // / \\ / \\\n // [ L ] [ R ] [...] [...]\n\n // set L-L\n node.child1 = ll;\n ll.parent = node;\n\n // [ L ]\n // / \\\n // [ N ] [L-R]\n // / \\ / \\\n // [L-L] [ R ] [...] [...]\n\n // fix bounds and heights\n node.aabb.combine(ll.aabb, r.aabb);\n t = llh - rh;\n node.height = llh - (t & t >> 31) + 1;\n\n l.aabb.combine(node.aabb, lr.aabb);\n t = nh - lrh;\n l.height = nh - (t & t >> 31) + 1;\n }\n // set new parent of L\n if (p != null) {\n if (p.child1 == node) {\n p.child1 = l;\n } else {\n p.child2 = l;\n }\n } else {\n this.root = l;\n }\n l.parent = p;\n return l;\n } else if (balance < -1) {\n var rl = r.child1;\n var rr = r.child2;\n var rlh = rl.height;\n var rrh = rr.height;\n\n // Is R-L higher than R-R?\n if (rlh > rrh) {\n // set N to R-R\n r.child2 = node;\n node.parent = r;\n\n // [ R ]\n // / \\\n // [R-L] [ N ]\n // / \\ / \\\n // [...] [...] [ L ] [ R ]\n\n // set R-R\n node.child2 = rr;\n rr.parent = node;\n\n // [ R ]\n // / \\\n // [R-L] [ N ]\n // / \\ / \\\n // [...] [...] [ L ] [R-R]\n\n // fix bounds and heights\n node.aabb.combine(l.aabb, rr.aabb);\n t = lh - rrh;\n node.height = lh - (t & t >> 31) + 1;\n r.aabb.combine(rl.aabb, node.aabb);\n t = rlh - nh;\n r.height = rlh - (t & t >> 31) + 1;\n } else {\n // set N to R-L\n r.child1 = node;\n node.parent = r;\n // [ R ]\n // / \\\n // [ N ] [R-R]\n // / \\ / \\\n // [ L ] [ R ] [...] [...]\n\n // set R-L\n node.child2 = rl;\n rl.parent = node;\n\n // [ R ]\n // / \\\n // [ N ] [R-R]\n // / \\ / \\\n // [ L ] [R-L] [...] [...]\n\n // fix bounds and heights\n node.aabb.combine(l.aabb, rl.aabb);\n t = lh - rlh;\n node.height = lh - (t & t >> 31) + 1;\n r.aabb.combine(node.aabb, rr.aabb);\n t = nh - rrh;\n r.height = nh - (t & t >> 31) + 1;\n }\n // set new parent of R\n if (p != null) {\n if (p.child1 == node) {\n p.child1 = r;\n } else {\n p.child2 = r;\n }\n } else {\n this.root = r;\n }\n r.parent = p;\n return r;\n }\n return node;\n },\n\n fix: function (node) {\n\n var c1 = node.child1;\n var c2 = node.child2;\n node.aabb.combine(c1.aabb, c2.aabb);\n node.height = c1.height < c2.height ? c2.height + 1 : c1.height + 1;\n }\n\n});\n\n/**\r\n* A proxy for dynamic bounding volume tree broad-phase.\r\n* @author saharan\r\n*/\n\nfunction DBVTProxy(shape) {\n\n Proxy.call(this, shape);\n // The leaf of the proxy.\n this.leaf = new DBVTNode();\n this.leaf.proxy = this;\n}\n\nDBVTProxy.prototype = Object.assign(Object.create(Proxy.prototype), {\n\n constructor: DBVTProxy,\n\n update: function () {}\n\n});\n\n/**\r\n * A broad-phase algorithm using dynamic bounding volume tree.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\n\nfunction DBVTBroadPhase() {\n\n BroadPhase.call(this);\n\n this.types = BR_BOUNDING_VOLUME_TREE;\n\n this.tree = new DBVT();\n this.stack = [];\n this.leaves = [];\n this.numLeaves = 0;\n}\n\nDBVTBroadPhase.prototype = Object.assign(Object.create(BroadPhase.prototype), {\n\n constructor: DBVTBroadPhase,\n\n createProxy: function (shape) {\n\n return new DBVTProxy(shape);\n },\n\n addProxy: function (proxy) {\n\n this.tree.insertLeaf(proxy.leaf);\n this.leaves.push(proxy.leaf);\n this.numLeaves++;\n },\n\n removeProxy: function (proxy) {\n\n this.tree.deleteLeaf(proxy.leaf);\n var n = this.leaves.indexOf(proxy.leaf);\n if (n > -1) {\n this.leaves.splice(n, 1);\n this.numLeaves--;\n }\n },\n\n collectPairs: function () {\n\n if (this.numLeaves < 2) return;\n\n var leaf,\n margin = 0.1,\n i = this.numLeaves;\n\n while (i--) {\n\n leaf = this.leaves[i];\n\n if (leaf.proxy.aabb.intersectTestTwo(leaf.aabb)) {\n\n leaf.aabb.copy(leaf.proxy.aabb, margin);\n this.tree.deleteLeaf(leaf);\n this.tree.insertLeaf(leaf);\n this.collide(leaf, this.tree.root);\n }\n }\n },\n\n collide: function (node1, node2) {\n\n var stackCount = 2;\n var s1, s2, n1, n2, l1, l2;\n this.stack[0] = node1;\n this.stack[1] = node2;\n\n while (stackCount > 0) {\n\n n1 = this.stack[--stackCount];\n n2 = this.stack[--stackCount];\n l1 = n1.proxy != null;\n l2 = n2.proxy != null;\n\n this.numPairChecks++;\n\n if (l1 && l2) {\n s1 = n1.proxy.shape;\n s2 = n2.proxy.shape;\n if (s1 == s2 || s1.aabb.intersectTest(s2.aabb) || !this.isAvailablePair(s1, s2)) continue;\n\n this.addPair(s1, s2);\n } else {\n\n if (n1.aabb.intersectTest(n2.aabb)) continue;\n\n /*if(stackCount+4>=this.maxStack){// expand the stack\r\n //this.maxStack<<=1;\r\n this.maxStack*=2;\r\n var newStack = [];// vector\r\n newStack.length = this.maxStack;\r\n for(var i=0;i n2.aabb.surfaceArea()) {\n this.stack[stackCount++] = n1.child1;\n this.stack[stackCount++] = n2;\n this.stack[stackCount++] = n1.child2;\n this.stack[stackCount++] = n2;\n } else {\n this.stack[stackCount++] = n1;\n this.stack[stackCount++] = n2.child1;\n this.stack[stackCount++] = n1;\n this.stack[stackCount++] = n2.child2;\n }\n }\n }\n }\n\n});\n\nfunction CollisionDetector() {\n\n this.flip = false;\n}\n\nObject.assign(CollisionDetector.prototype, {\n\n CollisionDetector: true,\n\n detectCollision: function (shape1, shape2, manifold) {\n\n printError(\"CollisionDetector\", \"Inheritance error.\");\n }\n\n});\n\n/**\r\n * A collision detector which detects collisions between two boxes.\r\n * @author saharan\r\n */\nfunction BoxBoxCollisionDetector() {\n\n CollisionDetector.call(this);\n this.clipVertices1 = new Float32Array(24); // 8 vertices x,y,z\n this.clipVertices2 = new Float32Array(24);\n this.used = new Float32Array(8);\n\n this.INF = 1 / 0;\n}\n\nBoxBoxCollisionDetector.prototype = Object.assign(Object.create(CollisionDetector.prototype), {\n\n constructor: BoxBoxCollisionDetector,\n\n detectCollision: function (shape1, shape2, manifold) {\n // What you are doing \n // · I to prepare a separate axis of the fifteen \n //-Six in each of three normal vectors of the xyz direction of the box both \n // · Remaining nine 3x3 a vector perpendicular to the side of the box 2 and the side of the box 1 \n // · Calculate the depth to the separation axis \n\n // Calculates the distance using the inner product and put the amount of embedment \n // · However a vertical separation axis and side to weight a little to avoid vibration \n // And end when there is a separate axis that is remote even one \n // · I look for separation axis with little to dent most \n // Men and if separation axis of the first six - end collision \n // Heng If it separate axis of nine other - side collision \n // Heng - case of a side collision \n // · Find points of two sides on which you made ​​the separation axis \n\n // Calculates the point of closest approach of a straight line consisting of separate axis points obtained, and the collision point \n //-Surface - the case of the plane crash \n //-Box A, box B and the other a box of better made ​​a separate axis \n // • 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 \n\n // When viewed from the front surface A, and the cut part exceeding the area of the surface A is a surface B \n //-Plane B becomes the 3-8 triangle, I a candidate for the collision point the vertex of surface B \n // • If more than one candidate 5 exists, scraping up to four \n\n // For potential collision points of all, to examine the distance between the surface A \n // • If you were on the inside surface of A, and the collision point\n\n var b1;\n var b2;\n if (shape1.id < shape2.id) {\n b1 = shape1;\n b2 = shape2;\n } else {\n b1 = shape2;\n b2 = shape1;\n }\n var V1 = b1.elements;\n var V2 = b2.elements;\n\n var D1 = b1.dimentions;\n var D2 = b2.dimentions;\n\n var p1 = b1.position;\n var p2 = b2.position;\n var p1x = p1.x;\n var p1y = p1.y;\n var p1z = p1.z;\n var p2x = p2.x;\n var p2y = p2.y;\n var p2z = p2.z;\n // diff\n var dx = p2x - p1x;\n var dy = p2y - p1y;\n var dz = p2z - p1z;\n // distance\n var w1 = b1.halfWidth;\n var h1 = b1.halfHeight;\n var d1 = b1.halfDepth;\n var w2 = b2.halfWidth;\n var h2 = b2.halfHeight;\n var d2 = b2.halfDepth;\n // direction\n\n // ----------------------------\n // 15 separating axes\n // 1~6: face\n // 7~f: edge\n // http://marupeke296.com/COL_3D_No13_OBBvsOBB.html\n // ----------------------------\n\n var a1x = D1[0];\n var a1y = D1[1];\n var a1z = D1[2];\n var a2x = D1[3];\n var a2y = D1[4];\n var a2z = D1[5];\n var a3x = D1[6];\n var a3y = D1[7];\n var a3z = D1[8];\n var d1x = D1[9];\n var d1y = D1[10];\n var d1z = D1[11];\n var d2x = D1[12];\n var d2y = D1[13];\n var d2z = D1[14];\n var d3x = D1[15];\n var d3y = D1[16];\n var d3z = D1[17];\n\n var a4x = D2[0];\n var a4y = D2[1];\n var a4z = D2[2];\n var a5x = D2[3];\n var a5y = D2[4];\n var a5z = D2[5];\n var a6x = D2[6];\n var a6y = D2[7];\n var a6z = D2[8];\n var d4x = D2[9];\n var d4y = D2[10];\n var d4z = D2[11];\n var d5x = D2[12];\n var d5y = D2[13];\n var d5z = D2[14];\n var d6x = D2[15];\n var d6y = D2[16];\n var d6z = D2[17];\n\n var a7x = a1y * a4z - a1z * a4y;\n var a7y = a1z * a4x - a1x * a4z;\n var a7z = a1x * a4y - a1y * a4x;\n var a8x = a1y * a5z - a1z * a5y;\n var a8y = a1z * a5x - a1x * a5z;\n var a8z = a1x * a5y - a1y * a5x;\n var a9x = a1y * a6z - a1z * a6y;\n var a9y = a1z * a6x - a1x * a6z;\n var a9z = a1x * a6y - a1y * a6x;\n var aax = a2y * a4z - a2z * a4y;\n var aay = a2z * a4x - a2x * a4z;\n var aaz = a2x * a4y - a2y * a4x;\n var abx = a2y * a5z - a2z * a5y;\n var aby = a2z * a5x - a2x * a5z;\n var abz = a2x * a5y - a2y * a5x;\n var acx = a2y * a6z - a2z * a6y;\n var acy = a2z * a6x - a2x * a6z;\n var acz = a2x * a6y - a2y * a6x;\n var adx = a3y * a4z - a3z * a4y;\n var ady = a3z * a4x - a3x * a4z;\n var adz = a3x * a4y - a3y * a4x;\n var aex = a3y * a5z - a3z * a5y;\n var aey = a3z * a5x - a3x * a5z;\n var aez = a3x * a5y - a3y * a5x;\n var afx = a3y * a6z - a3z * a6y;\n var afy = a3z * a6x - a3x * a6z;\n var afz = a3x * a6y - a3y * a6x;\n // right or left flags\n var right1;\n var right2;\n var right3;\n var right4;\n var right5;\n var right6;\n var right7;\n var right8;\n var right9;\n var righta;\n var rightb;\n var rightc;\n var rightd;\n var righte;\n var rightf;\n // overlapping distances\n var overlap1;\n var overlap2;\n var overlap3;\n var overlap4;\n var overlap5;\n var overlap6;\n var overlap7;\n var overlap8;\n var overlap9;\n var overlapa;\n var overlapb;\n var overlapc;\n var overlapd;\n var overlape;\n var overlapf;\n // invalid flags\n var invalid7 = false;\n var invalid8 = false;\n var invalid9 = false;\n var invalida = false;\n var invalidb = false;\n var invalidc = false;\n var invalidd = false;\n var invalide = false;\n var invalidf = false;\n // temporary variables\n var len;\n var len1;\n var len2;\n var dot1;\n var dot2;\n var dot3;\n // try axis 1\n len = a1x * dx + a1y * dy + a1z * dz;\n right1 = len > 0;\n if (!right1) len = -len;\n len1 = w1;\n dot1 = a1x * a4x + a1y * a4y + a1z * a4z;\n dot2 = a1x * a5x + a1y * a5y + a1z * a5z;\n dot3 = a1x * a6x + a1y * a6y + a1z * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n if (dot3 < 0) dot3 = -dot3;\n len2 = dot1 * w2 + dot2 * h2 + dot3 * d2;\n overlap1 = len - len1 - len2;\n if (overlap1 > 0) return;\n // try axis 2\n len = a2x * dx + a2y * dy + a2z * dz;\n right2 = len > 0;\n if (!right2) len = -len;\n len1 = h1;\n dot1 = a2x * a4x + a2y * a4y + a2z * a4z;\n dot2 = a2x * a5x + a2y * a5y + a2z * a5z;\n dot3 = a2x * a6x + a2y * a6y + a2z * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n if (dot3 < 0) dot3 = -dot3;\n len2 = dot1 * w2 + dot2 * h2 + dot3 * d2;\n overlap2 = len - len1 - len2;\n if (overlap2 > 0) return;\n // try axis 3\n len = a3x * dx + a3y * dy + a3z * dz;\n right3 = len > 0;\n if (!right3) len = -len;\n len1 = d1;\n dot1 = a3x * a4x + a3y * a4y + a3z * a4z;\n dot2 = a3x * a5x + a3y * a5y + a3z * a5z;\n dot3 = a3x * a6x + a3y * a6y + a3z * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n if (dot3 < 0) dot3 = -dot3;\n len2 = dot1 * w2 + dot2 * h2 + dot3 * d2;\n overlap3 = len - len1 - len2;\n if (overlap3 > 0) return;\n // try axis 4\n len = a4x * dx + a4y * dy + a4z * dz;\n right4 = len > 0;\n if (!right4) len = -len;\n dot1 = a4x * a1x + a4y * a1y + a4z * a1z;\n dot2 = a4x * a2x + a4y * a2y + a4z * a2z;\n dot3 = a4x * a3x + a4y * a3y + a4z * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n if (dot3 < 0) dot3 = -dot3;\n len1 = dot1 * w1 + dot2 * h1 + dot3 * d1;\n len2 = w2;\n overlap4 = (len - len1 - len2) * 1.0;\n if (overlap4 > 0) return;\n // try axis 5\n len = a5x * dx + a5y * dy + a5z * dz;\n right5 = len > 0;\n if (!right5) len = -len;\n dot1 = a5x * a1x + a5y * a1y + a5z * a1z;\n dot2 = a5x * a2x + a5y * a2y + a5z * a2z;\n dot3 = a5x * a3x + a5y * a3y + a5z * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n if (dot3 < 0) dot3 = -dot3;\n len1 = dot1 * w1 + dot2 * h1 + dot3 * d1;\n len2 = h2;\n overlap5 = (len - len1 - len2) * 1.0;\n if (overlap5 > 0) return;\n // try axis 6\n len = a6x * dx + a6y * dy + a6z * dz;\n right6 = len > 0;\n if (!right6) len = -len;\n dot1 = a6x * a1x + a6y * a1y + a6z * a1z;\n dot2 = a6x * a2x + a6y * a2y + a6z * a2z;\n dot3 = a6x * a3x + a6y * a3y + a6z * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n if (dot3 < 0) dot3 = -dot3;\n len1 = dot1 * w1 + dot2 * h1 + dot3 * d1;\n len2 = d2;\n overlap6 = (len - len1 - len2) * 1.0;\n if (overlap6 > 0) return;\n // try axis 7\n len = a7x * a7x + a7y * a7y + a7z * a7z;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n a7x *= len;\n a7y *= len;\n a7z *= len;\n len = a7x * dx + a7y * dy + a7z * dz;\n right7 = len > 0;\n if (!right7) len = -len;\n dot1 = a7x * a2x + a7y * a2y + a7z * a2z;\n dot2 = a7x * a3x + a7y * a3y + a7z * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * h1 + dot2 * d1;\n dot1 = a7x * a5x + a7y * a5y + a7z * a5z;\n dot2 = a7x * a6x + a7y * a6y + a7z * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * h2 + dot2 * d2;\n overlap7 = len - len1 - len2;\n if (overlap7 > 0) return;\n } else {\n right7 = false;\n overlap7 = 0;\n invalid7 = true;\n }\n // try axis 8\n len = a8x * a8x + a8y * a8y + a8z * a8z;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n a8x *= len;\n a8y *= len;\n a8z *= len;\n len = a8x * dx + a8y * dy + a8z * dz;\n right8 = len > 0;\n if (!right8) len = -len;\n dot1 = a8x * a2x + a8y * a2y + a8z * a2z;\n dot2 = a8x * a3x + a8y * a3y + a8z * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * h1 + dot2 * d1;\n dot1 = a8x * a4x + a8y * a4y + a8z * a4z;\n dot2 = a8x * a6x + a8y * a6y + a8z * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * w2 + dot2 * d2;\n overlap8 = len - len1 - len2;\n if (overlap8 > 0) return;\n } else {\n right8 = false;\n overlap8 = 0;\n invalid8 = true;\n }\n // try axis 9\n len = a9x * a9x + a9y * a9y + a9z * a9z;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n a9x *= len;\n a9y *= len;\n a9z *= len;\n len = a9x * dx + a9y * dy + a9z * dz;\n right9 = len > 0;\n if (!right9) len = -len;\n dot1 = a9x * a2x + a9y * a2y + a9z * a2z;\n dot2 = a9x * a3x + a9y * a3y + a9z * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * h1 + dot2 * d1;\n dot1 = a9x * a4x + a9y * a4y + a9z * a4z;\n dot2 = a9x * a5x + a9y * a5y + a9z * a5z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * w2 + dot2 * h2;\n overlap9 = len - len1 - len2;\n if (overlap9 > 0) return;\n } else {\n right9 = false;\n overlap9 = 0;\n invalid9 = true;\n }\n // try axis 10\n len = aax * aax + aay * aay + aaz * aaz;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n aax *= len;\n aay *= len;\n aaz *= len;\n len = aax * dx + aay * dy + aaz * dz;\n righta = len > 0;\n if (!righta) len = -len;\n dot1 = aax * a1x + aay * a1y + aaz * a1z;\n dot2 = aax * a3x + aay * a3y + aaz * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * w1 + dot2 * d1;\n dot1 = aax * a5x + aay * a5y + aaz * a5z;\n dot2 = aax * a6x + aay * a6y + aaz * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * h2 + dot2 * d2;\n overlapa = len - len1 - len2;\n if (overlapa > 0) return;\n } else {\n righta = false;\n overlapa = 0;\n invalida = true;\n }\n // try axis 11\n len = abx * abx + aby * aby + abz * abz;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n abx *= len;\n aby *= len;\n abz *= len;\n len = abx * dx + aby * dy + abz * dz;\n rightb = len > 0;\n if (!rightb) len = -len;\n dot1 = abx * a1x + aby * a1y + abz * a1z;\n dot2 = abx * a3x + aby * a3y + abz * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * w1 + dot2 * d1;\n dot1 = abx * a4x + aby * a4y + abz * a4z;\n dot2 = abx * a6x + aby * a6y + abz * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * w2 + dot2 * d2;\n overlapb = len - len1 - len2;\n if (overlapb > 0) return;\n } else {\n rightb = false;\n overlapb = 0;\n invalidb = true;\n }\n // try axis 12\n len = acx * acx + acy * acy + acz * acz;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n acx *= len;\n acy *= len;\n acz *= len;\n len = acx * dx + acy * dy + acz * dz;\n rightc = len > 0;\n if (!rightc) len = -len;\n dot1 = acx * a1x + acy * a1y + acz * a1z;\n dot2 = acx * a3x + acy * a3y + acz * a3z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * w1 + dot2 * d1;\n dot1 = acx * a4x + acy * a4y + acz * a4z;\n dot2 = acx * a5x + acy * a5y + acz * a5z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * w2 + dot2 * h2;\n overlapc = len - len1 - len2;\n if (overlapc > 0) return;\n } else {\n rightc = false;\n overlapc = 0;\n invalidc = true;\n }\n // try axis 13\n len = adx * adx + ady * ady + adz * adz;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n adx *= len;\n ady *= len;\n adz *= len;\n len = adx * dx + ady * dy + adz * dz;\n rightd = len > 0;\n if (!rightd) len = -len;\n dot1 = adx * a1x + ady * a1y + adz * a1z;\n dot2 = adx * a2x + ady * a2y + adz * a2z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * w1 + dot2 * h1;\n dot1 = adx * a5x + ady * a5y + adz * a5z;\n dot2 = adx * a6x + ady * a6y + adz * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * h2 + dot2 * d2;\n overlapd = len - len1 - len2;\n if (overlapd > 0) return;\n } else {\n rightd = false;\n overlapd = 0;\n invalidd = true;\n }\n // try axis 14\n len = aex * aex + aey * aey + aez * aez;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n aex *= len;\n aey *= len;\n aez *= len;\n len = aex * dx + aey * dy + aez * dz;\n righte = len > 0;\n if (!righte) len = -len;\n dot1 = aex * a1x + aey * a1y + aez * a1z;\n dot2 = aex * a2x + aey * a2y + aez * a2z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * w1 + dot2 * h1;\n dot1 = aex * a4x + aey * a4y + aez * a4z;\n dot2 = aex * a6x + aey * a6y + aez * a6z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * w2 + dot2 * d2;\n overlape = len - len1 - len2;\n if (overlape > 0) return;\n } else {\n righte = false;\n overlape = 0;\n invalide = true;\n }\n // try axis 15\n len = afx * afx + afy * afy + afz * afz;\n if (len > 1e-5) {\n len = 1 / _Math.sqrt(len);\n afx *= len;\n afy *= len;\n afz *= len;\n len = afx * dx + afy * dy + afz * dz;\n rightf = len > 0;\n if (!rightf) len = -len;\n dot1 = afx * a1x + afy * a1y + afz * a1z;\n dot2 = afx * a2x + afy * a2y + afz * a2z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len1 = dot1 * w1 + dot2 * h1;\n dot1 = afx * a4x + afy * a4y + afz * a4z;\n dot2 = afx * a5x + afy * a5y + afz * a5z;\n if (dot1 < 0) dot1 = -dot1;\n if (dot2 < 0) dot2 = -dot2;\n len2 = dot1 * w2 + dot2 * h2;\n overlapf = len - len1 - len2;\n if (overlapf > 0) return;\n } else {\n rightf = false;\n overlapf = 0;\n invalidf = true;\n }\n // boxes are overlapping\n var depth = overlap1;\n var depth2 = overlap1;\n var minIndex = 0;\n var right = right1;\n if (overlap2 > depth2) {\n depth = overlap2;\n depth2 = overlap2;\n minIndex = 1;\n right = right2;\n }\n if (overlap3 > depth2) {\n depth = overlap3;\n depth2 = overlap3;\n minIndex = 2;\n right = right3;\n }\n if (overlap4 > depth2) {\n depth = overlap4;\n depth2 = overlap4;\n minIndex = 3;\n right = right4;\n }\n if (overlap5 > depth2) {\n depth = overlap5;\n depth2 = overlap5;\n minIndex = 4;\n right = right5;\n }\n if (overlap6 > depth2) {\n depth = overlap6;\n depth2 = overlap6;\n minIndex = 5;\n right = right6;\n }\n if (overlap7 - 0.01 > depth2 && !invalid7) {\n depth = overlap7;\n depth2 = overlap7 - 0.01;\n minIndex = 6;\n right = right7;\n }\n if (overlap8 - 0.01 > depth2 && !invalid8) {\n depth = overlap8;\n depth2 = overlap8 - 0.01;\n minIndex = 7;\n right = right8;\n }\n if (overlap9 - 0.01 > depth2 && !invalid9) {\n depth = overlap9;\n depth2 = overlap9 - 0.01;\n minIndex = 8;\n right = right9;\n }\n if (overlapa - 0.01 > depth2 && !invalida) {\n depth = overlapa;\n depth2 = overlapa - 0.01;\n minIndex = 9;\n right = righta;\n }\n if (overlapb - 0.01 > depth2 && !invalidb) {\n depth = overlapb;\n depth2 = overlapb - 0.01;\n minIndex = 10;\n right = rightb;\n }\n if (overlapc - 0.01 > depth2 && !invalidc) {\n depth = overlapc;\n depth2 = overlapc - 0.01;\n minIndex = 11;\n right = rightc;\n }\n if (overlapd - 0.01 > depth2 && !invalidd) {\n depth = overlapd;\n depth2 = overlapd - 0.01;\n minIndex = 12;\n right = rightd;\n }\n if (overlape - 0.01 > depth2 && !invalide) {\n depth = overlape;\n depth2 = overlape - 0.01;\n minIndex = 13;\n right = righte;\n }\n if (overlapf - 0.01 > depth2 && !invalidf) {\n depth = overlapf;\n minIndex = 14;\n right = rightf;\n }\n // normal\n var nx = 0;\n var ny = 0;\n var nz = 0;\n // edge line or face side normal\n var n1x = 0;\n var n1y = 0;\n var n1z = 0;\n var n2x = 0;\n var n2y = 0;\n var n2z = 0;\n // center of current face\n var cx = 0;\n var cy = 0;\n var cz = 0;\n // face side\n var s1x = 0;\n var s1y = 0;\n var s1z = 0;\n var s2x = 0;\n var s2y = 0;\n var s2z = 0;\n // swap b1 b2\n var swap = false;\n\n //_______________________________________\n\n if (minIndex == 0) {\n // b1.x * b2\n if (right) {\n cx = p1x + d1x;cy = p1y + d1y;cz = p1z + d1z;\n nx = a1x;ny = a1y;nz = a1z;\n } else {\n cx = p1x - d1x;cy = p1y - d1y;cz = p1z - d1z;\n nx = -a1x;ny = -a1y;nz = -a1z;\n }\n s1x = d2x;s1y = d2y;s1z = d2z;\n n1x = -a2x;n1y = -a2y;n1z = -a2z;\n s2x = d3x;s2y = d3y;s2z = d3z;\n n2x = -a3x;n2y = -a3y;n2z = -a3z;\n } else if (minIndex == 1) {\n // b1.y * b2\n if (right) {\n cx = p1x + d2x;cy = p1y + d2y;cz = p1z + d2z;\n nx = a2x;ny = a2y;nz = a2z;\n } else {\n cx = p1x - d2x;cy = p1y - d2y;cz = p1z - d2z;\n nx = -a2x;ny = -a2y;nz = -a2z;\n }\n s1x = d1x;s1y = d1y;s1z = d1z;\n n1x = -a1x;n1y = -a1y;n1z = -a1z;\n s2x = d3x;s2y = d3y;s2z = d3z;\n n2x = -a3x;n2y = -a3y;n2z = -a3z;\n } else if (minIndex == 2) {\n // b1.z * b2\n if (right) {\n cx = p1x + d3x;cy = p1y + d3y;cz = p1z + d3z;\n nx = a3x;ny = a3y;nz = a3z;\n } else {\n cx = p1x - d3x;cy = p1y - d3y;cz = p1z - d3z;\n nx = -a3x;ny = -a3y;nz = -a3z;\n }\n s1x = d1x;s1y = d1y;s1z = d1z;\n n1x = -a1x;n1y = -a1y;n1z = -a1z;\n s2x = d2x;s2y = d2y;s2z = d2z;\n n2x = -a2x;n2y = -a2y;n2z = -a2z;\n } else if (minIndex == 3) {\n // b2.x * b1\n swap = true;\n if (!right) {\n cx = p2x + d4x;cy = p2y + d4y;cz = p2z + d4z;\n nx = a4x;ny = a4y;nz = a4z;\n } else {\n cx = p2x - d4x;cy = p2y - d4y;cz = p2z - d4z;\n nx = -a4x;ny = -a4y;nz = -a4z;\n }\n s1x = d5x;s1y = d5y;s1z = d5z;\n n1x = -a5x;n1y = -a5y;n1z = -a5z;\n s2x = d6x;s2y = d6y;s2z = d6z;\n n2x = -a6x;n2y = -a6y;n2z = -a6z;\n } else if (minIndex == 4) {\n // b2.y * b1\n swap = true;\n if (!right) {\n cx = p2x + d5x;cy = p2y + d5y;cz = p2z + d5z;\n nx = a5x;ny = a5y;nz = a5z;\n } else {\n cx = p2x - d5x;cy = p2y - d5y;cz = p2z - d5z;\n nx = -a5x;ny = -a5y;nz = -a5z;\n }\n s1x = d4x;s1y = d4y;s1z = d4z;\n n1x = -a4x;n1y = -a4y;n1z = -a4z;\n s2x = d6x;s2y = d6y;s2z = d6z;\n n2x = -a6x;n2y = -a6y;n2z = -a6z;\n } else if (minIndex == 5) {\n // b2.z * b1\n swap = true;\n if (!right) {\n cx = p2x + d6x;cy = p2y + d6y;cz = p2z + d6z;\n nx = a6x;ny = a6y;nz = a6z;\n } else {\n cx = p2x - d6x;cy = p2y - d6y;cz = p2z - d6z;\n nx = -a6x;ny = -a6y;nz = -a6z;\n }\n s1x = d4x;s1y = d4y;s1z = d4z;\n n1x = -a4x;n1y = -a4y;n1z = -a4z;\n s2x = d5x;s2y = d5y;s2z = d5z;\n n2x = -a5x;n2y = -a5y;n2z = -a5z;\n } else if (minIndex == 6) {\n // b1.x * b2.x\n nx = a7x;ny = a7y;nz = a7z;\n n1x = a1x;n1y = a1y;n1z = a1z;\n n2x = a4x;n2y = a4y;n2z = a4z;\n } else if (minIndex == 7) {\n // b1.x * b2.y\n nx = a8x;ny = a8y;nz = a8z;\n n1x = a1x;n1y = a1y;n1z = a1z;\n n2x = a5x;n2y = a5y;n2z = a5z;\n } else if (minIndex == 8) {\n // b1.x * b2.z\n nx = a9x;ny = a9y;nz = a9z;\n n1x = a1x;n1y = a1y;n1z = a1z;\n n2x = a6x;n2y = a6y;n2z = a6z;\n } else if (minIndex == 9) {\n // b1.y * b2.x\n nx = aax;ny = aay;nz = aaz;\n n1x = a2x;n1y = a2y;n1z = a2z;\n n2x = a4x;n2y = a4y;n2z = a4z;\n } else if (minIndex == 10) {\n // b1.y * b2.y\n nx = abx;ny = aby;nz = abz;\n n1x = a2x;n1y = a2y;n1z = a2z;\n n2x = a5x;n2y = a5y;n2z = a5z;\n } else if (minIndex == 11) {\n // b1.y * b2.z\n nx = acx;ny = acy;nz = acz;\n n1x = a2x;n1y = a2y;n1z = a2z;\n n2x = a6x;n2y = a6y;n2z = a6z;\n } else if (minIndex == 12) {\n // b1.z * b2.x\n nx = adx;ny = ady;nz = adz;\n n1x = a3x;n1y = a3y;n1z = a3z;\n n2x = a4x;n2y = a4y;n2z = a4z;\n } else if (minIndex == 13) {\n // b1.z * b2.y\n nx = aex;ny = aey;nz = aez;\n n1x = a3x;n1y = a3y;n1z = a3z;\n n2x = a5x;n2y = a5y;n2z = a5z;\n } else if (minIndex == 14) {\n // b1.z * b2.z\n nx = afx;ny = afy;nz = afz;\n n1x = a3x;n1y = a3y;n1z = a3z;\n n2x = a6x;n2y = a6y;n2z = a6z;\n }\n\n //__________________________________________\n\n //var v;\n if (minIndex > 5) {\n if (!right) {\n nx = -nx;ny = -ny;nz = -nz;\n }\n var distance;\n var maxDistance;\n var vx;\n var vy;\n var vz;\n var v1x;\n var v1y;\n var v1z;\n var v2x;\n var v2y;\n var v2z;\n //vertex1;\n v1x = V1[0];v1y = V1[1];v1z = V1[2];\n maxDistance = nx * v1x + ny * v1y + nz * v1z;\n //vertex2;\n vx = V1[3];vy = V1[4];vz = V1[5];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance > maxDistance) {\n maxDistance = distance;\n v1x = vx;v1y = vy;v1z = vz;\n }\n //vertex3;\n vx = V1[6];vy = V1[7];vz = V1[8];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance > maxDistance) {\n maxDistance = distance;\n v1x = vx;v1y = vy;v1z = vz;\n }\n //vertex4;\n vx = V1[9];vy = V1[10];vz = V1[11];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance > maxDistance) {\n maxDistance = distance;\n v1x = vx;v1y = vy;v1z = vz;\n }\n //vertex5;\n vx = V1[12];vy = V1[13];vz = V1[14];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance > maxDistance) {\n maxDistance = distance;\n v1x = vx;v1y = vy;v1z = vz;\n }\n //vertex6;\n vx = V1[15];vy = V1[16];vz = V1[17];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance > maxDistance) {\n maxDistance = distance;\n v1x = vx;v1y = vy;v1z = vz;\n }\n //vertex7;\n vx = V1[18];vy = V1[19];vz = V1[20];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance > maxDistance) {\n maxDistance = distance;\n v1x = vx;v1y = vy;v1z = vz;\n }\n //vertex8;\n vx = V1[21];vy = V1[22];vz = V1[23];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance > maxDistance) {\n maxDistance = distance;\n v1x = vx;v1y = vy;v1z = vz;\n }\n //vertex1;\n v2x = V2[0];v2y = V2[1];v2z = V2[2];\n maxDistance = nx * v2x + ny * v2y + nz * v2z;\n //vertex2;\n vx = V2[3];vy = V2[4];vz = V2[5];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance < maxDistance) {\n maxDistance = distance;\n v2x = vx;v2y = vy;v2z = vz;\n }\n //vertex3;\n vx = V2[6];vy = V2[7];vz = V2[8];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance < maxDistance) {\n maxDistance = distance;\n v2x = vx;v2y = vy;v2z = vz;\n }\n //vertex4;\n vx = V2[9];vy = V2[10];vz = V2[11];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance < maxDistance) {\n maxDistance = distance;\n v2x = vx;v2y = vy;v2z = vz;\n }\n //vertex5;\n vx = V2[12];vy = V2[13];vz = V2[14];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance < maxDistance) {\n maxDistance = distance;\n v2x = vx;v2y = vy;v2z = vz;\n }\n //vertex6;\n vx = V2[15];vy = V2[16];vz = V2[17];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance < maxDistance) {\n maxDistance = distance;\n v2x = vx;v2y = vy;v2z = vz;\n }\n //vertex7;\n vx = V2[18];vy = V2[19];vz = V2[20];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance < maxDistance) {\n maxDistance = distance;\n v2x = vx;v2y = vy;v2z = vz;\n }\n //vertex8;\n vx = V2[21];vy = V2[22];vz = V2[23];\n distance = nx * vx + ny * vy + nz * vz;\n if (distance < maxDistance) {\n maxDistance = distance;\n v2x = vx;v2y = vy;v2z = vz;\n }\n vx = v2x - v1x;vy = v2y - v1y;vz = v2z - v1z;\n dot1 = n1x * n2x + n1y * n2y + n1z * n2z;\n var t = (vx * (n1x - n2x * dot1) + vy * (n1y - n2y * dot1) + vz * (n1z - n2z * dot1)) / (1 - dot1 * dot1);\n manifold.addPoint(v1x + n1x * t + nx * depth * 0.5, v1y + n1y * t + ny * depth * 0.5, v1z + n1z * t + nz * depth * 0.5, nx, ny, nz, depth, false);\n return;\n }\n // now detect face-face collision...\n // target quad\n var q1x;\n var q1y;\n var q1z;\n var q2x;\n var q2y;\n var q2z;\n var q3x;\n var q3y;\n var q3z;\n var q4x;\n var q4y;\n var q4z;\n // search support face and vertex\n var minDot = 1;\n var dot = 0;\n var minDotIndex = 0;\n if (swap) {\n dot = a1x * nx + a1y * ny + a1z * nz;\n if (dot < minDot) {\n minDot = dot;\n minDotIndex = 0;\n }\n if (-dot < minDot) {\n minDot = -dot;\n minDotIndex = 1;\n }\n dot = a2x * nx + a2y * ny + a2z * nz;\n if (dot < minDot) {\n minDot = dot;\n minDotIndex = 2;\n }\n if (-dot < minDot) {\n minDot = -dot;\n minDotIndex = 3;\n }\n dot = a3x * nx + a3y * ny + a3z * nz;\n if (dot < minDot) {\n minDot = dot;\n minDotIndex = 4;\n }\n if (-dot < minDot) {\n minDot = -dot;\n minDotIndex = 5;\n }\n\n if (minDotIndex == 0) {\n // x+ face\n q1x = V1[0];q1y = V1[1];q1z = V1[2]; //vertex1\n q2x = V1[6];q2y = V1[7];q2z = V1[8]; //vertex3\n q3x = V1[9];q3y = V1[10];q3z = V1[11]; //vertex4\n q4x = V1[3];q4y = V1[4];q4z = V1[5]; //vertex2\n } else if (minDotIndex == 1) {\n // x- face\n q1x = V1[15];q1y = V1[16];q1z = V1[17]; //vertex6\n q2x = V1[21];q2y = V1[22];q2z = V1[23]; //vertex8\n q3x = V1[18];q3y = V1[19];q3z = V1[20]; //vertex7\n q4x = V1[12];q4y = V1[13];q4z = V1[14]; //vertex5\n } else if (minDotIndex == 2) {\n // y+ face\n q1x = V1[12];q1y = V1[13];q1z = V1[14]; //vertex5\n q2x = V1[0];q2y = V1[1];q2z = V1[2]; //vertex1\n q3x = V1[3];q3y = V1[4];q3z = V1[5]; //vertex2\n q4x = V1[15];q4y = V1[16];q4z = V1[17]; //vertex6\n } else if (minDotIndex == 3) {\n // y- face\n q1x = V1[21];q1y = V1[22];q1z = V1[23]; //vertex8\n q2x = V1[9];q2y = V1[10];q2z = V1[11]; //vertex4\n q3x = V1[6];q3y = V1[7];q3z = V1[8]; //vertex3\n q4x = V1[18];q4y = V1[19];q4z = V1[20]; //vertex7\n } else if (minDotIndex == 4) {\n // z+ face\n q1x = V1[12];q1y = V1[13];q1z = V1[14]; //vertex5\n q2x = V1[18];q2y = V1[19];q2z = V1[20]; //vertex7\n q3x = V1[6];q3y = V1[7];q3z = V1[8]; //vertex3\n q4x = V1[0];q4y = V1[1];q4z = V1[2]; //vertex1\n } else if (minDotIndex == 5) {\n // z- face\n q1x = V1[3];q1y = V1[4];q1z = V1[5]; //vertex2\n //2x=V1[6]; q2y=V1[7]; q2z=V1[8];//vertex4 !!!\n q2x = V2[9];q2y = V2[10];q2z = V2[11]; //vertex4\n q3x = V1[21];q3y = V1[22];q3z = V1[23]; //vertex8\n q4x = V1[15];q4y = V1[16];q4z = V1[17]; //vertex6\n }\n } else {\n dot = a4x * nx + a4y * ny + a4z * nz;\n if (dot < minDot) {\n minDot = dot;\n minDotIndex = 0;\n }\n if (-dot < minDot) {\n minDot = -dot;\n minDotIndex = 1;\n }\n dot = a5x * nx + a5y * ny + a5z * nz;\n if (dot < minDot) {\n minDot = dot;\n minDotIndex = 2;\n }\n if (-dot < minDot) {\n minDot = -dot;\n minDotIndex = 3;\n }\n dot = a6x * nx + a6y * ny + a6z * nz;\n if (dot < minDot) {\n minDot = dot;\n minDotIndex = 4;\n }\n if (-dot < minDot) {\n minDot = -dot;\n minDotIndex = 5;\n }\n\n //______________________________________________________\n\n if (minDotIndex == 0) {\n // x+ face\n q1x = V2[0];q1y = V2[1];q1z = V2[2]; //vertex1\n q2x = V2[6];q2y = V2[7];q2z = V2[8]; //vertex3\n q3x = V2[9];q3y = V2[10];q3z = V2[11]; //vertex4\n q4x = V2[3];q4y = V2[4];q4z = V2[5]; //vertex2\n } else if (minDotIndex == 1) {\n // x- face\n q1x = V2[15];q1y = V2[16];q1z = V2[17]; //vertex6\n q2x = V2[21];q2y = V2[22];q2z = V2[23]; //vertex8\n q3x = V2[18];q3y = V2[19];q3z = V2[20]; //vertex7\n q4x = V2[12];q4y = V2[13];q4z = V2[14]; //vertex5\n } else if (minDotIndex == 2) {\n // y+ face\n q1x = V2[12];q1y = V2[13];q1z = V2[14]; //vertex5\n q2x = V2[0];q2y = V2[1];q2z = V2[2]; //vertex1\n q3x = V2[3];q3y = V2[4];q3z = V2[5]; //vertex2\n q4x = V2[15];q4y = V2[16];q4z = V2[17]; //vertex6\n } else if (minDotIndex == 3) {\n // y- face\n q1x = V2[21];q1y = V2[22];q1z = V2[23]; //vertex8\n q2x = V2[9];q2y = V2[10];q2z = V2[11]; //vertex4\n q3x = V2[6];q3y = V2[7];q3z = V2[8]; //vertex3\n q4x = V2[18];q4y = V2[19];q4z = V2[20]; //vertex7\n } else if (minDotIndex == 4) {\n // z+ face\n q1x = V2[12];q1y = V2[13];q1z = V2[14]; //vertex5\n q2x = V2[18];q2y = V2[19];q2z = V2[20]; //vertex7\n q3x = V2[6];q3y = V2[7];q3z = V2[8]; //vertex3\n q4x = V2[0];q4y = V2[1];q4z = V2[2]; //vertex1\n } else if (minDotIndex == 5) {\n // z- face\n q1x = V2[3];q1y = V2[4];q1z = V2[5]; //vertex2\n q2x = V2[9];q2y = V2[10];q2z = V2[11]; //vertex4\n q3x = V2[21];q3y = V2[22];q3z = V2[23]; //vertex8\n q4x = V2[15];q4y = V2[16];q4z = V2[17]; //vertex6\n }\n }\n // clip vertices\n var numClipVertices;\n var numAddedClipVertices;\n var index;\n var x1;\n var y1;\n var z1;\n var x2;\n var y2;\n var z2;\n this.clipVertices1[0] = q1x;\n this.clipVertices1[1] = q1y;\n this.clipVertices1[2] = q1z;\n this.clipVertices1[3] = q2x;\n this.clipVertices1[4] = q2y;\n this.clipVertices1[5] = q2z;\n this.clipVertices1[6] = q3x;\n this.clipVertices1[7] = q3y;\n this.clipVertices1[8] = q3z;\n this.clipVertices1[9] = q4x;\n this.clipVertices1[10] = q4y;\n this.clipVertices1[11] = q4z;\n numAddedClipVertices = 0;\n x1 = this.clipVertices1[9];\n y1 = this.clipVertices1[10];\n z1 = this.clipVertices1[11];\n dot1 = (x1 - cx - s1x) * n1x + (y1 - cy - s1y) * n1y + (z1 - cz - s1z) * n1z;\n\n //var i = 4;\n //while(i--){\n for (var i = 0; i < 4; i++) {\n index = i * 3;\n x2 = this.clipVertices1[index];\n y2 = this.clipVertices1[index + 1];\n z2 = this.clipVertices1[index + 2];\n dot2 = (x2 - cx - s1x) * n1x + (y2 - cy - s1y) * n1y + (z2 - cz - s1z) * n1z;\n if (dot1 > 0) {\n if (dot2 > 0) {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n this.clipVertices2[index] = x2;\n this.clipVertices2[index + 1] = y2;\n this.clipVertices2[index + 2] = z2;\n } else {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n t = dot1 / (dot1 - dot2);\n this.clipVertices2[index] = x1 + (x2 - x1) * t;\n this.clipVertices2[index + 1] = y1 + (y2 - y1) * t;\n this.clipVertices2[index + 2] = z1 + (z2 - z1) * t;\n }\n } else {\n if (dot2 > 0) {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n t = dot1 / (dot1 - dot2);\n this.clipVertices2[index] = x1 + (x2 - x1) * t;\n this.clipVertices2[index + 1] = y1 + (y2 - y1) * t;\n this.clipVertices2[index + 2] = z1 + (z2 - z1) * t;\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n this.clipVertices2[index] = x2;\n this.clipVertices2[index + 1] = y2;\n this.clipVertices2[index + 2] = z2;\n }\n }\n x1 = x2;\n y1 = y2;\n z1 = z2;\n dot1 = dot2;\n }\n\n numClipVertices = numAddedClipVertices;\n if (numClipVertices == 0) return;\n numAddedClipVertices = 0;\n index = (numClipVertices - 1) * 3;\n x1 = this.clipVertices2[index];\n y1 = this.clipVertices2[index + 1];\n z1 = this.clipVertices2[index + 2];\n dot1 = (x1 - cx - s2x) * n2x + (y1 - cy - s2y) * n2y + (z1 - cz - s2z) * n2z;\n\n //i = numClipVertices;\n //while(i--){\n for (i = 0; i < numClipVertices; i++) {\n index = i * 3;\n x2 = this.clipVertices2[index];\n y2 = this.clipVertices2[index + 1];\n z2 = this.clipVertices2[index + 2];\n dot2 = (x2 - cx - s2x) * n2x + (y2 - cy - s2y) * n2y + (z2 - cz - s2z) * n2z;\n if (dot1 > 0) {\n if (dot2 > 0) {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n this.clipVertices1[index] = x2;\n this.clipVertices1[index + 1] = y2;\n this.clipVertices1[index + 2] = z2;\n } else {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n t = dot1 / (dot1 - dot2);\n this.clipVertices1[index] = x1 + (x2 - x1) * t;\n this.clipVertices1[index + 1] = y1 + (y2 - y1) * t;\n this.clipVertices1[index + 2] = z1 + (z2 - z1) * t;\n }\n } else {\n if (dot2 > 0) {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n t = dot1 / (dot1 - dot2);\n this.clipVertices1[index] = x1 + (x2 - x1) * t;\n this.clipVertices1[index + 1] = y1 + (y2 - y1) * t;\n this.clipVertices1[index + 2] = z1 + (z2 - z1) * t;\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n this.clipVertices1[index] = x2;\n this.clipVertices1[index + 1] = y2;\n this.clipVertices1[index + 2] = z2;\n }\n }\n x1 = x2;\n y1 = y2;\n z1 = z2;\n dot1 = dot2;\n }\n\n numClipVertices = numAddedClipVertices;\n if (numClipVertices == 0) return;\n numAddedClipVertices = 0;\n index = (numClipVertices - 1) * 3;\n x1 = this.clipVertices1[index];\n y1 = this.clipVertices1[index + 1];\n z1 = this.clipVertices1[index + 2];\n dot1 = (x1 - cx + s1x) * -n1x + (y1 - cy + s1y) * -n1y + (z1 - cz + s1z) * -n1z;\n\n //i = numClipVertices;\n //while(i--){\n for (i = 0; i < numClipVertices; i++) {\n index = i * 3;\n x2 = this.clipVertices1[index];\n y2 = this.clipVertices1[index + 1];\n z2 = this.clipVertices1[index + 2];\n dot2 = (x2 - cx + s1x) * -n1x + (y2 - cy + s1y) * -n1y + (z2 - cz + s1z) * -n1z;\n if (dot1 > 0) {\n if (dot2 > 0) {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n this.clipVertices2[index] = x2;\n this.clipVertices2[index + 1] = y2;\n this.clipVertices2[index + 2] = z2;\n } else {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n t = dot1 / (dot1 - dot2);\n this.clipVertices2[index] = x1 + (x2 - x1) * t;\n this.clipVertices2[index + 1] = y1 + (y2 - y1) * t;\n this.clipVertices2[index + 2] = z1 + (z2 - z1) * t;\n }\n } else {\n if (dot2 > 0) {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n t = dot1 / (dot1 - dot2);\n this.clipVertices2[index] = x1 + (x2 - x1) * t;\n this.clipVertices2[index + 1] = y1 + (y2 - y1) * t;\n this.clipVertices2[index + 2] = z1 + (z2 - z1) * t;\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n this.clipVertices2[index] = x2;\n this.clipVertices2[index + 1] = y2;\n this.clipVertices2[index + 2] = z2;\n }\n }\n x1 = x2;\n y1 = y2;\n z1 = z2;\n dot1 = dot2;\n }\n\n numClipVertices = numAddedClipVertices;\n if (numClipVertices == 0) return;\n numAddedClipVertices = 0;\n index = (numClipVertices - 1) * 3;\n x1 = this.clipVertices2[index];\n y1 = this.clipVertices2[index + 1];\n z1 = this.clipVertices2[index + 2];\n dot1 = (x1 - cx + s2x) * -n2x + (y1 - cy + s2y) * -n2y + (z1 - cz + s2z) * -n2z;\n\n //i = numClipVertices;\n //while(i--){\n for (i = 0; i < numClipVertices; i++) {\n index = i * 3;\n x2 = this.clipVertices2[index];\n y2 = this.clipVertices2[index + 1];\n z2 = this.clipVertices2[index + 2];\n dot2 = (x2 - cx + s2x) * -n2x + (y2 - cy + s2y) * -n2y + (z2 - cz + s2z) * -n2z;\n if (dot1 > 0) {\n if (dot2 > 0) {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n this.clipVertices1[index] = x2;\n this.clipVertices1[index + 1] = y2;\n this.clipVertices1[index + 2] = z2;\n } else {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n t = dot1 / (dot1 - dot2);\n this.clipVertices1[index] = x1 + (x2 - x1) * t;\n this.clipVertices1[index + 1] = y1 + (y2 - y1) * t;\n this.clipVertices1[index + 2] = z1 + (z2 - z1) * t;\n }\n } else {\n if (dot2 > 0) {\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n t = dot1 / (dot1 - dot2);\n this.clipVertices1[index] = x1 + (x2 - x1) * t;\n this.clipVertices1[index + 1] = y1 + (y2 - y1) * t;\n this.clipVertices1[index + 2] = z1 + (z2 - z1) * t;\n index = numAddedClipVertices * 3;\n numAddedClipVertices++;\n this.clipVertices1[index] = x2;\n this.clipVertices1[index + 1] = y2;\n this.clipVertices1[index + 2] = z2;\n }\n }\n x1 = x2;\n y1 = y2;\n z1 = z2;\n dot1 = dot2;\n }\n\n numClipVertices = numAddedClipVertices;\n if (swap) {\n var tb = b1;\n b1 = b2;\n b2 = tb;\n }\n if (numClipVertices == 0) return;\n var flipped = b1 != shape1;\n if (numClipVertices > 4) {\n x1 = (q1x + q2x + q3x + q4x) * 0.25;\n y1 = (q1y + q2y + q3y + q4y) * 0.25;\n z1 = (q1z + q2z + q3z + q4z) * 0.25;\n n1x = q1x - x1;\n n1y = q1y - y1;\n n1z = q1z - z1;\n n2x = q2x - x1;\n n2y = q2y - y1;\n n2z = q2z - z1;\n var index1 = 0;\n var index2 = 0;\n var index3 = 0;\n var index4 = 0;\n var maxDot = -this.INF;\n minDot = this.INF;\n\n //i = numClipVertices;\n //while(i--){\n for (i = 0; i < numClipVertices; i++) {\n this.used[i] = false;\n index = i * 3;\n x1 = this.clipVertices1[index];\n y1 = this.clipVertices1[index + 1];\n z1 = this.clipVertices1[index + 2];\n dot = x1 * n1x + y1 * n1y + z1 * n1z;\n if (dot < minDot) {\n minDot = dot;\n index1 = i;\n }\n if (dot > maxDot) {\n maxDot = dot;\n index3 = i;\n }\n }\n\n this.used[index1] = true;\n this.used[index3] = true;\n maxDot = -this.INF;\n minDot = this.INF;\n\n //i = numClipVertices;\n //while(i--){\n for (i = 0; i < numClipVertices; i++) {\n if (this.used[i]) continue;\n index = i * 3;\n x1 = this.clipVertices1[index];\n y1 = this.clipVertices1[index + 1];\n z1 = this.clipVertices1[index + 2];\n dot = x1 * n2x + y1 * n2y + z1 * n2z;\n if (dot < minDot) {\n minDot = dot;\n index2 = i;\n }\n if (dot > maxDot) {\n maxDot = dot;\n index4 = i;\n }\n }\n\n index = index1 * 3;\n x1 = this.clipVertices1[index];\n y1 = this.clipVertices1[index + 1];\n z1 = this.clipVertices1[index + 2];\n dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;\n if (dot < 0) manifold.addPoint(x1, y1, z1, nx, ny, nz, dot, flipped);\n\n index = index2 * 3;\n x1 = this.clipVertices1[index];\n y1 = this.clipVertices1[index + 1];\n z1 = this.clipVertices1[index + 2];\n dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;\n if (dot < 0) manifold.addPoint(x1, y1, z1, nx, ny, nz, dot, flipped);\n\n index = index3 * 3;\n x1 = this.clipVertices1[index];\n y1 = this.clipVertices1[index + 1];\n z1 = this.clipVertices1[index + 2];\n dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;\n if (dot < 0) manifold.addPoint(x1, y1, z1, nx, ny, nz, dot, flipped);\n\n index = index4 * 3;\n x1 = this.clipVertices1[index];\n y1 = this.clipVertices1[index + 1];\n z1 = this.clipVertices1[index + 2];\n dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;\n if (dot < 0) manifold.addPoint(x1, y1, z1, nx, ny, nz, dot, flipped);\n } else {\n //i = numClipVertices;\n //while(i--){\n for (i = 0; i < numClipVertices; i++) {\n index = i * 3;\n x1 = this.clipVertices1[index];\n y1 = this.clipVertices1[index + 1];\n z1 = this.clipVertices1[index + 2];\n dot = (x1 - cx) * nx + (y1 - cy) * ny + (z1 - cz) * nz;\n if (dot < 0) manifold.addPoint(x1, y1, z1, nx, ny, nz, dot, flipped);\n }\n }\n }\n\n});\n\nfunction BoxCylinderCollisionDetector(flip) {\n\n CollisionDetector.call(this);\n this.flip = flip;\n}\n\nBoxCylinderCollisionDetector.prototype = Object.assign(Object.create(CollisionDetector.prototype), {\n\n constructor: BoxCylinderCollisionDetector,\n\n getSep: function (c1, c2, sep, pos, dep) {\n\n var t1x;\n var t1y;\n var t1z;\n var t2x;\n var t2y;\n var t2z;\n var sup = new Vec3();\n var len;\n var p1x;\n var p1y;\n var p1z;\n var p2x;\n var p2y;\n var p2z;\n var v01x = c1.position.x;\n var v01y = c1.position.y;\n var v01z = c1.position.z;\n var v02x = c2.position.x;\n var v02y = c2.position.y;\n var v02z = c2.position.z;\n var v0x = v02x - v01x;\n var v0y = v02y - v01y;\n var v0z = v02z - v01z;\n if (v0x * v0x + v0y * v0y + v0z * v0z == 0) v0y = 0.001;\n var nx = -v0x;\n var ny = -v0y;\n var nz = -v0z;\n this.supportPointB(c1, -nx, -ny, -nz, sup);\n var v11x = sup.x;\n var v11y = sup.y;\n var v11z = sup.z;\n this.supportPointC(c2, nx, ny, nz, sup);\n var v12x = sup.x;\n var v12y = sup.y;\n var v12z = sup.z;\n var v1x = v12x - v11x;\n var v1y = v12y - v11y;\n var v1z = v12z - v11z;\n if (v1x * nx + v1y * ny + v1z * nz <= 0) {\n return false;\n }\n nx = v1y * v0z - v1z * v0y;\n ny = v1z * v0x - v1x * v0z;\n nz = v1x * v0y - v1y * v0x;\n if (nx * nx + ny * ny + nz * nz == 0) {\n sep.set(v1x - v0x, v1y - v0y, v1z - v0z).normalize();\n pos.set((v11x + v12x) * 0.5, (v11y + v12y) * 0.5, (v11z + v12z) * 0.5);\n return true;\n }\n this.supportPointB(c1, -nx, -ny, -nz, sup);\n var v21x = sup.x;\n var v21y = sup.y;\n var v21z = sup.z;\n this.supportPointC(c2, nx, ny, nz, sup);\n var v22x = sup.x;\n var v22y = sup.y;\n var v22z = sup.z;\n var v2x = v22x - v21x;\n var v2y = v22y - v21y;\n var v2z = v22z - v21z;\n if (v2x * nx + v2y * ny + v2z * nz <= 0) {\n return false;\n }\n t1x = v1x - v0x;\n t1y = v1y - v0y;\n t1z = v1z - v0z;\n t2x = v2x - v0x;\n t2y = v2y - v0y;\n t2z = v2z - v0z;\n nx = t1y * t2z - t1z * t2y;\n ny = t1z * t2x - t1x * t2z;\n nz = t1x * t2y - t1y * t2x;\n if (nx * v0x + ny * v0y + nz * v0z > 0) {\n t1x = v1x;\n t1y = v1y;\n t1z = v1z;\n v1x = v2x;\n v1y = v2y;\n v1z = v2z;\n v2x = t1x;\n v2y = t1y;\n v2z = t1z;\n t1x = v11x;\n t1y = v11y;\n t1z = v11z;\n v11x = v21x;\n v11y = v21y;\n v11z = v21z;\n v21x = t1x;\n v21y = t1y;\n v21z = t1z;\n t1x = v12x;\n t1y = v12y;\n t1z = v12z;\n v12x = v22x;\n v12y = v22y;\n v12z = v22z;\n v22x = t1x;\n v22y = t1y;\n v22z = t1z;\n nx = -nx;\n ny = -ny;\n nz = -nz;\n }\n var iterations = 0;\n while (true) {\n if (++iterations > 100) {\n return false;\n }\n this.supportPointB(c1, -nx, -ny, -nz, sup);\n var v31x = sup.x;\n var v31y = sup.y;\n var v31z = sup.z;\n this.supportPointC(c2, nx, ny, nz, sup);\n var v32x = sup.x;\n var v32y = sup.y;\n var v32z = sup.z;\n var v3x = v32x - v31x;\n var v3y = v32y - v31y;\n var v3z = v32z - v31z;\n if (v3x * nx + v3y * ny + v3z * nz <= 0) {\n return false;\n }\n if ((v1y * v3z - v1z * v3y) * v0x + (v1z * v3x - v1x * v3z) * v0y + (v1x * v3y - v1y * v3x) * v0z < 0) {\n v2x = v3x;\n v2y = v3y;\n v2z = v3z;\n v21x = v31x;\n v21y = v31y;\n v21z = v31z;\n v22x = v32x;\n v22y = v32y;\n v22z = v32z;\n t1x = v1x - v0x;\n t1y = v1y - v0y;\n t1z = v1z - v0z;\n t2x = v3x - v0x;\n t2y = v3y - v0y;\n t2z = v3z - v0z;\n nx = t1y * t2z - t1z * t2y;\n ny = t1z * t2x - t1x * t2z;\n nz = t1x * t2y - t1y * t2x;\n continue;\n }\n if ((v3y * v2z - v3z * v2y) * v0x + (v3z * v2x - v3x * v2z) * v0y + (v3x * v2y - v3y * v2x) * v0z < 0) {\n v1x = v3x;\n v1y = v3y;\n v1z = v3z;\n v11x = v31x;\n v11y = v31y;\n v11z = v31z;\n v12x = v32x;\n v12y = v32y;\n v12z = v32z;\n t1x = v3x - v0x;\n t1y = v3y - v0y;\n t1z = v3z - v0z;\n t2x = v2x - v0x;\n t2y = v2y - v0y;\n t2z = v2z - v0z;\n nx = t1y * t2z - t1z * t2y;\n ny = t1z * t2x - t1x * t2z;\n nz = t1x * t2y - t1y * t2x;\n continue;\n }\n var hit = false;\n while (true) {\n t1x = v2x - v1x;\n t1y = v2y - v1y;\n t1z = v2z - v1z;\n t2x = v3x - v1x;\n t2y = v3y - v1y;\n t2z = v3z - v1z;\n nx = t1y * t2z - t1z * t2y;\n ny = t1z * t2x - t1x * t2z;\n nz = t1x * t2y - t1y * t2x;\n len = 1 / _Math.sqrt(nx * nx + ny * ny + nz * nz);\n nx *= len;\n ny *= len;\n nz *= len;\n if (nx * v1x + ny * v1y + nz * v1z >= 0 && !hit) {\n var b0 = (v1y * v2z - v1z * v2y) * v3x + (v1z * v2x - v1x * v2z) * v3y + (v1x * v2y - v1y * v2x) * v3z;\n var b1 = (v3y * v2z - v3z * v2y) * v0x + (v3z * v2x - v3x * v2z) * v0y + (v3x * v2y - v3y * v2x) * v0z;\n var b2 = (v0y * v1z - v0z * v1y) * v3x + (v0z * v1x - v0x * v1z) * v3y + (v0x * v1y - v0y * v1x) * v3z;\n var b3 = (v2y * v1z - v2z * v1y) * v0x + (v2z * v1x - v2x * v1z) * v0y + (v2x * v1y - v2y * v1x) * v0z;\n var sum = b0 + b1 + b2 + b3;\n if (sum <= 0) {\n b0 = 0;\n b1 = (v2y * v3z - v2z * v3y) * nx + (v2z * v3x - v2x * v3z) * ny + (v2x * v3y - v2y * v3x) * nz;\n b2 = (v3y * v2z - v3z * v2y) * nx + (v3z * v2x - v3x * v2z) * ny + (v3x * v2y - v3y * v2x) * nz;\n b3 = (v1y * v2z - v1z * v2y) * nx + (v1z * v2x - v1x * v2z) * ny + (v1x * v2y - v1y * v2x) * nz;\n sum = b1 + b2 + b3;\n }\n var inv = 1 / sum;\n p1x = (v01x * b0 + v11x * b1 + v21x * b2 + v31x * b3) * inv;\n p1y = (v01y * b0 + v11y * b1 + v21y * b2 + v31y * b3) * inv;\n p1z = (v01z * b0 + v11z * b1 + v21z * b2 + v31z * b3) * inv;\n p2x = (v02x * b0 + v12x * b1 + v22x * b2 + v32x * b3) * inv;\n p2y = (v02y * b0 + v12y * b1 + v22y * b2 + v32y * b3) * inv;\n p2z = (v02z * b0 + v12z * b1 + v22z * b2 + v32z * b3) * inv;\n hit = true;\n }\n this.supportPointB(c1, -nx, -ny, -nz, sup);\n var v41x = sup.x;\n var v41y = sup.y;\n var v41z = sup.z;\n this.supportPointC(c2, nx, ny, nz, sup);\n var v42x = sup.x;\n var v42y = sup.y;\n var v42z = sup.z;\n var v4x = v42x - v41x;\n var v4y = v42y - v41y;\n var v4z = v42z - v41z;\n var separation = -(v4x * nx + v4y * ny + v4z * nz);\n if ((v4x - v3x) * nx + (v4y - v3y) * ny + (v4z - v3z) * nz <= 0.01 || separation >= 0) {\n if (hit) {\n sep.set(-nx, -ny, -nz);\n pos.set((p1x + p2x) * 0.5, (p1y + p2y) * 0.5, (p1z + p2z) * 0.5);\n dep.x = separation;\n return true;\n }\n return false;\n }\n if ((v4y * v1z - v4z * v1y) * v0x + (v4z * v1x - v4x * v1z) * v0y + (v4x * v1y - v4y * v1x) * v0z < 0) {\n if ((v4y * v2z - v4z * v2y) * v0x + (v4z * v2x - v4x * v2z) * v0y + (v4x * v2y - v4y * v2x) * v0z < 0) {\n v1x = v4x;\n v1y = v4y;\n v1z = v4z;\n v11x = v41x;\n v11y = v41y;\n v11z = v41z;\n v12x = v42x;\n v12y = v42y;\n v12z = v42z;\n } else {\n v3x = v4x;\n v3y = v4y;\n v3z = v4z;\n v31x = v41x;\n v31y = v41y;\n v31z = v41z;\n v32x = v42x;\n v32y = v42y;\n v32z = v42z;\n }\n } else {\n if ((v4y * v3z - v4z * v3y) * v0x + (v4z * v3x - v4x * v3z) * v0y + (v4x * v3y - v4y * v3x) * v0z < 0) {\n v2x = v4x;\n v2y = v4y;\n v2z = v4z;\n v21x = v41x;\n v21y = v41y;\n v21z = v41z;\n v22x = v42x;\n v22y = v42y;\n v22z = v42z;\n } else {\n v1x = v4x;\n v1y = v4y;\n v1z = v4z;\n v11x = v41x;\n v11y = v41y;\n v11z = v41z;\n v12x = v42x;\n v12y = v42y;\n v12z = v42z;\n }\n }\n }\n }\n //return false;\n },\n\n supportPointB: function (c, dx, dy, dz, out) {\n\n var rot = c.rotation.elements;\n var ldx = rot[0] * dx + rot[3] * dy + rot[6] * dz;\n var ldy = rot[1] * dx + rot[4] * dy + rot[7] * dz;\n var ldz = rot[2] * dx + rot[5] * dy + rot[8] * dz;\n var w = c.halfWidth;\n var h = c.halfHeight;\n var d = c.halfDepth;\n var ox;\n var oy;\n var oz;\n if (ldx < 0) ox = -w;else ox = w;\n if (ldy < 0) oy = -h;else oy = h;\n if (ldz < 0) oz = -d;else oz = d;\n ldx = rot[0] * ox + rot[1] * oy + rot[2] * oz + c.position.x;\n ldy = rot[3] * ox + rot[4] * oy + rot[5] * oz + c.position.y;\n ldz = rot[6] * ox + rot[7] * oy + rot[8] * oz + c.position.z;\n out.set(ldx, ldy, ldz);\n },\n\n supportPointC: function (c, dx, dy, dz, out) {\n\n var rot = c.rotation.elements;\n var ldx = rot[0] * dx + rot[3] * dy + rot[6] * dz;\n var ldy = rot[1] * dx + rot[4] * dy + rot[7] * dz;\n var ldz = rot[2] * dx + rot[5] * dy + rot[8] * dz;\n var radx = ldx;\n var radz = ldz;\n var len = radx * radx + radz * radz;\n var rad = c.radius;\n var hh = c.halfHeight;\n var ox;\n var oy;\n var oz;\n if (len == 0) {\n if (ldy < 0) {\n ox = rad;\n oy = -hh;\n oz = 0;\n } else {\n ox = rad;\n oy = hh;\n oz = 0;\n }\n } else {\n len = c.radius / _Math.sqrt(len);\n if (ldy < 0) {\n ox = radx * len;\n oy = -hh;\n oz = radz * len;\n } else {\n ox = radx * len;\n oy = hh;\n oz = radz * len;\n }\n }\n ldx = rot[0] * ox + rot[1] * oy + rot[2] * oz + c.position.x;\n ldy = rot[3] * ox + rot[4] * oy + rot[5] * oz + c.position.y;\n ldz = rot[6] * ox + rot[7] * oy + rot[8] * oz + c.position.z;\n out.set(ldx, ldy, ldz);\n },\n\n detectCollision: function (shape1, shape2, manifold) {\n\n var b;\n var c;\n if (this.flip) {\n b = shape2;\n c = shape1;\n } else {\n b = shape1;\n c = shape2;\n }\n var sep = new Vec3();\n var pos = new Vec3();\n var dep = new Vec3();\n\n if (!this.getSep(b, c, sep, pos, dep)) return;\n var pbx = b.position.x;\n var pby = b.position.y;\n var pbz = b.position.z;\n var pcx = c.position.x;\n var pcy = c.position.y;\n var pcz = c.position.z;\n var bw = b.halfWidth;\n var bh = b.halfHeight;\n var bd = b.halfDepth;\n var ch = c.halfHeight;\n var r = c.radius;\n\n var D = b.dimentions;\n\n var nwx = D[0]; //b.normalDirectionWidth.x;\n var nwy = D[1]; //b.normalDirectionWidth.y;\n var nwz = D[2]; //b.normalDirectionWidth.z;\n var nhx = D[3]; //b.normalDirectionHeight.x;\n var nhy = D[4]; //b.normalDirectionHeight.y;\n var nhz = D[5]; //b.normalDirectionHeight.z;\n var ndx = D[6]; //b.normalDirectionDepth.x;\n var ndy = D[7]; //b.normalDirectionDepth.y;\n var ndz = D[8]; //b.normalDirectionDepth.z;\n\n var dwx = D[9]; //b.halfDirectionWidth.x;\n var dwy = D[10]; //b.halfDirectionWidth.y;\n var dwz = D[11]; //b.halfDirectionWidth.z;\n var dhx = D[12]; //b.halfDirectionHeight.x;\n var dhy = D[13]; //b.halfDirectionHeight.y;\n var dhz = D[14]; //b.halfDirectionHeight.z;\n var ddx = D[15]; //b.halfDirectionDepth.x;\n var ddy = D[16]; //b.halfDirectionDepth.y;\n var ddz = D[17]; //b.halfDirectionDepth.z;\n\n var ncx = c.normalDirection.x;\n var ncy = c.normalDirection.y;\n var ncz = c.normalDirection.z;\n var dcx = c.halfDirection.x;\n var dcy = c.halfDirection.y;\n var dcz = c.halfDirection.z;\n var nx = sep.x;\n var ny = sep.y;\n var nz = sep.z;\n var dotw = nx * nwx + ny * nwy + nz * nwz;\n var doth = nx * nhx + ny * nhy + nz * nhz;\n var dotd = nx * ndx + ny * ndy + nz * ndz;\n var dotc = nx * ncx + ny * ncy + nz * ncz;\n var right1 = dotw > 0;\n var right2 = doth > 0;\n var right3 = dotd > 0;\n var right4 = dotc > 0;\n if (!right1) dotw = -dotw;\n if (!right2) doth = -doth;\n if (!right3) dotd = -dotd;\n if (!right4) dotc = -dotc;\n var state = 0;\n if (dotc > 0.999) {\n if (dotw > 0.999) {\n if (dotw > dotc) state = 1;else state = 4;\n } else if (doth > 0.999) {\n if (doth > dotc) state = 2;else state = 4;\n } else if (dotd > 0.999) {\n if (dotd > dotc) state = 3;else state = 4;\n } else state = 4;\n } else {\n if (dotw > 0.999) state = 1;else if (doth > 0.999) state = 2;else if (dotd > 0.999) state = 3;\n }\n var cbx;\n var cby;\n var cbz;\n var ccx;\n var ccy;\n var ccz;\n var r00;\n var r01;\n var r02;\n var r10;\n var r11;\n var r12;\n var r20;\n var r21;\n var r22;\n var px;\n var py;\n var pz;\n var pd;\n var dot;\n var len;\n var tx;\n var ty;\n var tz;\n var td;\n var dx;\n var dy;\n var dz;\n var d1x;\n var d1y;\n var d1z;\n var d2x;\n var d2y;\n var d2z;\n var sx;\n var sy;\n var sz;\n var sd;\n var ex;\n var ey;\n var ez;\n var ed;\n var dot1;\n var dot2;\n var t1;\n var dir1x;\n var dir1y;\n var dir1z;\n var dir2x;\n var dir2y;\n var dir2z;\n var dir1l;\n var dir2l;\n if (state == 0) {\n //manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,dep.x,b,c,0,0,false);\n manifold.addPoint(pos.x, pos.y, pos.z, nx, ny, nz, dep.x, this.flip);\n } else if (state == 4) {\n if (right4) {\n ccx = pcx - dcx;\n ccy = pcy - dcy;\n ccz = pcz - dcz;\n nx = -ncx;\n ny = -ncy;\n nz = -ncz;\n } else {\n ccx = pcx + dcx;\n ccy = pcy + dcy;\n ccz = pcz + dcz;\n nx = ncx;\n ny = ncy;\n nz = ncz;\n }\n var v1x;\n var v1y;\n var v1z;\n var v2x;\n var v2y;\n var v2z;\n var v3x;\n var v3y;\n var v3z;\n var v4x;\n var v4y;\n var v4z;\n\n dot = 1;\n state = 0;\n dot1 = nwx * nx + nwy * ny + nwz * nz;\n if (dot1 < dot) {\n dot = dot1;\n state = 0;\n }\n if (-dot1 < dot) {\n dot = -dot1;\n state = 1;\n }\n dot1 = nhx * nx + nhy * ny + nhz * nz;\n if (dot1 < dot) {\n dot = dot1;\n state = 2;\n }\n if (-dot1 < dot) {\n dot = -dot1;\n state = 3;\n }\n dot1 = ndx * nx + ndy * ny + ndz * nz;\n if (dot1 < dot) {\n dot = dot1;\n state = 4;\n }\n if (-dot1 < dot) {\n dot = -dot1;\n state = 5;\n }\n var v = b.elements;\n switch (state) {\n case 0:\n //v=b.vertex1;\n v1x = v[0]; //v.x;\n v1y = v[1]; //v.y;\n v1z = v[2]; //v.z;\n //v=b.vertex3;\n v2x = v[6]; //v.x;\n v2y = v[7]; //v.y;\n v2z = v[8]; //v.z;\n //v=b.vertex4;\n v3x = v[9]; //v.x;\n v3y = v[10]; //v.y;\n v3z = v[11]; //v.z;\n //v=b.vertex2;\n v4x = v[3]; //v.x;\n v4y = v[4]; //v.y;\n v4z = v[5]; //v.z;\n break;\n case 1:\n //v=b.vertex6;\n v1x = v[15]; //v.x;\n v1y = v[16]; //v.y;\n v1z = v[17]; //v.z;\n //v=b.vertex8;\n v2x = v[21]; //v.x;\n v2y = v[22]; //v.y;\n v2z = v[23]; //v.z;\n //v=b.vertex7;\n v3x = v[18]; //v.x;\n v3y = v[19]; //v.y;\n v3z = v[20]; //v.z;\n //v=b.vertex5;\n v4x = v[12]; //v.x;\n v4y = v[13]; //v.y;\n v4z = v[14]; //v.z;\n break;\n case 2:\n //v=b.vertex5;\n v1x = v[12]; //v.x;\n v1y = v[13]; //v.y;\n v1z = v[14]; //v.z;\n //v=b.vertex1;\n v2x = v[0]; //v.x;\n v2y = v[1]; //v.y;\n v2z = v[2]; //v.z;\n //v=b.vertex2;\n v3x = v[3]; //v.x;\n v3y = v[4]; //v.y;\n v3z = v[5]; //v.z;\n //v=b.vertex6;\n v4x = v[15]; //v.x;\n v4y = v[16]; //v.y;\n v4z = v[17]; //v.z;\n break;\n case 3:\n //v=b.vertex8;\n v1x = v[21]; //v.x;\n v1y = v[22]; //v.y;\n v1z = v[23]; //v.z;\n //v=b.vertex4;\n v2x = v[9]; //v.x;\n v2y = v[10]; //v.y;\n v2z = v[11]; //v.z;\n //v=b.vertex3;\n v3x = v[6]; //v.x;\n v3y = v[7]; //v.y;\n v3z = v[8]; //v.z;\n //v=b.vertex7;\n v4x = v[18]; //v.x;\n v4y = v[19]; //v.y;\n v4z = v[20]; //v.z;\n break;\n case 4:\n //v=b.vertex5;\n v1x = v[12]; //v.x;\n v1y = v[13]; //v.y;\n v1z = v[14]; //v.z;\n //v=b.vertex7;\n v2x = v[18]; //v.x;\n v2y = v[19]; //v.y;\n v2z = v[20]; //v.z;\n //v=b.vertex3;\n v3x = v[6]; //v.x;\n v3y = v[7]; //v.y;\n v3z = v[8]; //v.z;\n //v=b.vertex1;\n v4x = v[0]; //v.x;\n v4y = v[1]; //v.y;\n v4z = v[2]; //v.z;\n break;\n case 5:\n //v=b.vertex2;\n v1x = v[3]; //v.x;\n v1y = v[4]; //v.y;\n v1z = v[5]; //v.z;\n //v=b.vertex4;\n v2x = v[9]; //v.x;\n v2y = v[10]; //v.y;\n v2z = v[11]; //v.z;\n //v=b.vertex8;\n v3x = v[21]; //v.x;\n v3y = v[22]; //v.y;\n v3z = v[23]; //v.z;\n //v=b.vertex6;\n v4x = v[15]; //v.x;\n v4y = v[16]; //v.y;\n v4z = v[17]; //v.z;\n break;\n }\n pd = nx * (v1x - ccx) + ny * (v1y - ccy) + nz * (v1z - ccz);\n if (pd <= 0) manifold.addPoint(v1x, v1y, v1z, -nx, -ny, -nz, pd, this.flip);\n pd = nx * (v2x - ccx) + ny * (v2y - ccy) + nz * (v2z - ccz);\n if (pd <= 0) manifold.addPoint(v2x, v2y, v2z, -nx, -ny, -nz, pd, this.flip);\n pd = nx * (v3x - ccx) + ny * (v3y - ccy) + nz * (v3z - ccz);\n if (pd <= 0) manifold.addPoint(v3x, v3y, v3z, -nx, -ny, -nz, pd, this.flip);\n pd = nx * (v4x - ccx) + ny * (v4y - ccy) + nz * (v4z - ccz);\n if (pd <= 0) manifold.addPoint(v4x, v4y, v4z, -nx, -ny, -nz, pd, this.flip);\n } else {\n switch (state) {\n case 1:\n if (right1) {\n cbx = pbx + dwx;\n cby = pby + dwy;\n cbz = pbz + dwz;\n nx = nwx;\n ny = nwy;\n nz = nwz;\n } else {\n cbx = pbx - dwx;\n cby = pby - dwy;\n cbz = pbz - dwz;\n nx = -nwx;\n ny = -nwy;\n nz = -nwz;\n }\n dir1x = nhx;\n dir1y = nhy;\n dir1z = nhz;\n dir1l = bh;\n dir2x = ndx;\n dir2y = ndy;\n dir2z = ndz;\n dir2l = bd;\n break;\n case 2:\n if (right2) {\n cbx = pbx + dhx;\n cby = pby + dhy;\n cbz = pbz + dhz;\n nx = nhx;\n ny = nhy;\n nz = nhz;\n } else {\n cbx = pbx - dhx;\n cby = pby - dhy;\n cbz = pbz - dhz;\n nx = -nhx;\n ny = -nhy;\n nz = -nhz;\n }\n dir1x = nwx;\n dir1y = nwy;\n dir1z = nwz;\n dir1l = bw;\n dir2x = ndx;\n dir2y = ndy;\n dir2z = ndz;\n dir2l = bd;\n break;\n case 3:\n if (right3) {\n cbx = pbx + ddx;\n cby = pby + ddy;\n cbz = pbz + ddz;\n nx = ndx;\n ny = ndy;\n nz = ndz;\n } else {\n cbx = pbx - ddx;\n cby = pby - ddy;\n cbz = pbz - ddz;\n nx = -ndx;\n ny = -ndy;\n nz = -ndz;\n }\n dir1x = nwx;\n dir1y = nwy;\n dir1z = nwz;\n dir1l = bw;\n dir2x = nhx;\n dir2y = nhy;\n dir2z = nhz;\n dir2l = bh;\n break;\n }\n dot = nx * ncx + ny * ncy + nz * ncz;\n if (dot < 0) len = ch;else len = -ch;\n ccx = pcx + len * ncx;\n ccy = pcy + len * ncy;\n ccz = pcz + len * ncz;\n if (dotc >= 0.999999) {\n tx = -ny;\n ty = nz;\n tz = nx;\n } else {\n tx = nx;\n ty = ny;\n tz = nz;\n }\n len = tx * ncx + ty * ncy + tz * ncz;\n dx = len * ncx - tx;\n dy = len * ncy - ty;\n dz = len * ncz - tz;\n len = _Math.sqrt(dx * dx + dy * dy + dz * dz);\n if (len == 0) return;\n len = r / len;\n dx *= len;\n dy *= len;\n dz *= len;\n tx = ccx + dx;\n ty = ccy + dy;\n tz = ccz + dz;\n if (dot < -0.96 || dot > 0.96) {\n r00 = ncx * ncx * 1.5 - 0.5;\n r01 = ncx * ncy * 1.5 - ncz * 0.866025403;\n r02 = ncx * ncz * 1.5 + ncy * 0.866025403;\n r10 = ncy * ncx * 1.5 + ncz * 0.866025403;\n r11 = ncy * ncy * 1.5 - 0.5;\n r12 = ncy * ncz * 1.5 - ncx * 0.866025403;\n r20 = ncz * ncx * 1.5 - ncy * 0.866025403;\n r21 = ncz * ncy * 1.5 + ncx * 0.866025403;\n r22 = ncz * ncz * 1.5 - 0.5;\n px = tx;\n py = ty;\n pz = tz;\n pd = nx * (px - cbx) + ny * (py - cby) + nz * (pz - cbz);\n tx = px - pd * nx - cbx;\n ty = py - pd * ny - cby;\n tz = pz - pd * nz - cbz;\n sd = dir1x * tx + dir1y * ty + dir1z * tz;\n ed = dir2x * tx + dir2y * ty + dir2z * tz;\n if (sd < -dir1l) sd = -dir1l;else if (sd > dir1l) sd = dir1l;\n if (ed < -dir2l) ed = -dir2l;else if (ed > dir2l) ed = dir2l;\n tx = sd * dir1x + ed * dir2x;\n ty = sd * dir1y + ed * dir2y;\n tz = sd * dir1z + ed * dir2z;\n px = cbx + tx;\n py = cby + ty;\n pz = cbz + tz;\n manifold.addPoint(px, py, pz, nx, ny, nz, pd, this.flip);\n px = dx * r00 + dy * r01 + dz * r02;\n py = dx * r10 + dy * r11 + dz * r12;\n pz = dx * r20 + dy * r21 + dz * r22;\n px = (dx = px) + ccx;\n py = (dy = py) + ccy;\n pz = (dz = pz) + ccz;\n pd = nx * (px - cbx) + ny * (py - cby) + nz * (pz - cbz);\n if (pd <= 0) {\n tx = px - pd * nx - cbx;\n ty = py - pd * ny - cby;\n tz = pz - pd * nz - cbz;\n sd = dir1x * tx + dir1y * ty + dir1z * tz;\n ed = dir2x * tx + dir2y * ty + dir2z * tz;\n if (sd < -dir1l) sd = -dir1l;else if (sd > dir1l) sd = dir1l;\n if (ed < -dir2l) ed = -dir2l;else if (ed > dir2l) ed = dir2l;\n tx = sd * dir1x + ed * dir2x;\n ty = sd * dir1y + ed * dir2y;\n tz = sd * dir1z + ed * dir2z;\n px = cbx + tx;\n py = cby + ty;\n pz = cbz + tz;\n //manifold.addPoint(px,py,pz,nx,ny,nz,pd,b,c,2,0,false);\n manifold.addPoint(px, py, pz, nx, ny, nz, pd, this.flip);\n }\n px = dx * r00 + dy * r01 + dz * r02;\n py = dx * r10 + dy * r11 + dz * r12;\n pz = dx * r20 + dy * r21 + dz * r22;\n px = (dx = px) + ccx;\n py = (dy = py) + ccy;\n pz = (dz = pz) + ccz;\n pd = nx * (px - cbx) + ny * (py - cby) + nz * (pz - cbz);\n if (pd <= 0) {\n tx = px - pd * nx - cbx;\n ty = py - pd * ny - cby;\n tz = pz - pd * nz - cbz;\n sd = dir1x * tx + dir1y * ty + dir1z * tz;\n ed = dir2x * tx + dir2y * ty + dir2z * tz;\n if (sd < -dir1l) sd = -dir1l;else if (sd > dir1l) sd = dir1l;\n if (ed < -dir2l) ed = -dir2l;else if (ed > dir2l) ed = dir2l;\n tx = sd * dir1x + ed * dir2x;\n ty = sd * dir1y + ed * dir2y;\n tz = sd * dir1z + ed * dir2z;\n px = cbx + tx;\n py = cby + ty;\n pz = cbz + tz;\n //manifold.addPoint(px,py,pz,nx,ny,nz,pd,b,c,3,0,false);\n manifold.addPoint(px, py, pz, nx, ny, nz, pd, this.flip);\n }\n } else {\n sx = tx;\n sy = ty;\n sz = tz;\n sd = nx * (sx - cbx) + ny * (sy - cby) + nz * (sz - cbz);\n sx -= sd * nx;\n sy -= sd * ny;\n sz -= sd * nz;\n if (dot > 0) {\n ex = tx + dcx * 2;\n ey = ty + dcy * 2;\n ez = tz + dcz * 2;\n } else {\n ex = tx - dcx * 2;\n ey = ty - dcy * 2;\n ez = tz - dcz * 2;\n }\n ed = nx * (ex - cbx) + ny * (ey - cby) + nz * (ez - cbz);\n ex -= ed * nx;\n ey -= ed * ny;\n ez -= ed * nz;\n d1x = sx - cbx;\n d1y = sy - cby;\n d1z = sz - cbz;\n d2x = ex - cbx;\n d2y = ey - cby;\n d2z = ez - cbz;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n td = ed - sd;\n dotw = d1x * dir1x + d1y * dir1y + d1z * dir1z;\n doth = d2x * dir1x + d2y * dir1y + d2z * dir1z;\n dot1 = dotw - dir1l;\n dot2 = doth - dir1l;\n if (dot1 > 0) {\n if (dot2 > 0) return;\n t1 = dot1 / (dot1 - dot2);\n sx = sx + tx * t1;\n sy = sy + ty * t1;\n sz = sz + tz * t1;\n sd = sd + td * t1;\n d1x = sx - cbx;\n d1y = sy - cby;\n d1z = sz - cbz;\n dotw = d1x * dir1x + d1y * dir1y + d1z * dir1z;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n td = ed - sd;\n } else if (dot2 > 0) {\n t1 = dot1 / (dot1 - dot2);\n ex = sx + tx * t1;\n ey = sy + ty * t1;\n ez = sz + tz * t1;\n ed = sd + td * t1;\n d2x = ex - cbx;\n d2y = ey - cby;\n d2z = ez - cbz;\n doth = d2x * dir1x + d2y * dir1y + d2z * dir1z;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n td = ed - sd;\n }\n dot1 = dotw + dir1l;\n dot2 = doth + dir1l;\n if (dot1 < 0) {\n if (dot2 < 0) return;\n t1 = dot1 / (dot1 - dot2);\n sx = sx + tx * t1;\n sy = sy + ty * t1;\n sz = sz + tz * t1;\n sd = sd + td * t1;\n d1x = sx - cbx;\n d1y = sy - cby;\n d1z = sz - cbz;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n td = ed - sd;\n } else if (dot2 < 0) {\n t1 = dot1 / (dot1 - dot2);\n ex = sx + tx * t1;\n ey = sy + ty * t1;\n ez = sz + tz * t1;\n ed = sd + td * t1;\n d2x = ex - cbx;\n d2y = ey - cby;\n d2z = ez - cbz;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n td = ed - sd;\n }\n dotw = d1x * dir2x + d1y * dir2y + d1z * dir2z;\n doth = d2x * dir2x + d2y * dir2y + d2z * dir2z;\n dot1 = dotw - dir2l;\n dot2 = doth - dir2l;\n if (dot1 > 0) {\n if (dot2 > 0) return;\n t1 = dot1 / (dot1 - dot2);\n sx = sx + tx * t1;\n sy = sy + ty * t1;\n sz = sz + tz * t1;\n sd = sd + td * t1;\n d1x = sx - cbx;\n d1y = sy - cby;\n d1z = sz - cbz;\n dotw = d1x * dir2x + d1y * dir2y + d1z * dir2z;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n td = ed - sd;\n } else if (dot2 > 0) {\n t1 = dot1 / (dot1 - dot2);\n ex = sx + tx * t1;\n ey = sy + ty * t1;\n ez = sz + tz * t1;\n ed = sd + td * t1;\n d2x = ex - cbx;\n d2y = ey - cby;\n d2z = ez - cbz;\n doth = d2x * dir2x + d2y * dir2y + d2z * dir2z;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n td = ed - sd;\n }\n dot1 = dotw + dir2l;\n dot2 = doth + dir2l;\n if (dot1 < 0) {\n if (dot2 < 0) return;\n t1 = dot1 / (dot1 - dot2);\n sx = sx + tx * t1;\n sy = sy + ty * t1;\n sz = sz + tz * t1;\n sd = sd + td * t1;\n } else if (dot2 < 0) {\n t1 = dot1 / (dot1 - dot2);\n ex = sx + tx * t1;\n ey = sy + ty * t1;\n ez = sz + tz * t1;\n ed = sd + td * t1;\n }\n if (sd < 0) {\n //manifold.addPoint(sx,sy,sz,nx,ny,nz,sd,b,c,1,0,false);\n manifold.addPoint(sx, sy, sz, nx, ny, nz, sd, this.flip);\n }\n if (ed < 0) {\n //manifold.addPoint(ex,ey,ez,nx,ny,nz,ed,b,c,4,0,false);\n manifold.addPoint(ex, ey, ez, nx, ny, nz, ed, this.flip);\n }\n }\n }\n }\n\n});\n\nfunction CylinderCylinderCollisionDetector() {\n\n CollisionDetector.call(this);\n}\n\nCylinderCylinderCollisionDetector.prototype = Object.assign(Object.create(CollisionDetector.prototype), {\n\n constructor: CylinderCylinderCollisionDetector,\n\n getSep: function (c1, c2, sep, pos, dep) {\n\n var t1x;\n var t1y;\n var t1z;\n var t2x;\n var t2y;\n var t2z;\n var sup = new Vec3();\n var len;\n var p1x;\n var p1y;\n var p1z;\n var p2x;\n var p2y;\n var p2z;\n var v01x = c1.position.x;\n var v01y = c1.position.y;\n var v01z = c1.position.z;\n var v02x = c2.position.x;\n var v02y = c2.position.y;\n var v02z = c2.position.z;\n var v0x = v02x - v01x;\n var v0y = v02y - v01y;\n var v0z = v02z - v01z;\n if (v0x * v0x + v0y * v0y + v0z * v0z == 0) v0y = 0.001;\n var nx = -v0x;\n var ny = -v0y;\n var nz = -v0z;\n this.supportPoint(c1, -nx, -ny, -nz, sup);\n var v11x = sup.x;\n var v11y = sup.y;\n var v11z = sup.z;\n this.supportPoint(c2, nx, ny, nz, sup);\n var v12x = sup.x;\n var v12y = sup.y;\n var v12z = sup.z;\n var v1x = v12x - v11x;\n var v1y = v12y - v11y;\n var v1z = v12z - v11z;\n if (v1x * nx + v1y * ny + v1z * nz <= 0) {\n return false;\n }\n nx = v1y * v0z - v1z * v0y;\n ny = v1z * v0x - v1x * v0z;\n nz = v1x * v0y - v1y * v0x;\n if (nx * nx + ny * ny + nz * nz == 0) {\n sep.set(v1x - v0x, v1y - v0y, v1z - v0z).normalize();\n pos.set((v11x + v12x) * 0.5, (v11y + v12y) * 0.5, (v11z + v12z) * 0.5);\n return true;\n }\n this.supportPoint(c1, -nx, -ny, -nz, sup);\n var v21x = sup.x;\n var v21y = sup.y;\n var v21z = sup.z;\n this.supportPoint(c2, nx, ny, nz, sup);\n var v22x = sup.x;\n var v22y = sup.y;\n var v22z = sup.z;\n var v2x = v22x - v21x;\n var v2y = v22y - v21y;\n var v2z = v22z - v21z;\n if (v2x * nx + v2y * ny + v2z * nz <= 0) {\n return false;\n }\n t1x = v1x - v0x;\n t1y = v1y - v0y;\n t1z = v1z - v0z;\n t2x = v2x - v0x;\n t2y = v2y - v0y;\n t2z = v2z - v0z;\n nx = t1y * t2z - t1z * t2y;\n ny = t1z * t2x - t1x * t2z;\n nz = t1x * t2y - t1y * t2x;\n if (nx * v0x + ny * v0y + nz * v0z > 0) {\n t1x = v1x;\n t1y = v1y;\n t1z = v1z;\n v1x = v2x;\n v1y = v2y;\n v1z = v2z;\n v2x = t1x;\n v2y = t1y;\n v2z = t1z;\n t1x = v11x;\n t1y = v11y;\n t1z = v11z;\n v11x = v21x;\n v11y = v21y;\n v11z = v21z;\n v21x = t1x;\n v21y = t1y;\n v21z = t1z;\n t1x = v12x;\n t1y = v12y;\n t1z = v12z;\n v12x = v22x;\n v12y = v22y;\n v12z = v22z;\n v22x = t1x;\n v22y = t1y;\n v22z = t1z;\n nx = -nx;\n ny = -ny;\n nz = -nz;\n }\n var iterations = 0;\n while (true) {\n if (++iterations > 100) {\n return false;\n }\n this.supportPoint(c1, -nx, -ny, -nz, sup);\n var v31x = sup.x;\n var v31y = sup.y;\n var v31z = sup.z;\n this.supportPoint(c2, nx, ny, nz, sup);\n var v32x = sup.x;\n var v32y = sup.y;\n var v32z = sup.z;\n var v3x = v32x - v31x;\n var v3y = v32y - v31y;\n var v3z = v32z - v31z;\n if (v3x * nx + v3y * ny + v3z * nz <= 0) {\n return false;\n }\n if ((v1y * v3z - v1z * v3y) * v0x + (v1z * v3x - v1x * v3z) * v0y + (v1x * v3y - v1y * v3x) * v0z < 0) {\n v2x = v3x;\n v2y = v3y;\n v2z = v3z;\n v21x = v31x;\n v21y = v31y;\n v21z = v31z;\n v22x = v32x;\n v22y = v32y;\n v22z = v32z;\n t1x = v1x - v0x;\n t1y = v1y - v0y;\n t1z = v1z - v0z;\n t2x = v3x - v0x;\n t2y = v3y - v0y;\n t2z = v3z - v0z;\n nx = t1y * t2z - t1z * t2y;\n ny = t1z * t2x - t1x * t2z;\n nz = t1x * t2y - t1y * t2x;\n continue;\n }\n if ((v3y * v2z - v3z * v2y) * v0x + (v3z * v2x - v3x * v2z) * v0y + (v3x * v2y - v3y * v2x) * v0z < 0) {\n v1x = v3x;\n v1y = v3y;\n v1z = v3z;\n v11x = v31x;\n v11y = v31y;\n v11z = v31z;\n v12x = v32x;\n v12y = v32y;\n v12z = v32z;\n t1x = v3x - v0x;\n t1y = v3y - v0y;\n t1z = v3z - v0z;\n t2x = v2x - v0x;\n t2y = v2y - v0y;\n t2z = v2z - v0z;\n nx = t1y * t2z - t1z * t2y;\n ny = t1z * t2x - t1x * t2z;\n nz = t1x * t2y - t1y * t2x;\n continue;\n }\n var hit = false;\n while (true) {\n t1x = v2x - v1x;\n t1y = v2y - v1y;\n t1z = v2z - v1z;\n t2x = v3x - v1x;\n t2y = v3y - v1y;\n t2z = v3z - v1z;\n nx = t1y * t2z - t1z * t2y;\n ny = t1z * t2x - t1x * t2z;\n nz = t1x * t2y - t1y * t2x;\n len = 1 / _Math.sqrt(nx * nx + ny * ny + nz * nz);\n nx *= len;\n ny *= len;\n nz *= len;\n if (nx * v1x + ny * v1y + nz * v1z >= 0 && !hit) {\n var b0 = (v1y * v2z - v1z * v2y) * v3x + (v1z * v2x - v1x * v2z) * v3y + (v1x * v2y - v1y * v2x) * v3z;\n var b1 = (v3y * v2z - v3z * v2y) * v0x + (v3z * v2x - v3x * v2z) * v0y + (v3x * v2y - v3y * v2x) * v0z;\n var b2 = (v0y * v1z - v0z * v1y) * v3x + (v0z * v1x - v0x * v1z) * v3y + (v0x * v1y - v0y * v1x) * v3z;\n var b3 = (v2y * v1z - v2z * v1y) * v0x + (v2z * v1x - v2x * v1z) * v0y + (v2x * v1y - v2y * v1x) * v0z;\n var sum = b0 + b1 + b2 + b3;\n if (sum <= 0) {\n b0 = 0;\n b1 = (v2y * v3z - v2z * v3y) * nx + (v2z * v3x - v2x * v3z) * ny + (v2x * v3y - v2y * v3x) * nz;\n b2 = (v3y * v2z - v3z * v2y) * nx + (v3z * v2x - v3x * v2z) * ny + (v3x * v2y - v3y * v2x) * nz;\n b3 = (v1y * v2z - v1z * v2y) * nx + (v1z * v2x - v1x * v2z) * ny + (v1x * v2y - v1y * v2x) * nz;\n sum = b1 + b2 + b3;\n }\n var inv = 1 / sum;\n p1x = (v01x * b0 + v11x * b1 + v21x * b2 + v31x * b3) * inv;\n p1y = (v01y * b0 + v11y * b1 + v21y * b2 + v31y * b3) * inv;\n p1z = (v01z * b0 + v11z * b1 + v21z * b2 + v31z * b3) * inv;\n p2x = (v02x * b0 + v12x * b1 + v22x * b2 + v32x * b3) * inv;\n p2y = (v02y * b0 + v12y * b1 + v22y * b2 + v32y * b3) * inv;\n p2z = (v02z * b0 + v12z * b1 + v22z * b2 + v32z * b3) * inv;\n hit = true;\n }\n this.supportPoint(c1, -nx, -ny, -nz, sup);\n var v41x = sup.x;\n var v41y = sup.y;\n var v41z = sup.z;\n this.supportPoint(c2, nx, ny, nz, sup);\n var v42x = sup.x;\n var v42y = sup.y;\n var v42z = sup.z;\n var v4x = v42x - v41x;\n var v4y = v42y - v41y;\n var v4z = v42z - v41z;\n var separation = -(v4x * nx + v4y * ny + v4z * nz);\n if ((v4x - v3x) * nx + (v4y - v3y) * ny + (v4z - v3z) * nz <= 0.01 || separation >= 0) {\n if (hit) {\n sep.set(-nx, -ny, -nz);\n pos.set((p1x + p2x) * 0.5, (p1y + p2y) * 0.5, (p1z + p2z) * 0.5);\n dep.x = separation;\n return true;\n }\n return false;\n }\n if ((v4y * v1z - v4z * v1y) * v0x + (v4z * v1x - v4x * v1z) * v0y + (v4x * v1y - v4y * v1x) * v0z < 0) {\n if ((v4y * v2z - v4z * v2y) * v0x + (v4z * v2x - v4x * v2z) * v0y + (v4x * v2y - v4y * v2x) * v0z < 0) {\n v1x = v4x;\n v1y = v4y;\n v1z = v4z;\n v11x = v41x;\n v11y = v41y;\n v11z = v41z;\n v12x = v42x;\n v12y = v42y;\n v12z = v42z;\n } else {\n v3x = v4x;\n v3y = v4y;\n v3z = v4z;\n v31x = v41x;\n v31y = v41y;\n v31z = v41z;\n v32x = v42x;\n v32y = v42y;\n v32z = v42z;\n }\n } else {\n if ((v4y * v3z - v4z * v3y) * v0x + (v4z * v3x - v4x * v3z) * v0y + (v4x * v3y - v4y * v3x) * v0z < 0) {\n v2x = v4x;\n v2y = v4y;\n v2z = v4z;\n v21x = v41x;\n v21y = v41y;\n v21z = v41z;\n v22x = v42x;\n v22y = v42y;\n v22z = v42z;\n } else {\n v1x = v4x;\n v1y = v4y;\n v1z = v4z;\n v11x = v41x;\n v11y = v41y;\n v11z = v41z;\n v12x = v42x;\n v12y = v42y;\n v12z = v42z;\n }\n }\n }\n }\n //return false;\n },\n\n supportPoint: function (c, dx, dy, dz, out) {\n\n var rot = c.rotation.elements;\n var ldx = rot[0] * dx + rot[3] * dy + rot[6] * dz;\n var ldy = rot[1] * dx + rot[4] * dy + rot[7] * dz;\n var ldz = rot[2] * dx + rot[5] * dy + rot[8] * dz;\n var radx = ldx;\n var radz = ldz;\n var len = radx * radx + radz * radz;\n var rad = c.radius;\n var hh = c.halfHeight;\n var ox;\n var oy;\n var oz;\n if (len == 0) {\n if (ldy < 0) {\n ox = rad;\n oy = -hh;\n oz = 0;\n } else {\n ox = rad;\n oy = hh;\n oz = 0;\n }\n } else {\n len = c.radius / _Math.sqrt(len);\n if (ldy < 0) {\n ox = radx * len;\n oy = -hh;\n oz = radz * len;\n } else {\n ox = radx * len;\n oy = hh;\n oz = radz * len;\n }\n }\n ldx = rot[0] * ox + rot[1] * oy + rot[2] * oz + c.position.x;\n ldy = rot[3] * ox + rot[4] * oy + rot[5] * oz + c.position.y;\n ldz = rot[6] * ox + rot[7] * oy + rot[8] * oz + c.position.z;\n out.set(ldx, ldy, ldz);\n },\n\n detectCollision: function (shape1, shape2, manifold) {\n\n var c1;\n var c2;\n if (shape1.id < shape2.id) {\n c1 = shape1;\n c2 = shape2;\n } else {\n c1 = shape2;\n c2 = shape1;\n }\n var p1 = c1.position;\n var p2 = c2.position;\n var p1x = p1.x;\n var p1y = p1.y;\n var p1z = p1.z;\n var p2x = p2.x;\n var p2y = p2.y;\n var p2z = p2.z;\n var h1 = c1.halfHeight;\n var h2 = c2.halfHeight;\n var n1 = c1.normalDirection;\n var n2 = c2.normalDirection;\n var d1 = c1.halfDirection;\n var d2 = c2.halfDirection;\n var r1 = c1.radius;\n var r2 = c2.radius;\n var n1x = n1.x;\n var n1y = n1.y;\n var n1z = n1.z;\n var n2x = n2.x;\n var n2y = n2.y;\n var n2z = n2.z;\n var d1x = d1.x;\n var d1y = d1.y;\n var d1z = d1.z;\n var d2x = d2.x;\n var d2y = d2.y;\n var d2z = d2.z;\n var dx = p1x - p2x;\n var dy = p1y - p2y;\n var dz = p1z - p2z;\n var len;\n var c1x;\n var c1y;\n var c1z;\n var c2x;\n var c2y;\n var c2z;\n var tx;\n var ty;\n var tz;\n var sx;\n var sy;\n var sz;\n var ex;\n var ey;\n var ez;\n var depth1;\n var depth2;\n var dot;\n var t1;\n var t2;\n var sep = new Vec3();\n var pos = new Vec3();\n var dep = new Vec3();\n if (!this.getSep(c1, c2, sep, pos, dep)) return;\n var dot1 = sep.x * n1x + sep.y * n1y + sep.z * n1z;\n var dot2 = sep.x * n2x + sep.y * n2y + sep.z * n2z;\n var right1 = dot1 > 0;\n var right2 = dot2 > 0;\n if (!right1) dot1 = -dot1;\n if (!right2) dot2 = -dot2;\n var state = 0;\n if (dot1 > 0.999 || dot2 > 0.999) {\n if (dot1 > dot2) state = 1;else state = 2;\n }\n var nx;\n var ny;\n var nz;\n var depth = dep.x;\n var r00;\n var r01;\n var r02;\n var r10;\n var r11;\n var r12;\n var r20;\n var r21;\n var r22;\n var px;\n var py;\n var pz;\n var pd;\n var a;\n var b;\n var e;\n var f;\n nx = sep.x;\n ny = sep.y;\n nz = sep.z;\n switch (state) {\n case 0:\n manifold.addPoint(pos.x, pos.y, pos.z, nx, ny, nz, depth, false);\n break;\n case 1:\n if (right1) {\n c1x = p1x + d1x;\n c1y = p1y + d1y;\n c1z = p1z + d1z;\n nx = n1x;\n ny = n1y;\n nz = n1z;\n } else {\n c1x = p1x - d1x;\n c1y = p1y - d1y;\n c1z = p1z - d1z;\n nx = -n1x;\n ny = -n1y;\n nz = -n1z;\n }\n dot = nx * n2x + ny * n2y + nz * n2z;\n if (dot < 0) len = h2;else len = -h2;\n c2x = p2x + len * n2x;\n c2y = p2y + len * n2y;\n c2z = p2z + len * n2z;\n if (dot2 >= 0.999999) {\n tx = -ny;\n ty = nz;\n tz = nx;\n } else {\n tx = nx;\n ty = ny;\n tz = nz;\n }\n len = tx * n2x + ty * n2y + tz * n2z;\n dx = len * n2x - tx;\n dy = len * n2y - ty;\n dz = len * n2z - tz;\n len = _Math.sqrt(dx * dx + dy * dy + dz * dz);\n if (len == 0) break;\n len = r2 / len;\n dx *= len;\n dy *= len;\n dz *= len;\n tx = c2x + dx;\n ty = c2y + dy;\n tz = c2z + dz;\n if (dot < -0.96 || dot > 0.96) {\n r00 = n2x * n2x * 1.5 - 0.5;\n r01 = n2x * n2y * 1.5 - n2z * 0.866025403;\n r02 = n2x * n2z * 1.5 + n2y * 0.866025403;\n r10 = n2y * n2x * 1.5 + n2z * 0.866025403;\n r11 = n2y * n2y * 1.5 - 0.5;\n r12 = n2y * n2z * 1.5 - n2x * 0.866025403;\n r20 = n2z * n2x * 1.5 - n2y * 0.866025403;\n r21 = n2z * n2y * 1.5 + n2x * 0.866025403;\n r22 = n2z * n2z * 1.5 - 0.5;\n px = tx;\n py = ty;\n pz = tz;\n pd = nx * (px - c1x) + ny * (py - c1y) + nz * (pz - c1z);\n tx = px - pd * nx - c1x;\n ty = py - pd * ny - c1y;\n tz = pz - pd * nz - c1z;\n len = tx * tx + ty * ty + tz * tz;\n if (len > r1 * r1) {\n len = r1 / _Math.sqrt(len);\n tx *= len;\n ty *= len;\n tz *= len;\n }\n px = c1x + tx;\n py = c1y + ty;\n pz = c1z + tz;\n manifold.addPoint(px, py, pz, nx, ny, nz, pd, false);\n px = dx * r00 + dy * r01 + dz * r02;\n py = dx * r10 + dy * r11 + dz * r12;\n pz = dx * r20 + dy * r21 + dz * r22;\n px = (dx = px) + c2x;\n py = (dy = py) + c2y;\n pz = (dz = pz) + c2z;\n pd = nx * (px - c1x) + ny * (py - c1y) + nz * (pz - c1z);\n if (pd <= 0) {\n tx = px - pd * nx - c1x;\n ty = py - pd * ny - c1y;\n tz = pz - pd * nz - c1z;\n len = tx * tx + ty * ty + tz * tz;\n if (len > r1 * r1) {\n len = r1 / _Math.sqrt(len);\n tx *= len;\n ty *= len;\n tz *= len;\n }\n px = c1x + tx;\n py = c1y + ty;\n pz = c1z + tz;\n manifold.addPoint(px, py, pz, nx, ny, nz, pd, false);\n }\n px = dx * r00 + dy * r01 + dz * r02;\n py = dx * r10 + dy * r11 + dz * r12;\n pz = dx * r20 + dy * r21 + dz * r22;\n px = (dx = px) + c2x;\n py = (dy = py) + c2y;\n pz = (dz = pz) + c2z;\n pd = nx * (px - c1x) + ny * (py - c1y) + nz * (pz - c1z);\n if (pd <= 0) {\n tx = px - pd * nx - c1x;\n ty = py - pd * ny - c1y;\n tz = pz - pd * nz - c1z;\n len = tx * tx + ty * ty + tz * tz;\n if (len > r1 * r1) {\n len = r1 / _Math.sqrt(len);\n tx *= len;\n ty *= len;\n tz *= len;\n }\n px = c1x + tx;\n py = c1y + ty;\n pz = c1z + tz;\n manifold.addPoint(px, py, pz, nx, ny, nz, pd, false);\n }\n } else {\n sx = tx;\n sy = ty;\n sz = tz;\n depth1 = nx * (sx - c1x) + ny * (sy - c1y) + nz * (sz - c1z);\n sx -= depth1 * nx;\n sy -= depth1 * ny;\n sz -= depth1 * nz;\n if (dot > 0) {\n ex = tx + n2x * h2 * 2;\n ey = ty + n2y * h2 * 2;\n ez = tz + n2z * h2 * 2;\n } else {\n ex = tx - n2x * h2 * 2;\n ey = ty - n2y * h2 * 2;\n ez = tz - n2z * h2 * 2;\n }\n depth2 = nx * (ex - c1x) + ny * (ey - c1y) + nz * (ez - c1z);\n ex -= depth2 * nx;\n ey -= depth2 * ny;\n ez -= depth2 * nz;\n dx = c1x - sx;\n dy = c1y - sy;\n dz = c1z - sz;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n a = dx * dx + dy * dy + dz * dz;\n b = dx * tx + dy * ty + dz * tz;\n e = tx * tx + ty * ty + tz * tz;\n f = b * b - e * (a - r1 * r1);\n if (f < 0) break;\n f = _Math.sqrt(f);\n t1 = (b + f) / e;\n t2 = (b - f) / e;\n if (t2 < t1) {\n len = t1;\n t1 = t2;\n t2 = len;\n }\n if (t2 > 1) t2 = 1;\n if (t1 < 0) t1 = 0;\n tx = sx + (ex - sx) * t1;\n ty = sy + (ey - sy) * t1;\n tz = sz + (ez - sz) * t1;\n ex = sx + (ex - sx) * t2;\n ey = sy + (ey - sy) * t2;\n ez = sz + (ez - sz) * t2;\n sx = tx;\n sy = ty;\n sz = tz;\n len = depth1 + (depth2 - depth1) * t1;\n depth2 = depth1 + (depth2 - depth1) * t2;\n depth1 = len;\n if (depth1 < 0) manifold.addPoint(sx, sy, sz, nx, ny, nz, pd, false);\n if (depth2 < 0) manifold.addPoint(ex, ey, ez, nx, ny, nz, pd, false);\n }\n break;\n case 2:\n if (right2) {\n c2x = p2x - d2x;\n c2y = p2y - d2y;\n c2z = p2z - d2z;\n nx = -n2x;\n ny = -n2y;\n nz = -n2z;\n } else {\n c2x = p2x + d2x;\n c2y = p2y + d2y;\n c2z = p2z + d2z;\n nx = n2x;\n ny = n2y;\n nz = n2z;\n }\n dot = nx * n1x + ny * n1y + nz * n1z;\n if (dot < 0) len = h1;else len = -h1;\n c1x = p1x + len * n1x;\n c1y = p1y + len * n1y;\n c1z = p1z + len * n1z;\n if (dot1 >= 0.999999) {\n tx = -ny;\n ty = nz;\n tz = nx;\n } else {\n tx = nx;\n ty = ny;\n tz = nz;\n }\n len = tx * n1x + ty * n1y + tz * n1z;\n dx = len * n1x - tx;\n dy = len * n1y - ty;\n dz = len * n1z - tz;\n len = _Math.sqrt(dx * dx + dy * dy + dz * dz);\n if (len == 0) break;\n len = r1 / len;\n dx *= len;\n dy *= len;\n dz *= len;\n tx = c1x + dx;\n ty = c1y + dy;\n tz = c1z + dz;\n if (dot < -0.96 || dot > 0.96) {\n r00 = n1x * n1x * 1.5 - 0.5;\n r01 = n1x * n1y * 1.5 - n1z * 0.866025403;\n r02 = n1x * n1z * 1.5 + n1y * 0.866025403;\n r10 = n1y * n1x * 1.5 + n1z * 0.866025403;\n r11 = n1y * n1y * 1.5 - 0.5;\n r12 = n1y * n1z * 1.5 - n1x * 0.866025403;\n r20 = n1z * n1x * 1.5 - n1y * 0.866025403;\n r21 = n1z * n1y * 1.5 + n1x * 0.866025403;\n r22 = n1z * n1z * 1.5 - 0.5;\n px = tx;\n py = ty;\n pz = tz;\n pd = nx * (px - c2x) + ny * (py - c2y) + nz * (pz - c2z);\n tx = px - pd * nx - c2x;\n ty = py - pd * ny - c2y;\n tz = pz - pd * nz - c2z;\n len = tx * tx + ty * ty + tz * tz;\n if (len > r2 * r2) {\n len = r2 / _Math.sqrt(len);\n tx *= len;\n ty *= len;\n tz *= len;\n }\n px = c2x + tx;\n py = c2y + ty;\n pz = c2z + tz;\n manifold.addPoint(px, py, pz, -nx, -ny, -nz, pd, false);\n px = dx * r00 + dy * r01 + dz * r02;\n py = dx * r10 + dy * r11 + dz * r12;\n pz = dx * r20 + dy * r21 + dz * r22;\n px = (dx = px) + c1x;\n py = (dy = py) + c1y;\n pz = (dz = pz) + c1z;\n pd = nx * (px - c2x) + ny * (py - c2y) + nz * (pz - c2z);\n if (pd <= 0) {\n tx = px - pd * nx - c2x;\n ty = py - pd * ny - c2y;\n tz = pz - pd * nz - c2z;\n len = tx * tx + ty * ty + tz * tz;\n if (len > r2 * r2) {\n len = r2 / _Math.sqrt(len);\n tx *= len;\n ty *= len;\n tz *= len;\n }\n px = c2x + tx;\n py = c2y + ty;\n pz = c2z + tz;\n manifold.addPoint(px, py, pz, -nx, -ny, -nz, pd, false);\n }\n px = dx * r00 + dy * r01 + dz * r02;\n py = dx * r10 + dy * r11 + dz * r12;\n pz = dx * r20 + dy * r21 + dz * r22;\n px = (dx = px) + c1x;\n py = (dy = py) + c1y;\n pz = (dz = pz) + c1z;\n pd = nx * (px - c2x) + ny * (py - c2y) + nz * (pz - c2z);\n if (pd <= 0) {\n tx = px - pd * nx - c2x;\n ty = py - pd * ny - c2y;\n tz = pz - pd * nz - c2z;\n len = tx * tx + ty * ty + tz * tz;\n if (len > r2 * r2) {\n len = r2 / _Math.sqrt(len);\n tx *= len;\n ty *= len;\n tz *= len;\n }\n px = c2x + tx;\n py = c2y + ty;\n pz = c2z + tz;\n manifold.addPoint(px, py, pz, -nx, -ny, -nz, pd, false);\n }\n } else {\n sx = tx;\n sy = ty;\n sz = tz;\n depth1 = nx * (sx - c2x) + ny * (sy - c2y) + nz * (sz - c2z);\n sx -= depth1 * nx;\n sy -= depth1 * ny;\n sz -= depth1 * nz;\n if (dot > 0) {\n ex = tx + n1x * h1 * 2;\n ey = ty + n1y * h1 * 2;\n ez = tz + n1z * h1 * 2;\n } else {\n ex = tx - n1x * h1 * 2;\n ey = ty - n1y * h1 * 2;\n ez = tz - n1z * h1 * 2;\n }\n depth2 = nx * (ex - c2x) + ny * (ey - c2y) + nz * (ez - c2z);\n ex -= depth2 * nx;\n ey -= depth2 * ny;\n ez -= depth2 * nz;\n dx = c2x - sx;\n dy = c2y - sy;\n dz = c2z - sz;\n tx = ex - sx;\n ty = ey - sy;\n tz = ez - sz;\n a = dx * dx + dy * dy + dz * dz;\n b = dx * tx + dy * ty + dz * tz;\n e = tx * tx + ty * ty + tz * tz;\n f = b * b - e * (a - r2 * r2);\n if (f < 0) break;\n f = _Math.sqrt(f);\n t1 = (b + f) / e;\n t2 = (b - f) / e;\n if (t2 < t1) {\n len = t1;\n t1 = t2;\n t2 = len;\n }\n if (t2 > 1) t2 = 1;\n if (t1 < 0) t1 = 0;\n tx = sx + (ex - sx) * t1;\n ty = sy + (ey - sy) * t1;\n tz = sz + (ez - sz) * t1;\n ex = sx + (ex - sx) * t2;\n ey = sy + (ey - sy) * t2;\n ez = sz + (ez - sz) * t2;\n sx = tx;\n sy = ty;\n sz = tz;\n len = depth1 + (depth2 - depth1) * t1;\n depth2 = depth1 + (depth2 - depth1) * t2;\n depth1 = len;\n if (depth1 < 0) {\n manifold.addPoint(sx, sy, sz, -nx, -ny, -nz, depth1, false);\n }\n if (depth2 < 0) {\n manifold.addPoint(ex, ey, ez, -nx, -ny, -nz, depth2, false);\n }\n }\n break;\n }\n }\n\n});\n\n/**\n * A collision detector which detects collisions between sphere and box.\n * @author saharan\n */\nfunction SphereBoxCollisionDetector(flip) {\n\n CollisionDetector.call(this);\n this.flip = flip;\n}\n\nSphereBoxCollisionDetector.prototype = Object.assign(Object.create(CollisionDetector.prototype), {\n\n constructor: SphereBoxCollisionDetector,\n\n detectCollision: function (shape1, shape2, manifold) {\n\n var s;\n var b;\n if (this.flip) {\n s = shape2;\n b = shape1;\n } else {\n s = shape1;\n b = shape2;\n }\n\n var D = b.dimentions;\n\n var ps = s.position;\n var psx = ps.x;\n var psy = ps.y;\n var psz = ps.z;\n var pb = b.position;\n var pbx = pb.x;\n var pby = pb.y;\n var pbz = pb.z;\n var rad = s.radius;\n\n var hw = b.halfWidth;\n var hh = b.halfHeight;\n var hd = b.halfDepth;\n\n var dx = psx - pbx;\n var dy = psy - pby;\n var dz = psz - pbz;\n var sx = D[0] * dx + D[1] * dy + D[2] * dz;\n var sy = D[3] * dx + D[4] * dy + D[5] * dz;\n var sz = D[6] * dx + D[7] * dy + D[8] * dz;\n var cx;\n var cy;\n var cz;\n var len;\n var invLen;\n var overlap = 0;\n if (sx > hw) {\n sx = hw;\n } else if (sx < -hw) {\n sx = -hw;\n } else {\n overlap = 1;\n }\n if (sy > hh) {\n sy = hh;\n } else if (sy < -hh) {\n sy = -hh;\n } else {\n overlap |= 2;\n }\n if (sz > hd) {\n sz = hd;\n } else if (sz < -hd) {\n sz = -hd;\n } else {\n overlap |= 4;\n }\n if (overlap == 7) {\n // center of sphere is in the box\n if (sx < 0) {\n dx = hw + sx;\n } else {\n dx = hw - sx;\n }\n if (sy < 0) {\n dy = hh + sy;\n } else {\n dy = hh - sy;\n }\n if (sz < 0) {\n dz = hd + sz;\n } else {\n dz = hd - sz;\n }\n if (dx < dy) {\n if (dx < dz) {\n len = dx - hw;\n if (sx < 0) {\n sx = -hw;\n dx = D[0];\n dy = D[1];\n dz = D[2];\n } else {\n sx = hw;\n dx = -D[0];\n dy = -D[1];\n dz = -D[2];\n }\n } else {\n len = dz - hd;\n if (sz < 0) {\n sz = -hd;\n dx = D[6];\n dy = D[7];\n dz = D[8];\n } else {\n sz = hd;\n dx = -D[6];\n dy = -D[7];\n dz = -D[8];\n }\n }\n } else {\n if (dy < dz) {\n len = dy - hh;\n if (sy < 0) {\n sy = -hh;\n dx = D[3];\n dy = D[4];\n dz = D[5];\n } else {\n sy = hh;\n dx = -D[3];\n dy = -D[4];\n dz = -D[5];\n }\n } else {\n len = dz - hd;\n if (sz < 0) {\n sz = -hd;\n dx = D[6];\n dy = D[7];\n dz = D[8];\n } else {\n sz = hd;\n dx = -D[6];\n dy = -D[7];\n dz = -D[8];\n }\n }\n }\n cx = pbx + sx * D[0] + sy * D[3] + sz * D[6];\n cy = pby + sx * D[1] + sy * D[4] + sz * D[7];\n cz = pbz + sx * D[2] + sy * D[5] + sz * D[8];\n manifold.addPoint(psx + rad * dx, psy + rad * dy, psz + rad * dz, dx, dy, dz, len - rad, this.flip);\n } else {\n cx = pbx + sx * D[0] + sy * D[3] + sz * D[6];\n cy = pby + sx * D[1] + sy * D[4] + sz * D[7];\n cz = pbz + sx * D[2] + sy * D[5] + sz * D[8];\n dx = cx - ps.x;\n dy = cy - ps.y;\n dz = cz - ps.z;\n len = dx * dx + dy * dy + dz * dz;\n if (len > 0 && len < rad * rad) {\n len = _Math.sqrt(len);\n invLen = 1 / len;\n dx *= invLen;\n dy *= invLen;\n dz *= invLen;\n manifold.addPoint(psx + rad * dx, psy + rad * dy, psz + rad * dz, dx, dy, dz, len - rad, this.flip);\n }\n }\n }\n\n});\n\nfunction SphereCylinderCollisionDetector(flip) {\n\n CollisionDetector.call(this);\n this.flip = flip;\n}\n\nSphereCylinderCollisionDetector.prototype = Object.assign(Object.create(CollisionDetector.prototype), {\n\n constructor: SphereCylinderCollisionDetector,\n\n detectCollision: function (shape1, shape2, manifold) {\n\n var s;\n var c;\n if (this.flip) {\n s = shape2;\n c = shape1;\n } else {\n s = shape1;\n c = shape2;\n }\n var ps = s.position;\n var psx = ps.x;\n var psy = ps.y;\n var psz = ps.z;\n var pc = c.position;\n var pcx = pc.x;\n var pcy = pc.y;\n var pcz = pc.z;\n var dirx = c.normalDirection.x;\n var diry = c.normalDirection.y;\n var dirz = c.normalDirection.z;\n var rads = s.radius;\n var radc = c.radius;\n var rad2 = rads + radc;\n var halfh = c.halfHeight;\n var dx = psx - pcx;\n var dy = psy - pcy;\n var dz = psz - pcz;\n var dot = dx * dirx + dy * diry + dz * dirz;\n if (dot < -halfh - rads || dot > halfh + rads) return;\n var cx = pcx + dot * dirx;\n var cy = pcy + dot * diry;\n var cz = pcz + dot * dirz;\n var d2x = psx - cx;\n var d2y = psy - cy;\n var d2z = psz - cz;\n var len = d2x * d2x + d2y * d2y + d2z * d2z;\n if (len > rad2 * rad2) return;\n if (len > radc * radc) {\n len = radc / _Math.sqrt(len);\n d2x *= len;\n d2y *= len;\n d2z *= len;\n }\n if (dot < -halfh) dot = -halfh;else if (dot > halfh) dot = halfh;\n cx = pcx + dot * dirx + d2x;\n cy = pcy + dot * diry + d2y;\n cz = pcz + dot * dirz + d2z;\n dx = cx - psx;\n dy = cy - psy;\n dz = cz - psz;\n len = dx * dx + dy * dy + dz * dz;\n var invLen;\n if (len > 0 && len < rads * rads) {\n len = _Math.sqrt(len);\n invLen = 1 / len;\n dx *= invLen;\n dy *= invLen;\n dz *= invLen;\n ///result.addContactInfo(psx+dx*rads,psy+dy*rads,psz+dz*rads,dx,dy,dz,len-rads,s,c,0,0,false);\n manifold.addPoint(psx + dx * rads, psy + dy * rads, psz + dz * rads, dx, dy, dz, len - rads, this.flip);\n }\n }\n\n});\n\n/**\n * A collision detector which detects collisions between two spheres.\n * @author saharan\n */\n\nfunction SphereSphereCollisionDetector() {\n\n CollisionDetector.call(this);\n}\n\nSphereSphereCollisionDetector.prototype = Object.assign(Object.create(CollisionDetector.prototype), {\n\n constructor: SphereSphereCollisionDetector,\n\n detectCollision: function (shape1, shape2, manifold) {\n\n var s1 = shape1;\n var s2 = shape2;\n var p1 = s1.position;\n var p2 = s2.position;\n var dx = p2.x - p1.x;\n var dy = p2.y - p1.y;\n var dz = p2.z - p1.z;\n var len = dx * dx + dy * dy + dz * dz;\n var r1 = s1.radius;\n var r2 = s2.radius;\n var rad = r1 + r2;\n if (len > 0 && len < rad * rad) {\n len = _Math.sqrt(len);\n var invLen = 1 / len;\n dx *= invLen;\n dy *= invLen;\n dz *= invLen;\n manifold.addPoint(p1.x + dx * r1, p1.y + dy * r1, p1.z + dz * r1, dx, dy, dz, len - rad, false);\n }\n }\n\n});\n\n/**\r\n * A collision detector which detects collisions between two spheres.\r\n * @author saharan \r\n * @author lo-th\r\n */\n\nfunction SpherePlaneCollisionDetector(flip) {\n\n CollisionDetector.call(this);\n\n this.flip = flip;\n\n this.n = new Vec3();\n this.p = new Vec3();\n}\n\nSpherePlaneCollisionDetector.prototype = Object.assign(Object.create(CollisionDetector.prototype), {\n\n constructor: SpherePlaneCollisionDetector,\n\n detectCollision: function (shape1, shape2, manifold) {\n\n var n = this.n;\n var p = this.p;\n\n var s = this.flip ? shape2 : shape1;\n var pn = this.flip ? shape1 : shape2;\n var rad = s.radius;\n var len;\n\n n.sub(s.position, pn.position);\n //var h = _Math.dotVectors( pn.normal, n );\n\n n.x *= pn.normal.x; //+ rad;\n n.y *= pn.normal.y;\n n.z *= pn.normal.z; //+ rad;\n\n\n var len = n.lengthSq();\n\n if (len > 0 && len < rad * rad) {\n //&& h > rad*rad ){\n\n\n len = _Math.sqrt(len);\n //len = _Math.sqrt( h );\n n.copy(pn.normal).negate();\n //n.scaleEqual( 1/len );\n\n //(0, -1, 0)\n\n //n.normalize();\n p.copy(s.position).addScaledVector(n, rad);\n manifold.addPointVec(p, n, len - rad, this.flip);\n }\n }\n\n});\n\n/**\r\n * A collision detector which detects collisions between two spheres.\r\n * @author saharan \r\n * @author lo-th\r\n */\n\nfunction BoxPlaneCollisionDetector(flip) {\n\n CollisionDetector.call(this);\n\n this.flip = flip;\n\n this.n = new Vec3();\n this.p = new Vec3();\n\n this.dix = new Vec3();\n this.diy = new Vec3();\n this.diz = new Vec3();\n\n this.cc = new Vec3();\n this.cc2 = new Vec3();\n}\n\nBoxPlaneCollisionDetector.prototype = Object.assign(Object.create(CollisionDetector.prototype), {\n\n constructor: BoxPlaneCollisionDetector,\n\n detectCollision: function (shape1, shape2, manifold) {\n\n var n = this.n;\n var p = this.p;\n var cc = this.cc;\n\n var b = this.flip ? shape2 : shape1;\n var pn = this.flip ? shape1 : shape2;\n\n var D = b.dimentions;\n var hw = b.halfWidth;\n var hh = b.halfHeight;\n var hd = b.halfDepth;\n var len;\n var overlap = 0;\n\n this.dix.set(D[0], D[1], D[2]);\n this.diy.set(D[3], D[4], D[5]);\n this.diz.set(D[6], D[7], D[8]);\n\n n.sub(b.position, pn.position);\n\n n.x *= pn.normal.x; //+ rad;\n n.y *= pn.normal.y;\n n.z *= pn.normal.z; //+ rad;\n\n cc.set(_Math.dotVectors(this.dix, n), _Math.dotVectors(this.diy, n), _Math.dotVectors(this.diz, n));\n\n if (cc.x > hw) cc.x = hw;else if (cc.x < -hw) cc.x = -hw;else overlap = 1;\n\n if (cc.y > hh) cc.y = hh;else if (cc.y < -hh) cc.y = -hh;else overlap |= 2;\n\n if (cc.z > hd) cc.z = hd;else if (cc.z < -hd) cc.z = -hd;else overlap |= 4;\n\n if (overlap === 7) {\n\n // center of sphere is in the box\n\n 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);\n\n if (n.x < n.y) {\n if (n.x < n.z) {\n len = n.x - hw;\n if (cc.x < 0) {\n cc.x = -hw;\n n.copy(this.dix);\n } else {\n cc.x = hw;\n n.subEqual(this.dix);\n }\n } else {\n len = n.z - hd;\n if (cc.z < 0) {\n cc.z = -hd;\n n.copy(this.diz);\n } else {\n cc.z = hd;\n n.subEqual(this.diz);\n }\n }\n } else {\n if (n.y < n.z) {\n len = n.y - hh;\n if (cc.y < 0) {\n cc.y = -hh;\n n.copy(this.diy);\n } else {\n cc.y = hh;\n n.subEqual(this.diy);\n }\n } else {\n len = n.z - hd;\n if (cc.z < 0) {\n cc.z = -hd;\n n.copy(this.diz);\n } else {\n cc.z = hd;\n n.subEqual(this.diz);\n }\n }\n }\n\n p.copy(pn.position).addScaledVector(n, 1);\n manifold.addPointVec(p, n, len, this.flip);\n }\n }\n\n});\n\n//import { TetraShape } from '../collision/shape/TetraShape';\n\n/**\r\n * The class of physical computing world.\r\n * You must be added to the world physical all computing objects\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\n\n// timestep, broadphase, iterations, worldscale, random, stat\n\nfunction World(o) {\n\n if (!(o instanceof Object)) o = {};\n\n // this world scale defaut is 0.1 to 10 meters max for dynamique body\n this.scale = o.worldscale || 1;\n this.invScale = 1 / this.scale;\n\n // The time between each step\n this.timeStep = o.timestep || 0.01666; // 1/60;\n this.timerate = this.timeStep * 1000;\n this.timer = null;\n\n this.preLoop = null; //function(){};\n this.postLoop = null; //function(){};\n\n // The number of iterations for constraint solvers.\n this.numIterations = o.iterations || 8;\n\n // It is a wide-area collision judgment that is used in order to reduce as much as possible a detailed collision judgment.\n switch (o.broadphase || 2) {\n case 1:\n this.broadPhase = new BruteForceBroadPhase();break;\n case 2:default:\n this.broadPhase = new SAPBroadPhase();break;\n case 3:\n this.broadPhase = new DBVTBroadPhase();break;\n }\n\n this.Btypes = ['None', 'BruteForce', 'Sweep & Prune', 'Bounding Volume Tree'];\n this.broadPhaseType = this.Btypes[o.broadphase || 2];\n\n // This is the detailed information of the performance.\n this.performance = null;\n this.isStat = o.info === undefined ? false : o.info;\n if (this.isStat) this.performance = new InfoDisplay(this);\n\n /**\r\n * Whether the constraints randomizer is enabled or not.\r\n *\r\n * @property enableRandomizer\r\n * @type {Boolean}\r\n */\n this.enableRandomizer = o.random !== undefined ? o.random : true;\n\n // The rigid body list\n this.rigidBodies = null;\n // number of rigid body\n this.numRigidBodies = 0;\n // The contact list\n this.contacts = null;\n this.unusedContacts = null;\n // The number of contact\n this.numContacts = 0;\n // The number of contact points\n this.numContactPoints = 0;\n // The joint list\n this.joints = null;\n // The number of joints.\n this.numJoints = 0;\n // The number of simulation islands.\n this.numIslands = 0;\n\n // The gravity in the world.\n this.gravity = new Vec3(0, -9.8, 0);\n if (o.gravity !== undefined) this.gravity.fromArray(o.gravity);\n\n var numShapeTypes = 5; //4;//3;\n this.detectors = [];\n this.detectors.length = numShapeTypes;\n var i = numShapeTypes;\n while (i--) {\n this.detectors[i] = [];\n this.detectors[i].length = numShapeTypes;\n }\n\n this.detectors[SHAPE_SPHERE][SHAPE_SPHERE] = new SphereSphereCollisionDetector();\n this.detectors[SHAPE_SPHERE][SHAPE_BOX] = new SphereBoxCollisionDetector(false);\n this.detectors[SHAPE_BOX][SHAPE_SPHERE] = new SphereBoxCollisionDetector(true);\n this.detectors[SHAPE_BOX][SHAPE_BOX] = new BoxBoxCollisionDetector();\n\n // CYLINDER add\n this.detectors[SHAPE_CYLINDER][SHAPE_CYLINDER] = new CylinderCylinderCollisionDetector();\n\n this.detectors[SHAPE_CYLINDER][SHAPE_BOX] = new BoxCylinderCollisionDetector(true);\n this.detectors[SHAPE_BOX][SHAPE_CYLINDER] = new BoxCylinderCollisionDetector(false);\n\n this.detectors[SHAPE_CYLINDER][SHAPE_SPHERE] = new SphereCylinderCollisionDetector(true);\n this.detectors[SHAPE_SPHERE][SHAPE_CYLINDER] = new SphereCylinderCollisionDetector(false);\n\n // PLANE add\n\n this.detectors[SHAPE_PLANE][SHAPE_SPHERE] = new SpherePlaneCollisionDetector(true);\n this.detectors[SHAPE_SPHERE][SHAPE_PLANE] = new SpherePlaneCollisionDetector(false);\n\n this.detectors[SHAPE_PLANE][SHAPE_BOX] = new BoxPlaneCollisionDetector(true);\n this.detectors[SHAPE_BOX][SHAPE_PLANE] = new BoxPlaneCollisionDetector(false);\n\n // TETRA add\n //this.detectors[SHAPE_TETRA][SHAPE_TETRA] = new TetraTetraCollisionDetector();\n\n\n this.randX = 65535;\n this.randA = 98765;\n this.randB = 123456789;\n\n this.islandRigidBodies = [];\n this.islandStack = [];\n this.islandConstraints = [];\n}\n\nObject.assign(World.prototype, {\n\n World: true,\n\n play: function () {\n\n if (this.timer !== null) return;\n\n var _this = this;\n this.timer = setInterval(function () {\n _this.step();\n }, this.timerate);\n //this.timer = setInterval( this.loop.bind(this) , this.timerate );\n },\n\n stop: function () {\n\n if (this.timer === null) return;\n\n clearInterval(this.timer);\n this.timer = null;\n },\n\n setGravity: function (ar) {\n\n this.gravity.fromArray(ar);\n },\n\n getInfo: function () {\n\n return this.isStat ? this.performance.show() : '';\n },\n\n // Reset the world and remove all rigid bodies, shapes, joints and any object from the world.\n clear: function () {\n\n this.stop();\n this.preLoop = null;\n this.postLoop = null;\n\n this.randX = 65535;\n\n while (this.joints !== null) {\n this.removeJoint(this.joints);\n }\n while (this.contacts !== null) {\n this.removeContact(this.contacts);\n }\n while (this.rigidBodies !== null) {\n this.removeRigidBody(this.rigidBodies);\n }\n },\n /**\r\n * I'll add a rigid body to the world.\r\n * Rigid body that has been added will be the operands of each step.\r\n * @param rigidBody Rigid body that you want to add\r\n */\n addRigidBody: function (rigidBody) {\n\n if (rigidBody.parent) {\n printError(\"World\", \"It is not possible to be added to more than one world one of the rigid body\");\n }\n\n rigidBody.setParent(this);\n //rigidBody.awake();\n\n for (var shape = rigidBody.shapes; shape !== null; shape = shape.next) {\n this.addShape(shape);\n }\n if (this.rigidBodies !== null) (this.rigidBodies.prev = rigidBody).next = this.rigidBodies;\n this.rigidBodies = rigidBody;\n this.numRigidBodies++;\n },\n /**\r\n * I will remove the rigid body from the world.\r\n * Rigid body that has been deleted is excluded from the calculation on a step-by-step basis.\r\n * @param rigidBody Rigid body to be removed\r\n */\n removeRigidBody: function (rigidBody) {\n\n var remove = rigidBody;\n if (remove.parent !== this) return;\n remove.awake();\n var js = remove.jointLink;\n while (js != null) {\n var joint = js.joint;\n js = js.next;\n this.removeJoint(joint);\n }\n for (var shape = rigidBody.shapes; shape !== null; shape = shape.next) {\n this.removeShape(shape);\n }\n var prev = remove.prev;\n var next = remove.next;\n if (prev !== null) prev.next = next;\n if (next !== null) next.prev = prev;\n if (this.rigidBodies == remove) this.rigidBodies = next;\n remove.prev = null;\n remove.next = null;\n remove.parent = null;\n this.numRigidBodies--;\n },\n\n getByName: function (name) {\n\n var body = this.rigidBodies;\n while (body !== null) {\n if (body.name === name) return body;\n body = body.next;\n }\n\n var joint = this.joints;\n while (joint !== null) {\n if (joint.name === name) return joint;\n joint = joint.next;\n }\n\n return null;\n },\n\n /**\r\n * I'll add a shape to the world..\r\n * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world,\r\n * Shape will be added to the world automatically, please do not call from outside this method.\r\n * @param shape Shape you want to add\r\n */\n addShape: function (shape) {\n\n if (!shape.parent || !shape.parent.parent) {\n printError(\"World\", \"It is not possible to be added alone to shape world\");\n }\n\n shape.proxy = this.broadPhase.createProxy(shape);\n shape.updateProxy();\n this.broadPhase.addProxy(shape.proxy);\n },\n\n /**\r\n * I will remove the shape from the world.\r\n * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world,\r\n * Shape will be added to the world automatically, please do not call from outside this method.\r\n * @param shape Shape you want to delete\r\n */\n removeShape: function (shape) {\n\n this.broadPhase.removeProxy(shape.proxy);\n shape.proxy = null;\n },\n\n /**\r\n * I'll add a joint to the world.\r\n * Joint that has been added will be the operands of each step.\r\n * @param shape Joint to be added\r\n */\n addJoint: function (joint) {\n\n if (joint.parent) {\n printError(\"World\", \"It is not possible to be added to more than one world one of the joint\");\n }\n if (this.joints != null) (this.joints.prev = joint).next = this.joints;\n this.joints = joint;\n joint.setParent(this);\n this.numJoints++;\n joint.awake();\n joint.attach();\n },\n\n /**\r\n * I will remove the joint from the world.\r\n * Joint that has been added will be the operands of each step.\r\n * @param shape Joint to be deleted\r\n */\n removeJoint: function (joint) {\n\n var remove = joint;\n var prev = remove.prev;\n var next = remove.next;\n if (prev !== null) prev.next = next;\n if (next !== null) next.prev = prev;\n if (this.joints == remove) this.joints = next;\n remove.prev = null;\n remove.next = null;\n this.numJoints--;\n remove.awake();\n remove.detach();\n remove.parent = null;\n },\n\n addContact: function (s1, s2) {\n\n var newContact;\n if (this.unusedContacts !== null) {\n newContact = this.unusedContacts;\n this.unusedContacts = this.unusedContacts.next;\n } else {\n newContact = new Contact();\n }\n newContact.attach(s1, s2);\n newContact.detector = this.detectors[s1.type][s2.type];\n if (this.contacts) (this.contacts.prev = newContact).next = this.contacts;\n this.contacts = newContact;\n this.numContacts++;\n },\n\n removeContact: function (contact) {\n\n var prev = contact.prev;\n var next = contact.next;\n if (next) next.prev = prev;\n if (prev) prev.next = next;\n if (this.contacts == contact) this.contacts = next;\n contact.prev = null;\n contact.next = null;\n contact.detach();\n contact.next = this.unusedContacts;\n this.unusedContacts = contact;\n this.numContacts--;\n },\n\n getContact: function (b1, b2) {\n\n b1 = b1.constructor === RigidBody ? b1.name : b1;\n b2 = b2.constructor === RigidBody ? b2.name : b2;\n\n var n1, n2;\n var contact = this.contacts;\n while (contact !== null) {\n n1 = contact.body1.name;\n n2 = contact.body2.name;\n if (n1 === b1 && n2 === b2 || n2 === b1 && n1 === b2) {\n if (contact.touching) return contact;else return null;\n } else contact = contact.next;\n }\n return null;\n },\n\n checkContact: function (name1, name2) {\n\n var n1, n2;\n var contact = this.contacts;\n while (contact !== null) {\n n1 = contact.body1.name || ' ';\n n2 = contact.body2.name || ' ';\n if (n1 == name1 && n2 == name2 || n2 == name1 && n1 == name2) {\n if (contact.touching) return true;else return false;\n } else contact = contact.next;\n }\n //return false;\n },\n\n callSleep: function (body) {\n\n if (!body.allowSleep) return false;\n if (body.linearVelocity.lengthSq() > 0.04) return false;\n if (body.angularVelocity.lengthSq() > 0.25) return false;\n return true;\n },\n\n /**\r\n * I will proceed only time step seconds time of World.\r\n */\n step: function () {\n\n var stat = this.isStat;\n\n if (stat) this.performance.setTime(0);\n\n var body = this.rigidBodies;\n\n while (body !== null) {\n\n body.addedToIsland = false;\n\n if (body.sleeping) body.testWakeUp();\n\n body = body.next;\n }\n\n //------------------------------------------------------\n // UPDATE BROADPHASE CONTACT\n //------------------------------------------------------\n\n if (stat) this.performance.setTime(1);\n\n this.broadPhase.detectPairs();\n\n var pairs = this.broadPhase.pairs;\n\n var i = this.broadPhase.numPairs;\n //do{\n while (i--) {\n //for(var i=0, l=numPairs; i0);\n\n if (stat) this.performance.calcBroadPhase();\n\n //------------------------------------------------------\n // UPDATE NARROWPHASE CONTACT\n //------------------------------------------------------\n\n // update & narrow phase\n this.numContactPoints = 0;\n contact = this.contacts;\n while (contact !== null) {\n if (!contact.persisting) {\n if (contact.shape1.aabb.intersectTest(contact.shape2.aabb)) {\n /*var aabb1=contact.shape1.aabb;\r\n var aabb2=contact.shape2.aabb;\r\n if(\r\n aabb1.minX>aabb2.maxX || aabb1.maxXaabb2.maxY || aabb1.maxYaabb2.maxZ || aabb1.maxZ 0.5) base.sleep();else base.updatePosition(this.timeStep);\n } else {\n base.sleepTime = 0;\n base.updatePosition(this.timeStep);\n }\n this.numIslands++;\n continue;\n }\n\n var islandNumRigidBodies = 0;\n var islandNumConstraints = 0;\n var stackCount = 1;\n // add rigid body to stack\n this.islandStack[0] = base;\n base.addedToIsland = true;\n\n // build an island\n do {\n // get rigid body from stack\n body = this.islandStack[--stackCount];\n this.islandStack[stackCount] = null;\n body.sleeping = false;\n // add rigid body to the island\n this.islandRigidBodies[islandNumRigidBodies++] = body;\n if (body.isStatic) continue;\n\n // search connections\n for (var cs = body.contactLink; cs !== null; cs = cs.next) {\n var contact = cs.contact;\n constraint = contact.constraint;\n if (constraint.addedToIsland || !contact.touching) continue; // ignore\n\n // add constraint to the island\n this.islandConstraints[islandNumConstraints++] = constraint;\n constraint.addedToIsland = true;\n var next = cs.body;\n\n if (next.addedToIsland) continue;\n\n // add rigid body to stack\n this.islandStack[stackCount++] = next;\n next.addedToIsland = true;\n }\n for (var js = body.jointLink; js !== null; js = js.next) {\n constraint = js.joint;\n\n if (constraint.addedToIsland) continue; // ignore\n\n // add constraint to the island\n this.islandConstraints[islandNumConstraints++] = constraint;\n constraint.addedToIsland = true;\n next = js.body;\n if (next.addedToIsland || !next.isDynamic) continue;\n\n // add rigid body to stack\n this.islandStack[stackCount++] = next;\n next.addedToIsland = true;\n }\n } while (stackCount != 0);\n\n // update velocities\n var gVel = new Vec3().addScaledVector(this.gravity, this.timeStep);\n /*var gx=this.gravity.x*this.timeStep;\r\n var gy=this.gravity.y*this.timeStep;\r\n var gz=this.gravity.z*this.timeStep;*/\n var j = islandNumRigidBodies;\n while (j--) {\n //or(var j=0, l=islandNumRigidBodies; j 0.5) {\n // sleep the island\n j = islandNumRigidBodies;\n while (j--) {\n //for(j=0, l=islandNumRigidBodies;j 0 ) ? 1 : + x;\r\n\r\n\t};\r\n\r\n}\r\n\r\nif ( Function.prototype.name === undefined ) {\r\n\r\n\t// Missing in IE9-11.\r\n\t// https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Function/name\r\n\r\n\tObject.defineProperty( Function.prototype, 'name', {\r\n\r\n\t\tget: function () {\r\n\r\n\t\t\treturn this.toString().match( /^\\s*function\\s*([^\\(\\s]*)/ )[ 1 ];\r\n\r\n\t\t}\r\n\r\n\t} );\r\n\r\n}\r\n\r\nif ( Object.assign === undefined ) {\r\n\r\n\t// Missing in IE.\r\n\t// https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Object/assign\r\n\r\n\t( function () {\r\n\r\n\t\tObject.assign = function ( target ) {\r\n\r\n\t\t\t'use strict';\r\n\r\n\t\t\tif ( target === undefined || target === null ) {\r\n\r\n\t\t\t\tthrow new TypeError( 'Cannot convert undefined or null to object' );\r\n\r\n\t\t\t}\r\n\r\n\t\t\tvar output = Object( target );\r\n\r\n\t\t\tfor ( var index = 1; index < arguments.length; index ++ ) {\r\n\r\n\t\t\t\tvar source = arguments[ index ];\r\n\r\n\t\t\t\tif ( source !== undefined && source !== null ) {\r\n\r\n\t\t\t\t\tfor ( var nextKey in source ) {\r\n\r\n\t\t\t\t\t\tif ( Object.prototype.hasOwnProperty.call( source, nextKey ) ) {\r\n\r\n\t\t\t\t\t\t\toutput[ nextKey ] = source[ nextKey ];\r\n\r\n\t\t\t\t\t\t}\r\n\r\n\t\t\t\t\t}\r\n\r\n\t\t\t\t}\r\n\r\n\t\t\t}\r\n\r\n\t\t\treturn output;\r\n\r\n\t\t};\r\n\r\n\t} )();\r\n\r\n}\n\n/*\r\n * A list of constants built-in for\r\n * the physics engine.\r\n */\r\n\r\nvar REVISION = '1.0.9';\r\n\r\n// BroadPhase\r\nvar BR_NULL = 0;\r\nvar BR_BRUTE_FORCE = 1;\r\nvar BR_SWEEP_AND_PRUNE = 2;\r\nvar BR_BOUNDING_VOLUME_TREE = 3;\r\n\r\n// Body type\r\nvar BODY_NULL = 0;\r\nvar BODY_DYNAMIC = 1;\r\nvar BODY_STATIC = 2;\r\nvar BODY_KINEMATIC = 3;\r\nvar BODY_GHOST = 4;\r\n\r\n// Shape type\r\nvar SHAPE_NULL = 0;\r\nvar SHAPE_SPHERE = 1;\r\nvar SHAPE_BOX = 2;\r\nvar SHAPE_CYLINDER = 3;\r\nvar SHAPE_PLANE = 4;\r\nvar SHAPE_PARTICLE = 5;\r\nvar SHAPE_TETRA = 6;\r\n\r\n// Joint type\r\nvar JOINT_NULL = 0;\r\nvar JOINT_DISTANCE = 1;\r\nvar JOINT_BALL_AND_SOCKET = 2;\r\nvar JOINT_HINGE = 3;\r\nvar JOINT_WHEEL = 4;\r\nvar JOINT_SLIDER = 5;\r\nvar JOINT_PRISMATIC = 6;\r\n\r\n// AABB aproximation\r\nvar AABB_PROX = 0.005;\n\nvar _Math = {\r\n\r\n sqrt : Math.sqrt,\r\n abs : Math.abs,\r\n floor : Math.floor,\r\n cos : Math.cos,\r\n sin : Math.sin,\r\n acos : Math.acos,\r\n asin : Math.asin,\r\n atan2 : Math.atan2,\r\n round : Math.round,\r\n pow : Math.pow,\r\n max : Math.max,\r\n min : Math.min,\r\n random : Math.random,\r\n\r\n degtorad : 0.0174532925199432957,\r\n radtodeg : 57.295779513082320876,\r\n PI : 3.141592653589793,\r\n TwoPI : 6.283185307179586,\r\n PI90 : 1.570796326794896,\r\n PI270 : 4.712388980384689,\r\n\r\n INF : Infinity,\r\n EPZ : 0.00001,\r\n EPZ2 : 0.000001,\r\n\r\n lerp: function ( x, y, t ) { \r\n\r\n return ( 1 - t ) * x + t * y; \r\n\r\n },\r\n\r\n randInt: function ( low, high ) { \r\n\r\n return low + _Math.floor( _Math.random() * ( high - low + 1 ) ); \r\n\r\n },\r\n\r\n rand: function ( low, high ) { \r\n\r\n return low + _Math.random() * ( high - low ); \r\n\r\n },\r\n \r\n generateUUID: function () {\r\n\r\n // http://www.broofa.com/Tools/Math.uuid.htm\r\n\r\n var chars = '0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz'.split( '' );\r\n var uuid = new Array( 36 );\r\n var rnd = 0, r;\r\n\r\n return function generateUUID() {\r\n\r\n for ( var i = 0; i < 36; i ++ ) {\r\n\r\n if ( i === 8 || i === 13 || i === 18 || i === 23 ) {\r\n\r\n uuid[ i ] = '-';\r\n\r\n } else if ( i === 14 ) {\r\n\r\n uuid[ i ] = '4';\r\n\r\n } else {\r\n\r\n if ( rnd <= 0x02 ) rnd = 0x2000000 + ( Math.random() * 0x1000000 ) | 0;\r\n r = rnd & 0xf;\r\n rnd = rnd >> 4;\r\n uuid[ i ] = chars[ ( i === 19 ) ? ( r & 0x3 ) | 0x8 : r ];\r\n\r\n }\r\n\r\n }\r\n\r\n return uuid.join( '' );\r\n\r\n };\r\n\r\n }(),\r\n\r\n int: function( x ) { \r\n\r\n return _Math.floor(x); \r\n\r\n },\r\n\r\n fix: function( x, n ) { \r\n\r\n return x.toFixed(n || 3, 10); \r\n\r\n },\r\n\r\n clamp: function ( value, min, max ) { \r\n\r\n return _Math.max( min, _Math.min( max, value ) ); \r\n\r\n },\r\n \r\n //clamp: function ( x, a, b ) { return ( x < a ) ? a : ( ( x > b ) ? b : x ); },\r\n\r\n \r\n\r\n distance: function( p1, p2 ){\r\n\r\n var xd = p2[0]-p1[0];\r\n var yd = p2[1]-p1[1];\r\n var zd = p2[2]-p1[2];\r\n return _Math.sqrt(xd*xd + yd*yd + zd*zd);\r\n\r\n },\r\n\r\n /*unwrapDegrees: function ( r ) {\r\n\r\n r = r % 360;\r\n if (r > 180) r -= 360;\r\n if (r < -180) r += 360;\r\n return r;\r\n\r\n },\r\n\r\n unwrapRadian: function( r ){\r\n\r\n r = r % _Math.TwoPI;\r\n if (r > _Math.PI) r -= _Math.TwoPI;\r\n if (r < -_Math.PI) r += _Math.TwoPI;\r\n return r;\r\n\r\n },*/\r\n\r\n acosClamp: function ( cos ) {\r\n\r\n if(cos>1)return 0;\r\n else if(cos<-1)return _Math.PI;\r\n else return _Math.acos(cos);\r\n\r\n },\r\n\r\n distanceVector: function( v1, v2 ){\r\n\r\n var xd = v1.x - v2.x;\r\n var yd = v1.y - v2.y;\r\n var zd = v1.z - v2.z;\r\n return xd * xd + yd * yd + zd * zd;\r\n\r\n },\r\n\r\n dotVectors: function ( a, b ) {\r\n\r\n return a.x * b.x + a.y * b.y + a.z * b.z;\r\n\r\n },\r\n\r\n};\n\nfunction printError( clazz, msg ){\r\n console.error(\"[OIMO] \" + clazz + \": \" + msg);\r\n}\r\n\r\n// A performance evaluator\r\n\r\nfunction InfoDisplay(world){\r\n\r\n this.parent = world;\r\n\r\n this.infos = new Float32Array( 13 );\r\n this.f = [0,0,0];\r\n\r\n this.times = [0,0,0,0];\r\n\r\n this.broadPhase = this.parent.broadPhaseType;\r\n\r\n this.version = REVISION;\r\n\r\n this.fps = 0;\r\n\r\n this.tt = 0;\r\n\r\n this.broadPhaseTime = 0;\r\n this.narrowPhaseTime = 0;\r\n this.solvingTime = 0;\r\n this.totalTime = 0;\r\n this.updateTime = 0;\r\n\r\n this.MaxBroadPhaseTime = 0;\r\n this.MaxNarrowPhaseTime = 0;\r\n this.MaxSolvingTime = 0;\r\n this.MaxTotalTime = 0;\r\n this.MaxUpdateTime = 0;\r\n}\r\n\r\nObject.assign( InfoDisplay.prototype, {\r\n\r\n setTime: function(n){\r\n this.times[ n || 0 ] = performance.now();\r\n },\r\n\r\n resetMax: function(){\r\n\r\n this.MaxBroadPhaseTime = 0;\r\n this.MaxNarrowPhaseTime = 0;\r\n this.MaxSolvingTime = 0;\r\n this.MaxTotalTime = 0;\r\n this.MaxUpdateTime = 0;\r\n\r\n },\r\n\r\n calcBroadPhase: function () {\r\n\r\n this.setTime( 2 );\r\n this.broadPhaseTime = this.times[ 2 ] - this.times[ 1 ];\r\n\r\n },\r\n\r\n calcNarrowPhase: function () {\r\n\r\n this.setTime( 3 );\r\n this.narrowPhaseTime = this.times[ 3 ] - this.times[ 2 ];\r\n\r\n },\r\n\r\n calcEnd: function () {\r\n\r\n this.setTime( 2 );\r\n this.solvingTime = this.times[ 2 ] - this.times[ 1 ];\r\n this.totalTime = this.times[ 2 ] - this.times[ 0 ];\r\n this.updateTime = this.totalTime - ( this.broadPhaseTime + this.narrowPhaseTime + this.solvingTime );\r\n\r\n if( this.tt === 100 ) this.resetMax();\r\n\r\n if( this.tt > 100 ){\r\n if( this.broadPhaseTime > this.MaxBroadPhaseTime ) this.MaxBroadPhaseTime = this.broadPhaseTime;\r\n if( this.narrowPhaseTime > this.MaxNarrowPhaseTime ) this.MaxNarrowPhaseTime = this.narrowPhaseTime;\r\n if( this.solvingTime > this.MaxSolvingTime ) this.MaxSolvingTime = this.solvingTime;\r\n if( this.totalTime > this.MaxTotalTime ) this.MaxTotalTime = this.totalTime;\r\n if( this.updateTime > this.MaxUpdateTime ) this.MaxUpdateTime = this.updateTime;\r\n }\r\n\r\n\r\n this.upfps();\r\n\r\n this.tt ++;\r\n if(this.tt > 500) this.tt = 0;\r\n\r\n },\r\n\r\n\r\n upfps : function(){\r\n this.f[1] = Date.now();\r\n 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]++;\r\n },\r\n\r\n show: function(){\r\n var info =[\r\n \"Oimo.js \"+this.version+\"
\",\r\n this.broadPhase + \"
\",\r\n \"FPS: \" + this.fps +\" fps
\",\r\n \"rigidbody \"+this.parent.numRigidBodies+\"
\",\r\n \"contact \"+this.parent.numContacts+\"
\",\r\n \"ct-point \"+this.parent.numContactPoints+\"
\",\r\n \"paircheck \"+this.parent.broadPhase.numPairChecks+\"
\",\r\n \"island \"+this.parent.numIslands +\"
\",\r\n \"Time in milliseconds
\",\r\n \"broadphase \"+ _Math.fix(this.broadPhaseTime) + \" | \" + _Math.fix(this.MaxBroadPhaseTime) +\"
\",\r\n \"narrowphase \"+ _Math.fix(this.narrowPhaseTime) + \" | \" + _Math.fix(this.MaxNarrowPhaseTime) + \"
\",\r\n \"solving \"+ _Math.fix(this.solvingTime)+ \" | \" + _Math.fix(this.MaxSolvingTime) + \"
\",\r\n \"total \"+ _Math.fix(this.totalTime) + \" | \" + _Math.fix(this.MaxTotalTime) + \"
\",\r\n \"updating \"+ _Math.fix(this.updateTime) + \" | \" + _Math.fix(this.MaxUpdateTime) + \"
\"\r\n ].join(\"\\n\");\r\n return info;\r\n },\r\n\r\n toArray: function(){\r\n this.infos[0] = this.parent.broadPhase.types;\r\n this.infos[1] = this.parent.numRigidBodies;\r\n this.infos[2] = this.parent.numContacts;\r\n this.infos[3] = this.parent.broadPhase.numPairChecks;\r\n this.infos[4] = this.parent.numContactPoints;\r\n this.infos[5] = this.parent.numIslands;\r\n this.infos[6] = this.broadPhaseTime;\r\n this.infos[7] = this.narrowPhaseTime;\r\n this.infos[8] = this.solvingTime;\r\n this.infos[9] = this.updateTime;\r\n this.infos[10] = this.totalTime;\r\n this.infos[11] = this.fps;\r\n return this.infos;\r\n }\r\n \r\n});\n\nfunction Vec3 ( x, y, z ) {\n\n this.x = x || 0;\n this.y = y || 0;\n this.z = z || 0;\n \n}\n\nObject.assign( Vec3.prototype, {\n\n Vec3: true,\n\n set: function( x, y, z ){\n\n this.x = x;\n this.y = y;\n this.z = z;\n return this;\n\n },\n\n add: function ( a, b ) {\n\n if ( b !== undefined ) return this.addVectors( a, b );\n\n this.x += a.x;\n this.y += a.y;\n this.z += a.z;\n return this;\n\n },\n\n addVectors: function ( a, b ) {\n\n this.x = a.x + b.x;\n this.y = a.y + b.y;\n this.z = a.z + b.z;\n return this;\n\n },\n\n addEqual: function ( v ) {\n\n this.x += v.x;\n this.y += v.y;\n this.z += v.z;\n return this;\n\n },\n\n sub: function ( a, b ) {\n\n if ( b !== undefined ) return this.subVectors( a, b );\n\n this.x -= a.x;\n this.y -= a.y;\n this.z -= a.z;\n return this;\n\n },\n\n subVectors: function ( a, b ) {\n\n this.x = a.x - b.x;\n this.y = a.y - b.y;\n this.z = a.z - b.z;\n return this;\n\n },\n\n subEqual: function ( v ) {\n\n this.x -= v.x;\n this.y -= v.y;\n this.z -= v.z;\n return this;\n\n },\n\n scale: function ( v, s ) {\n\n this.x = v.x * s;\n this.y = v.y * s;\n this.z = v.z * s;\n return this;\n\n },\n\n scaleEqual: function( s ){\n\n this.x *= s;\n this.y *= s;\n this.z *= s;\n return this;\n\n },\n\n multiply: function( v ){\n\n this.x *= v.x;\n this.y *= v.y;\n this.z *= v.z;\n return this;\n\n },\n\n multiplyScalar: function( s ){\n\n this.x *= s;\n this.y *= s;\n this.z *= s;\n return this;\n\n },\n\n /*scaleV: function( v ){\n\n this.x *= v.x;\n this.y *= v.y;\n this.z *= v.z;\n return this;\n\n },\n\n scaleVectorEqual: function( v ){\n\n this.x *= v.x;\n this.y *= v.y;\n this.z *= v.z;\n return this;\n\n },*/\n\n addScaledVector: function ( v, s ) {\n\n this.x += v.x * s;\n this.y += v.y * s;\n this.z += v.z * s;\n\n return this;\n\n },\n\n subScaledVector: function ( v, s ) {\n\n this.x -= v.x * s;\n this.y -= v.y * s;\n this.z -= v.z * s;\n\n return this;\n\n },\n\n /*addTime: function ( v, t ) {\n\n this.x += v.x * t;\n this.y += v.y * t;\n this.z += v.z * t;\n return this;\n\n },\n \n addScale: function ( v, s ) {\n\n this.x += v.x * s;\n this.y += v.y * s;\n this.z += v.z * s;\n return this;\n\n },\n\n subScale: function ( v, s ) {\n\n this.x -= v.x * s;\n this.y -= v.y * s;\n this.z -= v.z * s;\n return this;\n\n },*/\n \n cross: function( a, b ) {\n\n if ( b !== undefined ) return this.crossVectors( a, b );\n\n var x = this.x, y = this.y, z = this.z;\n\n this.x = y * a.z - z * a.y;\n this.y = z * a.x - x * a.z;\n this.z = x * a.y - y * a.x;\n\n return this;\n\n },\n\n crossVectors: function ( a, b ) {\n\n var ax = a.x, ay = a.y, az = a.z;\n var bx = b.x, by = b.y, bz = b.z;\n\n this.x = ay * bz - az * by;\n this.y = az * bx - ax * bz;\n this.z = ax * by - ay * bx;\n\n return this;\n\n },\n\n tangent: function ( a ) {\n\n var ax = a.x, ay = a.y, az = a.z;\n\n this.x = ay * ax - az * az;\n this.y = - az * ay - ax * ax;\n this.z = ax * az + ay * ay;\n\n return this;\n\n },\n\n \n\n \n\n invert: function ( v ) {\n\n this.x=-v.x;\n this.y=-v.y;\n this.z=-v.z;\n return this;\n\n },\n\n negate: function () {\n\n this.x = - this.x;\n this.y = - this.y;\n this.z = - this.z;\n\n return this;\n\n },\n\n dot: function ( v ) {\n\n return this.x * v.x + this.y * v.y + this.z * v.z;\n\n },\n\n addition: function () {\n\n return this.x + this.y + this.z;\n\n },\n\n lengthSq: function () {\n\n return this.x * this.x + this.y * this.y + this.z * this.z;\n\n },\n\n length: function () {\n\n return _Math.sqrt( this.x * this.x + this.y * this.y + this.z * this.z );\n\n },\n\n copy: function( v ){\n\n this.x = v.x;\n this.y = v.y;\n this.z = v.z;\n return this;\n\n },\n\n /*mul: function( b, a, m ){\n\n return this.mulMat( m, a ).add( b );\n\n },\n\n mulMat: function( m, a ){\n\n var e = m.elements;\n var x = a.x, y = a.y, z = a.z;\n\n this.x = e[ 0 ] * x + e[ 1 ] * y + e[ 2 ] * z;\n this.y = e[ 3 ] * x + e[ 4 ] * y + e[ 5 ] * z;\n this.z = e[ 6 ] * x + e[ 7 ] * y + e[ 8 ] * z;\n return this;\n\n },*/\n\n applyMatrix3: function ( m, transpose ) {\n\n //if( transpose ) m = m.clone().transpose();\n var x = this.x, y = this.y, z = this.z;\n var e = m.elements;\n\n if( transpose ){\n \n this.x = e[ 0 ] * x + e[ 1 ] * y + e[ 2 ] * z;\n this.y = e[ 3 ] * x + e[ 4 ] * y + e[ 5 ] * z;\n this.z = e[ 6 ] * x + e[ 7 ] * y + e[ 8 ] * z;\n\n } else {\n \n this.x = e[ 0 ] * x + e[ 3 ] * y + e[ 6 ] * z;\n this.y = e[ 1 ] * x + e[ 4 ] * y + e[ 7 ] * z;\n this.z = e[ 2 ] * x + e[ 5 ] * y + e[ 8 ] * z;\n }\n\n return this;\n\n },\n\n applyQuaternion: function ( q ) {\n\n var x = this.x;\n var y = this.y;\n var z = this.z;\n\n var qx = q.x;\n var qy = q.y;\n var qz = q.z;\n var qw = q.w;\n\n // calculate quat * vector\n\n var ix = qw * x + qy * z - qz * y;\n var iy = qw * y + qz * x - qx * z;\n var iz = qw * z + qx * y - qy * x;\n var iw = - qx * x - qy * y - qz * z;\n\n // calculate result * inverse quat\n\n this.x = ix * qw + iw * - qx + iy * - qz - iz * - qy;\n this.y = iy * qw + iw * - qy + iz * - qx - ix * - qz;\n this.z = iz * qw + iw * - qz + ix * - qy - iy * - qx;\n\n return this;\n\n },\n\n testZero: function () {\n\n if(this.x!==0 || this.y!==0 || this.z!==0) return true;\n else return false;\n\n },\n\n testDiff: function( v ){\n\n return this.equals( v ) ? false : true;\n\n },\n\n equals: function ( v ) {\n\n return v.x === this.x && v.y === this.y && v.z === this.z;\n\n },\n\n clone: function () {\n\n return new this.constructor( this.x, this.y, this.z );\n\n },\n\n toString: function(){\n\n return\"Vec3[\"+this.x.toFixed(4)+\", \"+this.y.toFixed(4)+\", \"+this.z.toFixed(4)+\"]\";\n \n },\n\n multiplyScalar: function ( scalar ) {\n\n if ( isFinite( scalar ) ) {\n this.x *= scalar;\n this.y *= scalar;\n this.z *= scalar;\n } else {\n this.x = 0;\n this.y = 0;\n this.z = 0;\n }\n\n return this;\n\n },\n\n divideScalar: function ( scalar ) {\n\n return this.multiplyScalar( 1 / scalar );\n\n },\n\n normalize: function () {\n\n return this.divideScalar( this.length() );\n\n },\n\n toArray: function ( array, offset ) {\n\n if ( offset === undefined ) offset = 0;\n\n array[ offset ] = this.x;\n array[ offset + 1 ] = this.y;\n array[ offset + 2 ] = this.z;\n\n },\n\n fromArray: function( array, offset ){\n\n if ( offset === undefined ) offset = 0;\n \n this.x = array[ offset ];\n this.y = array[ offset + 1 ];\n this.z = array[ offset + 2 ];\n return this;\n\n },\n\n\n} );\n\nfunction Quat ( x, y, z, w ){\n\n this.x = x || 0;\n this.y = y || 0;\n this.z = z || 0;\n this.w = ( w !== undefined ) ? w : 1;\n\n}\n\nObject.assign( Quat.prototype, {\n\n Quat: true,\n\n set: function ( x, y, z, w ) {\n\n \n this.x = x;\n this.y = y;\n this.z = z;\n this.w = w;\n\n return this;\n\n },\n\n addTime: function( v, t ){\n\n var ax = v.x, ay = v.y, az = v.z;\n var qw = this.w, qx = this.x, qy = this.y, qz = this.z;\n t *= 0.5; \n this.x += t * ( ax*qw + ay*qz - az*qy );\n this.y += t * ( ay*qw + az*qx - ax*qz );\n this.z += t * ( az*qw + ax*qy - ay*qx );\n this.w += t * ( -ax*qx - ay*qy - az*qz );\n this.normalize();\n return this;\n\n },\n\n /*mul: function( q1, q2 ){\n\n var ax = q1.x, ay = q1.y, az = q1.z, as = q1.w,\n bx = q2.x, by = q2.y, bz = q2.z, bs = q2.w;\n this.x = ax * bs + as * bx + ay * bz - az * by;\n this.y = ay * bs + as * by + az * bx - ax * bz;\n this.z = az * bs + as * bz + ax * by - ay * bx;\n this.w = as * bs - ax * bx - ay * by - az * bz;\n return this;\n\n },*/\n\n multiply: function ( q, p ) {\n\n if ( p !== undefined ) return this.multiplyQuaternions( q, p );\n return this.multiplyQuaternions( this, q );\n\n },\n\n multiplyQuaternions: function ( a, b ) {\n\n var qax = a.x, qay = a.y, qaz = a.z, qaw = a.w;\n var qbx = b.x, qby = b.y, qbz = b.z, qbw = b.w;\n\n this.x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;\n this.y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;\n this.z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;\n this.w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;\n return this;\n\n },\n\n setFromUnitVectors: function( v1, v2 ) {\n\n var vx = new Vec3();\n var r = v1.dot( v2 ) + 1;\n\n if ( r < _Math.EPS2 ) {\n\n r = 0;\n if ( _Math.abs( v1.x ) > _Math.abs( v1.z ) ) vx.set( - v1.y, v1.x, 0 );\n else vx.set( 0, - v1.z, v1.y );\n\n } else {\n\n vx.crossVectors( v1, v2 );\n\n }\n\n this._x = vx.x;\n this._y = vx.y;\n this._z = vx.z;\n this._w = r;\n\n return this.normalize();\n\n },\n\n arc: function( v1, v2 ){\n\n var x1 = v1.x;\n var y1 = v1.y;\n var z1 = v1.z;\n var x2 = v2.x;\n var y2 = v2.y;\n var z2 = v2.z;\n var d = x1*x2 + y1*y2 + z1*z2;\n if( d==-1 ){\n x2 = y1*x1 - z1*z1;\n y2 = -z1*y1 - x1*x1;\n z2 = x1*z1 + y1*y1;\n d = 1 / _Math.sqrt( x2*x2 + y2*y2 + z2*z2 );\n this.w = 0;\n this.x = x2*d;\n this.y = y2*d;\n this.z = z2*d;\n return this;\n }\n var cx = y1*z2 - z1*y2;\n var cy = z1*x2 - x1*z2;\n var cz = x1*y2 - y1*x2;\n this.w = _Math.sqrt( ( 1 + d) * 0.5 );\n d = 0.5 / this.w;\n this.x = cx * d;\n this.y = cy * d;\n this.z = cz * d;\n return this;\n\n },\n\n normalize: function(){\n\n var l = this.length();\n if ( l === 0 ) {\n this.set( 0, 0, 0, 1 );\n } else {\n l = 1 / l;\n this.x = this.x * l;\n this.y = this.y * l;\n this.z = this.z * l;\n this.w = this.w * l;\n }\n return this;\n\n },\n\n inverse: function () {\n\n return this.conjugate().normalize();\n\n },\n\n invert: function ( q ) {\n\n this.x = q.x;\n this.y = q.y;\n this.z = q.z;\n this.w = q.w;\n this.conjugate().normalize();\n return this;\n\n },\n\n conjugate: function () {\n\n this.x *= - 1;\n this.y *= - 1;\n this.z *= - 1;\n return this;\n\n },\n\n length: function(){\n\n return _Math.sqrt( this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w );\n\n },\n\n lengthSq: function () {\n\n return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;\n\n },\n \n copy: function( q ){\n \n this.x = q.x;\n this.y = q.y;\n this.z = q.z;\n this.w = q.w;\n return this;\n\n },\n\n clone: function( q ){\n\n return new Quat( this.x, this.y, this.z, this.w );\n\n },\n\n testDiff: function ( q ) {\n\n return this.equals( q ) ? false : true;\n\n },\n\n equals: function ( q ) {\n\n return this.x === q.x && this.y === q.y && this.z === q.z && this.w === q.w;\n\n },\n\n toString: function(){\n\n return\"Quat[\"+this.x.toFixed(4)+\", (\"+this.y.toFixed(4)+\", \"+this.z.toFixed(4)+\", \"+this.w.toFixed(4)+\")]\";\n \n },\n\n setFromEuler: function ( x, y, z ){\n\n var c1 = Math.cos( x * 0.5 );\n var c2 = Math.cos( y * 0.5 );\n var c3 = Math.cos( z * 0.5 );\n var s1 = Math.sin( x * 0.5 );\n var s2 = Math.sin( y * 0.5 );\n var s3 = Math.sin( z * 0.5 );\n\n // XYZ\n this.x = s1 * c2 * c3 + c1 * s2 * s3;\n this.y = c1 * s2 * c3 - s1 * c2 * s3;\n this.z = c1 * c2 * s3 + s1 * s2 * c3;\n this.w = c1 * c2 * c3 - s1 * s2 * s3;\n\n return this;\n\n },\n \n setFromAxis: function ( axis, rad ) {\n\n axis.normalize();\n rad = rad * 0.5;\n var s = _Math.sin( rad );\n this.x = s * axis.x;\n this.y = s * axis.y;\n this.z = s * axis.z;\n this.w = _Math.cos( rad );\n return this;\n\n },\n\n setFromMat33: function ( m ) {\n\n var trace = m[0] + m[4] + m[8];\n var s;\n\n if ( trace > 0 ) {\n\n s = _Math.sqrt( trace + 1.0 );\n this.w = 0.5 / s;\n s = 0.5 / s;\n this.x = ( m[5] - m[7] ) * s;\n this.y = ( m[6] - m[2] ) * s;\n this.z = ( m[1] - m[3] ) * s;\n\n } else {\n\n var out = [];\n var i = 0;\n if ( m[4] > m[0] ) i = 1;\n if ( m[8] > m[i*3+i] ) i = 2;\n\n var j = (i+1)%3;\n var k = (i+2)%3;\n \n s = _Math.sqrt( m[i*3+i] - m[j*3+j] - m[k*3+k] + 1.0 );\n out[i] = 0.5 * fRoot;\n s = 0.5 / fRoot;\n this.w = ( m[j*3+k] - m[k*3+j] ) * s;\n out[j] = ( m[j*3+i] + m[i*3+j] ) * s;\n out[k] = ( m[k*3+i] + m[i*3+k] ) * s;\n\n this.x = out[1];\n this.y = out[2];\n this.z = out[3];\n\n }\n\n return this;\n\n },\n\n toArray: function ( array, offset ) {\n\n offset = offset || 0;\n\n array[ offset ] = this.x;\n array[ offset + 1 ] = this.y;\n array[ offset + 2 ] = this.z;\n array[ offset + 3 ] = this.w;\n\n },\n\n fromArray: function( array, offset ){\n\n offset = offset || 0;\n this.set( array[ offset ], array[ offset + 1 ], array[ offset + 2 ], array[ offset + 3 ] );\n return this;\n\n }\n\n});\n\nfunction Mat33 ( e00, e01, e02, e10, e11, e12, e20, e21, e22 ){\r\n\r\n this.elements = [\r\n 1, 0, 0,\r\n 0, 1, 0,\r\n 0, 0, 1\r\n ];\r\n\r\n if ( arguments.length > 0 ) {\r\n\r\n console.error( 'OIMO.Mat33: the constructor no longer reads arguments. use .set() instead.' );\r\n\r\n }\r\n\r\n}\r\n\r\nObject.assign( Mat33.prototype, {\r\n\r\n Mat33: true,\r\n\r\n set: function ( e00, e01, e02, e10, e11, e12, e20, e21, e22 ){\r\n\r\n var te = this.elements;\r\n te[0] = e00; te[1] = e01; te[2] = e02;\r\n te[3] = e10; te[4] = e11; te[5] = e12;\r\n te[6] = e20; te[7] = e21; te[8] = e22;\r\n return this;\r\n\r\n },\r\n \r\n add: function ( a, b ) {\r\n\r\n if( b !== undefined ) return this.addMatrixs( a, b );\r\n\r\n var e = this.elements, te = a.elements;\r\n e[0] += te[0]; e[1] += te[1]; e[2] += te[2];\r\n e[3] += te[3]; e[4] += te[4]; e[5] += te[5];\r\n e[6] += te[6]; e[7] += te[7]; e[8] += te[8];\r\n return this;\r\n\r\n },\r\n\r\n addMatrixs: function ( a, b ) {\r\n\r\n var te = this.elements, tem1 = a.elements, tem2 = b.elements;\r\n te[0] = tem1[0] + tem2[0]; te[1] = tem1[1] + tem2[1]; te[2] = tem1[2] + tem2[2];\r\n te[3] = tem1[3] + tem2[3]; te[4] = tem1[4] + tem2[4]; te[5] = tem1[5] + tem2[5];\r\n te[6] = tem1[6] + tem2[6]; te[7] = tem1[7] + tem2[7]; te[8] = tem1[8] + tem2[8];\r\n return this;\r\n\r\n },\r\n\r\n addEqual: function( m ){\r\n\r\n var te = this.elements, tem = m.elements;\r\n te[0] += tem[0]; te[1] += tem[1]; te[2] += tem[2];\r\n te[3] += tem[3]; te[4] += tem[4]; te[5] += tem[5];\r\n te[6] += tem[6]; te[7] += tem[7]; te[8] += tem[8];\r\n return this;\r\n\r\n },\r\n\r\n sub: function ( a, b ) {\r\n\r\n if( b !== undefined ) return this.subMatrixs( a, b );\r\n\r\n var e = this.elements, te = a.elements;\r\n e[0] -= te[0]; e[1] -= te[1]; e[2] -= te[2];\r\n e[3] -= te[3]; e[4] -= te[4]; e[5] -= te[5];\r\n e[6] -= te[6]; e[7] -= te[7]; e[8] -= te[8];\r\n return this;\r\n\r\n },\r\n\r\n subMatrixs: function ( a, b ) {\r\n\r\n var te = this.elements, tem1 = a.elements, tem2 = b.elements;\r\n te[0] = tem1[0] - tem2[0]; te[1] = tem1[1] - tem2[1]; te[2] = tem1[2] - tem2[2];\r\n te[3] = tem1[3] - tem2[3]; te[4] = tem1[4] - tem2[4]; te[5] = tem1[5] - tem2[5];\r\n te[6] = tem1[6] - tem2[6]; te[7] = tem1[7] - tem2[7]; te[8] = tem1[8] - tem2[8];\r\n return this;\r\n\r\n },\r\n\r\n subEqual: function ( m ) {\r\n\r\n var te = this.elements, tem = m.elements;\r\n te[0] -= tem[0]; te[1] -= tem[1]; te[2] -= tem[2];\r\n te[3] -= tem[3]; te[4] -= tem[4]; te[5] -= tem[5];\r\n te[6] -= tem[6]; te[7] -= tem[7]; te[8] -= tem[8];\r\n return this;\r\n\r\n },\r\n\r\n scale: function ( m, s ) {\r\n\r\n var te = this.elements, tm = m.elements;\r\n te[0] = tm[0] * s; te[1] = tm[1] * s; te[2] = tm[2] * s;\r\n te[3] = tm[3] * s; te[4] = tm[4] * s; te[5] = tm[5] * s;\r\n te[6] = tm[6] * s; te[7] = tm[7] * s; te[8] = tm[8] * s;\r\n return this;\r\n\r\n },\r\n\r\n scaleEqual: function ( s ){// multiplyScalar\r\n\r\n var te = this.elements;\r\n te[0] *= s; te[1] *= s; te[2] *= s;\r\n te[3] *= s; te[4] *= s; te[5] *= s;\r\n te[6] *= s; te[7] *= s; te[8] *= s;\r\n return this;\r\n\r\n },\r\n\r\n multiplyMatrices: function ( m1, m2, transpose ) {\r\n\r\n if( transpose ) m2 = m2.clone().transpose();\r\n\r\n var te = this.elements;\r\n var tm1 = m1.elements;\r\n var tm2 = m2.elements;\r\n\r\n var a0 = tm1[0], a3 = tm1[3], a6 = tm1[6];\r\n var a1 = tm1[1], a4 = tm1[4], a7 = tm1[7];\r\n var a2 = tm1[2], a5 = tm1[5], a8 = tm1[8];\r\n\r\n var b0 = tm2[0], b3 = tm2[3], b6 = tm2[6];\r\n var b1 = tm2[1], b4 = tm2[4], b7 = tm2[7];\r\n var b2 = tm2[2], b5 = tm2[5], b8 = tm2[8];\r\n\r\n te[0] = a0*b0 + a1*b3 + a2*b6;\r\n te[1] = a0*b1 + a1*b4 + a2*b7;\r\n te[2] = a0*b2 + a1*b5 + a2*b8;\r\n te[3] = a3*b0 + a4*b3 + a5*b6;\r\n te[4] = a3*b1 + a4*b4 + a5*b7;\r\n te[5] = a3*b2 + a4*b5 + a5*b8;\r\n te[6] = a6*b0 + a7*b3 + a8*b6;\r\n te[7] = a6*b1 + a7*b4 + a8*b7;\r\n te[8] = a6*b2 + a7*b5 + a8*b8;\r\n\r\n return this;\r\n\r\n },\r\n\r\n /*mul: function ( m1, m2, transpose ) {\r\n\r\n if( transpose ) m2 = m2.clone().transpose();\r\n\r\n var te = this.elements;\r\n var tm1 = m1.elements;\r\n var tm2 = m2.elements;\r\n //var tmp;\r\n\r\n var a0 = tm1[0], a3 = tm1[3], a6 = tm1[6];\r\n var a1 = tm1[1], a4 = tm1[4], a7 = tm1[7];\r\n var a2 = tm1[2], a5 = tm1[5], a8 = tm1[8];\r\n\r\n var b0 = tm2[0], b3 = tm2[3], b6 = tm2[6];\r\n var b1 = tm2[1], b4 = tm2[4], b7 = tm2[7];\r\n var b2 = tm2[2], b5 = tm2[5], b8 = tm2[8];\r\n\r\n /*if( transpose ){\r\n\r\n tmp = b1; b1 = b3; b3 = tmp;\r\n tmp = b2; b2 = b6; b6 = tmp;\r\n tmp = b5; b5 = b7; b7 = tmp;\r\n\r\n }\r\n\r\n te[0] = a0*b0 + a1*b3 + a2*b6;\r\n te[1] = a0*b1 + a1*b4 + a2*b7;\r\n te[2] = a0*b2 + a1*b5 + a2*b8;\r\n te[3] = a3*b0 + a4*b3 + a5*b6;\r\n te[4] = a3*b1 + a4*b4 + a5*b7;\r\n te[5] = a3*b2 + a4*b5 + a5*b8;\r\n te[6] = a6*b0 + a7*b3 + a8*b6;\r\n te[7] = a6*b1 + a7*b4 + a8*b7;\r\n te[8] = a6*b2 + a7*b5 + a8*b8;\r\n\r\n return this;\r\n\r\n },*/\r\n\r\n transpose: function ( m ) {\r\n \r\n if( m !== undefined ){\r\n var a = m.elements;\r\n this.set( a[0], a[3], a[6], a[1], a[4], a[7], a[2], a[5], a[8] );\r\n return this;\r\n }\r\n\r\n var te = this.elements;\r\n var a01 = te[1], a02 = te[2], a12 = te[5];\r\n te[1] = te[3];\r\n te[2] = te[6];\r\n te[3] = a01;\r\n te[5] = te[7];\r\n te[6] = a02;\r\n te[7] = a12;\r\n return this;\r\n\r\n },\r\n\r\n\r\n\r\n /*mulScale: function ( m, sx, sy, sz, Prepend ) {\r\n\r\n var prepend = Prepend || false;\r\n var te = this.elements, tm = m.elements;\r\n if(prepend){\r\n te[0] = sx*tm[0]; te[1] = sx*tm[1]; te[2] = sx*tm[2];\r\n te[3] = sy*tm[3]; te[4] = sy*tm[4]; te[5] = sy*tm[5];\r\n te[6] = sz*tm[6]; te[7] = sz*tm[7]; te[8] = sz*tm[8];\r\n }else{\r\n te[0] = tm[0]*sx; te[1] = tm[1]*sy; te[2] = tm[2]*sz;\r\n te[3] = tm[3]*sx; te[4] = tm[4]*sy; te[5] = tm[5]*sz;\r\n te[6] = tm[6]*sx; te[7] = tm[7]*sy; te[8] = tm[8]*sz;\r\n }\r\n return this;\r\n\r\n },\r\n\r\n transpose: function ( m ) {\r\n\r\n var te = this.elements, tm = m.elements;\r\n te[0] = tm[0]; te[1] = tm[3]; te[2] = tm[6];\r\n te[3] = tm[1]; te[4] = tm[4]; te[5] = tm[7];\r\n te[6] = tm[2]; te[7] = tm[5]; te[8] = tm[8];\r\n return this;\r\n\r\n },*/\r\n\r\n setQuat: function ( q ) {\r\n\r\n var te = this.elements;\r\n var x = q.x, y = q.y, z = q.z, w = q.w;\r\n var x2 = x + x, y2 = y + y, z2 = z + z;\r\n var xx = x * x2, xy = x * y2, xz = x * z2;\r\n var yy = y * y2, yz = y * z2, zz = z * z2;\r\n var wx = w * x2, wy = w * y2, wz = w * z2;\r\n \r\n te[0] = 1 - ( yy + zz );\r\n te[1] = xy - wz;\r\n te[2] = xz + wy;\r\n\r\n te[3] = xy + wz;\r\n te[4] = 1 - ( xx + zz );\r\n te[5] = yz - wx;\r\n\r\n te[6] = xz - wy;\r\n te[7] = yz + wx;\r\n te[8] = 1 - ( xx + yy );\r\n\r\n return this;\r\n\r\n },\r\n\r\n invert: function( m ) {\r\n\r\n var te = this.elements, tm = m.elements,\r\n a00 = tm[0], a10 = tm[3], a20 = tm[6],\r\n a01 = tm[1], a11 = tm[4], a21 = tm[7],\r\n a02 = tm[2], a12 = tm[5], a22 = tm[8],\r\n b01 = a22 * a11 - a12 * a21,\r\n b11 = -a22 * a10 + a12 * a20,\r\n b21 = a21 * a10 - a11 * a20,\r\n det = a00 * b01 + a01 * b11 + a02 * b21;\r\n\r\n if ( det === 0 ) {\r\n console.log( \"can't invert matrix, determinant is 0\");\r\n return this.identity();\r\n }\r\n\r\n det = 1.0 / det;\r\n te[0] = b01 * det;\r\n te[1] = (-a22 * a01 + a02 * a21) * det;\r\n te[2] = (a12 * a01 - a02 * a11) * det;\r\n te[3] = b11 * det;\r\n te[4] = (a22 * a00 - a02 * a20) * det;\r\n te[5] = (-a12 * a00 + a02 * a10) * det;\r\n te[6] = b21 * det;\r\n te[7] = (-a21 * a00 + a01 * a20) * det;\r\n te[8] = (a11 * a00 - a01 * a10) * det;\r\n return this;\r\n\r\n },\r\n\r\n addOffset: function ( m, v ) {\r\n\r\n var relX = v.x;\r\n var relY = v.y;\r\n var relZ = v.z;\r\n\r\n var te = this.elements;\r\n te[0] += m * ( relY * relY + relZ * relZ );\r\n te[4] += m * ( relX * relX + relZ * relZ );\r\n te[8] += m * ( relX * relX + relY * relY );\r\n var xy = m * relX * relY;\r\n var yz = m * relY * relZ;\r\n var zx = m * relZ * relX;\r\n te[1] -= xy;\r\n te[3] -= xy;\r\n te[2] -= yz;\r\n te[6] -= yz;\r\n te[5] -= zx;\r\n te[7] -= zx;\r\n return this;\r\n\r\n },\r\n\r\n subOffset: function ( m, v ) {\r\n\r\n var relX = v.x;\r\n var relY = v.y;\r\n var relZ = v.z;\r\n\r\n var te = this.elements;\r\n te[0] -= m * ( relY * relY + relZ * relZ );\r\n te[4] -= m * ( relX * relX + relZ * relZ );\r\n te[8] -= m * ( relX * relX + relY * relY );\r\n var xy = m * relX * relY;\r\n var yz = m * relY * relZ;\r\n var zx = m * relZ * relX;\r\n te[1] += xy;\r\n te[3] += xy;\r\n te[2] += yz;\r\n te[6] += yz;\r\n te[5] += zx;\r\n te[7] += zx;\r\n return this;\r\n\r\n },\r\n\r\n // OK \r\n\r\n multiplyScalar: function ( s ) {\r\n\r\n var te = this.elements;\r\n\r\n te[ 0 ] *= s; te[ 3 ] *= s; te[ 6 ] *= s;\r\n te[ 1 ] *= s; te[ 4 ] *= s; te[ 7 ] *= s;\r\n te[ 2 ] *= s; te[ 5 ] *= s; te[ 8 ] *= s;\r\n\r\n return this;\r\n\r\n },\r\n\r\n identity: function () {\r\n\r\n this.set( 1, 0, 0, 0, 1, 0, 0, 0, 1 );\r\n return this;\r\n\r\n },\r\n\r\n\r\n clone: function () {\r\n\r\n return new Mat33().fromArray( this.elements );\r\n\r\n },\r\n\r\n copy: function ( m ) {\r\n\r\n for ( var i = 0; i < 9; i ++ ) this.elements[ i ] = m.elements[ i ];\r\n return this;\r\n\r\n },\r\n\r\n determinant: function () {\r\n\r\n var te = this.elements;\r\n var a = te[ 0 ], b = te[ 1 ], c = te[ 2 ],\r\n d = te[ 3 ], e = te[ 4 ], f = te[ 5 ],\r\n g = te[ 6 ], h = te[ 7 ], i = te[ 8 ];\r\n\r\n return a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g;\r\n\r\n },\r\n\r\n fromArray: function ( array, offset ) {\r\n\r\n if ( offset === undefined ) offset = 0;\r\n\r\n for( var i = 0; i < 9; i ++ ) {\r\n\r\n this.elements[ i ] = array[ i + offset ];\r\n\r\n }\r\n\r\n return this;\r\n\r\n },\r\n\r\n toArray: function ( array, offset ) {\r\n\r\n if ( array === undefined ) array = [];\r\n if ( offset === undefined ) offset = 0;\r\n\r\n var te = this.elements;\r\n\r\n array[ offset ] = te[ 0 ];\r\n array[ offset + 1 ] = te[ 1 ];\r\n array[ offset + 2 ] = te[ 2 ];\r\n\r\n array[ offset + 3 ] = te[ 3 ];\r\n array[ offset + 4 ] = te[ 4 ];\r\n array[ offset + 5 ] = te[ 5 ];\r\n\r\n array[ offset + 6 ] = te[ 6 ];\r\n array[ offset + 7 ] = te[ 7 ];\r\n array[ offset + 8 ] = te[ 8 ];\r\n\r\n return array;\r\n\r\n }\r\n\r\n\r\n} );\n\n/**\r\n * An axis-aligned bounding box.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n\r\nfunction AABB( minX, maxX, minY, maxY, minZ, maxZ ){\r\n\r\n this.elements = new Float32Array( 6 );\r\n var te = this.elements;\r\n\r\n te[0] = minX || 0; te[1] = minY || 0; te[2] = minZ || 0;\r\n te[3] = maxX || 0; te[4] = maxY || 0; te[5] = maxZ || 0;\r\n\r\n}\r\n\r\nObject.assign( AABB.prototype, {\r\n\r\n\tAABB: true,\r\n\r\n\tset: function(minX, maxX, minY, maxY, minZ, maxZ){\r\n\r\n\t\tvar te = this.elements;\r\n\t\tte[0] = minX;\r\n\t\tte[3] = maxX;\r\n\t\tte[1] = minY;\r\n\t\tte[4] = maxY;\r\n\t\tte[2] = minZ;\r\n\t\tte[5] = maxZ;\r\n\t\treturn this;\r\n\t},\r\n\r\n\tintersectTest: function ( aabb ) {\r\n\r\n\t\tvar te = this.elements;\r\n\t\tvar ue = aabb.elements;\r\n\t\treturn 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;\r\n\r\n\t},\r\n\r\n\tintersectTestTwo: function ( aabb ) {\r\n\r\n\t\tvar te = this.elements;\r\n\t\tvar ue = aabb.elements;\r\n\t\treturn 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;\r\n\r\n\t},\r\n\r\n\tclone: function () {\r\n\r\n\t\treturn new this.constructor().fromArray( this.elements );\r\n\r\n\t},\r\n\r\n\tcopy: function ( aabb, margin ) {\r\n\r\n\t\tvar m = margin || 0;\r\n\t\tvar me = aabb.elements;\r\n\t\tthis.set( me[ 0 ]-m, me[ 3 ]+m, me[ 1 ]-m, me[ 4 ]+m, me[ 2 ]-m, me[ 5 ]+m );\r\n\t\treturn this;\r\n\r\n\t},\r\n\r\n\tfromArray: function ( array ) {\r\n\r\n\t\tthis.elements.set( array );\r\n\t\treturn this;\r\n\r\n\t},\r\n\r\n\t// Set this AABB to the combined AABB of aabb1 and aabb2.\r\n\r\n\tcombine: function( aabb1, aabb2 ) {\r\n\r\n\t\tvar a = aabb1.elements;\r\n\t\tvar b = aabb2.elements;\r\n\t\tvar te = this.elements;\r\n\r\n\t\tte[0] = a[0] < b[0] ? a[0] : b[0];\r\n\t\tte[1] = a[1] < b[1] ? a[1] : b[1];\r\n\t\tte[2] = a[2] < b[2] ? a[2] : b[2];\r\n\r\n\t\tte[3] = a[3] > b[3] ? a[3] : b[3];\r\n\t\tte[4] = a[4] > b[4] ? a[4] : b[4];\r\n\t\tte[5] = a[5] > b[5] ? a[5] : b[5];\r\n\r\n\t\treturn this;\r\n\r\n\t},\r\n\r\n\r\n\t// Get the surface area.\r\n\r\n\tsurfaceArea: function () {\r\n\r\n\t\tvar te = this.elements;\r\n\t\tvar a = te[3] - te[0];\r\n\t\tvar h = te[4] - te[1];\r\n\t\tvar d = te[5] - te[2];\r\n\t\treturn 2 * (a * (h + d) + h * d );\r\n\r\n\t},\r\n\r\n\r\n\t// Get whether the AABB intersects with the point or not.\r\n\r\n\tintersectsWithPoint:function(x,y,z){\r\n\r\n\t\tvar te = this.elements;\r\n\t\treturn x>=te[0] && x<=te[3] && y>=te[1] && y<=te[4] && z>=te[2] && z<=te[5];\r\n\r\n\t},\r\n\r\n\t/**\r\n\t * Set the AABB from an array\r\n\t * of vertices. From THREE.\r\n\t * @author WestLangley\r\n\t * @author xprogram\r\n\t */\r\n\r\n\tsetFromPoints: function(arr){\r\n\t\tthis.makeEmpty();\r\n\t\tfor(var i = 0; i < arr.length; i++){\r\n\t\t\tthis.expandByPoint(arr[i]);\r\n\t\t}\r\n\t},\r\n\r\n\tmakeEmpty: function(){\r\n\t\tthis.set(-Infinity, -Infinity, -Infinity, Infinity, Infinity, Infinity);\r\n\t},\r\n\r\n\texpandByPoint: function(pt){\r\n\t\tvar te = this.elements;\r\n\t\tthis.set(\r\n\t\t\t_Math.min(te[ 0 ], pt.x), _Math.min(te[ 1 ], pt.y), _Math.min(te[ 2 ], pt.z),\r\n\t\t\t_Math.max(te[ 3 ], pt.x), _Math.max(te[ 4 ], pt.y), _Math.max(te[ 5 ], pt.z)\r\n\t\t);\r\n\t},\r\n\r\n\texpandByScalar: function(s){\r\n\r\n\t\tvar te = this.elements;\r\n\t\tte[0] += -s;\r\n\t\tte[1] += -s;\r\n\t\tte[2] += -s;\r\n\t\tte[3] += s;\r\n\t\tte[4] += s;\r\n\t\tte[5] += s;\r\n\t}\r\n\r\n});\n\nvar count = 0;\r\nfunction ShapeIdCount() { return count++; }\r\n\r\n/**\r\n * A shape is used to detect collisions of rigid bodies.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n\r\nfunction Shape ( config ) {\r\n\r\n this.type = SHAPE_NULL;\r\n\r\n // global identification of the shape should be unique to the shape.\r\n this.id = ShapeIdCount();\r\n\r\n // previous shape in parent rigid body. Used for fast interations.\r\n this.prev = null;\r\n\r\n // next shape in parent rigid body. Used for fast interations.\r\n this.next = null;\r\n\r\n // proxy of the shape used for broad-phase collision detection.\r\n this.proxy = null;\r\n\r\n // parent rigid body of the shape.\r\n this.parent = null;\r\n\r\n // linked list of the contacts with the shape.\r\n this.contactLink = null;\r\n\r\n // number of the contacts with the shape.\r\n this.numContacts = 0;\r\n\r\n // center of gravity of the shape in world coordinate system.\r\n this.position = new Vec3();\r\n\r\n // rotation matrix of the shape in world coordinate system.\r\n this.rotation = new Mat33();\r\n\r\n // position of the shape in parent's coordinate system.\r\n this.relativePosition = new Vec3().copy( config.relativePosition );\r\n\r\n // rotation matrix of the shape in parent's coordinate system.\r\n this.relativeRotation = new Mat33().copy( config.relativeRotation );\r\n\r\n // axis-aligned bounding box of the shape.\r\n this.aabb = new AABB();\r\n\r\n // density of the shape.\r\n this.density = config.density;\r\n\r\n // coefficient of friction of the shape.\r\n this.friction = config.friction;\r\n\r\n // coefficient of restitution of the shape.\r\n this.restitution = config.restitution;\r\n\r\n // bits of the collision groups to which the shape belongs.\r\n this.belongsTo = config.belongsTo;\r\n\r\n // bits of the collision groups with which the shape collides.\r\n this.collidesWith = config.collidesWith;\r\n\r\n}\r\n\r\nObject.assign( Shape.prototype, {\r\n\r\n Shape: true,\r\n\r\n // Calculate the mass information of the shape.\r\n\r\n calculateMassInfo: function( out ){\r\n\r\n printError(\"Shape\", \"Inheritance error.\");\r\n\r\n },\r\n\r\n // Update the proxy of the shape.\r\n\r\n updateProxy: function(){\r\n\r\n printError(\"Shape\", \"Inheritance error.\");\r\n\r\n }\r\n\r\n});\n\n/**\r\n * Box shape.\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n \r\nfunction Box ( config, Width, Height, Depth ) {\r\n\r\n Shape.call( this, config );\r\n\r\n this.type = SHAPE_BOX;\r\n\r\n this.width = Width;\r\n this.height = Height;\r\n this.depth = Depth;\r\n\r\n this.halfWidth = Width * 0.5;\r\n this.halfHeight = Height * 0.5;\r\n this.halfDepth = Depth * 0.5;\r\n\r\n this.dimentions = new Float32Array( 18 );\r\n this.elements = new Float32Array( 24 );\r\n\r\n}\r\n\r\nBox.prototype = Object.assign( Object.create( Shape.prototype ), {\r\n\r\n\tconstructor: Box,\r\n\r\n\tcalculateMassInfo: function ( out ) {\r\n\r\n\t\tvar mass = this.width * this.height * this.depth * this.density;\r\n\t\tvar divid = 1/12;\r\n\t\tout.mass = mass;\r\n\t\tout.inertia.set(\r\n\t\t\tmass * ( this.height * this.height + this.depth * this.depth ) * divid, 0, 0,\r\n\t\t\t0, mass * ( this.width * this.width + this.depth * this.depth ) * divid, 0,\r\n\t\t\t0, 0, mass * ( this.width * this.width + this.height * this.height ) * divid\r\n\t\t);\r\n\r\n\t},\r\n\r\n\tupdateProxy: function () {\r\n\r\n\t\tvar te = this.rotation.elements;\r\n\t\tvar di = this.dimentions;\r\n\t\t// Width\r\n\t\tdi[0] = te[0];\r\n\t\tdi[1] = te[3];\r\n\t\tdi[2] = te[6];\r\n\t\t// Height\r\n\t\tdi[3] = te[1];\r\n\t\tdi[4] = te[4];\r\n\t\tdi[5] = te[7];\r\n\t\t// Depth\r\n\t\tdi[6] = te[2];\r\n\t\tdi[7] = te[5];\r\n\t\tdi[8] = te[8];\r\n\t\t// half Width\r\n\t\tdi[9] = te[0] * this.halfWidth;\r\n\t\tdi[10] = te[3] * this.halfWidth;\r\n\t\tdi[11] = te[6] * this.halfWidth;\r\n\t\t// half Height\r\n\t\tdi[12] = te[1] * this.halfHeight;\r\n\t\tdi[13] = te[4] * this.halfHeight;\r\n\t\tdi[14] = te[7] * this.halfHeight;\r\n\t\t// half Depth\r\n\t\tdi[15] = te[2] * this.halfDepth;\r\n\t\tdi[16] = te[5] * this.halfDepth;\r\n\t\tdi[17] = te[8] * this.halfDepth;\r\n\r\n\t\tvar wx = di[9];\r\n\t\tvar wy = di[10];\r\n\t\tvar wz = di[11];\r\n\t\tvar hx = di[12];\r\n\t\tvar hy = di[13];\r\n\t\tvar hz = di[14];\r\n\t\tvar dx = di[15];\r\n\t\tvar dy = di[16];\r\n\t\tvar dz = di[17];\r\n\r\n\t\tvar x = this.position.x;\r\n\t\tvar y = this.position.y;\r\n\t\tvar z = this.position.z;\r\n\r\n\t\tvar v = this.elements;\r\n\t\t//v1\r\n\t\tv[0] = x + wx + hx + dx;\r\n\t\tv[1] = y + wy + hy + dy;\r\n\t\tv[2] = z + wz + hz + dz;\r\n\t\t//v2\r\n\t\tv[3] = x + wx + hx - dx;\r\n\t\tv[4] = y + wy + hy - dy;\r\n\t\tv[5] = z + wz + hz - dz;\r\n\t\t//v3\r\n\t\tv[6] = x + wx - hx + dx;\r\n\t\tv[7] = y + wy - hy + dy;\r\n\t\tv[8] = z + wz - hz + dz;\r\n\t\t//v4\r\n\t\tv[9] = x + wx - hx - dx;\r\n\t\tv[10] = y + wy - hy - dy;\r\n\t\tv[11] = z + wz - hz - dz;\r\n\t\t//v5\r\n\t\tv[12] = x - wx + hx + dx;\r\n\t\tv[13] = y - wy + hy + dy;\r\n\t\tv[14] = z - wz + hz + dz;\r\n\t\t//v6\r\n\t\tv[15] = x - wx + hx - dx;\r\n\t\tv[16] = y - wy + hy - dy;\r\n\t\tv[17] = z - wz + hz - dz;\r\n\t\t//v7\r\n\t\tv[18] = x - wx - hx + dx;\r\n\t\tv[19] = y - wy - hy + dy;\r\n\t\tv[20] = z - wz - hz + dz;\r\n\t\t//v8\r\n\t\tv[21] = x - wx - hx - dx;\r\n\t\tv[22] = y - wy - hy - dy;\r\n\t\tv[23] = z - wz - hz - dz;\r\n\r\n\t\tvar w = di[9] < 0 ? -di[9] : di[9];\r\n\t\tvar h = di[10] < 0 ? -di[10] : di[10];\r\n\t\tvar d = di[11] < 0 ? -di[11] : di[11];\r\n\r\n\t\tw = di[12] < 0 ? w - di[12] : w + di[12];\r\n\t\th = di[13] < 0 ? h - di[13] : h + di[13];\r\n\t\td = di[14] < 0 ? d - di[14] : d + di[14];\r\n\r\n\t\tw = di[15] < 0 ? w - di[15] : w + di[15];\r\n\t\th = di[16] < 0 ? h - di[16] : h + di[16];\r\n\t\td = di[17] < 0 ? d - di[17] : d + di[17];\r\n\r\n\t\tvar p = AABB_PROX;\r\n\r\n\t\tthis.aabb.set(\r\n\t\t\tthis.position.x - w - p, this.position.x + w + p,\r\n\t\t\tthis.position.y - h - p, this.position.y + h + p,\r\n\t\t\tthis.position.z - d - p, this.position.z + d + p\r\n\t\t);\r\n\r\n\t\tif ( this.proxy != null ) this.proxy.update();\r\n\r\n\t}\r\n});\n\n/**\r\n * Sphere shape\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n\r\nfunction Sphere( config, radius ) {\r\n\r\n Shape.call( this, config );\r\n\r\n this.type = SHAPE_SPHERE;\r\n\r\n // radius of the shape.\r\n this.radius = radius;\r\n\r\n}\r\n\r\nSphere.prototype = Object.assign( Object.create( Shape.prototype ), {\r\n\r\n\tconstructor: Sphere,\r\n\r\n\tvolume: function () {\r\n\r\n\t\treturn _Math.PI * this.radius * 1.333333;\r\n\r\n\t},\r\n\r\n\tcalculateMassInfo: function ( out ) {\r\n\r\n\t\tvar mass = this.volume() * this.radius * this.radius * this.density; //1.333 * _Math.PI * this.radius * this.radius * this.radius * this.density;\r\n\t\tout.mass = mass;\r\n\t\tvar inertia = mass * this.radius * this.radius * 0.4;\r\n\t\tout.inertia.set( inertia, 0, 0, 0, inertia, 0, 0, 0, inertia );\r\n\r\n\t},\r\n\r\n\tupdateProxy: function () {\r\n\r\n\t\tvar p = AABB_PROX;\r\n\r\n\t\tthis.aabb.set(\r\n\t\t\tthis.position.x - this.radius - p, this.position.x + this.radius + p,\r\n\t\t\tthis.position.y - this.radius - p, this.position.y + this.radius + p,\r\n\t\t\tthis.position.z - this.radius - p, this.position.z + this.radius + p\r\n\t\t);\r\n\r\n\t\tif ( this.proxy != null ) this.proxy.update();\r\n\r\n\t}\r\n\r\n});\n\n/**\r\n * Cylinder shape\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n\r\nfunction Cylinder ( config, radius, height ) {\r\n\r\n Shape.call( this, config );\r\n\r\n this.type = SHAPE_CYLINDER;\r\n\r\n this.radius = radius;\r\n this.height = height;\r\n this.halfHeight = height * 0.5;\r\n\r\n this.normalDirection = new Vec3();\r\n this.halfDirection = new Vec3();\r\n\r\n}\r\n\r\nCylinder.prototype = Object.assign( Object.create( Shape.prototype ), {\r\n\r\n constructor: Cylinder,\r\n\r\n calculateMassInfo: function ( out ) {\r\n\r\n var rsq = this.radius * this.radius;\r\n var mass = _Math.PI * rsq * this.height * this.density;\r\n var inertiaXZ = ( ( 0.25 * rsq ) + ( 0.0833 * this.height * this.height ) ) * mass;\r\n var inertiaY = 0.5 * rsq;\r\n out.mass = mass;\r\n out.inertia.set( inertiaXZ, 0, 0, 0, inertiaY, 0, 0, 0, inertiaXZ );\r\n\r\n },\r\n\r\n updateProxy: function () {\r\n\r\n var te = this.rotation.elements;\r\n var len, wx, hy, dz, xx, yy, zz, w, h, d, p;\r\n\r\n xx = te[1] * te[1];\r\n yy = te[4] * te[4];\r\n zz = te[7] * te[7];\r\n\r\n this.normalDirection.set( te[1], te[4], te[7] );\r\n this.halfDirection.scale( this.normalDirection, this.halfHeight );\r\n\r\n wx = 1 - xx;\r\n len = _Math.sqrt(wx*wx + xx*yy + xx*zz);\r\n if(len>0) len = this.radius/len;\r\n wx *= len;\r\n hy = 1 - yy;\r\n len = _Math.sqrt(yy*xx + hy*hy + yy*zz);\r\n if(len>0) len = this.radius/len;\r\n hy *= len;\r\n dz = 1 - zz;\r\n len = _Math.sqrt(zz*xx + zz*yy + dz*dz);\r\n if(len>0) len = this.radius/len;\r\n dz *= len;\r\n\r\n w = this.halfDirection.x < 0 ? -this.halfDirection.x : this.halfDirection.x;\r\n h = this.halfDirection.y < 0 ? -this.halfDirection.y : this.halfDirection.y;\r\n d = this.halfDirection.z < 0 ? -this.halfDirection.z : this.halfDirection.z;\r\n\r\n w = wx < 0 ? w - wx : w + wx;\r\n h = hy < 0 ? h - hy : h + hy;\r\n d = dz < 0 ? d - dz : d + dz;\r\n\r\n p = AABB_PROX;\r\n\r\n this.aabb.set(\r\n this.position.x - w - p, this.position.x + w + p,\r\n this.position.y - h - p, this.position.y + h + p,\r\n this.position.z - d - p, this.position.z + d + p\r\n );\r\n\r\n if ( this.proxy != null ) this.proxy.update();\r\n\r\n }\r\n\r\n});\n\n/**\r\n * Plane shape.\r\n * @author lo-th\r\n */\r\n\r\nfunction Plane( config, normal ) {\r\n\r\n Shape.call( this, config );\r\n\r\n this.type = SHAPE_PLANE;\r\n\r\n // radius of the shape.\r\n this.normal = new Vec3( 0, 1, 0 );\r\n\r\n}\r\n\r\nPlane.prototype = Object.assign( Object.create( Shape.prototype ), {\r\n\r\n constructor: Plane,\r\n\r\n volume: function () {\r\n\r\n return Number.MAX_VALUE;\r\n\r\n },\r\n\r\n calculateMassInfo: function ( out ) {\r\n\r\n out.mass = this.density;//0.0001;\r\n var inertia = 1;\r\n out.inertia.set( inertia, 0, 0, 0, inertia, 0, 0, 0, inertia );\r\n\r\n },\r\n\r\n updateProxy: function () {\r\n\r\n var p = AABB_PROX;\r\n\r\n var min = -_Math.INF;\r\n var max = _Math.INF;\r\n var n = this.normal;\r\n // The plane AABB is infinite, except if the normal is pointing along any axis\r\n this.aabb.set(\r\n n.x === -1 ? this.position.x - p : min, n.x === 1 ? this.position.x + p : max,\r\n n.y === -1 ? this.position.y - p : min, n.y === 1 ? this.position.y + p : max,\r\n n.z === -1 ? this.position.z - p : min, n.z === 1 ? this.position.z + p : max\r\n );\r\n\r\n if ( this.proxy != null ) this.proxy.update();\r\n\r\n }\r\n\r\n});\n\n/**\r\n * A Particule shape\r\n * @author lo-th\r\n */\r\n\r\nfunction Particle( config, normal ) {\r\n\r\n Shape.call( this, config );\r\n\r\n this.type = SHAPE_PARTICLE;\r\n\r\n}\r\n\r\nParticle.prototype = Object.assign( Object.create( Shape.prototype ), {\r\n\r\n constructor: Particle,\r\n\r\n volume: function () {\r\n\r\n return Number.MAX_VALUE;\r\n\r\n },\r\n\r\n calculateMassInfo: function ( out ) {\r\n\r\n var inertia = 0;\r\n out.inertia.set( inertia, 0, 0, 0, inertia, 0, 0, 0, inertia );\r\n\r\n },\r\n\r\n updateProxy: function () {\r\n\r\n var p = 0;//AABB_PROX;\r\n\r\n this.aabb.set(\r\n this.position.x - p, this.position.x + p,\r\n this.position.y - p, this.position.y + p,\r\n this.position.z - p, this.position.z + p\r\n );\r\n\r\n if ( this.proxy != null ) this.proxy.update();\r\n\r\n }\r\n\r\n});\n\n/**\r\n * A shape configuration holds common configuration data for constructing a shape.\r\n * These configurations can be reused safely.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n \r\nfunction ShapeConfig(){\r\n\r\n // position of the shape in parent's coordinate system.\r\n this.relativePosition = new Vec3();\r\n // rotation matrix of the shape in parent's coordinate system.\r\n this.relativeRotation = new Mat33();\r\n // coefficient of friction of the shape.\r\n this.friction = 0.2; // 0.4\r\n // coefficient of restitution of the shape.\r\n this.restitution = 0.2;\r\n // density of the shape.\r\n this.density = 1;\r\n // bits of the collision groups to which the shape belongs.\r\n this.belongsTo = 1;\r\n // bits of the collision groups with which the shape collides.\r\n this.collidesWith = 0xffffffff;\r\n\r\n}\n\n/**\n* An information of limit and motor.\n*\n* @author saharan\n*/\n\nfunction LimitMotor ( axis, fixed ) {\n\n fixed = fixed || false;\n // The axis of the constraint.\n this.axis = axis;\n // The current angle for rotational constraints.\n this.angle = 0;\n // The lower limit. Set lower > upper to disable\n this.lowerLimit = fixed ? 0 : 1;\n\n // The upper limit. Set lower > upper to disable.\n this.upperLimit = 0;\n // The target motor speed.\n this.motorSpeed = 0;\n // The maximum motor force or torque. Set 0 to disable.\n this.maxMotorForce = 0;\n // The frequency of the spring. Set 0 to disable.\n this.frequency = 0;\n // The damping ratio of the spring. Set 0 for no damping, 1 for critical damping.\n this.dampingRatio = 0;\n\n}\n\nObject.assign( LimitMotor.prototype, {\n\n LimitMotor: true,\n\n // Set limit data into this constraint.\n setLimit:function ( lowerLimit, upperLimit ) {\n\n this.lowerLimit = lowerLimit;\n this.upperLimit = upperLimit;\n\n },\n\n // Set motor data into this constraint.\n setMotor:function ( motorSpeed, maxMotorForce ) {\n \n this.motorSpeed = motorSpeed;\n this.maxMotorForce = maxMotorForce;\n\n },\n\n // Set spring data into this constraint.\n setSpring:function ( frequency, dampingRatio ) {\n \n this.frequency = frequency;\n this.dampingRatio = dampingRatio;\n \n }\n\n});\n\n/**\r\n * The base class of all type of the constraints.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n\r\nfunction Constraint(){\r\n\r\n // parent world of the constraint.\r\n this.parent = null;\r\n\r\n // first body of the constraint.\r\n this.body1 = null;\r\n\r\n // second body of the constraint.\r\n this.body2 = null;\r\n\r\n // Internal\r\n this.addedToIsland = false;\r\n \r\n}\r\n\r\nObject.assign( Constraint.prototype, {\r\n\r\n Constraint: true,\r\n\r\n // Prepare for solving the constraint\r\n preSolve: function( timeStep, invTimeStep ){\r\n\r\n printError(\"Constraint\", \"Inheritance error.\");\r\n\r\n },\r\n\r\n // Solve the constraint. This is usually called iteratively.\r\n solve: function(){\r\n\r\n printError(\"Constraint\", \"Inheritance error.\");\r\n\r\n },\r\n\r\n // Do the post-processing.\r\n postSolve: function(){\r\n\r\n printError(\"Constraint\", \"Inheritance error.\");\r\n\r\n }\r\n\r\n});\n\nfunction JointLink ( joint ){\r\n \r\n // The previous joint link.\r\n this.prev = null;\r\n // The next joint link.\r\n this.next = null;\r\n // The other rigid body connected to the joint.\r\n this.body = null;\r\n // The joint of the link.\r\n this.joint = joint;\r\n\r\n}\n\n/**\n * Joints are used to constrain the motion between two rigid bodies.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction Joint ( config ){\n\n Constraint.call( this );\n\n this.scale = 1;\n this.invScale = 1;\n\n // joint name\n this.name = \"\";\n this.id = NaN;\n\n // The type of the joint.\n this.type = JOINT_NULL;\n // The previous joint in the world.\n this.prev = null;\n // The next joint in the world.\n this.next = null;\n\n this.body1 = config.body1;\n this.body2 = config.body2;\n\n // anchor point on the first rigid body in local coordinate system.\n this.localAnchorPoint1 = new Vec3().copy( config.localAnchorPoint1 );\n // anchor point on the second rigid body in local coordinate system.\n this.localAnchorPoint2 = new Vec3().copy( config.localAnchorPoint2 );\n // anchor point on the first rigid body in world coordinate system relative to the body's origin.\n this.relativeAnchorPoint1 = new Vec3();\n // anchor point on the second rigid body in world coordinate system relative to the body's origin.\n this.relativeAnchorPoint2 = new Vec3();\n // anchor point on the first rigid body in world coordinate system.\n this.anchorPoint1 = new Vec3();\n // anchor point on the second rigid body in world coordinate system.\n this.anchorPoint2 = new Vec3();\n // Whether allow collision between connected rigid bodies or not.\n this.allowCollision = config.allowCollision;\n\n this.b1Link = new JointLink( this );\n this.b2Link = new JointLink( this );\n\n}\n\nJoint.prototype = Object.assign( Object.create( Constraint.prototype ), {\n\n constructor: Joint,\n\n setId: function ( n ) { \n\n this.id = i; \n\n },\n\n setParent: function ( world ) {\n\n this.parent = world;\n this.scale = this.parent.scale;\n this.invScale = this.parent.invScale;\n this.id = this.parent.numJoints;\n if( !this.name ) this.name = 'J' + this.id;\n\n },\n\n // Update all the anchor points.\n\n updateAnchorPoints: function () {\n\n this.relativeAnchorPoint1.copy( this.localAnchorPoint1 ).applyMatrix3( this.body1.rotation, true );\n this.relativeAnchorPoint2.copy( this.localAnchorPoint2 ).applyMatrix3( this.body2.rotation, true );\n\n this.anchorPoint1.add( this.relativeAnchorPoint1, this.body1.position );\n this.anchorPoint2.add( this.relativeAnchorPoint2, this.body2.position );\n\n },\n\n // Attach the joint from the bodies.\n\n attach: function ( isX ) {\n\n this.b1Link.body = this.body2;\n this.b2Link.body = this.body1;\n\n if(isX){\n\n this.body1.jointLink.push( this.b1Link );\n this.body2.jointLink.push( this.b2Link );\n\n } else {\n\n if(this.body1.jointLink != null) (this.b1Link.next=this.body1.jointLink).prev = this.b1Link;\n else this.b1Link.next = null;\n this.body1.jointLink = this.b1Link;\n this.body1.numJoints++;\n if(this.body2.jointLink != null) (this.b2Link.next=this.body2.jointLink).prev = this.b2Link;\n else this.b2Link.next = null;\n this.body2.jointLink = this.b2Link;\n this.body2.numJoints++;\n\n }\n\n },\n\n // Detach the joint from the bodies.\n\n detach: function ( isX ) {\n\n if( isX ){\n\n this.body1.jointLink.splice( this.body1.jointLink.indexOf( this.b1Link ), 1 );\n this.body2.jointLink.splice( this.body2.jointLink.indexOf( this.b2Link ), 1 );\n\n } else {\n\n var prev = this.b1Link.prev;\n var next = this.b1Link.next;\n if(prev != null) prev.next = next;\n if(next != null) next.prev = prev;\n if(this.body1.jointLink == this.b1Link) this.body1.jointLink = next;\n this.b1Link.prev = null;\n this.b1Link.next = null;\n this.b1Link.body = null;\n this.body1.numJoints--;\n\n prev = this.b2Link.prev;\n next = this.b2Link.next;\n if(prev != null) prev.next = next;\n if(next != null) next.prev = prev;\n if(this.body2.jointLink==this.b2Link) this.body2.jointLink = next;\n this.b2Link.prev = null;\n this.b2Link.next = null;\n this.b2Link.body = null;\n this.body2.numJoints--;\n\n }\n\n this.b1Link.body = null;\n this.b2Link.body = null;\n\n },\n\n\n // Awake the bodies.\n\n awake: function () {\n\n this.body1.awake();\n this.body2.awake();\n\n },\n\n // calculation function\n\n preSolve: function ( timeStep, invTimeStep ) {\n\n },\n\n solve: function () {\n\n },\n\n postSolve: function () {\n\n },\n\n // Delete process\n\n remove: function () {\n\n this.dispose();\n\n },\n\n dispose: function () {\n\n this.parent.removeJoint( this );\n\n },\n\n\n // Three js add\n\n getPosition: function () {\n\n var p1 = new Vec3().scale( this.anchorPoint1, this.scale );\n var p2 = new Vec3().scale( this.anchorPoint2, this.scale );\n return [ p1, p2 ];\n\n }\n\n});\n\n/**\n* A linear constraint for all axes for various joints.\n* @author saharan\n*/\nfunction LinearConstraint ( joint ){\n\n this.m1=NaN;\n this.m2=NaN;\n\n this.ii1 = null;\n this.ii2 = null;\n this.dd = null;\n\n this.r1x=NaN;\n this.r1y=NaN;\n this.r1z=NaN;\n\n this.r2x=NaN;\n this.r2y=NaN;\n this.r2z=NaN;\n\n this.ax1x=NaN;\n this.ax1y=NaN;\n this.ax1z=NaN;\n this.ay1x=NaN;\n this.ay1y=NaN;\n this.ay1z=NaN;\n this.az1x=NaN;\n this.az1y=NaN;\n this.az1z=NaN;\n\n this.ax2x=NaN;\n this.ax2y=NaN;\n this.ax2z=NaN;\n this.ay2x=NaN;\n this.ay2y=NaN;\n this.ay2z=NaN;\n this.az2x=NaN;\n this.az2y=NaN;\n this.az2z=NaN;\n\n this.vel=NaN;\n this.velx=NaN;\n this.vely=NaN;\n this.velz=NaN;\n\n\n this.joint = joint;\n this.r1 = joint.relativeAnchorPoint1;\n this.r2 = joint.relativeAnchorPoint2;\n this.p1 = joint.anchorPoint1;\n this.p2 = joint.anchorPoint2;\n this.b1 = joint.body1;\n this.b2 = joint.body2;\n this.l1 = this.b1.linearVelocity;\n this.l2 = this.b2.linearVelocity;\n this.a1 = this.b1.angularVelocity;\n this.a2 = this.b2.angularVelocity;\n this.i1 = this.b1.inverseInertia;\n this.i2 = this.b2.inverseInertia;\n this.impx = 0;\n this.impy = 0;\n this.impz = 0;\n\n}\n\nObject.assign( LinearConstraint.prototype, {\n\n LinearConstraint: true,\n\n preSolve: function ( timeStep, invTimeStep ) {\n \n this.r1x = this.r1.x;\n this.r1y = this.r1.y;\n this.r1z = this.r1.z;\n\n this.r2x = this.r2.x;\n this.r2y = this.r2.y;\n this.r2z = this.r2.z;\n\n this.m1 = this.b1.inverseMass;\n this.m2 = this.b2.inverseMass;\n\n this.ii1 = this.i1.clone();\n this.ii2 = this.i2.clone();\n\n var ii1 = this.ii1.elements;\n var ii2 = this.ii2.elements;\n\n this.ax1x = this.r1z*ii1[1]+-this.r1y*ii1[2];\n this.ax1y = this.r1z*ii1[4]+-this.r1y*ii1[5];\n this.ax1z = this.r1z*ii1[7]+-this.r1y*ii1[8];\n this.ay1x = -this.r1z*ii1[0]+this.r1x*ii1[2];\n this.ay1y = -this.r1z*ii1[3]+this.r1x*ii1[5];\n this.ay1z = -this.r1z*ii1[6]+this.r1x*ii1[8];\n this.az1x = this.r1y*ii1[0]+-this.r1x*ii1[1];\n this.az1y = this.r1y*ii1[3]+-this.r1x*ii1[4];\n this.az1z = this.r1y*ii1[6]+-this.r1x*ii1[7];\n this.ax2x = this.r2z*ii2[1]+-this.r2y*ii2[2];\n this.ax2y = this.r2z*ii2[4]+-this.r2y*ii2[5];\n this.ax2z = this.r2z*ii2[7]+-this.r2y*ii2[8];\n this.ay2x = -this.r2z*ii2[0]+this.r2x*ii2[2];\n this.ay2y = -this.r2z*ii2[3]+this.r2x*ii2[5];\n this.ay2z = -this.r2z*ii2[6]+this.r2x*ii2[8];\n this.az2x = this.r2y*ii2[0]+-this.r2x*ii2[1];\n this.az2y = this.r2y*ii2[3]+-this.r2x*ii2[4];\n this.az2z = this.r2y*ii2[6]+-this.r2x*ii2[7];\n\n // calculate point-to-point mass matrix\n // from impulse equation\n // \n // M = ([/m] - [r^][/I][r^]) ^ -1\n // \n // where\n // \n // [/m] = |1/m, 0, 0|\n // |0, 1/m, 0|\n // |0, 0, 1/m|\n // \n // [r^] = |0, -rz, ry|\n // |rz, 0, -rx|\n // |-ry, rx, 0|\n // \n // [/I] = Inverted moment inertia\n\n var rxx = this.m1+this.m2;\n\n var kk = new Mat33().set( rxx, 0, 0, 0, rxx, 0, 0, 0, rxx );\n var k = kk.elements;\n\n k[0] += ii1[4]*this.r1z*this.r1z-(ii1[7]+ii1[5])*this.r1y*this.r1z+ii1[8]*this.r1y*this.r1y;\n k[1] += (ii1[6]*this.r1y+ii1[5]*this.r1x)*this.r1z-ii1[3]*this.r1z*this.r1z-ii1[8]*this.r1x*this.r1y;\n k[2] += (ii1[3]*this.r1y-ii1[4]*this.r1x)*this.r1z-ii1[6]*this.r1y*this.r1y+ii1[7]*this.r1x*this.r1y;\n k[3] += (ii1[2]*this.r1y+ii1[7]*this.r1x)*this.r1z-ii1[1]*this.r1z*this.r1z-ii1[8]*this.r1x*this.r1y;\n k[4] += ii1[0]*this.r1z*this.r1z-(ii1[6]+ii1[2])*this.r1x*this.r1z+ii1[8]*this.r1x*this.r1x;\n k[5] += (ii1[1]*this.r1x-ii1[0]*this.r1y)*this.r1z-ii1[7]*this.r1x*this.r1x+ii1[6]*this.r1x*this.r1y;\n k[6] += (ii1[1]*this.r1y-ii1[4]*this.r1x)*this.r1z-ii1[2]*this.r1y*this.r1y+ii1[5]*this.r1x*this.r1y;\n k[7] += (ii1[3]*this.r1x-ii1[0]*this.r1y)*this.r1z-ii1[5]*this.r1x*this.r1x+ii1[2]*this.r1x*this.r1y;\n k[8] += ii1[0]*this.r1y*this.r1y-(ii1[3]+ii1[1])*this.r1x*this.r1y+ii1[4]*this.r1x*this.r1x;\n\n k[0] += ii2[4]*this.r2z*this.r2z-(ii2[7]+ii2[5])*this.r2y*this.r2z+ii2[8]*this.r2y*this.r2y;\n k[1] += (ii2[6]*this.r2y+ii2[5]*this.r2x)*this.r2z-ii2[3]*this.r2z*this.r2z-ii2[8]*this.r2x*this.r2y;\n k[2] += (ii2[3]*this.r2y-ii2[4]*this.r2x)*this.r2z-ii2[6]*this.r2y*this.r2y+ii2[7]*this.r2x*this.r2y;\n k[3] += (ii2[2]*this.r2y+ii2[7]*this.r2x)*this.r2z-ii2[1]*this.r2z*this.r2z-ii2[8]*this.r2x*this.r2y;\n k[4] += ii2[0]*this.r2z*this.r2z-(ii2[6]+ii2[2])*this.r2x*this.r2z+ii2[8]*this.r2x*this.r2x;\n k[5] += (ii2[1]*this.r2x-ii2[0]*this.r2y)*this.r2z-ii2[7]*this.r2x*this.r2x+ii2[6]*this.r2x*this.r2y;\n k[6] += (ii2[1]*this.r2y-ii2[4]*this.r2x)*this.r2z-ii2[2]*this.r2y*this.r2y+ii2[5]*this.r2x*this.r2y;\n k[7] += (ii2[3]*this.r2x-ii2[0]*this.r2y)*this.r2z-ii2[5]*this.r2x*this.r2x+ii2[2]*this.r2x*this.r2y;\n k[8] += ii2[0]*this.r2y*this.r2y-(ii2[3]+ii2[1])*this.r2x*this.r2y+ii2[4]*this.r2x*this.r2x;\n\n 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]) );\n this.dd = new Mat33().set(\n k[4]*k[8]-k[5]*k[7], k[2]*k[7]-k[1]*k[8], k[1]*k[5]-k[2]*k[4],\n k[5]*k[6]-k[3]*k[8], k[0]*k[8]-k[2]*k[6], k[2]*k[3]-k[0]*k[5],\n k[3]*k[7]-k[4]*k[6], k[1]*k[6]-k[0]*k[7], k[0]*k[4]-k[1]*k[3]\n ).scaleEqual( inv );\n\n this.velx = this.p2.x-this.p1.x;\n this.vely = this.p2.y-this.p1.y;\n this.velz = this.p2.z-this.p1.z;\n var len = _Math.sqrt(this.velx*this.velx+this.vely*this.vely+this.velz*this.velz);\n if(len>0.005){\n len = (0.005-len)/len*invTimeStep*0.05;\n this.velx *= len;\n this.vely *= len;\n this.velz *= len;\n }else{\n this.velx = 0;\n this.vely = 0;\n this.velz = 0;\n }\n\n this.impx *= 0.95;\n this.impy *= 0.95;\n this.impz *= 0.95;\n \n this.l1.x += this.impx*this.m1;\n this.l1.y += this.impy*this.m1;\n this.l1.z += this.impz*this.m1;\n this.a1.x += this.impx*this.ax1x+this.impy*this.ay1x+this.impz*this.az1x;\n this.a1.y += this.impx*this.ax1y+this.impy*this.ay1y+this.impz*this.az1y;\n this.a1.z += this.impx*this.ax1z+this.impy*this.ay1z+this.impz*this.az1z;\n this.l2.x -= this.impx*this.m2;\n this.l2.y -= this.impy*this.m2;\n this.l2.z -= this.impz*this.m2;\n this.a2.x -= this.impx*this.ax2x+this.impy*this.ay2x+this.impz*this.az2x;\n this.a2.y -= this.impx*this.ax2y+this.impy*this.ay2y+this.impz*this.az2y;\n this.a2.z -= this.impx*this.ax2z+this.impy*this.ay2z+this.impz*this.az2z;\n },\n\n solve: function () {\n\n var d = this.dd.elements;\n 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;\n 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;\n 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;\n var nimpx = rvx*d[0]+rvy*d[1]+rvz*d[2];\n var nimpy = rvx*d[3]+rvy*d[4]+rvz*d[5];\n var nimpz = rvx*d[6]+rvy*d[7]+rvz*d[8];\n this.impx += nimpx;\n this.impy += nimpy;\n this.impz += nimpz;\n this.l1.x += nimpx*this.m1;\n this.l1.y += nimpy*this.m1;\n this.l1.z += nimpz*this.m1;\n this.a1.x += nimpx*this.ax1x+nimpy*this.ay1x+nimpz*this.az1x;\n this.a1.y += nimpx*this.ax1y+nimpy*this.ay1y+nimpz*this.az1y;\n this.a1.z += nimpx*this.ax1z+nimpy*this.ay1z+nimpz*this.az1z;\n this.l2.x -= nimpx*this.m2;\n this.l2.y -= nimpy*this.m2;\n this.l2.z -= nimpz*this.m2;\n this.a2.x -= nimpx*this.ax2x+nimpy*this.ay2x+nimpz*this.az2x;\n this.a2.y -= nimpx*this.ax2y+nimpy*this.ay2y+nimpz*this.az2y;\n this.a2.z -= nimpx*this.ax2z+nimpy*this.ay2z+nimpz*this.az2z;\n\n }\n\n} );\n\n/**\n* A three-axis rotational constraint for various joints.\n* @author saharan\n*/\n\nfunction Rotational3Constraint ( joint, limitMotor1, limitMotor2, limitMotor3 ) {\n \n this.cfm1=NaN;\n this.cfm2=NaN;\n this.cfm3=NaN;\n this.i1e00=NaN;\n this.i1e01=NaN;\n this.i1e02=NaN;\n this.i1e10=NaN;\n this.i1e11=NaN;\n this.i1e12=NaN;\n this.i1e20=NaN;\n this.i1e21=NaN;\n this.i1e22=NaN;\n this.i2e00=NaN;\n this.i2e01=NaN;\n this.i2e02=NaN;\n this.i2e10=NaN;\n this.i2e11=NaN;\n this.i2e12=NaN;\n this.i2e20=NaN;\n this.i2e21=NaN;\n this.i2e22=NaN;\n this.ax1=NaN;\n this.ay1=NaN;\n this.az1=NaN;\n this.ax2=NaN;\n this.ay2=NaN;\n this.az2=NaN;\n this.ax3=NaN;\n this.ay3=NaN;\n this.az3=NaN;\n\n this.a1x1=NaN; // jacoians\n this.a1y1=NaN;\n this.a1z1=NaN;\n this.a2x1=NaN;\n this.a2y1=NaN;\n this.a2z1=NaN;\n this.a1x2=NaN;\n this.a1y2=NaN;\n this.a1z2=NaN;\n this.a2x2=NaN;\n this.a2y2=NaN;\n this.a2z2=NaN;\n this.a1x3=NaN;\n this.a1y3=NaN;\n this.a1z3=NaN;\n this.a2x3=NaN;\n this.a2y3=NaN;\n this.a2z3=NaN;\n\n this.lowerLimit1=NaN;\n this.upperLimit1=NaN;\n this.limitVelocity1=NaN;\n this.limitState1=0; // -1: at lower, 0: locked, 1: at upper, 2: free\n this.enableMotor1=false;\n this.motorSpeed1=NaN;\n this.maxMotorForce1=NaN;\n this.maxMotorImpulse1=NaN;\n this.lowerLimit2=NaN;\n this.upperLimit2=NaN;\n this.limitVelocity2=NaN;\n this.limitState2=0; // -1: at lower, 0: locked, 1: at upper, 2: free\n this.enableMotor2=false;\n this.motorSpeed2=NaN;\n this.maxMotorForce2=NaN;\n this.maxMotorImpulse2=NaN;\n this.lowerLimit3=NaN;\n this.upperLimit3=NaN;\n this.limitVelocity3=NaN;\n this.limitState3=0; // -1: at lower, 0: locked, 1: at upper, 2: free\n this.enableMotor3=false;\n this.motorSpeed3=NaN;\n this.maxMotorForce3=NaN;\n this.maxMotorImpulse3=NaN;\n\n this.k00=NaN; // K = J*M*JT\n this.k01=NaN;\n this.k02=NaN;\n this.k10=NaN;\n this.k11=NaN;\n this.k12=NaN;\n this.k20=NaN;\n this.k21=NaN;\n this.k22=NaN;\n\n this.kv00=NaN; // diagonals without CFMs\n this.kv11=NaN;\n this.kv22=NaN;\n\n this.dv00=NaN; // ...inverted\n this.dv11=NaN;\n this.dv22=NaN;\n\n this.d00=NaN; // K^-1\n this.d01=NaN;\n this.d02=NaN;\n this.d10=NaN;\n this.d11=NaN;\n this.d12=NaN;\n this.d20=NaN;\n this.d21=NaN;\n this.d22=NaN;\n\n this.limitMotor1=limitMotor1;\n this.limitMotor2=limitMotor2;\n this.limitMotor3=limitMotor3;\n this.b1=joint.body1;\n this.b2=joint.body2;\n this.a1=this.b1.angularVelocity;\n this.a2=this.b2.angularVelocity;\n this.i1=this.b1.inverseInertia;\n this.i2=this.b2.inverseInertia;\n this.limitImpulse1=0;\n this.motorImpulse1=0;\n this.limitImpulse2=0;\n this.motorImpulse2=0;\n this.limitImpulse3=0;\n this.motorImpulse3=0;\n\n}\n\nObject.assign( Rotational3Constraint.prototype, {\n\n Rotational3Constraint: true,\n\n preSolve: function( timeStep, invTimeStep ){\n\n this.ax1=this.limitMotor1.axis.x;\n this.ay1=this.limitMotor1.axis.y;\n this.az1=this.limitMotor1.axis.z;\n this.ax2=this.limitMotor2.axis.x;\n this.ay2=this.limitMotor2.axis.y;\n this.az2=this.limitMotor2.axis.z;\n this.ax3=this.limitMotor3.axis.x;\n this.ay3=this.limitMotor3.axis.y;\n this.az3=this.limitMotor3.axis.z;\n this.lowerLimit1=this.limitMotor1.lowerLimit;\n this.upperLimit1=this.limitMotor1.upperLimit;\n this.motorSpeed1=this.limitMotor1.motorSpeed;\n this.maxMotorForce1=this.limitMotor1.maxMotorForce;\n this.enableMotor1=this.maxMotorForce1>0;\n this.lowerLimit2=this.limitMotor2.lowerLimit;\n this.upperLimit2=this.limitMotor2.upperLimit;\n this.motorSpeed2=this.limitMotor2.motorSpeed;\n this.maxMotorForce2=this.limitMotor2.maxMotorForce;\n this.enableMotor2=this.maxMotorForce2>0;\n this.lowerLimit3=this.limitMotor3.lowerLimit;\n this.upperLimit3=this.limitMotor3.upperLimit;\n this.motorSpeed3=this.limitMotor3.motorSpeed;\n this.maxMotorForce3=this.limitMotor3.maxMotorForce;\n this.enableMotor3=this.maxMotorForce3>0;\n\n var ti1 = this.i1.elements;\n var ti2 = this.i2.elements;\n this.i1e00=ti1[0];\n this.i1e01=ti1[1];\n this.i1e02=ti1[2];\n this.i1e10=ti1[3];\n this.i1e11=ti1[4];\n this.i1e12=ti1[5];\n this.i1e20=ti1[6];\n this.i1e21=ti1[7];\n this.i1e22=ti1[8];\n\n this.i2e00=ti2[0];\n this.i2e01=ti2[1];\n this.i2e02=ti2[2];\n this.i2e10=ti2[3];\n this.i2e11=ti2[4];\n this.i2e12=ti2[5];\n this.i2e20=ti2[6];\n this.i2e21=ti2[7];\n this.i2e22=ti2[8];\n\n var frequency1=this.limitMotor1.frequency;\n var frequency2=this.limitMotor2.frequency;\n var frequency3=this.limitMotor3.frequency;\n var enableSpring1=frequency1>0;\n var enableSpring2=frequency2>0;\n var enableSpring3=frequency3>0;\n var enableLimit1=this.lowerLimit1<=this.upperLimit1;\n var enableLimit2=this.lowerLimit2<=this.upperLimit2;\n var enableLimit3=this.lowerLimit3<=this.upperLimit3;\n var angle1=this.limitMotor1.angle;\n if(enableLimit1){\n if(this.lowerLimit1==this.upperLimit1){\n if(this.limitState1!=0){\n this.limitState1=0;\n this.limitImpulse1=0;\n }\n this.limitVelocity1=this.lowerLimit1-angle1;\n }else if(angle1this.upperLimit1){\n if(this.limitState1!=1){\n this.limitState1=1;\n this.limitImpulse1=0;\n }\n this.limitVelocity1=this.upperLimit1-angle1;\n }else{\n this.limitState1=2;\n this.limitImpulse1=0;\n this.limitVelocity1=0;\n }\n if(!enableSpring1){\n if(this.limitVelocity1>0.02)this.limitVelocity1-=0.02;\n else if(this.limitVelocity1<-0.02)this.limitVelocity1+=0.02;\n else this.limitVelocity1=0;\n }\n }else{\n this.limitState1=2;\n this.limitImpulse1=0;\n }\n\n var angle2=this.limitMotor2.angle;\n if(enableLimit2){\n if(this.lowerLimit2==this.upperLimit2){\n if(this.limitState2!=0){\n this.limitState2=0;\n this.limitImpulse2=0;\n }\n this.limitVelocity2=this.lowerLimit2-angle2;\n }else if(angle2this.upperLimit2){\n if(this.limitState2!=1){\n this.limitState2=1;\n this.limitImpulse2=0;\n }\n this.limitVelocity2=this.upperLimit2-angle2;\n }else{\n this.limitState2=2;\n this.limitImpulse2=0;\n this.limitVelocity2=0;\n }\n if(!enableSpring2){\n if(this.limitVelocity2>0.02)this.limitVelocity2-=0.02;\n else if(this.limitVelocity2<-0.02)this.limitVelocity2+=0.02;\n else this.limitVelocity2=0;\n }\n }else{\n this.limitState2=2;\n this.limitImpulse2=0;\n }\n\n var angle3=this.limitMotor3.angle;\n if(enableLimit3){\n if(this.lowerLimit3==this.upperLimit3){\n if(this.limitState3!=0){\n this.limitState3=0;\n this.limitImpulse3=0;\n }\n this.limitVelocity3=this.lowerLimit3-angle3;\n }else if(angle3this.upperLimit3){\n if(this.limitState3!=1){\n this.limitState3=1;\n this.limitImpulse3=0;\n }\n this.limitVelocity3=this.upperLimit3-angle3;\n }else{\n this.limitState3=2;\n this.limitImpulse3=0;\n this.limitVelocity3=0;\n }\n if(!enableSpring3){\n if(this.limitVelocity3>0.02)this.limitVelocity3-=0.02;\n else if(this.limitVelocity3<-0.02)this.limitVelocity3+=0.02;\n else this.limitVelocity3=0;\n }\n }else{\n this.limitState3=2;\n this.limitImpulse3=0;\n }\n\n if(this.enableMotor1&&(this.limitState1!=0||enableSpring1)){\n this.maxMotorImpulse1=this.maxMotorForce1*timeStep;\n }else{\n this.motorImpulse1=0;\n this.maxMotorImpulse1=0;\n }\n if(this.enableMotor2&&(this.limitState2!=0||enableSpring2)){\n this.maxMotorImpulse2=this.maxMotorForce2*timeStep;\n }else{\n this.motorImpulse2=0;\n this.maxMotorImpulse2=0;\n }\n if(this.enableMotor3&&(this.limitState3!=0||enableSpring3)){\n this.maxMotorImpulse3=this.maxMotorForce3*timeStep;\n }else{\n this.motorImpulse3=0;\n this.maxMotorImpulse3=0;\n }\n\n // build jacobians\n this.a1x1=this.ax1*this.i1e00+this.ay1*this.i1e01+this.az1*this.i1e02;\n this.a1y1=this.ax1*this.i1e10+this.ay1*this.i1e11+this.az1*this.i1e12;\n this.a1z1=this.ax1*this.i1e20+this.ay1*this.i1e21+this.az1*this.i1e22;\n this.a2x1=this.ax1*this.i2e00+this.ay1*this.i2e01+this.az1*this.i2e02;\n this.a2y1=this.ax1*this.i2e10+this.ay1*this.i2e11+this.az1*this.i2e12;\n this.a2z1=this.ax1*this.i2e20+this.ay1*this.i2e21+this.az1*this.i2e22;\n\n this.a1x2=this.ax2*this.i1e00+this.ay2*this.i1e01+this.az2*this.i1e02;\n this.a1y2=this.ax2*this.i1e10+this.ay2*this.i1e11+this.az2*this.i1e12;\n this.a1z2=this.ax2*this.i1e20+this.ay2*this.i1e21+this.az2*this.i1e22;\n this.a2x2=this.ax2*this.i2e00+this.ay2*this.i2e01+this.az2*this.i2e02;\n this.a2y2=this.ax2*this.i2e10+this.ay2*this.i2e11+this.az2*this.i2e12;\n this.a2z2=this.ax2*this.i2e20+this.ay2*this.i2e21+this.az2*this.i2e22;\n\n this.a1x3=this.ax3*this.i1e00+this.ay3*this.i1e01+this.az3*this.i1e02;\n this.a1y3=this.ax3*this.i1e10+this.ay3*this.i1e11+this.az3*this.i1e12;\n this.a1z3=this.ax3*this.i1e20+this.ay3*this.i1e21+this.az3*this.i1e22;\n this.a2x3=this.ax3*this.i2e00+this.ay3*this.i2e01+this.az3*this.i2e02;\n this.a2y3=this.ax3*this.i2e10+this.ay3*this.i2e11+this.az3*this.i2e12;\n this.a2z3=this.ax3*this.i2e20+this.ay3*this.i2e21+this.az3*this.i2e22;\n\n // build an impulse matrix\n this.k00=this.ax1*(this.a1x1+this.a2x1)+this.ay1*(this.a1y1+this.a2y1)+this.az1*(this.a1z1+this.a2z1);\n this.k01=this.ax1*(this.a1x2+this.a2x2)+this.ay1*(this.a1y2+this.a2y2)+this.az1*(this.a1z2+this.a2z2);\n this.k02=this.ax1*(this.a1x3+this.a2x3)+this.ay1*(this.a1y3+this.a2y3)+this.az1*(this.a1z3+this.a2z3);\n this.k10=this.ax2*(this.a1x1+this.a2x1)+this.ay2*(this.a1y1+this.a2y1)+this.az2*(this.a1z1+this.a2z1);\n this.k11=this.ax2*(this.a1x2+this.a2x2)+this.ay2*(this.a1y2+this.a2y2)+this.az2*(this.a1z2+this.a2z2);\n this.k12=this.ax2*(this.a1x3+this.a2x3)+this.ay2*(this.a1y3+this.a2y3)+this.az2*(this.a1z3+this.a2z3);\n this.k20=this.ax3*(this.a1x1+this.a2x1)+this.ay3*(this.a1y1+this.a2y1)+this.az3*(this.a1z1+this.a2z1);\n this.k21=this.ax3*(this.a1x2+this.a2x2)+this.ay3*(this.a1y2+this.a2y2)+this.az3*(this.a1z2+this.a2z2);\n this.k22=this.ax3*(this.a1x3+this.a2x3)+this.ay3*(this.a1y3+this.a2y3)+this.az3*(this.a1z3+this.a2z3);\n\n this.kv00=this.k00;\n this.kv11=this.k11;\n this.kv22=this.k22;\n this.dv00=1/this.kv00;\n this.dv11=1/this.kv11;\n this.dv22=1/this.kv22;\n\n if(enableSpring1&&this.limitState1!=2){\n var omega=6.2831853*frequency1;\n var k=omega*omega*timeStep;\n var dmp=invTimeStep/(k+2*this.limitMotor1.dampingRatio*omega);\n this.cfm1=this.kv00*dmp;\n this.limitVelocity1*=k*dmp;\n }else{\n this.cfm1=0;\n this.limitVelocity1*=invTimeStep*0.05;\n }\n\n if(enableSpring2&&this.limitState2!=2){\n omega=6.2831853*frequency2;\n k=omega*omega*timeStep;\n dmp=invTimeStep/(k+2*this.limitMotor2.dampingRatio*omega);\n this.cfm2=this.kv11*dmp;\n this.limitVelocity2*=k*dmp;\n }else{\n this.cfm2=0;\n this.limitVelocity2*=invTimeStep*0.05;\n }\n\n if(enableSpring3&&this.limitState3!=2){\n omega=6.2831853*frequency3;\n k=omega*omega*timeStep;\n dmp=invTimeStep/(k+2*this.limitMotor3.dampingRatio*omega);\n this.cfm3=this.kv22*dmp;\n this.limitVelocity3*=k*dmp;\n }else{\n this.cfm3=0;\n this.limitVelocity3*=invTimeStep*0.05;\n }\n\n this.k00+=this.cfm1;\n this.k11+=this.cfm2;\n this.k22+=this.cfm3;\n\n var inv=1/(\n this.k00*(this.k11*this.k22-this.k21*this.k12)+\n this.k10*(this.k21*this.k02-this.k01*this.k22)+\n this.k20*(this.k01*this.k12-this.k11*this.k02)\n );\n this.d00=(this.k11*this.k22-this.k12*this.k21)*inv;\n this.d01=(this.k02*this.k21-this.k01*this.k22)*inv;\n this.d02=(this.k01*this.k12-this.k02*this.k11)*inv;\n this.d10=(this.k12*this.k20-this.k10*this.k22)*inv;\n this.d11=(this.k00*this.k22-this.k02*this.k20)*inv;\n this.d12=(this.k02*this.k10-this.k00*this.k12)*inv;\n this.d20=(this.k10*this.k21-this.k11*this.k20)*inv;\n this.d21=(this.k01*this.k20-this.k00*this.k21)*inv;\n this.d22=(this.k00*this.k11-this.k01*this.k10)*inv;\n \n this.limitImpulse1*=0.95;\n this.motorImpulse1*=0.95;\n this.limitImpulse2*=0.95;\n this.motorImpulse2*=0.95;\n this.limitImpulse3*=0.95;\n this.motorImpulse3*=0.95;\n var totalImpulse1=this.limitImpulse1+this.motorImpulse1;\n var totalImpulse2=this.limitImpulse2+this.motorImpulse2;\n var totalImpulse3=this.limitImpulse3+this.motorImpulse3;\n this.a1.x+=totalImpulse1*this.a1x1+totalImpulse2*this.a1x2+totalImpulse3*this.a1x3;\n this.a1.y+=totalImpulse1*this.a1y1+totalImpulse2*this.a1y2+totalImpulse3*this.a1y3;\n this.a1.z+=totalImpulse1*this.a1z1+totalImpulse2*this.a1z2+totalImpulse3*this.a1z3;\n this.a2.x-=totalImpulse1*this.a2x1+totalImpulse2*this.a2x2+totalImpulse3*this.a2x3;\n this.a2.y-=totalImpulse1*this.a2y1+totalImpulse2*this.a2y2+totalImpulse3*this.a2y3;\n this.a2.z-=totalImpulse1*this.a2z1+totalImpulse2*this.a2z2+totalImpulse3*this.a2z3;\n },\n solve_:function(){\n\n var rvx=this.a2.x-this.a1.x;\n var rvy=this.a2.y-this.a1.y;\n var rvz=this.a2.z-this.a1.z;\n\n this.limitVelocity3=30;\n var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1-this.limitVelocity1;\n var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2-this.limitVelocity2;\n var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3-this.limitVelocity3;\n\n var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02;\n var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12;\n var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22;\n\n this.limitImpulse1+=dLimitImpulse1;\n this.limitImpulse2+=dLimitImpulse2;\n this.limitImpulse3+=dLimitImpulse3;\n\n this.a1.x+=dLimitImpulse1*this.a1x1+dLimitImpulse2*this.a1x2+dLimitImpulse3*this.a1x3;\n this.a1.y+=dLimitImpulse1*this.a1y1+dLimitImpulse2*this.a1y2+dLimitImpulse3*this.a1y3;\n this.a1.z+=dLimitImpulse1*this.a1z1+dLimitImpulse2*this.a1z2+dLimitImpulse3*this.a1z3;\n this.a2.x-=dLimitImpulse1*this.a2x1+dLimitImpulse2*this.a2x2+dLimitImpulse3*this.a2x3;\n this.a2.y-=dLimitImpulse1*this.a2y1+dLimitImpulse2*this.a2y2+dLimitImpulse3*this.a2y3;\n this.a2.z-=dLimitImpulse1*this.a2z1+dLimitImpulse2*this.a2z2+dLimitImpulse3*this.a2z3;\n },\n solve:function(){\n\n var rvx=this.a2.x-this.a1.x;\n var rvy=this.a2.y-this.a1.y;\n var rvz=this.a2.z-this.a1.z;\n\n var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1;\n var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2;\n var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3;\n\n var oldMotorImpulse1=this.motorImpulse1;\n var oldMotorImpulse2=this.motorImpulse2;\n var oldMotorImpulse3=this.motorImpulse3;\n\n var dMotorImpulse1=0;\n var dMotorImpulse2=0;\n var dMotorImpulse3=0;\n\n if(this.enableMotor1){\n dMotorImpulse1=(rvn1-this.motorSpeed1)*this.dv00;\n this.motorImpulse1+=dMotorImpulse1;\n if(this.motorImpulse1>this.maxMotorImpulse1){ // clamp motor impulse\n this.motorImpulse1=this.maxMotorImpulse1;\n }else if(this.motorImpulse1<-this.maxMotorImpulse1){\n this.motorImpulse1=-this.maxMotorImpulse1;\n }\n dMotorImpulse1=this.motorImpulse1-oldMotorImpulse1;\n }\n if(this.enableMotor2){\n dMotorImpulse2=(rvn2-this.motorSpeed2)*this.dv11;\n this.motorImpulse2+=dMotorImpulse2;\n if(this.motorImpulse2>this.maxMotorImpulse2){ // clamp motor impulse\n this.motorImpulse2=this.maxMotorImpulse2;\n }else if(this.motorImpulse2<-this.maxMotorImpulse2){\n this.motorImpulse2=-this.maxMotorImpulse2;\n }\n dMotorImpulse2=this.motorImpulse2-oldMotorImpulse2;\n }\n if(this.enableMotor3){\n dMotorImpulse3=(rvn3-this.motorSpeed3)*this.dv22;\n this.motorImpulse3+=dMotorImpulse3;\n if(this.motorImpulse3>this.maxMotorImpulse3){ // clamp motor impulse\n this.motorImpulse3=this.maxMotorImpulse3;\n }else if(this.motorImpulse3<-this.maxMotorImpulse3){\n this.motorImpulse3=-this.maxMotorImpulse3;\n }\n dMotorImpulse3=this.motorImpulse3-oldMotorImpulse3;\n }\n\n // apply motor impulse to relative velocity\n rvn1+=dMotorImpulse1*this.kv00+dMotorImpulse2*this.k01+dMotorImpulse3*this.k02;\n rvn2+=dMotorImpulse1*this.k10+dMotorImpulse2*this.kv11+dMotorImpulse3*this.k12;\n rvn3+=dMotorImpulse1*this.k20+dMotorImpulse2*this.k21+dMotorImpulse3*this.kv22;\n\n // subtract target velocity and applied impulse\n rvn1-=this.limitVelocity1+this.limitImpulse1*this.cfm1;\n rvn2-=this.limitVelocity2+this.limitImpulse2*this.cfm2;\n rvn3-=this.limitVelocity3+this.limitImpulse3*this.cfm3;\n\n var oldLimitImpulse1=this.limitImpulse1;\n var oldLimitImpulse2=this.limitImpulse2;\n var oldLimitImpulse3=this.limitImpulse3;\n\n var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02;\n var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12;\n var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22;\n\n this.limitImpulse1+=dLimitImpulse1;\n this.limitImpulse2+=dLimitImpulse2;\n this.limitImpulse3+=dLimitImpulse3;\n\n // clamp\n var clampState=0;\n if(this.limitState1==2||this.limitImpulse1*this.limitState1<0){\n dLimitImpulse1=-oldLimitImpulse1;\n rvn2+=dLimitImpulse1*this.k10;\n rvn3+=dLimitImpulse1*this.k20;\n clampState|=1;\n }\n if(this.limitState2==2||this.limitImpulse2*this.limitState2<0){\n dLimitImpulse2=-oldLimitImpulse2;\n rvn1+=dLimitImpulse2*this.k01;\n rvn3+=dLimitImpulse2*this.k21;\n clampState|=2;\n }\n if(this.limitState3==2||this.limitImpulse3*this.limitState3<0){\n dLimitImpulse3=-oldLimitImpulse3;\n rvn1+=dLimitImpulse3*this.k02;\n rvn2+=dLimitImpulse3*this.k12;\n clampState|=4;\n }\n\n // update un-clamped impulse\n // TODO: isolate division\n var det;\n switch(clampState){\n case 1: // update 2 3\n det=1/(this.k11*this.k22-this.k12*this.k21);\n dLimitImpulse2=(this.k22*rvn2+-this.k12*rvn3)*det;\n dLimitImpulse3=(-this.k21*rvn2+this.k11*rvn3)*det;\n break;\n case 2: // update 1 3\n det=1/(this.k00*this.k22-this.k02*this.k20);\n dLimitImpulse1=(this.k22*rvn1+-this.k02*rvn3)*det;\n dLimitImpulse3=(-this.k20*rvn1+this.k00*rvn3)*det;\n break;\n case 3: // update 3\n dLimitImpulse3=rvn3/this.k22;\n break;\n case 4: // update 1 2\n det=1/(this.k00*this.k11-this.k01*this.k10);\n dLimitImpulse1=(this.k11*rvn1+-this.k01*rvn2)*det;\n dLimitImpulse2=(-this.k10*rvn1+this.k00*rvn2)*det;\n break;\n case 5: // update 2\n dLimitImpulse2=rvn2/this.k11;\n break;\n case 6: // update 1\n dLimitImpulse1=rvn1/this.k00;\n break;\n }\n\n this.limitImpulse1=dLimitImpulse1+oldLimitImpulse1;\n this.limitImpulse2=dLimitImpulse2+oldLimitImpulse2;\n this.limitImpulse3=dLimitImpulse3+oldLimitImpulse3;\n\n var dImpulse1=dMotorImpulse1+dLimitImpulse1;\n var dImpulse2=dMotorImpulse2+dLimitImpulse2;\n var dImpulse3=dMotorImpulse3+dLimitImpulse3;\n\n // apply impulse\n this.a1.x+=dImpulse1*this.a1x1+dImpulse2*this.a1x2+dImpulse3*this.a1x3;\n this.a1.y+=dImpulse1*this.a1y1+dImpulse2*this.a1y2+dImpulse3*this.a1y3;\n this.a1.z+=dImpulse1*this.a1z1+dImpulse2*this.a1z2+dImpulse3*this.a1z3;\n this.a2.x-=dImpulse1*this.a2x1+dImpulse2*this.a2x2+dImpulse3*this.a2x3;\n this.a2.y-=dImpulse1*this.a2y1+dImpulse2*this.a2y2+dImpulse3*this.a2y3;\n this.a2.z-=dImpulse1*this.a2z1+dImpulse2*this.a2z2+dImpulse3*this.a2z3;\n rvx=this.a2.x-this.a1.x;\n rvy=this.a2.y-this.a1.y;\n rvz=this.a2.z-this.a1.z;\n\n rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2;\n }\n \n} );\n\n/**\n * A hinge joint allows only for relative rotation of rigid bodies along the axis.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction HingeJoint ( config, lowerAngleLimit, upperAngleLimit ) {\n\n Joint.call( this, config );\n\n this.type = JOINT_HINGE;\n\n // The axis in the first body's coordinate system.\n this.localAxis1 = config.localAxis1.clone().normalize();\n // The axis in the second body's coordinate system.\n this.localAxis2 = config.localAxis2.clone().normalize();\n\n // make angle axis\n var arc = new Mat33().setQuat( new Quat().setFromUnitVectors( this.localAxis1, this.localAxis2 ) );\n this.localAngle1 = new Vec3().tangent( this.localAxis1 ).normalize();\n this.localAngle2 = this.localAngle1.clone().applyMatrix3( arc, true );\n\n this.ax1 = new Vec3();\n this.ax2 = new Vec3();\n this.an1 = new Vec3();\n this.an2 = new Vec3();\n\n this.tmp = new Vec3();\n\n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n // The rotational limit and motor information of the joint.\n this.limitMotor = new LimitMotor( this.nor, false );\n this.limitMotor.lowerLimit = lowerAngleLimit;\n this.limitMotor.upperLimit = upperAngleLimit;\n\n this.lc = new LinearConstraint( this );\n this.r3 = new Rotational3Constraint( this, this.limitMotor, new LimitMotor( this.tan, true ), new LimitMotor( this.bin, true ) );\n}\n\nHingeJoint.prototype = Object.assign( Object.create( Joint.prototype ), {\n\n constructor: HingeJoint,\n\n\n preSolve: function ( timeStep, invTimeStep ) {\n\n this.updateAnchorPoints();\n\n this.ax1.copy( this.localAxis1 ).applyMatrix3( this.body1.rotation, true );\n this.ax2.copy( this.localAxis2 ).applyMatrix3( this.body2.rotation, true );\n\n this.an1.copy( this.localAngle1 ).applyMatrix3( this.body1.rotation, true );\n this.an2.copy( this.localAngle2 ).applyMatrix3( this.body2.rotation, true );\n\n // normal tangent binormal\n\n this.nor.set(\n this.ax1.x*this.body2.inverseMass + this.ax2.x*this.body1.inverseMass,\n this.ax1.y*this.body2.inverseMass + this.ax2.y*this.body1.inverseMass,\n this.ax1.z*this.body2.inverseMass + this.ax2.z*this.body1.inverseMass\n ).normalize();\n\n this.tan.tangent( this.nor ).normalize();\n\n this.bin.crossVectors( this.nor, this.tan );\n\n // calculate hinge angle\n\n var limite = _Math.acosClamp( _Math.dotVectors( this.an1, this.an2 ) );\n\n this.tmp.crossVectors( this.an1, this.an2 );\n\n if( _Math.dotVectors( this.nor, this.tmp ) < 0 ) this.limitMotor.angle = -limite;\n else this.limitMotor.angle = limite;\n\n this.tmp.crossVectors( this.ax1, this.ax2 );\n\n this.r3.limitMotor2.angle = _Math.dotVectors( this.tan, this.tmp );\n this.r3.limitMotor3.angle = _Math.dotVectors( this.bin, this.tmp );\n\n // preSolve\n \n this.r3.preSolve( timeStep, invTimeStep );\n this.lc.preSolve( timeStep, invTimeStep );\n\n },\n\n solve: function () {\n\n this.r3.solve();\n this.lc.solve();\n\n },\n\n postSolve: function () {\n\n }\n\n});\n\n/**\n * A ball-and-socket joint limits relative translation on two anchor points on rigid bodies.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction BallAndSocketJoint ( config ){\n\n Joint.call( this, config );\n\n this.type = JOINT_BALL_AND_SOCKET;\n \n this.lc = new LinearConstraint( this );\n\n}\n\nBallAndSocketJoint.prototype = Object.assign( Object.create( Joint.prototype ), {\n\n constructor: BallAndSocketJoint,\n\n preSolve: function ( timeStep, invTimeStep ) {\n\n this.updateAnchorPoints();\n\n // preSolve\n\n this.lc.preSolve( timeStep, invTimeStep );\n\n },\n\n solve: function () {\n\n this.lc.solve();\n\n },\n\n postSolve: function () {\n\n }\n\n});\n\n/**\n* A translational constraint for various joints.\n* @author saharan\n*/\nfunction TranslationalConstraint ( joint, limitMotor ){\n this.cfm=NaN;\n this.m1=NaN;\n this.m2=NaN;\n this.i1e00=NaN;\n this.i1e01=NaN;\n this.i1e02=NaN;\n this.i1e10=NaN;\n this.i1e11=NaN;\n this.i1e12=NaN;\n this.i1e20=NaN;\n this.i1e21=NaN;\n this.i1e22=NaN;\n this.i2e00=NaN;\n this.i2e01=NaN;\n this.i2e02=NaN;\n this.i2e10=NaN;\n this.i2e11=NaN;\n this.i2e12=NaN;\n this.i2e20=NaN;\n this.i2e21=NaN;\n this.i2e22=NaN;\n this.motorDenom=NaN;\n this.invMotorDenom=NaN;\n this.invDenom=NaN;\n this.ax=NaN;\n this.ay=NaN;\n this.az=NaN;\n this.r1x=NaN;\n this.r1y=NaN;\n this.r1z=NaN;\n this.r2x=NaN;\n this.r2y=NaN;\n this.r2z=NaN;\n this.t1x=NaN;\n this.t1y=NaN;\n this.t1z=NaN;\n this.t2x=NaN;\n this.t2y=NaN;\n this.t2z=NaN;\n this.l1x=NaN;\n this.l1y=NaN;\n this.l1z=NaN;\n this.l2x=NaN;\n this.l2y=NaN;\n this.l2z=NaN;\n this.a1x=NaN;\n this.a1y=NaN;\n this.a1z=NaN;\n this.a2x=NaN;\n this.a2y=NaN;\n this.a2z=NaN;\n this.lowerLimit=NaN;\n this.upperLimit=NaN;\n this.limitVelocity=NaN;\n this.limitState=0; // -1: at lower, 0: locked, 1: at upper, 2: free\n this.enableMotor=false;\n this.motorSpeed=NaN;\n this.maxMotorForce=NaN;\n this.maxMotorImpulse=NaN;\n\n this.limitMotor=limitMotor;\n this.b1=joint.body1;\n this.b2=joint.body2;\n this.p1=joint.anchorPoint1;\n this.p2=joint.anchorPoint2;\n this.r1=joint.relativeAnchorPoint1;\n this.r2=joint.relativeAnchorPoint2;\n this.l1=this.b1.linearVelocity;\n this.l2=this.b2.linearVelocity;\n this.a1=this.b1.angularVelocity;\n this.a2=this.b2.angularVelocity;\n this.i1=this.b1.inverseInertia;\n this.i2=this.b2.inverseInertia;\n this.limitImpulse=0;\n this.motorImpulse=0;\n}\n\nObject.assign( TranslationalConstraint.prototype, {\n\n TranslationalConstraint: true,\n\n preSolve:function(timeStep,invTimeStep){\n this.ax=this.limitMotor.axis.x;\n this.ay=this.limitMotor.axis.y;\n this.az=this.limitMotor.axis.z;\n this.lowerLimit=this.limitMotor.lowerLimit;\n this.upperLimit=this.limitMotor.upperLimit;\n this.motorSpeed=this.limitMotor.motorSpeed;\n this.maxMotorForce=this.limitMotor.maxMotorForce;\n this.enableMotor=this.maxMotorForce>0;\n this.m1=this.b1.inverseMass;\n this.m2=this.b2.inverseMass;\n\n var ti1 = this.i1.elements;\n var ti2 = this.i2.elements;\n this.i1e00=ti1[0];\n this.i1e01=ti1[1];\n this.i1e02=ti1[2];\n this.i1e10=ti1[3];\n this.i1e11=ti1[4];\n this.i1e12=ti1[5];\n this.i1e20=ti1[6];\n this.i1e21=ti1[7];\n this.i1e22=ti1[8];\n\n this.i2e00=ti2[0];\n this.i2e01=ti2[1];\n this.i2e02=ti2[2];\n this.i2e10=ti2[3];\n this.i2e11=ti2[4];\n this.i2e12=ti2[5];\n this.i2e20=ti2[6];\n this.i2e21=ti2[7];\n this.i2e22=ti2[8];\n\n var dx=this.p2.x-this.p1.x;\n var dy=this.p2.y-this.p1.y;\n var dz=this.p2.z-this.p1.z;\n var d=dx*this.ax+dy*this.ay+dz*this.az;\n var frequency=this.limitMotor.frequency;\n var enableSpring=frequency>0;\n var enableLimit=this.lowerLimit<=this.upperLimit;\n if(enableSpring&&d>20||d<-20){\n enableSpring=false;\n }\n\n if(enableLimit){\n if(this.lowerLimit==this.upperLimit){\n if(this.limitState!=0){\n this.limitState=0;\n this.limitImpulse=0;\n }\n this.limitVelocity=this.lowerLimit-d;\n if(!enableSpring)d=this.lowerLimit;\n }else if(dthis.upperLimit){\n if(this.limitState!=1){\n this.limitState=1;\n this.limitImpulse=0;\n }\n this.limitVelocity=this.upperLimit-d;\n if(!enableSpring)d=this.upperLimit;\n }else{\n this.limitState=2;\n this.limitImpulse=0;\n this.limitVelocity=0;\n }\n if(!enableSpring){\n if(this.limitVelocity>0.005)this.limitVelocity-=0.005;\n else if(this.limitVelocity<-0.005)this.limitVelocity+=0.005;\n else this.limitVelocity=0;\n }\n }else{\n this.limitState=2;\n this.limitImpulse=0;\n }\n\n if(this.enableMotor&&(this.limitState!=0||enableSpring)){\n this.maxMotorImpulse=this.maxMotorForce*timeStep;\n }else{\n this.motorImpulse=0;\n this.maxMotorImpulse=0;\n }\n\n var rdx=d*this.ax;\n var rdy=d*this.ay;\n var rdz=d*this.az;\n var w1=this.m1/(this.m1+this.m2);\n var w2=1-w1;\n this.r1x=this.r1.x+rdx*w1;\n this.r1y=this.r1.y+rdy*w1;\n this.r1z=this.r1.z+rdz*w1;\n this.r2x=this.r2.x-rdx*w2;\n this.r2y=this.r2.y-rdy*w2;\n this.r2z=this.r2.z-rdz*w2;\n\n this.t1x=this.r1y*this.az-this.r1z*this.ay;\n this.t1y=this.r1z*this.ax-this.r1x*this.az;\n this.t1z=this.r1x*this.ay-this.r1y*this.ax;\n this.t2x=this.r2y*this.az-this.r2z*this.ay;\n this.t2y=this.r2z*this.ax-this.r2x*this.az;\n this.t2z=this.r2x*this.ay-this.r2y*this.ax;\n this.l1x=this.ax*this.m1;\n this.l1y=this.ay*this.m1;\n this.l1z=this.az*this.m1;\n this.l2x=this.ax*this.m2;\n this.l2y=this.ay*this.m2;\n this.l2z=this.az*this.m2;\n this.a1x=this.t1x*this.i1e00+this.t1y*this.i1e01+this.t1z*this.i1e02;\n this.a1y=this.t1x*this.i1e10+this.t1y*this.i1e11+this.t1z*this.i1e12;\n this.a1z=this.t1x*this.i1e20+this.t1y*this.i1e21+this.t1z*this.i1e22;\n this.a2x=this.t2x*this.i2e00+this.t2y*this.i2e01+this.t2z*this.i2e02;\n this.a2y=this.t2x*this.i2e10+this.t2y*this.i2e11+this.t2z*this.i2e12;\n this.a2z=this.t2x*this.i2e20+this.t2y*this.i2e21+this.t2z*this.i2e22;\n this.motorDenom=\n this.m1+this.m2+\n this.ax*(this.a1y*this.r1z-this.a1z*this.r1y+this.a2y*this.r2z-this.a2z*this.r2y)+\n this.ay*(this.a1z*this.r1x-this.a1x*this.r1z+this.a2z*this.r2x-this.a2x*this.r2z)+\n this.az*(this.a1x*this.r1y-this.a1y*this.r1x+this.a2x*this.r2y-this.a2y*this.r2x);\n\n this.invMotorDenom=1/this.motorDenom;\n\n if(enableSpring&&this.limitState!=2){\n var omega=6.2831853*frequency;\n var k=omega*omega*timeStep;\n var dmp=invTimeStep/(k+2*this.limitMotor.dampingRatio*omega);\n this.cfm=this.motorDenom*dmp;\n this.limitVelocity*=k*dmp;\n }else{\n this.cfm=0;\n this.limitVelocity*=invTimeStep*0.05;\n }\n\n this.invDenom=1/(this.motorDenom+this.cfm);\n\n var totalImpulse=this.limitImpulse+this.motorImpulse;\n this.l1.x+=totalImpulse*this.l1x;\n this.l1.y+=totalImpulse*this.l1y;\n this.l1.z+=totalImpulse*this.l1z;\n this.a1.x+=totalImpulse*this.a1x;\n this.a1.y+=totalImpulse*this.a1y;\n this.a1.z+=totalImpulse*this.a1z;\n this.l2.x-=totalImpulse*this.l2x;\n this.l2.y-=totalImpulse*this.l2y;\n this.l2.z-=totalImpulse*this.l2z;\n this.a2.x-=totalImpulse*this.a2x;\n this.a2.y-=totalImpulse*this.a2y;\n this.a2.z-=totalImpulse*this.a2z;\n },\n solve:function(){\n var rvn=\n this.ax*(this.l2.x-this.l1.x)+this.ay*(this.l2.y-this.l1.y)+this.az*(this.l2.z-this.l1.z)+\n 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;\n\n // motor part\n var newMotorImpulse;\n if(this.enableMotor){\n newMotorImpulse=(rvn-this.motorSpeed)*this.invMotorDenom;\n var oldMotorImpulse=this.motorImpulse;\n this.motorImpulse+=newMotorImpulse;\n if(this.motorImpulse>this.maxMotorImpulse)this.motorImpulse=this.maxMotorImpulse;\n else if(this.motorImpulse<-this.maxMotorImpulse)this.motorImpulse=-this.maxMotorImpulse;\n newMotorImpulse=this.motorImpulse-oldMotorImpulse;\n rvn-=newMotorImpulse*this.motorDenom;\n }else newMotorImpulse=0;\n\n // limit part\n var newLimitImpulse;\n if(this.limitState!=2){\n newLimitImpulse=(rvn-this.limitVelocity-this.limitImpulse*this.cfm)*this.invDenom;\n var oldLimitImpulse=this.limitImpulse;\n this.limitImpulse+=newLimitImpulse;\n if(this.limitImpulse*this.limitState<0)this.limitImpulse=0;\n newLimitImpulse=this.limitImpulse-oldLimitImpulse;\n }else newLimitImpulse=0;\n \n var totalImpulse=newLimitImpulse+newMotorImpulse;\n this.l1.x+=totalImpulse*this.l1x;\n this.l1.y+=totalImpulse*this.l1y;\n this.l1.z+=totalImpulse*this.l1z;\n this.a1.x+=totalImpulse*this.a1x;\n this.a1.y+=totalImpulse*this.a1y;\n this.a1.z+=totalImpulse*this.a1z;\n this.l2.x-=totalImpulse*this.l2x;\n this.l2.y-=totalImpulse*this.l2y;\n this.l2.z-=totalImpulse*this.l2z;\n this.a2.x-=totalImpulse*this.a2x;\n this.a2.y-=totalImpulse*this.a2y;\n this.a2.z-=totalImpulse*this.a2z;\n }\n} );\n\n/**\n * A distance joint limits the distance between two anchor points on rigid bodies.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction DistanceJoint ( config, minDistance, maxDistance ){\n\n Joint.call( this, config );\n\n this.type = JOINT_DISTANCE;\n \n this.nor = new Vec3();\n\n // The limit and motor information of the joint.\n this.limitMotor = new LimitMotor( this.nor, true );\n this.limitMotor.lowerLimit = minDistance;\n this.limitMotor.upperLimit = maxDistance;\n\n this.t = new TranslationalConstraint( this, this.limitMotor );\n\n}\n\nDistanceJoint.prototype = Object.assign( Object.create( Joint.prototype ), {\n\n constructor: DistanceJoint,\n\n preSolve: function ( timeStep, invTimeStep ) {\n\n this.updateAnchorPoints();\n\n this.nor.sub( this.anchorPoint2, this.anchorPoint1 ).normalize();\n\n // preSolve\n\n this.t.preSolve( timeStep, invTimeStep );\n\n },\n\n solve: function () {\n\n this.t.solve();\n\n },\n\n postSolve: function () {\n\n }\n\n});\n\n/**\n* An angular constraint for all axes for various joints.\n* @author saharan\n*/\n\nfunction AngularConstraint( joint, targetOrientation ) {\n\n this.joint = joint;\n\n this.targetOrientation = new Quat().invert( targetOrientation );\n\n this.relativeOrientation = new Quat();\n\n this.ii1 = null;\n this.ii2 = null;\n this.dd = null;\n\n this.vel = new Vec3();\n this.imp = new Vec3();\n\n this.rn0 = new Vec3();\n this.rn1 = new Vec3();\n this.rn2 = new Vec3();\n\n this.b1 = joint.body1;\n this.b2 = joint.body2;\n this.a1 = this.b1.angularVelocity;\n this.a2 = this.b2.angularVelocity;\n this.i1 = this.b1.inverseInertia;\n this.i2 = this.b2.inverseInertia;\n\n}\n\nObject.assign( AngularConstraint.prototype, {\n\n AngularConstraint: true,\n\n preSolve: function ( timeStep, invTimeStep ) {\n\n var inv, len, v;\n\n this.ii1 = this.i1.clone();\n this.ii2 = this.i2.clone();\n\n v = new Mat33().add(this.ii1, this.ii2).elements;\n 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]) );\n this.dd = new Mat33().set(\n v[4]*v[8]-v[5]*v[7], v[2]*v[7]-v[1]*v[8], v[1]*v[5]-v[2]*v[4],\n v[5]*v[6]-v[3]*v[8], v[0]*v[8]-v[2]*v[6], v[2]*v[3]-v[0]*v[5],\n v[3]*v[7]-v[4]*v[6], v[1]*v[6]-v[0]*v[7], v[0]*v[4]-v[1]*v[3]\n ).multiplyScalar( inv );\n \n this.relativeOrientation.invert( this.b1.orientation ).multiply( this.targetOrientation ).multiply( this.b2.orientation );\n\n inv = this.relativeOrientation.w*2;\n\n this.vel.copy( this.relativeOrientation ).multiplyScalar( inv );\n\n len = this.vel.length();\n\n if( len > 0.02 ) {\n len = (0.02-len)/len*invTimeStep*0.05;\n this.vel.multiplyScalar( len );\n }else{\n this.vel.set(0,0,0);\n }\n\n this.rn1.copy( this.imp ).applyMatrix3( this.ii1, true );\n this.rn2.copy( this.imp ).applyMatrix3( this.ii2, true );\n\n this.a1.add( this.rn1 );\n this.a2.sub( this.rn2 );\n\n },\n\n solve: function () {\n\n var r = this.a2.clone().sub( this.a1 ).sub( this.vel );\n\n this.rn0.copy( r ).applyMatrix3( this.dd, true );\n this.rn1.copy( this.rn0 ).applyMatrix3( this.ii1, true );\n this.rn2.copy( this.rn0 ).applyMatrix3( this.ii2, true );\n\n this.imp.add( this.rn0 );\n this.a1.add( this.rn1 );\n this.a2.sub( this.rn2 );\n\n }\n\n} );\n\n/**\n* A three-axis translational constraint for various joints.\n* @author saharan\n*/\nfunction Translational3Constraint (joint,limitMotor1,limitMotor2,limitMotor3){\n\n this.m1=NaN;\n this.m2=NaN;\n this.i1e00=NaN;\n this.i1e01=NaN;\n this.i1e02=NaN;\n this.i1e10=NaN;\n this.i1e11=NaN;\n this.i1e12=NaN;\n this.i1e20=NaN;\n this.i1e21=NaN;\n this.i1e22=NaN;\n this.i2e00=NaN;\n this.i2e01=NaN;\n this.i2e02=NaN;\n this.i2e10=NaN;\n this.i2e11=NaN;\n this.i2e12=NaN;\n this.i2e20=NaN;\n this.i2e21=NaN;\n this.i2e22=NaN;\n this.ax1=NaN;\n this.ay1=NaN;\n this.az1=NaN;\n this.ax2=NaN;\n this.ay2=NaN;\n this.az2=NaN;\n this.ax3=NaN;\n this.ay3=NaN;\n this.az3=NaN;\n this.r1x=NaN;\n this.r1y=NaN;\n this.r1z=NaN;\n this.r2x=NaN;\n this.r2y=NaN;\n this.r2z=NaN;\n this.t1x1=NaN;// jacobians\n this.t1y1=NaN;\n this.t1z1=NaN;\n this.t2x1=NaN;\n this.t2y1=NaN;\n this.t2z1=NaN;\n this.l1x1=NaN;\n this.l1y1=NaN;\n this.l1z1=NaN;\n this.l2x1=NaN;\n this.l2y1=NaN;\n this.l2z1=NaN;\n this.a1x1=NaN;\n this.a1y1=NaN;\n this.a1z1=NaN;\n this.a2x1=NaN;\n this.a2y1=NaN;\n this.a2z1=NaN;\n this.t1x2=NaN;\n this.t1y2=NaN;\n this.t1z2=NaN;\n this.t2x2=NaN;\n this.t2y2=NaN;\n this.t2z2=NaN;\n this.l1x2=NaN;\n this.l1y2=NaN;\n this.l1z2=NaN;\n this.l2x2=NaN;\n this.l2y2=NaN;\n this.l2z2=NaN;\n this.a1x2=NaN;\n this.a1y2=NaN;\n this.a1z2=NaN;\n this.a2x2=NaN;\n this.a2y2=NaN;\n this.a2z2=NaN;\n this.t1x3=NaN;\n this.t1y3=NaN;\n this.t1z3=NaN;\n this.t2x3=NaN;\n this.t2y3=NaN;\n this.t2z3=NaN;\n this.l1x3=NaN;\n this.l1y3=NaN;\n this.l1z3=NaN;\n this.l2x3=NaN;\n this.l2y3=NaN;\n this.l2z3=NaN;\n this.a1x3=NaN;\n this.a1y3=NaN;\n this.a1z3=NaN;\n this.a2x3=NaN;\n this.a2y3=NaN;\n this.a2z3=NaN;\n this.lowerLimit1=NaN;\n this.upperLimit1=NaN;\n this.limitVelocity1=NaN;\n this.limitState1=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited\n this.enableMotor1=false;\n this.motorSpeed1=NaN;\n this.maxMotorForce1=NaN;\n this.maxMotorImpulse1=NaN;\n this.lowerLimit2=NaN;\n this.upperLimit2=NaN;\n this.limitVelocity2=NaN;\n this.limitState2=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited\n this.enableMotor2=false;\n this.motorSpeed2=NaN;\n this.maxMotorForce2=NaN;\n this.maxMotorImpulse2=NaN;\n this.lowerLimit3=NaN;\n this.upperLimit3=NaN;\n this.limitVelocity3=NaN;\n this.limitState3=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited\n this.enableMotor3=false;\n this.motorSpeed3=NaN;\n this.maxMotorForce3=NaN;\n this.maxMotorImpulse3=NaN;\n this.k00=NaN; // K = J*M*JT\n this.k01=NaN;\n this.k02=NaN;\n this.k10=NaN;\n this.k11=NaN;\n this.k12=NaN;\n this.k20=NaN;\n this.k21=NaN;\n this.k22=NaN;\n this.kv00=NaN; // diagonals without CFMs\n this.kv11=NaN;\n this.kv22=NaN;\n this.dv00=NaN; // ...inverted\n this.dv11=NaN;\n this.dv22=NaN;\n this.d00=NaN; // K^-1\n this.d01=NaN;\n this.d02=NaN;\n this.d10=NaN;\n this.d11=NaN;\n this.d12=NaN;\n this.d20=NaN;\n this.d21=NaN;\n this.d22=NaN;\n\n this.limitMotor1=limitMotor1;\n this.limitMotor2=limitMotor2;\n this.limitMotor3=limitMotor3;\n this.b1=joint.body1;\n this.b2=joint.body2;\n this.p1=joint.anchorPoint1;\n this.p2=joint.anchorPoint2;\n this.r1=joint.relativeAnchorPoint1;\n this.r2=joint.relativeAnchorPoint2;\n this.l1=this.b1.linearVelocity;\n this.l2=this.b2.linearVelocity;\n this.a1=this.b1.angularVelocity;\n this.a2=this.b2.angularVelocity;\n this.i1=this.b1.inverseInertia;\n this.i2=this.b2.inverseInertia;\n this.limitImpulse1=0;\n this.motorImpulse1=0;\n this.limitImpulse2=0;\n this.motorImpulse2=0;\n this.limitImpulse3=0;\n this.motorImpulse3=0;\n this.cfm1=0;// Constraint Force Mixing\n this.cfm2=0;\n this.cfm3=0;\n this.weight=-1;\n}\n\nObject.assign( Translational3Constraint.prototype, {\n\n Translational3Constraint: true,\n\n preSolve:function(timeStep,invTimeStep){\n this.ax1=this.limitMotor1.axis.x;\n this.ay1=this.limitMotor1.axis.y;\n this.az1=this.limitMotor1.axis.z;\n this.ax2=this.limitMotor2.axis.x;\n this.ay2=this.limitMotor2.axis.y;\n this.az2=this.limitMotor2.axis.z;\n this.ax3=this.limitMotor3.axis.x;\n this.ay3=this.limitMotor3.axis.y;\n this.az3=this.limitMotor3.axis.z;\n this.lowerLimit1=this.limitMotor1.lowerLimit;\n this.upperLimit1=this.limitMotor1.upperLimit;\n this.motorSpeed1=this.limitMotor1.motorSpeed;\n this.maxMotorForce1=this.limitMotor1.maxMotorForce;\n this.enableMotor1=this.maxMotorForce1>0;\n this.lowerLimit2=this.limitMotor2.lowerLimit;\n this.upperLimit2=this.limitMotor2.upperLimit;\n this.motorSpeed2=this.limitMotor2.motorSpeed;\n this.maxMotorForce2=this.limitMotor2.maxMotorForce;\n this.enableMotor2=this.maxMotorForce2>0;\n this.lowerLimit3=this.limitMotor3.lowerLimit;\n this.upperLimit3=this.limitMotor3.upperLimit;\n this.motorSpeed3=this.limitMotor3.motorSpeed;\n this.maxMotorForce3=this.limitMotor3.maxMotorForce;\n this.enableMotor3=this.maxMotorForce3>0;\n this.m1=this.b1.inverseMass;\n this.m2=this.b2.inverseMass;\n\n var ti1 = this.i1.elements;\n var ti2 = this.i2.elements;\n this.i1e00=ti1[0];\n this.i1e01=ti1[1];\n this.i1e02=ti1[2];\n this.i1e10=ti1[3];\n this.i1e11=ti1[4];\n this.i1e12=ti1[5];\n this.i1e20=ti1[6];\n this.i1e21=ti1[7];\n this.i1e22=ti1[8];\n\n this.i2e00=ti2[0];\n this.i2e01=ti2[1];\n this.i2e02=ti2[2];\n this.i2e10=ti2[3];\n this.i2e11=ti2[4];\n this.i2e12=ti2[5];\n this.i2e20=ti2[6];\n this.i2e21=ti2[7];\n this.i2e22=ti2[8];\n\n var dx=this.p2.x-this.p1.x;\n var dy=this.p2.y-this.p1.y;\n var dz=this.p2.z-this.p1.z;\n var d1=dx*this.ax1+dy*this.ay1+dz*this.az1;\n var d2=dx*this.ax2+dy*this.ay2+dz*this.az2;\n var d3=dx*this.ax3+dy*this.ay3+dz*this.az3;\n var frequency1=this.limitMotor1.frequency;\n var frequency2=this.limitMotor2.frequency;\n var frequency3=this.limitMotor3.frequency;\n var enableSpring1=frequency1>0;\n var enableSpring2=frequency2>0;\n var enableSpring3=frequency3>0;\n var enableLimit1=this.lowerLimit1<=this.upperLimit1;\n var enableLimit2=this.lowerLimit2<=this.upperLimit2;\n var enableLimit3=this.lowerLimit3<=this.upperLimit3;\n\n // for stability\n if(enableSpring1&&d1>20||d1<-20){\n enableSpring1=false;\n }\n if(enableSpring2&&d2>20||d2<-20){\n enableSpring2=false;\n }\n if(enableSpring3&&d3>20||d3<-20){\n enableSpring3=false;\n }\n\n if(enableLimit1){\n if(this.lowerLimit1==this.upperLimit1){\n if(this.limitState1!=0){\n this.limitState1=0;\n this.limitImpulse1=0;\n }\n this.limitVelocity1=this.lowerLimit1-d1;\n if(!enableSpring1)d1=this.lowerLimit1;\n }else if(d1this.upperLimit1){\n if(this.limitState1!=1){\n this.limitState1=1;\n this.limitImpulse1=0;\n }\n this.limitVelocity1=this.upperLimit1-d1;\n if(!enableSpring1)d1=this.upperLimit1;\n }else{\n this.limitState1=2;\n this.limitImpulse1=0;\n this.limitVelocity1=0;\n }\n if(!enableSpring1){\n if(this.limitVelocity1>0.005)this.limitVelocity1-=0.005;\n else if(this.limitVelocity1<-0.005)this.limitVelocity1+=0.005;\n else this.limitVelocity1=0;\n }\n }else{\n this.limitState1=2;\n this.limitImpulse1=0;\n }\n\n if(enableLimit2){\n if(this.lowerLimit2==this.upperLimit2){\n if(this.limitState2!=0){\n this.limitState2=0;\n this.limitImpulse2=0;\n }\n this.limitVelocity2=this.lowerLimit2-d2;\n if(!enableSpring2)d2=this.lowerLimit2;\n }else if(d2this.upperLimit2){\n if(this.limitState2!=1){\n this.limitState2=1;\n this.limitImpulse2=0;\n }\n this.limitVelocity2=this.upperLimit2-d2;\n if(!enableSpring2)d2=this.upperLimit2;\n }else{\n this.limitState2=2;\n this.limitImpulse2=0;\n this.limitVelocity2=0;\n }\n if(!enableSpring2){\n if(this.limitVelocity2>0.005)this.limitVelocity2-=0.005;\n else if(this.limitVelocity2<-0.005)this.limitVelocity2+=0.005;\n else this.limitVelocity2=0;\n }\n }else{\n this.limitState2=2;\n this.limitImpulse2=0;\n }\n\n if(enableLimit3){\n if(this.lowerLimit3==this.upperLimit3){\n if(this.limitState3!=0){\n this.limitState3=0;\n this.limitImpulse3=0;\n }\n this.limitVelocity3=this.lowerLimit3-d3;\n if(!enableSpring3)d3=this.lowerLimit3;\n }else if(d3this.upperLimit3){\n if(this.limitState3!=1){\n this.limitState3=1;\n this.limitImpulse3=0;\n }\n this.limitVelocity3=this.upperLimit3-d3;\n if(!enableSpring3)d3=this.upperLimit3;\n }else{\n this.limitState3=2;\n this.limitImpulse3=0;\n this.limitVelocity3=0;\n }\n if(!enableSpring3){\n if(this.limitVelocity3>0.005)this.limitVelocity3-=0.005;\n else if(this.limitVelocity3<-0.005)this.limitVelocity3+=0.005;\n else this.limitVelocity3=0;\n }\n }else{\n this.limitState3=2;\n this.limitImpulse3=0;\n }\n\n if(this.enableMotor1&&(this.limitState1!=0||enableSpring1)){\n this.maxMotorImpulse1=this.maxMotorForce1*timeStep;\n }else{\n this.motorImpulse1=0;\n this.maxMotorImpulse1=0;\n }\n\n if(this.enableMotor2&&(this.limitState2!=0||enableSpring2)){\n this.maxMotorImpulse2=this.maxMotorForce2*timeStep;\n }else{\n this.motorImpulse2=0;\n this.maxMotorImpulse2=0;\n }\n\n if(this.enableMotor3&&(this.limitState3!=0||enableSpring3)){\n this.maxMotorImpulse3=this.maxMotorForce3*timeStep;\n }else{\n this.motorImpulse3=0;\n this.maxMotorImpulse3=0;\n }\n \n var rdx=d1*this.ax1+d2*this.ax2+d3*this.ax2;\n var rdy=d1*this.ay1+d2*this.ay2+d3*this.ay2;\n var rdz=d1*this.az1+d2*this.az2+d3*this.az2;\n var w1=this.m2/(this.m1+this.m2);\n if(this.weight>=0)w1=this.weight; // use given weight\n var w2=1-w1;\n this.r1x=this.r1.x+rdx*w1;\n this.r1y=this.r1.y+rdy*w1;\n this.r1z=this.r1.z+rdz*w1;\n this.r2x=this.r2.x-rdx*w2;\n this.r2y=this.r2.y-rdy*w2;\n this.r2z=this.r2.z-rdz*w2;\n\n // build jacobians\n this.t1x1=this.r1y*this.az1-this.r1z*this.ay1;\n this.t1y1=this.r1z*this.ax1-this.r1x*this.az1;\n this.t1z1=this.r1x*this.ay1-this.r1y*this.ax1;\n this.t2x1=this.r2y*this.az1-this.r2z*this.ay1;\n this.t2y1=this.r2z*this.ax1-this.r2x*this.az1;\n this.t2z1=this.r2x*this.ay1-this.r2y*this.ax1;\n this.l1x1=this.ax1*this.m1;\n this.l1y1=this.ay1*this.m1;\n this.l1z1=this.az1*this.m1;\n this.l2x1=this.ax1*this.m2;\n this.l2y1=this.ay1*this.m2;\n this.l2z1=this.az1*this.m2;\n this.a1x1=this.t1x1*this.i1e00+this.t1y1*this.i1e01+this.t1z1*this.i1e02;\n this.a1y1=this.t1x1*this.i1e10+this.t1y1*this.i1e11+this.t1z1*this.i1e12;\n this.a1z1=this.t1x1*this.i1e20+this.t1y1*this.i1e21+this.t1z1*this.i1e22;\n this.a2x1=this.t2x1*this.i2e00+this.t2y1*this.i2e01+this.t2z1*this.i2e02;\n this.a2y1=this.t2x1*this.i2e10+this.t2y1*this.i2e11+this.t2z1*this.i2e12;\n this.a2z1=this.t2x1*this.i2e20+this.t2y1*this.i2e21+this.t2z1*this.i2e22;\n\n this.t1x2=this.r1y*this.az2-this.r1z*this.ay2;\n this.t1y2=this.r1z*this.ax2-this.r1x*this.az2;\n this.t1z2=this.r1x*this.ay2-this.r1y*this.ax2;\n this.t2x2=this.r2y*this.az2-this.r2z*this.ay2;\n this.t2y2=this.r2z*this.ax2-this.r2x*this.az2;\n this.t2z2=this.r2x*this.ay2-this.r2y*this.ax2;\n this.l1x2=this.ax2*this.m1;\n this.l1y2=this.ay2*this.m1;\n this.l1z2=this.az2*this.m1;\n this.l2x2=this.ax2*this.m2;\n this.l2y2=this.ay2*this.m2;\n this.l2z2=this.az2*this.m2;\n this.a1x2=this.t1x2*this.i1e00+this.t1y2*this.i1e01+this.t1z2*this.i1e02;\n this.a1y2=this.t1x2*this.i1e10+this.t1y2*this.i1e11+this.t1z2*this.i1e12;\n this.a1z2=this.t1x2*this.i1e20+this.t1y2*this.i1e21+this.t1z2*this.i1e22;\n this.a2x2=this.t2x2*this.i2e00+this.t2y2*this.i2e01+this.t2z2*this.i2e02;\n this.a2y2=this.t2x2*this.i2e10+this.t2y2*this.i2e11+this.t2z2*this.i2e12;\n this.a2z2=this.t2x2*this.i2e20+this.t2y2*this.i2e21+this.t2z2*this.i2e22;\n\n this.t1x3=this.r1y*this.az3-this.r1z*this.ay3;\n this.t1y3=this.r1z*this.ax3-this.r1x*this.az3;\n this.t1z3=this.r1x*this.ay3-this.r1y*this.ax3;\n this.t2x3=this.r2y*this.az3-this.r2z*this.ay3;\n this.t2y3=this.r2z*this.ax3-this.r2x*this.az3;\n this.t2z3=this.r2x*this.ay3-this.r2y*this.ax3;\n this.l1x3=this.ax3*this.m1;\n this.l1y3=this.ay3*this.m1;\n this.l1z3=this.az3*this.m1;\n this.l2x3=this.ax3*this.m2;\n this.l2y3=this.ay3*this.m2;\n this.l2z3=this.az3*this.m2;\n this.a1x3=this.t1x3*this.i1e00+this.t1y3*this.i1e01+this.t1z3*this.i1e02;\n this.a1y3=this.t1x3*this.i1e10+this.t1y3*this.i1e11+this.t1z3*this.i1e12;\n this.a1z3=this.t1x3*this.i1e20+this.t1y3*this.i1e21+this.t1z3*this.i1e22;\n this.a2x3=this.t2x3*this.i2e00+this.t2y3*this.i2e01+this.t2z3*this.i2e02;\n this.a2y3=this.t2x3*this.i2e10+this.t2y3*this.i2e11+this.t2z3*this.i2e12;\n this.a2z3=this.t2x3*this.i2e20+this.t2y3*this.i2e21+this.t2z3*this.i2e22;\n\n // build an impulse matrix\n var m12=this.m1+this.m2;\n this.k00=(this.ax1*this.ax1+this.ay1*this.ay1+this.az1*this.az1)*m12;\n this.k01=(this.ax1*this.ax2+this.ay1*this.ay2+this.az1*this.az2)*m12;\n this.k02=(this.ax1*this.ax3+this.ay1*this.ay3+this.az1*this.az3)*m12;\n this.k10=(this.ax2*this.ax1+this.ay2*this.ay1+this.az2*this.az1)*m12;\n this.k11=(this.ax2*this.ax2+this.ay2*this.ay2+this.az2*this.az2)*m12;\n this.k12=(this.ax2*this.ax3+this.ay2*this.ay3+this.az2*this.az3)*m12;\n this.k20=(this.ax3*this.ax1+this.ay3*this.ay1+this.az3*this.az1)*m12;\n this.k21=(this.ax3*this.ax2+this.ay3*this.ay2+this.az3*this.az2)*m12;\n this.k22=(this.ax3*this.ax3+this.ay3*this.ay3+this.az3*this.az3)*m12;\n\n this.k00+=this.t1x1*this.a1x1+this.t1y1*this.a1y1+this.t1z1*this.a1z1;\n this.k01+=this.t1x1*this.a1x2+this.t1y1*this.a1y2+this.t1z1*this.a1z2;\n this.k02+=this.t1x1*this.a1x3+this.t1y1*this.a1y3+this.t1z1*this.a1z3;\n this.k10+=this.t1x2*this.a1x1+this.t1y2*this.a1y1+this.t1z2*this.a1z1;\n this.k11+=this.t1x2*this.a1x2+this.t1y2*this.a1y2+this.t1z2*this.a1z2;\n this.k12+=this.t1x2*this.a1x3+this.t1y2*this.a1y3+this.t1z2*this.a1z3;\n this.k20+=this.t1x3*this.a1x1+this.t1y3*this.a1y1+this.t1z3*this.a1z1;\n this.k21+=this.t1x3*this.a1x2+this.t1y3*this.a1y2+this.t1z3*this.a1z2;\n this.k22+=this.t1x3*this.a1x3+this.t1y3*this.a1y3+this.t1z3*this.a1z3;\n\n this.k00+=this.t2x1*this.a2x1+this.t2y1*this.a2y1+this.t2z1*this.a2z1;\n this.k01+=this.t2x1*this.a2x2+this.t2y1*this.a2y2+this.t2z1*this.a2z2;\n this.k02+=this.t2x1*this.a2x3+this.t2y1*this.a2y3+this.t2z1*this.a2z3;\n this.k10+=this.t2x2*this.a2x1+this.t2y2*this.a2y1+this.t2z2*this.a2z1;\n this.k11+=this.t2x2*this.a2x2+this.t2y2*this.a2y2+this.t2z2*this.a2z2;\n this.k12+=this.t2x2*this.a2x3+this.t2y2*this.a2y3+this.t2z2*this.a2z3;\n this.k20+=this.t2x3*this.a2x1+this.t2y3*this.a2y1+this.t2z3*this.a2z1;\n this.k21+=this.t2x3*this.a2x2+this.t2y3*this.a2y2+this.t2z3*this.a2z2;\n this.k22+=this.t2x3*this.a2x3+this.t2y3*this.a2y3+this.t2z3*this.a2z3;\n\n this.kv00=this.k00;\n this.kv11=this.k11;\n this.kv22=this.k22;\n\n this.dv00=1/this.kv00;\n this.dv11=1/this.kv11;\n this.dv22=1/this.kv22;\n\n if(enableSpring1&&this.limitState1!=2){\n var omega=6.2831853*frequency1;\n var k=omega*omega*timeStep;\n var dmp=invTimeStep/(k+2*this.limitMotor1.dampingRatio*omega);\n this.cfm1=this.kv00*dmp;\n this.limitVelocity1*=k*dmp;\n }else{\n this.cfm1=0;\n this.limitVelocity1*=invTimeStep*0.05;\n }\n if(enableSpring2&&this.limitState2!=2){\n omega=6.2831853*frequency2;\n k=omega*omega*timeStep;\n dmp=invTimeStep/(k+2*this.limitMotor2.dampingRatio*omega);\n this.cfm2=this.kv11*dmp;\n this.limitVelocity2*=k*dmp;\n }else{\n this.cfm2=0;\n this.limitVelocity2*=invTimeStep*0.05;\n }\n if(enableSpring3&&this.limitState3!=2){\n omega=6.2831853*frequency3;\n k=omega*omega*timeStep;\n dmp=invTimeStep/(k+2*this.limitMotor3.dampingRatio*omega);\n this.cfm3=this.kv22*dmp;\n this.limitVelocity3*=k*dmp;\n }else{\n this.cfm3=0;\n this.limitVelocity3*=invTimeStep*0.05;\n }\n this.k00+=this.cfm1;\n this.k11+=this.cfm2;\n this.k22+=this.cfm3;\n\n var inv=1/(\n this.k00*(this.k11*this.k22-this.k21*this.k12)+\n this.k10*(this.k21*this.k02-this.k01*this.k22)+\n this.k20*(this.k01*this.k12-this.k11*this.k02)\n );\n this.d00=(this.k11*this.k22-this.k12*this.k21)*inv;\n this.d01=(this.k02*this.k21-this.k01*this.k22)*inv;\n this.d02=(this.k01*this.k12-this.k02*this.k11)*inv;\n this.d10=(this.k12*this.k20-this.k10*this.k22)*inv;\n this.d11=(this.k00*this.k22-this.k02*this.k20)*inv;\n this.d12=(this.k02*this.k10-this.k00*this.k12)*inv;\n this.d20=(this.k10*this.k21-this.k11*this.k20)*inv;\n this.d21=(this.k01*this.k20-this.k00*this.k21)*inv;\n this.d22=(this.k00*this.k11-this.k01*this.k10)*inv;\n\n // warm starting\n var totalImpulse1=this.limitImpulse1+this.motorImpulse1;\n var totalImpulse2=this.limitImpulse2+this.motorImpulse2;\n var totalImpulse3=this.limitImpulse3+this.motorImpulse3;\n this.l1.x+=totalImpulse1*this.l1x1+totalImpulse2*this.l1x2+totalImpulse3*this.l1x3;\n this.l1.y+=totalImpulse1*this.l1y1+totalImpulse2*this.l1y2+totalImpulse3*this.l1y3;\n this.l1.z+=totalImpulse1*this.l1z1+totalImpulse2*this.l1z2+totalImpulse3*this.l1z3;\n this.a1.x+=totalImpulse1*this.a1x1+totalImpulse2*this.a1x2+totalImpulse3*this.a1x3;\n this.a1.y+=totalImpulse1*this.a1y1+totalImpulse2*this.a1y2+totalImpulse3*this.a1y3;\n this.a1.z+=totalImpulse1*this.a1z1+totalImpulse2*this.a1z2+totalImpulse3*this.a1z3;\n this.l2.x-=totalImpulse1*this.l2x1+totalImpulse2*this.l2x2+totalImpulse3*this.l2x3;\n this.l2.y-=totalImpulse1*this.l2y1+totalImpulse2*this.l2y2+totalImpulse3*this.l2y3;\n this.l2.z-=totalImpulse1*this.l2z1+totalImpulse2*this.l2z2+totalImpulse3*this.l2z3;\n this.a2.x-=totalImpulse1*this.a2x1+totalImpulse2*this.a2x2+totalImpulse3*this.a2x3;\n this.a2.y-=totalImpulse1*this.a2y1+totalImpulse2*this.a2y2+totalImpulse3*this.a2y3;\n this.a2.z-=totalImpulse1*this.a2z1+totalImpulse2*this.a2z2+totalImpulse3*this.a2z3;\n },\n\n solve:function(){\n 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;\n 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;\n 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;\n var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1;\n var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2;\n var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3;\n var oldMotorImpulse1=this.motorImpulse1;\n var oldMotorImpulse2=this.motorImpulse2;\n var oldMotorImpulse3=this.motorImpulse3;\n var dMotorImpulse1=0;\n var dMotorImpulse2=0;\n var dMotorImpulse3=0;\n if(this.enableMotor1){\n dMotorImpulse1=(rvn1-this.motorSpeed1)*this.dv00;\n this.motorImpulse1+=dMotorImpulse1;\n if(this.motorImpulse1>this.maxMotorImpulse1){ // clamp motor impulse\n this.motorImpulse1=this.maxMotorImpulse1;\n }else if(this.motorImpulse1<-this.maxMotorImpulse1){\n this.motorImpulse1=-this.maxMotorImpulse1;\n }\n dMotorImpulse1=this.motorImpulse1-oldMotorImpulse1;\n }\n if(this.enableMotor2){\n dMotorImpulse2=(rvn2-this.motorSpeed2)*this.dv11;\n this.motorImpulse2+=dMotorImpulse2;\n if(this.motorImpulse2>this.maxMotorImpulse2){ // clamp motor impulse\n this.motorImpulse2=this.maxMotorImpulse2;\n }else if(this.motorImpulse2<-this.maxMotorImpulse2){\n this.motorImpulse2=-this.maxMotorImpulse2;\n }\n dMotorImpulse2=this.motorImpulse2-oldMotorImpulse2;\n }\n if(this.enableMotor3){\n dMotorImpulse3=(rvn3-this.motorSpeed3)*this.dv22;\n this.motorImpulse3+=dMotorImpulse3;\n if(this.motorImpulse3>this.maxMotorImpulse3){ // clamp motor impulse\n this.motorImpulse3=this.maxMotorImpulse3;\n }else if(this.motorImpulse3<-this.maxMotorImpulse3){\n this.motorImpulse3=-this.maxMotorImpulse3;\n }\n dMotorImpulse3=this.motorImpulse3-oldMotorImpulse3;\n }\n\n // apply motor impulse to relative velocity\n rvn1+=dMotorImpulse1*this.kv00+dMotorImpulse2*this.k01+dMotorImpulse3*this.k02;\n rvn2+=dMotorImpulse1*this.k10+dMotorImpulse2*this.kv11+dMotorImpulse3*this.k12;\n rvn3+=dMotorImpulse1*this.k20+dMotorImpulse2*this.k21+dMotorImpulse3*this.kv22;\n\n // subtract target velocity and applied impulse\n rvn1-=this.limitVelocity1+this.limitImpulse1*this.cfm1;\n rvn2-=this.limitVelocity2+this.limitImpulse2*this.cfm2;\n rvn3-=this.limitVelocity3+this.limitImpulse3*this.cfm3;\n\n var oldLimitImpulse1=this.limitImpulse1;\n var oldLimitImpulse2=this.limitImpulse2;\n var oldLimitImpulse3=this.limitImpulse3;\n\n var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02;\n var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12;\n var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22;\n\n this.limitImpulse1+=dLimitImpulse1;\n this.limitImpulse2+=dLimitImpulse2;\n this.limitImpulse3+=dLimitImpulse3;\n\n // clamp\n var clampState=0;\n if(this.limitState1==2||this.limitImpulse1*this.limitState1<0){\n dLimitImpulse1=-oldLimitImpulse1;\n rvn2+=dLimitImpulse1*this.k10;\n rvn3+=dLimitImpulse1*this.k20;\n clampState|=1;\n }\n if(this.limitState2==2||this.limitImpulse2*this.limitState2<0){\n dLimitImpulse2=-oldLimitImpulse2;\n rvn1+=dLimitImpulse2*this.k01;\n rvn3+=dLimitImpulse2*this.k21;\n clampState|=2;\n }\n if(this.limitState3==2||this.limitImpulse3*this.limitState3<0){\n dLimitImpulse3=-oldLimitImpulse3;\n rvn1+=dLimitImpulse3*this.k02;\n rvn2+=dLimitImpulse3*this.k12;\n clampState|=4;\n }\n\n // update un-clamped impulse\n // TODO: isolate division\n var det;\n switch(clampState){\n case 1:// update 2 3\n det=1/(this.k11*this.k22-this.k12*this.k21);\n dLimitImpulse2=(this.k22*rvn2+-this.k12*rvn3)*det;\n dLimitImpulse3=(-this.k21*rvn2+this.k11*rvn3)*det;\n break;\n case 2:// update 1 3\n det=1/(this.k00*this.k22-this.k02*this.k20);\n dLimitImpulse1=(this.k22*rvn1+-this.k02*rvn3)*det;\n dLimitImpulse3=(-this.k20*rvn1+this.k00*rvn3)*det;\n break;\n case 3:// update 3\n dLimitImpulse3=rvn3/this.k22;\n break;\n case 4:// update 1 2\n det=1/(this.k00*this.k11-this.k01*this.k10);\n dLimitImpulse1=(this.k11*rvn1+-this.k01*rvn2)*det;\n dLimitImpulse2=(-this.k10*rvn1+this.k00*rvn2)*det;\n break;\n case 5:// update 2\n dLimitImpulse2=rvn2/this.k11;\n break;\n case 6:// update 1\n dLimitImpulse1=rvn1/this.k00;\n break;\n }\n\n this.limitImpulse1=oldLimitImpulse1+dLimitImpulse1;\n this.limitImpulse2=oldLimitImpulse2+dLimitImpulse2;\n this.limitImpulse3=oldLimitImpulse3+dLimitImpulse3;\n\n var dImpulse1=dMotorImpulse1+dLimitImpulse1;\n var dImpulse2=dMotorImpulse2+dLimitImpulse2;\n var dImpulse3=dMotorImpulse3+dLimitImpulse3;\n\n // apply impulse\n this.l1.x+=dImpulse1*this.l1x1+dImpulse2*this.l1x2+dImpulse3*this.l1x3;\n this.l1.y+=dImpulse1*this.l1y1+dImpulse2*this.l1y2+dImpulse3*this.l1y3;\n this.l1.z+=dImpulse1*this.l1z1+dImpulse2*this.l1z2+dImpulse3*this.l1z3;\n this.a1.x+=dImpulse1*this.a1x1+dImpulse2*this.a1x2+dImpulse3*this.a1x3;\n this.a1.y+=dImpulse1*this.a1y1+dImpulse2*this.a1y2+dImpulse3*this.a1y3;\n this.a1.z+=dImpulse1*this.a1z1+dImpulse2*this.a1z2+dImpulse3*this.a1z3;\n this.l2.x-=dImpulse1*this.l2x1+dImpulse2*this.l2x2+dImpulse3*this.l2x3;\n this.l2.y-=dImpulse1*this.l2y1+dImpulse2*this.l2y2+dImpulse3*this.l2y3;\n this.l2.z-=dImpulse1*this.l2z1+dImpulse2*this.l2z2+dImpulse3*this.l2z3;\n this.a2.x-=dImpulse1*this.a2x1+dImpulse2*this.a2x2+dImpulse3*this.a2x3;\n this.a2.y-=dImpulse1*this.a2y1+dImpulse2*this.a2y2+dImpulse3*this.a2y3;\n this.a2.z-=dImpulse1*this.a2z1+dImpulse2*this.a2z2+dImpulse3*this.a2z3;\n }\n \n} );\n\n/**\n * A prismatic joint allows only for relative translation of rigid bodies along the axis.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction PrismaticJoint( config, lowerTranslation, upperTranslation ){\n\n Joint.call( this, config );\n\n this.type = JOINT_PRISMATIC;\n\n // The axis in the first body's coordinate system.\n this.localAxis1 = config.localAxis1.clone().normalize();\n // The axis in the second body's coordinate system.\n this.localAxis2 = config.localAxis2.clone().normalize();\n\n this.ax1 = new Vec3();\n this.ax2 = new Vec3();\n \n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n this.ac = new AngularConstraint( this, new Quat().setFromUnitVectors( this.localAxis1, this.localAxis2 ) );\n\n // The translational limit and motor information of the joint.\n this.limitMotor = new LimitMotor( this.nor, true );\n this.limitMotor.lowerLimit = lowerTranslation;\n this.limitMotor.upperLimit = upperTranslation;\n this.t3 = new Translational3Constraint( this, this.limitMotor, new LimitMotor( this.tan, true ), new LimitMotor( this.bin, true ) );\n\n}\n\nPrismaticJoint.prototype = Object.assign( Object.create( Joint.prototype ), {\n\n constructor: PrismaticJoint,\n\n preSolve: function ( timeStep, invTimeStep ) {\n\n this.updateAnchorPoints();\n\n this.ax1.copy( this.localAxis1 ).applyMatrix3( this.body1.rotation, true );\n this.ax2.copy( this.localAxis2 ).applyMatrix3( this.body2.rotation, true );\n\n // normal tangent binormal\n\n this.nor.set(\n this.ax1.x*this.body2.inverseMass + this.ax2.x*this.body1.inverseMass,\n this.ax1.y*this.body2.inverseMass + this.ax2.y*this.body1.inverseMass,\n this.ax1.z*this.body2.inverseMass + this.ax2.z*this.body1.inverseMass\n ).normalize();\n this.tan.tangent( this.nor ).normalize();\n this.bin.crossVectors( this.nor, this.tan );\n\n // preSolve\n\n this.ac.preSolve( timeStep, invTimeStep );\n this.t3.preSolve( timeStep, invTimeStep );\n\n },\n\n solve: function () {\n\n this.ac.solve();\n this.t3.solve();\n \n },\n\n postSolve: function () {\n\n }\n\n});\n\n/**\n * A slider joint allows for relative translation and relative rotation between two rigid bodies along the axis.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction SliderJoint( config, lowerTranslation, upperTranslation ){\n\n Joint.call( this, config );\n\n this.type = JOINT_SLIDER;\n\n // The axis in the first body's coordinate system.\n this.localAxis1 = config.localAxis1.clone().normalize();\n // The axis in the second body's coordinate system.\n this.localAxis2 = config.localAxis2.clone().normalize();\n\n // make angle axis\n var arc = new Mat33().setQuat( new Quat().setFromUnitVectors( this.localAxis1, this.localAxis2 ) );\n this.localAngle1 = new Vec3().tangent( this.localAxis1 ).normalize();\n this.localAngle2 = this.localAngle1.clone().applyMatrix3( arc, true );\n\n this.ax1 = new Vec3();\n this.ax2 = new Vec3();\n this.an1 = new Vec3();\n this.an2 = new Vec3();\n\n this.tmp = new Vec3();\n \n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n // The limit and motor for the rotation\n this.rotationalLimitMotor = new LimitMotor( this.nor, false );\n this.r3 = new Rotational3Constraint( this, this.rotationalLimitMotor, new LimitMotor( this.tan, true ), new LimitMotor( this.bin, true ) );\n\n // The limit and motor for the translation.\n this.translationalLimitMotor = new LimitMotor( this.nor, true );\n this.translationalLimitMotor.lowerLimit = lowerTranslation;\n this.translationalLimitMotor.upperLimit = upperTranslation;\n this.t3 = new Translational3Constraint( this, this.translationalLimitMotor, new LimitMotor( this.tan, true ), new LimitMotor( this.bin, true ) );\n\n}\n\nSliderJoint.prototype = Object.assign( Object.create( Joint.prototype ), {\n\n constructor: SliderJoint,\n\n preSolve: function ( timeStep, invTimeStep ) {\n\n this.updateAnchorPoints();\n\n this.ax1.copy( this.localAxis1 ).applyMatrix3( this.body1.rotation, true );\n this.an1.copy( this.localAngle1 ).applyMatrix3( this.body1.rotation, true );\n\n this.ax2.copy( this.localAxis2 ).applyMatrix3( this.body2.rotation, true );\n this.an2.copy( this.localAngle2 ).applyMatrix3( this.body2.rotation, true );\n\n // normal tangent binormal\n\n this.nor.set(\n this.ax1.x*this.body2.inverseMass + this.ax2.x*this.body1.inverseMass,\n this.ax1.y*this.body2.inverseMass + this.ax2.y*this.body1.inverseMass,\n this.ax1.z*this.body2.inverseMass + this.ax2.z*this.body1.inverseMass\n ).normalize();\n this.tan.tangent( this.nor ).normalize();\n this.bin.crossVectors( this.nor, this.tan );\n\n // calculate hinge angle\n\n this.tmp.crossVectors( this.an1, this.an2 );\n\n var limite = _Math.acosClamp( _Math.dotVectors( this.an1, this.an2 ) );\n\n if( _Math.dotVectors( this.nor, this.tmp ) < 0 ) this.rotationalLimitMotor.angle = -limite;\n else this.rotationalLimitMotor.angle = limite;\n\n // angular error\n\n this.tmp.crossVectors( this.ax1, this.ax2 );\n this.r3.limitMotor2.angle = _Math.dotVectors( this.tan, this.tmp );\n this.r3.limitMotor3.angle = _Math.dotVectors( this.bin, this.tmp );\n\n // preSolve\n \n this.r3.preSolve( timeStep, invTimeStep );\n this.t3.preSolve( timeStep, invTimeStep );\n\n },\n\n solve: function () {\n\n this.r3.solve();\n this.t3.solve();\n\n },\n\n postSolve: function () {\n\n }\n\n});\n\n/**\n * A wheel joint allows for relative rotation between two rigid bodies along two axes.\n * The wheel joint also allows for relative translation for the suspension.\n *\n * @author saharan\n * @author lo-th\n */\n\nfunction WheelJoint ( config ){\n\n Joint.call( this, config );\n\n this.type = JOINT_WHEEL;\n\n // The axis in the first body's coordinate system.\n this.localAxis1 = config.localAxis1.clone().normalize();\n // The axis in the second body's coordinate system.\n this.localAxis2 = config.localAxis2.clone().normalize();\n\n this.localAngle1 = new Vec3();\n this.localAngle2 = new Vec3();\n\n var dot = _Math.dotVectors( this.localAxis1, this.localAxis2 );\n\n if( dot > -1 && dot < 1 ){\n\n this.localAngle1.set(\n this.localAxis2.x - dot*this.localAxis1.x,\n this.localAxis2.y - dot*this.localAxis1.y,\n this.localAxis2.z - dot*this.localAxis1.z\n ).normalize();\n\n this.localAngle2.set(\n this.localAxis1.x - dot*this.localAxis2.x,\n this.localAxis1.y - dot*this.localAxis2.y,\n this.localAxis1.z - dot*this.localAxis2.z\n ).normalize();\n\n } else {\n\n var arc = new Mat33().setQuat( new Quat().setFromUnitVectors( this.localAxis1, this.localAxis2 ) );\n this.localAngle1.tangent( this.localAxis1 ).normalize();\n this.localAngle2 = this.localAngle1.clone().applyMatrix3( arc, true );\n\n }\n\n this.ax1 = new Vec3();\n this.ax2 = new Vec3();\n this.an1 = new Vec3();\n this.an2 = new Vec3();\n\n this.tmp = new Vec3();\n\n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n // The translational limit and motor information of the joint.\n this.translationalLimitMotor = new LimitMotor( this.tan,true );\n this.translationalLimitMotor.frequency = 8;\n this.translationalLimitMotor.dampingRatio = 1;\n // The first rotational limit and motor information of the joint.\n this.rotationalLimitMotor1 = new LimitMotor( this.tan, false );\n // The second rotational limit and motor information of the joint.\n this.rotationalLimitMotor2 = new LimitMotor( this.bin, false );\n\n this.t3 = new Translational3Constraint( this, new LimitMotor( this.nor, true ),this.translationalLimitMotor,new LimitMotor( this.bin, true ));\n this.t3.weight = 1;\n this.r3 = new Rotational3Constraint(this,new LimitMotor( this.nor, true ),this.rotationalLimitMotor1,this.rotationalLimitMotor2);\n\n}\n\nWheelJoint.prototype = Object.assign( Object.create( Joint.prototype ), {\n\n constructor: WheelJoint,\n\n preSolve: function ( timeStep, invTimeStep ) {\n\n this.updateAnchorPoints();\n\n this.ax1.copy( this.localAxis1 ).applyMatrix3( this.body1.rotation, true );\n this.an1.copy( this.localAngle1 ).applyMatrix3( this.body1.rotation, true );\n\n this.ax2.copy( this.localAxis2 ).applyMatrix3( this.body2.rotation, true );\n this.an2.copy( this.localAngle2 ).applyMatrix3( this.body2.rotation, true );\n\n this.r3.limitMotor1.angle = _Math.dotVectors( this.ax1, this.ax2 );\n\n var limite = _Math.dotVectors( this.an1, this.ax2 );\n\n if( _Math.dotVectors( this.ax1, this.tmp.crossVectors( this.an1, this.ax2 ) ) < 0 ) this.rotationalLimitMotor1.angle = -limite;\n else this.rotationalLimitMotor1.angle = limite;\n\n limite = _Math.dotVectors( this.an2, this.ax1 );\n\n if( _Math.dotVectors( this.ax2, this.tmp.crossVectors( this.an2, this.ax1 ) ) < 0 ) this.rotationalLimitMotor2.angle = -limite;\n else this.rotationalLimitMotor2.angle = limite;\n\n this.nor.crossVectors( this.ax1, this.ax2 ).normalize();\n this.tan.crossVectors( this.nor, this.ax2 ).normalize();\n this.bin.crossVectors( this.nor, this.ax1 ).normalize();\n \n this.r3.preSolve(timeStep,invTimeStep);\n this.t3.preSolve(timeStep,invTimeStep);\n\n },\n\n solve: function () {\n\n this.r3.solve();\n this.t3.solve();\n\n },\n\n postSolve: function () {\n\n }\n\n});\n\nfunction JointConfig(){\r\n\r\n this.scale = 1;\r\n this.invScale = 1;\r\n\r\n // The first rigid body of the joint.\r\n this.body1 = null;\r\n // The second rigid body of the joint.\r\n this.body2 = null;\r\n // The anchor point on the first rigid body in local coordinate system.\r\n this.localAnchorPoint1 = new Vec3();\r\n // The anchor point on the second rigid body in local coordinate system.\r\n this.localAnchorPoint2 = new Vec3();\r\n // The axis in the first body's coordinate system.\r\n // his property is available in some joints.\r\n this.localAxis1 = new Vec3();\r\n // The axis in the second body's coordinate system.\r\n // This property is available in some joints.\r\n this.localAxis2 = new Vec3();\r\n // Whether allow collision between connected rigid bodies or not.\r\n this.allowCollision = false;\r\n\r\n}\n\n/**\r\n * This class holds mass information of a shape.\r\n * @author lo-th\r\n * @author saharan\r\n */\r\n\r\nfunction MassInfo (){\r\n\r\n // Mass of the shape.\r\n this.mass = 0;\r\n\r\n // The moment inertia of the shape.\r\n this.inertia = new Mat33();\r\n\r\n}\n\n/**\n* A link list of contacts.\n* @author saharan\n*/\nfunction ContactLink ( contact ){\n \n\t// The previous contact link.\n this.prev = null;\n // The next contact link.\n this.next = null;\n // The shape of the contact.\n this.shape = null;\n // The other rigid body.\n this.body = null;\n // The contact of the link.\n this.contact = contact;\n\n}\n\nfunction ImpulseDataBuffer (){\n\n this.lp1X = NaN;\n this.lp1Y = NaN;\n this.lp1Z = NaN;\n this.lp2X = NaN;\n this.lp2Y = NaN;\n this.lp2Z = NaN;\n this.impulse = NaN;\n\n}\n\n/**\n* The class holds details of the contact point.\n* @author saharan\n*/\n\nfunction ManifoldPoint(){\n\n // Whether this manifold point is persisting or not.\n this.warmStarted = false;\n // The position of this manifold point.\n this.position = new Vec3();\n // The position in the first shape's coordinate.\n this.localPoint1 = new Vec3();\n // The position in the second shape's coordinate.\n this.localPoint2 = new Vec3();\n // The normal vector of this manifold point.\n this.normal = new Vec3();\n // The tangent vector of this manifold point.\n this.tangent = new Vec3();\n // The binormal vector of this manifold point.\n this.binormal = new Vec3();\n // The impulse in normal direction.\n this.normalImpulse = 0;\n // The impulse in tangent direction.\n this.tangentImpulse = 0;\n // The impulse in binormal direction.\n this.binormalImpulse = 0;\n // The denominator in normal direction.\n this.normalDenominator = 0;\n // The denominator in tangent direction.\n this.tangentDenominator = 0;\n // The denominator in binormal direction.\n this.binormalDenominator = 0;\n // The depth of penetration.\n this.penetration = 0;\n\n}\n\n/**\n* A contact manifold between two shapes.\n* @author saharan\n* @author lo-th\n*/\n\nfunction ContactManifold () {\n\n // The first rigid body.\n this.body1 = null;\n // The second rigid body.\n this.body2 = null;\n // The number of manifold points.\n this.numPoints = 0;\n // The manifold points.\n this.points = [\n new ManifoldPoint(),\n new ManifoldPoint(),\n new ManifoldPoint(),\n new ManifoldPoint()\n ];\n\n}\n\nContactManifold.prototype = {\n\n constructor: ContactManifold,\n\n //Reset the manifold.\n reset:function( shape1, shape2 ){\n\n this.body1 = shape1.parent;\n this.body2 = shape2.parent;\n this.numPoints = 0;\n\n },\n\n // Add a point into this manifold.\n addPointVec: function ( pos, norm, penetration, flip ) {\n \n var p = this.points[ this.numPoints++ ];\n\n p.position.copy( pos );\n p.localPoint1.sub( pos, this.body1.position ).applyMatrix3( this.body1.rotation );\n p.localPoint2.sub( pos, this.body2.position ).applyMatrix3( this.body2.rotation );\n\n p.normal.copy( norm );\n if( flip ) p.normal.negate();\n\n p.normalImpulse = 0;\n p.penetration = penetration;\n p.warmStarted = false;\n \n },\n\n // Add a point into this manifold.\n addPoint: function ( x, y, z, nx, ny, nz, penetration, flip ) {\n \n var p = this.points[ this.numPoints++ ];\n\n p.position.set( x, y, z );\n p.localPoint1.sub( p.position, this.body1.position ).applyMatrix3( this.body1.rotation );\n p.localPoint2.sub( p.position, this.body2.position ).applyMatrix3( this.body2.rotation );\n\n p.normalImpulse = 0;\n\n p.normal.set( nx, ny, nz );\n if( flip ) p.normal.negate();\n\n p.penetration = penetration;\n p.warmStarted = false;\n \n }\n};\n\nfunction ContactPointDataBuffer (){\n\n this.nor = new Vec3();\n this.tan = new Vec3();\n this.bin = new Vec3();\n\n this.norU1 = new Vec3();\n this.tanU1 = new Vec3();\n this.binU1 = new Vec3();\n\n this.norU2 = new Vec3();\n this.tanU2 = new Vec3();\n this.binU2 = new Vec3();\n\n this.norT1 = new Vec3();\n this.tanT1 = new Vec3();\n this.binT1 = new Vec3();\n\n this.norT2 = new Vec3();\n this.tanT2 = new Vec3();\n this.binT2 = new Vec3();\n\n this.norTU1 = new Vec3();\n this.tanTU1 = new Vec3();\n this.binTU1 = new Vec3();\n\n this.norTU2 = new Vec3();\n this.tanTU2 = new Vec3();\n this.binTU2 = new Vec3();\n\n this.norImp = 0;\n this.tanImp = 0;\n this.binImp = 0;\n\n this.norDen = 0;\n this.tanDen = 0;\n this.binDen = 0;\n\n this.norTar = 0;\n\n this.next = null;\n this.last = false;\n\n}\n\n/**\n* ...\n* @author saharan\n*/\nfunction ContactConstraint ( manifold ){\n \n Constraint.call( this );\n // The contact manifold of the constraint.\n this.manifold = manifold;\n // The coefficient of restitution of the constraint.\n this.restitution=NaN;\n // The coefficient of friction of the constraint.\n this.friction=NaN;\n this.p1=null;\n this.p2=null;\n this.lv1=null;\n this.lv2=null;\n this.av1=null;\n this.av2=null;\n this.i1=null;\n this.i2=null;\n\n //this.ii1 = null;\n //this.ii2 = null;\n\n this.tmp = new Vec3();\n this.tmpC1 = new Vec3();\n this.tmpC2 = new Vec3();\n\n this.tmpP1 = new Vec3();\n this.tmpP2 = new Vec3();\n\n this.tmplv1 = new Vec3();\n this.tmplv2 = new Vec3();\n this.tmpav1 = new Vec3();\n this.tmpav2 = new Vec3();\n\n this.m1=NaN;\n this.m2=NaN;\n this.num=0;\n \n this.ps = manifold.points;\n this.cs = new ContactPointDataBuffer();\n this.cs.next = new ContactPointDataBuffer();\n this.cs.next.next = new ContactPointDataBuffer();\n this.cs.next.next.next = new ContactPointDataBuffer();\n}\n\nContactConstraint.prototype = Object.assign( Object.create( Constraint.prototype ), {\n\n constructor: ContactConstraint,\n\n // Attach the constraint to the bodies.\n attach: function(){\n\n this.p1=this.body1.position;\n this.p2=this.body2.position;\n this.lv1=this.body1.linearVelocity;\n this.av1=this.body1.angularVelocity;\n this.lv2=this.body2.linearVelocity;\n this.av2=this.body2.angularVelocity;\n this.i1=this.body1.inverseInertia;\n this.i2=this.body2.inverseInertia;\n\n },\n\n // Detach the constraint from the bodies.\n detach: function(){\n\n this.p1=null;\n this.p2=null;\n this.lv1=null;\n this.lv2=null;\n this.av1=null;\n this.av2=null;\n this.i1=null;\n this.i2=null;\n\n },\n\n preSolve: function( timeStep, invTimeStep ){\n\n this.m1 = this.body1.inverseMass;\n this.m2 = this.body2.inverseMass;\n\n var m1m2 = this.m1 + this.m2;\n\n this.num = this.manifold.numPoints;\n\n var c = this.cs;\n var p, rvn, len, norImp, norTar, sepV, i1, i2;\n\n for( var i=0; i < this.num; i++ ){\n\n p = this.ps[i];\n\n this.tmpP1.sub( p.position, this.p1 );\n this.tmpP2.sub( p.position, this.p2 );\n\n this.tmpC1.crossVectors( this.av1, this.tmpP1 );\n this.tmpC2.crossVectors( this.av2, this.tmpP2 );\n\n c.norImp = p.normalImpulse;\n c.tanImp = p.tangentImpulse;\n c.binImp = p.binormalImpulse;\n\n c.nor.copy( p.normal );\n\n this.tmp.set(\n\n ( this.lv2.x + this.tmpC2.x ) - ( this.lv1.x + this.tmpC1.x ),\n ( this.lv2.y + this.tmpC2.y ) - ( this.lv1.y + this.tmpC1.y ),\n ( this.lv2.z + this.tmpC2.z ) - ( this.lv1.z + this.tmpC1.z )\n\n );\n\n rvn = _Math.dotVectors( c.nor, this.tmp );\n\n c.tan.set(\n this.tmp.x - rvn * c.nor.x,\n this.tmp.y - rvn * c.nor.y,\n this.tmp.z - rvn * c.nor.z\n );\n\n len = _Math.dotVectors( c.tan, c.tan );\n\n if( len <= 0.04 ) {\n c.tan.tangent( c.nor );\n }\n\n c.tan.normalize();\n\n c.bin.crossVectors( c.nor, c.tan );\n\n c.norU1.scale( c.nor, this.m1 );\n c.norU2.scale( c.nor, this.m2 );\n\n c.tanU1.scale( c.tan, this.m1 );\n c.tanU2.scale( c.tan, this.m2 );\n\n c.binU1.scale( c.bin, this.m1 );\n c.binU2.scale( c.bin, this.m2 );\n\n c.norT1.crossVectors( this.tmpP1, c.nor );\n c.tanT1.crossVectors( this.tmpP1, c.tan );\n c.binT1.crossVectors( this.tmpP1, c.bin );\n\n c.norT2.crossVectors( this.tmpP2, c.nor );\n c.tanT2.crossVectors( this.tmpP2, c.tan );\n c.binT2.crossVectors( this.tmpP2, c.bin );\n\n i1 = this.i1;\n i2 = this.i2;\n\n c.norTU1.copy( c.norT1 ).applyMatrix3( i1, true );\n c.tanTU1.copy( c.tanT1 ).applyMatrix3( i1, true );\n c.binTU1.copy( c.binT1 ).applyMatrix3( i1, true );\n\n c.norTU2.copy( c.norT2 ).applyMatrix3( i2, true );\n c.tanTU2.copy( c.tanT2 ).applyMatrix3( i2, true );\n c.binTU2.copy( c.binT2 ).applyMatrix3( i2, true );\n\n /*c.norTU1.mulMat( this.i1, c.norT1 );\n c.tanTU1.mulMat( this.i1, c.tanT1 );\n c.binTU1.mulMat( this.i1, c.binT1 );\n\n c.norTU2.mulMat( this.i2, c.norT2 );\n c.tanTU2.mulMat( this.i2, c.tanT2 );\n c.binTU2.mulMat( this.i2, c.binT2 );*/\n\n this.tmpC1.crossVectors( c.norTU1, this.tmpP1 );\n this.tmpC2.crossVectors( c.norTU2, this.tmpP2 );\n this.tmp.add( this.tmpC1, this.tmpC2 );\n c.norDen = 1 / ( m1m2 +_Math.dotVectors( c.nor, this.tmp ));\n\n this.tmpC1.crossVectors( c.tanTU1, this.tmpP1 );\n this.tmpC2.crossVectors( c.tanTU2, this.tmpP2 );\n this.tmp.add( this.tmpC1, this.tmpC2 );\n c.tanDen = 1 / ( m1m2 +_Math.dotVectors( c.tan, this.tmp ));\n\n this.tmpC1.crossVectors( c.binTU1, this.tmpP1 );\n this.tmpC2.crossVectors( c.binTU2, this.tmpP2 );\n this.tmp.add( this.tmpC1, this.tmpC2 );\n c.binDen = 1 / ( m1m2 +_Math.dotVectors( c.bin, this.tmp ));\n\n if( p.warmStarted ){\n\n norImp = p.normalImpulse;\n\n this.lv1.addScaledVector( c.norU1, norImp );\n this.av1.addScaledVector( c.norTU1, norImp );\n\n this.lv2.subScaledVector( c.norU2, norImp );\n this.av2.subScaledVector( c.norTU2, norImp );\n\n c.norImp = norImp;\n c.tanImp = 0;\n c.binImp = 0;\n rvn = 0; // disable bouncing\n\n } else {\n\n c.norImp=0;\n c.tanImp=0;\n c.binImp=0;\n\n }\n\n\n if(rvn>-1) rvn=0; // disable bouncing\n \n norTar = this.restitution*-rvn;\n sepV = -(p.penetration+0.005)*invTimeStep*0.05; // allow 0.5cm error\n if(norTar max * max ){\n len = max/_Math.sqrt(len);\n tanImp *= len;\n binImp *= len;\n }\n\n newImp1 = tanImp-oldImp1;\n newImp2 = binImp-oldImp2;\n\n //\n\n this.tmp.set( \n c.tanU1.x*newImp1 + c.binU1.x*newImp2,\n c.tanU1.y*newImp1 + c.binU1.y*newImp2,\n c.tanU1.z*newImp1 + c.binU1.z*newImp2\n );\n\n this.tmplv1.addEqual( this.tmp );\n\n this.tmp.set(\n c.tanTU1.x*newImp1 + c.binTU1.x*newImp2,\n c.tanTU1.y*newImp1 + c.binTU1.y*newImp2,\n c.tanTU1.z*newImp1 + c.binTU1.z*newImp2\n );\n\n this.tmpav1.addEqual( this.tmp );\n\n this.tmp.set(\n c.tanU2.x*newImp1 + c.binU2.x*newImp2,\n c.tanU2.y*newImp1 + c.binU2.y*newImp2,\n c.tanU2.z*newImp1 + c.binU2.z*newImp2\n );\n\n this.tmplv2.subEqual( this.tmp );\n\n this.tmp.set(\n c.tanTU2.x*newImp1 + c.binTU2.x*newImp2,\n c.tanTU2.y*newImp1 + c.binTU2.y*newImp2,\n c.tanTU2.z*newImp1 + c.binTU2.z*newImp2\n );\n\n this.tmpav2.subEqual( this.tmp );\n\n // restitution part\n\n this.tmp.sub( this.tmplv2, this.tmplv1 );\n\n rvn = _Math.dotVectors( this.tmp, c.nor ) + _Math.dotVectors( this.tmpav2, c.norT2 ) - _Math.dotVectors( this.tmpav1, c.norT1 );\n\n oldImp1 = norImp;\n newImp1 = (rvn-c.norTar)*c.norDen;\n norImp += newImp1;\n if( norImp > 0 ) norImp = 0;\n\n newImp1 = norImp - oldImp1;\n\n this.tmplv1.addScaledVector( c.norU1, newImp1 );\n this.tmpav1.addScaledVector( c.norTU1, newImp1 );\n this.tmplv2.subScaledVector( c.norU2, newImp1 );\n this.tmpav2.subScaledVector( c.norTU2, newImp1 );\n\n c.norImp = norImp;\n c.tanImp = tanImp;\n c.binImp = binImp;\n\n if(c.last)break;\n c = c.next;\n }\n\n this.lv1.copy( this.tmplv1 );\n this.lv2.copy( this.tmplv2 );\n this.av1.copy( this.tmpav1 );\n this.av2.copy( this.tmpav2 );\n\n },\n\n postSolve: function(){\n\n var c = this.cs, p;\n var i = this.num;\n while(i--){\n //for(var i=0;i -1 ){\r\n this.proxies.splice( n, 1 );\r\n //this.numProxies--;\r\n }\r\n\r\n /*var i = this.numProxies;\r\n while(i--){\r\n //for(var i=0, l=this.numProxies;i>1;\r\n //this.numPairChecks=this.numProxies*(this.numProxies-1)*0.5;\r\n\r\n while( i < l ){\r\n p1 = px[i++];\r\n j = i + 1;\r\n while( j < l ){\r\n p2 = px[j++];\r\n if ( p1.aabb.intersectTest( p2.aabb ) || !this.isAvailablePair( p1.shape, p2.shape ) ) continue;\r\n this.addPair( p1.shape, p2.shape );\r\n }\r\n }\r\n\r\n }\r\n\r\n});\n\n/**\r\n * A projection axis for sweep and prune broad-phase.\r\n * @author saharan\r\n */\r\n\r\nfunction SAPAxis (){\r\n\r\n this.numElements = 0;\r\n this.bufferSize = 256;\r\n this.elements = [];\r\n this.elements.length = this.bufferSize;\r\n this.stack = new Float32Array( 64 );\r\n\r\n}\r\n\r\nObject.assign( SAPAxis.prototype, {\r\n\r\n SAPAxis: true,\r\n\r\n addElements: function ( min, max ) {\r\n\r\n if(this.numElements+2>=this.bufferSize){\r\n //this.bufferSize<<=1;\r\n this.bufferSize*=2;\r\n var newElements=[];\r\n var i = this.numElements;\r\n while(i--){\r\n //for(var i=0, l=this.numElements; i> threshold) != 0 ) threshold++;\r\n threshold = threshold * this.numElements >> 2;\r\n count = 0;\r\n\r\n var giveup = false;\r\n var elements = this.elements;\r\n for( var i = 1, l = this.numElements; i < l; i++){ // try insertion sort\r\n var tmp=elements[i];\r\n var pivot=tmp.value;\r\n var tmp2=elements[i-1];\r\n if(tmp2.value>pivot){\r\n var j=i;\r\n do{\r\n elements[j]=tmp2;\r\n if(--j==0)break;\r\n tmp2=elements[j-1];\r\n }while(tmp2.value>pivot);\r\n elements[j]=tmp;\r\n count+=i-j;\r\n if(count>threshold){\r\n giveup=true; // stop and use quick sort\r\n break;\r\n }\r\n }\r\n }\r\n if(!giveup)return;\r\n count=2;var stack=this.stack;\r\n stack[0]=0;\r\n stack[1]=this.numElements-1;\r\n while(count>0){\r\n var right=stack[--count];\r\n var left=stack[--count];\r\n var diff=right-left;\r\n if(diff>16){ // quick sort\r\n //var mid=left+(diff>>1);\r\n var mid = left + (_Math.floor(diff*0.5));\r\n tmp = elements[mid];\r\n elements[mid] = elements[right];\r\n elements[right] = tmp;\r\n pivot = tmp.value;\r\n i = left-1;\r\n j = right;\r\n while( true ){\r\n var ei;\r\n var ej;\r\n do{ ei = elements[++i]; } while( ei.value < pivot);\r\n do{ ej = elements[--j]; } while( pivot < ej.value && j != left );\r\n if( i >= j ) break;\r\n elements[i] = ej;\r\n elements[j] = ei;\r\n }\r\n\r\n elements[right] = elements[i];\r\n elements[i] = tmp;\r\n if( i - left > right - i ) {\r\n stack[count++] = left;\r\n stack[count++] = i - 1;\r\n stack[count++] = i + 1;\r\n stack[count++] = right;\r\n }else{\r\n stack[count++] = i + 1;\r\n stack[count++] = right;\r\n stack[count++] = left;\r\n stack[count++] = i - 1;\r\n }\r\n }else{\r\n for( i = left + 1; i <= right; i++ ) {\r\n tmp = elements[i];\r\n pivot = tmp.value;\r\n tmp2 = elements[i-1];\r\n if( tmp2.value > pivot ) {\r\n j = i;\r\n do{\r\n elements[j] = tmp2;\r\n if( --j == 0 ) break;\r\n tmp2 = elements[j-1];\r\n }while( tmp2.value > pivot );\r\n elements[j] = tmp;\r\n }\r\n }\r\n }\r\n }\r\n \r\n },\r\n\r\n calculateTestCount: function () {\r\n\r\n var num = 1;\r\n var sum = 0;\r\n 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;\r\n this.addPair( s1, s2 );\r\n }\r\n if( dyn ){\r\n for( e2 = activeS; e2 != null; e2 = e2.pair ) {// test for static\r\n s2 = e2.proxy.shape;\r\n\r\n this.numPairChecks++;\r\n\r\n if( min1 > e2.max1.value || max1 < e2.min1.value|| min2 > e2.max2.value || max2 < e2.min2.value || !this.isAvailablePair(s1,s2) ) continue;\r\n this.addPair( s1, s2 );\r\n }\r\n e1.pair = activeD;\r\n activeD = e1;\r\n }else{\r\n e1.pair = activeS;\r\n activeS = e1;\r\n }\r\n }else{\r\n var min = e1.pair;\r\n if( dyn ){\r\n if( min == activeD ){\r\n activeD = activeD.pair;\r\n continue;\r\n }else{\r\n e1 = activeD;\r\n }\r\n }else{\r\n if( min == activeS ){\r\n activeS = activeS.pair;\r\n continue;\r\n }else{\r\n e1 = activeS;\r\n }\r\n }\r\n do{\r\n e2 = e1.pair;\r\n if( e2 == min ){\r\n e1.pair = e2.pair;\r\n break;\r\n }\r\n e1 = e2;\r\n }while( e1 != null );\r\n }\r\n }\r\n this.index2 = (this.index1|this.index2)^3;\r\n \r\n }\r\n\r\n});\n\n/**\r\n* A node of the dynamic bounding volume tree.\r\n* @author saharan\r\n*/\r\n\r\nfunction DBVTNode(){\r\n \r\n\t// The first child node of this node.\r\n this.child1 = null;\r\n // The second child node of this node.\r\n this.child2 = null;\r\n // The parent node of this tree.\r\n this.parent = null;\r\n // The proxy of this node. This has no value if this node is not leaf.\r\n this.proxy = null;\r\n // The maximum distance from leaf nodes.\r\n this.height = 0;\r\n // The AABB of this node.\r\n this.aabb = new AABB();\r\n\r\n}\n\n/**\r\n * A dynamic bounding volume tree for the broad-phase algorithm.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n\r\nfunction DBVT(){\r\n\r\n // The root of the tree.\r\n this.root = null;\r\n this.freeNodes = [];\r\n this.freeNodes.length = 16384;\r\n this.numFreeNodes = 0;\r\n this.aabb = new AABB();\r\n\r\n}\r\n\r\nObject.assign( DBVT.prototype, {\r\n\r\n DBVT: true,\r\n\r\n moveLeaf: function( leaf ) {\r\n\r\n this.deleteLeaf( leaf );\r\n this.insertLeaf( leaf );\r\n \r\n },\r\n\r\n insertLeaf: function ( leaf ) {\r\n\r\n if(this.root == null){\r\n this.root = leaf;\r\n return;\r\n }\r\n var lb = leaf.aabb;\r\n var sibling = this.root;\r\n var oldArea;\r\n var newArea;\r\n while(sibling.proxy == null){ // descend the node to search the best pair\r\n var c1 = sibling.child1;\r\n var c2 = sibling.child2;\r\n var b = sibling.aabb;\r\n var c1b = c1.aabb;\r\n var c2b = c2.aabb;\r\n oldArea = b.surfaceArea();\r\n this.aabb.combine(lb,b);\r\n newArea = this.aabb.surfaceArea();\r\n var creatingCost = newArea*2;\r\n var incrementalCost = (newArea-oldArea)*2; // cost of creating a new pair with the node\r\n var discendingCost1 = incrementalCost;\r\n this.aabb.combine(lb,c1b);\r\n if(c1.proxy!=null){\r\n // leaf cost = area(combined aabb)\r\n discendingCost1+=this.aabb.surfaceArea();\r\n }else{\r\n // node cost = area(combined aabb) - area(old aabb)\r\n discendingCost1+=this.aabb.surfaceArea()-c1b.surfaceArea();\r\n }\r\n var discendingCost2=incrementalCost;\r\n this.aabb.combine(lb,c2b);\r\n if(c2.proxy!=null){\r\n // leaf cost = area(combined aabb)\r\n discendingCost2+=this.aabb.surfaceArea();\r\n }else{\r\n // node cost = area(combined aabb) - area(old aabb)\r\n discendingCost2+=this.aabb.surfaceArea()-c2b.surfaceArea();\r\n }\r\n if(discendingCost10){\r\n newParent = this.freeNodes[--this.numFreeNodes];\r\n }else{\r\n newParent = new DBVTNode();\r\n }\r\n\r\n newParent.parent = oldParent;\r\n newParent.child1 = leaf;\r\n newParent.child2 = sibling;\r\n newParent.aabb.combine(leaf.aabb,sibling.aabb);\r\n newParent.height = sibling.height+1;\r\n sibling.parent = newParent;\r\n leaf.parent = newParent;\r\n if(sibling == this.root){\r\n // replace root\r\n this.root = newParent;\r\n }else{\r\n // replace child\r\n if(oldParent.child1 == sibling){\r\n oldParent.child1 = newParent;\r\n }else{\r\n oldParent.child2 = newParent;\r\n }\r\n }\r\n // update whole tree\r\n do{\r\n newParent = this.balance(newParent);\r\n this.fix(newParent);\r\n newParent = newParent.parent;\r\n }while(newParent != null);\r\n },\r\n\r\n getBalance: function( node ) {\r\n\r\n if(node.proxy!=null)return 0;\r\n return node.child1.height-node.child2.height;\r\n\r\n },\r\n\r\n deleteLeaf: function( leaf ) {\r\n\r\n if(leaf == this.root){\r\n this.root = null;\r\n return;\r\n }\r\n var parent = leaf.parent;\r\n var sibling;\r\n if(parent.child1==leaf){\r\n sibling=parent.child2;\r\n }else{\r\n sibling=parent.child1;\r\n }\r\n if(parent==this.root){\r\n this.root=sibling;\r\n sibling.parent=null;\r\n return;\r\n }\r\n var grandParent = parent.parent;\r\n sibling.parent = grandParent;\r\n if(grandParent.child1 == parent ) {\r\n grandParent.child1 = sibling;\r\n }else{\r\n grandParent.child2 = sibling;\r\n }\r\n if(this.numFreeNodes<16384){\r\n this.freeNodes[this.numFreeNodes++] = parent;\r\n }\r\n do{\r\n grandParent = this.balance(grandParent);\r\n this.fix(grandParent);\r\n grandParent = grandParent.parent;\r\n }while( grandParent != null );\r\n \r\n },\r\n\r\n balance: function( node ) {\r\n\r\n var nh = node.height;\r\n if(nh<2){\r\n return node;\r\n }\r\n var p = node.parent;\r\n var l = node.child1;\r\n var r = node.child2;\r\n var lh = l.height;\r\n var rh = r.height;\r\n var balance = lh-rh;\r\n var t;// for bit operation\r\n\r\n // [ N ]\r\n // / \\\r\n // [ L ] [ R ]\r\n // / \\ / \\\r\n // [L-L] [L-R] [R-L] [R-R]\r\n\r\n // Is the tree balanced?\r\n if(balance>1){\r\n var ll = l.child1;\r\n var lr = l.child2;\r\n var llh = ll.height;\r\n var lrh = lr.height;\r\n\r\n // Is L-L higher than L-R?\r\n if(llh>lrh){\r\n // set N to L-R\r\n l.child2 = node;\r\n node.parent = l;\r\n\r\n // [ L ]\r\n // / \\\r\n // [L-L] [ N ]\r\n // / \\ / \\\r\n // [...] [...] [ L ] [ R ]\r\n \r\n // set L-R\r\n node.child1 = lr;\r\n lr.parent = node;\r\n\r\n // [ L ]\r\n // / \\\r\n // [L-L] [ N ]\r\n // / \\ / \\\r\n // [...] [...] [L-R] [ R ]\r\n \r\n // fix bounds and heights\r\n node.aabb.combine( lr.aabb, r.aabb );\r\n t = lrh-rh;\r\n node.height=lrh-(t&t>>31)+1;\r\n l.aabb.combine(ll.aabb,node.aabb);\r\n t=llh-nh;\r\n l.height=llh-(t&t>>31)+1;\r\n }else{\r\n // set N to L-L\r\n l.child1=node;\r\n node.parent=l;\r\n\r\n // [ L ]\r\n // / \\\r\n // [ N ] [L-R]\r\n // / \\ / \\\r\n // [ L ] [ R ] [...] [...]\r\n \r\n // set L-L\r\n node.child1 = ll;\r\n ll.parent = node;\r\n\r\n // [ L ]\r\n // / \\\r\n // [ N ] [L-R]\r\n // / \\ / \\\r\n // [L-L] [ R ] [...] [...]\r\n \r\n // fix bounds and heights\r\n node.aabb.combine(ll.aabb,r.aabb);\r\n t = llh - rh;\r\n node.height=llh-(t&t>>31)+1;\r\n\r\n l.aabb.combine(node.aabb,lr.aabb);\r\n t=nh-lrh;\r\n l.height=nh-(t&t>>31)+1;\r\n }\r\n // set new parent of L\r\n if(p!=null){\r\n if(p.child1==node){\r\n p.child1=l;\r\n }else{\r\n p.child2=l;\r\n }\r\n }else{\r\n this.root=l;\r\n }\r\n l.parent=p;\r\n return l;\r\n }else if(balance<-1){\r\n var rl = r.child1;\r\n var rr = r.child2;\r\n var rlh = rl.height;\r\n var rrh = rr.height;\r\n\r\n // Is R-L higher than R-R?\r\n if( rlh > rrh ) {\r\n // set N to R-R\r\n r.child2 = node;\r\n node.parent = r;\r\n\r\n // [ R ]\r\n // / \\\r\n // [R-L] [ N ]\r\n // / \\ / \\\r\n // [...] [...] [ L ] [ R ]\r\n \r\n // set R-R\r\n node.child2 = rr;\r\n rr.parent = node;\r\n\r\n // [ R ]\r\n // / \\\r\n // [R-L] [ N ]\r\n // / \\ / \\\r\n // [...] [...] [ L ] [R-R]\r\n \r\n // fix bounds and heights\r\n node.aabb.combine(l.aabb,rr.aabb);\r\n t = lh-rrh;\r\n node.height = lh-(t&t>>31)+1;\r\n r.aabb.combine(rl.aabb,node.aabb);\r\n t = rlh-nh;\r\n r.height = rlh-(t&t>>31)+1;\r\n }else{\r\n // set N to R-L\r\n r.child1 = node;\r\n node.parent = r;\r\n // [ R ]\r\n // / \\\r\n // [ N ] [R-R]\r\n // / \\ / \\\r\n // [ L ] [ R ] [...] [...]\r\n \r\n // set R-L\r\n node.child2 = rl;\r\n rl.parent = node;\r\n\r\n // [ R ]\r\n // / \\\r\n // [ N ] [R-R]\r\n // / \\ / \\\r\n // [ L ] [R-L] [...] [...]\r\n \r\n // fix bounds and heights\r\n node.aabb.combine(l.aabb,rl.aabb);\r\n t=lh-rlh;\r\n node.height=lh-(t&t>>31)+1;\r\n r.aabb.combine(node.aabb,rr.aabb);\r\n t=nh-rrh;\r\n r.height=nh-(t&t>>31)+1;\r\n }\r\n // set new parent of R\r\n if(p!=null){\r\n if(p.child1==node){\r\n p.child1=r;\r\n }else{\r\n p.child2=r;\r\n }\r\n }else{\r\n this.root=r;\r\n }\r\n r.parent=p;\r\n return r;\r\n }\r\n return node;\r\n },\r\n\r\n fix: function ( node ) {\r\n\r\n var c1 = node.child1;\r\n var c2 = node.child2;\r\n node.aabb.combine( c1.aabb, c2.aabb );\r\n node.height = c1.height < c2.height ? c2.height+1 : c1.height+1; \r\n\r\n }\r\n \r\n});\n\n/**\r\n* A proxy for dynamic bounding volume tree broad-phase.\r\n* @author saharan\r\n*/\r\n\r\nfunction DBVTProxy ( shape ) {\r\n\r\n Proxy.call( this, shape);\r\n // The leaf of the proxy.\r\n this.leaf = new DBVTNode();\r\n this.leaf.proxy = this;\r\n\r\n}\r\n\r\nDBVTProxy.prototype = Object.assign( Object.create( Proxy.prototype ), {\r\n\r\n constructor: DBVTProxy,\r\n\r\n update: function () {\r\n\r\n }\r\n\r\n});\n\n/**\r\n * A broad-phase algorithm using dynamic bounding volume tree.\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n\r\nfunction DBVTBroadPhase(){\r\n\r\n BroadPhase.call( this );\r\n\r\n this.types = BR_BOUNDING_VOLUME_TREE;\r\n\r\n this.tree = new DBVT();\r\n this.stack = [];\r\n this.leaves = [];\r\n this.numLeaves = 0;\r\n\r\n}\r\n\r\nDBVTBroadPhase.prototype = Object.assign( Object.create( BroadPhase.prototype ), {\r\n\r\n constructor: DBVTBroadPhase,\r\n\r\n createProxy: function ( shape ) {\r\n\r\n return new DBVTProxy( shape );\r\n\r\n },\r\n\r\n addProxy: function ( proxy ) {\r\n\r\n this.tree.insertLeaf( proxy.leaf );\r\n this.leaves.push( proxy.leaf );\r\n this.numLeaves++;\r\n\r\n },\r\n\r\n removeProxy: function ( proxy ) {\r\n\r\n this.tree.deleteLeaf( proxy.leaf );\r\n var n = this.leaves.indexOf( proxy.leaf );\r\n if ( n > -1 ) {\r\n this.leaves.splice(n,1);\r\n this.numLeaves--;\r\n }\r\n\r\n },\r\n\r\n collectPairs: function () {\r\n\r\n if ( this.numLeaves < 2 ) return;\r\n\r\n var leaf, margin = 0.1, i = this.numLeaves;\r\n\r\n while(i--){\r\n\r\n leaf = this.leaves[i];\r\n\r\n if ( leaf.proxy.aabb.intersectTestTwo( leaf.aabb ) ){\r\n\r\n leaf.aabb.copy( leaf.proxy.aabb, margin );\r\n this.tree.deleteLeaf( leaf );\r\n this.tree.insertLeaf( leaf );\r\n this.collide( leaf, this.tree.root );\r\n\r\n }\r\n }\r\n\r\n },\r\n\r\n collide: function ( node1, node2 ) {\r\n\r\n var stackCount = 2;\r\n var s1, s2, n1, n2, l1, l2;\r\n this.stack[0] = node1;\r\n this.stack[1] = node2;\r\n\r\n while( stackCount > 0 ){\r\n\r\n n1 = this.stack[--stackCount];\r\n n2 = this.stack[--stackCount];\r\n l1 = n1.proxy != null;\r\n l2 = n2.proxy != null;\r\n \r\n this.numPairChecks++;\r\n\r\n if( l1 && l2 ){\r\n s1 = n1.proxy.shape;\r\n s2 = n2.proxy.shape;\r\n if ( s1 == s2 || s1.aabb.intersectTest( s2.aabb ) || !this.isAvailablePair( s1, s2 ) ) continue;\r\n\r\n this.addPair(s1,s2);\r\n\r\n }else{\r\n\r\n if ( n1.aabb.intersectTest( n2.aabb ) ) continue;\r\n \r\n /*if(stackCount+4>=this.maxStack){// expand the stack\r\n //this.maxStack<<=1;\r\n this.maxStack*=2;\r\n var newStack = [];// vector\r\n newStack.length = this.maxStack;\r\n for(var i=0;i n2.aabb.surfaceArea()) ){\r\n this.stack[stackCount++] = n1.child1;\r\n this.stack[stackCount++] = n2;\r\n this.stack[stackCount++] = n1.child2;\r\n this.stack[stackCount++] = n2;\r\n }else{\r\n this.stack[stackCount++] = n1;\r\n this.stack[stackCount++] = n2.child1;\r\n this.stack[stackCount++] = n1;\r\n this.stack[stackCount++] = n2.child2;\r\n }\r\n }\r\n }\r\n\r\n }\r\n\r\n});\n\nfunction CollisionDetector (){\r\n\r\n this.flip = false;\r\n\r\n}\r\n\r\nObject.assign( CollisionDetector.prototype, {\r\n\r\n CollisionDetector: true,\r\n\r\n detectCollision: function ( shape1, shape2, manifold ) {\r\n\r\n printError(\"CollisionDetector\", \"Inheritance error.\");\r\n\r\n }\r\n\r\n} );\n\n/**\r\n * A collision detector which detects collisions between two boxes.\r\n * @author saharan\r\n */\r\nfunction BoxBoxCollisionDetector() {\r\n\r\n CollisionDetector.call( this );\r\n this.clipVertices1 = new Float32Array( 24 ); // 8 vertices x,y,z\r\n this.clipVertices2 = new Float32Array( 24 );\r\n this.used = new Float32Array( 8 );\r\n \r\n this.INF = 1/0;\r\n\r\n}\r\n\r\nBoxBoxCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), {\r\n\r\n constructor: BoxBoxCollisionDetector,\r\n\r\n detectCollision: function ( shape1, shape2, manifold ) {\r\n // What you are doing \r\n // · I to prepare a separate axis of the fifteen \r\n //-Six in each of three normal vectors of the xyz direction of the box both \r\n // · Remaining nine 3x3 a vector perpendicular to the side of the box 2 and the side of the box 1 \r\n // · Calculate the depth to the separation axis \r\n\r\n // Calculates the distance using the inner product and put the amount of embedment \r\n // · However a vertical separation axis and side to weight a little to avoid vibration \r\n // And end when there is a separate axis that is remote even one \r\n // · I look for separation axis with little to dent most \r\n // Men and if separation axis of the first six - end collision \r\n // Heng If it separate axis of nine other - side collision \r\n // Heng - case of a side collision \r\n // · Find points of two sides on which you made ​​the separation axis \r\n\r\n // Calculates the point of closest approach of a straight line consisting of separate axis points obtained, and the collision point \r\n //-Surface - the case of the plane crash \r\n //-Box A, box B and the other a box of better made ​​a separate axis \r\n // • 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 \r\n\r\n // When viewed from the front surface A, and the cut part exceeding the area of the surface A is a surface B \r\n //-Plane B becomes the 3-8 triangle, I a candidate for the collision point the vertex of surface B \r\n // • If more than one candidate 5 exists, scraping up to four \r\n\r\n // For potential collision points of all, to examine the distance between the surface A \r\n // • If you were on the inside surface of A, and the collision point\r\n\r\n var b1;\r\n var b2;\r\n if(shape1.id0;\r\n if(!right1)len=-len;\r\n len1=w1;\r\n dot1=a1x*a4x+a1y*a4y+a1z*a4z;\r\n dot2=a1x*a5x+a1y*a5y+a1z*a5z;\r\n dot3=a1x*a6x+a1y*a6y+a1z*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n if(dot3<0)dot3=-dot3;\r\n len2=dot1*w2+dot2*h2+dot3*d2;\r\n overlap1=len-len1-len2;\r\n if(overlap1>0)return;\r\n // try axis 2\r\n len=a2x*dx+a2y*dy+a2z*dz;\r\n right2=len>0;\r\n if(!right2)len=-len;\r\n len1=h1;\r\n dot1=a2x*a4x+a2y*a4y+a2z*a4z;\r\n dot2=a2x*a5x+a2y*a5y+a2z*a5z;\r\n dot3=a2x*a6x+a2y*a6y+a2z*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n if(dot3<0)dot3=-dot3;\r\n len2=dot1*w2+dot2*h2+dot3*d2;\r\n overlap2=len-len1-len2;\r\n if(overlap2>0)return;\r\n // try axis 3\r\n len=a3x*dx+a3y*dy+a3z*dz;\r\n right3=len>0;\r\n if(!right3)len=-len;\r\n len1=d1;\r\n dot1=a3x*a4x+a3y*a4y+a3z*a4z;\r\n dot2=a3x*a5x+a3y*a5y+a3z*a5z;\r\n dot3=a3x*a6x+a3y*a6y+a3z*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n if(dot3<0)dot3=-dot3;\r\n len2=dot1*w2+dot2*h2+dot3*d2;\r\n overlap3=len-len1-len2;\r\n if(overlap3>0)return;\r\n // try axis 4\r\n len=a4x*dx+a4y*dy+a4z*dz;\r\n right4=len>0;\r\n if(!right4)len=-len;\r\n dot1=a4x*a1x+a4y*a1y+a4z*a1z;\r\n dot2=a4x*a2x+a4y*a2y+a4z*a2z;\r\n dot3=a4x*a3x+a4y*a3y+a4z*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n if(dot3<0)dot3=-dot3;\r\n len1=dot1*w1+dot2*h1+dot3*d1;\r\n len2=w2;\r\n overlap4=(len-len1-len2)*1.0;\r\n if(overlap4>0)return;\r\n // try axis 5\r\n len=a5x*dx+a5y*dy+a5z*dz;\r\n right5=len>0;\r\n if(!right5)len=-len;\r\n dot1=a5x*a1x+a5y*a1y+a5z*a1z;\r\n dot2=a5x*a2x+a5y*a2y+a5z*a2z;\r\n dot3=a5x*a3x+a5y*a3y+a5z*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n if(dot3<0)dot3=-dot3;\r\n len1=dot1*w1+dot2*h1+dot3*d1;\r\n len2=h2;\r\n overlap5=(len-len1-len2)*1.0;\r\n if(overlap5>0)return;\r\n // try axis 6\r\n len=a6x*dx+a6y*dy+a6z*dz;\r\n right6=len>0;\r\n if(!right6)len=-len;\r\n dot1=a6x*a1x+a6y*a1y+a6z*a1z;\r\n dot2=a6x*a2x+a6y*a2y+a6z*a2z;\r\n dot3=a6x*a3x+a6y*a3y+a6z*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n if(dot3<0)dot3=-dot3;\r\n len1=dot1*w1+dot2*h1+dot3*d1;\r\n len2=d2;\r\n overlap6=(len-len1-len2)*1.0;\r\n if(overlap6>0)return;\r\n // try axis 7\r\n len=a7x*a7x+a7y*a7y+a7z*a7z;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n a7x*=len;\r\n a7y*=len;\r\n a7z*=len;\r\n len=a7x*dx+a7y*dy+a7z*dz;\r\n right7=len>0;\r\n if(!right7)len=-len;\r\n dot1=a7x*a2x+a7y*a2y+a7z*a2z;\r\n dot2=a7x*a3x+a7y*a3y+a7z*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*h1+dot2*d1;\r\n dot1=a7x*a5x+a7y*a5y+a7z*a5z;\r\n dot2=a7x*a6x+a7y*a6y+a7z*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*h2+dot2*d2;\r\n overlap7=len-len1-len2;\r\n if(overlap7>0)return;\r\n }else{\r\n right7=false;\r\n overlap7=0;\r\n invalid7=true;\r\n }\r\n // try axis 8\r\n len=a8x*a8x+a8y*a8y+a8z*a8z;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n a8x*=len;\r\n a8y*=len;\r\n a8z*=len;\r\n len=a8x*dx+a8y*dy+a8z*dz;\r\n right8=len>0;\r\n if(!right8)len=-len;\r\n dot1=a8x*a2x+a8y*a2y+a8z*a2z;\r\n dot2=a8x*a3x+a8y*a3y+a8z*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*h1+dot2*d1;\r\n dot1=a8x*a4x+a8y*a4y+a8z*a4z;\r\n dot2=a8x*a6x+a8y*a6y+a8z*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*w2+dot2*d2;\r\n overlap8=len-len1-len2;\r\n if(overlap8>0)return;\r\n }else{\r\n right8=false;\r\n overlap8=0;\r\n invalid8=true;\r\n }\r\n // try axis 9\r\n len=a9x*a9x+a9y*a9y+a9z*a9z;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n a9x*=len;\r\n a9y*=len;\r\n a9z*=len;\r\n len=a9x*dx+a9y*dy+a9z*dz;\r\n right9=len>0;\r\n if(!right9)len=-len;\r\n dot1=a9x*a2x+a9y*a2y+a9z*a2z;\r\n dot2=a9x*a3x+a9y*a3y+a9z*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*h1+dot2*d1;\r\n dot1=a9x*a4x+a9y*a4y+a9z*a4z;\r\n dot2=a9x*a5x+a9y*a5y+a9z*a5z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*w2+dot2*h2;\r\n overlap9=len-len1-len2;\r\n if(overlap9>0)return;\r\n }else{\r\n right9=false;\r\n overlap9=0;\r\n invalid9=true;\r\n }\r\n // try axis 10\r\n len=aax*aax+aay*aay+aaz*aaz;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n aax*=len;\r\n aay*=len;\r\n aaz*=len;\r\n len=aax*dx+aay*dy+aaz*dz;\r\n righta=len>0;\r\n if(!righta)len=-len;\r\n dot1=aax*a1x+aay*a1y+aaz*a1z;\r\n dot2=aax*a3x+aay*a3y+aaz*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*w1+dot2*d1;\r\n dot1=aax*a5x+aay*a5y+aaz*a5z;\r\n dot2=aax*a6x+aay*a6y+aaz*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*h2+dot2*d2;\r\n overlapa=len-len1-len2;\r\n if(overlapa>0)return;\r\n }else{\r\n righta=false;\r\n overlapa=0;\r\n invalida=true;\r\n }\r\n // try axis 11\r\n len=abx*abx+aby*aby+abz*abz;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n abx*=len;\r\n aby*=len;\r\n abz*=len;\r\n len=abx*dx+aby*dy+abz*dz;\r\n rightb=len>0;\r\n if(!rightb)len=-len;\r\n dot1=abx*a1x+aby*a1y+abz*a1z;\r\n dot2=abx*a3x+aby*a3y+abz*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*w1+dot2*d1;\r\n dot1=abx*a4x+aby*a4y+abz*a4z;\r\n dot2=abx*a6x+aby*a6y+abz*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*w2+dot2*d2;\r\n overlapb=len-len1-len2;\r\n if(overlapb>0)return;\r\n }else{\r\n rightb=false;\r\n overlapb=0;\r\n invalidb=true;\r\n }\r\n // try axis 12\r\n len=acx*acx+acy*acy+acz*acz;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n acx*=len;\r\n acy*=len;\r\n acz*=len;\r\n len=acx*dx+acy*dy+acz*dz;\r\n rightc=len>0;\r\n if(!rightc)len=-len;\r\n dot1=acx*a1x+acy*a1y+acz*a1z;\r\n dot2=acx*a3x+acy*a3y+acz*a3z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*w1+dot2*d1;\r\n dot1=acx*a4x+acy*a4y+acz*a4z;\r\n dot2=acx*a5x+acy*a5y+acz*a5z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*w2+dot2*h2;\r\n overlapc=len-len1-len2;\r\n if(overlapc>0)return;\r\n }else{\r\n rightc=false;\r\n overlapc=0;\r\n invalidc=true;\r\n }\r\n // try axis 13\r\n len=adx*adx+ady*ady+adz*adz;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n adx*=len;\r\n ady*=len;\r\n adz*=len;\r\n len=adx*dx+ady*dy+adz*dz;\r\n rightd=len>0;\r\n if(!rightd)len=-len;\r\n dot1=adx*a1x+ady*a1y+adz*a1z;\r\n dot2=adx*a2x+ady*a2y+adz*a2z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*w1+dot2*h1;\r\n dot1=adx*a5x+ady*a5y+adz*a5z;\r\n dot2=adx*a6x+ady*a6y+adz*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*h2+dot2*d2;\r\n overlapd=len-len1-len2;\r\n if(overlapd>0)return;\r\n }else{\r\n rightd=false;\r\n overlapd=0;\r\n invalidd=true;\r\n }\r\n // try axis 14\r\n len=aex*aex+aey*aey+aez*aez;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n aex*=len;\r\n aey*=len;\r\n aez*=len;\r\n len=aex*dx+aey*dy+aez*dz;\r\n righte=len>0;\r\n if(!righte)len=-len;\r\n dot1=aex*a1x+aey*a1y+aez*a1z;\r\n dot2=aex*a2x+aey*a2y+aez*a2z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*w1+dot2*h1;\r\n dot1=aex*a4x+aey*a4y+aez*a4z;\r\n dot2=aex*a6x+aey*a6y+aez*a6z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*w2+dot2*d2;\r\n overlape=len-len1-len2;\r\n if(overlape>0)return;\r\n }else{\r\n righte=false;\r\n overlape=0;\r\n invalide=true;\r\n }\r\n // try axis 15\r\n len=afx*afx+afy*afy+afz*afz;\r\n if(len>1e-5){\r\n len=1/_Math.sqrt(len);\r\n afx*=len;\r\n afy*=len;\r\n afz*=len;\r\n len=afx*dx+afy*dy+afz*dz;\r\n rightf=len>0;\r\n if(!rightf)len=-len;\r\n dot1=afx*a1x+afy*a1y+afz*a1z;\r\n dot2=afx*a2x+afy*a2y+afz*a2z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len1=dot1*w1+dot2*h1;\r\n dot1=afx*a4x+afy*a4y+afz*a4z;\r\n dot2=afx*a5x+afy*a5y+afz*a5z;\r\n if(dot1<0)dot1=-dot1;\r\n if(dot2<0)dot2=-dot2;\r\n len2=dot1*w2+dot2*h2;\r\n overlapf=len-len1-len2;\r\n if(overlapf>0)return;\r\n }else{\r\n rightf=false;\r\n overlapf=0;\r\n invalidf=true;\r\n }\r\n // boxes are overlapping\r\n var depth=overlap1;\r\n var depth2=overlap1;\r\n var minIndex=0;\r\n var right=right1;\r\n if(overlap2>depth2){\r\n depth=overlap2;\r\n depth2=overlap2;\r\n minIndex=1;\r\n right=right2;\r\n }\r\n if(overlap3>depth2){\r\n depth=overlap3;\r\n depth2=overlap3;\r\n minIndex=2;\r\n right=right3;\r\n }\r\n if(overlap4>depth2){\r\n depth=overlap4;\r\n depth2=overlap4;\r\n minIndex=3;\r\n right=right4;\r\n }\r\n if(overlap5>depth2){\r\n depth=overlap5;\r\n depth2=overlap5;\r\n minIndex=4;\r\n right=right5;\r\n }\r\n if(overlap6>depth2){\r\n depth=overlap6;\r\n depth2=overlap6;\r\n minIndex=5;\r\n right=right6;\r\n }\r\n if(overlap7-0.01>depth2&&!invalid7){\r\n depth=overlap7;\r\n depth2=overlap7-0.01;\r\n minIndex=6;\r\n right=right7;\r\n }\r\n if(overlap8-0.01>depth2&&!invalid8){\r\n depth=overlap8;\r\n depth2=overlap8-0.01;\r\n minIndex=7;\r\n right=right8;\r\n }\r\n if(overlap9-0.01>depth2&&!invalid9){\r\n depth=overlap9;\r\n depth2=overlap9-0.01;\r\n minIndex=8;\r\n right=right9;\r\n }\r\n if(overlapa-0.01>depth2&&!invalida){\r\n depth=overlapa;\r\n depth2=overlapa-0.01;\r\n minIndex=9;\r\n right=righta;\r\n }\r\n if(overlapb-0.01>depth2&&!invalidb){\r\n depth=overlapb;\r\n depth2=overlapb-0.01;\r\n minIndex=10;\r\n right=rightb;\r\n }\r\n if(overlapc-0.01>depth2&&!invalidc){\r\n depth=overlapc;\r\n depth2=overlapc-0.01;\r\n minIndex=11;\r\n right=rightc;\r\n }\r\n if(overlapd-0.01>depth2&&!invalidd){\r\n depth=overlapd;\r\n depth2=overlapd-0.01;\r\n minIndex=12;\r\n right=rightd;\r\n }\r\n if(overlape-0.01>depth2&&!invalide){\r\n depth=overlape;\r\n depth2=overlape-0.01;\r\n minIndex=13;\r\n right=righte;\r\n }\r\n if(overlapf-0.01>depth2&&!invalidf){\r\n depth=overlapf;\r\n minIndex=14;\r\n right=rightf;\r\n }\r\n // normal\r\n var nx=0;\r\n var ny=0;\r\n var nz=0;\r\n // edge line or face side normal\r\n var n1x=0;\r\n var n1y=0;\r\n var n1z=0;\r\n var n2x=0;\r\n var n2y=0;\r\n var n2z=0;\r\n // center of current face\r\n var cx=0;\r\n var cy=0;\r\n var cz=0;\r\n // face side\r\n var s1x=0;\r\n var s1y=0;\r\n var s1z=0;\r\n var s2x=0;\r\n var s2y=0;\r\n var s2z=0;\r\n // swap b1 b2\r\n var swap=false;\r\n\r\n //_______________________________________\r\n\r\n if(minIndex==0){// b1.x * b2\r\n if(right){\r\n cx=p1x+d1x; cy=p1y+d1y; cz=p1z+d1z;\r\n nx=a1x; ny=a1y; nz=a1z;\r\n }else{\r\n cx=p1x-d1x; cy=p1y-d1y; cz=p1z-d1z;\r\n nx=-a1x; ny=-a1y; nz=-a1z;\r\n }\r\n s1x=d2x; s1y=d2y; s1z=d2z;\r\n n1x=-a2x; n1y=-a2y; n1z=-a2z;\r\n s2x=d3x; s2y=d3y; s2z=d3z;\r\n n2x=-a3x; n2y=-a3y; n2z=-a3z;\r\n }\r\n else if(minIndex==1){// b1.y * b2\r\n if(right){\r\n cx=p1x+d2x; cy=p1y+d2y; cz=p1z+d2z;\r\n nx=a2x; ny=a2y; nz=a2z;\r\n }else{\r\n cx=p1x-d2x; cy=p1y-d2y; cz=p1z-d2z;\r\n nx=-a2x; ny=-a2y; nz=-a2z;\r\n }\r\n s1x=d1x; s1y=d1y; s1z=d1z;\r\n n1x=-a1x; n1y=-a1y; n1z=-a1z;\r\n s2x=d3x; s2y=d3y; s2z=d3z;\r\n n2x=-a3x; n2y=-a3y; n2z=-a3z;\r\n }\r\n else if(minIndex==2){// b1.z * b2\r\n if(right){\r\n cx=p1x+d3x; cy=p1y+d3y; cz=p1z+d3z;\r\n nx=a3x; ny=a3y; nz=a3z;\r\n }else{\r\n cx=p1x-d3x; cy=p1y-d3y; cz=p1z-d3z;\r\n nx=-a3x; ny=-a3y; nz=-a3z;\r\n }\r\n s1x=d1x; s1y=d1y; s1z=d1z;\r\n n1x=-a1x; n1y=-a1y; n1z=-a1z;\r\n s2x=d2x; s2y=d2y; s2z=d2z;\r\n n2x=-a2x; n2y=-a2y; n2z=-a2z;\r\n }\r\n else if(minIndex==3){// b2.x * b1\r\n swap=true;\r\n if(!right){\r\n cx=p2x+d4x; cy=p2y+d4y; cz=p2z+d4z;\r\n nx=a4x; ny=a4y; nz=a4z;\r\n }else{\r\n cx=p2x-d4x; cy=p2y-d4y; cz=p2z-d4z;\r\n nx=-a4x; ny=-a4y; nz=-a4z;\r\n }\r\n s1x=d5x; s1y=d5y; s1z=d5z;\r\n n1x=-a5x; n1y=-a5y; n1z=-a5z;\r\n s2x=d6x; s2y=d6y; s2z=d6z;\r\n n2x=-a6x; n2y=-a6y; n2z=-a6z;\r\n }\r\n else if(minIndex==4){// b2.y * b1\r\n swap=true;\r\n if(!right){\r\n cx=p2x+d5x; cy=p2y+d5y; cz=p2z+d5z;\r\n nx=a5x; ny=a5y; nz=a5z;\r\n }else{\r\n cx=p2x-d5x; cy=p2y-d5y; cz=p2z-d5z;\r\n nx=-a5x; ny=-a5y; nz=-a5z;\r\n }\r\n s1x=d4x; s1y=d4y; s1z=d4z;\r\n n1x=-a4x; n1y=-a4y; n1z=-a4z;\r\n s2x=d6x; s2y=d6y; s2z=d6z;\r\n n2x=-a6x; n2y=-a6y; n2z=-a6z;\r\n }\r\n else if(minIndex==5){// b2.z * b1\r\n swap=true;\r\n if(!right){\r\n cx=p2x+d6x; cy=p2y+d6y; cz=p2z+d6z;\r\n nx=a6x; ny=a6y; nz=a6z;\r\n }else{\r\n cx=p2x-d6x; cy=p2y-d6y; cz=p2z-d6z;\r\n nx=-a6x; ny=-a6y; nz=-a6z;\r\n }\r\n s1x=d4x; s1y=d4y; s1z=d4z;\r\n n1x=-a4x; n1y=-a4y; n1z=-a4z;\r\n s2x=d5x; s2y=d5y; s2z=d5z;\r\n n2x=-a5x; n2y=-a5y; n2z=-a5z;\r\n }\r\n else if(minIndex==6){// b1.x * b2.x\r\n nx=a7x; ny=a7y; nz=a7z;\r\n n1x=a1x; n1y=a1y; n1z=a1z;\r\n n2x=a4x; n2y=a4y; n2z=a4z;\r\n }\r\n else if(minIndex==7){// b1.x * b2.y\r\n nx=a8x; ny=a8y; nz=a8z;\r\n n1x=a1x; n1y=a1y; n1z=a1z;\r\n n2x=a5x; n2y=a5y; n2z=a5z;\r\n }\r\n else if(minIndex==8){// b1.x * b2.z\r\n nx=a9x; ny=a9y; nz=a9z;\r\n n1x=a1x; n1y=a1y; n1z=a1z;\r\n n2x=a6x; n2y=a6y; n2z=a6z;\r\n }\r\n else if(minIndex==9){// b1.y * b2.x\r\n nx=aax; ny=aay; nz=aaz;\r\n n1x=a2x; n1y=a2y; n1z=a2z;\r\n n2x=a4x; n2y=a4y; n2z=a4z;\r\n }\r\n else if(minIndex==10){// b1.y * b2.y\r\n nx=abx; ny=aby; nz=abz;\r\n n1x=a2x; n1y=a2y; n1z=a2z;\r\n n2x=a5x; n2y=a5y; n2z=a5z;\r\n }\r\n else if(minIndex==11){// b1.y * b2.z\r\n nx=acx; ny=acy; nz=acz;\r\n n1x=a2x; n1y=a2y; n1z=a2z;\r\n n2x=a6x; n2y=a6y; n2z=a6z;\r\n }\r\n else if(minIndex==12){// b1.z * b2.x\r\n nx=adx; ny=ady; nz=adz;\r\n n1x=a3x; n1y=a3y; n1z=a3z;\r\n n2x=a4x; n2y=a4y; n2z=a4z;\r\n }\r\n else if(minIndex==13){// b1.z * b2.y\r\n nx=aex; ny=aey; nz=aez;\r\n n1x=a3x; n1y=a3y; n1z=a3z;\r\n n2x=a5x; n2y=a5y; n2z=a5z;\r\n }\r\n else if(minIndex==14){// b1.z * b2.z\r\n nx=afx; ny=afy; nz=afz;\r\n n1x=a3x; n1y=a3y; n1z=a3z;\r\n n2x=a6x; n2y=a6y; n2z=a6z;\r\n }\r\n\r\n //__________________________________________\r\n\r\n //var v;\r\n if(minIndex>5){\r\n if(!right){\r\n nx=-nx; ny=-ny; nz=-nz;\r\n }\r\n var distance;\r\n var maxDistance;\r\n var vx;\r\n var vy;\r\n var vz;\r\n var v1x;\r\n var v1y;\r\n var v1z;\r\n var v2x;\r\n var v2y;\r\n var v2z;\r\n //vertex1;\r\n v1x=V1[0]; v1y=V1[1]; v1z=V1[2];\r\n maxDistance=nx*v1x+ny*v1y+nz*v1z;\r\n //vertex2;\r\n vx=V1[3]; vy=V1[4]; vz=V1[5];\r\n distance=nx*vx+ny*vy+nz*vz;\r\n if(distance>maxDistance){\r\n maxDistance=distance;\r\n v1x=vx; v1y=vy; v1z=vz;\r\n }\r\n //vertex3;\r\n vx=V1[6]; vy=V1[7]; vz=V1[8];\r\n distance=nx*vx+ny*vy+nz*vz;\r\n if(distance>maxDistance){\r\n maxDistance=distance;\r\n v1x=vx; v1y=vy; v1z=vz;\r\n }\r\n //vertex4;\r\n vx=V1[9]; vy=V1[10]; vz=V1[11];\r\n distance=nx*vx+ny*vy+nz*vz;\r\n if(distance>maxDistance){\r\n maxDistance=distance;\r\n v1x=vx; v1y=vy; v1z=vz;\r\n }\r\n //vertex5;\r\n vx=V1[12]; vy=V1[13]; vz=V1[14];\r\n distance=nx*vx+ny*vy+nz*vz;\r\n if(distance>maxDistance){\r\n maxDistance=distance;\r\n v1x=vx; v1y=vy; v1z=vz;\r\n }\r\n //vertex6;\r\n vx=V1[15]; vy=V1[16]; vz=V1[17];\r\n distance=nx*vx+ny*vy+nz*vz;\r\n if(distance>maxDistance){\r\n maxDistance=distance;\r\n v1x=vx; v1y=vy; v1z=vz;\r\n }\r\n //vertex7;\r\n vx=V1[18]; vy=V1[19]; vz=V1[20];\r\n distance=nx*vx+ny*vy+nz*vz;\r\n if(distance>maxDistance){\r\n maxDistance=distance;\r\n v1x=vx; v1y=vy; v1z=vz;\r\n }\r\n //vertex8;\r\n vx=V1[21]; vy=V1[22]; vz=V1[23];\r\n distance=nx*vx+ny*vy+nz*vz;\r\n if(distance>maxDistance){\r\n maxDistance=distance;\r\n v1x=vx; v1y=vy; v1z=vz;\r\n }\r\n //vertex1;\r\n v2x=V2[0]; v2y=V2[1]; v2z=V2[2];\r\n maxDistance=nx*v2x+ny*v2y+nz*v2z;\r\n //vertex2;\r\n vx=V2[3]; vy=V2[4]; vz=V2[5];\r\n distance=nx*vx+ny*vy+nz*vz;\r\n if(distance0){\r\n if(dot2>0){\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n this.clipVertices2[index]=x2;\r\n this.clipVertices2[index+1]=y2;\r\n this.clipVertices2[index+2]=z2;\r\n }else{\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n t=dot1/(dot1-dot2);\r\n this.clipVertices2[index]=x1+(x2-x1)*t;\r\n this.clipVertices2[index+1]=y1+(y2-y1)*t;\r\n this.clipVertices2[index+2]=z1+(z2-z1)*t;\r\n }\r\n }else{\r\n if(dot2>0){\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n t=dot1/(dot1-dot2);\r\n this.clipVertices2[index]=x1+(x2-x1)*t;\r\n this.clipVertices2[index+1]=y1+(y2-y1)*t;\r\n this.clipVertices2[index+2]=z1+(z2-z1)*t;\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n this.clipVertices2[index]=x2;\r\n this.clipVertices2[index+1]=y2;\r\n this.clipVertices2[index+2]=z2;\r\n }\r\n }\r\n x1=x2;\r\n y1=y2;\r\n z1=z2;\r\n dot1=dot2;\r\n }\r\n\r\n numClipVertices=numAddedClipVertices;\r\n if(numClipVertices==0)return;\r\n numAddedClipVertices=0;\r\n index=(numClipVertices-1)*3;\r\n x1=this.clipVertices2[index];\r\n y1=this.clipVertices2[index+1];\r\n z1=this.clipVertices2[index+2];\r\n dot1=(x1-cx-s2x)*n2x+(y1-cy-s2y)*n2y+(z1-cz-s2z)*n2z;\r\n\r\n //i = numClipVertices;\r\n //while(i--){\r\n for(i=0;i0){\r\n if(dot2>0){\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n this.clipVertices1[index]=x2;\r\n this.clipVertices1[index+1]=y2;\r\n this.clipVertices1[index+2]=z2;\r\n }else{\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n t=dot1/(dot1-dot2);\r\n this.clipVertices1[index]=x1+(x2-x1)*t;\r\n this.clipVertices1[index+1]=y1+(y2-y1)*t;\r\n this.clipVertices1[index+2]=z1+(z2-z1)*t;\r\n }\r\n }else{\r\n if(dot2>0){\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n t=dot1/(dot1-dot2);\r\n this.clipVertices1[index]=x1+(x2-x1)*t;\r\n this.clipVertices1[index+1]=y1+(y2-y1)*t;\r\n this.clipVertices1[index+2]=z1+(z2-z1)*t;\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n this.clipVertices1[index]=x2;\r\n this.clipVertices1[index+1]=y2;\r\n this.clipVertices1[index+2]=z2;\r\n }\r\n }\r\n x1=x2;\r\n y1=y2;\r\n z1=z2;\r\n dot1=dot2;\r\n }\r\n\r\n numClipVertices=numAddedClipVertices;\r\n if(numClipVertices==0)return;\r\n numAddedClipVertices=0;\r\n index=(numClipVertices-1)*3;\r\n x1=this.clipVertices1[index];\r\n y1=this.clipVertices1[index+1];\r\n z1=this.clipVertices1[index+2];\r\n dot1=(x1-cx+s1x)*-n1x+(y1-cy+s1y)*-n1y+(z1-cz+s1z)*-n1z;\r\n\r\n //i = numClipVertices;\r\n //while(i--){\r\n for(i=0;i0){\r\n if(dot2>0){\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n this.clipVertices2[index]=x2;\r\n this.clipVertices2[index+1]=y2;\r\n this.clipVertices2[index+2]=z2;\r\n }else{\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n t=dot1/(dot1-dot2);\r\n this.clipVertices2[index]=x1+(x2-x1)*t;\r\n this.clipVertices2[index+1]=y1+(y2-y1)*t;\r\n this.clipVertices2[index+2]=z1+(z2-z1)*t;\r\n }\r\n }else{\r\n if(dot2>0){\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n t=dot1/(dot1-dot2);\r\n this.clipVertices2[index]=x1+(x2-x1)*t;\r\n this.clipVertices2[index+1]=y1+(y2-y1)*t;\r\n this.clipVertices2[index+2]=z1+(z2-z1)*t;\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n this.clipVertices2[index]=x2;\r\n this.clipVertices2[index+1]=y2;\r\n this.clipVertices2[index+2]=z2;\r\n }\r\n }\r\n x1=x2;\r\n y1=y2;\r\n z1=z2;\r\n dot1=dot2;\r\n }\r\n\r\n numClipVertices=numAddedClipVertices;\r\n if(numClipVertices==0)return;\r\n numAddedClipVertices=0;\r\n index=(numClipVertices-1)*3;\r\n x1=this.clipVertices2[index];\r\n y1=this.clipVertices2[index+1];\r\n z1=this.clipVertices2[index+2];\r\n dot1=(x1-cx+s2x)*-n2x+(y1-cy+s2y)*-n2y+(z1-cz+s2z)*-n2z;\r\n\r\n //i = numClipVertices;\r\n //while(i--){\r\n for(i=0;i0){\r\n if(dot2>0){\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n this.clipVertices1[index]=x2;\r\n this.clipVertices1[index+1]=y2;\r\n this.clipVertices1[index+2]=z2;\r\n }else{\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n t=dot1/(dot1-dot2);\r\n this.clipVertices1[index]=x1+(x2-x1)*t;\r\n this.clipVertices1[index+1]=y1+(y2-y1)*t;\r\n this.clipVertices1[index+2]=z1+(z2-z1)*t;\r\n }\r\n }else{\r\n if(dot2>0){\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n t=dot1/(dot1-dot2);\r\n this.clipVertices1[index]=x1+(x2-x1)*t;\r\n this.clipVertices1[index+1]=y1+(y2-y1)*t;\r\n this.clipVertices1[index+2]=z1+(z2-z1)*t;\r\n index=numAddedClipVertices*3;\r\n numAddedClipVertices++;\r\n this.clipVertices1[index]=x2;\r\n this.clipVertices1[index+1]=y2;\r\n this.clipVertices1[index+2]=z2;\r\n }\r\n }\r\n x1=x2;\r\n y1=y2;\r\n z1=z2;\r\n dot1=dot2;\r\n }\r\n\r\n numClipVertices=numAddedClipVertices;\r\n if(swap){\r\n var tb=b1;\r\n b1=b2;\r\n b2=tb;\r\n }\r\n if(numClipVertices==0)return;\r\n var flipped=b1!=shape1;\r\n if(numClipVertices>4){\r\n x1=(q1x+q2x+q3x+q4x)*0.25;\r\n y1=(q1y+q2y+q3y+q4y)*0.25;\r\n z1=(q1z+q2z+q3z+q4z)*0.25;\r\n n1x=q1x-x1;\r\n n1y=q1y-y1;\r\n n1z=q1z-z1;\r\n n2x=q2x-x1;\r\n n2y=q2y-y1;\r\n n2z=q2z-z1;\r\n var index1=0;\r\n var index2=0;\r\n var index3=0;\r\n var index4=0;\r\n var maxDot=-this.INF;\r\n minDot=this.INF;\r\n\r\n //i = numClipVertices;\r\n //while(i--){\r\n for(i=0;imaxDot){\r\n maxDot=dot;\r\n index3=i;\r\n }\r\n }\r\n\r\n this.used[index1]=true;\r\n this.used[index3]=true;\r\n maxDot=-this.INF;\r\n minDot=this.INF;\r\n\r\n //i = numClipVertices;\r\n //while(i--){\r\n for(i=0;imaxDot){\r\n maxDot=dot;\r\n index4=i;\r\n }\r\n }\r\n\r\n index=index1*3;\r\n x1=this.clipVertices1[index];\r\n y1=this.clipVertices1[index+1];\r\n z1=this.clipVertices1[index+2];\r\n dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;\r\n if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);\r\n \r\n index=index2*3;\r\n x1=this.clipVertices1[index];\r\n y1=this.clipVertices1[index+1];\r\n z1=this.clipVertices1[index+2];\r\n dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;\r\n if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);\r\n \r\n index=index3*3;\r\n x1=this.clipVertices1[index];\r\n y1=this.clipVertices1[index+1];\r\n z1=this.clipVertices1[index+2];\r\n dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;\r\n if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);\r\n \r\n index=index4*3;\r\n x1=this.clipVertices1[index];\r\n y1=this.clipVertices1[index+1];\r\n z1=this.clipVertices1[index+2];\r\n dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz;\r\n if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped);\r\n \r\n }else{\r\n //i = numClipVertices;\r\n //while(i--){\r\n for(i=0;i0){\r\n t1x=v1x;\r\n t1y=v1y;\r\n t1z=v1z;\r\n v1x=v2x;\r\n v1y=v2y;\r\n v1z=v2z;\r\n v2x=t1x;\r\n v2y=t1y;\r\n v2z=t1z;\r\n t1x=v11x;\r\n t1y=v11y;\r\n t1z=v11z;\r\n v11x=v21x;\r\n v11y=v21y;\r\n v11z=v21z;\r\n v21x=t1x;\r\n v21y=t1y;\r\n v21z=t1z;\r\n t1x=v12x;\r\n t1y=v12y;\r\n t1z=v12z;\r\n v12x=v22x;\r\n v12y=v22y;\r\n v12z=v22z;\r\n v22x=t1x;\r\n v22y=t1y;\r\n v22z=t1z;\r\n nx=-nx;\r\n ny=-ny;\r\n nz=-nz;\r\n }\r\n var iterations=0;\r\n while(true){\r\n if(++iterations>100){\r\n return false;\r\n }\r\n this.supportPointB(c1,-nx,-ny,-nz,sup);\r\n var v31x=sup.x;\r\n var v31y=sup.y;\r\n var v31z=sup.z;\r\n this.supportPointC(c2,nx,ny,nz,sup);\r\n var v32x=sup.x;\r\n var v32y=sup.y;\r\n var v32z=sup.z;\r\n var v3x=v32x-v31x;\r\n var v3y=v32y-v31y;\r\n var v3z=v32z-v31z;\r\n if(v3x*nx+v3y*ny+v3z*nz<=0){\r\n return false;\r\n }\r\n if((v1y*v3z-v1z*v3y)*v0x+(v1z*v3x-v1x*v3z)*v0y+(v1x*v3y-v1y*v3x)*v0z<0){\r\n v2x=v3x;\r\n v2y=v3y;\r\n v2z=v3z;\r\n v21x=v31x;\r\n v21y=v31y;\r\n v21z=v31z;\r\n v22x=v32x;\r\n v22y=v32y;\r\n v22z=v32z;\r\n t1x=v1x-v0x;\r\n t1y=v1y-v0y;\r\n t1z=v1z-v0z;\r\n t2x=v3x-v0x;\r\n t2y=v3y-v0y;\r\n t2z=v3z-v0z;\r\n nx=t1y*t2z-t1z*t2y;\r\n ny=t1z*t2x-t1x*t2z;\r\n nz=t1x*t2y-t1y*t2x;\r\n continue;\r\n }\r\n if((v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z<0){\r\n v1x=v3x;\r\n v1y=v3y;\r\n v1z=v3z;\r\n v11x=v31x;\r\n v11y=v31y;\r\n v11z=v31z;\r\n v12x=v32x;\r\n v12y=v32y;\r\n v12z=v32z;\r\n t1x=v3x-v0x;\r\n t1y=v3y-v0y;\r\n t1z=v3z-v0z;\r\n t2x=v2x-v0x;\r\n t2y=v2y-v0y;\r\n t2z=v2z-v0z;\r\n nx=t1y*t2z-t1z*t2y;\r\n ny=t1z*t2x-t1x*t2z;\r\n nz=t1x*t2y-t1y*t2x;\r\n continue;\r\n }\r\n var hit=false;\r\n while(true){\r\n t1x=v2x-v1x;\r\n t1y=v2y-v1y;\r\n t1z=v2z-v1z;\r\n t2x=v3x-v1x;\r\n t2y=v3y-v1y;\r\n t2z=v3z-v1z;\r\n nx=t1y*t2z-t1z*t2y;\r\n ny=t1z*t2x-t1x*t2z;\r\n nz=t1x*t2y-t1y*t2x;\r\n len=1/_Math.sqrt(nx*nx+ny*ny+nz*nz);\r\n nx*=len;\r\n ny*=len;\r\n nz*=len;\r\n if(nx*v1x+ny*v1y+nz*v1z>=0&&!hit){\r\n var b0=(v1y*v2z-v1z*v2y)*v3x+(v1z*v2x-v1x*v2z)*v3y+(v1x*v2y-v1y*v2x)*v3z;\r\n var b1=(v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z;\r\n var b2=(v0y*v1z-v0z*v1y)*v3x+(v0z*v1x-v0x*v1z)*v3y+(v0x*v1y-v0y*v1x)*v3z;\r\n var b3=(v2y*v1z-v2z*v1y)*v0x+(v2z*v1x-v2x*v1z)*v0y+(v2x*v1y-v2y*v1x)*v0z;\r\n var sum=b0+b1+b2+b3;\r\n if(sum<=0){\r\n b0=0;\r\n b1=(v2y*v3z-v2z*v3y)*nx+(v2z*v3x-v2x*v3z)*ny+(v2x*v3y-v2y*v3x)*nz;\r\n b2=(v3y*v2z-v3z*v2y)*nx+(v3z*v2x-v3x*v2z)*ny+(v3x*v2y-v3y*v2x)*nz;\r\n b3=(v1y*v2z-v1z*v2y)*nx+(v1z*v2x-v1x*v2z)*ny+(v1x*v2y-v1y*v2x)*nz;\r\n sum=b1+b2+b3;\r\n }\r\n var inv=1/sum;\r\n p1x=(v01x*b0+v11x*b1+v21x*b2+v31x*b3)*inv;\r\n p1y=(v01y*b0+v11y*b1+v21y*b2+v31y*b3)*inv;\r\n p1z=(v01z*b0+v11z*b1+v21z*b2+v31z*b3)*inv;\r\n p2x=(v02x*b0+v12x*b1+v22x*b2+v32x*b3)*inv;\r\n p2y=(v02y*b0+v12y*b1+v22y*b2+v32y*b3)*inv;\r\n p2z=(v02z*b0+v12z*b1+v22z*b2+v32z*b3)*inv;\r\n hit=true;\r\n }\r\n this.supportPointB(c1,-nx,-ny,-nz,sup);\r\n var v41x=sup.x;\r\n var v41y=sup.y;\r\n var v41z=sup.z;\r\n this.supportPointC(c2,nx,ny,nz,sup);\r\n var v42x=sup.x;\r\n var v42y=sup.y;\r\n var v42z=sup.z;\r\n var v4x=v42x-v41x;\r\n var v4y=v42y-v41y;\r\n var v4z=v42z-v41z;\r\n var separation=-(v4x*nx+v4y*ny+v4z*nz);\r\n if((v4x-v3x)*nx+(v4y-v3y)*ny+(v4z-v3z)*nz<=0.01||separation>=0){\r\n if(hit){\r\n sep.set( -nx, -ny, -nz );\r\n pos.set( (p1x+p2x)*0.5, (p1y+p2y)*0.5, (p1z+p2z)*0.5 );\r\n dep.x=separation;\r\n return true;\r\n }\r\n return false;\r\n }\r\n if(\r\n (v4y*v1z-v4z*v1y)*v0x+\r\n (v4z*v1x-v4x*v1z)*v0y+\r\n (v4x*v1y-v4y*v1x)*v0z<0\r\n ){\r\n if(\r\n (v4y*v2z-v4z*v2y)*v0x+\r\n (v4z*v2x-v4x*v2z)*v0y+\r\n (v4x*v2y-v4y*v2x)*v0z<0\r\n ){\r\n v1x=v4x;\r\n v1y=v4y;\r\n v1z=v4z;\r\n v11x=v41x;\r\n v11y=v41y;\r\n v11z=v41z;\r\n v12x=v42x;\r\n v12y=v42y;\r\n v12z=v42z;\r\n }else{\r\n v3x=v4x;\r\n v3y=v4y;\r\n v3z=v4z;\r\n v31x=v41x;\r\n v31y=v41y;\r\n v31z=v41z;\r\n v32x=v42x;\r\n v32y=v42y;\r\n v32z=v42z;\r\n }\r\n }else{\r\n if(\r\n (v4y*v3z-v4z*v3y)*v0x+\r\n (v4z*v3x-v4x*v3z)*v0y+\r\n (v4x*v3y-v4y*v3x)*v0z<0\r\n ){\r\n v2x=v4x;\r\n v2y=v4y;\r\n v2z=v4z;\r\n v21x=v41x;\r\n v21y=v41y;\r\n v21z=v41z;\r\n v22x=v42x;\r\n v22y=v42y;\r\n v22z=v42z;\r\n }else{\r\n v1x=v4x;\r\n v1y=v4y;\r\n v1z=v4z;\r\n v11x=v41x;\r\n v11y=v41y;\r\n v11z=v41z;\r\n v12x=v42x;\r\n v12y=v42y;\r\n v12z=v42z;\r\n }\r\n }\r\n }\r\n }\r\n //return false;\r\n },\r\n\r\n supportPointB: function( c, dx, dy, dz, out ) {\r\n\r\n var rot=c.rotation.elements;\r\n var ldx=rot[0]*dx+rot[3]*dy+rot[6]*dz;\r\n var ldy=rot[1]*dx+rot[4]*dy+rot[7]*dz;\r\n var ldz=rot[2]*dx+rot[5]*dy+rot[8]*dz;\r\n var w=c.halfWidth;\r\n var h=c.halfHeight;\r\n var d=c.halfDepth;\r\n var ox;\r\n var oy;\r\n var oz;\r\n if(ldx<0)ox=-w;\r\n else ox=w;\r\n if(ldy<0)oy=-h;\r\n else oy=h;\r\n if(ldz<0)oz=-d;\r\n else oz=d;\r\n ldx=rot[0]*ox+rot[1]*oy+rot[2]*oz+c.position.x;\r\n ldy=rot[3]*ox+rot[4]*oy+rot[5]*oz+c.position.y;\r\n ldz=rot[6]*ox+rot[7]*oy+rot[8]*oz+c.position.z;\r\n out.set( ldx, ldy, ldz );\r\n\r\n },\r\n\r\n supportPointC: function ( c, dx, dy, dz, out ) {\r\n\r\n var rot=c.rotation.elements;\r\n var ldx=rot[0]*dx+rot[3]*dy+rot[6]*dz;\r\n var ldy=rot[1]*dx+rot[4]*dy+rot[7]*dz;\r\n var ldz=rot[2]*dx+rot[5]*dy+rot[8]*dz;\r\n var radx=ldx;\r\n var radz=ldz;\r\n var len=radx*radx+radz*radz;\r\n var rad=c.radius;\r\n var hh=c.halfHeight;\r\n var ox;\r\n var oy;\r\n var oz;\r\n if(len==0){\r\n if(ldy<0){\r\n ox=rad;\r\n oy=-hh;\r\n oz=0;\r\n }else{\r\n ox=rad;\r\n oy=hh;\r\n oz=0;\r\n }\r\n }else{\r\n len=c.radius/_Math.sqrt(len);\r\n if(ldy<0){\r\n ox=radx*len;\r\n oy=-hh;\r\n oz=radz*len;\r\n }else{\r\n ox=radx*len;\r\n oy=hh;\r\n oz=radz*len;\r\n }\r\n }\r\n ldx=rot[0]*ox+rot[1]*oy+rot[2]*oz+c.position.x;\r\n ldy=rot[3]*ox+rot[4]*oy+rot[5]*oz+c.position.y;\r\n ldz=rot[6]*ox+rot[7]*oy+rot[8]*oz+c.position.z;\r\n out.set( ldx, ldy, ldz );\r\n\r\n },\r\n\r\n detectCollision: function( shape1, shape2, manifold ) {\r\n\r\n var b;\r\n var c;\r\n if(this.flip){\r\n b=shape2;\r\n c=shape1;\r\n }else{\r\n b=shape1;\r\n c=shape2;\r\n }\r\n var sep=new Vec3();\r\n var pos=new Vec3();\r\n var dep=new Vec3();\r\n\r\n if(!this.getSep(b,c,sep,pos,dep))return;\r\n var pbx=b.position.x;\r\n var pby=b.position.y;\r\n var pbz=b.position.z;\r\n var pcx=c.position.x;\r\n var pcy=c.position.y;\r\n var pcz=c.position.z;\r\n var bw=b.halfWidth;\r\n var bh=b.halfHeight;\r\n var bd=b.halfDepth;\r\n var ch=c.halfHeight;\r\n var r=c.radius;\r\n\r\n var D = b.dimentions;\r\n\r\n var nwx=D[0];//b.normalDirectionWidth.x;\r\n var nwy=D[1];//b.normalDirectionWidth.y;\r\n var nwz=D[2];//b.normalDirectionWidth.z;\r\n var nhx=D[3];//b.normalDirectionHeight.x;\r\n var nhy=D[4];//b.normalDirectionHeight.y;\r\n var nhz=D[5];//b.normalDirectionHeight.z;\r\n var ndx=D[6];//b.normalDirectionDepth.x;\r\n var ndy=D[7];//b.normalDirectionDepth.y;\r\n var ndz=D[8];//b.normalDirectionDepth.z;\r\n\r\n var dwx=D[9];//b.halfDirectionWidth.x;\r\n var dwy=D[10];//b.halfDirectionWidth.y;\r\n var dwz=D[11];//b.halfDirectionWidth.z;\r\n var dhx=D[12];//b.halfDirectionHeight.x;\r\n var dhy=D[13];//b.halfDirectionHeight.y;\r\n var dhz=D[14];//b.halfDirectionHeight.z;\r\n var ddx=D[15];//b.halfDirectionDepth.x;\r\n var ddy=D[16];//b.halfDirectionDepth.y;\r\n var ddz=D[17];//b.halfDirectionDepth.z;\r\n\r\n var ncx=c.normalDirection.x;\r\n var ncy=c.normalDirection.y;\r\n var ncz=c.normalDirection.z;\r\n var dcx=c.halfDirection.x;\r\n var dcy=c.halfDirection.y;\r\n var dcz=c.halfDirection.z;\r\n var nx=sep.x;\r\n var ny=sep.y;\r\n var nz=sep.z;\r\n var dotw=nx*nwx+ny*nwy+nz*nwz;\r\n var doth=nx*nhx+ny*nhy+nz*nhz;\r\n var dotd=nx*ndx+ny*ndy+nz*ndz;\r\n var dotc=nx*ncx+ny*ncy+nz*ncz;\r\n var right1=dotw>0;\r\n var right2=doth>0;\r\n var right3=dotd>0;\r\n var right4=dotc>0;\r\n if(!right1)dotw=-dotw;\r\n if(!right2)doth=-doth;\r\n if(!right3)dotd=-dotd;\r\n if(!right4)dotc=-dotc;\r\n var state=0;\r\n if(dotc>0.999){\r\n if(dotw>0.999){\r\n if(dotw>dotc)state=1;\r\n else state=4;\r\n }else if(doth>0.999){\r\n if(doth>dotc)state=2;\r\n else state=4;\r\n }else if(dotd>0.999){\r\n if(dotd>dotc)state=3;\r\n else state=4;\r\n }else state=4;\r\n }else{\r\n if(dotw>0.999)state=1;\r\n else if(doth>0.999)state=2;\r\n else if(dotd>0.999)state=3;\r\n }\r\n var cbx;\r\n var cby;\r\n var cbz;\r\n var ccx;\r\n var ccy;\r\n var ccz;\r\n var r00;\r\n var r01;\r\n var r02;\r\n var r10;\r\n var r11;\r\n var r12;\r\n var r20;\r\n var r21;\r\n var r22;\r\n var px;\r\n var py;\r\n var pz;\r\n var pd;\r\n var dot;\r\n var len;\r\n var tx;\r\n var ty;\r\n var tz;\r\n var td;\r\n var dx;\r\n var dy;\r\n var dz;\r\n var d1x;\r\n var d1y;\r\n var d1z;\r\n var d2x;\r\n var d2y;\r\n var d2z;\r\n var sx;\r\n var sy;\r\n var sz;\r\n var sd;\r\n var ex;\r\n var ey;\r\n var ez;\r\n var ed;\r\n var dot1;\r\n var dot2;\r\n var t1;\r\n var dir1x;\r\n var dir1y;\r\n var dir1z;\r\n var dir2x;\r\n var dir2y;\r\n var dir2z;\r\n var dir1l;\r\n var dir2l;\r\n if(state==0){\r\n //manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,dep.x,b,c,0,0,false);\r\n manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,dep.x,this.flip);\r\n }else if(state==4){\r\n if(right4){\r\n ccx=pcx-dcx;\r\n ccy=pcy-dcy;\r\n ccz=pcz-dcz;\r\n nx=-ncx;\r\n ny=-ncy;\r\n nz=-ncz;\r\n }else{\r\n ccx=pcx+dcx;\r\n ccy=pcy+dcy;\r\n ccz=pcz+dcz;\r\n nx=ncx;\r\n ny=ncy;\r\n nz=ncz;\r\n }\r\n var v1x;\r\n var v1y;\r\n var v1z;\r\n var v2x;\r\n var v2y;\r\n var v2z;\r\n var v3x;\r\n var v3y;\r\n var v3z;\r\n var v4x;\r\n var v4y;\r\n var v4z;\r\n \r\n dot=1;\r\n state=0;\r\n dot1=nwx*nx+nwy*ny+nwz*nz;\r\n if(dot1=0.999999){\r\n tx=-ny;\r\n ty=nz;\r\n tz=nx;\r\n }else{\r\n tx=nx;\r\n ty=ny;\r\n tz=nz;\r\n }\r\n len=tx*ncx+ty*ncy+tz*ncz;\r\n dx=len*ncx-tx;\r\n dy=len*ncy-ty;\r\n dz=len*ncz-tz;\r\n len=_Math.sqrt(dx*dx+dy*dy+dz*dz);\r\n if(len==0)return;\r\n len=r/len;\r\n dx*=len;\r\n dy*=len;\r\n dz*=len;\r\n tx=ccx+dx;\r\n ty=ccy+dy;\r\n tz=ccz+dz;\r\n if(dot<-0.96||dot>0.96){\r\n r00=ncx*ncx*1.5-0.5;\r\n r01=ncx*ncy*1.5-ncz*0.866025403;\r\n r02=ncx*ncz*1.5+ncy*0.866025403;\r\n r10=ncy*ncx*1.5+ncz*0.866025403;\r\n r11=ncy*ncy*1.5-0.5;\r\n r12=ncy*ncz*1.5-ncx*0.866025403;\r\n r20=ncz*ncx*1.5-ncy*0.866025403;\r\n r21=ncz*ncy*1.5+ncx*0.866025403;\r\n r22=ncz*ncz*1.5-0.5;\r\n px=tx;\r\n py=ty;\r\n pz=tz;\r\n pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz);\r\n tx=px-pd*nx-cbx;\r\n ty=py-pd*ny-cby;\r\n tz=pz-pd*nz-cbz;\r\n sd=dir1x*tx+dir1y*ty+dir1z*tz;\r\n ed=dir2x*tx+dir2y*ty+dir2z*tz;\r\n if(sd<-dir1l)sd=-dir1l;\r\n else if(sd>dir1l)sd=dir1l;\r\n if(ed<-dir2l)ed=-dir2l;\r\n else if(ed>dir2l)ed=dir2l;\r\n tx=sd*dir1x+ed*dir2x;\r\n ty=sd*dir1y+ed*dir2y;\r\n tz=sd*dir1z+ed*dir2z;\r\n px=cbx+tx;\r\n py=cby+ty;\r\n pz=cbz+tz;\r\n manifold.addPoint(px,py,pz,nx,ny,nz,pd,this.flip);\r\n px=dx*r00+dy*r01+dz*r02;\r\n py=dx*r10+dy*r11+dz*r12;\r\n pz=dx*r20+dy*r21+dz*r22;\r\n px=(dx=px)+ccx;\r\n py=(dy=py)+ccy;\r\n pz=(dz=pz)+ccz;\r\n pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz);\r\n if(pd<=0){\r\n tx=px-pd*nx-cbx;\r\n ty=py-pd*ny-cby;\r\n tz=pz-pd*nz-cbz;\r\n sd=dir1x*tx+dir1y*ty+dir1z*tz;\r\n ed=dir2x*tx+dir2y*ty+dir2z*tz;\r\n if(sd<-dir1l)sd=-dir1l;\r\n else if(sd>dir1l)sd=dir1l;\r\n if(ed<-dir2l)ed=-dir2l;\r\n else if(ed>dir2l)ed=dir2l;\r\n tx=sd*dir1x+ed*dir2x;\r\n ty=sd*dir1y+ed*dir2y;\r\n tz=sd*dir1z+ed*dir2z;\r\n px=cbx+tx;\r\n py=cby+ty;\r\n pz=cbz+tz;\r\n //manifold.addPoint(px,py,pz,nx,ny,nz,pd,b,c,2,0,false);\r\n manifold.addPoint(px,py,pz,nx,ny,nz,pd,this.flip);\r\n }\r\n px=dx*r00+dy*r01+dz*r02;\r\n py=dx*r10+dy*r11+dz*r12;\r\n pz=dx*r20+dy*r21+dz*r22;\r\n px=(dx=px)+ccx;\r\n py=(dy=py)+ccy;\r\n pz=(dz=pz)+ccz;\r\n pd=nx*(px-cbx)+ny*(py-cby)+nz*(pz-cbz);\r\n if(pd<=0){\r\n tx=px-pd*nx-cbx;\r\n ty=py-pd*ny-cby;\r\n tz=pz-pd*nz-cbz;\r\n sd=dir1x*tx+dir1y*ty+dir1z*tz;\r\n ed=dir2x*tx+dir2y*ty+dir2z*tz;\r\n if(sd<-dir1l)sd=-dir1l;\r\n else if(sd>dir1l)sd=dir1l;\r\n if(ed<-dir2l)ed=-dir2l;\r\n else if(ed>dir2l)ed=dir2l;\r\n tx=sd*dir1x+ed*dir2x;\r\n ty=sd*dir1y+ed*dir2y;\r\n tz=sd*dir1z+ed*dir2z;\r\n px=cbx+tx;\r\n py=cby+ty;\r\n pz=cbz+tz;\r\n //manifold.addPoint(px,py,pz,nx,ny,nz,pd,b,c,3,0,false);\r\n manifold.addPoint(px,py,pz,nx,ny,nz,pd,this.flip);\r\n }\r\n }else{\r\n sx=tx;\r\n sy=ty;\r\n sz=tz;\r\n sd=nx*(sx-cbx)+ny*(sy-cby)+nz*(sz-cbz);\r\n sx-=sd*nx;\r\n sy-=sd*ny;\r\n sz-=sd*nz;\r\n if(dot>0){\r\n ex=tx+dcx*2;\r\n ey=ty+dcy*2;\r\n ez=tz+dcz*2;\r\n }else{\r\n ex=tx-dcx*2;\r\n ey=ty-dcy*2;\r\n ez=tz-dcz*2;\r\n }\r\n ed=nx*(ex-cbx)+ny*(ey-cby)+nz*(ez-cbz);\r\n ex-=ed*nx;\r\n ey-=ed*ny;\r\n ez-=ed*nz;\r\n d1x=sx-cbx;\r\n d1y=sy-cby;\r\n d1z=sz-cbz;\r\n d2x=ex-cbx;\r\n d2y=ey-cby;\r\n d2z=ez-cbz;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n td=ed-sd;\r\n dotw=d1x*dir1x+d1y*dir1y+d1z*dir1z;\r\n doth=d2x*dir1x+d2y*dir1y+d2z*dir1z;\r\n dot1=dotw-dir1l;\r\n dot2=doth-dir1l;\r\n if(dot1>0){\r\n if(dot2>0)return;\r\n t1=dot1/(dot1-dot2);\r\n sx=sx+tx*t1;\r\n sy=sy+ty*t1;\r\n sz=sz+tz*t1;\r\n sd=sd+td*t1;\r\n d1x=sx-cbx;\r\n d1y=sy-cby;\r\n d1z=sz-cbz;\r\n dotw=d1x*dir1x+d1y*dir1y+d1z*dir1z;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n td=ed-sd;\r\n }else if(dot2>0){\r\n t1=dot1/(dot1-dot2);\r\n ex=sx+tx*t1;\r\n ey=sy+ty*t1;\r\n ez=sz+tz*t1;\r\n ed=sd+td*t1;\r\n d2x=ex-cbx;\r\n d2y=ey-cby;\r\n d2z=ez-cbz;\r\n doth=d2x*dir1x+d2y*dir1y+d2z*dir1z;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n td=ed-sd;\r\n }\r\n dot1=dotw+dir1l;\r\n dot2=doth+dir1l;\r\n if(dot1<0){\r\n if(dot2<0)return;\r\n t1=dot1/(dot1-dot2);\r\n sx=sx+tx*t1;\r\n sy=sy+ty*t1;\r\n sz=sz+tz*t1;\r\n sd=sd+td*t1;\r\n d1x=sx-cbx;\r\n d1y=sy-cby;\r\n d1z=sz-cbz;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n td=ed-sd;\r\n }else if(dot2<0){\r\n t1=dot1/(dot1-dot2);\r\n ex=sx+tx*t1;\r\n ey=sy+ty*t1;\r\n ez=sz+tz*t1;\r\n ed=sd+td*t1;\r\n d2x=ex-cbx;\r\n d2y=ey-cby;\r\n d2z=ez-cbz;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n td=ed-sd;\r\n }\r\n dotw=d1x*dir2x+d1y*dir2y+d1z*dir2z;\r\n doth=d2x*dir2x+d2y*dir2y+d2z*dir2z;\r\n dot1=dotw-dir2l;\r\n dot2=doth-dir2l;\r\n if(dot1>0){\r\n if(dot2>0)return;\r\n t1=dot1/(dot1-dot2);\r\n sx=sx+tx*t1;\r\n sy=sy+ty*t1;\r\n sz=sz+tz*t1;\r\n sd=sd+td*t1;\r\n d1x=sx-cbx;\r\n d1y=sy-cby;\r\n d1z=sz-cbz;\r\n dotw=d1x*dir2x+d1y*dir2y+d1z*dir2z;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n td=ed-sd;\r\n }else if(dot2>0){\r\n t1=dot1/(dot1-dot2);\r\n ex=sx+tx*t1;\r\n ey=sy+ty*t1;\r\n ez=sz+tz*t1;\r\n ed=sd+td*t1;\r\n d2x=ex-cbx;\r\n d2y=ey-cby;\r\n d2z=ez-cbz;\r\n doth=d2x*dir2x+d2y*dir2y+d2z*dir2z;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n td=ed-sd;\r\n }\r\n dot1=dotw+dir2l;\r\n dot2=doth+dir2l;\r\n if(dot1<0){\r\n if(dot2<0)return;\r\n t1=dot1/(dot1-dot2);\r\n sx=sx+tx*t1;\r\n sy=sy+ty*t1;\r\n sz=sz+tz*t1;\r\n sd=sd+td*t1;\r\n }else if(dot2<0){\r\n t1=dot1/(dot1-dot2);\r\n ex=sx+tx*t1;\r\n ey=sy+ty*t1;\r\n ez=sz+tz*t1;\r\n ed=sd+td*t1;\r\n }\r\n if(sd<0){\r\n //manifold.addPoint(sx,sy,sz,nx,ny,nz,sd,b,c,1,0,false);\r\n manifold.addPoint(sx,sy,sz,nx,ny,nz,sd,this.flip);\r\n }\r\n if(ed<0){\r\n //manifold.addPoint(ex,ey,ez,nx,ny,nz,ed,b,c,4,0,false);\r\n manifold.addPoint(ex,ey,ez,nx,ny,nz,ed,this.flip);\r\n }\r\n }\r\n }\r\n\r\n }\r\n\r\n });\n\nfunction CylinderCylinderCollisionDetector() {\r\n \r\n CollisionDetector.call( this );\r\n\r\n}\r\n\r\nCylinderCylinderCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), {\r\n\r\n constructor: CylinderCylinderCollisionDetector,\r\n\r\n\r\n getSep: function ( c1, c2, sep, pos, dep ) {\r\n\r\n var t1x;\r\n var t1y;\r\n var t1z;\r\n var t2x;\r\n var t2y;\r\n var t2z;\r\n var sup=new Vec3();\r\n var len;\r\n var p1x;\r\n var p1y;\r\n var p1z;\r\n var p2x;\r\n var p2y;\r\n var p2z;\r\n var v01x=c1.position.x;\r\n var v01y=c1.position.y;\r\n var v01z=c1.position.z;\r\n var v02x=c2.position.x;\r\n var v02y=c2.position.y;\r\n var v02z=c2.position.z;\r\n var v0x=v02x-v01x;\r\n var v0y=v02y-v01y;\r\n var v0z=v02z-v01z;\r\n if(v0x*v0x+v0y*v0y+v0z*v0z==0)v0y=0.001;\r\n var nx=-v0x;\r\n var ny=-v0y;\r\n var nz=-v0z;\r\n this.supportPoint(c1,-nx,-ny,-nz,sup);\r\n var v11x=sup.x;\r\n var v11y=sup.y;\r\n var v11z=sup.z;\r\n this.supportPoint(c2,nx,ny,nz,sup);\r\n var v12x=sup.x;\r\n var v12y=sup.y;\r\n var v12z=sup.z;\r\n var v1x=v12x-v11x;\r\n var v1y=v12y-v11y;\r\n var v1z=v12z-v11z;\r\n if(v1x*nx+v1y*ny+v1z*nz<=0){\r\n return false;\r\n }\r\n nx=v1y*v0z-v1z*v0y;\r\n ny=v1z*v0x-v1x*v0z;\r\n nz=v1x*v0y-v1y*v0x;\r\n if(nx*nx+ny*ny+nz*nz==0){\r\n sep.set( v1x-v0x, v1y-v0y, v1z-v0z ).normalize();\r\n pos.set( (v11x+v12x)*0.5, (v11y+v12y)*0.5, (v11z+v12z)*0.5 );\r\n return true;\r\n }\r\n this.supportPoint(c1,-nx,-ny,-nz,sup);\r\n var v21x=sup.x;\r\n var v21y=sup.y;\r\n var v21z=sup.z;\r\n this.supportPoint(c2,nx,ny,nz,sup);\r\n var v22x=sup.x;\r\n var v22y=sup.y;\r\n var v22z=sup.z;\r\n var v2x=v22x-v21x;\r\n var v2y=v22y-v21y;\r\n var v2z=v22z-v21z;\r\n if(v2x*nx+v2y*ny+v2z*nz<=0){\r\n return false;\r\n }\r\n t1x=v1x-v0x;\r\n t1y=v1y-v0y;\r\n t1z=v1z-v0z;\r\n t2x=v2x-v0x;\r\n t2y=v2y-v0y;\r\n t2z=v2z-v0z;\r\n nx=t1y*t2z-t1z*t2y;\r\n ny=t1z*t2x-t1x*t2z;\r\n nz=t1x*t2y-t1y*t2x;\r\n if(nx*v0x+ny*v0y+nz*v0z>0){\r\n t1x=v1x;\r\n t1y=v1y;\r\n t1z=v1z;\r\n v1x=v2x;\r\n v1y=v2y;\r\n v1z=v2z;\r\n v2x=t1x;\r\n v2y=t1y;\r\n v2z=t1z;\r\n t1x=v11x;\r\n t1y=v11y;\r\n t1z=v11z;\r\n v11x=v21x;\r\n v11y=v21y;\r\n v11z=v21z;\r\n v21x=t1x;\r\n v21y=t1y;\r\n v21z=t1z;\r\n t1x=v12x;\r\n t1y=v12y;\r\n t1z=v12z;\r\n v12x=v22x;\r\n v12y=v22y;\r\n v12z=v22z;\r\n v22x=t1x;\r\n v22y=t1y;\r\n v22z=t1z;\r\n nx=-nx;\r\n ny=-ny;\r\n nz=-nz;\r\n }\r\n var iterations=0;\r\n while(true){\r\n if(++iterations>100){\r\n return false;\r\n }\r\n this.supportPoint(c1,-nx,-ny,-nz,sup);\r\n var v31x=sup.x;\r\n var v31y=sup.y;\r\n var v31z=sup.z;\r\n this.supportPoint(c2,nx,ny,nz,sup);\r\n var v32x=sup.x;\r\n var v32y=sup.y;\r\n var v32z=sup.z;\r\n var v3x=v32x-v31x;\r\n var v3y=v32y-v31y;\r\n var v3z=v32z-v31z;\r\n if(v3x*nx+v3y*ny+v3z*nz<=0){\r\n return false;\r\n }\r\n if((v1y*v3z-v1z*v3y)*v0x+(v1z*v3x-v1x*v3z)*v0y+(v1x*v3y-v1y*v3x)*v0z<0){\r\n v2x=v3x;\r\n v2y=v3y;\r\n v2z=v3z;\r\n v21x=v31x;\r\n v21y=v31y;\r\n v21z=v31z;\r\n v22x=v32x;\r\n v22y=v32y;\r\n v22z=v32z;\r\n t1x=v1x-v0x;\r\n t1y=v1y-v0y;\r\n t1z=v1z-v0z;\r\n t2x=v3x-v0x;\r\n t2y=v3y-v0y;\r\n t2z=v3z-v0z;\r\n nx=t1y*t2z-t1z*t2y;\r\n ny=t1z*t2x-t1x*t2z;\r\n nz=t1x*t2y-t1y*t2x;\r\n continue;\r\n }\r\n if((v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z<0){\r\n v1x=v3x;\r\n v1y=v3y;\r\n v1z=v3z;\r\n v11x=v31x;\r\n v11y=v31y;\r\n v11z=v31z;\r\n v12x=v32x;\r\n v12y=v32y;\r\n v12z=v32z;\r\n t1x=v3x-v0x;\r\n t1y=v3y-v0y;\r\n t1z=v3z-v0z;\r\n t2x=v2x-v0x;\r\n t2y=v2y-v0y;\r\n t2z=v2z-v0z;\r\n nx=t1y*t2z-t1z*t2y;\r\n ny=t1z*t2x-t1x*t2z;\r\n nz=t1x*t2y-t1y*t2x;\r\n continue;\r\n }\r\n var hit=false;\r\n while(true){\r\n t1x=v2x-v1x;\r\n t1y=v2y-v1y;\r\n t1z=v2z-v1z;\r\n t2x=v3x-v1x;\r\n t2y=v3y-v1y;\r\n t2z=v3z-v1z;\r\n nx=t1y*t2z-t1z*t2y;\r\n ny=t1z*t2x-t1x*t2z;\r\n nz=t1x*t2y-t1y*t2x;\r\n len=1/_Math.sqrt(nx*nx+ny*ny+nz*nz);\r\n nx*=len;\r\n ny*=len;\r\n nz*=len;\r\n if(nx*v1x+ny*v1y+nz*v1z>=0&&!hit){\r\n var b0=(v1y*v2z-v1z*v2y)*v3x+(v1z*v2x-v1x*v2z)*v3y+(v1x*v2y-v1y*v2x)*v3z;\r\n var b1=(v3y*v2z-v3z*v2y)*v0x+(v3z*v2x-v3x*v2z)*v0y+(v3x*v2y-v3y*v2x)*v0z;\r\n var b2=(v0y*v1z-v0z*v1y)*v3x+(v0z*v1x-v0x*v1z)*v3y+(v0x*v1y-v0y*v1x)*v3z;\r\n var b3=(v2y*v1z-v2z*v1y)*v0x+(v2z*v1x-v2x*v1z)*v0y+(v2x*v1y-v2y*v1x)*v0z;\r\n var sum=b0+b1+b2+b3;\r\n if(sum<=0){\r\n b0=0;\r\n b1=(v2y*v3z-v2z*v3y)*nx+(v2z*v3x-v2x*v3z)*ny+(v2x*v3y-v2y*v3x)*nz;\r\n b2=(v3y*v2z-v3z*v2y)*nx+(v3z*v2x-v3x*v2z)*ny+(v3x*v2y-v3y*v2x)*nz;\r\n b3=(v1y*v2z-v1z*v2y)*nx+(v1z*v2x-v1x*v2z)*ny+(v1x*v2y-v1y*v2x)*nz;\r\n sum=b1+b2+b3;\r\n }\r\n var inv=1/sum;\r\n p1x=(v01x*b0+v11x*b1+v21x*b2+v31x*b3)*inv;\r\n p1y=(v01y*b0+v11y*b1+v21y*b2+v31y*b3)*inv;\r\n p1z=(v01z*b0+v11z*b1+v21z*b2+v31z*b3)*inv;\r\n p2x=(v02x*b0+v12x*b1+v22x*b2+v32x*b3)*inv;\r\n p2y=(v02y*b0+v12y*b1+v22y*b2+v32y*b3)*inv;\r\n p2z=(v02z*b0+v12z*b1+v22z*b2+v32z*b3)*inv;\r\n hit=true;\r\n }\r\n this.supportPoint(c1,-nx,-ny,-nz,sup);\r\n var v41x=sup.x;\r\n var v41y=sup.y;\r\n var v41z=sup.z;\r\n this.supportPoint(c2,nx,ny,nz,sup);\r\n var v42x=sup.x;\r\n var v42y=sup.y;\r\n var v42z=sup.z;\r\n var v4x=v42x-v41x;\r\n var v4y=v42y-v41y;\r\n var v4z=v42z-v41z;\r\n var separation=-(v4x*nx+v4y*ny+v4z*nz);\r\n if((v4x-v3x)*nx+(v4y-v3y)*ny+(v4z-v3z)*nz<=0.01||separation>=0){\r\n if(hit){\r\n sep.set( -nx, -ny, -nz );\r\n pos.set( (p1x+p2x)*0.5, (p1y+p2y)*0.5, (p1z+p2z)*0.5 );\r\n dep.x=separation;\r\n return true;\r\n }\r\n return false;\r\n }\r\n if(\r\n (v4y*v1z-v4z*v1y)*v0x+\r\n (v4z*v1x-v4x*v1z)*v0y+\r\n (v4x*v1y-v4y*v1x)*v0z<0\r\n ){\r\n if(\r\n (v4y*v2z-v4z*v2y)*v0x+\r\n (v4z*v2x-v4x*v2z)*v0y+\r\n (v4x*v2y-v4y*v2x)*v0z<0\r\n ){\r\n v1x=v4x;\r\n v1y=v4y;\r\n v1z=v4z;\r\n v11x=v41x;\r\n v11y=v41y;\r\n v11z=v41z;\r\n v12x=v42x;\r\n v12y=v42y;\r\n v12z=v42z;\r\n }else{\r\n v3x=v4x;\r\n v3y=v4y;\r\n v3z=v4z;\r\n v31x=v41x;\r\n v31y=v41y;\r\n v31z=v41z;\r\n v32x=v42x;\r\n v32y=v42y;\r\n v32z=v42z;\r\n }\r\n }else{\r\n if(\r\n (v4y*v3z-v4z*v3y)*v0x+\r\n (v4z*v3x-v4x*v3z)*v0y+\r\n (v4x*v3y-v4y*v3x)*v0z<0\r\n ){\r\n v2x=v4x;\r\n v2y=v4y;\r\n v2z=v4z;\r\n v21x=v41x;\r\n v21y=v41y;\r\n v21z=v41z;\r\n v22x=v42x;\r\n v22y=v42y;\r\n v22z=v42z;\r\n }else{\r\n v1x=v4x;\r\n v1y=v4y;\r\n v1z=v4z;\r\n v11x=v41x;\r\n v11y=v41y;\r\n v11z=v41z;\r\n v12x=v42x;\r\n v12y=v42y;\r\n v12z=v42z;\r\n }\r\n }\r\n }\r\n }\r\n //return false;\r\n },\r\n\r\n supportPoint: function ( c, dx, dy, dz, out ) {\r\n\r\n var rot=c.rotation.elements;\r\n var ldx=rot[0]*dx+rot[3]*dy+rot[6]*dz;\r\n var ldy=rot[1]*dx+rot[4]*dy+rot[7]*dz;\r\n var ldz=rot[2]*dx+rot[5]*dy+rot[8]*dz;\r\n var radx=ldx;\r\n var radz=ldz;\r\n var len=radx*radx+radz*radz;\r\n var rad=c.radius;\r\n var hh=c.halfHeight;\r\n var ox;\r\n var oy;\r\n var oz;\r\n if(len==0){\r\n if(ldy<0){\r\n ox=rad;\r\n oy=-hh;\r\n oz=0;\r\n }else{\r\n ox=rad;\r\n oy=hh;\r\n oz=0;\r\n }\r\n }else{\r\n len=c.radius/_Math.sqrt(len);\r\n if(ldy<0){\r\n ox=radx*len;\r\n oy=-hh;\r\n oz=radz*len;\r\n }else{\r\n ox=radx*len;\r\n oy=hh;\r\n oz=radz*len;\r\n }\r\n }\r\n ldx=rot[0]*ox+rot[1]*oy+rot[2]*oz+c.position.x;\r\n ldy=rot[3]*ox+rot[4]*oy+rot[5]*oz+c.position.y;\r\n ldz=rot[6]*ox+rot[7]*oy+rot[8]*oz+c.position.z;\r\n out.set( ldx, ldy, ldz );\r\n\r\n },\r\n\r\n detectCollision: function ( shape1, shape2, manifold ) {\r\n\r\n var c1;\r\n var c2;\r\n if(shape1.id0;\r\n var right2=dot2>0;\r\n if(!right1)dot1=-dot1;\r\n if(!right2)dot2=-dot2;\r\n var state=0;\r\n if(dot1>0.999||dot2>0.999){\r\n if(dot1>dot2)state=1;\r\n else state=2;\r\n }\r\n var nx;\r\n var ny;\r\n var nz;\r\n var depth=dep.x;\r\n var r00;\r\n var r01;\r\n var r02;\r\n var r10;\r\n var r11;\r\n var r12;\r\n var r20;\r\n var r21;\r\n var r22;\r\n var px;\r\n var py;\r\n var pz;\r\n var pd;\r\n var a;\r\n var b;\r\n var e;\r\n var f;\r\n nx=sep.x;\r\n ny=sep.y;\r\n nz=sep.z;\r\n switch(state){\r\n case 0:\r\n manifold.addPoint(pos.x,pos.y,pos.z,nx,ny,nz,depth,false);\r\n break;\r\n case 1:\r\n if(right1){\r\n c1x=p1x+d1x;\r\n c1y=p1y+d1y;\r\n c1z=p1z+d1z;\r\n nx=n1x;\r\n ny=n1y;\r\n nz=n1z;\r\n }else{\r\n c1x=p1x-d1x;\r\n c1y=p1y-d1y;\r\n c1z=p1z-d1z;\r\n nx=-n1x;\r\n ny=-n1y;\r\n nz=-n1z;\r\n }\r\n dot=nx*n2x+ny*n2y+nz*n2z;\r\n if(dot<0)len=h2;\r\n else len=-h2;\r\n c2x=p2x+len*n2x;\r\n c2y=p2y+len*n2y;\r\n c2z=p2z+len*n2z;\r\n if(dot2>=0.999999){\r\n tx=-ny;\r\n ty=nz;\r\n tz=nx;\r\n }else{\r\n tx=nx;\r\n ty=ny;\r\n tz=nz;\r\n }\r\n len=tx*n2x+ty*n2y+tz*n2z;\r\n dx=len*n2x-tx;\r\n dy=len*n2y-ty;\r\n dz=len*n2z-tz;\r\n len=_Math.sqrt(dx*dx+dy*dy+dz*dz);\r\n if(len==0)break;\r\n len=r2/len;\r\n dx*=len;\r\n dy*=len;\r\n dz*=len;\r\n tx=c2x+dx;\r\n ty=c2y+dy;\r\n tz=c2z+dz;\r\n if(dot<-0.96||dot>0.96){\r\n r00=n2x*n2x*1.5-0.5;\r\n r01=n2x*n2y*1.5-n2z*0.866025403;\r\n r02=n2x*n2z*1.5+n2y*0.866025403;\r\n r10=n2y*n2x*1.5+n2z*0.866025403;\r\n r11=n2y*n2y*1.5-0.5;\r\n r12=n2y*n2z*1.5-n2x*0.866025403;\r\n r20=n2z*n2x*1.5-n2y*0.866025403;\r\n r21=n2z*n2y*1.5+n2x*0.866025403;\r\n r22=n2z*n2z*1.5-0.5;\r\n px=tx;\r\n py=ty;\r\n pz=tz;\r\n pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z);\r\n tx=px-pd*nx-c1x;\r\n ty=py-pd*ny-c1y;\r\n tz=pz-pd*nz-c1z;\r\n len=tx*tx+ty*ty+tz*tz;\r\n if(len>r1*r1){\r\n len=r1/_Math.sqrt(len);\r\n tx*=len;\r\n ty*=len;\r\n tz*=len;\r\n }\r\n px=c1x+tx;\r\n py=c1y+ty;\r\n pz=c1z+tz;\r\n manifold.addPoint(px,py,pz,nx,ny,nz,pd,false);\r\n px=dx*r00+dy*r01+dz*r02;\r\n py=dx*r10+dy*r11+dz*r12;\r\n pz=dx*r20+dy*r21+dz*r22;\r\n px=(dx=px)+c2x;\r\n py=(dy=py)+c2y;\r\n pz=(dz=pz)+c2z;\r\n pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z);\r\n if(pd<=0){\r\n tx=px-pd*nx-c1x;\r\n ty=py-pd*ny-c1y;\r\n tz=pz-pd*nz-c1z;\r\n len=tx*tx+ty*ty+tz*tz;\r\n if(len>r1*r1){\r\n len=r1/_Math.sqrt(len);\r\n tx*=len;\r\n ty*=len;\r\n tz*=len;\r\n }\r\n px=c1x+tx;\r\n py=c1y+ty;\r\n pz=c1z+tz;\r\n manifold.addPoint(px,py,pz,nx,ny,nz,pd,false);\r\n }\r\n px=dx*r00+dy*r01+dz*r02;\r\n py=dx*r10+dy*r11+dz*r12;\r\n pz=dx*r20+dy*r21+dz*r22;\r\n px=(dx=px)+c2x;\r\n py=(dy=py)+c2y;\r\n pz=(dz=pz)+c2z;\r\n pd=nx*(px-c1x)+ny*(py-c1y)+nz*(pz-c1z);\r\n if(pd<=0){\r\n tx=px-pd*nx-c1x;\r\n ty=py-pd*ny-c1y;\r\n tz=pz-pd*nz-c1z;\r\n len=tx*tx+ty*ty+tz*tz;\r\n if(len>r1*r1){\r\n len=r1/_Math.sqrt(len);\r\n tx*=len;\r\n ty*=len;\r\n tz*=len;\r\n }\r\n px=c1x+tx;\r\n py=c1y+ty;\r\n pz=c1z+tz;\r\n manifold.addPoint(px,py,pz,nx,ny,nz,pd,false);\r\n }\r\n }else{\r\n sx=tx;\r\n sy=ty;\r\n sz=tz;\r\n depth1=nx*(sx-c1x)+ny*(sy-c1y)+nz*(sz-c1z);\r\n sx-=depth1*nx;\r\n sy-=depth1*ny;\r\n sz-=depth1*nz;\r\n if(dot>0){\r\n ex=tx+n2x*h2*2;\r\n ey=ty+n2y*h2*2;\r\n ez=tz+n2z*h2*2;\r\n }else{\r\n ex=tx-n2x*h2*2;\r\n ey=ty-n2y*h2*2;\r\n ez=tz-n2z*h2*2;\r\n }\r\n depth2=nx*(ex-c1x)+ny*(ey-c1y)+nz*(ez-c1z);\r\n ex-=depth2*nx;\r\n ey-=depth2*ny;\r\n ez-=depth2*nz;\r\n dx=c1x-sx;\r\n dy=c1y-sy;\r\n dz=c1z-sz;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n a=dx*dx+dy*dy+dz*dz;\r\n b=dx*tx+dy*ty+dz*tz;\r\n e=tx*tx+ty*ty+tz*tz;\r\n f=b*b-e*(a-r1*r1);\r\n if(f<0)break;\r\n f=_Math.sqrt(f);\r\n t1=(b+f)/e;\r\n t2=(b-f)/e;\r\n if(t21)t2=1;\r\n if(t1<0)t1=0;\r\n tx=sx+(ex-sx)*t1;\r\n ty=sy+(ey-sy)*t1;\r\n tz=sz+(ez-sz)*t1;\r\n ex=sx+(ex-sx)*t2;\r\n ey=sy+(ey-sy)*t2;\r\n ez=sz+(ez-sz)*t2;\r\n sx=tx;\r\n sy=ty;\r\n sz=tz;\r\n len=depth1+(depth2-depth1)*t1;\r\n depth2=depth1+(depth2-depth1)*t2;\r\n depth1=len;\r\n if(depth1<0) manifold.addPoint(sx,sy,sz,nx,ny,nz,pd,false);\r\n if(depth2<0) manifold.addPoint(ex,ey,ez,nx,ny,nz,pd,false);\r\n \r\n }\r\n break;\r\n case 2:\r\n if(right2){\r\n c2x=p2x-d2x;\r\n c2y=p2y-d2y;\r\n c2z=p2z-d2z;\r\n nx=-n2x;\r\n ny=-n2y;\r\n nz=-n2z;\r\n }else{\r\n c2x=p2x+d2x;\r\n c2y=p2y+d2y;\r\n c2z=p2z+d2z;\r\n nx=n2x;\r\n ny=n2y;\r\n nz=n2z;\r\n }\r\n dot=nx*n1x+ny*n1y+nz*n1z;\r\n if(dot<0)len=h1;\r\n else len=-h1;\r\n c1x=p1x+len*n1x;\r\n c1y=p1y+len*n1y;\r\n c1z=p1z+len*n1z;\r\n if(dot1>=0.999999){\r\n tx=-ny;\r\n ty=nz;\r\n tz=nx;\r\n }else{\r\n tx=nx;\r\n ty=ny;\r\n tz=nz;\r\n }\r\n len=tx*n1x+ty*n1y+tz*n1z;\r\n dx=len*n1x-tx;\r\n dy=len*n1y-ty;\r\n dz=len*n1z-tz;\r\n len=_Math.sqrt(dx*dx+dy*dy+dz*dz);\r\n if(len==0)break;\r\n len=r1/len;\r\n dx*=len;\r\n dy*=len;\r\n dz*=len;\r\n tx=c1x+dx;\r\n ty=c1y+dy;\r\n tz=c1z+dz;\r\n if(dot<-0.96||dot>0.96){\r\n r00=n1x*n1x*1.5-0.5;\r\n r01=n1x*n1y*1.5-n1z*0.866025403;\r\n r02=n1x*n1z*1.5+n1y*0.866025403;\r\n r10=n1y*n1x*1.5+n1z*0.866025403;\r\n r11=n1y*n1y*1.5-0.5;\r\n r12=n1y*n1z*1.5-n1x*0.866025403;\r\n r20=n1z*n1x*1.5-n1y*0.866025403;\r\n r21=n1z*n1y*1.5+n1x*0.866025403;\r\n r22=n1z*n1z*1.5-0.5;\r\n px=tx;\r\n py=ty;\r\n pz=tz;\r\n pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z);\r\n tx=px-pd*nx-c2x;\r\n ty=py-pd*ny-c2y;\r\n tz=pz-pd*nz-c2z;\r\n len=tx*tx+ty*ty+tz*tz;\r\n if(len>r2*r2){\r\n len=r2/_Math.sqrt(len);\r\n tx*=len;\r\n ty*=len;\r\n tz*=len;\r\n }\r\n px=c2x+tx;\r\n py=c2y+ty;\r\n pz=c2z+tz;\r\n manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false);\r\n px=dx*r00+dy*r01+dz*r02;\r\n py=dx*r10+dy*r11+dz*r12;\r\n pz=dx*r20+dy*r21+dz*r22;\r\n px=(dx=px)+c1x;\r\n py=(dy=py)+c1y;\r\n pz=(dz=pz)+c1z;\r\n pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z);\r\n if(pd<=0){\r\n tx=px-pd*nx-c2x;\r\n ty=py-pd*ny-c2y;\r\n tz=pz-pd*nz-c2z;\r\n len=tx*tx+ty*ty+tz*tz;\r\n if(len>r2*r2){\r\n len=r2/_Math.sqrt(len);\r\n tx*=len;\r\n ty*=len;\r\n tz*=len;\r\n }\r\n px=c2x+tx;\r\n py=c2y+ty;\r\n pz=c2z+tz;\r\n manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false);\r\n }\r\n px=dx*r00+dy*r01+dz*r02;\r\n py=dx*r10+dy*r11+dz*r12;\r\n pz=dx*r20+dy*r21+dz*r22;\r\n px=(dx=px)+c1x;\r\n py=(dy=py)+c1y;\r\n pz=(dz=pz)+c1z;\r\n pd=nx*(px-c2x)+ny*(py-c2y)+nz*(pz-c2z);\r\n if(pd<=0){\r\n tx=px-pd*nx-c2x;\r\n ty=py-pd*ny-c2y;\r\n tz=pz-pd*nz-c2z;\r\n len=tx*tx+ty*ty+tz*tz;\r\n if(len>r2*r2){\r\n len=r2/_Math.sqrt(len);\r\n tx*=len;\r\n ty*=len;\r\n tz*=len;\r\n }\r\n px=c2x+tx;\r\n py=c2y+ty;\r\n pz=c2z+tz;\r\n manifold.addPoint(px,py,pz,-nx,-ny,-nz,pd,false);\r\n }\r\n }else{\r\n sx=tx;\r\n sy=ty;\r\n sz=tz;\r\n depth1=nx*(sx-c2x)+ny*(sy-c2y)+nz*(sz-c2z);\r\n sx-=depth1*nx;\r\n sy-=depth1*ny;\r\n sz-=depth1*nz;\r\n if(dot>0){\r\n ex=tx+n1x*h1*2;\r\n ey=ty+n1y*h1*2;\r\n ez=tz+n1z*h1*2;\r\n }else{\r\n ex=tx-n1x*h1*2;\r\n ey=ty-n1y*h1*2;\r\n ez=tz-n1z*h1*2;\r\n }\r\n depth2=nx*(ex-c2x)+ny*(ey-c2y)+nz*(ez-c2z);\r\n ex-=depth2*nx;\r\n ey-=depth2*ny;\r\n ez-=depth2*nz;\r\n dx=c2x-sx;\r\n dy=c2y-sy;\r\n dz=c2z-sz;\r\n tx=ex-sx;\r\n ty=ey-sy;\r\n tz=ez-sz;\r\n a=dx*dx+dy*dy+dz*dz;\r\n b=dx*tx+dy*ty+dz*tz;\r\n e=tx*tx+ty*ty+tz*tz;\r\n f=b*b-e*(a-r2*r2);\r\n if(f<0)break;\r\n f=_Math.sqrt(f);\r\n t1=(b+f)/e;\r\n t2=(b-f)/e;\r\n if(t21)t2=1;\r\n if(t1<0)t1=0;\r\n tx=sx+(ex-sx)*t1;\r\n ty=sy+(ey-sy)*t1;\r\n tz=sz+(ez-sz)*t1;\r\n ex=sx+(ex-sx)*t2;\r\n ey=sy+(ey-sy)*t2;\r\n ez=sz+(ez-sz)*t2;\r\n sx=tx;\r\n sy=ty;\r\n sz=tz;\r\n len=depth1+(depth2-depth1)*t1;\r\n depth2=depth1+(depth2-depth1)*t2;\r\n depth1=len;\r\n if(depth1<0){\r\n manifold.addPoint(sx,sy,sz,-nx,-ny,-nz,depth1,false);\r\n }\r\n if(depth2<0){\r\n manifold.addPoint(ex,ey,ez,-nx,-ny,-nz,depth2,false);\r\n }\r\n }\r\n break;\r\n }\r\n\r\n }\r\n\r\n});\n\n/**\n * A collision detector which detects collisions between sphere and box.\n * @author saharan\n */\nfunction SphereBoxCollisionDetector ( flip ) {\n \n CollisionDetector.call( this );\n this.flip = flip;\n\n}\n\nSphereBoxCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), {\n\n constructor: SphereBoxCollisionDetector,\n\n detectCollision: function ( shape1, shape2, manifold ) {\n\n var s;\n var b;\n if(this.flip){\n s=(shape2);\n b=(shape1);\n }else{\n s=(shape1);\n b=(shape2);\n }\n\n var D = b.dimentions;\n\n var ps=s.position;\n var psx=ps.x;\n var psy=ps.y;\n var psz=ps.z;\n var pb=b.position;\n var pbx=pb.x;\n var pby=pb.y;\n var pbz=pb.z;\n var rad=s.radius;\n\n var hw=b.halfWidth;\n var hh=b.halfHeight;\n var hd=b.halfDepth;\n\n var dx=psx-pbx;\n var dy=psy-pby;\n var dz=psz-pbz;\n var sx=D[0]*dx+D[1]*dy+D[2]*dz;\n var sy=D[3]*dx+D[4]*dy+D[5]*dz;\n var sz=D[6]*dx+D[7]*dy+D[8]*dz;\n var cx;\n var cy;\n var cz;\n var len;\n var invLen;\n var overlap=0;\n if(sx>hw){\n sx=hw;\n }else if(sx<-hw){\n sx=-hw;\n }else{\n overlap=1;\n }\n if(sy>hh){\n sy=hh;\n }else if(sy<-hh){\n sy=-hh;\n }else{\n overlap|=2;\n }\n if(sz>hd){\n sz=hd;\n }else if(sz<-hd){\n sz=-hd;\n }else{\n overlap|=4;\n }\n if(overlap==7){\n // center of sphere is in the box\n if(sx<0){\n dx=hw+sx;\n }else{\n dx=hw-sx;\n }\n if(sy<0){\n dy=hh+sy;\n }else{\n dy=hh-sy;\n }\n if(sz<0){\n dz=hd+sz;\n }else{\n dz=hd-sz;\n }\n if(dx0&&len halfh + rads ) return;\n var cx = pcx + dot * dirx;\n var cy = pcy + dot * diry;\n var cz = pcz + dot * dirz;\n var d2x = psx - cx;\n var d2y = psy - cy;\n var d2z = psz - cz;\n var len = d2x * d2x + d2y * d2y + d2z * d2z;\n if ( len > rad2 * rad2 ) return;\n if ( len > radc * radc ) {\n len = radc / _Math.sqrt( len );\n d2x *= len;\n d2y *= len;\n d2z *= len;\n }\n if( dot < -halfh ) dot = -halfh;\n else if( dot > halfh ) dot = halfh;\n cx = pcx + dot * dirx + d2x;\n cy = pcy + dot * diry + d2y;\n cz = pcz + dot * dirz + d2z;\n dx = cx - psx;\n dy = cy - psy;\n dz = cz - psz;\n len = dx * dx + dy * dy + dz * dz;\n var invLen;\n if ( len > 0 && len < rads * rads ) {\n len = _Math.sqrt(len);\n invLen = 1 / len;\n dx *= invLen;\n dy *= invLen;\n dz *= invLen;\n ///result.addContactInfo(psx+dx*rads,psy+dy*rads,psz+dz*rads,dx,dy,dz,len-rads,s,c,0,0,false);\n manifold.addPoint( psx + dx * rads, psy + dy * rads, psz + dz * rads, dx, dy, dz, len - rads, this.flip );\n }\n\n }\n\n\n});\n\n/**\n * A collision detector which detects collisions between two spheres.\n * @author saharan\n */\n \nfunction SphereSphereCollisionDetector (){\n\n CollisionDetector.call( this );\n\n}\n\nSphereSphereCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), {\n\n constructor: SphereSphereCollisionDetector,\n\n detectCollision: function ( shape1, shape2, manifold ) {\n\n var s1 = shape1;\n var s2 = shape2;\n var p1 = s1.position;\n var p2 = s2.position;\n var dx = p2.x - p1.x;\n var dy = p2.y - p1.y;\n var dz = p2.z - p1.z;\n var len = dx * dx + dy * dy + dz * dz;\n var r1 = s1.radius;\n var r2 = s2.radius;\n var rad = r1 + r2;\n if ( len > 0 && len < rad * rad ){\n len = _Math.sqrt( len );\n var invLen = 1 / len;\n dx *= invLen;\n dy *= invLen;\n dz *= invLen;\n manifold.addPoint( p1.x + dx * r1, p1.y + dy * r1, p1.z + dz * r1, dx, dy, dz, len - rad, false );\n }\n\n }\n\n});\n\n/**\r\n * A collision detector which detects collisions between two spheres.\r\n * @author saharan \r\n * @author lo-th\r\n */\r\n \r\nfunction SpherePlaneCollisionDetector ( flip ){\r\n\r\n CollisionDetector.call( this );\r\n\r\n this.flip = flip;\r\n\r\n this.n = new Vec3();\r\n this.p = new Vec3();\r\n\r\n}\r\n\r\nSpherePlaneCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), {\r\n\r\n constructor: SpherePlaneCollisionDetector,\r\n\r\n detectCollision: function ( shape1, shape2, manifold ) {\r\n\r\n var n = this.n;\r\n var p = this.p;\r\n\r\n var s = this.flip ? shape2 : shape1;\r\n var pn = this.flip ? shape1 : shape2;\r\n var rad = s.radius;\r\n var len;\r\n\r\n n.sub( s.position, pn.position );\r\n //var h = _Math.dotVectors( pn.normal, n );\r\n\r\n n.x *= pn.normal.x;//+ rad;\r\n n.y *= pn.normal.y;\r\n n.z *= pn.normal.z;//+ rad;\r\n\r\n \r\n var len = n.lengthSq();\r\n \r\n if( len > 0 && len < rad * rad){//&& h > rad*rad ){\r\n\r\n \r\n len = _Math.sqrt( len );\r\n //len = _Math.sqrt( h );\r\n n.copy(pn.normal).negate();\r\n //n.scaleEqual( 1/len );\r\n\r\n //(0, -1, 0)\r\n\r\n //n.normalize();\r\n p.copy( s.position ).addScaledVector( n, rad );\r\n manifold.addPointVec( p, n, len - rad, this.flip );\r\n\r\n }\r\n\r\n }\r\n\r\n});\n\n/**\r\n * A collision detector which detects collisions between two spheres.\r\n * @author saharan \r\n * @author lo-th\r\n */\r\n \r\nfunction BoxPlaneCollisionDetector ( flip ){\r\n\r\n CollisionDetector.call( this );\r\n\r\n this.flip = flip;\r\n\r\n this.n = new Vec3();\r\n this.p = new Vec3();\r\n\r\n this.dix = new Vec3();\r\n this.diy = new Vec3();\r\n this.diz = new Vec3();\r\n\r\n this.cc = new Vec3();\r\n this.cc2 = new Vec3();\r\n\r\n}\r\n\r\nBoxPlaneCollisionDetector.prototype = Object.assign( Object.create( CollisionDetector.prototype ), {\r\n\r\n constructor: BoxPlaneCollisionDetector,\r\n\r\n detectCollision: function ( shape1, shape2, manifold ) {\r\n\r\n var n = this.n;\r\n var p = this.p;\r\n var cc = this.cc;\r\n\r\n var b = this.flip ? shape2 : shape1;\r\n var pn = this.flip ? shape1 : shape2;\r\n\r\n var D = b.dimentions;\r\n var hw = b.halfWidth;\r\n var hh = b.halfHeight;\r\n var hd = b.halfDepth;\r\n var len;\r\n var overlap = 0;\r\n\r\n this.dix.set( D[0], D[1], D[2] );\r\n this.diy.set( D[3], D[4], D[5] );\r\n this.diz.set( D[6], D[7], D[8] );\r\n\r\n n.sub( b.position, pn.position );\r\n\r\n n.x *= pn.normal.x;//+ rad;\r\n n.y *= pn.normal.y;\r\n n.z *= pn.normal.z;//+ rad;\r\n\r\n cc.set(\r\n _Math.dotVectors( this.dix, n ),\r\n _Math.dotVectors( this.diy, n ),\r\n _Math.dotVectors( this.diz, n )\r\n );\r\n\r\n\r\n if( cc.x > hw ) cc.x = hw;\r\n else if( cc.x < -hw ) cc.x = -hw;\r\n else overlap = 1;\r\n \r\n if( cc.y > hh ) cc.y = hh;\r\n else if( cc.y < -hh ) cc.y = -hh;\r\n else overlap |= 2;\r\n \r\n if( cc.z > hd ) cc.z = hd;\r\n else if( cc.z < -hd ) cc.z = -hd;\r\n else overlap |= 4;\r\n\r\n \r\n\r\n if( overlap === 7 ){\r\n\r\n // center of sphere is in the box\r\n \r\n n.set(\r\n cc.x < 0 ? hw + cc.x : hw - cc.x,\r\n cc.y < 0 ? hh + cc.y : hh - cc.y,\r\n cc.z < 0 ? hd + cc.z : hd - cc.z\r\n );\r\n \r\n if( n.x < n.y ){\r\n if( n.x < n.z ){\r\n len = n.x - hw;\r\n if( cc.x < 0 ){\r\n cc.x = -hw;\r\n n.copy( this.dix );\r\n }else{\r\n cc.x = hw;\r\n n.subEqual( this.dix );\r\n }\r\n }else{\r\n len = n.z - hd;\r\n if( cc.z < 0 ){\r\n cc.z = -hd;\r\n n.copy( this.diz );\r\n }else{\r\n cc.z = hd;\r\n n.subEqual( this.diz );\r\n }\r\n }\r\n }else{\r\n if( n.y < n.z ){\r\n len = n.y - hh;\r\n if( cc.y < 0 ){\r\n cc.y = -hh;\r\n n.copy( this.diy );\r\n }else{\r\n cc.y = hh;\r\n n.subEqual( this.diy );\r\n }\r\n }else{\r\n len = n.z - hd;\r\n if( cc.z < 0 ){\r\n cc.z = -hd;\r\n n.copy( this.diz );\r\n }else{\r\n cc.z = hd;\r\n n.subEqual( this.diz );\r\n }\r\n }\r\n }\r\n\r\n p.copy( pn.position ).addScaledVector( n, 1 );\r\n manifold.addPointVec( p, n, len, this.flip );\r\n\r\n }\r\n\r\n }\r\n\r\n});\n\n//import { TetraShape } from '../collision/shape/TetraShape';\r\n\r\n/**\r\n * The class of physical computing world.\r\n * You must be added to the world physical all computing objects\r\n *\r\n * @author saharan\r\n * @author lo-th\r\n */\r\n\r\n // timestep, broadphase, iterations, worldscale, random, stat\r\n\r\nfunction World ( o ) {\r\n\r\n if( !(o instanceof Object) ) o = {};\r\n\r\n // this world scale defaut is 0.1 to 10 meters max for dynamique body\r\n this.scale = o.worldscale || 1;\r\n this.invScale = 1/this.scale;\r\n\r\n // The time between each step\r\n this.timeStep = o.timestep || 0.01666; // 1/60;\r\n this.timerate = this.timeStep * 1000;\r\n this.timer = null;\r\n\r\n this.preLoop = null;//function(){};\r\n this.postLoop = null;//function(){};\r\n\r\n // The number of iterations for constraint solvers.\r\n this.numIterations = o.iterations || 8;\r\n\r\n // It is a wide-area collision judgment that is used in order to reduce as much as possible a detailed collision judgment.\r\n switch( o.broadphase || 2 ){\r\n case 1: this.broadPhase = new BruteForceBroadPhase(); break;\r\n case 2: default: this.broadPhase = new SAPBroadPhase(); break;\r\n case 3: this.broadPhase = new DBVTBroadPhase(); break;\r\n }\r\n\r\n this.Btypes = ['None','BruteForce','Sweep & Prune', 'Bounding Volume Tree' ];\r\n this.broadPhaseType = this.Btypes[ o.broadphase || 2 ];\r\n\r\n // This is the detailed information of the performance.\r\n this.performance = null;\r\n this.isStat = o.info === undefined ? false : o.info;\r\n if( this.isStat ) this.performance = new InfoDisplay( this );\r\n\r\n /**\r\n * Whether the constraints randomizer is enabled or not.\r\n *\r\n * @property enableRandomizer\r\n * @type {Boolean}\r\n */\r\n this.enableRandomizer = o.random !== undefined ? o.random : true;\r\n\r\n // The rigid body list\r\n this.rigidBodies=null;\r\n // number of rigid body\r\n this.numRigidBodies=0;\r\n // The contact list\r\n this.contacts=null;\r\n this.unusedContacts=null;\r\n // The number of contact\r\n this.numContacts=0;\r\n // The number of contact points\r\n this.numContactPoints=0;\r\n // The joint list\r\n this.joints=null;\r\n // The number of joints.\r\n this.numJoints=0;\r\n // The number of simulation islands.\r\n this.numIslands=0;\r\n\r\n\r\n // The gravity in the world.\r\n this.gravity = new Vec3(0,-9.8,0);\r\n if( o.gravity !== undefined ) this.gravity.fromArray( o.gravity );\r\n\r\n\r\n\r\n var numShapeTypes = 5;//4;//3;\r\n this.detectors=[];\r\n this.detectors.length = numShapeTypes;\r\n var i = numShapeTypes;\r\n while(i--){\r\n this.detectors[i]=[];\r\n this.detectors[i].length = numShapeTypes;\r\n }\r\n\r\n this.detectors[SHAPE_SPHERE][SHAPE_SPHERE] = new SphereSphereCollisionDetector();\r\n this.detectors[SHAPE_SPHERE][SHAPE_BOX] = new SphereBoxCollisionDetector(false);\r\n this.detectors[SHAPE_BOX][SHAPE_SPHERE] = new SphereBoxCollisionDetector(true);\r\n this.detectors[SHAPE_BOX][SHAPE_BOX] = new BoxBoxCollisionDetector();\r\n\r\n // CYLINDER add\r\n this.detectors[SHAPE_CYLINDER][SHAPE_CYLINDER] = new CylinderCylinderCollisionDetector();\r\n\r\n this.detectors[SHAPE_CYLINDER][SHAPE_BOX] = new BoxCylinderCollisionDetector(true);\r\n this.detectors[SHAPE_BOX][SHAPE_CYLINDER] = new BoxCylinderCollisionDetector(false);\r\n\r\n this.detectors[SHAPE_CYLINDER][SHAPE_SPHERE] = new SphereCylinderCollisionDetector(true);\r\n this.detectors[SHAPE_SPHERE][SHAPE_CYLINDER] = new SphereCylinderCollisionDetector(false);\r\n\r\n // PLANE add\r\n\r\n this.detectors[SHAPE_PLANE][SHAPE_SPHERE] = new SpherePlaneCollisionDetector(true);\r\n this.detectors[SHAPE_SPHERE][SHAPE_PLANE] = new SpherePlaneCollisionDetector(false);\r\n\r\n this.detectors[SHAPE_PLANE][SHAPE_BOX] = new BoxPlaneCollisionDetector(true);\r\n this.detectors[SHAPE_BOX][SHAPE_PLANE] = new BoxPlaneCollisionDetector(false);\r\n\r\n // TETRA add\r\n //this.detectors[SHAPE_TETRA][SHAPE_TETRA] = new TetraTetraCollisionDetector();\r\n\r\n\r\n this.randX = 65535;\r\n this.randA = 98765;\r\n this.randB = 123456789;\r\n\r\n this.islandRigidBodies = [];\r\n this.islandStack = [];\r\n this.islandConstraints = [];\r\n\r\n}\r\n\r\nObject.assign( World.prototype, {\r\n\r\n World: true,\r\n\r\n play: function(){\r\n \r\n if( this.timer !== null ) return;\r\n\r\n var _this = this;\r\n this.timer = setInterval( function(){ _this.step(); } , this.timerate );\r\n //this.timer = setInterval( this.loop.bind(this) , this.timerate );\r\n\r\n },\r\n\r\n stop: function(){\r\n\r\n if( this.timer === null ) return;\r\n\r\n clearInterval( this.timer );\r\n this.timer = null;\r\n\r\n },\r\n\r\n setGravity: function ( ar ) {\r\n\r\n this.gravity.fromArray( ar );\r\n\r\n },\r\n\r\n getInfo: function () {\r\n\r\n return this.isStat ? this.performance.show() : '';\r\n\r\n },\r\n\r\n // Reset the world and remove all rigid bodies, shapes, joints and any object from the world.\r\n clear:function(){\r\n\r\n this.stop();\r\n this.preLoop = null;\r\n this.postLoop = null;\r\n\r\n this.randX = 65535;\r\n\r\n while(this.joints!==null){\r\n this.removeJoint( this.joints );\r\n }\r\n while(this.contacts!==null){\r\n this.removeContact( this.contacts );\r\n }\r\n while(this.rigidBodies!==null){\r\n this.removeRigidBody( this.rigidBodies );\r\n }\r\n\r\n },\r\n /**\r\n * I'll add a rigid body to the world.\r\n * Rigid body that has been added will be the operands of each step.\r\n * @param rigidBody Rigid body that you want to add\r\n */\r\n addRigidBody:function( rigidBody ){\r\n\r\n if(rigidBody.parent){\r\n printError(\"World\", \"It is not possible to be added to more than one world one of the rigid body\");\r\n }\r\n\r\n rigidBody.setParent( this );\r\n //rigidBody.awake();\r\n\r\n for(var shape = rigidBody.shapes; shape !== null; shape = shape.next){\r\n this.addShape( shape );\r\n }\r\n if(this.rigidBodies!==null)(this.rigidBodies.prev=rigidBody).next=this.rigidBodies;\r\n this.rigidBodies = rigidBody;\r\n this.numRigidBodies++;\r\n\r\n },\r\n /**\r\n * I will remove the rigid body from the world.\r\n * Rigid body that has been deleted is excluded from the calculation on a step-by-step basis.\r\n * @param rigidBody Rigid body to be removed\r\n */\r\n removeRigidBody:function( rigidBody ){\r\n\r\n var remove=rigidBody;\r\n if(remove.parent!==this)return;\r\n remove.awake();\r\n var js=remove.jointLink;\r\n while(js!=null){\r\n\t var joint=js.joint;\r\n\t js=js.next;\r\n\t this.removeJoint(joint);\r\n }\r\n for(var shape=rigidBody.shapes; shape!==null; shape=shape.next){\r\n this.removeShape(shape);\r\n }\r\n var prev=remove.prev;\r\n var next=remove.next;\r\n if(prev!==null) prev.next=next;\r\n if(next!==null) next.prev=prev;\r\n if(this.rigidBodies==remove) this.rigidBodies=next;\r\n remove.prev=null;\r\n remove.next=null;\r\n remove.parent=null;\r\n this.numRigidBodies--;\r\n\r\n },\r\n\r\n getByName: function( name ){\r\n\r\n var body = this.rigidBodies;\r\n while( body !== null ){\r\n if( body.name === name ) return body;\r\n body=body.next;\r\n }\r\n\r\n var joint = this.joints;\r\n while( joint !== null ){\r\n if( joint.name === name ) return joint;\r\n joint = joint.next;\r\n }\r\n\r\n return null;\r\n\r\n },\r\n\r\n /**\r\n * I'll add a shape to the world..\r\n * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world,\r\n * Shape will be added to the world automatically, please do not call from outside this method.\r\n * @param shape Shape you want to add\r\n */\r\n addShape:function ( shape ){\r\n\r\n if(!shape.parent || !shape.parent.parent){\r\n printError(\"World\", \"It is not possible to be added alone to shape world\");\r\n }\r\n\r\n shape.proxy = this.broadPhase.createProxy(shape);\r\n shape.updateProxy();\r\n this.broadPhase.addProxy(shape.proxy);\r\n\r\n },\r\n\r\n /**\r\n * I will remove the shape from the world.\r\n * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world,\r\n * Shape will be added to the world automatically, please do not call from outside this method.\r\n * @param shape Shape you want to delete\r\n */\r\n removeShape: function ( shape ){\r\n\r\n this.broadPhase.removeProxy(shape.proxy);\r\n shape.proxy = null;\r\n\r\n },\r\n\r\n /**\r\n * I'll add a joint to the world.\r\n * Joint that has been added will be the operands of each step.\r\n * @param shape Joint to be added\r\n */\r\n addJoint: function ( joint ) {\r\n\r\n if(joint.parent){\r\n printError(\"World\", \"It is not possible to be added to more than one world one of the joint\");\r\n }\r\n if(this.joints!=null)(this.joints.prev=joint).next=this.joints;\r\n this.joints=joint;\r\n joint.setParent( this );\r\n this.numJoints++;\r\n joint.awake();\r\n joint.attach();\r\n\r\n },\r\n\r\n /**\r\n * I will remove the joint from the world.\r\n * Joint that has been added will be the operands of each step.\r\n * @param shape Joint to be deleted\r\n */\r\n removeJoint: function ( joint ) {\r\n\r\n var remove=joint;\r\n var prev=remove.prev;\r\n var next=remove.next;\r\n if(prev!==null)prev.next=next;\r\n if(next!==null)next.prev=prev;\r\n if(this.joints==remove)this.joints=next;\r\n remove.prev=null;\r\n remove.next=null;\r\n this.numJoints--;\r\n remove.awake();\r\n remove.detach();\r\n remove.parent=null;\r\n\r\n },\r\n\r\n addContact: function ( s1, s2 ) {\r\n\r\n var newContact;\r\n if(this.unusedContacts!==null){\r\n newContact=this.unusedContacts;\r\n this.unusedContacts=this.unusedContacts.next;\r\n }else{\r\n newContact = new Contact();\r\n }\r\n newContact.attach(s1,s2);\r\n newContact.detector = this.detectors[s1.type][s2.type];\r\n if(this.contacts)(this.contacts.prev = newContact).next = this.contacts;\r\n this.contacts = newContact;\r\n this.numContacts++;\r\n\r\n },\r\n\r\n removeContact: function ( contact ) {\r\n\r\n var prev = contact.prev;\r\n var next = contact.next;\r\n if(next) next.prev = prev;\r\n if(prev) prev.next = next;\r\n if(this.contacts == contact) this.contacts = next;\r\n contact.prev = null;\r\n contact.next = null;\r\n contact.detach();\r\n contact.next = this.unusedContacts;\r\n this.unusedContacts = contact;\r\n this.numContacts--;\r\n\r\n },\r\n\r\n getContact: function ( b1, b2 ) {\r\n\r\n b1 = b1.constructor === RigidBody ? b1.name : b1;\r\n b2 = b2.constructor === RigidBody ? b2.name : b2;\r\n\r\n var n1, n2;\r\n var contact = this.contacts;\r\n while(contact!==null){\r\n n1 = contact.body1.name;\r\n n2 = contact.body2.name;\r\n if((n1===b1 && n2===b2) || (n2===b1 && n1===b2)){ if(contact.touching) return contact; else return null;}\r\n else contact = contact.next;\r\n }\r\n return null;\r\n\r\n },\r\n\r\n checkContact: function ( name1, name2 ) {\r\n\r\n var n1, n2;\r\n var contact = this.contacts;\r\n while(contact!==null){\r\n n1 = contact.body1.name || ' ';\r\n n2 = contact.body2.name || ' ';\r\n if((n1==name1 && n2==name2) || (n2==name1 && n1==name2)){ if(contact.touching) return true; else return false;}\r\n else contact = contact.next;\r\n }\r\n //return false;\r\n\r\n },\r\n\r\n callSleep: function( body ) {\r\n\r\n if( !body.allowSleep ) return false;\r\n if( body.linearVelocity.lengthSq() > 0.04 ) return false;\r\n if( body.angularVelocity.lengthSq() > 0.25 ) return false;\r\n return true;\r\n\r\n },\r\n\r\n /**\r\n * I will proceed only time step seconds time of World.\r\n */\r\n step: function () {\r\n\r\n var stat = this.isStat;\r\n\r\n if( stat ) this.performance.setTime( 0 );\r\n\r\n var body = this.rigidBodies;\r\n\r\n while( body !== null ){\r\n\r\n body.addedToIsland = false;\r\n\r\n if( body.sleeping ) body.testWakeUp();\r\n\r\n body = body.next;\r\n\r\n }\r\n\r\n\r\n\r\n //------------------------------------------------------\r\n // UPDATE BROADPHASE CONTACT\r\n //------------------------------------------------------\r\n\r\n if( stat ) this.performance.setTime( 1 );\r\n\r\n this.broadPhase.detectPairs();\r\n\r\n var pairs = this.broadPhase.pairs;\r\n\r\n var i = this.broadPhase.numPairs;\r\n //do{\r\n while(i--){\r\n //for(var i=0, l=numPairs; i0);\r\n\r\n if( stat ) this.performance.calcBroadPhase();\r\n\r\n //------------------------------------------------------\r\n // UPDATE NARROWPHASE CONTACT\r\n //------------------------------------------------------\r\n\r\n // update & narrow phase\r\n this.numContactPoints = 0;\r\n contact = this.contacts;\r\n while( contact!==null ){\r\n if(!contact.persisting){\r\n if ( contact.shape1.aabb.intersectTest( contact.shape2.aabb ) ) {\r\n /*var aabb1=contact.shape1.aabb;\r\n var aabb2=contact.shape2.aabb;\r\n if(\r\n\t aabb1.minX>aabb2.maxX || aabb1.maxXaabb2.maxY || aabb1.maxYaabb2.maxZ || aabb1.maxZ 0.5 ) base.sleep();\r\n else base.updatePosition( this.timeStep );\r\n }else{\r\n base.sleepTime = 0;\r\n base.updatePosition( this.timeStep );\r\n }\r\n this.numIslands++;\r\n continue;\r\n }\r\n\r\n var islandNumRigidBodies = 0;\r\n var islandNumConstraints = 0;\r\n var stackCount = 1;\r\n // add rigid body to stack\r\n this.islandStack[0] = base;\r\n base.addedToIsland = true;\r\n\r\n // build an island\r\n do{\r\n // get rigid body from stack\r\n body = this.islandStack[--stackCount];\r\n this.islandStack[stackCount] = null;\r\n body.sleeping = false;\r\n // add rigid body to the island\r\n this.islandRigidBodies[islandNumRigidBodies++] = body;\r\n if(body.isStatic) continue;\r\n\r\n // search connections\r\n for( var cs = body.contactLink; cs !== null; cs = cs.next ) {\r\n var contact = cs.contact;\r\n constraint = contact.constraint;\r\n if( constraint.addedToIsland || !contact.touching ) continue;// ignore\r\n\r\n // add constraint to the island\r\n this.islandConstraints[islandNumConstraints++] = constraint;\r\n constraint.addedToIsland = true;\r\n var next = cs.body;\r\n\r\n if(next.addedToIsland) continue;\r\n\r\n // add rigid body to stack\r\n this.islandStack[stackCount++] = next;\r\n next.addedToIsland = true;\r\n }\r\n for( var js = body.jointLink; js !== null; js = js.next ) {\r\n constraint = js.joint;\r\n\r\n if(constraint.addedToIsland) continue;// ignore\r\n\r\n // add constraint to the island\r\n this.islandConstraints[islandNumConstraints++] = constraint;\r\n constraint.addedToIsland = true;\r\n next = js.body;\r\n if( next.addedToIsland || !next.isDynamic ) continue;\r\n\r\n // add rigid body to stack\r\n this.islandStack[stackCount++] = next;\r\n next.addedToIsland = true;\r\n }\r\n } while( stackCount != 0 );\r\n\r\n // update velocities\r\n var gVel = new Vec3().addScaledVector( this.gravity, this.timeStep );\r\n /*var gx=this.gravity.x*this.timeStep;\r\n var gy=this.gravity.y*this.timeStep;\r\n var gz=this.gravity.z*this.timeStep;*/\r\n var j = islandNumRigidBodies;\r\n while (j--){\r\n //or(var j=0, l=islandNumRigidBodies; j 0.5){\r\n // sleep the island\r\n j = islandNumRigidBodies;\r\n while(j--){\r\n //for(j=0, l=islandNumRigidBodies;j