three
Version:
JavaScript 3D library
37,072 lines • 1.14 MB
JavaScript
// Generated by Haxe 4.2.0
var oimo = oimo || {};
if(!oimo.collision) oimo.collision = {};
if(!oimo.collision.broadphase) oimo.collision.broadphase = {};
oimo.collision.broadphase.BroadPhase = class oimo_collision_broadphase_BroadPhase {
constructor(type) {
this._type = type;
this._numProxies = 0;
this._proxyList = null;
this._proxyListLast = null;
this._proxyPairList = null;
this._incremental = false;
this._testCount = 0;
this._proxyPairPool = null;
this._idCount = 0;
this._convexSweep = new oimo.collision.broadphase._BroadPhase.ConvexSweepGeometry();
this._aabb = new oimo.collision.broadphase._BroadPhase.AabbGeometry();
this.identity = new oimo.common.Transform();
this.zero = new oimo.common.Vec3();
this.rayCastHit = new oimo.collision.geometry.RayCastHit();
}
createProxy(userData,aabb) {
return null;
}
destroyProxy(proxy) {
}
moveProxy(proxy,aabb,displacement) {
}
isOverlapping(proxy1,proxy2) {
if(proxy1._aabbMinX < proxy2._aabbMaxX && proxy1._aabbMaxX > proxy2._aabbMinX && proxy1._aabbMinY < proxy2._aabbMaxY && proxy1._aabbMaxY > proxy2._aabbMinY && proxy1._aabbMinZ < proxy2._aabbMaxZ) {
return proxy1._aabbMaxZ > proxy2._aabbMinZ;
} else {
return false;
}
}
collectPairs() {
}
getProxyPairList() {
return this._proxyPairList;
}
isIncremental() {
return this._incremental;
}
getTestCount() {
return this._testCount;
}
rayCast(begin,end,callback) {
}
convexCast(convex,begin,translation,callback) {
}
aabbTest(aabb,callback) {
}
}
if(!oimo.collision.geometry) oimo.collision.geometry = {};
oimo.collision.geometry.Geometry = class oimo_collision_geometry_Geometry {
constructor(type) {
this._type = type;
this._volume = 0;
}
_updateMass() {
}
_computeAabb(aabb,tf) {
}
_rayCastLocal(beginX,beginY,beginZ,endX,endY,endZ,hit) {
return false;
}
getType() {
return this._type;
}
getVolume() {
return this._volume;
}
rayCast(begin,end,transform,hit) {
let beginLocalX;
let beginLocalY;
let beginLocalZ;
let endLocalX;
let endLocalY;
let endLocalZ;
beginLocalX = begin.x;
beginLocalY = begin.y;
beginLocalZ = begin.z;
endLocalX = end.x;
endLocalY = end.y;
endLocalZ = end.z;
beginLocalX -= transform._positionX;
beginLocalY -= transform._positionY;
beginLocalZ -= transform._positionZ;
endLocalX -= transform._positionX;
endLocalY -= transform._positionY;
endLocalZ -= transform._positionZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = transform._rotation00 * beginLocalX + transform._rotation10 * beginLocalY + transform._rotation20 * beginLocalZ;
__tmp__Y = transform._rotation01 * beginLocalX + transform._rotation11 * beginLocalY + transform._rotation21 * beginLocalZ;
__tmp__Z = transform._rotation02 * beginLocalX + transform._rotation12 * beginLocalY + transform._rotation22 * beginLocalZ;
beginLocalX = __tmp__X;
beginLocalY = __tmp__Y;
beginLocalZ = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = transform._rotation00 * endLocalX + transform._rotation10 * endLocalY + transform._rotation20 * endLocalZ;
__tmp__Y1 = transform._rotation01 * endLocalX + transform._rotation11 * endLocalY + transform._rotation21 * endLocalZ;
__tmp__Z1 = transform._rotation02 * endLocalX + transform._rotation12 * endLocalY + transform._rotation22 * endLocalZ;
endLocalX = __tmp__X1;
endLocalY = __tmp__Y1;
endLocalZ = __tmp__Z1;
if(this._rayCastLocal(beginLocalX,beginLocalY,beginLocalZ,endLocalX,endLocalY,endLocalZ,hit)) {
let localPosX;
let localPosY;
let localPosZ;
let localNormalX;
let localNormalY;
let localNormalZ;
let v = hit.position;
localPosX = v.x;
localPosY = v.y;
localPosZ = v.z;
let v1 = hit.normal;
localNormalX = v1.x;
localNormalY = v1.y;
localNormalZ = v1.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = transform._rotation00 * localPosX + transform._rotation01 * localPosY + transform._rotation02 * localPosZ;
__tmp__Y = transform._rotation10 * localPosX + transform._rotation11 * localPosY + transform._rotation12 * localPosZ;
__tmp__Z = transform._rotation20 * localPosX + transform._rotation21 * localPosY + transform._rotation22 * localPosZ;
localPosX = __tmp__X;
localPosY = __tmp__Y;
localPosZ = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = transform._rotation00 * localNormalX + transform._rotation01 * localNormalY + transform._rotation02 * localNormalZ;
__tmp__Y1 = transform._rotation10 * localNormalX + transform._rotation11 * localNormalY + transform._rotation12 * localNormalZ;
__tmp__Z1 = transform._rotation20 * localNormalX + transform._rotation21 * localNormalY + transform._rotation22 * localNormalZ;
localNormalX = __tmp__X1;
localNormalY = __tmp__Y1;
localNormalZ = __tmp__Z1;
localPosX += transform._positionX;
localPosY += transform._positionY;
localPosZ += transform._positionZ;
let v2 = hit.position;
v2.x = localPosX;
v2.y = localPosY;
v2.z = localPosZ;
let v3 = hit.normal;
v3.x = localNormalX;
v3.y = localNormalY;
v3.z = localNormalZ;
return true;
}
return false;
}
}
oimo.collision.geometry.ConvexGeometry = class oimo_collision_geometry_ConvexGeometry extends oimo.collision.geometry.Geometry {
constructor(type) {
super(type);
this._gjkMargin = oimo.common.Setting.defaultGJKMargin;
this._useGjkRayCast = false;
}
getGjkMergin() {
return this._gjkMargin;
}
setGjkMergin(gjkMergin) {
if(gjkMergin < 0) {
gjkMergin = 0;
}
this._gjkMargin = gjkMergin;
}
computeLocalSupportingVertex(dir,out) {
}
rayCast(begin,end,transform,hit) {
if(this._useGjkRayCast) {
return oimo.collision.narrowphase.detector.gjkepa.GjkEpa.instance.rayCast(this,transform,begin,end,hit);
} else {
return super.rayCast(begin,end,transform,hit);
}
}
}
if(!oimo.collision.broadphase._BroadPhase) oimo.collision.broadphase._BroadPhase = {};
oimo.collision.broadphase._BroadPhase.ConvexSweepGeometry = class oimo_collision_broadphase__$BroadPhase_ConvexSweepGeometry extends oimo.collision.geometry.ConvexGeometry {
constructor() {
super(-1);
}
init(c,transform,translation) {
this.c = c;
let trX;
let trY;
let trZ;
trX = translation.x;
trY = translation.y;
trZ = translation.z;
let localTrX;
let localTrY;
let localTrZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = transform._rotation00 * trX + transform._rotation10 * trY + transform._rotation20 * trZ;
__tmp__Y = transform._rotation01 * trX + transform._rotation11 * trY + transform._rotation21 * trZ;
__tmp__Z = transform._rotation02 * trX + transform._rotation12 * trY + transform._rotation22 * trZ;
localTrX = __tmp__X;
localTrY = __tmp__Y;
localTrZ = __tmp__Z;
this.localTranslation = new oimo.common.Vec3();
let v = this.localTranslation;
v.x = localTrX;
v.y = localTrY;
v.z = localTrZ;
this._gjkMargin = c._gjkMargin;
}
computeLocalSupportingVertex(dir,out) {
this.c.computeLocalSupportingVertex(dir,out);
let v = this.localTranslation;
if(dir.x * v.x + dir.y * v.y + dir.z * v.z > 0) {
let v = this.localTranslation;
out.x += v.x;
out.y += v.y;
out.z += v.z;
}
}
}
oimo.collision.broadphase._BroadPhase.AabbGeometry = class oimo_collision_broadphase__$BroadPhase_AabbGeometry extends oimo.collision.geometry.ConvexGeometry {
constructor() {
super(-1);
this.min = new oimo.common.Vec3();
this.max = new oimo.common.Vec3();
}
computeLocalSupportingVertex(dir,out) {
out.x = dir.x > 0 ? this.max.x : this.min.x;
out.y = dir.y > 0 ? this.max.y : this.min.y;
out.z = dir.z > 0 ? this.max.z : this.min.z;
}
}
oimo.collision.broadphase.BroadPhaseProxyCallback = class oimo_collision_broadphase_BroadPhaseProxyCallback {
constructor() {
}
process(proxy) {
}
}
oimo.collision.broadphase.BroadPhaseType = class oimo_collision_broadphase_BroadPhaseType {
}
oimo.collision.broadphase.Proxy = class oimo_collision_broadphase_Proxy {
constructor(userData,id) {
this.userData = userData;
this._id = id;
this._prev = null;
this._next = null;
this._aabbMinX = 0;
this._aabbMinY = 0;
this._aabbMinZ = 0;
this._aabbMaxX = 0;
this._aabbMaxY = 0;
this._aabbMaxZ = 0;
}
getId() {
return this._id;
}
getFatAabb() {
let aabb = new oimo.collision.geometry.Aabb();
aabb._minX = this._aabbMinX;
aabb._minY = this._aabbMinY;
aabb._minZ = this._aabbMinZ;
aabb._maxX = this._aabbMaxX;
aabb._maxY = this._aabbMaxY;
aabb._maxZ = this._aabbMaxZ;
return aabb;
}
getFatAabbTo(aabb) {
aabb._minX = this._aabbMinX;
aabb._minY = this._aabbMinY;
aabb._minZ = this._aabbMinZ;
aabb._maxX = this._aabbMaxX;
aabb._maxY = this._aabbMaxY;
aabb._maxZ = this._aabbMaxZ;
}
}
oimo.collision.broadphase.ProxyPair = class oimo_collision_broadphase_ProxyPair {
constructor() {
this._p1 = null;
this._p2 = null;
}
getProxy1() {
return this._p1;
}
getProxy2() {
return this._p2;
}
getNext() {
return this._next;
}
}
if(!oimo.collision.broadphase.bruteforce) oimo.collision.broadphase.bruteforce = {};
oimo.collision.broadphase.bruteforce.BruteForceBroadPhase = class oimo_collision_broadphase_bruteforce_BruteForceBroadPhase extends oimo.collision.broadphase.BroadPhase {
constructor() {
super(1);
this._incremental = false;
}
createProxy(userData,aabb) {
let proxy = new oimo.collision.broadphase.Proxy(userData,this._idCount++);
this._numProxies++;
if(this._proxyList == null) {
this._proxyList = proxy;
this._proxyListLast = proxy;
} else {
this._proxyListLast._next = proxy;
proxy._prev = this._proxyListLast;
this._proxyListLast = proxy;
}
proxy._aabbMinX = aabb._minX;
proxy._aabbMinY = aabb._minY;
proxy._aabbMinZ = aabb._minZ;
proxy._aabbMaxX = aabb._maxX;
proxy._aabbMaxY = aabb._maxY;
proxy._aabbMaxZ = aabb._maxZ;
return proxy;
}
destroyProxy(proxy) {
this._numProxies--;
let prev = proxy._prev;
let next = proxy._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(proxy == this._proxyList) {
this._proxyList = this._proxyList._next;
}
if(proxy == this._proxyListLast) {
this._proxyListLast = this._proxyListLast._prev;
}
proxy._next = null;
proxy._prev = null;
proxy.userData = null;
}
moveProxy(proxy,aabb,dislacement) {
proxy._aabbMinX = aabb._minX;
proxy._aabbMinY = aabb._minY;
proxy._aabbMinZ = aabb._minZ;
proxy._aabbMaxX = aabb._maxX;
proxy._aabbMaxY = aabb._maxY;
proxy._aabbMaxZ = aabb._maxZ;
}
collectPairs() {
let p = this._proxyPairList;
if(p != null) {
while(true) {
p._p1 = null;
p._p2 = null;
p = p._next;
if(!(p != null)) {
break;
}
}
this._proxyPairList._next = this._proxyPairPool;
this._proxyPairPool = this._proxyPairList;
this._proxyPairList = null;
}
this._testCount = 0;
let p1 = this._proxyList;
while(p1 != null) {
let n = p1._next;
let p2 = p1._next;
while(p2 != null) {
let n = p2._next;
this._testCount++;
if(p1._aabbMinX < p2._aabbMaxX && p1._aabbMaxX > p2._aabbMinX && p1._aabbMinY < p2._aabbMaxY && p1._aabbMaxY > p2._aabbMinY && p1._aabbMinZ < p2._aabbMaxZ && p1._aabbMaxZ > p2._aabbMinZ) {
let first = this._proxyPairPool;
if(first != null) {
this._proxyPairPool = first._next;
first._next = null;
} else {
first = new oimo.collision.broadphase.ProxyPair();
}
let pp = first;
if(this._proxyPairList == null) {
this._proxyPairList = pp;
} else {
pp._next = this._proxyPairList;
this._proxyPairList = pp;
}
pp._p1 = p1;
pp._p2 = p2;
}
p2 = n;
}
p1 = n;
}
}
rayCast(begin,end,callback) {
let p1X;
let p1Y;
let p1Z;
let p2X;
let p2Y;
let p2Z;
p1X = begin.x;
p1Y = begin.y;
p1Z = begin.z;
p2X = end.x;
p2Y = end.y;
p2Z = end.z;
let p = this._proxyList;
while(p != null) {
let n = p._next;
let x1 = p1X;
let y1 = p1Y;
let z1 = p1Z;
let x2 = p2X;
let y2 = p2Y;
let z2 = p2Z;
let pminx = p._aabbMinX;
let pminy = p._aabbMinY;
let pminz = p._aabbMinZ;
let pmaxx = p._aabbMaxX;
let pmaxy = p._aabbMaxY;
let pmaxz = p._aabbMaxZ;
let tmp;
if(pminx > (x1 > x2 ? x1 : x2) || pmaxx < (x1 < x2 ? x1 : x2) || pminy > (y1 > y2 ? y1 : y2) || pmaxy < (y1 < y2 ? y1 : y2) || pminz > (z1 > z2 ? z1 : z2) || pmaxz < (z1 < z2 ? z1 : z2)) {
tmp = false;
} else {
let dx = x2 - x1;
let dy = y2 - y1;
let dz = z2 - z1;
let adx = dx < 0 ? -dx : dx;
let ady = dy < 0 ? -dy : dy;
let adz = dz < 0 ? -dz : dz;
let pextx = (pmaxx - pminx) * 0.5;
let pexty = (pmaxy - pminy) * 0.5;
let pextz = (pmaxz - pminz) * 0.5;
let cpx = x1 - (pmaxx + pminx) * 0.5;
let cpy = y1 - (pmaxy + pminy) * 0.5;
let cpz = z1 - (pmaxz + pminz) * 0.5;
let tmp1;
let tmp2;
let x = cpy * dz - cpz * dy;
if(!((x < 0 ? -x : x) - (pexty * adz + pextz * ady) > 0)) {
let x = cpz * dx - cpx * dz;
tmp2 = (x < 0 ? -x : x) - (pextz * adx + pextx * adz) > 0;
} else {
tmp2 = true;
}
if(!tmp2) {
let x = cpx * dy - cpy * dx;
tmp1 = (x < 0 ? -x : x) - (pextx * ady + pexty * adx) > 0;
} else {
tmp1 = true;
}
tmp = tmp1 ? false : true;
}
if(tmp) {
callback.process(p);
}
p = n;
}
}
convexCast(convex,begin,translation,callback) {
let p = this._proxyList;
while(p != null) {
let n = p._next;
let v = this._aabb.min;
v.x = p._aabbMinX;
v.y = p._aabbMinY;
v.z = p._aabbMinZ;
let v1 = this._aabb.max;
v1.x = p._aabbMaxX;
v1.y = p._aabbMaxY;
v1.z = p._aabbMaxZ;
this._convexSweep.init(convex,begin,translation);
let gjkEpa = oimo.collision.narrowphase.detector.gjkepa.GjkEpa.instance;
if(gjkEpa.computeClosestPointsImpl(this._convexSweep,this._aabb,begin,this.identity,null,false) == 0 && gjkEpa.distance <= 0) {
callback.process(p);
}
p = n;
}
}
aabbTest(aabb,callback) {
let p = this._proxyList;
while(p != null) {
let n = p._next;
if(aabb._minX < p._aabbMaxX && aabb._maxX > p._aabbMinX && aabb._minY < p._aabbMaxY && aabb._maxY > p._aabbMinY && aabb._minZ < p._aabbMaxZ && aabb._maxZ > p._aabbMinZ) {
callback.process(p);
}
p = n;
}
}
}
if(!oimo.collision.broadphase.bvh) oimo.collision.broadphase.bvh = {};
oimo.collision.broadphase.bvh.BvhBroadPhase = class oimo_collision_broadphase_bvh_BvhBroadPhase extends oimo.collision.broadphase.BroadPhase {
constructor() {
super(2);
this._incremental = true;
this._tree = new oimo.collision.broadphase.bvh.BvhTree();
this.movedProxies = new Array(1024);
this.numMovedProxies = 0;
}
collide(n1,n2) {
this._testCount++;
let l1 = n1._height == 0;
let l2 = n2._height == 0;
if(n1 == n2) {
if(l1) {
return;
}
this.collide(n1._children[0],n2);
this.collide(n1._children[1],n2);
return;
}
if(!(n1._aabbMinX < n2._aabbMaxX && n1._aabbMaxX > n2._aabbMinX && n1._aabbMinY < n2._aabbMaxY && n1._aabbMaxY > n2._aabbMinY && n1._aabbMinZ < n2._aabbMaxZ && n1._aabbMaxZ > n2._aabbMinZ)) {
return;
}
if(l1 && l2) {
let first = this._proxyPairPool;
if(first != null) {
this._proxyPairPool = first._next;
first._next = null;
} else {
first = new oimo.collision.broadphase.ProxyPair();
}
let pp = first;
if(this._proxyPairList == null) {
this._proxyPairList = pp;
} else {
pp._next = this._proxyPairList;
this._proxyPairList = pp;
}
pp._p1 = n1._proxy;
pp._p2 = n2._proxy;
return;
}
if(l2 || n1._height > n2._height) {
this.collide(n1._children[0],n2);
this.collide(n1._children[1],n2);
} else {
this.collide(n2._children[0],n1);
this.collide(n2._children[1],n1);
}
}
rayCastRecursive(node,_p1X,_p1Y,_p1Z,_p2X,_p2Y,_p2Z,callback) {
let x1 = _p1X;
let y1 = _p1Y;
let z1 = _p1Z;
let x2 = _p2X;
let y2 = _p2Y;
let z2 = _p2Z;
let pminx = node._aabbMinX;
let pminy = node._aabbMinY;
let pminz = node._aabbMinZ;
let pmaxx = node._aabbMaxX;
let pmaxy = node._aabbMaxY;
let pmaxz = node._aabbMaxZ;
let tmp;
if(pminx > (x1 > x2 ? x1 : x2) || pmaxx < (x1 < x2 ? x1 : x2) || pminy > (y1 > y2 ? y1 : y2) || pmaxy < (y1 < y2 ? y1 : y2) || pminz > (z1 > z2 ? z1 : z2) || pmaxz < (z1 < z2 ? z1 : z2)) {
tmp = false;
} else {
let dx = x2 - x1;
let dy = y2 - y1;
let dz = z2 - z1;
let adx = dx < 0 ? -dx : dx;
let ady = dy < 0 ? -dy : dy;
let adz = dz < 0 ? -dz : dz;
let pextx = (pmaxx - pminx) * 0.5;
let pexty = (pmaxy - pminy) * 0.5;
let pextz = (pmaxz - pminz) * 0.5;
let cpx = x1 - (pmaxx + pminx) * 0.5;
let cpy = y1 - (pmaxy + pminy) * 0.5;
let cpz = z1 - (pmaxz + pminz) * 0.5;
let tmp1;
let tmp2;
let x = cpy * dz - cpz * dy;
if(!((x < 0 ? -x : x) - (pexty * adz + pextz * ady) > 0)) {
let x = cpz * dx - cpx * dz;
tmp2 = (x < 0 ? -x : x) - (pextz * adx + pextx * adz) > 0;
} else {
tmp2 = true;
}
if(!tmp2) {
let x = cpx * dy - cpy * dx;
tmp1 = (x < 0 ? -x : x) - (pextx * ady + pexty * adx) > 0;
} else {
tmp1 = true;
}
tmp = tmp1 ? false : true;
}
if(!tmp) {
return;
}
if(node._height == 0) {
callback.process(node._proxy);
return;
}
this.rayCastRecursive(node._children[0],_p1X,_p1Y,_p1Z,_p2X,_p2Y,_p2Z,callback);
this.rayCastRecursive(node._children[1],_p1X,_p1Y,_p1Z,_p2X,_p2Y,_p2Z,callback);
}
convexCastRecursive(node,convex,begin,translation,callback) {
let v = this._aabb.min;
v.x = node._aabbMinX;
v.y = node._aabbMinY;
v.z = node._aabbMinZ;
let v1 = this._aabb.max;
v1.x = node._aabbMaxX;
v1.y = node._aabbMaxY;
v1.z = node._aabbMaxZ;
this._convexSweep.init(convex,begin,translation);
let gjkEpa = oimo.collision.narrowphase.detector.gjkepa.GjkEpa.instance;
if(!(gjkEpa.computeClosestPointsImpl(this._convexSweep,this._aabb,begin,this.identity,null,false) == 0 && gjkEpa.distance <= 0)) {
return;
}
if(node._height == 0) {
callback.process(node._proxy);
return;
}
this.convexCastRecursive(node._children[0],convex,begin,translation,callback);
this.convexCastRecursive(node._children[1],convex,begin,translation,callback);
}
aabbTestRecursive(node,aabb,callback) {
if(!(node._aabbMinX < aabb._maxX && node._aabbMaxX > aabb._minX && node._aabbMinY < aabb._maxY && node._aabbMaxY > aabb._minY && node._aabbMinZ < aabb._maxZ && node._aabbMaxZ > aabb._minZ)) {
return;
}
if(node._height == 0) {
callback.process(node._proxy);
return;
}
this.aabbTestRecursive(node._children[0],aabb,callback);
this.aabbTestRecursive(node._children[1],aabb,callback);
}
createProxy(userData,aabb) {
let p = new oimo.collision.broadphase.bvh.BvhProxy(userData,this._idCount++);
this._numProxies++;
if(this._proxyList == null) {
this._proxyList = p;
this._proxyListLast = p;
} else {
this._proxyListLast._next = p;
p._prev = this._proxyListLast;
this._proxyListLast = p;
}
p._aabbMinX = aabb._minX;
p._aabbMinY = aabb._minY;
p._aabbMinZ = aabb._minZ;
p._aabbMaxX = aabb._maxX;
p._aabbMaxY = aabb._maxY;
p._aabbMaxZ = aabb._maxZ;
let padding = oimo.common.Setting.bvhProxyPadding;
p._aabbMinX -= padding;
p._aabbMinY -= padding;
p._aabbMinZ -= padding;
p._aabbMaxX += padding;
p._aabbMaxY += padding;
p._aabbMaxZ += padding;
let _this = this._tree;
let first = _this._nodePool;
if(first != null) {
_this._nodePool = first._next;
first._next = null;
} else {
first = new oimo.collision.broadphase.bvh.BvhNode();
}
let leaf = first;
leaf._proxy = p;
p._leaf = leaf;
leaf._aabbMinX = p._aabbMinX;
leaf._aabbMinY = p._aabbMinY;
leaf._aabbMinZ = p._aabbMinZ;
leaf._aabbMaxX = p._aabbMaxX;
leaf._aabbMaxY = p._aabbMaxY;
leaf._aabbMaxZ = p._aabbMaxZ;
_this._numLeaves++;
if(_this.leafList == null) {
_this.leafList = leaf;
_this.leafListLast = leaf;
} else {
_this.leafListLast._nextLeaf = leaf;
leaf._prevLeaf = _this.leafListLast;
_this.leafListLast = leaf;
}
if(_this._root == null) {
_this._root = leaf;
} else {
let sibling = _this._root;
while(sibling._height > 0) {
let nextStep = _this._strategy._decideInsertion(sibling,leaf);
if(nextStep == -1) {
break;
} else {
sibling = sibling._children[nextStep];
}
}
let parent = sibling._parent;
let first = _this._nodePool;
if(first != null) {
_this._nodePool = first._next;
first._next = null;
} else {
first = new oimo.collision.broadphase.bvh.BvhNode();
}
let node = first;
if(parent == null) {
_this._root = node;
} else {
let index = sibling._childIndex;
parent._children[index] = node;
node._parent = parent;
node._childIndex = index;
}
let index = sibling._childIndex;
node._children[index] = sibling;
sibling._parent = node;
sibling._childIndex = index;
let index1 = sibling._childIndex ^ 1;
node._children[index1] = leaf;
leaf._parent = node;
leaf._childIndex = index1;
while(node != null) {
if(_this._strategy._balancingEnabled) {
if(node._height >= 2) {
let p = node._parent;
let l = node._children[0];
let r = node._children[1];
let balance = l._height - r._height;
let nodeIndex = node._childIndex;
if(balance > 1) {
let ll = l._children[0];
let lr = l._children[1];
if(ll._height > lr._height) {
l._children[1] = node;
node._parent = l;
node._childIndex = 1;
node._children[0] = lr;
lr._parent = node;
lr._childIndex = 0;
let c1 = l._children[0];
let c2 = l._children[1];
l._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
l._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
l._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
l._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
l._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
l._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = l._children[0]._height;
let h2 = l._children[1]._height;
l._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
} else {
l._children[0] = node;
node._parent = l;
node._childIndex = 0;
node._children[0] = ll;
ll._parent = node;
ll._childIndex = 0;
let c1 = l._children[0];
let c2 = l._children[1];
l._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
l._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
l._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
l._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
l._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
l._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = l._children[0]._height;
let h2 = l._children[1]._height;
l._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
}
if(p != null) {
p._children[nodeIndex] = l;
l._parent = p;
l._childIndex = nodeIndex;
} else {
_this._root = l;
l._parent = null;
}
node = l;
} else if(balance < -1) {
let rl = r._children[0];
let rr = r._children[1];
if(rl._height > rr._height) {
r._children[1] = node;
node._parent = r;
node._childIndex = 1;
node._children[1] = rr;
rr._parent = node;
rr._childIndex = 1;
let c1 = r._children[0];
let c2 = r._children[1];
r._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
r._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
r._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
r._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
r._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
r._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = r._children[0]._height;
let h2 = r._children[1]._height;
r._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
} else {
r._children[0] = node;
node._parent = r;
node._childIndex = 0;
node._children[1] = rl;
rl._parent = node;
rl._childIndex = 1;
let c1 = r._children[0];
let c2 = r._children[1];
r._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
r._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
r._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
r._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
r._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
r._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = r._children[0]._height;
let h2 = r._children[1]._height;
r._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
}
if(p != null) {
p._children[nodeIndex] = r;
r._parent = p;
r._childIndex = nodeIndex;
} else {
_this._root = r;
r._parent = null;
}
node = r;
}
}
}
let h1 = node._children[0]._height;
let h2 = node._children[1]._height;
node._height = (h1 > h2 ? h1 : h2) + 1;
let c1 = node._children[0];
let c2 = node._children[1];
node._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
node._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
node._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
node._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
node._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
node._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
node = node._parent;
}
}
if(!p._moved) {
p._moved = true;
if(this.movedProxies.length == this.numMovedProxies) {
let newArray = new Array(this.numMovedProxies << 1);
let _g = 0;
let _g1 = this.numMovedProxies;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.movedProxies[i];
this.movedProxies[i] = null;
}
this.movedProxies = newArray;
}
this.movedProxies[this.numMovedProxies++] = p;
}
return p;
}
destroyProxy(proxy) {
this._numProxies--;
let prev = proxy._prev;
let next = proxy._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(proxy == this._proxyList) {
this._proxyList = this._proxyList._next;
}
if(proxy == this._proxyListLast) {
this._proxyListLast = this._proxyListLast._prev;
}
proxy._next = null;
proxy._prev = null;
let bvhProxy = proxy;
let _this = this._tree;
let leaf = bvhProxy._leaf;
_this._numLeaves--;
let prev1 = leaf._prevLeaf;
let next1 = leaf._nextLeaf;
if(prev1 != null) {
prev1._nextLeaf = next1;
}
if(next1 != null) {
next1._prevLeaf = prev1;
}
if(leaf == _this.leafList) {
_this.leafList = _this.leafList._nextLeaf;
}
if(leaf == _this.leafListLast) {
_this.leafListLast = _this.leafListLast._prevLeaf;
}
leaf._nextLeaf = null;
leaf._prevLeaf = null;
if(_this._root == leaf) {
_this._root = null;
} else {
let parent = leaf._parent;
let sibling = parent._children[leaf._childIndex ^ 1];
let grandParent = parent._parent;
if(grandParent == null) {
sibling._parent = null;
sibling._childIndex = 0;
_this._root = sibling;
parent._next = null;
parent._childIndex = 0;
parent._children[0] = null;
parent._children[1] = null;
parent._childIndex = 0;
parent._parent = null;
parent._height = 0;
parent._proxy = null;
parent._next = _this._nodePool;
_this._nodePool = parent;
} else {
sibling._parent = grandParent;
let index = parent._childIndex;
grandParent._children[index] = sibling;
sibling._parent = grandParent;
sibling._childIndex = index;
parent._next = null;
parent._childIndex = 0;
parent._children[0] = null;
parent._children[1] = null;
parent._childIndex = 0;
parent._parent = null;
parent._height = 0;
parent._proxy = null;
parent._next = _this._nodePool;
_this._nodePool = parent;
let node = grandParent;
while(node != null) {
if(_this._strategy._balancingEnabled) {
if(node._height >= 2) {
let p = node._parent;
let l = node._children[0];
let r = node._children[1];
let balance = l._height - r._height;
let nodeIndex = node._childIndex;
if(balance > 1) {
let ll = l._children[0];
let lr = l._children[1];
if(ll._height > lr._height) {
l._children[1] = node;
node._parent = l;
node._childIndex = 1;
node._children[0] = lr;
lr._parent = node;
lr._childIndex = 0;
let c1 = l._children[0];
let c2 = l._children[1];
l._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
l._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
l._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
l._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
l._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
l._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = l._children[0]._height;
let h2 = l._children[1]._height;
l._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
} else {
l._children[0] = node;
node._parent = l;
node._childIndex = 0;
node._children[0] = ll;
ll._parent = node;
ll._childIndex = 0;
let c1 = l._children[0];
let c2 = l._children[1];
l._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
l._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
l._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
l._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
l._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
l._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = l._children[0]._height;
let h2 = l._children[1]._height;
l._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
}
if(p != null) {
p._children[nodeIndex] = l;
l._parent = p;
l._childIndex = nodeIndex;
} else {
_this._root = l;
l._parent = null;
}
node = l;
} else if(balance < -1) {
let rl = r._children[0];
let rr = r._children[1];
if(rl._height > rr._height) {
r._children[1] = node;
node._parent = r;
node._childIndex = 1;
node._children[1] = rr;
rr._parent = node;
rr._childIndex = 1;
let c1 = r._children[0];
let c2 = r._children[1];
r._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
r._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
r._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
r._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
r._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
r._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = r._children[0]._height;
let h2 = r._children[1]._height;
r._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
} else {
r._children[0] = node;
node._parent = r;
node._childIndex = 0;
node._children[1] = rl;
rl._parent = node;
rl._childIndex = 1;
let c1 = r._children[0];
let c2 = r._children[1];
r._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
r._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
r._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
r._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
r._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
r._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = r._children[0]._height;
let h2 = r._children[1]._height;
r._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
}
if(p != null) {
p._children[nodeIndex] = r;
r._parent = p;
r._childIndex = nodeIndex;
} else {
_this._root = r;
r._parent = null;
}
node = r;
}
}
}
let h1 = node._children[0]._height;
let h2 = node._children[1]._height;
node._height = (h1 > h2 ? h1 : h2) + 1;
let c1 = node._children[0];
let c2 = node._children[1];
node._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
node._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
node._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
node._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
node._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
node._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
node = node._parent;
}
}
}
bvhProxy._leaf = null;
leaf._next = null;
leaf._childIndex = 0;
leaf._children[0] = null;
leaf._children[1] = null;
leaf._childIndex = 0;
leaf._parent = null;
leaf._height = 0;
leaf._proxy = null;
leaf._next = _this._nodePool;
_this._nodePool = leaf;
bvhProxy.userData = null;
bvhProxy._next = null;
bvhProxy._prev = null;
if(bvhProxy._moved) {
bvhProxy._moved = false;
}
}
moveProxy(proxy,aabb,displacement) {
let p = proxy;
if(p._aabbMinX <= aabb._minX && p._aabbMaxX >= aabb._maxX && p._aabbMinY <= aabb._minY && p._aabbMaxY >= aabb._maxY && p._aabbMinZ <= aabb._minZ && p._aabbMaxZ >= aabb._maxZ) {
return;
}
p._aabbMinX = aabb._minX;
p._aabbMinY = aabb._minY;
p._aabbMinZ = aabb._minZ;
p._aabbMaxX = aabb._maxX;
p._aabbMaxY = aabb._maxY;
p._aabbMaxZ = aabb._maxZ;
let padding = oimo.common.Setting.bvhProxyPadding;
p._aabbMinX -= padding;
p._aabbMinY -= padding;
p._aabbMinZ -= padding;
p._aabbMaxX += padding;
p._aabbMaxY += padding;
p._aabbMaxZ += padding;
if(displacement != null) {
let dX;
let dY;
let dZ;
let zeroX;
let zeroY;
let zeroZ;
let addToMinX;
let addToMinY;
let addToMinZ;
let addToMaxX;
let addToMaxY;
let addToMaxZ;
zeroX = 0;
zeroY = 0;
zeroZ = 0;
dX = displacement.x;
dY = displacement.y;
dZ = displacement.z;
addToMinX = zeroX < dX ? zeroX : dX;
addToMinY = zeroY < dY ? zeroY : dY;
addToMinZ = zeroZ < dZ ? zeroZ : dZ;
addToMaxX = zeroX > dX ? zeroX : dX;
addToMaxY = zeroY > dY ? zeroY : dY;
addToMaxZ = zeroZ > dZ ? zeroZ : dZ;
p._aabbMinX += addToMinX;
p._aabbMinY += addToMinY;
p._aabbMinZ += addToMinZ;
p._aabbMaxX += addToMaxX;
p._aabbMaxY += addToMaxY;
p._aabbMaxZ += addToMaxZ;
}
if(!p._moved) {
p._moved = true;
if(this.movedProxies.length == this.numMovedProxies) {
let newArray = new Array(this.numMovedProxies << 1);
let _g = 0;
let _g1 = this.numMovedProxies;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.movedProxies[i];
this.movedProxies[i] = null;
}
this.movedProxies = newArray;
}
this.movedProxies[this.numMovedProxies++] = p;
}
}
collectPairs() {
let p = this._proxyPairList;
if(p != null) {
while(true) {
p._p1 = null;
p._p2 = null;
p = p._next;
if(!(p != null)) {
break;
}
}
this._proxyPairList._next = this._proxyPairPool;
this._proxyPairPool = this._proxyPairList;
this._proxyPairList = null;
}
this._testCount = 0;
if(this._numProxies < 2) {
return;
}
let incrementalCollision = this.numMovedProxies / this._numProxies < oimo.common.Setting.bvhIncrementalCollisionThreshold;
let _g = 0;
let _g1 = this.numMovedProxies;
while(_g < _g1) {
let i = _g++;
let p = this.movedProxies[i];
if(p._moved) {
let _this = this._tree;
let leaf = p._leaf;
_this._numLeaves--;
let prev = leaf._prevLeaf;
let next = leaf._nextLeaf;
if(prev != null) {
prev._nextLeaf = next;
}
if(next != null) {
next._prevLeaf = prev;
}
if(leaf == _this.leafList) {
_this.leafList = _this.leafList._nextLeaf;
}
if(leaf == _this.leafListLast) {
_this.leafListLast = _this.leafListLast._prevLeaf;
}
leaf._nextLeaf = null;
leaf._prevLeaf = null;
if(_this._root == leaf) {
_this._root = null;
} else {
let parent = leaf._parent;
let sibling = parent._children[leaf._childIndex ^ 1];
let grandParent = parent._parent;
if(grandParent == null) {
sibling._parent = null;
sibling._childIndex = 0;
_this._root = sibling;
parent._next = null;
parent._childIndex = 0;
parent._children[0] = null;
parent._children[1] = null;
parent._childIndex = 0;
parent._parent = null;
parent._height = 0;
parent._proxy = null;
parent._next = _this._nodePool;
_this._nodePool = parent;
} else {
sibling._parent = grandParent;
let index = parent._childIndex;
grandParent._children[index] = sibling;
sibling._parent = grandParent;
sibling._childIndex = index;
parent._next = null;
parent._childIndex = 0;
parent._children[0] = null;
parent._children[1] = null;
parent._childIndex = 0;
parent._parent = null;
parent._height = 0;
parent._proxy = null;
parent._next = _this._nodePool;
_this._nodePool = parent;
let node = grandParent;
while(node != null) {
if(_this._strategy._balancingEnabled) {
if(node._height >= 2) {
let p = node._parent;
let l = node._children[0];
let r = node._children[1];
let balance = l._height - r._height;
let nodeIndex = node._childIndex;
if(balance > 1) {
let ll = l._children[0];
let lr = l._children[1];
if(ll._height > lr._height) {
l._children[1] = node;
node._parent = l;
node._childIndex = 1;
node._children[0] = lr;
lr._parent = node;
lr._childIndex = 0;
let c1 = l._children[0];
let c2 = l._children[1];
l._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
l._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
l._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
l._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
l._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
l._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = l._children[0]._height;
let h2 = l._children[1]._height;
l._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
} else {
l._children[0] = node;
node._parent = l;
node._childIndex = 0;
node._children[0] = ll;
ll._parent = node;
ll._childIndex = 0;
let c1 = l._children[0];
let c2 = l._children[1];
l._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
l._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
l._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
l._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
l._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
l._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = l._children[0]._height;
let h2 = l._children[1]._height;
l._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
}
if(p != null) {
p._children[nodeIndex] = l;
l._parent = p;
l._childIndex = nodeIndex;
} else {
_this._root = l;
l._parent = null;
}
node = l;
} else if(balance < -1) {
let rl = r._children[0];
let rr = r._children[1];
if(rl._height > rr._height) {
r._children[1] = node;
node._parent = r;
node._childIndex = 1;
node._children[1] = rr;
rr._parent = node;
rr._childIndex = 1;
let c1 = r._children[0];
let c2 = r._children[1];
r._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
r._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
r._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
r._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
r._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
r._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = r._children[0]._height;
let h2 = r._children[1]._height;
r._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
} else {
r._children[0] = node;
node._parent = r;
node._childIndex = 0;
node._children[1] = rl;
rl._parent = node;
rl._childIndex = 1;
let c1 = r._children[0];
let c2 = r._children[1];
r._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
r._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
r._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
r._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
r._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
r._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = r._children[0]._height;
let h2 = r._children[1]._height;
r._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
}
if(p != null) {
p._children[nodeIndex] = r;
r._parent = p;
r._childIndex = nodeIndex;
} else {
_this._root = r;
r._parent = null;
}
node = r;
}
}
}
let h1 = node._children[0]._height;
let h2 = node._children[1]._height;
node._height = (h1 > h2 ? h1 : h2) + 1;
let c1 = node._children[0];
let c2 = node._children[1];
node._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
node._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
node._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
node._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
node._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
node._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
node = node._parent;
}
}
}
p._leaf = null;
leaf._next = null;
leaf._childIndex = 0;
leaf._children[0] = null;
leaf._children[1] = null;
leaf._childIndex = 0;
leaf._parent = null;
leaf._height = 0;
leaf._proxy = null;
leaf._next = _this._nodePool;
_this._nodePool = leaf;
let _this1 = this._tree;
let first = _this1._nodePool;
if(first != null) {
_this1._nodePool = first._next;
first._next = null;
} else {
first = new oimo.collision.broadphase.bvh.BvhNode();
}
let leaf1 = first;
leaf1._proxy = p;
p._leaf = leaf1;
leaf1._aabbMinX = p._aabbMinX;
leaf1._aabbMinY = p._aabbMinY;
leaf1._aabbMinZ = p._aabbMinZ;
leaf1._aabbMaxX = p._aabbMaxX;
leaf1._aabbMaxY = p._aabbMaxY;
leaf1._aabbMaxZ = p._aabbMaxZ;
_this1._numLeaves++;
if(_this1.leafList == null) {
_this1.leafList = leaf1;
_this1.leafListLast = leaf1;
} else {
_this1.leafListLast._nextLeaf = leaf1;
leaf1._prevLeaf = _this1.leafListLast;
_this1.leafListLast = leaf1;
}
if(_this1._root == null) {
_this1._root = leaf1;
} else {
let sibling = _this1._root;
while(sibling._height > 0) {
let nextStep = _this1._strategy._decideInsertion(sibling,leaf1);
if(nextStep == -1) {
break;
} else {
sibling = sibling._children[nextStep];
}
}
let parent = sibling._parent;
let first = _this1._nodePool;
if(first != null) {
_this1._nodePool = first._next;
first._next = null;
} else {
first = new oimo.collision.broadphase.bvh.BvhNode();
}
let node = first;
if(parent == null) {
_this1._root = node;
} else {
let index = sibling._childIndex;
parent._children[index] = node;
node._parent = parent;
node._childIndex = index;
}
let index = sibling._childIndex;
node._children[index] = sibling;
sibling._parent = node;
sibling._childIndex = index;
let index1 = sibling._childIndex ^ 1;
node._children[index1] = leaf1;
leaf1._parent = node;
leaf1._childIndex = index1;
while(node != null) {
if(_this1._strategy._balancingEnabled) {
if(node._height >= 2) {
let p = node._parent;
let l = node._children[0];
let r = node._children[1];
let balance = l._height - r._height;
let nodeIndex = node._childIndex;
if(balance > 1) {
let ll = l._children[0];
let lr = l._children[1];
if(ll._height > lr._height) {
l._children[1] = node;
node._parent = l;
node._childIndex = 1;
node._children[0] = lr;
lr._parent = node;
lr._childIndex = 0;
let c1 = l._children[0];
let c2 = l._children[1];
l._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
l._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
l._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
l._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
l._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
l._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = l._children[0]._height;
let h2 = l._children[1]._height;
l._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
} else {
l._children[0] = node;
node._parent = l;
node._childIndex = 0;
node._children[0] = ll;
ll._parent = node;
ll._childIndex = 0;
let c1 = l._children[0];
let c2 = l._children[1];
l._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
l._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
l._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
l._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
l._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
l._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = l._children[0]._height;
let h2 = l._children[1]._height;
l._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
}
if(p != null) {
p._children[nodeIndex] = l;
l._parent = p;
l._childIndex = nodeIndex;
} else {
_this1._root = l;
l._parent = null;
}
node = l;
} else if(balance < -1) {
let rl = r._children[0];
let rr = r._children[1];
if(rl._height > rr._height) {
r._children[1] = node;
node._parent = r;
node._childIndex = 1;
node._children[1] = rr;
rr._parent = node;
rr._childIndex = 1;
let c1 = r._children[0];
let c2 = r._children[1];
r._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
r._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
r._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
r._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
r._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
r._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = r._children[0]._height;
let h2 = r._children[1]._height;
r._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
} else {
r._children[0] = node;
node._parent = r;
node._childIndex = 0;
node._children[1] = rl;
rl._parent = node;
rl._childIndex = 1;
let c1 = r._children[0];
let c2 = r._children[1];
r._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
r._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
r._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
r._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
r._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
r._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = r._children[0]._height;
let h2 = r._children[1]._height;
r._height = (h1 > h2 ? h1 : h2) + 1;
let c11 = node._children[0];
let c21 = node._children[1];
node._aabbMinX = c11._aabbMinX < c21._aabbMinX ? c11._aabbMinX : c21._aabbMinX;
node._aabbMinY = c11._aabbMinY < c21._aabbMinY ? c11._aabbMinY : c21._aabbMinY;
node._aabbMinZ = c11._aabbMinZ < c21._aabbMinZ ? c11._aabbMinZ : c21._aabbMinZ;
node._aabbMaxX = c11._aabbMaxX > c21._aabbMaxX ? c11._aabbMaxX : c21._aabbMaxX;
node._aabbMaxY = c11._aabbMaxY > c21._aabbMaxY ? c11._aabbMaxY : c21._aabbMaxY;
node._aabbMaxZ = c11._aabbMaxZ > c21._aabbMaxZ ? c11._aabbMaxZ : c21._aabbMaxZ;
let h11 = node._children[0]._height;
let h21 = node._children[1]._height;
node._height = (h11 > h21 ? h11 : h21) + 1;
}
if(p != null) {
p._children[nodeIndex] = r;
r._parent = p;
r._childIndex = nodeIndex;
} else {
_this1._root = r;
r._parent = null;
}
node = r;
}
}
}
let h1 = node._children[0]._height;
let h2 = node._children[1]._height;
node._height = (h1 > h2 ? h1 : h2) + 1;
let c1 = node._children[0];
let c2 = node._children[1];
node._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
node._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
node._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
node._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
node._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
node._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
node = node._parent;
}
}
if(incrementalCollision) {
this.collide(this._tree._root,p._leaf);
}
p._moved = false;
}
this.movedProxies[i] = null;
}
if(!incrementalCollision) {
this.collide(this._tree._root,this._tree._root);
}
this.numMovedProxies = 0;
}
rayCast(begin,end,callback) {
if(this._tree._root == null) {
return;
}
let p1X;
let p1Y;
let p1Z;
let p2X;
let p2Y;
let p2Z;
p1X = begin.x;
p1Y = begin.y;
p1Z = begin.z;
p2X = end.x;
p2Y = end.y;
p2Z = end.z;
this.rayCastRecursive(this._tree._root,p1X,p1Y,p1Z,p2X,p2Y,p2Z,callback);
}
convexCast(convex,begin,translation,callback) {
if(this._tree._root == null) {
return;
}
this.convexCastRecursive(this._tree._root,convex,begin,translation,callback);
}
aabbTest(aabb,callback) {
if(this._tree._root == null) {
return;
}
this.aabbTestRecursive(this._tree._root,aabb,callback);
}
getTreeBalance() {
return this._tree._getBalance();
}
}
oimo.collision.broadphase.bvh.BvhInsertionStrategy = class oimo_collision_broadphase_bvh_BvhInsertionStrategy {
}
oimo.collision.broadphase.bvh.BvhNode = class oimo_collision_broadphase_bvh_BvhNode {
constructor() {
this._next = null;
this._prevLeaf = null;
this._nextLeaf = null;
this._children = new Array(2);
this._childIndex = 0;
this._parent = null;
this._height = 0;
this._proxy = null;
this._aabbMinX = 0;
this._aabbMinY = 0;
this._aabbMinZ = 0;
this._aabbMaxX = 0;
this._aabbMaxY = 0;
this._aabbMaxZ = 0;
}
}
oimo.collision.broadphase.bvh.BvhProxy = class oimo_collision_broadphase_bvh_BvhProxy extends oimo.collision.broadphase.Proxy {
constructor(userData,id) {
super(userData,id);
this._leaf = null;
this._moved = false;
}
}
oimo.collision.broadphase.bvh.BvhStrategy = class oimo_collision_broadphase_bvh_BvhStrategy {
constructor() {
this._insertionStrategy = 0;
this._balancingEnabled = false;
}
_decideInsertion(currentNode,leaf) {
switch(this._insertionStrategy) {
case 0:
let centerX;
let centerY;
let centerZ;
centerX = leaf._aabbMinX + leaf._aabbMaxX;
centerY = leaf._aabbMinY + leaf._aabbMaxY;
centerZ = leaf._aabbMinZ + leaf._aabbMaxZ;
let c1 = currentNode._children[0];
let c2 = currentNode._children[1];
let diff1X;
let diff1Y;
let diff1Z;
let diff2X;
let diff2Y;
let diff2Z;
diff1X = c1._aabbMinX + c1._aabbMaxX;
diff1Y = c1._aabbMinY + c1._aabbMaxY;
diff1Z = c1._aabbMinZ + c1._aabbMaxZ;
diff2X = c2._aabbMinX + c2._aabbMaxX;
diff2Y = c2._aabbMinY + c2._aabbMaxY;
diff2Z = c2._aabbMinZ + c2._aabbMaxZ;
diff1X -= centerX;
diff1Y -= centerY;
diff1Z -= centerZ;
diff2X -= centerX;
diff2Y -= centerY;
diff2Z -= centerZ;
if(diff1X * diff1X + diff1Y * diff1Y + diff1Z * diff1Z < diff2X * diff2X + diff2Y * diff2Y + diff2Z * diff2Z) {
return 0;
} else {
return 1;
}
break;
case 1:
let c11 = currentNode._children[0];
let c21 = currentNode._children[1];
let ey = currentNode._aabbMaxY - currentNode._aabbMinY;
let ez = currentNode._aabbMaxZ - currentNode._aabbMinZ;
let combinedMinX;
let combinedMinY;
let combinedMinZ;
let combinedMaxX;
let combinedMaxY;
let combinedMaxZ;
combinedMinX = currentNode._aabbMinX < leaf._aabbMinX ? currentNode._aabbMinX : leaf._aabbMinX;
combinedMinY = currentNode._aabbMinY < leaf._aabbMinY ? currentNode._aabbMinY : leaf._aabbMinY;
combinedMinZ = currentNode._aabbMinZ < leaf._aabbMinZ ? currentNode._aabbMinZ : leaf._aabbMinZ;
combinedMaxX = currentNode._aabbMaxX > leaf._aabbMaxX ? currentNode._aabbMaxX : leaf._aabbMaxX;
combinedMaxY = currentNode._aabbMaxY > leaf._aabbMaxY ? currentNode._aabbMaxY : leaf._aabbMaxY;
combinedMaxZ = currentNode._aabbMaxZ > leaf._aabbMaxZ ? currentNode._aabbMaxZ : leaf._aabbMaxZ;
let ey1 = combinedMaxY - combinedMinY;
let ez1 = combinedMaxZ - combinedMinZ;
let newArea = ((combinedMaxX - combinedMinX) * (ey1 + ez1) + ey1 * ez1) * 2;
let creatingCost = newArea * 2;
let incrementalCost = (newArea - ((currentNode._aabbMaxX - currentNode._aabbMinX) * (ey + ez) + ey * ez) * 2) * 2;
let descendingCost1;
combinedMinX = c11._aabbMinX < leaf._aabbMinX ? c11._aabbMinX : leaf._aabbMinX;
combinedMinY = c11._aabbMinY < leaf._aabbMinY ? c11._aabbMinY : leaf._aabbMinY;
combinedMinZ = c11._aabbMinZ < leaf._aabbMinZ ? c11._aabbMinZ : leaf._aabbMinZ;
combinedMaxX = c11._aabbMaxX > leaf._aabbMaxX ? c11._aabbMaxX : leaf._aabbMaxX;
combinedMaxY = c11._aabbMaxY > leaf._aabbMaxY ? c11._aabbMaxY : leaf._aabbMaxY;
combinedMaxZ = c11._aabbMaxZ > leaf._aabbMaxZ ? c11._aabbMaxZ : leaf._aabbMaxZ;
if(c11._height == 0) {
let ey = combinedMaxY - combinedMinY;
let ez = combinedMaxZ - combinedMinZ;
descendingCost1 = incrementalCost + ((combinedMaxX - combinedMinX) * (ey + ez) + ey * ez) * 2;
} else {
let ey = combinedMaxY - combinedMinY;
let ez = combinedMaxZ - combinedMinZ;
let ey1 = c11._aabbMaxY - c11._aabbMinY;
let ez1 = c11._aabbMaxZ - c11._aabbMinZ;
descendingCost1 = incrementalCost + (((combinedMaxX - combinedMinX) * (ey + ez) + ey * ez) * 2 - ((c11._aabbMaxX - c11._aabbMinX) * (ey1 + ez1) + ey1 * ez1) * 2);
}
let descendingCost2;
combinedMinX = c21._aabbMinX < leaf._aabbMinX ? c21._aabbMinX : leaf._aabbMinX;
combinedMinY = c21._aabbMinY < leaf._aabbMinY ? c21._aabbMinY : leaf._aabbMinY;
combinedMinZ = c21._aabbMinZ < leaf._aabbMinZ ? c21._aabbMinZ : leaf._aabbMinZ;
combinedMaxX = c21._aabbMaxX > leaf._aabbMaxX ? c21._aabbMaxX : leaf._aabbMaxX;
combinedMaxY = c21._aabbMaxY > leaf._aabbMaxY ? c21._aabbMaxY : leaf._aabbMaxY;
combinedMaxZ = c21._aabbMaxZ > leaf._aabbMaxZ ? c21._aabbMaxZ : leaf._aabbMaxZ;
if(c21._height == 0) {
let ey = combinedMaxY - combinedMinY;
let ez = combinedMaxZ - combinedMinZ;
descendingCost2 = incrementalCost + ((combinedMaxX - combinedMinX) * (ey + ez) + ey * ez) * 2;
} else {
let ey = combinedMaxY - combinedMinY;
let ez = combinedMaxZ - combinedMinZ;
let ey1 = c21._aabbMaxY - c21._aabbMinY;
let ez1 = c21._aabbMaxZ - c21._aabbMinZ;
descendingCost2 = incrementalCost + (((combinedMaxX - combinedMinX) * (ey + ez) + ey * ez) * 2 - ((c21._aabbMaxX - c21._aabbMinX) * (ey1 + ez1) + ey1 * ez1) * 2);
}
if(creatingCost < descendingCost1) {
if(creatingCost < descendingCost2) {
return -1;
} else {
return 1;
}
} else if(descendingCost1 < descendingCost2) {
return 0;
} else {
return 1;
}
break;
default:
console.log("src/oimo/collision/broadphase/bvh/BvhStrategy.hx:37:","invalid BVH insertion strategy: " + this._insertionStrategy);
return -1;
}
}
_splitLeaves(leaves,from,until) {
let invN = 1.0 / (until - from);
let centerMeanX;
let centerMeanY;
let centerMeanZ;
centerMeanX = 0;
centerMeanY = 0;
centerMeanZ = 0;
let _g = from;
while(_g < until) {
let leaf = leaves[_g++];
leaf._tmpX = leaf._aabbMaxX + leaf._aabbMinX;
leaf._tmpY = leaf._aabbMaxY + leaf._aabbMinY;
leaf._tmpZ = leaf._aabbMaxZ + leaf._aabbMinZ;
centerMeanX += leaf._tmpX;
centerMeanY += leaf._tmpY;
centerMeanZ += leaf._tmpZ;
}
centerMeanX *= invN;
centerMeanY *= invN;
centerMeanZ *= invN;
let varianceX;
let varianceY;
let varianceZ;
varianceX = 0;
varianceY = 0;
varianceZ = 0;
let _g1 = from;
while(_g1 < until) {
let leaf = leaves[_g1++];
let diffX;
let diffY;
let diffZ;
diffX = leaf._tmpX - centerMeanX;
diffY = leaf._tmpY - centerMeanY;
diffZ = leaf._tmpZ - centerMeanZ;
diffX *= diffX;
diffY *= diffY;
diffZ *= diffZ;
varianceX += diffX;
varianceY += diffY;
varianceZ += diffZ;
}
let varX = varianceX;
let varY = varianceY;
let varZ = varianceZ;
let l = from;
let r = until - 1;
if(varX > varY) {
if(varX > varZ) {
let mean = centerMeanX;
while(true) {
while(!(leaves[l]._tmpX <= mean)) ++l;
while(!(leaves[r]._tmpX >= mean)) --r;
if(l >= r) {
break;
}
let tmp = leaves[l];
leaves[l] = leaves[r];
leaves[r] = tmp;
++l;
--r;
}
} else {
let mean = centerMeanZ;
while(true) {
while(!(leaves[l]._tmpZ <= mean)) ++l;
while(!(leaves[r]._tmpZ >= mean)) --r;
if(l >= r) {
break;
}
let tmp = leaves[l];
leaves[l] = leaves[r];
leaves[r] = tmp;
++l;
--r;
}
}
} else if(varY > varZ) {
let mean = centerMeanY;
while(true) {
while(!(leaves[l]._tmpY <= mean)) ++l;
while(!(leaves[r]._tmpY >= mean)) --r;
if(l >= r) {
break;
}
let tmp = leaves[l];
leaves[l] = leaves[r];
leaves[r] = tmp;
++l;
--r;
}
} else {
let mean = centerMeanZ;
while(true) {
while(!(leaves[l]._tmpZ <= mean)) ++l;
while(!(leaves[r]._tmpZ >= mean)) --r;
if(l >= r) {
break;
}
let tmp = leaves[l];
leaves[l] = leaves[r];
leaves[r] = tmp;
++l;
--r;
}
}
return l;
}
}
oimo.collision.broadphase.bvh.BvhTree = class oimo_collision_broadphase_bvh_BvhTree {
constructor() {
this._root = null;
this._numLeaves = 0;
this._strategy = new oimo.collision.broadphase.bvh.BvhStrategy();
this._nodePool = null;
this.leafList = null;
this.leafListLast = null;
this.tmp = new Array(1024);
}
_print(root,indent) {
if(indent == null) {
indent = "";
}
if(root == null) {
return;
}
if(root._height == 0) {
console.log("src/oimo/collision/broadphase/bvh/BvhTree.hx:39:",indent + root._proxy._id);
} else {
this._print(root._children[0],indent + " ");
let tmp;
let sizeX;
let sizeY;
let sizeZ;
sizeX = root._aabbMaxX - root._aabbMinX;
sizeY = root._aabbMaxY - root._aabbMinY;
sizeZ = root._aabbMaxZ - root._aabbMinZ;
let y = sizeY;
let z = sizeZ;
if(sizeX * (y + z) + y * z > 0) {
let sizeX;
let sizeY;
let sizeZ;
sizeX = root._aabbMaxX - root._aabbMinX;
sizeY = root._aabbMaxY - root._aabbMinY;
sizeZ = root._aabbMaxZ - root._aabbMinZ;
let y = sizeY;
let z = sizeZ;
tmp = ((sizeX * (y + z) + y * z) * 1000 + 0.5 | 0) / 1000;
} else {
let sizeX;
let sizeY;
let sizeZ;
sizeX = root._aabbMaxX - root._aabbMinX;
sizeY = root._aabbMaxY - root._aabbMinY;
sizeZ = root._aabbMaxZ - root._aabbMinZ;
let y = sizeY;
let z = sizeZ;
tmp = ((sizeX * (y + z) + y * z) * 1000 - 0.5 | 0) / 1000;
}
console.log("src/oimo/collision/broadphase/bvh/BvhTree.hx:42:",indent + "#" + root._height + ", " + tmp);
this._print(root._children[1],indent + " ");
}
}
_getBalance() {
return this.getBalanceRecursive(this._root);
}
deleteRecursive(root) {
if(root._height == 0) {
let prev = root._prevLeaf;
let next = root._nextLeaf;
if(prev != null) {
prev._nextLeaf = next;
}
if(next != null) {
next._prevLeaf = prev;
}
if(root == this.leafList) {
this.leafList = this.leafList._nextLeaf;
}
if(root == this.leafListLast) {
this.leafListLast = this.leafListLast._prevLeaf;
}
root._nextLeaf = null;
root._prevLeaf = null;
root._proxy._leaf = null;
root._next = null;
root._childIndex = 0;
root._children[0] = null;
root._children[1] = null;
root._childIndex = 0;
root._parent = null;
root._height = 0;
root._proxy = null;
root._next = this._nodePool;
this._nodePool = root;
return;
}
this.deleteRecursive(root._children[0]);
this.deleteRecursive(root._children[1]);
root._next = null;
root._childIndex = 0;
root._children[0] = null;
root._children[1] = null;
root._childIndex = 0;
root._parent = null;
root._height = 0;
root._proxy = null;
root._next = this._nodePool;
this._nodePool = root;
}
decomposeRecursive(root) {
if(root._height == 0) {
root._childIndex = 0;
root._parent = null;
return;
}
this.decomposeRecursive(root._children[0]);
this.decomposeRecursive(root._children[1]);
root._next = null;
root._childIndex = 0;
root._children[0] = null;
root._children[1] = null;
root._childIndex = 0;
root._parent = null;
root._height = 0;
root._proxy = null;
root._next = this._nodePool;
this._nodePool = root;
}
buildTopDownRecursive(leaves,from,until) {
if(until - from == 1) {
let leaf = leaves[from];
let proxy = leaf._proxy;
leaf._aabbMinX = proxy._aabbMinX;
leaf._aabbMinY = proxy._aabbMinY;
leaf._aabbMinZ = proxy._aabbMinZ;
leaf._aabbMaxX = proxy._aabbMaxX;
leaf._aabbMaxY = proxy._aabbMaxY;
leaf._aabbMaxZ = proxy._aabbMaxZ;
return leaf;
}
let splitAt = this._strategy._splitLeaves(leaves,from,until);
let child1 = this.buildTopDownRecursive(leaves,from,splitAt);
let child2 = this.buildTopDownRecursive(leaves,splitAt,until);
let first = this._nodePool;
if(first != null) {
this._nodePool = first._next;
first._next = null;
} else {
first = new oimo.collision.broadphase.bvh.BvhNode();
}
let parent = first;
parent._children[0] = child1;
child1._parent = parent;
child1._childIndex = 0;
parent._children[1] = child2;
child2._parent = parent;
child2._childIndex = 1;
let c1 = parent._children[0];
let c2 = parent._children[1];
parent._aabbMinX = c1._aabbMinX < c2._aabbMinX ? c1._aabbMinX : c2._aabbMinX;
parent._aabbMinY = c1._aabbMinY < c2._aabbMinY ? c1._aabbMinY : c2._aabbMinY;
parent._aabbMinZ = c1._aabbMinZ < c2._aabbMinZ ? c1._aabbMinZ : c2._aabbMinZ;
parent._aabbMaxX = c1._aabbMaxX > c2._aabbMaxX ? c1._aabbMaxX : c2._aabbMaxX;
parent._aabbMaxY = c1._aabbMaxY > c2._aabbMaxY ? c1._aabbMaxY : c2._aabbMaxY;
parent._aabbMaxZ = c1._aabbMaxZ > c2._aabbMaxZ ? c1._aabbMaxZ : c2._aabbMaxZ;
let h1 = parent._children[0]._height;
let h2 = parent._children[1]._height;
parent._height = (h1 > h2 ? h1 : h2) + 1;
return parent;
}
getBalanceRecursive(root) {
if(root == null || root._height == 0) {
return 0;
}
let balance = root._children[0]._height - root._children[1]._height;
if(balance < 0) {
balance = -balance;
}
return balance + this.getBalanceRecursive(root._children[0]) + this.getBalanceRecursive(root._children[1]);
}
}
oimo.collision.geometry.Aabb = class oimo_collision_geometry_Aabb {
constructor() {
this._minX = 0;
this._minY = 0;
this._minZ = 0;
this._maxX = 0;
this._maxY = 0;
this._maxZ = 0;
}
init(min,max) {
this._minX = min.x;
this._minY = min.y;
this._minZ = min.z;
this._maxX = max.x;
this._maxY = max.y;
this._maxZ = max.z;
return this;
}
getMin() {
let min = new oimo.common.Vec3();
min.x = this._minX;
min.y = this._minY;
min.z = this._minZ;
return min;
}
getMinTo(min) {
min.x = this._minX;
min.y = this._minY;
min.z = this._minZ;
}
setMin(min) {
this._minX = min.x;
this._minY = min.y;
this._minZ = min.z;
return this;
}
getMax() {
let max = new oimo.common.Vec3();
max.x = this._maxX;
max.y = this._maxY;
max.z = this._maxZ;
return max;
}
getMaxTo(max) {
max.x = this._maxX;
max.y = this._maxY;
max.z = this._maxZ;
}
setMax(max) {
this._maxX = max.x;
this._maxY = max.y;
this._maxZ = max.z;
return this;
}
getCenter() {
let v = new oimo.common.Vec3();
let cX;
let cY;
let cZ;
cX = this._minX + this._maxX;
cY = this._minY + this._maxY;
cZ = this._minZ + this._maxZ;
cX *= 0.5;
cY *= 0.5;
cZ *= 0.5;
v.x = cX;
v.y = cY;
v.z = cZ;
return v;
}
getCenterTo(center) {
let cX;
let cY;
let cZ;
cX = this._minX + this._maxX;
cY = this._minY + this._maxY;
cZ = this._minZ + this._maxZ;
cX *= 0.5;
cY *= 0.5;
cZ *= 0.5;
center.x = cX;
center.y = cY;
center.z = cZ;
}
getExtents() {
let v = new oimo.common.Vec3();
let cX;
let cY;
let cZ;
cX = this._maxX - this._minX;
cY = this._maxY - this._minY;
cZ = this._maxZ - this._minZ;
cX *= 0.5;
cY *= 0.5;
cZ *= 0.5;
v.x = cX;
v.y = cY;
v.z = cZ;
return v;
}
getExtentsTo(halfExtents) {
let cX;
let cY;
let cZ;
cX = this._maxX - this._minX;
cY = this._maxY - this._minY;
cZ = this._maxZ - this._minZ;
cX *= 0.5;
cY *= 0.5;
cZ *= 0.5;
halfExtents.x = cX;
halfExtents.y = cY;
halfExtents.z = cZ;
}
combine(other) {
this._minX = this._minX < other._minX ? this._minX : other._minX;
this._minY = this._minY < other._minY ? this._minY : other._minY;
this._minZ = this._minZ < other._minZ ? this._minZ : other._minZ;
this._maxX = this._maxX > other._maxX ? this._maxX : other._maxX;
this._maxY = this._maxY > other._maxY ? this._maxY : other._maxY;
this._maxZ = this._maxZ > other._maxZ ? this._maxZ : other._maxZ;
return this;
}
combined(other) {
let aabb = new oimo.collision.geometry.Aabb();
aabb._minX = this._minX < other._minX ? this._minX : other._minX;
aabb._minY = this._minY < other._minY ? this._minY : other._minY;
aabb._minZ = this._minZ < other._minZ ? this._minZ : other._minZ;
aabb._maxX = this._maxX > other._maxX ? this._maxX : other._maxX;
aabb._maxY = this._maxY > other._maxY ? this._maxY : other._maxY;
aabb._maxZ = this._maxZ > other._maxZ ? this._maxZ : other._maxZ;
return aabb;
}
overlap(other) {
if(this._minX < other._maxX && this._maxX > other._minX && this._minY < other._maxY && this._maxY > other._minY && this._minZ < other._maxZ) {
return this._maxZ > other._minZ;
} else {
return false;
}
}
getIntersection(other) {
let aabb = new oimo.collision.geometry.Aabb();
aabb._minX = this._minX > other._minX ? this._minX : other._minX;
aabb._minY = this._minY > other._minY ? this._minY : other._minY;
aabb._minZ = this._minZ > other._minZ ? this._minZ : other._minZ;
aabb._maxX = this._maxX < other._maxX ? this._maxX : other._maxX;
aabb._maxY = this._maxY < other._maxY ? this._maxY : other._maxY;
aabb._maxZ = this._maxZ < other._maxZ ? this._maxZ : other._maxZ;
return aabb;
}
getIntersectionTo(other,intersection) {
intersection._minX = this._minX > other._minX ? this._minX : other._minX;
intersection._minY = this._minY > other._minY ? this._minY : other._minY;
intersection._minZ = this._minZ > other._minZ ? this._minZ : other._minZ;
intersection._maxX = this._maxX < other._maxX ? this._maxX : other._maxX;
intersection._maxY = this._maxY < other._maxY ? this._maxY : other._maxY;
intersection._maxZ = this._maxZ < other._maxZ ? this._maxZ : other._maxZ;
}
copyFrom(aabb) {
this._minX = aabb._minX;
this._minY = aabb._minY;
this._minZ = aabb._minZ;
this._maxX = aabb._maxX;
this._maxY = aabb._maxY;
this._maxZ = aabb._maxZ;
return this;
}
clone() {
let aabb = new oimo.collision.geometry.Aabb();
aabb._minX = this._minX;
aabb._minY = this._minY;
aabb._minZ = this._minZ;
aabb._maxX = this._maxX;
aabb._maxY = this._maxY;
aabb._maxZ = this._maxZ;
return aabb;
}
}
oimo.collision.geometry.BoxGeometry = class oimo_collision_geometry_BoxGeometry extends oimo.collision.geometry.ConvexGeometry {
constructor(halfExtents) {
super(1);
this._halfExtentsX = halfExtents.x;
this._halfExtentsY = halfExtents.y;
this._halfExtentsZ = halfExtents.z;
this._halfAxisXX = halfExtents.x;
this._halfAxisXY = 0;
this._halfAxisXZ = 0;
this._halfAxisYX = 0;
this._halfAxisYY = halfExtents.y;
this._halfAxisYZ = 0;
this._halfAxisZX = 0;
this._halfAxisZY = 0;
this._halfAxisZZ = halfExtents.z;
this._updateMass();
let minHalfExtents = halfExtents.x < halfExtents.y ? halfExtents.z < halfExtents.x ? halfExtents.z : halfExtents.x : halfExtents.z < halfExtents.y ? halfExtents.z : halfExtents.y;
if(this._gjkMargin > minHalfExtents * 0.2) {
this._gjkMargin = minHalfExtents * 0.2;
}
}
getHalfExtents() {
let v = new oimo.common.Vec3();
v.x = this._halfExtentsX;
v.y = this._halfExtentsY;
v.z = this._halfExtentsZ;
return v;
}
getHalfExtentsTo(halfExtents) {
halfExtents.x = this._halfExtentsX;
halfExtents.y = this._halfExtentsY;
halfExtents.z = this._halfExtentsZ;
}
_updateMass() {
this._volume = 8 * (this._halfExtentsX * this._halfExtentsY * this._halfExtentsZ);
let sqX;
let sqY;
let sqZ;
sqX = this._halfExtentsX * this._halfExtentsX;
sqY = this._halfExtentsY * this._halfExtentsY;
sqZ = this._halfExtentsZ * this._halfExtentsZ;
this._inertiaCoeff00 = 0.33333333333333331 * (sqY + sqZ);
this._inertiaCoeff01 = 0;
this._inertiaCoeff02 = 0;
this._inertiaCoeff10 = 0;
this._inertiaCoeff11 = 0.33333333333333331 * (sqZ + sqX);
this._inertiaCoeff12 = 0;
this._inertiaCoeff20 = 0;
this._inertiaCoeff21 = 0;
this._inertiaCoeff22 = 0.33333333333333331 * (sqX + sqY);
}
_computeAabb(aabb,tf) {
let tfxX;
let tfxY;
let tfxZ;
let tfyX;
let tfyY;
let tfyZ;
let tfzX;
let tfzY;
let tfzZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf._rotation00 * this._halfAxisXX + tf._rotation01 * this._halfAxisXY + tf._rotation02 * this._halfAxisXZ;
__tmp__Y = tf._rotation10 * this._halfAxisXX + tf._rotation11 * this._halfAxisXY + tf._rotation12 * this._halfAxisXZ;
__tmp__Z = tf._rotation20 * this._halfAxisXX + tf._rotation21 * this._halfAxisXY + tf._rotation22 * this._halfAxisXZ;
tfxX = __tmp__X;
tfxY = __tmp__Y;
tfxZ = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf._rotation00 * this._halfAxisYX + tf._rotation01 * this._halfAxisYY + tf._rotation02 * this._halfAxisYZ;
__tmp__Y1 = tf._rotation10 * this._halfAxisYX + tf._rotation11 * this._halfAxisYY + tf._rotation12 * this._halfAxisYZ;
__tmp__Z1 = tf._rotation20 * this._halfAxisYX + tf._rotation21 * this._halfAxisYY + tf._rotation22 * this._halfAxisYZ;
tfyX = __tmp__X1;
tfyY = __tmp__Y1;
tfyZ = __tmp__Z1;
let __tmp__X2;
let __tmp__Y2;
let __tmp__Z2;
__tmp__X2 = tf._rotation00 * this._halfAxisZX + tf._rotation01 * this._halfAxisZY + tf._rotation02 * this._halfAxisZZ;
__tmp__Y2 = tf._rotation10 * this._halfAxisZX + tf._rotation11 * this._halfAxisZY + tf._rotation12 * this._halfAxisZZ;
__tmp__Z2 = tf._rotation20 * this._halfAxisZX + tf._rotation21 * this._halfAxisZY + tf._rotation22 * this._halfAxisZZ;
tfzX = __tmp__X2;
tfzY = __tmp__Y2;
tfzZ = __tmp__Z2;
if(tfxX < 0) {
tfxX = -tfxX;
}
if(tfxY < 0) {
tfxY = -tfxY;
}
if(tfxZ < 0) {
tfxZ = -tfxZ;
}
if(tfyX < 0) {
tfyX = -tfyX;
}
if(tfyY < 0) {
tfyY = -tfyY;
}
if(tfyZ < 0) {
tfyZ = -tfyZ;
}
if(tfzX < 0) {
tfzX = -tfzX;
}
if(tfzY < 0) {
tfzY = -tfzY;
}
if(tfzZ < 0) {
tfzZ = -tfzZ;
}
let tfsX;
let tfsY;
let tfsZ;
tfsX = tfxX + tfyX;
tfsY = tfxY + tfyY;
tfsZ = tfxZ + tfyZ;
tfsX += tfzX;
tfsY += tfzY;
tfsZ += tfzZ;
aabb._minX = tf._positionX - tfsX;
aabb._minY = tf._positionY - tfsY;
aabb._minZ = tf._positionZ - tfsZ;
aabb._maxX = tf._positionX + tfsX;
aabb._maxY = tf._positionY + tfsY;
aabb._maxZ = tf._positionZ + tfsZ;
}
computeLocalSupportingVertex(dir,out) {
let gjkMarginsX;
let gjkMarginsY;
let gjkMarginsZ;
let coreExtentsX;
let coreExtentsY;
let coreExtentsZ;
gjkMarginsX = this._gjkMargin;
gjkMarginsY = this._gjkMargin;
gjkMarginsZ = this._gjkMargin;
if(!(gjkMarginsX < this._halfExtentsX)) {
gjkMarginsX = this._halfExtentsX;
}
if(!(gjkMarginsY < this._halfExtentsY)) {
gjkMarginsY = this._halfExtentsY;
}
if(!(gjkMarginsZ < this._halfExtentsZ)) {
gjkMarginsZ = this._halfExtentsZ;
}
coreExtentsX = this._halfExtentsX - gjkMarginsX;
coreExtentsY = this._halfExtentsY - gjkMarginsY;
coreExtentsZ = this._halfExtentsZ - gjkMarginsZ;
out.x = dir.x > 0 ? coreExtentsX : -coreExtentsX;
out.y = dir.y > 0 ? coreExtentsY : -coreExtentsY;
out.z = dir.z > 0 ? coreExtentsZ : -coreExtentsZ;
}
_rayCastLocal(beginX,beginY,beginZ,endX,endY,endZ,hit) {
let halfW = this._halfExtentsX;
let halfH = this._halfExtentsY;
let halfD = this._halfExtentsZ;
let dx = endX - beginX;
let dy = endY - beginY;
let dz = endZ - beginZ;
let tminx = 0;
let tminy = 0;
let tminz = 0;
let tmaxx = 1;
let tmaxy = 1;
let tmaxz = 1;
if(dx > -1e-6 && dx < 1e-6) {
if(beginX <= -halfW || beginX >= halfW) {
return false;
}
} else {
let invDx = 1 / dx;
let t1 = (-halfW - beginX) * invDx;
let t2 = (halfW - beginX) * invDx;
if(t1 > t2) {
let tmp = t1;
t1 = t2;
t2 = tmp;
}
if(t1 > 0) {
tminx = t1;
}
if(t2 < 1) {
tmaxx = t2;
}
}
if(dy > -1e-6 && dy < 1e-6) {
if(beginY <= -halfH || beginY >= halfH) {
return false;
}
} else {
let invDy = 1 / dy;
let t1 = (-halfH - beginY) * invDy;
let t2 = (halfH - beginY) * invDy;
if(t1 > t2) {
let tmp = t1;
t1 = t2;
t2 = tmp;
}
if(t1 > 0) {
tminy = t1;
}
if(t2 < 1) {
tmaxy = t2;
}
}
if(dz > -1e-6 && dz < 1e-6) {
if(beginZ <= -halfD || beginZ >= halfD) {
return false;
}
} else {
let invDz = 1 / dz;
let t1 = (-halfD - beginZ) * invDz;
let t2 = (halfD - beginZ) * invDz;
if(t1 > t2) {
let tmp = t1;
t1 = t2;
t2 = tmp;
}
if(t1 > 0) {
tminz = t1;
}
if(t2 < 1) {
tmaxz = t2;
}
}
if(tminx >= 1 || tminy >= 1 || tminz >= 1 || tmaxx <= 0 || tmaxy <= 0 || tmaxz <= 0) {
return false;
}
let min = tminx;
let max = tmaxx;
let hitDirection = 0;
if(tminy > min) {
min = tminy;
hitDirection = 1;
}
if(tminz > min) {
min = tminz;
hitDirection = 2;
}
if(tmaxy < max) {
max = tmaxy;
}
if(tmaxz < max) {
max = tmaxz;
}
if(min > max) {
return false;
}
if(min == 0) {
return false;
}
switch(hitDirection) {
case 0:
hit.normal.init(dx > 0 ? -1 : 1,0,0);
break;
case 1:
hit.normal.init(0,dy > 0 ? -1 : 1,0);
break;
case 2:
hit.normal.init(0,0,dz > 0 ? -1 : 1);
break;
}
hit.position.init(beginX + min * dx,beginY + min * dy,beginZ + min * dz);
hit.fraction = min;
return true;
}
}
oimo.collision.geometry.CapsuleGeometry = class oimo_collision_geometry_CapsuleGeometry extends oimo.collision.geometry.ConvexGeometry {
constructor(radius,halfHeight) {
super(4);
this._radius = radius;
this._halfHeight = halfHeight;
this._gjkMargin = this._radius;
this._updateMass();
}
getRadius() {
return this._radius;
}
getHalfHeight() {
return this._halfHeight;
}
_updateMass() {
let r2 = this._radius * this._radius;
let hh2 = this._halfHeight * this._halfHeight;
let cylinderVolume = 6.28318530717958 * r2 * this._halfHeight;
let sphereVolume = 3.14159265358979 * r2 * this._radius * 4 / 3;
this._volume = cylinderVolume + sphereVolume;
let invVolume = this._volume == 0 ? 0 : 1 / this._volume;
let inertiaXZ = invVolume * (cylinderVolume * (r2 * 0.25 + hh2 / 3) + sphereVolume * (r2 * 0.4 + this._halfHeight * this._radius * 0.75 + hh2));
this._inertiaCoeff00 = inertiaXZ;
this._inertiaCoeff01 = 0;
this._inertiaCoeff02 = 0;
this._inertiaCoeff10 = 0;
this._inertiaCoeff11 = invVolume * (cylinderVolume * r2 * 0.5 + sphereVolume * r2 * 0.4);
this._inertiaCoeff12 = 0;
this._inertiaCoeff20 = 0;
this._inertiaCoeff21 = 0;
this._inertiaCoeff22 = inertiaXZ;
}
_computeAabb(aabb,tf) {
let radVecX;
let radVecY;
let radVecZ;
radVecX = this._radius;
radVecY = this._radius;
radVecZ = this._radius;
let axisX;
let axisY;
let axisZ;
axisX = tf._rotation01;
axisY = tf._rotation11;
axisZ = tf._rotation21;
if(axisX < 0) {
axisX = -axisX;
}
if(axisY < 0) {
axisY = -axisY;
}
if(axisZ < 0) {
axisZ = -axisZ;
}
axisX *= this._halfHeight;
axisY *= this._halfHeight;
axisZ *= this._halfHeight;
radVecX += axisX;
radVecY += axisY;
radVecZ += axisZ;
aabb._minX = tf._positionX - radVecX;
aabb._minY = tf._positionY - radVecY;
aabb._minZ = tf._positionZ - radVecZ;
aabb._maxX = tf._positionX + radVecX;
aabb._maxY = tf._positionY + radVecY;
aabb._maxZ = tf._positionZ + radVecZ;
}
computeLocalSupportingVertex(dir,out) {
if(dir.y > 0) {
out.init(0,this._halfHeight,0);
} else {
out.init(0,-this._halfHeight,0);
}
}
_rayCastLocal(beginX,beginY,beginZ,endX,endY,endZ,hit) {
let halfH = this._halfHeight;
let dx = endX - beginX;
let dz = endZ - beginZ;
let tminxz = 0;
let tmaxxz;
let a = dx * dx + dz * dz;
let b = beginX * dx + beginZ * dz;
let c = beginX * beginX + beginZ * beginZ - this._radius * this._radius;
let D = b * b - a * c;
if(D < 0) {
return false;
}
if(a > 0) {
let sqrtD = Math.sqrt(D);
tminxz = (-b - sqrtD) / a;
tmaxxz = (-b + sqrtD) / a;
if(tminxz >= 1 || tmaxxz <= 0) {
return false;
}
} else {
if(c >= 0) {
return false;
}
tminxz = 0;
}
let crossY = beginY + (endY - beginY) * tminxz;
let min;
if(crossY > -halfH && crossY < halfH) {
if(tminxz > 0) {
min = tminxz;
let _this = hit.normal.init(beginX + dx * min,0,beginZ + dz * min);
let invLen = Math.sqrt(_this.x * _this.x + _this.y * _this.y + _this.z * _this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this.x *= invLen;
_this.y *= invLen;
_this.z *= invLen;
hit.position.init(beginX + min * dx,crossY,beginZ + min * dz);
hit.fraction = min;
return true;
}
return false;
}
let spherePosX;
let spherePosY;
let spherePosZ;
let sphereToBeginX;
let sphereToBeginY;
let sphereToBeginZ;
spherePosX = 0;
spherePosY = crossY < 0 ? -halfH : halfH;
spherePosZ = 0;
sphereToBeginX = beginX - spherePosX;
sphereToBeginY = beginY - spherePosY;
sphereToBeginZ = beginZ - spherePosZ;
let dX;
let dY;
let dZ;
dX = endX - beginX;
dY = endY - beginY;
dZ = endZ - beginZ;
a = dX * dX + dY * dY + dZ * dZ;
b = sphereToBeginX * dX + sphereToBeginY * dY + sphereToBeginZ * dZ;
c = sphereToBeginX * sphereToBeginX + sphereToBeginY * sphereToBeginY + sphereToBeginZ * sphereToBeginZ - this._radius * this._radius;
D = b * b - a * c;
if(D < 0) {
return false;
}
let t = (-b - Math.sqrt(D)) / a;
if(t < 0 || t > 1) {
return false;
}
let hitPosX;
let hitPosY;
let hitPosZ;
let hitNormalX;
let hitNormalY;
let hitNormalZ;
hitPosX = sphereToBeginX + dX * t;
hitPosY = sphereToBeginY + dY * t;
hitPosZ = sphereToBeginZ + dZ * t;
let l = hitPosX * hitPosX + hitPosY * hitPosY + hitPosZ * hitPosZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
hitNormalX = hitPosX * l;
hitNormalY = hitPosY * l;
hitNormalZ = hitPosZ * l;
hitPosX += spherePosX;
hitPosY += spherePosY;
hitPosZ += spherePosZ;
let v = hit.position;
v.x = hitPosX;
v.y = hitPosY;
v.z = hitPosZ;
let v1 = hit.normal;
v1.x = hitNormalX;
v1.y = hitNormalY;
v1.z = hitNormalZ;
hit.fraction = t;
return true;
}
}
oimo.collision.geometry.ConeGeometry = class oimo_collision_geometry_ConeGeometry extends oimo.collision.geometry.ConvexGeometry {
constructor(radius,halfHeight) {
super(3);
this._radius = radius;
this._halfHeight = halfHeight;
this.sinTheta = radius / Math.sqrt(radius * radius + 4 * halfHeight * halfHeight);
this.cosTheta = 2 * halfHeight / Math.sqrt(radius * radius + 4 * halfHeight * halfHeight);
this._updateMass();
}
getRadius() {
return this._radius;
}
getHalfHeight() {
return this._halfHeight;
}
_updateMass() {
let r2 = this._radius * this._radius;
let h2 = this._halfHeight * this._halfHeight * 4;
this._volume = 3.14159265358979 * r2 * this._halfHeight * 2 / 3;
this._inertiaCoeff00 = 0.05 * (3 * r2 + 2 * h2);
this._inertiaCoeff01 = 0;
this._inertiaCoeff02 = 0;
this._inertiaCoeff10 = 0;
this._inertiaCoeff11 = 0.3 * r2;
this._inertiaCoeff12 = 0;
this._inertiaCoeff20 = 0;
this._inertiaCoeff21 = 0;
this._inertiaCoeff22 = 0.05 * (3 * r2 + 2 * h2);
}
_computeAabb(aabb,tf) {
let axisX;
let axisY;
let axisZ;
let axis2X;
let axis2Y;
let axis2Z;
let ehX;
let ehY;
let ehZ;
let erX;
let erY;
let erZ;
axisX = tf._rotation01;
axisY = tf._rotation11;
axisZ = tf._rotation21;
axis2X = axisX * axisX;
axis2Y = axisY * axisY;
axis2Z = axisZ * axisZ;
erX = Math.sqrt(1 - axis2X);
erY = Math.sqrt(1 - axis2Y);
erZ = Math.sqrt(1 - axis2Z);
erX *= this._radius;
erY *= this._radius;
erZ *= this._radius;
ehX = axisX * this._halfHeight;
ehY = axisY * this._halfHeight;
ehZ = axisZ * this._halfHeight;
let rminX;
let rminY;
let rminZ;
let rmaxX;
let rmaxY;
let rmaxZ;
rminX = -ehX;
rminY = -ehY;
rminZ = -ehZ;
rminX -= erX;
rminY -= erY;
rminZ -= erZ;
rmaxX = -ehX;
rmaxY = -ehY;
rmaxZ = -ehZ;
rmaxX += erX;
rmaxY += erY;
rmaxZ += erZ;
let maxX;
let maxY;
let maxZ;
let minX;
let minY;
let minZ;
maxX = rminX > rmaxX ? rminX : rmaxX;
maxY = rminY > rmaxY ? rminY : rmaxY;
maxZ = rminZ > rmaxZ ? rminZ : rmaxZ;
if(!(maxX > ehX)) {
maxX = ehX;
}
if(!(maxY > ehY)) {
maxY = ehY;
}
if(!(maxZ > ehZ)) {
maxZ = ehZ;
}
minX = rminX < rmaxX ? rminX : rmaxX;
minY = rminY < rmaxY ? rminY : rmaxY;
minZ = rminZ < rmaxZ ? rminZ : rmaxZ;
if(!(minX < ehX)) {
minX = ehX;
}
if(!(minY < ehY)) {
minY = ehY;
}
if(!(minZ < ehZ)) {
minZ = ehZ;
}
aabb._minX = tf._positionX + minX;
aabb._minY = tf._positionY + minY;
aabb._minZ = tf._positionZ + minZ;
aabb._maxX = tf._positionX + maxX;
aabb._maxY = tf._positionY + maxY;
aabb._maxZ = tf._positionZ + maxZ;
}
computeLocalSupportingVertex(dir,out) {
let dx = dir.x;
let dy = dir.y;
let dz = dir.z;
if(dy > 0 && dy * dy > this.sinTheta * this.sinTheta * (dx * dx + dy * dy + dz * dz)) {
out.init(0,this._halfHeight - this._gjkMargin / this.sinTheta,0);
if(out.y < 0) {
out.y = 0;
}
return;
}
let rx = dir.x;
let rz = dir.z;
let len = rx * rx + rz * rz;
let height = 2 * this._halfHeight;
let coreRadius = (height - this._gjkMargin) / height * this._radius - this._gjkMargin / this.cosTheta;
if(coreRadius < 0) {
coreRadius = 0;
}
let invLen = len > 0 ? coreRadius / Math.sqrt(len) : 0;
let coreHalfHeight = this._halfHeight - this._gjkMargin;
if(coreHalfHeight < 0) {
coreHalfHeight = 0;
}
out.x = rx * invLen;
out.y = -coreHalfHeight;
out.z = rz * invLen;
}
_rayCastLocal(beginX,beginY,beginZ,endX,endY,endZ,hit) {
let p1y;
let halfH = this._halfHeight;
let dx = endX - beginX;
let dy = endY - beginY;
let dz = endZ - beginZ;
let tminy = 0;
let tmaxy = 1;
if(dy > -1e-6 && dy < 1e-6) {
if(beginY <= -halfH || beginY >= halfH) {
return false;
}
} else {
let invDy = 1 / dy;
let t1 = (-halfH - beginY) * invDy;
let t2 = (halfH - beginY) * invDy;
if(t1 > t2) {
let tmp = t1;
t1 = t2;
t2 = tmp;
}
if(t1 > 0) {
tminy = t1;
}
if(t2 < 1) {
tmaxy = t2;
}
}
if(tminy >= 1 || tmaxy <= 0) {
return false;
}
let tminxz = 0;
let tmaxxz = 0;
p1y = beginY - halfH;
let cos2 = this.cosTheta * this.cosTheta;
let a = cos2 * (dx * dx + dy * dy + dz * dz) - dy * dy;
let b = cos2 * (beginX * dx + p1y * dy + beginZ * dz) - p1y * dy;
let c = cos2 * (beginX * beginX + p1y * p1y + beginZ * beginZ) - p1y * p1y;
let D = b * b - a * c;
if(a != 0) {
if(D < 0) {
return false;
}
let sqrtD = Math.sqrt(D);
if(a < 0) {
if(dy > 0) {
tminxz = 0;
tmaxxz = (-b + sqrtD) / a;
if(tmaxxz <= 0) {
return false;
}
} else {
tminxz = (-b - sqrtD) / a;
tmaxxz = 1;
if(tminxz >= 1) {
return false;
}
}
} else {
tminxz = (-b - sqrtD) / a;
tmaxxz = (-b + sqrtD) / a;
if(tminxz >= 1 || tmaxxz <= 0) {
return false;
}
}
} else {
let t = -c / (2 * b);
if(b > 0) {
tminxz = 0;
tmaxxz = t;
if(t <= 0) {
return false;
}
} else {
tminxz = t;
tmaxxz = 1;
if(t >= 1) {
return false;
}
}
}
p1y += halfH;
let min;
if(tmaxxz <= tminy || tmaxy <= tminxz) {
return false;
}
if(tminxz < tminy) {
min = tminy;
if(min == 0) {
return false;
}
hit.normal.init(0,dy > 0 ? -1 : 1,0);
} else {
min = tminxz;
if(min == 0) {
return false;
}
let _this = hit.normal.init(beginX + dx * min,0,beginZ + dz * min);
let invLen = Math.sqrt(_this.x * _this.x + _this.y * _this.y + _this.z * _this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this.x *= invLen;
_this.y *= invLen;
_this.z *= invLen;
let s = this.cosTheta;
_this.x *= s;
_this.y *= s;
_this.z *= s;
hit.normal.y += this.sinTheta;
}
hit.position.init(beginX + min * dx,p1y + min * dy,beginZ + min * dz);
hit.fraction = min;
return true;
}
}
oimo.collision.geometry.ConvexHullGeometry = class oimo_collision_geometry_ConvexHullGeometry extends oimo.collision.geometry.ConvexGeometry {
constructor(vertices) {
super(5);
this._numVertices = vertices.length;
this._vertices = new Array(this._numVertices);
this._tmpVertices = new Array(this._numVertices);
let _g = 0;
let _g1 = this._numVertices;
while(_g < _g1) {
let i = _g++;
this._vertices[i] = vertices[i];
this._tmpVertices[i] = new oimo.common.Vec3();
}
this._useGjkRayCast = true;
this._updateMass();
}
getVertices() {
return this._vertices;
}
_updateMass() {
this._volume = 1;
this._inertiaCoeff00 = 1;
this._inertiaCoeff01 = 0;
this._inertiaCoeff02 = 0;
this._inertiaCoeff10 = 0;
this._inertiaCoeff11 = 1;
this._inertiaCoeff12 = 0;
this._inertiaCoeff20 = 0;
this._inertiaCoeff21 = 0;
this._inertiaCoeff22 = 1;
let minx = this._vertices[0].x;
let miny = this._vertices[0].y;
let minz = this._vertices[0].z;
let maxx = this._vertices[0].x;
let maxy = this._vertices[0].y;
let maxz = this._vertices[0].z;
let _g = 1;
let _g1 = this._numVertices;
while(_g < _g1) {
let i = _g++;
let vx = this._vertices[i].x;
let vy = this._vertices[i].y;
let vz = this._vertices[i].z;
if(vx < minx) {
minx = vx;
} else if(vx > maxx) {
maxx = vx;
}
if(vy < miny) {
miny = vy;
} else if(vy > maxy) {
maxy = vy;
}
if(vz < minz) {
minz = vz;
} else if(vz > maxz) {
maxz = vz;
}
}
let sizex = maxx - minx;
let sizey = maxy - miny;
let sizez = maxz - minz;
this._volume = sizex * sizey * sizez;
let diffCog = ((minx + maxx) * (minx + maxx) + (miny + maxy) * (miny + maxy) + (minz + maxz) * (minz + maxz)) * 0.25;
sizex = sizex * sizex * 0.25;
sizey = sizey * sizey * 0.25;
sizez = sizez * sizez * 0.25;
this._inertiaCoeff00 = 0.33333333333333331 * (sizey + sizez) + diffCog;
this._inertiaCoeff01 = 0;
this._inertiaCoeff02 = 0;
this._inertiaCoeff10 = 0;
this._inertiaCoeff11 = 0.33333333333333331 * (sizez + sizex) + diffCog;
this._inertiaCoeff12 = 0;
this._inertiaCoeff20 = 0;
this._inertiaCoeff21 = 0;
this._inertiaCoeff22 = 0.33333333333333331 * (sizex + sizey) + diffCog;
}
_computeAabb(aabb,tf) {
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
let marginX;
let marginY;
let marginZ;
marginX = this._gjkMargin;
marginY = this._gjkMargin;
marginZ = this._gjkMargin;
let localVX;
let localVY;
let localVZ;
let v = this._vertices[0];
localVX = v.x;
localVY = v.y;
localVZ = v.z;
let worldVX;
let worldVY;
let worldVZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf._rotation00 * localVX + tf._rotation01 * localVY + tf._rotation02 * localVZ;
__tmp__Y = tf._rotation10 * localVX + tf._rotation11 * localVY + tf._rotation12 * localVZ;
__tmp__Z = tf._rotation20 * localVX + tf._rotation21 * localVY + tf._rotation22 * localVZ;
worldVX = __tmp__X;
worldVY = __tmp__Y;
worldVZ = __tmp__Z;
worldVX += tf._positionX;
worldVY += tf._positionY;
worldVZ += tf._positionZ;
minX = worldVX;
minY = worldVY;
minZ = worldVZ;
maxX = worldVX;
maxY = worldVY;
maxZ = worldVZ;
let _g = 1;
let _g1 = this._numVertices;
while(_g < _g1) {
let v = this._vertices[_g++];
localVX = v.x;
localVY = v.y;
localVZ = v.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf._rotation00 * localVX + tf._rotation01 * localVY + tf._rotation02 * localVZ;
__tmp__Y = tf._rotation10 * localVX + tf._rotation11 * localVY + tf._rotation12 * localVZ;
__tmp__Z = tf._rotation20 * localVX + tf._rotation21 * localVY + tf._rotation22 * localVZ;
worldVX = __tmp__X;
worldVY = __tmp__Y;
worldVZ = __tmp__Z;
worldVX += tf._positionX;
worldVY += tf._positionY;
worldVZ += tf._positionZ;
if(!(minX < worldVX)) {
minX = worldVX;
}
if(!(minY < worldVY)) {
minY = worldVY;
}
if(!(minZ < worldVZ)) {
minZ = worldVZ;
}
if(!(maxX > worldVX)) {
maxX = worldVX;
}
if(!(maxY > worldVY)) {
maxY = worldVY;
}
if(!(maxZ > worldVZ)) {
maxZ = worldVZ;
}
}
aabb._minX = minX - marginX;
aabb._minY = minY - marginY;
aabb._minZ = minZ - marginZ;
aabb._maxX = maxX + marginX;
aabb._maxY = maxY + marginY;
aabb._maxZ = maxZ + marginZ;
}
computeLocalSupportingVertex(dir,out) {
let _this = this._vertices[0];
let maxDot = _this.x * dir.x + _this.y * dir.y + _this.z * dir.z;
let maxIndex = 0;
let _g = 1;
let _g1 = this._numVertices;
while(_g < _g1) {
let i = _g++;
let _this = this._vertices[i];
let dot = _this.x * dir.x + _this.y * dir.y + _this.z * dir.z;
if(dot > maxDot) {
maxDot = dot;
maxIndex = i;
}
}
let v = this._vertices[maxIndex];
out.x = v.x;
out.y = v.y;
out.z = v.z;
}
}
oimo.collision.geometry.CylinderGeometry = class oimo_collision_geometry_CylinderGeometry extends oimo.collision.geometry.ConvexGeometry {
constructor(radius,halfHeight) {
super(2);
this._radius = radius;
this._halfHeight = halfHeight;
this._updateMass();
}
getRadius() {
return this._radius;
}
getHalfHeight() {
return this._halfHeight;
}
_updateMass() {
let r2 = this._radius * this._radius;
let h2 = this._halfHeight * this._halfHeight * 4;
this._volume = 3.14159265358979 * r2 * this._halfHeight * 2;
this._inertiaCoeff00 = 0.083333333333333329 * (3 * r2 + h2);
this._inertiaCoeff01 = 0;
this._inertiaCoeff02 = 0;
this._inertiaCoeff10 = 0;
this._inertiaCoeff11 = 0.5 * r2;
this._inertiaCoeff12 = 0;
this._inertiaCoeff20 = 0;
this._inertiaCoeff21 = 0;
this._inertiaCoeff22 = 0.083333333333333329 * (3 * r2 + h2);
}
_computeAabb(aabb,tf) {
let axisX;
let axisY;
let axisZ;
let axis2X;
let axis2Y;
let axis2Z;
let ehX;
let ehY;
let ehZ;
let erX;
let erY;
let erZ;
axisX = tf._rotation01;
axisY = tf._rotation11;
axisZ = tf._rotation21;
if(axisX < 0) {
axisX = -axisX;
}
if(axisY < 0) {
axisY = -axisY;
}
if(axisZ < 0) {
axisZ = -axisZ;
}
axis2X = axisX * axisX;
axis2Y = axisY * axisY;
axis2Z = axisZ * axisZ;
erX = Math.sqrt(1 - axis2X);
erY = Math.sqrt(1 - axis2Y);
erZ = Math.sqrt(1 - axis2Z);
erX *= this._radius;
erY *= this._radius;
erZ *= this._radius;
ehX = axisX * this._halfHeight;
ehY = axisY * this._halfHeight;
ehZ = axisZ * this._halfHeight;
let maxX;
let maxY;
let maxZ;
maxX = erX + ehX;
maxY = erY + ehY;
maxZ = erZ + ehZ;
aabb._minX = tf._positionX - maxX;
aabb._minY = tf._positionY - maxY;
aabb._minZ = tf._positionZ - maxZ;
aabb._maxX = tf._positionX + maxX;
aabb._maxY = tf._positionY + maxY;
aabb._maxZ = tf._positionZ + maxZ;
}
computeLocalSupportingVertex(dir,out) {
let rx = dir.x;
let rz = dir.z;
let len = rx * rx + rz * rz;
let coreRadius = this._radius - this._gjkMargin;
if(coreRadius < 0) {
coreRadius = 0;
}
let invLen = len > 0 ? coreRadius / Math.sqrt(len) : 0;
let coreHeight = this._halfHeight - this._gjkMargin;
if(coreHeight < 0) {
coreHeight = 0;
}
out.x = rx * invLen;
out.y = dir.y > 0 ? coreHeight : -coreHeight;
out.z = rz * invLen;
}
_rayCastLocal(beginX,beginY,beginZ,endX,endY,endZ,hit) {
let halfH = this._halfHeight;
let dx = endX - beginX;
let dy = endY - beginY;
let dz = endZ - beginZ;
let tminy = 0;
let tmaxy = 1;
if(dy > -1e-6 && dy < 1e-6) {
if(beginY <= -halfH || beginY >= halfH) {
return false;
}
} else {
let invDy = 1 / dy;
let t1 = (-halfH - beginY) * invDy;
let t2 = (halfH - beginY) * invDy;
if(t1 > t2) {
let tmp = t1;
t1 = t2;
t2 = tmp;
}
if(t1 > 0) {
tminy = t1;
}
if(t2 < 1) {
tmaxy = t2;
}
}
if(tminy >= 1 || tmaxy <= 0) {
return false;
}
let tminxz = 0;
let tmaxxz;
let a = dx * dx + dz * dz;
let b = beginX * dx + beginZ * dz;
let c = beginX * beginX + beginZ * beginZ - this._radius * this._radius;
let D = b * b - a * c;
if(D < 0) {
return false;
}
if(a > 0) {
let sqrtD = Math.sqrt(D);
tminxz = (-b - sqrtD) / a;
tmaxxz = (-b + sqrtD) / a;
if(tminxz >= 1 || tmaxxz <= 0) {
return false;
}
} else {
if(c >= 0) {
return false;
}
tminxz = 0;
tmaxxz = 1;
}
let min;
if(tmaxxz <= tminy || tmaxy <= tminxz) {
return false;
}
if(tminxz < tminy) {
min = tminy;
if(min == 0) {
return false;
}
hit.normal.init(0,dy > 0 ? -1 : 1,0);
} else {
min = tminxz;
if(min == 0) {
return false;
}
let _this = hit.normal.init(beginX + dx * min,0,beginZ + dz * min);
let invLen = Math.sqrt(_this.x * _this.x + _this.y * _this.y + _this.z * _this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this.x *= invLen;
_this.y *= invLen;
_this.z *= invLen;
}
hit.position.init(beginX + min * dx,beginY + min * dy,beginZ + min * dz);
hit.fraction = min;
return true;
}
}
oimo.collision.geometry.GeometryType = class oimo_collision_geometry_GeometryType {
}
oimo.collision.geometry.RayCastHit = class oimo_collision_geometry_RayCastHit {
constructor() {
this.position = new oimo.common.Vec3();
this.normal = new oimo.common.Vec3();
this.fraction = 0;
}
}
oimo.collision.geometry.SphereGeometry = class oimo_collision_geometry_SphereGeometry extends oimo.collision.geometry.ConvexGeometry {
constructor(radius) {
super(0);
this._radius = radius;
this._gjkMargin = this._radius;
this._updateMass();
}
getRadius() {
return this._radius;
}
_updateMass() {
this._volume = 4.1887902047863861 * this._radius * this._radius * this._radius;
this._inertiaCoeff00 = 0.4 * this._radius * this._radius;
this._inertiaCoeff01 = 0;
this._inertiaCoeff02 = 0;
this._inertiaCoeff10 = 0;
this._inertiaCoeff11 = 0.4 * this._radius * this._radius;
this._inertiaCoeff12 = 0;
this._inertiaCoeff20 = 0;
this._inertiaCoeff21 = 0;
this._inertiaCoeff22 = 0.4 * this._radius * this._radius;
}
_computeAabb(aabb,tf) {
let radVecX;
let radVecY;
let radVecZ;
radVecX = this._radius;
radVecY = this._radius;
radVecZ = this._radius;
aabb._minX = tf._positionX - radVecX;
aabb._minY = tf._positionY - radVecY;
aabb._minZ = tf._positionZ - radVecZ;
aabb._maxX = tf._positionX + radVecX;
aabb._maxY = tf._positionY + radVecY;
aabb._maxZ = tf._positionZ + radVecZ;
}
computeLocalSupportingVertex(dir,out) {
out.zero();
}
_rayCastLocal(beginX,beginY,beginZ,endX,endY,endZ,hit) {
let dX;
let dY;
let dZ;
dX = endX - beginX;
dY = endY - beginY;
dZ = endZ - beginZ;
let a = dX * dX + dY * dY + dZ * dZ;
let b = beginX * dX + beginY * dY + beginZ * dZ;
let D = b * b - a * (beginX * beginX + beginY * beginY + beginZ * beginZ - this._radius * this._radius);
if(D < 0) {
return false;
}
let t = (-b - Math.sqrt(D)) / a;
if(t < 0 || t > 1) {
return false;
}
let hitPosX;
let hitPosY;
let hitPosZ;
let hitNormalX;
let hitNormalY;
let hitNormalZ;
hitPosX = beginX + dX * t;
hitPosY = beginY + dY * t;
hitPosZ = beginZ + dZ * t;
let l = hitPosX * hitPosX + hitPosY * hitPosY + hitPosZ * hitPosZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
hitNormalX = hitPosX * l;
hitNormalY = hitPosY * l;
hitNormalZ = hitPosZ * l;
let v = hit.position;
v.x = hitPosX;
v.y = hitPosY;
v.z = hitPosZ;
let v1 = hit.normal;
v1.x = hitNormalX;
v1.y = hitNormalY;
v1.z = hitNormalZ;
hit.fraction = t;
return true;
}
}
if(!oimo.collision.narrowphase) oimo.collision.narrowphase = {};
oimo.collision.narrowphase.CollisionMatrix = class oimo_collision_narrowphase_CollisionMatrix {
constructor() {
this.detectors = new Array(8);
this.detectors[0] = new Array(8);
this.detectors[1] = new Array(8);
this.detectors[2] = new Array(8);
this.detectors[3] = new Array(8);
this.detectors[4] = new Array(8);
this.detectors[5] = new Array(8);
let gjkEpaDetector = new oimo.collision.narrowphase.detector.GjkEpaDetector();
this.detectors[0][0] = new oimo.collision.narrowphase.detector.SphereSphereDetector();
this.detectors[0][1] = new oimo.collision.narrowphase.detector.SphereBoxDetector(false);
this.detectors[0][2] = gjkEpaDetector;
this.detectors[0][3] = gjkEpaDetector;
this.detectors[0][4] = new oimo.collision.narrowphase.detector.SphereCapsuleDetector(false);
this.detectors[0][5] = gjkEpaDetector;
this.detectors[1][0] = new oimo.collision.narrowphase.detector.SphereBoxDetector(true);
this.detectors[1][1] = new oimo.collision.narrowphase.detector.BoxBoxDetector();
this.detectors[1][2] = gjkEpaDetector;
this.detectors[1][3] = gjkEpaDetector;
this.detectors[1][4] = gjkEpaDetector;
this.detectors[1][5] = gjkEpaDetector;
this.detectors[2][0] = gjkEpaDetector;
this.detectors[2][1] = gjkEpaDetector;
this.detectors[2][2] = gjkEpaDetector;
this.detectors[2][3] = gjkEpaDetector;
this.detectors[2][4] = gjkEpaDetector;
this.detectors[2][5] = gjkEpaDetector;
this.detectors[3][0] = gjkEpaDetector;
this.detectors[3][1] = gjkEpaDetector;
this.detectors[3][2] = gjkEpaDetector;
this.detectors[3][3] = gjkEpaDetector;
this.detectors[3][4] = gjkEpaDetector;
this.detectors[3][5] = gjkEpaDetector;
this.detectors[4][0] = new oimo.collision.narrowphase.detector.SphereCapsuleDetector(true);
this.detectors[4][1] = gjkEpaDetector;
this.detectors[4][2] = gjkEpaDetector;
this.detectors[4][3] = gjkEpaDetector;
this.detectors[4][4] = new oimo.collision.narrowphase.detector.CapsuleCapsuleDetector();
this.detectors[4][5] = gjkEpaDetector;
this.detectors[5][0] = gjkEpaDetector;
this.detectors[5][1] = gjkEpaDetector;
this.detectors[5][2] = gjkEpaDetector;
this.detectors[5][3] = gjkEpaDetector;
this.detectors[5][4] = gjkEpaDetector;
this.detectors[5][5] = gjkEpaDetector;
}
getDetector(geomType1,geomType2) {
return this.detectors[geomType1][geomType2];
}
}
oimo.collision.narrowphase.DetectorResult = class oimo_collision_narrowphase_DetectorResult {
constructor() {
this.numPoints = 0;
this.normal = new oimo.common.Vec3();
this.points = new Array(oimo.common.Setting.maxManifoldPoints);
this.incremental = false;
let _g = 0;
let _g1 = oimo.common.Setting.maxManifoldPoints;
while(_g < _g1) this.points[_g++] = new oimo.collision.narrowphase.DetectorResultPoint();
}
getMaxDepth() {
let max = 0;
let _g = 0;
let _g1 = this.numPoints;
while(_g < _g1) {
let i = _g++;
if(this.points[i].depth > max) {
max = this.points[i].depth;
}
}
return max;
}
clear() {
this.numPoints = 0;
let _g = 0;
let _g1 = this.points;
while(_g < _g1.length) {
let p = _g1[_g];
++_g;
p.position1.zero();
p.position2.zero();
p.depth = 0;
p.id = 0;
}
this.normal.zero();
}
}
oimo.collision.narrowphase.DetectorResultPoint = class oimo_collision_narrowphase_DetectorResultPoint {
constructor() {
this.position1 = new oimo.common.Vec3();
this.position2 = new oimo.common.Vec3();
this.depth = 0;
this.id = 0;
}
}
if(!oimo.collision.narrowphase.detector) oimo.collision.narrowphase.detector = {};
oimo.collision.narrowphase.detector.Detector = class oimo_collision_narrowphase_detector_Detector {
constructor(swapped) {
this.swapped = swapped;
}
setNormal(result,nX,nY,nZ) {
let v = result.normal;
v.x = nX;
v.y = nY;
v.z = nZ;
if(this.swapped) {
let _this = result.normal;
_this.x = -_this.x;
_this.y = -_this.y;
_this.z = -_this.z;
}
}
addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,depth,id) {
let p = result.points[result.numPoints++];
p.depth = depth;
p.id = id;
if(this.swapped) {
let v = p.position1;
v.x = pos2X;
v.y = pos2Y;
v.z = pos2Z;
let v1 = p.position2;
v1.x = pos1X;
v1.y = pos1Y;
v1.z = pos1Z;
} else {
let v = p.position1;
v.x = pos1X;
v.y = pos1Y;
v.z = pos1Z;
let v1 = p.position2;
v1.x = pos2X;
v1.y = pos2Y;
v1.z = pos2Z;
}
}
detectImpl(result,geom1,geom2,tf1,tf2,cachedData) {
}
detect(result,geom1,geom2,transform1,transform2,cachedData) {
result.numPoints = 0;
let _g = 0;
let _g1 = result.points;
while(_g < _g1.length) {
let p = _g1[_g];
++_g;
p.position1.zero();
p.position2.zero();
p.depth = 0;
p.id = 0;
}
result.normal.zero();
if(this.swapped) {
this.detectImpl(result,geom2,geom1,transform2,transform1,cachedData);
} else {
this.detectImpl(result,geom1,geom2,transform1,transform2,cachedData);
}
}
}
oimo.collision.narrowphase.detector.BoxBoxDetector = class oimo_collision_narrowphase_detector_BoxBoxDetector extends oimo.collision.narrowphase.detector.Detector {
constructor() {
super(false);
this.clipper = new oimo.collision.narrowphase.detector._BoxBoxDetector.FaceClipper();
}
detectImpl(result,geom1,geom2,tf1,tf2,cachedData) {
let b1 = geom1;
let b2 = geom2;
result.incremental = false;
let c1X;
let c1Y;
let c1Z;
let c2X;
let c2Y;
let c2Z;
let c12X;
let c12Y;
let c12Z;
c1X = tf1._positionX;
c1Y = tf1._positionY;
c1Z = tf1._positionZ;
c2X = tf2._positionX;
c2Y = tf2._positionY;
c2Z = tf2._positionZ;
c12X = c2X - c1X;
c12Y = c2Y - c1Y;
c12Z = c2Z - c1Z;
let x1X;
let x1Y;
let x1Z;
let y1X;
let y1Y;
let y1Z;
let z1X;
let z1Y;
let z1Z;
let x2X;
let x2Y;
let x2Z;
let y2X;
let y2Y;
let y2Z;
let z2X;
let z2Y;
let z2Z;
x1X = tf1._rotation00;
x1Y = tf1._rotation10;
x1Z = tf1._rotation20;
y1X = tf1._rotation01;
y1Y = tf1._rotation11;
y1Z = tf1._rotation21;
z1X = tf1._rotation02;
z1Y = tf1._rotation12;
z1Z = tf1._rotation22;
x2X = tf2._rotation00;
x2Y = tf2._rotation10;
x2Z = tf2._rotation20;
y2X = tf2._rotation01;
y2Y = tf2._rotation11;
y2Z = tf2._rotation21;
z2X = tf2._rotation02;
z2Y = tf2._rotation12;
z2Z = tf2._rotation22;
let w1 = b1._halfExtentsX;
let h1 = b1._halfExtentsY;
let d1 = b1._halfExtentsZ;
let w2 = b2._halfExtentsX;
let h2 = b2._halfExtentsY;
let d2 = b2._halfExtentsZ;
let sx1X;
let sx1Y;
let sx1Z;
let sy1X;
let sy1Y;
let sy1Z;
let sz1X;
let sz1Y;
let sz1Z;
let sx2X;
let sx2Y;
let sx2Z;
let sy2X;
let sy2Y;
let sy2Z;
let sz2X;
let sz2Y;
let sz2Z;
sx1X = x1X * w1;
sx1Y = x1Y * w1;
sx1Z = x1Z * w1;
sy1X = y1X * h1;
sy1Y = y1Y * h1;
sy1Z = y1Z * h1;
sz1X = z1X * d1;
sz1Y = z1Y * d1;
sz1Z = z1Z * d1;
sx2X = x2X * w2;
sx2Y = x2Y * w2;
sx2Z = x2Z * w2;
sy2X = y2X * h2;
sy2Y = y2Y * h2;
sy2Z = y2Z * h2;
sz2X = z2X * d2;
sz2Y = z2Y * d2;
sz2Z = z2Z * d2;
let mDepth = 1e65536;
let mId = -1;
let mSign = 0;
let mAxisX;
let mAxisY;
let mAxisZ;
mAxisX = 0;
mAxisY = 0;
mAxisZ = 0;
let proj1 = w1;
let dx = x1X * sx2X + x1Y * sx2Y + x1Z * sx2Z;
let dy = x1X * sy2X + x1Y * sy2Y + x1Z * sy2Z;
let dz = x1X * sz2X + x1Y * sz2Y + x1Z * sz2Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
if(dz < 0) {
dz = -dz;
}
let proj2 = dx + dy + dz;
let projC12 = x1X * c12X + x1Y * c12Y + x1Z * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < 1e65536) {
mDepth = depth;
mId = 0;
mAxisX = x1X;
mAxisY = x1Y;
mAxisZ = x1Z;
mSign = neg ? -1 : 1;
}
} else {
return;
}
proj1 = h1;
let dx1 = y1X * sx2X + y1Y * sx2Y + y1Z * sx2Z;
let dy1 = y1X * sy2X + y1Y * sy2Y + y1Z * sy2Z;
let dz1 = y1X * sz2X + y1Y * sz2Y + y1Z * sz2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
if(dz1 < 0) {
dz1 = -dz1;
}
proj2 = dx1 + dy1 + dz1;
projC12 = y1X * c12X + y1Y * c12Y + y1Z * c12Z;
let sum1 = proj1 + proj2;
let neg1 = projC12 < 0;
let abs1 = neg1 ? -projC12 : projC12;
if(abs1 < sum1) {
let depth = sum1 - abs1;
if(depth < mDepth) {
mDepth = depth;
mId = 1;
mAxisX = y1X;
mAxisY = y1Y;
mAxisZ = y1Z;
mSign = neg1 ? -1 : 1;
}
} else {
return;
}
proj1 = d1;
let dx2 = z1X * sx2X + z1Y * sx2Y + z1Z * sx2Z;
let dy2 = z1X * sy2X + z1Y * sy2Y + z1Z * sy2Z;
let dz2 = z1X * sz2X + z1Y * sz2Y + z1Z * sz2Z;
if(dx2 < 0) {
dx2 = -dx2;
}
if(dy2 < 0) {
dy2 = -dy2;
}
if(dz2 < 0) {
dz2 = -dz2;
}
proj2 = dx2 + dy2 + dz2;
projC12 = z1X * c12X + z1Y * c12Y + z1Z * c12Z;
let sum2 = proj1 + proj2;
let neg2 = projC12 < 0;
let abs2 = neg2 ? -projC12 : projC12;
if(abs2 < sum2) {
let depth = sum2 - abs2;
if(depth < mDepth) {
mDepth = depth;
mId = 2;
mAxisX = z1X;
mAxisY = z1Y;
mAxisZ = z1Z;
mSign = neg2 ? -1 : 1;
}
} else {
return;
}
if(mDepth > oimo.common.Setting.linearSlop) {
mDepth -= oimo.common.Setting.linearSlop;
} else {
mDepth = 0;
}
let dx3 = x2X * sx1X + x2Y * sx1Y + x2Z * sx1Z;
let dy3 = x2X * sy1X + x2Y * sy1Y + x2Z * sy1Z;
let dz3 = x2X * sz1X + x2Y * sz1Y + x2Z * sz1Z;
if(dx3 < 0) {
dx3 = -dx3;
}
if(dy3 < 0) {
dy3 = -dy3;
}
if(dz3 < 0) {
dz3 = -dz3;
}
proj1 = dx3 + dy3 + dz3;
proj2 = w2;
projC12 = x2X * c12X + x2Y * c12Y + x2Z * c12Z;
let sum3 = proj1 + proj2;
let neg3 = projC12 < 0;
let abs3 = neg3 ? -projC12 : projC12;
if(abs3 < sum3) {
let depth = sum3 - abs3;
if(depth < mDepth) {
mDepth = depth;
mId = 3;
mAxisX = x2X;
mAxisY = x2Y;
mAxisZ = x2Z;
mSign = neg3 ? -1 : 1;
}
} else {
return;
}
let dx4 = y2X * sx1X + y2Y * sx1Y + y2Z * sx1Z;
let dy4 = y2X * sy1X + y2Y * sy1Y + y2Z * sy1Z;
let dz4 = y2X * sz1X + y2Y * sz1Y + y2Z * sz1Z;
if(dx4 < 0) {
dx4 = -dx4;
}
if(dy4 < 0) {
dy4 = -dy4;
}
if(dz4 < 0) {
dz4 = -dz4;
}
proj1 = dx4 + dy4 + dz4;
proj2 = h2;
projC12 = y2X * c12X + y2Y * c12Y + y2Z * c12Z;
let sum4 = proj1 + proj2;
let neg4 = projC12 < 0;
let abs4 = neg4 ? -projC12 : projC12;
if(abs4 < sum4) {
let depth = sum4 - abs4;
if(depth < mDepth) {
mDepth = depth;
mId = 4;
mAxisX = y2X;
mAxisY = y2Y;
mAxisZ = y2Z;
mSign = neg4 ? -1 : 1;
}
} else {
return;
}
let dx5 = z2X * sx1X + z2Y * sx1Y + z2Z * sx1Z;
let dy5 = z2X * sy1X + z2Y * sy1Y + z2Z * sy1Z;
let dz5 = z2X * sz1X + z2Y * sz1Y + z2Z * sz1Z;
if(dx5 < 0) {
dx5 = -dx5;
}
if(dy5 < 0) {
dy5 = -dy5;
}
if(dz5 < 0) {
dz5 = -dz5;
}
proj1 = dx5 + dy5 + dz5;
proj2 = d2;
projC12 = z2X * c12X + z2Y * c12Y + z2Z * c12Z;
let sum5 = proj1 + proj2;
let neg5 = projC12 < 0;
let abs5 = neg5 ? -projC12 : projC12;
if(abs5 < sum5) {
let depth = sum5 - abs5;
if(depth < mDepth) {
mDepth = depth;
mId = 5;
mAxisX = z2X;
mAxisY = z2Y;
mAxisZ = z2Z;
mSign = neg5 ? -1 : 1;
}
} else {
return;
}
if(mDepth > oimo.common.Setting.linearSlop) {
mDepth -= oimo.common.Setting.linearSlop;
} else {
mDepth = 0;
}
let edgeAxisX;
let edgeAxisY;
let edgeAxisZ;
edgeAxisX = x1Y * x2Z - x1Z * x2Y;
edgeAxisY = x1Z * x2X - x1X * x2Z;
edgeAxisZ = x1X * x2Y - x1Y * x2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sy1X + edgeAxisY * sy1Y + edgeAxisZ * sy1Z;
let dy = edgeAxisX * sz1X + edgeAxisY * sz1Y + edgeAxisZ * sz1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sy2X + edgeAxisY * sy2Y + edgeAxisZ * sy2Z;
let dy1 = edgeAxisX * sz2X + edgeAxisY * sz2Y + edgeAxisZ * sz2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 6;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
edgeAxisX = x1Y * y2Z - x1Z * y2Y;
edgeAxisY = x1Z * y2X - x1X * y2Z;
edgeAxisZ = x1X * y2Y - x1Y * y2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sy1X + edgeAxisY * sy1Y + edgeAxisZ * sy1Z;
let dy = edgeAxisX * sz1X + edgeAxisY * sz1Y + edgeAxisZ * sz1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sx2X + edgeAxisY * sx2Y + edgeAxisZ * sx2Z;
let dy1 = edgeAxisX * sz2X + edgeAxisY * sz2Y + edgeAxisZ * sz2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 7;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
edgeAxisX = x1Y * z2Z - x1Z * z2Y;
edgeAxisY = x1Z * z2X - x1X * z2Z;
edgeAxisZ = x1X * z2Y - x1Y * z2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sy1X + edgeAxisY * sy1Y + edgeAxisZ * sy1Z;
let dy = edgeAxisX * sz1X + edgeAxisY * sz1Y + edgeAxisZ * sz1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sx2X + edgeAxisY * sx2Y + edgeAxisZ * sx2Z;
let dy1 = edgeAxisX * sy2X + edgeAxisY * sy2Y + edgeAxisZ * sy2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 8;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
edgeAxisX = y1Y * x2Z - y1Z * x2Y;
edgeAxisY = y1Z * x2X - y1X * x2Z;
edgeAxisZ = y1X * x2Y - y1Y * x2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sx1X + edgeAxisY * sx1Y + edgeAxisZ * sx1Z;
let dy = edgeAxisX * sz1X + edgeAxisY * sz1Y + edgeAxisZ * sz1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sy2X + edgeAxisY * sy2Y + edgeAxisZ * sy2Z;
let dy1 = edgeAxisX * sz2X + edgeAxisY * sz2Y + edgeAxisZ * sz2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 9;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
edgeAxisX = y1Y * y2Z - y1Z * y2Y;
edgeAxisY = y1Z * y2X - y1X * y2Z;
edgeAxisZ = y1X * y2Y - y1Y * y2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sx1X + edgeAxisY * sx1Y + edgeAxisZ * sx1Z;
let dy = edgeAxisX * sz1X + edgeAxisY * sz1Y + edgeAxisZ * sz1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sx2X + edgeAxisY * sx2Y + edgeAxisZ * sx2Z;
let dy1 = edgeAxisX * sz2X + edgeAxisY * sz2Y + edgeAxisZ * sz2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 10;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
edgeAxisX = y1Y * z2Z - y1Z * z2Y;
edgeAxisY = y1Z * z2X - y1X * z2Z;
edgeAxisZ = y1X * z2Y - y1Y * z2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sx1X + edgeAxisY * sx1Y + edgeAxisZ * sx1Z;
let dy = edgeAxisX * sz1X + edgeAxisY * sz1Y + edgeAxisZ * sz1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sx2X + edgeAxisY * sx2Y + edgeAxisZ * sx2Z;
let dy1 = edgeAxisX * sy2X + edgeAxisY * sy2Y + edgeAxisZ * sy2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 11;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
edgeAxisX = z1Y * x2Z - z1Z * x2Y;
edgeAxisY = z1Z * x2X - z1X * x2Z;
edgeAxisZ = z1X * x2Y - z1Y * x2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sx1X + edgeAxisY * sx1Y + edgeAxisZ * sx1Z;
let dy = edgeAxisX * sy1X + edgeAxisY * sy1Y + edgeAxisZ * sy1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sy2X + edgeAxisY * sy2Y + edgeAxisZ * sy2Z;
let dy1 = edgeAxisX * sz2X + edgeAxisY * sz2Y + edgeAxisZ * sz2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 12;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
edgeAxisX = z1Y * y2Z - z1Z * y2Y;
edgeAxisY = z1Z * y2X - z1X * y2Z;
edgeAxisZ = z1X * y2Y - z1Y * y2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sx1X + edgeAxisY * sx1Y + edgeAxisZ * sx1Z;
let dy = edgeAxisX * sy1X + edgeAxisY * sy1Y + edgeAxisZ * sy1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sx2X + edgeAxisY * sx2Y + edgeAxisZ * sx2Z;
let dy1 = edgeAxisX * sz2X + edgeAxisY * sz2Y + edgeAxisZ * sz2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 13;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
edgeAxisX = z1Y * z2Z - z1Z * z2Y;
edgeAxisY = z1Z * z2X - z1X * z2Z;
edgeAxisZ = z1X * z2Y - z1Y * z2X;
if(!(edgeAxisX == 0 && edgeAxisY == 0 && edgeAxisZ == 0)) {
let l = edgeAxisX * edgeAxisX + edgeAxisY * edgeAxisY + edgeAxisZ * edgeAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
edgeAxisX *= l;
edgeAxisY *= l;
edgeAxisZ *= l;
let dx = edgeAxisX * sx1X + edgeAxisY * sx1Y + edgeAxisZ * sx1Z;
let dy = edgeAxisX * sy1X + edgeAxisY * sy1Y + edgeAxisZ * sy1Z;
if(dx < 0) {
dx = -dx;
}
if(dy < 0) {
dy = -dy;
}
proj1 = dx + dy;
let dx1 = edgeAxisX * sx2X + edgeAxisY * sx2Y + edgeAxisZ * sx2Z;
let dy1 = edgeAxisX * sy2X + edgeAxisY * sy2Y + edgeAxisZ * sy2Z;
if(dx1 < 0) {
dx1 = -dx1;
}
if(dy1 < 0) {
dy1 = -dy1;
}
proj2 = dx1 + dy1;
projC12 = edgeAxisX * c12X + edgeAxisY * c12Y + edgeAxisZ * c12Z;
let sum = proj1 + proj2;
let neg = projC12 < 0;
let abs = neg ? -projC12 : projC12;
if(abs < sum) {
let depth = sum - abs;
if(depth < mDepth) {
mDepth = depth;
mId = 14;
mAxisX = edgeAxisX;
mAxisY = edgeAxisY;
mAxisZ = edgeAxisZ;
mSign = neg ? -1 : 1;
}
} else {
return;
}
}
if(mId >= 6) {
mAxisX *= mSign;
mAxisY *= mSign;
mAxisZ *= mSign;
let id1 = (mId - 6) / 3 | 0;
let id2 = mId - 6 - id1 * 3;
let p1X;
let p1Y;
let p1Z;
let p2X;
let p2Y;
let p2Z;
let d1X;
let d1Y;
let d1Z;
let d2X;
let d2Y;
let d2Z;
switch(id1) {
case 0:
d1X = x1X;
d1Y = x1Y;
d1Z = x1Z;
let signY = sz1X * mAxisX + sz1Y * mAxisY + sz1Z * mAxisZ > 0;
if(sy1X * mAxisX + sy1Y * mAxisY + sy1Z * mAxisZ > 0) {
if(signY) {
p1X = sy1X + sz1X;
p1Y = sy1Y + sz1Y;
p1Z = sy1Z + sz1Z;
} else {
p1X = sy1X - sz1X;
p1Y = sy1Y - sz1Y;
p1Z = sy1Z - sz1Z;
}
} else if(signY) {
p1X = sz1X - sy1X;
p1Y = sz1Y - sy1Y;
p1Z = sz1Z - sy1Z;
} else {
p1X = sy1X + sz1X;
p1Y = sy1Y + sz1Y;
p1Z = sy1Z + sz1Z;
p1X = -p1X;
p1Y = -p1Y;
p1Z = -p1Z;
}
break;
case 1:
d1X = y1X;
d1Y = y1Y;
d1Z = y1Z;
let signY1 = sz1X * mAxisX + sz1Y * mAxisY + sz1Z * mAxisZ > 0;
if(sx1X * mAxisX + sx1Y * mAxisY + sx1Z * mAxisZ > 0) {
if(signY1) {
p1X = sx1X + sz1X;
p1Y = sx1Y + sz1Y;
p1Z = sx1Z + sz1Z;
} else {
p1X = sx1X - sz1X;
p1Y = sx1Y - sz1Y;
p1Z = sx1Z - sz1Z;
}
} else if(signY1) {
p1X = sz1X - sx1X;
p1Y = sz1Y - sx1Y;
p1Z = sz1Z - sx1Z;
} else {
p1X = sx1X + sz1X;
p1Y = sx1Y + sz1Y;
p1Z = sx1Z + sz1Z;
p1X = -p1X;
p1Y = -p1Y;
p1Z = -p1Z;
}
break;
default:
d1X = z1X;
d1Y = z1Y;
d1Z = z1Z;
let signY2 = sy1X * mAxisX + sy1Y * mAxisY + sy1Z * mAxisZ > 0;
if(sx1X * mAxisX + sx1Y * mAxisY + sx1Z * mAxisZ > 0) {
if(signY2) {
p1X = sx1X + sy1X;
p1Y = sx1Y + sy1Y;
p1Z = sx1Z + sy1Z;
} else {
p1X = sx1X - sy1X;
p1Y = sx1Y - sy1Y;
p1Z = sx1Z - sy1Z;
}
} else if(signY2) {
p1X = sy1X - sx1X;
p1Y = sy1Y - sx1Y;
p1Z = sy1Z - sx1Z;
} else {
p1X = sx1X + sy1X;
p1Y = sx1Y + sy1Y;
p1Z = sx1Z + sy1Z;
p1X = -p1X;
p1Y = -p1Y;
p1Z = -p1Z;
}
}
p1X = c1X + p1X;
p1Y = c1Y + p1Y;
p1Z = c1Z + p1Z;
switch(id2) {
case 0:
d2X = x2X;
d2Y = x2Y;
d2Z = x2Z;
let signY3 = sz2X * mAxisX + sz2Y * mAxisY + sz2Z * mAxisZ > 0;
if(sy2X * mAxisX + sy2Y * mAxisY + sy2Z * mAxisZ > 0) {
if(signY3) {
p2X = sy2X + sz2X;
p2Y = sy2Y + sz2Y;
p2Z = sy2Z + sz2Z;
} else {
p2X = sy2X - sz2X;
p2Y = sy2Y - sz2Y;
p2Z = sy2Z - sz2Z;
}
} else if(signY3) {
p2X = sz2X - sy2X;
p2Y = sz2Y - sy2Y;
p2Z = sz2Z - sy2Z;
} else {
p2X = sy2X + sz2X;
p2Y = sy2Y + sz2Y;
p2Z = sy2Z + sz2Z;
p2X = -p2X;
p2Y = -p2Y;
p2Z = -p2Z;
}
break;
case 1:
d2X = y2X;
d2Y = y2Y;
d2Z = y2Z;
let signY4 = sz2X * mAxisX + sz2Y * mAxisY + sz2Z * mAxisZ > 0;
if(sx2X * mAxisX + sx2Y * mAxisY + sx2Z * mAxisZ > 0) {
if(signY4) {
p2X = sx2X + sz2X;
p2Y = sx2Y + sz2Y;
p2Z = sx2Z + sz2Z;
} else {
p2X = sx2X - sz2X;
p2Y = sx2Y - sz2Y;
p2Z = sx2Z - sz2Z;
}
} else if(signY4) {
p2X = sz2X - sx2X;
p2Y = sz2Y - sx2Y;
p2Z = sz2Z - sx2Z;
} else {
p2X = sx2X + sz2X;
p2Y = sx2Y + sz2Y;
p2Z = sx2Z + sz2Z;
p2X = -p2X;
p2Y = -p2Y;
p2Z = -p2Z;
}
break;
default:
d2X = z2X;
d2Y = z2Y;
d2Z = z2Z;
let signY5 = sy2X * mAxisX + sy2Y * mAxisY + sy2Z * mAxisZ > 0;
if(sx2X * mAxisX + sx2Y * mAxisY + sx2Z * mAxisZ > 0) {
if(signY5) {
p2X = sx2X + sy2X;
p2Y = sx2Y + sy2Y;
p2Z = sx2Z + sy2Z;
} else {
p2X = sx2X - sy2X;
p2Y = sx2Y - sy2Y;
p2Z = sx2Z - sy2Z;
}
} else if(signY5) {
p2X = sy2X - sx2X;
p2Y = sy2Y - sx2Y;
p2Z = sy2Z - sx2Z;
} else {
p2X = sx2X + sy2X;
p2Y = sx2Y + sy2Y;
p2Z = sx2Z + sy2Z;
p2X = -p2X;
p2Y = -p2Y;
p2Z = -p2Z;
}
}
p2X = c2X - p2X;
p2Y = c2Y - p2Y;
p2Z = c2Z - p2Z;
let rX;
let rY;
let rZ;
rX = p1X - p2X;
rY = p1Y - p2Y;
rZ = p1Z - p2Z;
let dot12 = d1X * d2X + d1Y * d2Y + d1Z * d2Z;
let dot1r = d1X * rX + d1Y * rY + d1Z * rZ;
let dot2r = d2X * rX + d2Y * rY + d2Z * rZ;
let invDet = 1 / (1 - dot12 * dot12);
let t1 = (dot12 * dot2r - dot1r) * invDet;
let t2 = (dot2r - dot12 * dot1r) * invDet;
let cp1X;
let cp1Y;
let cp1Z;
let cp2X;
let cp2Y;
let cp2Z;
cp1X = p1X + d1X * t1;
cp1Y = p1Y + d1Y * t1;
cp1Z = p1Z + d1Z * t1;
cp2X = p2X + d2X * t2;
cp2Y = p2Y + d2Y * t2;
cp2Z = p2Z + d2Z * t2;
let normalX;
let normalY;
let normalZ;
normalX = -mAxisX;
normalY = -mAxisY;
normalZ = -mAxisZ;
this.setNormal(result,normalX,normalY,normalZ);
this.addPoint(result,cp1X,cp1Y,cp1Z,cp2X,cp2Y,cp2Z,mDepth,4);
return;
}
let tmpX;
let tmpY;
let tmpZ;
let swapped;
if(mId >= 3) {
mSign = -mSign;
c12X = -c12X;
c12Y = -c12Y;
c12Z = -c12Z;
w1 = w2;
h1 = h2;
d1 = d2;
c1X = c2X;
c1Y = c2Y;
c1Z = c2Z;
tmpX = x1X;
tmpY = x1Y;
tmpZ = x1Z;
x1X = x2X;
x1Y = x2Y;
x1Z = x2Z;
x2X = tmpX;
x2Y = tmpY;
x2Z = tmpZ;
tmpX = y1X;
tmpY = y1Y;
tmpZ = y1Z;
y1X = y2X;
y1Y = y2Y;
y1Z = y2Z;
y2X = tmpX;
y2Y = tmpY;
y2Z = tmpZ;
tmpX = z1X;
tmpY = z1Y;
tmpZ = z1Z;
z1X = z2X;
z1Y = z2Y;
z1Z = z2Z;
z2X = tmpX;
z2Y = tmpY;
z2Z = tmpZ;
tmpX = sx1X;
tmpY = sx1Y;
tmpZ = sx1Z;
sx1X = sx2X;
sx1Y = sx2Y;
sx1Z = sx2Z;
sx2X = tmpX;
sx2Y = tmpY;
sx2Z = tmpZ;
tmpX = sy1X;
tmpY = sy1Y;
tmpZ = sy1Z;
sy1X = sy2X;
sy1Y = sy2Y;
sy1Z = sy2Z;
sy2X = tmpX;
sy2Y = tmpY;
sy2Z = tmpZ;
tmpX = sz1X;
tmpY = sz1Y;
tmpZ = sz1Z;
sz1X = sz2X;
sz1Y = sz2Y;
sz1Z = sz2Z;
sz2X = tmpX;
sz2Y = tmpY;
sz2Z = tmpZ;
mId -= 3;
swapped = true;
} else {
swapped = false;
}
let refCenterX;
let refCenterY;
let refCenterZ;
let refNormalX;
let refNormalY;
let refNormalZ;
let refXX;
let refXY;
let refXZ;
let refYX;
let refYY;
let refYZ;
let refW;
let refH;
switch(mId) {
case 0:
refCenterX = sx1X;
refCenterY = sx1Y;
refCenterZ = sx1Z;
refNormalX = x1X;
refNormalY = x1Y;
refNormalZ = x1Z;
refXX = y1X;
refXY = y1Y;
refXZ = y1Z;
refYX = z1X;
refYY = z1Y;
refYZ = z1Z;
refW = h1;
refH = d1;
break;
case 1:
refCenterX = sy1X;
refCenterY = sy1Y;
refCenterZ = sy1Z;
refNormalX = y1X;
refNormalY = y1Y;
refNormalZ = y1Z;
refXX = z1X;
refXY = z1Y;
refXZ = z1Z;
refYX = x1X;
refYY = x1Y;
refYZ = x1Z;
refW = d1;
refH = w1;
break;
default:
refCenterX = sz1X;
refCenterY = sz1Y;
refCenterZ = sz1Z;
refNormalX = z1X;
refNormalY = z1Y;
refNormalZ = z1Z;
refXX = x1X;
refXY = x1Y;
refXZ = x1Z;
refYX = y1X;
refYY = y1Y;
refYZ = y1Z;
refW = w1;
refH = h1;
}
if(mSign < 0) {
refCenterX = -refCenterX;
refCenterY = -refCenterY;
refCenterZ = -refCenterZ;
refNormalX = -refNormalX;
refNormalY = -refNormalY;
refNormalZ = -refNormalZ;
tmpX = refXX;
tmpY = refXY;
tmpZ = refXZ;
refXX = refYX;
refXY = refYY;
refXZ = refYZ;
refYX = tmpX;
refYY = tmpY;
refYZ = tmpZ;
let tmp = refW;
refW = refH;
refH = tmp;
}
refCenterX += c1X;
refCenterY += c1Y;
refCenterZ += c1Z;
let minIncDot = 1;
let incId = 0;
let incDot = refNormalX * x2X + refNormalY * x2Y + refNormalZ * x2Z;
if(incDot < minIncDot) {
minIncDot = incDot;
incId = 0;
}
if(-incDot < minIncDot) {
minIncDot = -incDot;
incId = 1;
}
incDot = refNormalX * y2X + refNormalY * y2Y + refNormalZ * y2Z;
if(incDot < minIncDot) {
minIncDot = incDot;
incId = 2;
}
if(-incDot < minIncDot) {
minIncDot = -incDot;
incId = 3;
}
incDot = refNormalX * z2X + refNormalY * z2Y + refNormalZ * z2Z;
if(incDot < minIncDot) {
minIncDot = incDot;
incId = 4;
}
if(-incDot < minIncDot) {
incId = 5;
}
let incV1X;
let incV1Y;
let incV1Z;
let incV2X;
let incV2Y;
let incV2Z;
let incV3X;
let incV3Y;
let incV3Z;
let incV4X;
let incV4Y;
let incV4Z;
switch(incId) {
case 0:
incV1X = sx2X + sy2X;
incV1Y = sx2Y + sy2Y;
incV1Z = sx2Z + sy2Z;
incV1X += sz2X;
incV1Y += sz2Y;
incV1Z += sz2Z;
incV2X = sx2X - sy2X;
incV2Y = sx2Y - sy2Y;
incV2Z = sx2Z - sy2Z;
incV2X += sz2X;
incV2Y += sz2Y;
incV2Z += sz2Z;
incV3X = sx2X - sy2X;
incV3Y = sx2Y - sy2Y;
incV3Z = sx2Z - sy2Z;
incV3X -= sz2X;
incV3Y -= sz2Y;
incV3Z -= sz2Z;
incV4X = sx2X + sy2X;
incV4Y = sx2Y + sy2Y;
incV4Z = sx2Z + sy2Z;
incV4X -= sz2X;
incV4Y -= sz2Y;
incV4Z -= sz2Z;
break;
case 1:
incV1X = sy2X - sx2X;
incV1Y = sy2Y - sx2Y;
incV1Z = sy2Z - sx2Z;
incV1X += sz2X;
incV1Y += sz2Y;
incV1Z += sz2Z;
incV2X = sy2X - sx2X;
incV2Y = sy2Y - sx2Y;
incV2Z = sy2Z - sx2Z;
incV2X -= sz2X;
incV2Y -= sz2Y;
incV2Z -= sz2Z;
incV3X = sx2X + sy2X;
incV3Y = sx2Y + sy2Y;
incV3Z = sx2Z + sy2Z;
incV3X = -incV3X;
incV3Y = -incV3Y;
incV3Z = -incV3Z;
incV3X -= sz2X;
incV3Y -= sz2Y;
incV3Z -= sz2Z;
incV4X = sx2X + sy2X;
incV4Y = sx2Y + sy2Y;
incV4Z = sx2Z + sy2Z;
incV4X = -incV4X;
incV4Y = -incV4Y;
incV4Z = -incV4Z;
incV4X += sz2X;
incV4Y += sz2Y;
incV4Z += sz2Z;
break;
case 2:
incV1X = sx2X + sy2X;
incV1Y = sx2Y + sy2Y;
incV1Z = sx2Z + sy2Z;
incV1X += sz2X;
incV1Y += sz2Y;
incV1Z += sz2Z;
incV2X = sx2X + sy2X;
incV2Y = sx2Y + sy2Y;
incV2Z = sx2Z + sy2Z;
incV2X -= sz2X;
incV2Y -= sz2Y;
incV2Z -= sz2Z;
incV3X = sy2X - sx2X;
incV3Y = sy2Y - sx2Y;
incV3Z = sy2Z - sx2Z;
incV3X -= sz2X;
incV3Y -= sz2Y;
incV3Z -= sz2Z;
incV4X = sy2X - sx2X;
incV4Y = sy2Y - sx2Y;
incV4Z = sy2Z - sx2Z;
incV4X += sz2X;
incV4Y += sz2Y;
incV4Z += sz2Z;
break;
case 3:
incV1X = sx2X - sy2X;
incV1Y = sx2Y - sy2Y;
incV1Z = sx2Z - sy2Z;
incV1X += sz2X;
incV1Y += sz2Y;
incV1Z += sz2Z;
incV2X = sx2X + sy2X;
incV2Y = sx2Y + sy2Y;
incV2Z = sx2Z + sy2Z;
incV2X = -incV2X;
incV2Y = -incV2Y;
incV2Z = -incV2Z;
incV2X += sz2X;
incV2Y += sz2Y;
incV2Z += sz2Z;
incV3X = sx2X + sy2X;
incV3Y = sx2Y + sy2Y;
incV3Z = sx2Z + sy2Z;
incV3X = -incV3X;
incV3Y = -incV3Y;
incV3Z = -incV3Z;
incV3X -= sz2X;
incV3Y -= sz2Y;
incV3Z -= sz2Z;
incV4X = sx2X - sy2X;
incV4Y = sx2Y - sy2Y;
incV4Z = sx2Z - sy2Z;
incV4X -= sz2X;
incV4Y -= sz2Y;
incV4Z -= sz2Z;
break;
case 4:
incV1X = sx2X + sy2X;
incV1Y = sx2Y + sy2Y;
incV1Z = sx2Z + sy2Z;
incV1X += sz2X;
incV1Y += sz2Y;
incV1Z += sz2Z;
incV2X = sy2X - sx2X;
incV2Y = sy2Y - sx2Y;
incV2Z = sy2Z - sx2Z;
incV2X += sz2X;
incV2Y += sz2Y;
incV2Z += sz2Z;
incV3X = sx2X + sy2X;
incV3Y = sx2Y + sy2Y;
incV3Z = sx2Z + sy2Z;
incV3X = -incV3X;
incV3Y = -incV3Y;
incV3Z = -incV3Z;
incV3X += sz2X;
incV3Y += sz2Y;
incV3Z += sz2Z;
incV4X = sx2X - sy2X;
incV4Y = sx2Y - sy2Y;
incV4Z = sx2Z - sy2Z;
incV4X += sz2X;
incV4Y += sz2Y;
incV4Z += sz2Z;
break;
default:
incV1X = sx2X + sy2X;
incV1Y = sx2Y + sy2Y;
incV1Z = sx2Z + sy2Z;
incV1X -= sz2X;
incV1Y -= sz2Y;
incV1Z -= sz2Z;
incV2X = sx2X - sy2X;
incV2Y = sx2Y - sy2Y;
incV2Z = sx2Z - sy2Z;
incV2X -= sz2X;
incV2Y -= sz2Y;
incV2Z -= sz2Z;
incV3X = sx2X + sy2X;
incV3Y = sx2Y + sy2Y;
incV3Z = sx2Z + sy2Z;
incV3X = -incV3X;
incV3Y = -incV3Y;
incV3Z = -incV3Z;
incV3X -= sz2X;
incV3Y -= sz2Y;
incV3Z -= sz2Z;
incV4X = sy2X - sx2X;
incV4Y = sy2Y - sx2Y;
incV4Z = sy2Z - sx2Z;
incV4X -= sz2X;
incV4Y -= sz2Y;
incV4Z -= sz2Z;
}
incV1X += c12X;
incV1Y += c12Y;
incV1Z += c12Z;
incV2X += c12X;
incV2Y += c12Y;
incV2Z += c12Z;
incV3X += c12X;
incV3Y += c12Y;
incV3Z += c12Z;
incV4X += c12X;
incV4Y += c12Y;
incV4Z += c12Z;
let _this = this.clipper;
_this.w = refW;
_this.h = refH;
_this.numVertices = 0;
_this.numTmpVertices = 0;
let _this1 = this.clipper;
let _this2 = _this1.vertices[_this1.numVertices++];
_this2.x = incV1X * refXX + incV1Y * refXY + incV1Z * refXZ;
_this2.y = incV1X * refYX + incV1Y * refYY + incV1Z * refYZ;
_this2.wx = incV1X;
_this2.wy = incV1Y;
_this2.wz = incV1Z;
let _this3 = this.clipper;
let _this4 = _this3.vertices[_this3.numVertices++];
_this4.x = incV2X * refXX + incV2Y * refXY + incV2Z * refXZ;
_this4.y = incV2X * refYX + incV2Y * refYY + incV2Z * refYZ;
_this4.wx = incV2X;
_this4.wy = incV2Y;
_this4.wz = incV2Z;
let _this5 = this.clipper;
let _this6 = _this5.vertices[_this5.numVertices++];
_this6.x = incV3X * refXX + incV3Y * refXY + incV3Z * refXZ;
_this6.y = incV3X * refYX + incV3Y * refYY + incV3Z * refYZ;
_this6.wx = incV3X;
_this6.wy = incV3Y;
_this6.wz = incV3Z;
let _this7 = this.clipper;
let _this8 = _this7.vertices[_this7.numVertices++];
_this8.x = incV4X * refXX + incV4Y * refXY + incV4Z * refXZ;
_this8.y = incV4X * refYX + incV4Y * refYY + incV4Z * refYZ;
_this8.wx = incV4X;
_this8.wy = incV4Y;
_this8.wz = incV4Z;
this.clipper.clip();
this.clipper.reduce();
let normalX;
let normalY;
let normalZ;
if(swapped) {
normalX = refNormalX;
normalY = refNormalY;
normalZ = refNormalZ;
} else {
normalX = -refNormalX;
normalY = -refNormalY;
normalZ = -refNormalZ;
}
this.setNormal(result,normalX,normalY,normalZ);
let _g = 0;
let _g1 = this.clipper.numVertices;
while(_g < _g1) {
let i = _g++;
let v = this.clipper.vertices[i];
let clippedVertexX;
let clippedVertexY;
let clippedVertexZ;
clippedVertexX = v.wx;
clippedVertexY = v.wy;
clippedVertexZ = v.wz;
clippedVertexX += c1X;
clippedVertexY += c1Y;
clippedVertexZ += c1Z;
let clippedVertexToRefCenterX;
let clippedVertexToRefCenterY;
let clippedVertexToRefCenterZ;
clippedVertexToRefCenterX = refCenterX - clippedVertexX;
clippedVertexToRefCenterY = refCenterY - clippedVertexY;
clippedVertexToRefCenterZ = refCenterZ - clippedVertexZ;
let depth = clippedVertexToRefCenterX * refNormalX + clippedVertexToRefCenterY * refNormalY + clippedVertexToRefCenterZ * refNormalZ;
let clippedVertexOnRefFaceX;
let clippedVertexOnRefFaceY;
let clippedVertexOnRefFaceZ;
clippedVertexOnRefFaceX = clippedVertexX + refNormalX * depth;
clippedVertexOnRefFaceY = clippedVertexY + refNormalY * depth;
clippedVertexOnRefFaceZ = clippedVertexZ + refNormalZ * depth;
if(depth > -oimo.common.Setting.contactPersistenceThreshold) {
if(swapped) {
this.addPoint(result,clippedVertexX,clippedVertexY,clippedVertexZ,clippedVertexOnRefFaceX,clippedVertexOnRefFaceY,clippedVertexOnRefFaceZ,depth,i);
} else {
this.addPoint(result,clippedVertexOnRefFaceX,clippedVertexOnRefFaceY,clippedVertexOnRefFaceZ,clippedVertexX,clippedVertexY,clippedVertexZ,depth,i);
}
}
}
}
}
if(!oimo.collision.narrowphase.detector._BoxBoxDetector) oimo.collision.narrowphase.detector._BoxBoxDetector = {};
oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex = class oimo_collision_narrowphase_detector__$BoxBoxDetector_IncidentVertex {
constructor() {
this.x = 0;
this.y = 0;
this.wx = 0;
this.wy = 0;
this.wz = 0;
}
}
oimo.collision.narrowphase.detector._BoxBoxDetector.FaceClipper = class oimo_collision_narrowphase_detector__$BoxBoxDetector_FaceClipper {
constructor() {
this.w = 0;
this.h = 0;
this.numVertices = 0;
this.numTmpVertices = 0;
this.vertices = new Array(8);
this.tmpVertices = new Array(8);
this.vertices[0] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.tmpVertices[0] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.vertices[1] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.tmpVertices[1] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.vertices[2] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.tmpVertices[2] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.vertices[3] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.tmpVertices[3] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.vertices[4] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.tmpVertices[4] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.vertices[5] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.tmpVertices[5] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.vertices[6] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.tmpVertices[6] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.vertices[7] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
this.tmpVertices[7] = new oimo.collision.narrowphase.detector._BoxBoxDetector.IncidentVertex();
}
clip() {
let _g = 0;
let _g1 = this.numVertices;
while(_g < _g1) {
let i = _g++;
let v1 = this.vertices[i];
let v2 = this.vertices[(i + 1) % this.numVertices];
let s1 = this.w + v1.x;
let s2 = this.w + v2.x;
if(s1 > 0 && s2 > 0) {
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x;
_this.y = v1.y;
_this.wx = v1.wx;
_this.wy = v1.wy;
_this.wz = v1.wz;
} else if(s1 > 0 && s2 <= 0) {
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x;
_this.y = v1.y;
_this.wx = v1.wx;
_this.wy = v1.wy;
_this.wz = v1.wz;
let t = s1 / (s1 - s2);
let _this1 = this.tmpVertices[this.numTmpVertices++];
_this1.x = v1.x + (v2.x - v1.x) * t;
_this1.y = v1.y + (v2.y - v1.y) * t;
_this1.wx = v1.wx + (v2.wx - v1.wx) * t;
_this1.wy = v1.wy + (v2.wy - v1.wy) * t;
_this1.wz = v1.wz + (v2.wz - v1.wz) * t;
} else if(s1 <= 0 && s2 > 0) {
let t = s1 / (s1 - s2);
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x + (v2.x - v1.x) * t;
_this.y = v1.y + (v2.y - v1.y) * t;
_this.wx = v1.wx + (v2.wx - v1.wx) * t;
_this.wy = v1.wy + (v2.wy - v1.wy) * t;
_this.wz = v1.wz + (v2.wz - v1.wz) * t;
}
}
let tmp = this.vertices;
this.vertices = this.tmpVertices;
this.tmpVertices = tmp;
this.numVertices = this.numTmpVertices;
this.numTmpVertices = 0;
let _g2 = 0;
let _g3 = this.numVertices;
while(_g2 < _g3) {
let i = _g2++;
let v1 = this.vertices[i];
let v2 = this.vertices[(i + 1) % this.numVertices];
let s1 = this.w - v1.x;
let s2 = this.w - v2.x;
if(s1 > 0 && s2 > 0) {
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x;
_this.y = v1.y;
_this.wx = v1.wx;
_this.wy = v1.wy;
_this.wz = v1.wz;
} else if(s1 > 0 && s2 <= 0) {
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x;
_this.y = v1.y;
_this.wx = v1.wx;
_this.wy = v1.wy;
_this.wz = v1.wz;
let t = s1 / (s1 - s2);
let _this1 = this.tmpVertices[this.numTmpVertices++];
_this1.x = v1.x + (v2.x - v1.x) * t;
_this1.y = v1.y + (v2.y - v1.y) * t;
_this1.wx = v1.wx + (v2.wx - v1.wx) * t;
_this1.wy = v1.wy + (v2.wy - v1.wy) * t;
_this1.wz = v1.wz + (v2.wz - v1.wz) * t;
} else if(s1 <= 0 && s2 > 0) {
let t = s1 / (s1 - s2);
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x + (v2.x - v1.x) * t;
_this.y = v1.y + (v2.y - v1.y) * t;
_this.wx = v1.wx + (v2.wx - v1.wx) * t;
_this.wy = v1.wy + (v2.wy - v1.wy) * t;
_this.wz = v1.wz + (v2.wz - v1.wz) * t;
}
}
let tmp1 = this.vertices;
this.vertices = this.tmpVertices;
this.tmpVertices = tmp1;
this.numVertices = this.numTmpVertices;
this.numTmpVertices = 0;
let _g4 = 0;
let _g5 = this.numVertices;
while(_g4 < _g5) {
let i = _g4++;
let v1 = this.vertices[i];
let v2 = this.vertices[(i + 1) % this.numVertices];
let s1 = this.h + v1.y;
let s2 = this.h + v2.y;
if(s1 > 0 && s2 > 0) {
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x;
_this.y = v1.y;
_this.wx = v1.wx;
_this.wy = v1.wy;
_this.wz = v1.wz;
} else if(s1 > 0 && s2 <= 0) {
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x;
_this.y = v1.y;
_this.wx = v1.wx;
_this.wy = v1.wy;
_this.wz = v1.wz;
let t = s1 / (s1 - s2);
let _this1 = this.tmpVertices[this.numTmpVertices++];
_this1.x = v1.x + (v2.x - v1.x) * t;
_this1.y = v1.y + (v2.y - v1.y) * t;
_this1.wx = v1.wx + (v2.wx - v1.wx) * t;
_this1.wy = v1.wy + (v2.wy - v1.wy) * t;
_this1.wz = v1.wz + (v2.wz - v1.wz) * t;
} else if(s1 <= 0 && s2 > 0) {
let t = s1 / (s1 - s2);
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x + (v2.x - v1.x) * t;
_this.y = v1.y + (v2.y - v1.y) * t;
_this.wx = v1.wx + (v2.wx - v1.wx) * t;
_this.wy = v1.wy + (v2.wy - v1.wy) * t;
_this.wz = v1.wz + (v2.wz - v1.wz) * t;
}
}
let tmp2 = this.vertices;
this.vertices = this.tmpVertices;
this.tmpVertices = tmp2;
this.numVertices = this.numTmpVertices;
this.numTmpVertices = 0;
let _g6 = 0;
let _g7 = this.numVertices;
while(_g6 < _g7) {
let i = _g6++;
let v1 = this.vertices[i];
let v2 = this.vertices[(i + 1) % this.numVertices];
let s1 = this.h - v1.y;
let s2 = this.h - v2.y;
if(s1 > 0 && s2 > 0) {
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x;
_this.y = v1.y;
_this.wx = v1.wx;
_this.wy = v1.wy;
_this.wz = v1.wz;
} else if(s1 > 0 && s2 <= 0) {
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x;
_this.y = v1.y;
_this.wx = v1.wx;
_this.wy = v1.wy;
_this.wz = v1.wz;
let t = s1 / (s1 - s2);
let _this1 = this.tmpVertices[this.numTmpVertices++];
_this1.x = v1.x + (v2.x - v1.x) * t;
_this1.y = v1.y + (v2.y - v1.y) * t;
_this1.wx = v1.wx + (v2.wx - v1.wx) * t;
_this1.wy = v1.wy + (v2.wy - v1.wy) * t;
_this1.wz = v1.wz + (v2.wz - v1.wz) * t;
} else if(s1 <= 0 && s2 > 0) {
let t = s1 / (s1 - s2);
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = v1.x + (v2.x - v1.x) * t;
_this.y = v1.y + (v2.y - v1.y) * t;
_this.wx = v1.wx + (v2.wx - v1.wx) * t;
_this.wy = v1.wy + (v2.wy - v1.wy) * t;
_this.wz = v1.wz + (v2.wz - v1.wz) * t;
}
}
let tmp3 = this.vertices;
this.vertices = this.tmpVertices;
this.tmpVertices = tmp3;
this.numVertices = this.numTmpVertices;
this.numTmpVertices = 0;
}
reduce() {
if(this.numVertices < 4) {
return;
}
let max1 = -1e65536;
let min1 = 1e65536;
let max2 = -1e65536;
let min2 = 1e65536;
let max1V = null;
let min1V = null;
let max2V = null;
let min2V = null;
let e1x = 1;
let e1y = 1;
let e2x = -1;
let e2y = 1;
let _g = 0;
let _g1 = this.numVertices;
while(_g < _g1) {
let v = this.vertices[_g++];
let dot1 = v.x * e1x + v.y * e1y;
let dot2 = v.x * e2x + v.y * e2y;
if(dot1 > max1) {
max1 = dot1;
max1V = v;
}
if(dot1 < min1) {
min1 = dot1;
min1V = v;
}
if(dot2 > max2) {
max2 = dot2;
max2V = v;
}
if(dot2 < min2) {
min2 = dot2;
min2V = v;
}
}
let _this = this.tmpVertices[this.numTmpVertices++];
_this.x = max1V.x;
_this.y = max1V.y;
_this.wx = max1V.wx;
_this.wy = max1V.wy;
_this.wz = max1V.wz;
let _this1 = this.tmpVertices[this.numTmpVertices++];
_this1.x = max2V.x;
_this1.y = max2V.y;
_this1.wx = max2V.wx;
_this1.wy = max2V.wy;
_this1.wz = max2V.wz;
let _this2 = this.tmpVertices[this.numTmpVertices++];
_this2.x = min1V.x;
_this2.y = min1V.y;
_this2.wx = min1V.wx;
_this2.wy = min1V.wy;
_this2.wz = min1V.wz;
let _this3 = this.tmpVertices[this.numTmpVertices++];
_this3.x = min2V.x;
_this3.y = min2V.y;
_this3.wx = min2V.wx;
_this3.wy = min2V.wy;
_this3.wz = min2V.wz;
let tmp = this.vertices;
this.vertices = this.tmpVertices;
this.tmpVertices = tmp;
this.numVertices = this.numTmpVertices;
this.numTmpVertices = 0;
}
}
oimo.collision.narrowphase.detector.BoxBoxDetectorMacro = class oimo_collision_narrowphase_detector_BoxBoxDetectorMacro {
}
oimo.collision.narrowphase.detector.CachedDetectorData = class oimo_collision_narrowphase_detector_CachedDetectorData {
constructor() {
}
_clear() {
if(this._gjkCache != null) {
this._gjkCache.clear();
}
}
}
oimo.collision.narrowphase.detector.CapsuleCapsuleDetector = class oimo_collision_narrowphase_detector_CapsuleCapsuleDetector extends oimo.collision.narrowphase.detector.Detector {
constructor() {
super(false);
}
detectImpl(result,geom1,geom2,tf1,tf2,cachedData) {
let c1 = geom1;
let c2 = geom2;
result.incremental = false;
let axis1X;
let axis1Y;
let axis1Z;
let axis2X;
let axis2Y;
let axis2Z;
axis1X = tf1._rotation01;
axis1Y = tf1._rotation11;
axis1Z = tf1._rotation21;
axis2X = tf2._rotation01;
axis2Y = tf2._rotation11;
axis2Z = tf2._rotation21;
let hh1 = c1._halfHeight;
let hh2 = c2._halfHeight;
let r1 = c1._radius;
let r2 = c2._radius;
let p1X;
let p1Y;
let p1Z;
let q1X;
let q1Y;
let q1Z;
let p2X;
let p2Y;
let p2Z;
let q2X;
let q2Y;
let q2Z;
p1X = tf1._positionX + axis1X * -hh1;
p1Y = tf1._positionY + axis1Y * -hh1;
p1Z = tf1._positionZ + axis1Z * -hh1;
q1X = tf1._positionX + axis1X * hh1;
q1Y = tf1._positionY + axis1Y * hh1;
q1Z = tf1._positionZ + axis1Z * hh1;
p2X = tf2._positionX + axis2X * -hh2;
p2Y = tf2._positionY + axis2Y * -hh2;
p2Z = tf2._positionZ + axis2Z * -hh2;
q2X = tf2._positionX + axis2X * hh2;
q2Y = tf2._positionY + axis2Y * hh2;
q2Z = tf2._positionZ + axis2Z * hh2;
let p12X;
let p12Y;
let p12Z;
p12X = p1X - p2X;
p12Y = p1Y - p2Y;
p12Z = p1Z - p2Z;
let d1X;
let d1Y;
let d1Z;
let d2X;
let d2Y;
let d2Z;
d1X = q1X - p1X;
d1Y = q1Y - p1Y;
d1Z = q1Z - p1Z;
d2X = q2X - p2X;
d2Y = q2Y - p2Y;
d2Z = q2Z - p2Z;
let p21d1 = -(p12X * d1X + p12Y * d1Y + p12Z * d1Z);
let p12d2 = p12X * d2X + p12Y * d2Y + p12Z * d2Z;
let d11 = hh1 * hh1 * 4;
let d12 = d1X * d2X + d1Y * d2Y + d1Z * d2Z;
let d22 = hh2 * hh2 * 4;
let t1;
let t2;
if(d11 == 0 && d22 == 0) {
t1 = 0;
t2 = 0;
} else if(d11 == 0) {
t1 = 0;
if(p12d2 < 0) {
t2 = 0;
} else if(p12d2 > d22) {
t2 = 1;
} else {
t2 = p12d2 / d22;
}
} else if(d22 == 0) {
t2 = 0;
if(p21d1 < 0) {
t1 = 0;
} else if(p21d1 > d11) {
t1 = 1;
} else {
t1 = p21d1 / d11;
}
} else {
let det = d11 * d22 - d12 * d12;
if(det == 0) {
t1 = 0;
} else {
t1 = d12 * p12d2 + d22 * p21d1;
if(t1 < 0) {
t1 = 0;
} else if(t1 > det) {
t1 = 1;
} else {
t1 /= det;
}
}
t2 = t1 * d12 + p12d2;
if(t2 < 0) {
t2 = 0;
if(p21d1 < 0) {
t1 = 0;
} else if(p21d1 > d11) {
t1 = 1;
} else {
t1 = p21d1 / d11;
}
} else if(t2 > d22) {
t2 = 1;
t1 = d12 + p21d1;
if(t1 < 0) {
t1 = 0;
} else if(t1 > d11) {
t1 = 1;
} else {
t1 /= d11;
}
} else {
t2 /= d22;
}
}
let cp1X;
let cp1Y;
let cp1Z;
let cp2X;
let cp2Y;
let cp2Z;
cp1X = p1X + d1X * t1;
cp1Y = p1Y + d1Y * t1;
cp1Z = p1Z + d1Z * t1;
cp2X = p2X + d2X * t2;
cp2Y = p2Y + d2Y * t2;
cp2Z = p2Z + d2Z * t2;
let dX;
let dY;
let dZ;
dX = cp1X - cp2X;
dY = cp1Y - cp2Y;
dZ = cp1Z - cp2Z;
let len2 = dX * dX + dY * dY + dZ * dZ;
if(len2 >= (r1 + r2) * (r1 + r2)) {
return;
}
let len = Math.sqrt(len2);
let nX;
let nY;
let nZ;
if(len > 0) {
nX = dX * (1 / len);
nY = dY * (1 / len);
nZ = dZ * (1 / len);
} else {
nX = 1;
nY = 0;
nZ = 0;
}
this.setNormal(result,nX,nY,nZ);
let pos1X;
let pos1Y;
let pos1Z;
let pos2X;
let pos2Y;
let pos2Z;
pos1X = cp1X + nX * -r1;
pos1Y = cp1Y + nY * -r1;
pos1Z = cp1Z + nZ * -r1;
pos2X = cp2X + nX * r2;
pos2Y = cp2Y + nY * r2;
pos2Z = cp2Z + nZ * r2;
this.addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,r1 + r2 - len,0);
}
}
oimo.collision.narrowphase.detector.GjkEpaDetector = class oimo_collision_narrowphase_detector_GjkEpaDetector extends oimo.collision.narrowphase.detector.Detector {
constructor() {
super(false);
}
detectImpl(result,geom1,geom2,tf1,tf2,cachedData) {
let gjkEpa = oimo.collision.narrowphase.detector.gjkepa.GjkEpa.instance;
let g1 = geom1;
let g2 = geom2;
let status = gjkEpa.computeClosestPointsImpl(g1,g2,tf1,tf2,oimo.common.Setting.enableGJKCaching ? cachedData : null,true);
result.incremental = true;
if(status != oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.SUCCEEDED) {
console.log("src/oimo/collision/narrowphase/detector/GjkEpaDetector.hx:28:","GJK/EPA failed: status=" + status);
return;
}
if(gjkEpa.distance > g1._gjkMargin + g2._gjkMargin) {
return;
}
let pos1X;
let pos1Y;
let pos1Z;
let pos2X;
let pos2Y;
let pos2Z;
let v = gjkEpa.closestPoint1;
pos1X = v.x;
pos1Y = v.y;
pos1Z = v.z;
let v1 = gjkEpa.closestPoint2;
pos2X = v1.x;
pos2Y = v1.y;
pos2Z = v1.z;
let normalX;
let normalY;
let normalZ;
normalX = pos1X - pos2X;
normalY = pos1Y - pos2Y;
normalZ = pos1Z - pos2Z;
if(normalX * normalX + normalY * normalY + normalZ * normalZ == 0) {
return;
}
if(gjkEpa.distance < 0) {
normalX = -normalX;
normalY = -normalY;
normalZ = -normalZ;
}
let l = normalX * normalX + normalY * normalY + normalZ * normalZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
normalX *= l;
normalY *= l;
normalZ *= l;
this.setNormal(result,normalX,normalY,normalZ);
pos1X += normalX * -g1._gjkMargin;
pos1Y += normalY * -g1._gjkMargin;
pos1Z += normalZ * -g1._gjkMargin;
pos2X += normalX * g2._gjkMargin;
pos2Y += normalY * g2._gjkMargin;
pos2Z += normalZ * g2._gjkMargin;
this.addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,g1._gjkMargin + g2._gjkMargin - gjkEpa.distance,0);
}
}
oimo.collision.narrowphase.detector.SphereBoxDetector = class oimo_collision_narrowphase_detector_SphereBoxDetector extends oimo.collision.narrowphase.detector.Detector {
constructor(swapped) {
super(swapped);
}
detectImpl(result,geom1,geom2,tf1,tf2,cachedData) {
let b = geom2;
result.incremental = false;
let halfExtX;
let halfExtY;
let halfExtZ;
let negHalfExtX;
let negHalfExtY;
let negHalfExtZ;
halfExtX = b._halfExtentsX;
halfExtY = b._halfExtentsY;
halfExtZ = b._halfExtentsZ;
negHalfExtX = -halfExtX;
negHalfExtY = -halfExtY;
negHalfExtZ = -halfExtZ;
let r = geom1._radius;
let boxToSphereX;
let boxToSphereY;
let boxToSphereZ;
boxToSphereX = tf1._positionX - tf2._positionX;
boxToSphereY = tf1._positionY - tf2._positionY;
boxToSphereZ = tf1._positionZ - tf2._positionZ;
let boxToSphereInBoxX;
let boxToSphereInBoxY;
let boxToSphereInBoxZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf2._rotation00 * boxToSphereX + tf2._rotation10 * boxToSphereY + tf2._rotation20 * boxToSphereZ;
__tmp__Y = tf2._rotation01 * boxToSphereX + tf2._rotation11 * boxToSphereY + tf2._rotation21 * boxToSphereZ;
__tmp__Z = tf2._rotation02 * boxToSphereX + tf2._rotation12 * boxToSphereY + tf2._rotation22 * boxToSphereZ;
boxToSphereInBoxX = __tmp__X;
boxToSphereInBoxY = __tmp__Y;
boxToSphereInBoxZ = __tmp__Z;
if(negHalfExtX < boxToSphereInBoxX && halfExtX > boxToSphereInBoxX && negHalfExtY < boxToSphereInBoxY && halfExtY > boxToSphereInBoxY && negHalfExtZ < boxToSphereInBoxZ && halfExtZ > boxToSphereInBoxZ) {
let sphereToBoxSurfaceX;
let sphereToBoxSurfaceY;
let sphereToBoxSurfaceZ;
sphereToBoxSurfaceX = boxToSphereInBoxX < 0 ? -boxToSphereInBoxX : boxToSphereInBoxX;
sphereToBoxSurfaceY = boxToSphereInBoxY < 0 ? -boxToSphereInBoxY : boxToSphereInBoxY;
sphereToBoxSurfaceZ = boxToSphereInBoxZ < 0 ? -boxToSphereInBoxZ : boxToSphereInBoxZ;
sphereToBoxSurfaceX = halfExtX - sphereToBoxSurfaceX;
sphereToBoxSurfaceY = halfExtY - sphereToBoxSurfaceY;
sphereToBoxSurfaceZ = halfExtZ - sphereToBoxSurfaceZ;
let normalInBoxX;
let normalInBoxY;
let normalInBoxZ;
let distX = sphereToBoxSurfaceX;
let distY = sphereToBoxSurfaceY;
let distZ = sphereToBoxSurfaceZ;
let depth;
let projectionMaskX;
let projectionMaskY;
let projectionMaskZ;
if(distX < distY) {
if(distX < distZ) {
if(boxToSphereInBoxX > 0) {
normalInBoxX = 1;
normalInBoxY = 0;
normalInBoxZ = 0;
} else {
normalInBoxX = -1;
normalInBoxY = 0;
normalInBoxZ = 0;
}
projectionMaskX = 0;
projectionMaskY = 1;
projectionMaskZ = 1;
depth = distX;
} else {
if(boxToSphereInBoxZ > 0) {
normalInBoxX = 0;
normalInBoxY = 0;
normalInBoxZ = 1;
} else {
normalInBoxX = 0;
normalInBoxY = 0;
normalInBoxZ = -1;
}
projectionMaskX = 1;
projectionMaskY = 1;
projectionMaskZ = 0;
depth = distZ;
}
} else if(distY < distZ) {
if(boxToSphereInBoxY > 0) {
normalInBoxX = 0;
normalInBoxY = 1;
normalInBoxZ = 0;
} else {
normalInBoxX = 0;
normalInBoxY = -1;
normalInBoxZ = 0;
}
projectionMaskX = 1;
projectionMaskY = 0;
projectionMaskZ = 1;
depth = distY;
} else {
if(boxToSphereInBoxZ > 0) {
normalInBoxX = 0;
normalInBoxY = 0;
normalInBoxZ = 1;
} else {
normalInBoxX = 0;
normalInBoxY = 0;
normalInBoxZ = -1;
}
projectionMaskX = 1;
projectionMaskY = 1;
projectionMaskZ = 0;
depth = distZ;
}
let baseX;
let baseY;
let baseZ;
baseX = projectionMaskX * boxToSphereInBoxX;
baseY = projectionMaskY * boxToSphereInBoxY;
baseZ = projectionMaskZ * boxToSphereInBoxZ;
let boxToClosestPointInBoxX;
let boxToClosestPointInBoxY;
let boxToClosestPointInBoxZ;
boxToClosestPointInBoxX = normalInBoxX * halfExtX;
boxToClosestPointInBoxY = normalInBoxY * halfExtY;
boxToClosestPointInBoxZ = normalInBoxZ * halfExtZ;
boxToClosestPointInBoxX += baseX;
boxToClosestPointInBoxY += baseY;
boxToClosestPointInBoxZ += baseZ;
let boxToClosestPointX;
let boxToClosestPointY;
let boxToClosestPointZ;
let normalX;
let normalY;
let normalZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf2._rotation00 * boxToClosestPointInBoxX + tf2._rotation01 * boxToClosestPointInBoxY + tf2._rotation02 * boxToClosestPointInBoxZ;
__tmp__Y = tf2._rotation10 * boxToClosestPointInBoxX + tf2._rotation11 * boxToClosestPointInBoxY + tf2._rotation12 * boxToClosestPointInBoxZ;
__tmp__Z = tf2._rotation20 * boxToClosestPointInBoxX + tf2._rotation21 * boxToClosestPointInBoxY + tf2._rotation22 * boxToClosestPointInBoxZ;
boxToClosestPointX = __tmp__X;
boxToClosestPointY = __tmp__Y;
boxToClosestPointZ = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * normalInBoxX + tf2._rotation01 * normalInBoxY + tf2._rotation02 * normalInBoxZ;
__tmp__Y1 = tf2._rotation10 * normalInBoxX + tf2._rotation11 * normalInBoxY + tf2._rotation12 * normalInBoxZ;
__tmp__Z1 = tf2._rotation20 * normalInBoxX + tf2._rotation21 * normalInBoxY + tf2._rotation22 * normalInBoxZ;
normalX = __tmp__X1;
normalY = __tmp__Y1;
normalZ = __tmp__Z1;
this.setNormal(result,normalX,normalY,normalZ);
let pos1X;
let pos1Y;
let pos1Z;
let pos2X;
let pos2Y;
let pos2Z;
pos1X = tf1._positionX + normalX * -r;
pos1Y = tf1._positionY + normalY * -r;
pos1Z = tf1._positionZ + normalZ * -r;
pos2X = tf2._positionX + boxToClosestPointX;
pos2Y = tf2._positionY + boxToClosestPointY;
pos2Z = tf2._positionZ + boxToClosestPointZ;
this.addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,depth,0);
return;
}
let boxToClosestPointInBoxX;
let boxToClosestPointInBoxY;
let boxToClosestPointInBoxZ;
halfExtX -= 1e-9;
halfExtY -= 1e-9;
halfExtZ -= 1e-9;
negHalfExtX += 1e-9;
negHalfExtY += 1e-9;
negHalfExtZ += 1e-9;
boxToClosestPointInBoxX = boxToSphereInBoxX < halfExtX ? boxToSphereInBoxX : halfExtX;
boxToClosestPointInBoxY = boxToSphereInBoxY < halfExtY ? boxToSphereInBoxY : halfExtY;
boxToClosestPointInBoxZ = boxToSphereInBoxZ < halfExtZ ? boxToSphereInBoxZ : halfExtZ;
if(!(boxToClosestPointInBoxX > negHalfExtX)) {
boxToClosestPointInBoxX = negHalfExtX;
}
if(!(boxToClosestPointInBoxY > negHalfExtY)) {
boxToClosestPointInBoxY = negHalfExtY;
}
if(!(boxToClosestPointInBoxZ > negHalfExtZ)) {
boxToClosestPointInBoxZ = negHalfExtZ;
}
let closestPointToSphereInBoxX;
let closestPointToSphereInBoxY;
let closestPointToSphereInBoxZ;
closestPointToSphereInBoxX = boxToSphereInBoxX - boxToClosestPointInBoxX;
closestPointToSphereInBoxY = boxToSphereInBoxY - boxToClosestPointInBoxY;
closestPointToSphereInBoxZ = boxToSphereInBoxZ - boxToClosestPointInBoxZ;
let dist = closestPointToSphereInBoxX * closestPointToSphereInBoxX + closestPointToSphereInBoxY * closestPointToSphereInBoxY + closestPointToSphereInBoxZ * closestPointToSphereInBoxZ;
if(dist >= r * r) {
return;
}
dist = Math.sqrt(dist);
let boxToClosestPointX;
let boxToClosestPointY;
let boxToClosestPointZ;
let closestPointToSphereX;
let closestPointToSphereY;
let closestPointToSphereZ;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * boxToClosestPointInBoxX + tf2._rotation01 * boxToClosestPointInBoxY + tf2._rotation02 * boxToClosestPointInBoxZ;
__tmp__Y1 = tf2._rotation10 * boxToClosestPointInBoxX + tf2._rotation11 * boxToClosestPointInBoxY + tf2._rotation12 * boxToClosestPointInBoxZ;
__tmp__Z1 = tf2._rotation20 * boxToClosestPointInBoxX + tf2._rotation21 * boxToClosestPointInBoxY + tf2._rotation22 * boxToClosestPointInBoxZ;
boxToClosestPointX = __tmp__X1;
boxToClosestPointY = __tmp__Y1;
boxToClosestPointZ = __tmp__Z1;
let __tmp__X2;
let __tmp__Y2;
let __tmp__Z2;
__tmp__X2 = tf2._rotation00 * closestPointToSphereInBoxX + tf2._rotation01 * closestPointToSphereInBoxY + tf2._rotation02 * closestPointToSphereInBoxZ;
__tmp__Y2 = tf2._rotation10 * closestPointToSphereInBoxX + tf2._rotation11 * closestPointToSphereInBoxY + tf2._rotation12 * closestPointToSphereInBoxZ;
__tmp__Z2 = tf2._rotation20 * closestPointToSphereInBoxX + tf2._rotation21 * closestPointToSphereInBoxY + tf2._rotation22 * closestPointToSphereInBoxZ;
closestPointToSphereX = __tmp__X2;
closestPointToSphereY = __tmp__Y2;
closestPointToSphereZ = __tmp__Z2;
let normalX;
let normalY;
let normalZ;
let l = closestPointToSphereX * closestPointToSphereX + closestPointToSphereY * closestPointToSphereY + closestPointToSphereZ * closestPointToSphereZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
normalX = closestPointToSphereX * l;
normalY = closestPointToSphereY * l;
normalZ = closestPointToSphereZ * l;
this.setNormal(result,normalX,normalY,normalZ);
let pos1X;
let pos1Y;
let pos1Z;
let pos2X;
let pos2Y;
let pos2Z;
pos1X = tf1._positionX + normalX * -r;
pos1Y = tf1._positionY + normalY * -r;
pos1Z = tf1._positionZ + normalZ * -r;
pos2X = tf2._positionX + boxToClosestPointX;
pos2Y = tf2._positionY + boxToClosestPointY;
pos2Z = tf2._positionZ + boxToClosestPointZ;
this.addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,r - dist,0);
}
}
oimo.collision.narrowphase.detector.SphereCapsuleDetector = class oimo_collision_narrowphase_detector_SphereCapsuleDetector extends oimo.collision.narrowphase.detector.Detector {
constructor(swapped) {
super(swapped);
}
detectImpl(result,geom1,geom2,tf1,tf2,cachedData) {
let c2 = geom2;
result.incremental = false;
let hh2 = c2._halfHeight;
let r1 = geom1._radius;
let r2 = c2._radius;
let axis2X;
let axis2Y;
let axis2Z;
axis2X = tf2._rotation01;
axis2Y = tf2._rotation11;
axis2Z = tf2._rotation21;
let cp1X;
let cp1Y;
let cp1Z;
cp1X = tf1._positionX;
cp1Y = tf1._positionY;
cp1Z = tf1._positionZ;
let p2X;
let p2Y;
let p2Z;
let q2X;
let q2Y;
let q2Z;
p2X = tf2._positionX + axis2X * -hh2;
p2Y = tf2._positionY + axis2Y * -hh2;
p2Z = tf2._positionZ + axis2Z * -hh2;
q2X = tf2._positionX + axis2X * hh2;
q2Y = tf2._positionY + axis2Y * hh2;
q2Z = tf2._positionZ + axis2Z * hh2;
let p12X;
let p12Y;
let p12Z;
p12X = cp1X - p2X;
p12Y = cp1Y - p2Y;
p12Z = cp1Z - p2Z;
let d2X;
let d2Y;
let d2Z;
d2X = q2X - p2X;
d2Y = q2Y - p2Y;
d2Z = q2Z - p2Z;
let d22 = hh2 * hh2 * 4;
let t = p12X * d2X + p12Y * d2Y + p12Z * d2Z;
if(t < 0) {
t = 0;
} else if(t > d22) {
t = 1;
} else {
t /= d22;
}
let cp2X;
let cp2Y;
let cp2Z;
cp2X = p2X + d2X * t;
cp2Y = p2Y + d2Y * t;
cp2Z = p2Z + d2Z * t;
let dX;
let dY;
let dZ;
dX = cp1X - cp2X;
dY = cp1Y - cp2Y;
dZ = cp1Z - cp2Z;
let len2 = dX * dX + dY * dY + dZ * dZ;
if(len2 >= (r1 + r2) * (r1 + r2)) {
return;
}
let len = Math.sqrt(len2);
let nX;
let nY;
let nZ;
if(len > 0) {
nX = dX * (1 / len);
nY = dY * (1 / len);
nZ = dZ * (1 / len);
} else {
nX = 1;
nY = 0;
nZ = 0;
}
this.setNormal(result,nX,nY,nZ);
let pos1X;
let pos1Y;
let pos1Z;
let pos2X;
let pos2Y;
let pos2Z;
pos1X = cp1X + nX * -r1;
pos1Y = cp1Y + nY * -r1;
pos1Z = cp1Z + nZ * -r1;
pos2X = cp2X + nX * r2;
pos2Y = cp2Y + nY * r2;
pos2Z = cp2Z + nZ * r2;
this.addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,r1 + r2 - len,0);
}
}
oimo.collision.narrowphase.detector.SphereSphereDetector = class oimo_collision_narrowphase_detector_SphereSphereDetector extends oimo.collision.narrowphase.detector.Detector {
constructor() {
super(false);
}
detectImpl(result,geom1,geom2,tf1,tf2,cachedData) {
result.incremental = false;
let dX;
let dY;
let dZ;
dX = tf1._positionX - tf2._positionX;
dY = tf1._positionY - tf2._positionY;
dZ = tf1._positionZ - tf2._positionZ;
let r1 = geom1._radius;
let r2 = geom2._radius;
let len2 = dX * dX + dY * dY + dZ * dZ;
if(len2 >= (r1 + r2) * (r1 + r2)) {
return;
}
let len = Math.sqrt(len2);
let nX;
let nY;
let nZ;
if(len > 0) {
nX = dX * (1 / len);
nY = dY * (1 / len);
nZ = dZ * (1 / len);
} else {
nX = 1;
nY = 0;
nZ = 0;
}
this.setNormal(result,nX,nY,nZ);
let pos1X;
let pos1Y;
let pos1Z;
let pos2X;
let pos2Y;
let pos2Z;
pos1X = tf1._positionX + nX * -r1;
pos1Y = tf1._positionY + nY * -r1;
pos1Z = tf1._positionZ + nZ * -r1;
pos2X = tf2._positionX + nX * r2;
pos2Y = tf2._positionY + nY * r2;
pos2Z = tf2._positionZ + nZ * r2;
this.addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,r1 + r2 - len,0);
}
}
if(!oimo.collision.narrowphase.detector.gjkepa) oimo.collision.narrowphase.detector.gjkepa = {};
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedron = class oimo_collision_narrowphase_detector_gjkepa_EpaPolyhedron {
constructor() {
this._vertices = new Array(oimo.common.Setting.maxEPAVertices);
this._center = new oimo.common.Vec3();
this._numVertices = 0;
this._triangleList = null;
this._triangleListLast = null;
this._numTriangles = 0;
this._trianglePool = null;
this._vertexPool = null;
}
dumpHoleEdge(first) {
}
validate() {
let t = this._triangleList;
while(t != null) {
t._vertices[0]._tmpEdgeLoopOuterTriangle = null;
t._vertices[0]._tmpEdgeLoopNext = null;
if(t._adjacentPairIndex[0] == -1) {
this._status = 2;
return false;
}
if(t._adjacentTriangles[0] == null) {
this._status = 3;
return false;
}
t._vertices[1]._tmpEdgeLoopOuterTriangle = null;
t._vertices[1]._tmpEdgeLoopNext = null;
if(t._adjacentPairIndex[1] == -1) {
this._status = 2;
return false;
}
if(t._adjacentTriangles[1] == null) {
this._status = 3;
return false;
}
t._vertices[2]._tmpEdgeLoopOuterTriangle = null;
t._vertices[2]._tmpEdgeLoopNext = null;
if(t._adjacentPairIndex[2] == -1) {
this._status = 2;
return false;
}
if(t._adjacentTriangles[2] == null) {
this._status = 3;
return false;
}
t = t._next;
}
return true;
}
findEdgeLoop(id,base,from) {
if(base._tmpDfsId == id) {
return;
}
base._tmpDfsId = id;
let _this = base.tmp;
_this.x = from.x;
_this.y = from.y;
_this.z = from.z;
let v = base._vertices[0].v;
_this.x -= v.x;
_this.y -= v.y;
_this.z -= v.z;
let _this1 = base.tmp;
let v1 = base._normal;
base._tmpDfsVisible = _this1.x * v1.x + _this1.y * v1.y + _this1.z * v1.z > 0;
if(!base._tmpDfsVisible) {
this._status = 6;
return;
}
let _g = 0;
while(_g < 3) {
let i = _g++;
let t = base._adjacentTriangles[i];
if(t == null) {
continue;
}
let _this = t.tmp;
_this.x = from.x;
_this.y = from.y;
_this.z = from.z;
let v = t._vertices[0].v;
_this.x -= v.x;
_this.y -= v.y;
_this.z -= v.z;
let _this1 = t.tmp;
let v1 = t._normal;
t._tmpDfsVisible = _this1.x * v1.x + _this1.y * v1.y + _this1.z * v1.z > 0;
if(t._tmpDfsVisible) {
this.findEdgeLoop(id,t,from);
} else {
let v1 = base._vertices[i];
v1._tmpEdgeLoopNext = base._vertices[base._nextIndex[i]];
v1._tmpEdgeLoopOuterTriangle = t;
}
}
let triangle = base._adjacentTriangles[0];
if(triangle != null) {
let pairIndex = base._adjacentPairIndex[0];
triangle._adjacentTriangles[pairIndex] = null;
triangle._adjacentPairIndex[pairIndex] = -1;
base._adjacentTriangles[0] = null;
base._adjacentPairIndex[0] = -1;
}
let triangle1 = base._adjacentTriangles[1];
if(triangle1 != null) {
let pairIndex = base._adjacentPairIndex[1];
triangle1._adjacentTriangles[pairIndex] = null;
triangle1._adjacentPairIndex[pairIndex] = -1;
base._adjacentTriangles[1] = null;
base._adjacentPairIndex[1] = -1;
}
let triangle2 = base._adjacentTriangles[2];
if(triangle2 != null) {
let pairIndex = base._adjacentPairIndex[2];
triangle2._adjacentTriangles[pairIndex] = null;
triangle2._adjacentPairIndex[pairIndex] = -1;
base._adjacentTriangles[2] = null;
base._adjacentPairIndex[2] = -1;
}
this._numTriangles--;
let prev = base._prev;
let next = base._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(base == this._triangleList) {
this._triangleList = this._triangleList._next;
}
if(base == this._triangleListLast) {
this._triangleListLast = this._triangleListLast._prev;
}
base._next = null;
base._prev = null;
base.removeReferences();
base._next = this._trianglePool;
this._trianglePool = base;
}
_init(v1,v2,v3,v4) {
this._status = 0;
this._numVertices = 4;
this._vertices[0] = v1;
this._vertices[1] = v2;
this._vertices[2] = v3;
this._vertices[3] = v4;
let _this = this._center;
let v = v1.v;
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let v5 = v2.v;
_this.x += v5.x;
_this.y += v5.y;
_this.z += v5.z;
let v6 = v3.v;
_this.x += v6.x;
_this.y += v6.y;
_this.z += v6.z;
let v7 = v4.v;
_this.x += v7.x;
_this.y += v7.y;
_this.z += v7.z;
_this.x *= 0.25;
_this.y *= 0.25;
_this.z *= 0.25;
let first = this._trianglePool;
if(first != null) {
this._trianglePool = first._next;
first._next = null;
} else {
first = new oimo.collision.narrowphase.detector.gjkepa.EpaTriangle();
}
let t1 = first;
let first1 = this._trianglePool;
if(first1 != null) {
this._trianglePool = first1._next;
first1._next = null;
} else {
first1 = new oimo.collision.narrowphase.detector.gjkepa.EpaTriangle();
}
let t2 = first1;
let first2 = this._trianglePool;
if(first2 != null) {
this._trianglePool = first2._next;
first2._next = null;
} else {
first2 = new oimo.collision.narrowphase.detector.gjkepa.EpaTriangle();
}
let t3 = first2;
let first3 = this._trianglePool;
if(first3 != null) {
this._trianglePool = first3._next;
first3._next = null;
} else {
first3 = new oimo.collision.narrowphase.detector.gjkepa.EpaTriangle();
}
let t4 = first3;
if(!t1.init(v1,v2,v3,this._center,true)) {
this._status = 1;
}
if(!t2.init(v1,v2,v4,this._center,true)) {
this._status = 1;
}
if(!t3.init(v1,v3,v4,this._center,true)) {
this._status = 1;
}
if(!t4.init(v2,v3,v4,this._center,true)) {
this._status = 1;
}
if(!t1.setAdjacentTriangle(t2)) {
this._status = 1;
}
if(!t1.setAdjacentTriangle(t3)) {
this._status = 1;
}
if(!t1.setAdjacentTriangle(t4)) {
this._status = 1;
}
if(!t2.setAdjacentTriangle(t3)) {
this._status = 1;
}
if(!t2.setAdjacentTriangle(t4)) {
this._status = 1;
}
if(!t3.setAdjacentTriangle(t4)) {
this._status = 1;
}
this._numTriangles++;
if(this._triangleList == null) {
this._triangleList = t1;
this._triangleListLast = t1;
} else {
this._triangleListLast._next = t1;
t1._prev = this._triangleListLast;
this._triangleListLast = t1;
}
this._numTriangles++;
if(this._triangleList == null) {
this._triangleList = t2;
this._triangleListLast = t2;
} else {
this._triangleListLast._next = t2;
t2._prev = this._triangleListLast;
this._triangleListLast = t2;
}
this._numTriangles++;
if(this._triangleList == null) {
this._triangleList = t3;
this._triangleListLast = t3;
} else {
this._triangleListLast._next = t3;
t3._prev = this._triangleListLast;
this._triangleListLast = t3;
}
this._numTriangles++;
if(this._triangleList == null) {
this._triangleList = t4;
this._triangleListLast = t4;
} else {
this._triangleListLast._next = t4;
t4._prev = this._triangleListLast;
this._triangleListLast = t4;
}
return this._status == 0;
}
_addVertex(vertex,base) {
this._vertices[this._numVertices++] = vertex;
let v1 = base._vertices[0];
this.findEdgeLoop(this._numVertices,base,vertex.v);
if(this._status != 0) {
return false;
}
let v = v1;
let prevT = null;
let firstT = null;
while(true) {
if(v._tmpEdgeLoopNext == null) {
this._dumpAsObjModel();
this._status = 4;
return false;
}
if(v._tmpEdgeLoopOuterTriangle == null) {
this._status = 5;
return false;
}
let first = this._trianglePool;
if(first != null) {
this._trianglePool = first._next;
first._next = null;
} else {
first = new oimo.collision.narrowphase.detector.gjkepa.EpaTriangle();
}
let t = first;
if(firstT == null) {
firstT = t;
}
if(!t.init(v,v._tmpEdgeLoopNext,vertex,this._center,false)) {
this._status = 1;
}
if(this._status != 0) {
return false;
}
this._numTriangles++;
if(this._triangleList == null) {
this._triangleList = t;
this._triangleListLast = t;
} else {
this._triangleListLast._next = t;
t._prev = this._triangleListLast;
this._triangleListLast = t;
}
if(!t.setAdjacentTriangle(v._tmpEdgeLoopOuterTriangle)) {
this._status = 1;
}
if(prevT != null) {
if(!t.setAdjacentTriangle(prevT)) {
this._status = 1;
}
}
prevT = t;
v = v._tmpEdgeLoopNext;
if(!(v != v1)) {
break;
}
}
if(!prevT.setAdjacentTriangle(firstT)) {
this._status = 1;
}
if(this._status == 0) {
return this.validate();
} else {
return false;
}
}
_dumpAsObjModel() {
}
}
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedronState = class oimo_collision_narrowphase_detector_gjkepa_EpaPolyhedronState {
}
oimo.collision.narrowphase.detector.gjkepa.EpaTriangle = class oimo_collision_narrowphase_detector_gjkepa_EpaTriangle {
constructor() {
this.id = ++oimo.collision.narrowphase.detector.gjkepa.EpaTriangle.count;
this._next = null;
this._prev = null;
this._normal = new oimo.common.Vec3();
this._distanceSq = 0;
this._tmpDfsId = 0;
this._tmpDfsVisible = false;
this._vertices = new Array(3);
this._adjacentTriangles = new Array(3);
this._adjacentPairIndex = new Array(3);
this.tmp = new oimo.common.Vec3();
this._nextIndex = new Array(3);
this._nextIndex[0] = 1;
this._nextIndex[1] = 2;
this._nextIndex[2] = 0;
}
init(vertex1,vertex2,vertex3,center,autoCheck) {
if(autoCheck == null) {
autoCheck = false;
}
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let vcX;
let vcY;
let vcZ;
let v = vertex1.v;
v1X = v.x;
v1Y = v.y;
v1Z = v.z;
let v1 = vertex2.v;
v2X = v1.x;
v2Y = v1.y;
v2Z = v1.z;
let v2 = vertex3.v;
v3X = v2.x;
v3Y = v2.y;
v3Z = v2.z;
vcX = center.x;
vcY = center.y;
vcZ = center.z;
let v12X;
let v12Y;
let v12Z;
let v13X;
let v13Y;
let v13Z;
let vc1X;
let vc1Y;
let vc1Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v13X = v3X - v1X;
v13Y = v3Y - v1Y;
v13Z = v3Z - v1Z;
vc1X = v1X - vcX;
vc1Y = v1Y - vcY;
vc1Z = v1Z - vcZ;
let inorX;
let inorY;
let inorZ;
inorX = v12Y * v13Z - v12Z * v13Y;
inorY = v12Z * v13X - v12X * v13Z;
inorZ = v12X * v13Y - v12Y * v13X;
let inverted = false;
if(vc1X * inorX + vc1Y * inorY + vc1Z * inorZ < 0) {
if(autoCheck) {
let tmp = vertex2;
vertex2 = vertex3;
vertex3 = tmp;
inorX *= -1;
inorY *= -1;
inorZ *= -1;
} else {
inverted = true;
}
}
this._vertices[0] = vertex1;
this._vertices[1] = vertex2;
this._vertices[2] = vertex3;
let v3 = this._normal;
v3.x = inorX;
v3.y = inorY;
v3.z = inorZ;
let vec1 = vertex1.v;
let vec2 = vertex2.v;
let vec3 = vertex3.v;
let out = this.tmp;
let v1X1;
let v1Y1;
let v1Z1;
let v2X1;
let v2Y1;
let v2Z1;
let v3X1;
let v3Y1;
let v3Z1;
let v12X1;
let v12Y1;
let v12Z1;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X1 = vec1.x;
v1Y1 = vec1.y;
v1Z1 = vec1.z;
v2X1 = vec2.x;
v2Y1 = vec2.y;
v2Z1 = vec2.z;
v3X1 = vec3.x;
v3Y1 = vec3.y;
v3Z1 = vec3.z;
v12X1 = v2X1 - v1X1;
v12Y1 = v2Y1 - v1Y1;
v12Z1 = v2Z1 - v1Z1;
v23X = v3X1 - v2X1;
v23Y = v3Y1 - v2Y1;
v23Z = v3Z1 - v2Z1;
v31X = v1X1 - v3X1;
v31Y = v1Y1 - v3Y1;
v31Z = v1Z1 - v3Z1;
let nX;
let nY;
let nZ;
nX = v12Y1 * v23Z - v12Z1 * v23Y;
nY = v12Z1 * v23X - v12X1 * v23Z;
nZ = v12X1 * v23Y - v12Y1 * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y1 * nZ - v12Z1 * nY;
n12Y = v12Z1 * nX - v12X1 * nZ;
n12Z = v12X1 * nY - v12Y1 * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X1 * n12X + v1Y1 * n12Y + v1Z1 * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
}
mind = out.x * out.x + out.y * out.y + out.z * out.z;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
if(v2X1 * n23X + v2Y1 * n23Y + v2Z1 * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind < 0 || d < mind) {
mind = d;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
}
if(v3X1 * n31X + v3Y1 * n31Y + v3Z1 * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind < 0 || d < mind) {
mind = d;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
}
if(mind > 0) {
out.x = minvX;
out.y = minvY;
out.z = minvZ;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X1 * nX + v1Y1 * nY + v1Z1 * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
out.x = minvX;
out.y = minvY;
out.z = minvZ;
}
let _this = this.tmp;
this._distanceSq = _this.x * _this.x + _this.y * _this.y + _this.z * _this.z;
this._adjacentTriangles[0] = null;
this._adjacentTriangles[1] = null;
this._adjacentTriangles[2] = null;
this._adjacentPairIndex[0] = -1;
this._adjacentPairIndex[1] = -1;
this._adjacentPairIndex[2] = -1;
return !inverted;
}
setAdjacentTriangle(triangle) {
let count = 0;
if(this._vertices[0] == triangle._vertices[this._nextIndex[0]] && this._vertices[this._nextIndex[0]] == triangle._vertices[0]) {
this._adjacentTriangles[0] = triangle;
this._adjacentPairIndex[0] = 0;
triangle._adjacentTriangles[0] = this;
triangle._adjacentPairIndex[0] = 0;
count = 1;
}
if(this._vertices[0] == triangle._vertices[this._nextIndex[1]] && this._vertices[this._nextIndex[0]] == triangle._vertices[1]) {
this._adjacentTriangles[0] = triangle;
this._adjacentPairIndex[0] = 1;
triangle._adjacentTriangles[1] = this;
triangle._adjacentPairIndex[1] = 0;
++count;
}
if(this._vertices[0] == triangle._vertices[this._nextIndex[2]] && this._vertices[this._nextIndex[0]] == triangle._vertices[2]) {
this._adjacentTriangles[0] = triangle;
this._adjacentPairIndex[0] = 2;
triangle._adjacentTriangles[2] = this;
triangle._adjacentPairIndex[2] = 0;
++count;
}
if(this._vertices[1] == triangle._vertices[this._nextIndex[0]] && this._vertices[this._nextIndex[1]] == triangle._vertices[0]) {
this._adjacentTriangles[1] = triangle;
this._adjacentPairIndex[1] = 0;
triangle._adjacentTriangles[0] = this;
triangle._adjacentPairIndex[0] = 1;
++count;
}
if(this._vertices[1] == triangle._vertices[this._nextIndex[1]] && this._vertices[this._nextIndex[1]] == triangle._vertices[1]) {
this._adjacentTriangles[1] = triangle;
this._adjacentPairIndex[1] = 1;
triangle._adjacentTriangles[1] = this;
triangle._adjacentPairIndex[1] = 1;
++count;
}
if(this._vertices[1] == triangle._vertices[this._nextIndex[2]] && this._vertices[this._nextIndex[1]] == triangle._vertices[2]) {
this._adjacentTriangles[1] = triangle;
this._adjacentPairIndex[1] = 2;
triangle._adjacentTriangles[2] = this;
triangle._adjacentPairIndex[2] = 1;
++count;
}
if(this._vertices[2] == triangle._vertices[this._nextIndex[0]] && this._vertices[this._nextIndex[2]] == triangle._vertices[0]) {
this._adjacentTriangles[2] = triangle;
this._adjacentPairIndex[2] = 0;
triangle._adjacentTriangles[0] = this;
triangle._adjacentPairIndex[0] = 2;
++count;
}
if(this._vertices[2] == triangle._vertices[this._nextIndex[1]] && this._vertices[this._nextIndex[2]] == triangle._vertices[1]) {
this._adjacentTriangles[2] = triangle;
this._adjacentPairIndex[2] = 1;
triangle._adjacentTriangles[1] = this;
triangle._adjacentPairIndex[1] = 2;
++count;
}
if(this._vertices[2] == triangle._vertices[this._nextIndex[2]] && this._vertices[this._nextIndex[2]] == triangle._vertices[2]) {
this._adjacentTriangles[2] = triangle;
this._adjacentPairIndex[2] = 2;
triangle._adjacentTriangles[2] = this;
triangle._adjacentPairIndex[2] = 2;
++count;
}
if(count != 1) {
return false;
}
return true;
}
removeAdjacentTriangles() {
let triangle = this._adjacentTriangles[0];
if(triangle != null) {
let pairIndex = this._adjacentPairIndex[0];
triangle._adjacentTriangles[pairIndex] = null;
triangle._adjacentPairIndex[pairIndex] = -1;
this._adjacentTriangles[0] = null;
this._adjacentPairIndex[0] = -1;
}
let triangle1 = this._adjacentTriangles[1];
if(triangle1 != null) {
let pairIndex = this._adjacentPairIndex[1];
triangle1._adjacentTriangles[pairIndex] = null;
triangle1._adjacentPairIndex[pairIndex] = -1;
this._adjacentTriangles[1] = null;
this._adjacentPairIndex[1] = -1;
}
let triangle2 = this._adjacentTriangles[2];
if(triangle2 != null) {
let pairIndex = this._adjacentPairIndex[2];
triangle2._adjacentTriangles[pairIndex] = null;
triangle2._adjacentPairIndex[pairIndex] = -1;
this._adjacentTriangles[2] = null;
this._adjacentPairIndex[2] = -1;
}
}
removeReferences() {
this._next = null;
this._prev = null;
this._tmpDfsId = 0;
this._tmpDfsVisible = false;
this._distanceSq = 0;
this._vertices[0] = null;
this._vertices[1] = null;
this._vertices[2] = null;
this._adjacentTriangles[0] = null;
this._adjacentTriangles[1] = null;
this._adjacentTriangles[2] = null;
this._adjacentPairIndex[0] = 0;
this._adjacentPairIndex[1] = 0;
this._adjacentPairIndex[2] = 0;
}
dump() {
}
}
oimo.collision.narrowphase.detector.gjkepa.EpaVertex = class oimo_collision_narrowphase_detector_gjkepa_EpaVertex {
constructor() {
this.randId = Math.random() * 100000 | 0;
this.v = new oimo.common.Vec3();
this.w1 = new oimo.common.Vec3();
this.w2 = new oimo.common.Vec3();
}
init(v,w1,w2) {
let _this = this.v;
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let _this1 = this.w1;
_this1.x = w1.x;
_this1.y = w1.y;
_this1.z = w1.z;
let _this2 = this.w2;
_this2.x = w2.x;
_this2.y = w2.y;
_this2.z = w2.z;
this._next = null;
this._tmpEdgeLoopNext = null;
this._tmpEdgeLoopOuterTriangle = null;
return this;
}
removeReferences() {
this._next = null;
this._tmpEdgeLoopNext = null;
this._tmpEdgeLoopOuterTriangle = null;
}
}
oimo.collision.narrowphase.detector.gjkepa.GjkCache = class oimo_collision_narrowphase_detector_gjkepa_GjkCache {
constructor() {
this.prevClosestDir = new oimo.common.Vec3();
}
clear() {
this.prevClosestDir.zero();
}
}
if(!oimo.common) oimo.common = {};
oimo.common.Vec3 = class oimo_common_Vec3 {
constructor(x,y,z) {
if(z == null) {
z = 0;
}
if(y == null) {
y = 0;
}
if(x == null) {
x = 0;
}
this.x = x;
this.y = y;
this.z = z;
oimo.common.Vec3.numCreations++;
}
init(x,y,z) {
this.x = x;
this.y = y;
this.z = z;
return this;
}
zero() {
this.x = 0;
this.y = 0;
this.z = 0;
return this;
}
add(v) {
return new oimo.common.Vec3(this.x + v.x,this.y + v.y,this.z + v.z);
}
add3(vx,vy,vz) {
return new oimo.common.Vec3(this.x + vx,this.y + vy,this.z + vz);
}
addScaled(v,s) {
return new oimo.common.Vec3(this.x + v.x * s,this.y + v.y * s,this.z + v.z * s);
}
sub(v) {
return new oimo.common.Vec3(this.x - v.x,this.y - v.y,this.z - v.z);
}
sub3(vx,vy,vz) {
return new oimo.common.Vec3(this.x - vx,this.y - vy,this.z - vz);
}
scale(s) {
return new oimo.common.Vec3(this.x * s,this.y * s,this.z * s);
}
scale3(sx,sy,sz) {
return new oimo.common.Vec3(this.x * sx,this.y * sy,this.z * sz);
}
dot(v) {
return this.x * v.x + this.y * v.y + this.z * v.z;
}
cross(v) {
return new oimo.common.Vec3(this.y * v.z - this.z * v.y,this.z * v.x - this.x * v.z,this.x * v.y - this.y * v.x);
}
addEq(v) {
this.x += v.x;
this.y += v.y;
this.z += v.z;
return this;
}
add3Eq(vx,vy,vz) {
this.x += vx;
this.y += vy;
this.z += vz;
return this;
}
addScaledEq(v,s) {
this.x += v.x * s;
this.y += v.y * s;
this.z += v.z * s;
return this;
}
subEq(v) {
this.x -= v.x;
this.y -= v.y;
this.z -= v.z;
return this;
}
sub3Eq(vx,vy,vz) {
this.x -= vx;
this.y -= vy;
this.z -= vz;
return this;
}
scaleEq(s) {
this.x *= s;
this.y *= s;
this.z *= s;
return this;
}
scale3Eq(sx,sy,sz) {
this.x *= sx;
this.y *= sy;
this.z *= sz;
return this;
}
crossEq(v) {
let y = this.z * v.x - this.x * v.z;
let z = this.x * v.y - this.y * v.x;
this.x = this.y * v.z - this.z * v.y;
this.y = y;
this.z = z;
return this;
}
mulMat3(m) {
return new oimo.common.Vec3(this.x * m.e00 + this.y * m.e01 + this.z * m.e02,this.x * m.e10 + this.y * m.e11 + this.z * m.e12,this.x * m.e20 + this.y * m.e21 + this.z * m.e22);
}
mulMat4(m) {
return new oimo.common.Vec3(this.x * m.e00 + this.y * m.e01 + this.z * m.e02 + m.e03,this.x * m.e10 + this.y * m.e11 + this.z * m.e12 + m.e13,this.x * m.e20 + this.y * m.e21 + this.z * m.e22 + m.e23);
}
mulTransform(tf) {
let vX;
let vY;
let vZ;
vX = this.x;
vY = this.y;
vZ = this.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf._rotation00 * vX + tf._rotation01 * vY + tf._rotation02 * vZ;
__tmp__Y = tf._rotation10 * vX + tf._rotation11 * vY + tf._rotation12 * vZ;
__tmp__Z = tf._rotation20 * vX + tf._rotation21 * vY + tf._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
vX += tf._positionX;
vY += tf._positionY;
vZ += tf._positionZ;
let res = new oimo.common.Vec3();
res.x = vX;
res.y = vY;
res.z = vZ;
return res;
}
mulMat3Eq(m) {
let y = this.x * m.e10 + this.y * m.e11 + this.z * m.e12;
let z = this.x * m.e20 + this.y * m.e21 + this.z * m.e22;
this.x = this.x * m.e00 + this.y * m.e01 + this.z * m.e02;
this.y = y;
this.z = z;
return this;
}
mulMat4Eq(m) {
let y = this.x * m.e10 + this.y * m.e11 + this.z * m.e12 + m.e13;
let z = this.x * m.e20 + this.y * m.e21 + this.z * m.e22 + m.e23;
this.x = this.x * m.e00 + this.y * m.e01 + this.z * m.e02 + m.e03;
this.y = y;
this.z = z;
return this;
}
mulTransformEq(tf) {
let vX;
let vY;
let vZ;
vX = this.x;
vY = this.y;
vZ = this.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf._rotation00 * vX + tf._rotation01 * vY + tf._rotation02 * vZ;
__tmp__Y = tf._rotation10 * vX + tf._rotation11 * vY + tf._rotation12 * vZ;
__tmp__Z = tf._rotation20 * vX + tf._rotation21 * vY + tf._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
vX += tf._positionX;
vY += tf._positionY;
vZ += tf._positionZ;
this.x = vX;
this.y = vY;
this.z = vZ;
return this;
}
length() {
return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
}
lengthSq() {
return this.x * this.x + this.y * this.y + this.z * this.z;
}
normalized() {
let invLen = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
return new oimo.common.Vec3(this.x * invLen,this.y * invLen,this.z * invLen);
}
normalize() {
let invLen = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
this.x *= invLen;
this.y *= invLen;
this.z *= invLen;
return this;
}
negate() {
return new oimo.common.Vec3(-this.x,-this.y,-this.z);
}
negateEq() {
this.x = -this.x;
this.y = -this.y;
this.z = -this.z;
return this;
}
copyFrom(v) {
this.x = v.x;
this.y = v.y;
this.z = v.z;
return this;
}
clone() {
return new oimo.common.Vec3(this.x,this.y,this.z);
}
toString() {
return "Vec3[" + (this.x > 0 ? (this.x * 10000000 + 0.5 | 0) / 10000000 : (this.x * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.y > 0 ? (this.y * 10000000 + 0.5 | 0) / 10000000 : (this.y * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.z > 0 ? (this.z * 10000000 + 0.5 | 0) / 10000000 : (this.z * 10000000 - 0.5 | 0) / 10000000) + "]";
}
}
oimo.common.Transform = class oimo_common_Transform {
constructor() {
this._positionX = 0;
this._positionY = 0;
this._positionZ = 0;
this._rotation00 = 1;
this._rotation01 = 0;
this._rotation02 = 0;
this._rotation10 = 0;
this._rotation11 = 1;
this._rotation12 = 0;
this._rotation20 = 0;
this._rotation21 = 0;
this._rotation22 = 1;
}
identity() {
this._positionX = 0;
this._positionY = 0;
this._positionZ = 0;
this._rotation00 = 1;
this._rotation01 = 0;
this._rotation02 = 0;
this._rotation10 = 0;
this._rotation11 = 1;
this._rotation12 = 0;
this._rotation20 = 0;
this._rotation21 = 0;
this._rotation22 = 1;
return this;
}
getPosition() {
let position = new oimo.common.Vec3();
position.x = this._positionX;
position.y = this._positionY;
position.z = this._positionZ;
return position;
}
getPositionTo(position) {
position.x = this._positionX;
position.y = this._positionY;
position.z = this._positionZ;
}
setPosition(position) {
this._positionX = position.x;
this._positionY = position.y;
this._positionZ = position.z;
return this;
}
translate(translation) {
let diffX;
let diffY;
let diffZ;
diffX = translation.x;
diffY = translation.y;
diffZ = translation.z;
this._positionX += diffX;
this._positionY += diffY;
this._positionZ += diffZ;
}
getRotation() {
let rotation = new oimo.common.Mat3();
rotation.e00 = this._rotation00;
rotation.e01 = this._rotation01;
rotation.e02 = this._rotation02;
rotation.e10 = this._rotation10;
rotation.e11 = this._rotation11;
rotation.e12 = this._rotation12;
rotation.e20 = this._rotation20;
rotation.e21 = this._rotation21;
rotation.e22 = this._rotation22;
return rotation;
}
getRotationTo(out) {
out.e00 = this._rotation00;
out.e01 = this._rotation01;
out.e02 = this._rotation02;
out.e10 = this._rotation10;
out.e11 = this._rotation11;
out.e12 = this._rotation12;
out.e20 = this._rotation20;
out.e21 = this._rotation21;
out.e22 = this._rotation22;
}
setRotation(rotation) {
this._rotation00 = rotation.e00;
this._rotation01 = rotation.e01;
this._rotation02 = rotation.e02;
this._rotation10 = rotation.e10;
this._rotation11 = rotation.e11;
this._rotation12 = rotation.e12;
this._rotation20 = rotation.e20;
this._rotation21 = rotation.e21;
this._rotation22 = rotation.e22;
return this;
}
setRotationXyz(eulerAngles) {
let xyzX;
let xyzY;
let xyzZ;
xyzX = eulerAngles.x;
xyzY = eulerAngles.y;
xyzZ = eulerAngles.z;
let sx = Math.sin(xyzX);
let sy = Math.sin(xyzY);
let sz = Math.sin(xyzZ);
let cx = Math.cos(xyzX);
let cy = Math.cos(xyzY);
let cz = Math.cos(xyzZ);
this._rotation00 = cy * cz;
this._rotation01 = -cy * sz;
this._rotation02 = sy;
this._rotation10 = cx * sz + cz * sx * sy;
this._rotation11 = cx * cz - sx * sy * sz;
this._rotation12 = -cy * sx;
this._rotation20 = sx * sz - cx * cz * sy;
this._rotation21 = cz * sx + cx * sy * sz;
this._rotation22 = cx * cy;
}
rotate(rotation) {
let rot00;
let rot01;
let rot02;
let rot10;
let rot11;
let rot12;
let rot20;
let rot21;
let rot22;
rot00 = rotation.e00;
rot01 = rotation.e01;
rot02 = rotation.e02;
rot10 = rotation.e10;
rot11 = rotation.e11;
rot12 = rotation.e12;
rot20 = rotation.e20;
rot21 = rotation.e21;
rot22 = rotation.e22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = rot00 * this._rotation00 + rot01 * this._rotation10 + rot02 * this._rotation20;
__tmp__01 = rot00 * this._rotation01 + rot01 * this._rotation11 + rot02 * this._rotation21;
__tmp__02 = rot00 * this._rotation02 + rot01 * this._rotation12 + rot02 * this._rotation22;
__tmp__10 = rot10 * this._rotation00 + rot11 * this._rotation10 + rot12 * this._rotation20;
__tmp__11 = rot10 * this._rotation01 + rot11 * this._rotation11 + rot12 * this._rotation21;
__tmp__12 = rot10 * this._rotation02 + rot11 * this._rotation12 + rot12 * this._rotation22;
__tmp__20 = rot20 * this._rotation00 + rot21 * this._rotation10 + rot22 * this._rotation20;
__tmp__21 = rot20 * this._rotation01 + rot21 * this._rotation11 + rot22 * this._rotation21;
__tmp__22 = rot20 * this._rotation02 + rot21 * this._rotation12 + rot22 * this._rotation22;
this._rotation00 = __tmp__00;
this._rotation01 = __tmp__01;
this._rotation02 = __tmp__02;
this._rotation10 = __tmp__10;
this._rotation11 = __tmp__11;
this._rotation12 = __tmp__12;
this._rotation20 = __tmp__20;
this._rotation21 = __tmp__21;
this._rotation22 = __tmp__22;
}
rotateXyz(eulerAngles) {
let xyzX;
let xyzY;
let xyzZ;
let rot00;
let rot01;
let rot02;
let rot10;
let rot11;
let rot12;
let rot20;
let rot21;
let rot22;
xyzX = eulerAngles.x;
xyzY = eulerAngles.y;
xyzZ = eulerAngles.z;
let sx = Math.sin(xyzX);
let sy = Math.sin(xyzY);
let sz = Math.sin(xyzZ);
let cx = Math.cos(xyzX);
let cy = Math.cos(xyzY);
let cz = Math.cos(xyzZ);
rot00 = cy * cz;
rot01 = -cy * sz;
rot02 = sy;
rot10 = cx * sz + cz * sx * sy;
rot11 = cx * cz - sx * sy * sz;
rot12 = -cy * sx;
rot20 = sx * sz - cx * cz * sy;
rot21 = cz * sx + cx * sy * sz;
rot22 = cx * cy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = rot00 * this._rotation00 + rot01 * this._rotation10 + rot02 * this._rotation20;
__tmp__01 = rot00 * this._rotation01 + rot01 * this._rotation11 + rot02 * this._rotation21;
__tmp__02 = rot00 * this._rotation02 + rot01 * this._rotation12 + rot02 * this._rotation22;
__tmp__10 = rot10 * this._rotation00 + rot11 * this._rotation10 + rot12 * this._rotation20;
__tmp__11 = rot10 * this._rotation01 + rot11 * this._rotation11 + rot12 * this._rotation21;
__tmp__12 = rot10 * this._rotation02 + rot11 * this._rotation12 + rot12 * this._rotation22;
__tmp__20 = rot20 * this._rotation00 + rot21 * this._rotation10 + rot22 * this._rotation20;
__tmp__21 = rot20 * this._rotation01 + rot21 * this._rotation11 + rot22 * this._rotation21;
__tmp__22 = rot20 * this._rotation02 + rot21 * this._rotation12 + rot22 * this._rotation22;
this._rotation00 = __tmp__00;
this._rotation01 = __tmp__01;
this._rotation02 = __tmp__02;
this._rotation10 = __tmp__10;
this._rotation11 = __tmp__11;
this._rotation12 = __tmp__12;
this._rotation20 = __tmp__20;
this._rotation21 = __tmp__21;
this._rotation22 = __tmp__22;
}
getOrientation() {
let q = new oimo.common.Quat();
let iqX;
let iqY;
let iqZ;
let iqW;
let e00 = this._rotation00;
let e11 = this._rotation11;
let e22 = this._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
iqW = 0.5 * s;
s = 0.5 / s;
iqX = (this._rotation21 - this._rotation12) * s;
iqY = (this._rotation02 - this._rotation20) * s;
iqZ = (this._rotation10 - this._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
iqX = 0.5 * s;
s = 0.5 / s;
iqY = (this._rotation01 + this._rotation10) * s;
iqZ = (this._rotation02 + this._rotation20) * s;
iqW = (this._rotation21 - this._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
iqZ = 0.5 * s;
s = 0.5 / s;
iqX = (this._rotation02 + this._rotation20) * s;
iqY = (this._rotation12 + this._rotation21) * s;
iqW = (this._rotation10 - this._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
iqY = 0.5 * s;
s = 0.5 / s;
iqX = (this._rotation01 + this._rotation10) * s;
iqZ = (this._rotation12 + this._rotation21) * s;
iqW = (this._rotation02 - this._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
iqZ = 0.5 * s;
s = 0.5 / s;
iqX = (this._rotation02 + this._rotation20) * s;
iqY = (this._rotation12 + this._rotation21) * s;
iqW = (this._rotation10 - this._rotation01) * s;
}
q.x = iqX;
q.y = iqY;
q.z = iqZ;
q.w = iqW;
return q;
}
getOrientationTo(orientation) {
let iqX;
let iqY;
let iqZ;
let iqW;
let e00 = this._rotation00;
let e11 = this._rotation11;
let e22 = this._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
iqW = 0.5 * s;
s = 0.5 / s;
iqX = (this._rotation21 - this._rotation12) * s;
iqY = (this._rotation02 - this._rotation20) * s;
iqZ = (this._rotation10 - this._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
iqX = 0.5 * s;
s = 0.5 / s;
iqY = (this._rotation01 + this._rotation10) * s;
iqZ = (this._rotation02 + this._rotation20) * s;
iqW = (this._rotation21 - this._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
iqZ = 0.5 * s;
s = 0.5 / s;
iqX = (this._rotation02 + this._rotation20) * s;
iqY = (this._rotation12 + this._rotation21) * s;
iqW = (this._rotation10 - this._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
iqY = 0.5 * s;
s = 0.5 / s;
iqX = (this._rotation01 + this._rotation10) * s;
iqZ = (this._rotation12 + this._rotation21) * s;
iqW = (this._rotation02 - this._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
iqZ = 0.5 * s;
s = 0.5 / s;
iqX = (this._rotation02 + this._rotation20) * s;
iqY = (this._rotation12 + this._rotation21) * s;
iqW = (this._rotation10 - this._rotation01) * s;
}
orientation.x = iqX;
orientation.y = iqY;
orientation.z = iqZ;
orientation.w = iqW;
}
setOrientation(quaternion) {
let qX;
let qY;
let qZ;
let qW;
qX = quaternion.x;
qY = quaternion.y;
qZ = quaternion.z;
qW = quaternion.w;
let x = qX;
let y = qY;
let z = qZ;
let w = qW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
this._rotation00 = 1 - yy - zz;
this._rotation01 = xy - wz;
this._rotation02 = xz + wy;
this._rotation10 = xy + wz;
this._rotation11 = 1 - xx - zz;
this._rotation12 = yz - wx;
this._rotation20 = xz - wy;
this._rotation21 = yz + wx;
this._rotation22 = 1 - xx - yy;
return this;
}
clone() {
let tf = new oimo.common.Transform();
tf._positionX = this._positionX;
tf._positionY = this._positionY;
tf._positionZ = this._positionZ;
tf._rotation00 = this._rotation00;
tf._rotation01 = this._rotation01;
tf._rotation02 = this._rotation02;
tf._rotation10 = this._rotation10;
tf._rotation11 = this._rotation11;
tf._rotation12 = this._rotation12;
tf._rotation20 = this._rotation20;
tf._rotation21 = this._rotation21;
tf._rotation22 = this._rotation22;
return tf;
}
copyFrom(transform) {
this._positionX = transform._positionX;
this._positionY = transform._positionY;
this._positionZ = transform._positionZ;
this._rotation00 = transform._rotation00;
this._rotation01 = transform._rotation01;
this._rotation02 = transform._rotation02;
this._rotation10 = transform._rotation10;
this._rotation11 = transform._rotation11;
this._rotation12 = transform._rotation12;
this._rotation20 = transform._rotation20;
this._rotation21 = transform._rotation21;
this._rotation22 = transform._rotation22;
return this;
}
}
oimo.common.Setting = class oimo_common_Setting {
}
oimo.collision.narrowphase.detector.gjkepa.GjkEpa = class oimo_collision_narrowphase_detector_gjkepa_GjkEpa {
constructor() {
this.s = new Array(4);
this.w1 = new Array(4);
this.w2 = new Array(4);
this.baseDirs = new Array(3);
this.baseDirs[0] = new oimo.common.Vec3(1,0,0);
this.baseDirs[1] = new oimo.common.Vec3(0,1,0);
this.baseDirs[2] = new oimo.common.Vec3(0,0,1);
this.tl1 = new oimo.common.Vec3();
this.tl2 = new oimo.common.Vec3();
this.rayX = new oimo.common.Vec3();
this.rayR = new oimo.common.Vec3();
this.tempTransform = new oimo.common.Transform();
this.s[0] = new oimo.common.Vec3();
this.w1[0] = new oimo.common.Vec3();
this.w2[0] = new oimo.common.Vec3();
this.s[1] = new oimo.common.Vec3();
this.w1[1] = new oimo.common.Vec3();
this.w2[1] = new oimo.common.Vec3();
this.s[2] = new oimo.common.Vec3();
this.w1[2] = new oimo.common.Vec3();
this.w2[2] = new oimo.common.Vec3();
this.s[3] = new oimo.common.Vec3();
this.w1[3] = new oimo.common.Vec3();
this.w2[3] = new oimo.common.Vec3();
this.dir = new oimo.common.Vec3();
this.closest = new oimo.common.Vec3();
this.closestPoint1 = new oimo.common.Vec3();
this.closestPoint2 = new oimo.common.Vec3();
this.polyhedron = new oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedron();
}
computeClosestPointsImpl(c1,c2,tf1,tf2,cache,useEpa) {
this.c1 = c1;
this.c2 = c2;
this.tf1 = tf1;
this.tf2 = tf2;
let s = this.s;
let w1 = this.w1;
let w2 = this.w2;
let closest = this.closest;
let dir = this.dir;
if(cache != null) {
if(cache._gjkCache == null) {
cache._gjkCache = new oimo.collision.narrowphase.detector.gjkepa.GjkCache();
}
this.loadCache(cache._gjkCache);
} else {
dir.zero();
}
if(dir.x * dir.x + dir.y * dir.y + dir.z * dir.z == 0) {
let firstDirX;
let firstDirY;
let firstDirZ;
firstDirX = tf2._positionX - tf1._positionX;
firstDirY = tf2._positionY - tf1._positionY;
firstDirZ = tf2._positionZ - tf1._positionZ;
dir.x = firstDirX;
dir.y = firstDirY;
dir.z = firstDirZ;
if(dir.x * dir.x + dir.y * dir.y + dir.z * dir.z < 1e-6) {
dir.init(1,0,0);
}
}
this.simplexSize = 0;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this = this.s[this.simplexSize];
let v = this.w1[this.simplexSize];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let v1 = this.w2[this.simplexSize];
_this.x -= v1.x;
_this.y -= v1.y;
_this.z -= v1.z;
this.simplexSize = 1;
let count = 0;
while(count < 40) {
let v = 0;
switch(this.simplexSize) {
case 1:
let v1 = s[0];
closest.x = v1.x;
closest.y = v1.y;
closest.z = v1.z;
v = 1;
break;
case 2:
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v2 = s[0];
v1X = v2.x;
v1Y = v2.y;
v1Z = v2.z;
let v3 = s[1];
v2X = v3.x;
v2Y = v3.y;
v2Z = v3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
v = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
v = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
v = 3;
}
break;
case 3:
let vec1 = s[0];
let vec2 = s[1];
let vec3 = s[2];
let v1X1;
let v1Y1;
let v1Z1;
let v2X1;
let v2Y1;
let v2Z1;
let v3X;
let v3Y;
let v3Z;
let v12X1;
let v12Y1;
let v12Z1;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X1 = vec1.x;
v1Y1 = vec1.y;
v1Z1 = vec1.z;
v2X1 = vec2.x;
v2Y1 = vec2.y;
v2Z1 = vec2.z;
v3X = vec3.x;
v3Y = vec3.y;
v3Z = vec3.z;
v12X1 = v2X1 - v1X1;
v12Y1 = v2Y1 - v1Y1;
v12Z1 = v2Z1 - v1Z1;
v23X = v3X - v2X1;
v23Y = v3Y - v2Y1;
v23Z = v3Z - v2Z1;
v31X = v1X1 - v3X;
v31Y = v1Y1 - v3Y;
v31Z = v1Z1 - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y1 * v23Z - v12Z1 * v23Y;
nY = v12Z1 * v23X - v12X1 * v23Z;
nZ = v12X1 * v23Y - v12Y1 * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y1 * nZ - v12Z1 * nY;
n12Y = v12Z1 * nX - v12X1 * nZ;
n12Z = v12X1 * nY - v12Y1 * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X1 * n12X + v1Y1 * n12Y + v1Z1 * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X1 * n23X + v2Y1 * n23Y + v2Z1 * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
v = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X1 * nX + v1Y1 * nY + v1Z1 * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
v = 7;
}
break;
case 4:
let vec11 = s[0];
let vec21 = s[1];
let vec31 = s[2];
let vec4 = s[3];
let v1X2;
let v1Y2;
let v1Z2;
let v2X2;
let v2Y2;
let v2Z2;
let v3X1;
let v3Y1;
let v3Z1;
let v4X;
let v4Y;
let v4Z;
let v12X2;
let v12Y2;
let v12Z2;
let v13X;
let v13Y;
let v13Z;
let v14X;
let v14Y;
let v14Z;
let v23X1;
let v23Y1;
let v23Z1;
let v24X;
let v24Y;
let v24Z;
v1X2 = vec11.x;
v1Y2 = vec11.y;
v1Z2 = vec11.z;
v2X2 = vec21.x;
v2Y2 = vec21.y;
v2Z2 = vec21.z;
v3X1 = vec31.x;
v3Y1 = vec31.y;
v3Z1 = vec31.z;
v4X = vec4.x;
v4Y = vec4.y;
v4Z = vec4.z;
v12X2 = v2X2 - v1X2;
v12Y2 = v2Y2 - v1Y2;
v12Z2 = v2Z2 - v1Z2;
v13X = v3X1 - v1X2;
v13Y = v3Y1 - v1Y2;
v13Z = v3Z1 - v1Z2;
v14X = v4X - v1X2;
v14Y = v4Y - v1Y2;
v14Z = v4Z - v1Z2;
v23X1 = v3X1 - v2X2;
v23Y1 = v3Y1 - v2Y2;
v23Z1 = v3Z1 - v2Z2;
v24X = v4X - v2X2;
v24Y = v4Y - v2Y2;
v24Z = v4Z - v2Z2;
let n123X;
let n123Y;
let n123Z;
let n134X;
let n134Y;
let n134Z;
let n142X;
let n142Y;
let n142Z;
let n243X;
let n243Y;
let n243Z;
n123X = v12Y2 * v13Z - v12Z2 * v13Y;
n123Y = v12Z2 * v13X - v12X2 * v13Z;
n123Z = v12X2 * v13Y - v12Y2 * v13X;
n134X = v13Y * v14Z - v13Z * v14Y;
n134Y = v13Z * v14X - v13X * v14Z;
n134Z = v13X * v14Y - v13Y * v14X;
n142X = v14Y * v12Z2 - v14Z * v12Y2;
n142Y = v14Z * v12X2 - v14X * v12Z2;
n142Z = v14X * v12Y2 - v14Y * v12X2;
n243X = v24Y * v23Z1 - v24Z * v23Y1;
n243Y = v24Z * v23X1 - v24X * v23Z1;
n243Z = v24X * v23Y1 - v24Y * v23X1;
let sign = v12X2 * n243X + v12Y2 * n243Y + v12Z2 * n243Z > 0 ? 1 : -1;
let mind1 = -1;
let minvX1;
let minvY1;
let minvZ1;
let mini1 = 0;
minvX1 = 0;
minvY1 = 0;
minvZ1 = 0;
if((v1X2 * n123X + v1Y2 * n123Y + v1Z2 * n123Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec21.x;
v2Y = vec21.y;
v2Z = vec21.z;
v3X = vec31.x;
v3Y = vec31.y;
v3Z = vec31.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec21.x;
v2Y = vec21.y;
v2Z = vec21.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
let b;
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = 7;
}
mini1 = b;
mind1 = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX1 = closest.x;
minvY1 = closest.y;
minvZ1 = closest.z;
}
if((v1X2 * n134X + v1Y2 * n134Y + v1Z2 * n134Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec31.x;
v1Y = vec31.y;
v1Z = vec31.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
let b;
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = 7;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind1 < 0 || d < mind1) {
mini1 = b & 1 | (b & 6) << 1;
mind1 = d;
minvX1 = closest.x;
minvY1 = closest.y;
minvZ1 = closest.z;
}
}
if((v1X2 * n142X + v1Y2 * n142Y + v1Z2 * n142Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec21.x;
v2Y = vec21.y;
v2Z = vec21.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec21.x;
v2Y = vec21.y;
v2Z = vec21.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
let b;
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = 7;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind1 < 0 || d < mind1) {
mini1 = b & 3 | (b & 4) << 1;
mind1 = d;
minvX1 = closest.x;
minvY1 = closest.y;
minvZ1 = closest.z;
}
}
if((v2X2 * n243X + v2Y2 * n243Y + v2Z2 * n243Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec31.x;
v1Y = vec31.y;
v1Z = vec31.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
let b;
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = 7;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind1 < 0 || d < mind1) {
mini1 = b << 1;
mind1 = d;
minvX1 = closest.x;
minvY1 = closest.y;
minvZ1 = closest.z;
}
}
if(mind1 > 0) {
closest.x = minvX1;
closest.y = minvY1;
closest.z = minvZ1;
v = mini1;
} else {
closest.zero();
v = 15;
}
break;
}
if(closest.x * closest.x + closest.y * closest.y + closest.z * closest.z < 1e-008) {
if(!useEpa) {
this.distance = 0;
return 0;
}
switch(this.simplexSize) {
case 1:
this.pointToTetrahedron();
break;
case 2:
this.lineToTetrahedron();
break;
case 3:
this.triangleToTetrahedron();
break;
}
if(this.simplexSize == 4) {
let epaState = this.computeDepth(c1,c2,tf1,tf2,s,w1,w2);
if(epaState != 0) {
this.distance = 0;
return epaState;
}
this.distance = -this.depth;
return 0;
}
this.distance = 0;
return 1;
}
this.shrinkSimplex(v);
dir.x = closest.x;
dir.y = closest.y;
dir.z = closest.z;
dir.x = -dir.x;
dir.y = -dir.y;
dir.z = -dir.z;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this = this.s[this.simplexSize];
let v4 = this.w1[this.simplexSize];
_this.x = v4.x;
_this.y = v4.y;
_this.z = v4.z;
let v5 = this.w2[this.simplexSize];
_this.x -= v5.x;
_this.y -= v5.y;
_this.z -= v5.z;
if(dir.x * dir.x + dir.y * dir.y + dir.z * dir.z < 1e-008) {
throw new Error("!?");
}
let _this1 = s[this.simplexSize];
if(_this1.x * dir.x + _this1.y * dir.y + _this1.z * dir.z - (closest.x * dir.x + closest.y * dir.y + closest.z * dir.z) < 1e-008) {
this.interpolateClosestPoints();
this.distance = Math.sqrt(closest.x * closest.x + closest.y * closest.y + closest.z * closest.z);
if(cache != null && cache._gjkCache != null) {
this.saveCache(cache._gjkCache);
}
return 0;
}
this.simplexSize++;
++count;
}
return 2;
}
convexCastImpl(c1,c2,tf1,tf2,tl1,tl2,hit) {
this.c1 = c1;
this.c2 = c2;
this.tf1 = tf1;
this.tf2 = tf2;
let s = this.s;
let closest = this.closest;
let dir = this.dir;
let firstDirX;
let firstDirY;
let firstDirZ;
firstDirX = tf2._positionX - tf1._positionX;
firstDirY = tf2._positionY - tf1._positionY;
firstDirZ = tf2._positionZ - tf1._positionZ;
dir.x = firstDirX;
dir.y = firstDirY;
dir.z = firstDirZ;
if(dir.x * dir.x + dir.y * dir.y + dir.z * dir.z < 1e-6) {
dir.init(1,0,0);
}
this.simplexSize = 0;
if(this.c1 != null) {
this.computeWitnessPoint1(true);
} else {
let v = this.w1[this.simplexSize];
v.x = this.tf1._positionX;
v.y = this.tf1._positionY;
v.z = this.tf1._positionZ;
}
this.computeWitnessPoint2(true);
let _this = this.s[this.simplexSize];
let v = this.w1[this.simplexSize];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let v1 = this.w2[this.simplexSize];
_this.x -= v1.x;
_this.y -= v1.y;
_this.z -= v1.z;
this.simplexSize = 1;
let count = 0;
let lambda = 0.0;
let rayX = this.rayX;
let rayR = this.rayR;
rayX.zero();
rayR.x = tl2.x;
rayR.y = tl2.y;
rayR.z = tl2.z;
rayR.x -= tl1.x;
rayR.y -= tl1.y;
rayR.z -= tl1.z;
while(count < 40) {
let v = 0;
switch(this.simplexSize) {
case 1:
let v1 = s[0];
closest.x = v1.x;
closest.y = v1.y;
closest.z = v1.z;
v = 1;
break;
case 2:
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v2 = s[0];
v1X = v2.x;
v1Y = v2.y;
v1Z = v2.z;
let v3 = s[1];
v2X = v3.x;
v2Y = v3.y;
v2Z = v3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
v = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
v = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
v = 3;
}
break;
case 3:
let vec1 = s[0];
let vec2 = s[1];
let vec3 = s[2];
let v1X1;
let v1Y1;
let v1Z1;
let v2X1;
let v2Y1;
let v2Z1;
let v3X;
let v3Y;
let v3Z;
let v12X1;
let v12Y1;
let v12Z1;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X1 = vec1.x;
v1Y1 = vec1.y;
v1Z1 = vec1.z;
v2X1 = vec2.x;
v2Y1 = vec2.y;
v2Z1 = vec2.z;
v3X = vec3.x;
v3Y = vec3.y;
v3Z = vec3.z;
v12X1 = v2X1 - v1X1;
v12Y1 = v2Y1 - v1Y1;
v12Z1 = v2Z1 - v1Z1;
v23X = v3X - v2X1;
v23Y = v3Y - v2Y1;
v23Z = v3Z - v2Z1;
v31X = v1X1 - v3X;
v31Y = v1Y1 - v3Y;
v31Z = v1Z1 - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y1 * v23Z - v12Z1 * v23Y;
nY = v12Z1 * v23X - v12X1 * v23Z;
nZ = v12X1 * v23Y - v12Y1 * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y1 * nZ - v12Z1 * nY;
n12Y = v12Z1 * nX - v12X1 * nZ;
n12Z = v12X1 * nY - v12Y1 * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X1 * n12X + v1Y1 * n12Y + v1Z1 * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X1 * n23X + v2Y1 * n23Y + v2Z1 * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
v = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X1 * nX + v1Y1 * nY + v1Z1 * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
v = 7;
}
break;
case 4:
let vec11 = s[0];
let vec21 = s[1];
let vec31 = s[2];
let vec4 = s[3];
let v1X2;
let v1Y2;
let v1Z2;
let v2X2;
let v2Y2;
let v2Z2;
let v3X1;
let v3Y1;
let v3Z1;
let v4X;
let v4Y;
let v4Z;
let v12X2;
let v12Y2;
let v12Z2;
let v13X;
let v13Y;
let v13Z;
let v14X;
let v14Y;
let v14Z;
let v23X1;
let v23Y1;
let v23Z1;
let v24X;
let v24Y;
let v24Z;
v1X2 = vec11.x;
v1Y2 = vec11.y;
v1Z2 = vec11.z;
v2X2 = vec21.x;
v2Y2 = vec21.y;
v2Z2 = vec21.z;
v3X1 = vec31.x;
v3Y1 = vec31.y;
v3Z1 = vec31.z;
v4X = vec4.x;
v4Y = vec4.y;
v4Z = vec4.z;
v12X2 = v2X2 - v1X2;
v12Y2 = v2Y2 - v1Y2;
v12Z2 = v2Z2 - v1Z2;
v13X = v3X1 - v1X2;
v13Y = v3Y1 - v1Y2;
v13Z = v3Z1 - v1Z2;
v14X = v4X - v1X2;
v14Y = v4Y - v1Y2;
v14Z = v4Z - v1Z2;
v23X1 = v3X1 - v2X2;
v23Y1 = v3Y1 - v2Y2;
v23Z1 = v3Z1 - v2Z2;
v24X = v4X - v2X2;
v24Y = v4Y - v2Y2;
v24Z = v4Z - v2Z2;
let n123X;
let n123Y;
let n123Z;
let n134X;
let n134Y;
let n134Z;
let n142X;
let n142Y;
let n142Z;
let n243X;
let n243Y;
let n243Z;
n123X = v12Y2 * v13Z - v12Z2 * v13Y;
n123Y = v12Z2 * v13X - v12X2 * v13Z;
n123Z = v12X2 * v13Y - v12Y2 * v13X;
n134X = v13Y * v14Z - v13Z * v14Y;
n134Y = v13Z * v14X - v13X * v14Z;
n134Z = v13X * v14Y - v13Y * v14X;
n142X = v14Y * v12Z2 - v14Z * v12Y2;
n142Y = v14Z * v12X2 - v14X * v12Z2;
n142Z = v14X * v12Y2 - v14Y * v12X2;
n243X = v24Y * v23Z1 - v24Z * v23Y1;
n243Y = v24Z * v23X1 - v24X * v23Z1;
n243Z = v24X * v23Y1 - v24Y * v23X1;
let sign = v12X2 * n243X + v12Y2 * n243Y + v12Z2 * n243Z > 0 ? 1 : -1;
let mind1 = -1;
let minvX1;
let minvY1;
let minvZ1;
let mini1 = 0;
minvX1 = 0;
minvY1 = 0;
minvZ1 = 0;
if((v1X2 * n123X + v1Y2 * n123Y + v1Z2 * n123Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec21.x;
v2Y = vec21.y;
v2Z = vec21.z;
v3X = vec31.x;
v3Y = vec31.y;
v3Z = vec31.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec21.x;
v2Y = vec21.y;
v2Z = vec21.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
let b;
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = 7;
}
mini1 = b;
mind1 = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX1 = closest.x;
minvY1 = closest.y;
minvZ1 = closest.z;
}
if((v1X2 * n134X + v1Y2 * n134Y + v1Z2 * n134Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec31.x;
v1Y = vec31.y;
v1Z = vec31.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
let b;
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = 7;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind1 < 0 || d < mind1) {
mini1 = b & 1 | (b & 6) << 1;
mind1 = d;
minvX1 = closest.x;
minvY1 = closest.y;
minvZ1 = closest.z;
}
}
if((v1X2 * n142X + v1Y2 * n142Y + v1Z2 * n142Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec21.x;
v2Y = vec21.y;
v2Z = vec21.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec21.x;
v2Y = vec21.y;
v2Z = vec21.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec11.x;
v1Y = vec11.y;
v1Z = vec11.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
let b;
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = 7;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind1 < 0 || d < mind1) {
mini1 = b & 3 | (b & 4) << 1;
mind1 = d;
minvX1 = closest.x;
minvY1 = closest.y;
minvZ1 = closest.z;
}
}
if((v2X2 * n243X + v2Y2 * n243Y + v2Z2 * n243Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec31.x;
v2Y = vec31.y;
v2Z = vec31.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
mini = b;
mind = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec31.x;
v1Y = vec31.y;
v1Z = vec31.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec21.x;
v1Y = vec21.y;
v1Z = vec21.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
closest.x = v1X;
closest.y = v1Y;
closest.z = v1Z;
b = 1;
} else if(t > 1) {
closest.x = v2X;
closest.y = v2Y;
closest.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
closest.x = pX;
closest.y = pY;
closest.z = pZ;
b = 3;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = closest.x;
minvY = closest.y;
minvZ = closest.z;
}
}
let b;
if(mind > 0) {
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = mini;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
closest.x = minvX;
closest.y = minvY;
closest.z = minvZ;
b = 7;
}
let d = closest.x * closest.x + closest.y * closest.y + closest.z * closest.z;
if(mind1 < 0 || d < mind1) {
mini1 = b << 1;
mind1 = d;
minvX1 = closest.x;
minvY1 = closest.y;
minvZ1 = closest.z;
}
}
if(mind1 > 0) {
closest.x = minvX1;
closest.y = minvY1;
closest.z = minvZ1;
v = mini1;
} else {
closest.zero();
v = 15;
}
break;
}
this.shrinkSimplex(v);
if(closest.x * closest.x + closest.y * closest.y + closest.z * closest.z < 1e-008) {
if(lambda == 0 || this.simplexSize == 4) {
hit.fraction = lambda;
return false;
}
this.interpolateClosestPoints();
hit.fraction = lambda;
let _this = hit.normal;
_this.x = dir.x;
_this.y = dir.y;
_this.z = dir.z;
let invLen = Math.sqrt(_this.x * _this.x + _this.y * _this.y + _this.z * _this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this.x *= invLen;
_this.y *= invLen;
_this.z *= invLen;
let _this1 = hit.position;
let v = this.closestPoint1;
_this1.x = v.x;
_this1.y = v.y;
_this1.z = v.z;
_this1.x += tl1.x * lambda;
_this1.y += tl1.y * lambda;
_this1.z += tl1.z * lambda;
return true;
}
dir.x = closest.x;
dir.y = closest.y;
dir.z = closest.z;
dir.x = -dir.x;
dir.y = -dir.y;
dir.z = -dir.z;
if(this.c1 != null) {
this.computeWitnessPoint1(true);
} else {
let v = this.w1[this.simplexSize];
v.x = this.tf1._positionX;
v.y = this.tf1._positionY;
v.z = this.tf1._positionZ;
}
this.computeWitnessPoint2(true);
let _this = this.s[this.simplexSize];
let v4 = this.w1[this.simplexSize];
_this.x = v4.x;
_this.y = v4.y;
_this.z = v4.z;
let v5 = this.w2[this.simplexSize];
_this.x -= v5.x;
_this.y -= v5.y;
_this.z -= v5.z;
let _this1 = s[this.simplexSize];
_this1.x -= rayX.x;
_this1.y -= rayX.y;
_this1.z -= rayX.z;
if(dir.x * dir.x + dir.y * dir.y + dir.z * dir.z < 1e-008) {
throw new Error("!?");
}
let p = s[this.simplexSize];
let pn = p.x * dir.x + p.y * dir.y + p.z * dir.z;
if(pn < 0) {
if(rayR.x * dir.x + rayR.y * dir.y + rayR.z * dir.z >= 0) {
return false;
}
let dLambda = pn / (rayR.x * dir.x + rayR.y * dir.y + rayR.z * dir.z);
lambda += dLambda;
if(lambda >= 1) {
return false;
}
rayX.x += rayR.x * dLambda;
rayX.y += rayR.y * dLambda;
rayX.z += rayR.z * dLambda;
let _g = 0;
let _g1 = this.simplexSize + 1;
while(_g < _g1) {
let _this = s[_g++];
let s1 = -dLambda;
_this.x += rayR.x * s1;
_this.y += rayR.y * s1;
_this.z += rayR.z * s1;
}
}
let duplicate = false;
let _g = 0;
let _g1 = this.simplexSize;
while(_g < _g1) {
let i = _g++;
let dx = s[i].x - s[this.simplexSize].x;
let dy = s[i].y - s[this.simplexSize].y;
let dz = s[i].z - s[this.simplexSize].z;
if(dx * dx + dy * dy + dz * dz < 1e-008) {
duplicate = true;
break;
}
}
if(!duplicate) {
this.simplexSize++;
}
++count;
}
return false;
}
interpolateClosestPoints() {
switch(this.simplexSize) {
case 1:
let _this = this.closestPoint1;
let v = this.w1[0];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let _this1 = this.closestPoint2;
let v1 = this.w2[0];
_this1.x = v1.x;
_this1.y = v1.y;
_this1.z = v1.z;
break;
case 2:
let cX;
let cY;
let cZ;
let v2 = this.closest;
cX = v2.x;
cY = v2.y;
cZ = v2.z;
let s0X;
let s0Y;
let s0Z;
let w10X;
let w10Y;
let w10Z;
let w20X;
let w20Y;
let w20Z;
let s1X;
let s1Y;
let s1Z;
let w11X;
let w11Y;
let w11Z;
let w21X;
let w21Y;
let w21Z;
let v3 = this.s[0];
s0X = v3.x;
s0Y = v3.y;
s0Z = v3.z;
let v4 = this.w1[0];
w10X = v4.x;
w10Y = v4.y;
w10Z = v4.z;
let v5 = this.w2[0];
w20X = v5.x;
w20Y = v5.y;
w20Z = v5.z;
let v6 = this.s[1];
s1X = v6.x;
s1Y = v6.y;
s1Z = v6.z;
let v7 = this.w1[1];
w11X = v7.x;
w11Y = v7.y;
w11Z = v7.z;
let v8 = this.w2[1];
w21X = v8.x;
w21Y = v8.y;
w21Z = v8.z;
let s01X;
let s01Y;
let s01Z;
s01X = s1X - s0X;
s01Y = s1Y - s0Y;
s01Z = s1Z - s0Z;
let invDet = s01X * s01X + s01Y * s01Y + s01Z * s01Z;
if(invDet != 0) {
invDet = 1 / invDet;
}
let s0cX;
let s0cY;
let s0cZ;
s0cX = cX - s0X;
s0cY = cY - s0Y;
s0cZ = cZ - s0Z;
let t = (s0cX * s01X + s0cY * s01Y + s0cZ * s01Z) * invDet;
let diffX;
let diffY;
let diffZ;
let cp1X;
let cp1Y;
let cp1Z;
let cp2X;
let cp2Y;
let cp2Z;
diffX = w11X - w10X;
diffY = w11Y - w10Y;
diffZ = w11Z - w10Z;
cp1X = w10X + diffX * t;
cp1Y = w10Y + diffY * t;
cp1Z = w10Z + diffZ * t;
diffX = w21X - w20X;
diffY = w21Y - w20Y;
diffZ = w21Z - w20Z;
cp2X = w20X + diffX * t;
cp2Y = w20Y + diffY * t;
cp2Z = w20Z + diffZ * t;
let v9 = this.closestPoint1;
v9.x = cp1X;
v9.y = cp1Y;
v9.z = cp1Z;
let v10 = this.closestPoint2;
v10.x = cp2X;
v10.y = cp2Y;
v10.z = cp2Z;
break;
case 3:
let cX1;
let cY1;
let cZ1;
let v11 = this.closest;
cX1 = v11.x;
cY1 = v11.y;
cZ1 = v11.z;
let s0X1;
let s0Y1;
let s0Z1;
let w10X1;
let w10Y1;
let w10Z1;
let w20X1;
let w20Y1;
let w20Z1;
let s1X1;
let s1Y1;
let s1Z1;
let w11X1;
let w11Y1;
let w11Z1;
let w21X1;
let w21Y1;
let w21Z1;
let s2X;
let s2Y;
let s2Z;
let w12X;
let w12Y;
let w12Z;
let w22X;
let w22Y;
let w22Z;
let v12 = this.s[0];
s0X1 = v12.x;
s0Y1 = v12.y;
s0Z1 = v12.z;
let v13 = this.w1[0];
w10X1 = v13.x;
w10Y1 = v13.y;
w10Z1 = v13.z;
let v14 = this.w2[0];
w20X1 = v14.x;
w20Y1 = v14.y;
w20Z1 = v14.z;
let v15 = this.s[1];
s1X1 = v15.x;
s1Y1 = v15.y;
s1Z1 = v15.z;
let v16 = this.w1[1];
w11X1 = v16.x;
w11Y1 = v16.y;
w11Z1 = v16.z;
let v17 = this.w2[1];
w21X1 = v17.x;
w21Y1 = v17.y;
w21Z1 = v17.z;
let v18 = this.s[2];
s2X = v18.x;
s2Y = v18.y;
s2Z = v18.z;
let v19 = this.w1[2];
w12X = v19.x;
w12Y = v19.y;
w12Z = v19.z;
let v20 = this.w2[2];
w22X = v20.x;
w22Y = v20.y;
w22Z = v20.z;
let s01X1;
let s01Y1;
let s01Z1;
let s02X;
let s02Y;
let s02Z;
let s0cX1;
let s0cY1;
let s0cZ1;
s01X1 = s1X1 - s0X1;
s01Y1 = s1Y1 - s0Y1;
s01Z1 = s1Z1 - s0Z1;
s02X = s2X - s0X1;
s02Y = s2Y - s0Y1;
s02Z = s2Z - s0Z1;
s0cX1 = cX1 - s0X1;
s0cY1 = cY1 - s0Y1;
s0cZ1 = cZ1 - s0Z1;
let d11 = s01X1 * s01X1 + s01Y1 * s01Y1 + s01Z1 * s01Z1;
let d12 = s01X1 * s02X + s01Y1 * s02Y + s01Z1 * s02Z;
let d22 = s02X * s02X + s02Y * s02Y + s02Z * s02Z;
let d1c = s01X1 * s0cX1 + s01Y1 * s0cY1 + s01Z1 * s0cZ1;
let d2c = s02X * s0cX1 + s02Y * s0cY1 + s02Z * s0cZ1;
let invDet1 = d11 * d22 - d12 * d12;
if(invDet1 != 0) {
invDet1 = 1 / invDet1;
}
let s = (d1c * d22 - d2c * d12) * invDet1;
let t1 = (-d1c * d12 + d2c * d11) * invDet1;
let diffX1;
let diffY1;
let diffZ1;
let cp1X1;
let cp1Y1;
let cp1Z1;
let cp2X1;
let cp2Y1;
let cp2Z1;
diffX1 = w11X1 - w10X1;
diffY1 = w11Y1 - w10Y1;
diffZ1 = w11Z1 - w10Z1;
cp1X1 = w10X1 + diffX1 * s;
cp1Y1 = w10Y1 + diffY1 * s;
cp1Z1 = w10Z1 + diffZ1 * s;
diffX1 = w12X - w10X1;
diffY1 = w12Y - w10Y1;
diffZ1 = w12Z - w10Z1;
cp1X1 += diffX1 * t1;
cp1Y1 += diffY1 * t1;
cp1Z1 += diffZ1 * t1;
diffX1 = w21X1 - w20X1;
diffY1 = w21Y1 - w20Y1;
diffZ1 = w21Z1 - w20Z1;
cp2X1 = w20X1 + diffX1 * s;
cp2Y1 = w20Y1 + diffY1 * s;
cp2Z1 = w20Z1 + diffZ1 * s;
diffX1 = w22X - w20X1;
diffY1 = w22Y - w20Y1;
diffZ1 = w22Z - w20Z1;
cp2X1 += diffX1 * t1;
cp2Y1 += diffY1 * t1;
cp2Z1 += diffZ1 * t1;
let v21 = this.closestPoint1;
v21.x = cp1X1;
v21.y = cp1Y1;
v21.z = cp1Z1;
let v22 = this.closestPoint2;
v22.x = cp2X1;
v22.y = cp2Y1;
v22.z = cp2Z1;
break;
default:
throw new Error("!?");
}
}
loadCache(gjkCache) {
let _this = this.dir;
let v = gjkCache.prevClosestDir;
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
}
saveCache(gjkCache) {
let _this = gjkCache.prevClosestDir;
let v = this.closest;
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
_this.x = -_this.x;
_this.y = -_this.y;
_this.z = -_this.z;
}
shrinkSimplex(vertexBits) {
this.simplexSize = vertexBits;
this.simplexSize = (this.simplexSize & 5) + (this.simplexSize >> 1 & 5);
this.simplexSize = (this.simplexSize & 3) + (this.simplexSize >> 2 & 3);
switch(vertexBits) {
case 2:
let _this = this.s[0];
let v = this.s[1];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let _this1 = this.w1[0];
let v1 = this.w1[1];
_this1.x = v1.x;
_this1.y = v1.y;
_this1.z = v1.z;
let _this2 = this.w2[0];
let v2 = this.w2[1];
_this2.x = v2.x;
_this2.y = v2.y;
_this2.z = v2.z;
break;
case 4:
let _this3 = this.s[0];
let v3 = this.s[2];
_this3.x = v3.x;
_this3.y = v3.y;
_this3.z = v3.z;
let _this4 = this.w1[0];
let v4 = this.w1[2];
_this4.x = v4.x;
_this4.y = v4.y;
_this4.z = v4.z;
let _this5 = this.w2[0];
let v5 = this.w2[2];
_this5.x = v5.x;
_this5.y = v5.y;
_this5.z = v5.z;
break;
case 5:
let _this6 = this.s[1];
let v6 = this.s[2];
_this6.x = v6.x;
_this6.y = v6.y;
_this6.z = v6.z;
let _this7 = this.w1[1];
let v7 = this.w1[2];
_this7.x = v7.x;
_this7.y = v7.y;
_this7.z = v7.z;
let _this8 = this.w2[1];
let v8 = this.w2[2];
_this8.x = v8.x;
_this8.y = v8.y;
_this8.z = v8.z;
break;
case 6:
let _this9 = this.s[0];
let v9 = this.s[2];
_this9.x = v9.x;
_this9.y = v9.y;
_this9.z = v9.z;
let _this10 = this.w1[0];
let v10 = this.w1[2];
_this10.x = v10.x;
_this10.y = v10.y;
_this10.z = v10.z;
let _this11 = this.w2[0];
let v11 = this.w2[2];
_this11.x = v11.x;
_this11.y = v11.y;
_this11.z = v11.z;
break;
case 8:
let _this12 = this.s[0];
let v12 = this.s[3];
_this12.x = v12.x;
_this12.y = v12.y;
_this12.z = v12.z;
let _this13 = this.w1[0];
let v13 = this.w1[3];
_this13.x = v13.x;
_this13.y = v13.y;
_this13.z = v13.z;
let _this14 = this.w2[0];
let v14 = this.w2[3];
_this14.x = v14.x;
_this14.y = v14.y;
_this14.z = v14.z;
break;
case 9:
let _this15 = this.s[1];
let v15 = this.s[3];
_this15.x = v15.x;
_this15.y = v15.y;
_this15.z = v15.z;
let _this16 = this.w1[1];
let v16 = this.w1[3];
_this16.x = v16.x;
_this16.y = v16.y;
_this16.z = v16.z;
let _this17 = this.w2[1];
let v17 = this.w2[3];
_this17.x = v17.x;
_this17.y = v17.y;
_this17.z = v17.z;
break;
case 10:
let _this18 = this.s[0];
let v18 = this.s[3];
_this18.x = v18.x;
_this18.y = v18.y;
_this18.z = v18.z;
let _this19 = this.w1[0];
let v19 = this.w1[3];
_this19.x = v19.x;
_this19.y = v19.y;
_this19.z = v19.z;
let _this20 = this.w2[0];
let v20 = this.w2[3];
_this20.x = v20.x;
_this20.y = v20.y;
_this20.z = v20.z;
break;
case 11:
let _this21 = this.s[2];
let v21 = this.s[3];
_this21.x = v21.x;
_this21.y = v21.y;
_this21.z = v21.z;
let _this22 = this.w1[2];
let v22 = this.w1[3];
_this22.x = v22.x;
_this22.y = v22.y;
_this22.z = v22.z;
let _this23 = this.w2[2];
let v23 = this.w2[3];
_this23.x = v23.x;
_this23.y = v23.y;
_this23.z = v23.z;
break;
case 12:
let _this24 = this.s[0];
let v24 = this.s[2];
_this24.x = v24.x;
_this24.y = v24.y;
_this24.z = v24.z;
let _this25 = this.w1[0];
let v25 = this.w1[2];
_this25.x = v25.x;
_this25.y = v25.y;
_this25.z = v25.z;
let _this26 = this.w2[0];
let v26 = this.w2[2];
_this26.x = v26.x;
_this26.y = v26.y;
_this26.z = v26.z;
let _this27 = this.s[1];
let v27 = this.s[3];
_this27.x = v27.x;
_this27.y = v27.y;
_this27.z = v27.z;
let _this28 = this.w1[1];
let v28 = this.w1[3];
_this28.x = v28.x;
_this28.y = v28.y;
_this28.z = v28.z;
let _this29 = this.w2[1];
let v29 = this.w2[3];
_this29.x = v29.x;
_this29.y = v29.y;
_this29.z = v29.z;
break;
case 13:
let _this30 = this.s[1];
let v30 = this.s[3];
_this30.x = v30.x;
_this30.y = v30.y;
_this30.z = v30.z;
let _this31 = this.w1[1];
let v31 = this.w1[3];
_this31.x = v31.x;
_this31.y = v31.y;
_this31.z = v31.z;
let _this32 = this.w2[1];
let v32 = this.w2[3];
_this32.x = v32.x;
_this32.y = v32.y;
_this32.z = v32.z;
break;
case 14:
let _this33 = this.s[0];
let v33 = this.s[3];
_this33.x = v33.x;
_this33.y = v33.y;
_this33.z = v33.z;
let _this34 = this.w1[0];
let v34 = this.w1[3];
_this34.x = v34.x;
_this34.y = v34.y;
_this34.z = v34.z;
let _this35 = this.w2[0];
let v35 = this.w2[3];
_this35.x = v35.x;
_this35.y = v35.y;
_this35.z = v35.z;
break;
}
}
computeWitnessPoint1(addMargin) {
let tmpX;
let tmpY;
let tmpZ;
let idirX;
let idirY;
let idirZ;
let v = this.dir;
idirX = v.x;
idirY = v.y;
idirZ = v.z;
let ldir1X;
let ldir1Y;
let ldir1Z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this.tf1._rotation00 * idirX + this.tf1._rotation10 * idirY + this.tf1._rotation20 * idirZ;
__tmp__Y = this.tf1._rotation01 * idirX + this.tf1._rotation11 * idirY + this.tf1._rotation21 * idirZ;
__tmp__Z = this.tf1._rotation02 * idirX + this.tf1._rotation12 * idirY + this.tf1._rotation22 * idirZ;
ldir1X = __tmp__X;
ldir1Y = __tmp__Y;
ldir1Z = __tmp__Z;
let iw1X;
let iw1Y;
let iw1Z;
let v1 = this.dir;
v1.x = ldir1X;
v1.y = ldir1Y;
v1.z = ldir1Z;
this.c1.computeLocalSupportingVertex(this.dir,this.w1[this.simplexSize]);
if(addMargin) {
let _this = this.dir;
let invLen = Math.sqrt(_this.x * _this.x + _this.y * _this.y + _this.z * _this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this.x *= invLen;
_this.y *= invLen;
_this.z *= invLen;
let _this1 = this.w1[this.simplexSize];
let v = this.dir;
let s = this.c1._gjkMargin;
_this1.x += v.x * s;
_this1.y += v.y * s;
_this1.z += v.z * s;
}
let v2 = this.w1[this.simplexSize];
tmpX = v2.x;
tmpY = v2.y;
tmpZ = v2.z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = this.tf1._rotation00 * tmpX + this.tf1._rotation01 * tmpY + this.tf1._rotation02 * tmpZ;
__tmp__Y1 = this.tf1._rotation10 * tmpX + this.tf1._rotation11 * tmpY + this.tf1._rotation12 * tmpZ;
__tmp__Z1 = this.tf1._rotation20 * tmpX + this.tf1._rotation21 * tmpY + this.tf1._rotation22 * tmpZ;
iw1X = __tmp__X1;
iw1Y = __tmp__Y1;
iw1Z = __tmp__Z1;
iw1X += this.tf1._positionX;
iw1Y += this.tf1._positionY;
iw1Z += this.tf1._positionZ;
let v3 = this.w1[this.simplexSize];
v3.x = iw1X;
v3.y = iw1Y;
v3.z = iw1Z;
let v4 = this.dir;
v4.x = idirX;
v4.y = idirY;
v4.z = idirZ;
}
computeWitnessPoint2(addMargin) {
let tmpX;
let tmpY;
let tmpZ;
let idirX;
let idirY;
let idirZ;
let v = this.dir;
idirX = v.x;
idirY = v.y;
idirZ = v.z;
let ldir2X;
let ldir2Y;
let ldir2Z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this.tf2._rotation00 * idirX + this.tf2._rotation10 * idirY + this.tf2._rotation20 * idirZ;
__tmp__Y = this.tf2._rotation01 * idirX + this.tf2._rotation11 * idirY + this.tf2._rotation21 * idirZ;
__tmp__Z = this.tf2._rotation02 * idirX + this.tf2._rotation12 * idirY + this.tf2._rotation22 * idirZ;
ldir2X = __tmp__X;
ldir2Y = __tmp__Y;
ldir2Z = __tmp__Z;
ldir2X = -ldir2X;
ldir2Y = -ldir2Y;
ldir2Z = -ldir2Z;
let iw2X;
let iw2Y;
let iw2Z;
let v1 = this.dir;
v1.x = ldir2X;
v1.y = ldir2Y;
v1.z = ldir2Z;
this.c2.computeLocalSupportingVertex(this.dir,this.w2[this.simplexSize]);
if(addMargin) {
let _this = this.dir;
let invLen = Math.sqrt(_this.x * _this.x + _this.y * _this.y + _this.z * _this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this.x *= invLen;
_this.y *= invLen;
_this.z *= invLen;
let _this1 = this.w2[this.simplexSize];
let v = this.dir;
let s = this.c2._gjkMargin;
_this1.x += v.x * s;
_this1.y += v.y * s;
_this1.z += v.z * s;
}
let v2 = this.w2[this.simplexSize];
tmpX = v2.x;
tmpY = v2.y;
tmpZ = v2.z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = this.tf2._rotation00 * tmpX + this.tf2._rotation01 * tmpY + this.tf2._rotation02 * tmpZ;
__tmp__Y1 = this.tf2._rotation10 * tmpX + this.tf2._rotation11 * tmpY + this.tf2._rotation12 * tmpZ;
__tmp__Z1 = this.tf2._rotation20 * tmpX + this.tf2._rotation21 * tmpY + this.tf2._rotation22 * tmpZ;
iw2X = __tmp__X1;
iw2Y = __tmp__Y1;
iw2Z = __tmp__Z1;
iw2X += this.tf2._positionX;
iw2Y += this.tf2._positionY;
iw2Z += this.tf2._positionZ;
let v3 = this.w2[this.simplexSize];
v3.x = iw2X;
v3.y = iw2Y;
v3.z = iw2Z;
let v4 = this.dir;
v4.x = idirX;
v4.y = idirY;
v4.z = idirZ;
}
pointToTetrahedron() {
let _g = 0;
while(_g < 3) {
let _this = this.dir;
let v = this.baseDirs[_g++];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this1 = this.s[this.simplexSize];
let v1 = this.w1[this.simplexSize];
_this1.x = v1.x;
_this1.y = v1.y;
_this1.z = v1.z;
let v2 = this.w2[this.simplexSize];
_this1.x -= v2.x;
_this1.y -= v2.y;
_this1.z -= v2.z;
this.simplexSize++;
this.lineToTetrahedron();
if(this.simplexSize == 4) {
break;
}
this.simplexSize--;
let _this2 = this.dir;
_this2.x = -_this2.x;
_this2.y = -_this2.y;
_this2.z = -_this2.z;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this3 = this.s[this.simplexSize];
let v3 = this.w1[this.simplexSize];
_this3.x = v3.x;
_this3.y = v3.y;
_this3.z = v3.z;
let v4 = this.w2[this.simplexSize];
_this3.x -= v4.x;
_this3.y -= v4.y;
_this3.z -= v4.z;
this.simplexSize++;
this.lineToTetrahedron();
if(this.simplexSize == 4) {
break;
}
this.simplexSize--;
}
}
lineToTetrahedron() {
let oldDirX;
let oldDirY;
let oldDirZ;
let v = this.dir;
oldDirX = v.x;
oldDirY = v.y;
oldDirZ = v.z;
let s0X;
let s0Y;
let s0Z;
let s1X;
let s1Y;
let s1Z;
let lineDirX;
let lineDirY;
let lineDirZ;
let v1 = this.s[0];
s0X = v1.x;
s0Y = v1.y;
s0Z = v1.z;
let v2 = this.s[1];
s1X = v2.x;
s1Y = v2.y;
s1Z = v2.z;
lineDirX = s0X - s1X;
lineDirY = s0Y - s1Y;
lineDirZ = s0Z - s1Z;
let _g = 0;
while(_g < 3) {
let baseDirX;
let baseDirY;
let baseDirZ;
let v = this.baseDirs[_g++];
baseDirX = v.x;
baseDirY = v.y;
baseDirZ = v.z;
let newDirX;
let newDirY;
let newDirZ;
newDirX = lineDirY * baseDirZ - lineDirZ * baseDirY;
newDirY = lineDirZ * baseDirX - lineDirX * baseDirZ;
newDirZ = lineDirX * baseDirY - lineDirY * baseDirX;
let v1 = this.dir;
v1.x = newDirX;
v1.y = newDirY;
v1.z = newDirZ;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this = this.s[this.simplexSize];
let v2 = this.w1[this.simplexSize];
_this.x = v2.x;
_this.y = v2.y;
_this.z = v2.z;
let v3 = this.w2[this.simplexSize];
_this.x -= v3.x;
_this.y -= v3.y;
_this.z -= v3.z;
this.simplexSize++;
this.triangleToTetrahedron();
if(this.simplexSize == 4) {
break;
}
this.simplexSize--;
let _this1 = this.dir;
_this1.x = -_this1.x;
_this1.y = -_this1.y;
_this1.z = -_this1.z;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this2 = this.s[this.simplexSize];
let v4 = this.w1[this.simplexSize];
_this2.x = v4.x;
_this2.y = v4.y;
_this2.z = v4.z;
let v5 = this.w2[this.simplexSize];
_this2.x -= v5.x;
_this2.y -= v5.y;
_this2.z -= v5.z;
this.simplexSize++;
this.triangleToTetrahedron();
if(this.simplexSize == 4) {
break;
}
this.simplexSize--;
}
let v3 = this.dir;
v3.x = oldDirX;
v3.y = oldDirY;
v3.z = oldDirZ;
}
triangleToTetrahedron() {
let oldDirX;
let oldDirY;
let oldDirZ;
let v = this.dir;
oldDirX = v.x;
oldDirY = v.y;
oldDirZ = v.z;
while(true) {
let s0X;
let s0Y;
let s0Z;
let s1X;
let s1Y;
let s1Z;
let s2X;
let s2Y;
let s2Z;
let s01X;
let s01Y;
let s01Z;
let s02X;
let s02Y;
let s02Z;
let v = this.s[0];
s0X = v.x;
s0Y = v.y;
s0Z = v.z;
let v1 = this.s[1];
s1X = v1.x;
s1Y = v1.y;
s1Z = v1.z;
let v2 = this.s[2];
s2X = v2.x;
s2Y = v2.y;
s2Z = v2.z;
s01X = s1X - s0X;
s01Y = s1Y - s0Y;
s01Z = s1Z - s0Z;
s02X = s2X - s0X;
s02Y = s2Y - s0Y;
s02Z = s2Z - s0Z;
let nX;
let nY;
let nZ;
nX = s01Y * s02Z - s01Z * s02Y;
nY = s01Z * s02X - s01X * s02Z;
nZ = s01X * s02Y - s01Y * s02X;
let v3 = this.dir;
v3.x = nX;
v3.y = nY;
v3.z = nZ;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this = this.s[this.simplexSize];
let v4 = this.w1[this.simplexSize];
_this.x = v4.x;
_this.y = v4.y;
_this.z = v4.z;
let v5 = this.w2[this.simplexSize];
_this.x -= v5.x;
_this.y -= v5.y;
_this.z -= v5.z;
this.simplexSize++;
if(this.isValidTetrahedron()) {
break;
}
this.simplexSize--;
let _this1 = this.dir;
_this1.x = -_this1.x;
_this1.y = -_this1.y;
_this1.z = -_this1.z;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this2 = this.s[this.simplexSize];
let v6 = this.w1[this.simplexSize];
_this2.x = v6.x;
_this2.y = v6.y;
_this2.z = v6.z;
let v7 = this.w2[this.simplexSize];
_this2.x -= v7.x;
_this2.y -= v7.y;
_this2.z -= v7.z;
this.simplexSize++;
if(this.isValidTetrahedron()) {
break;
}
this.simplexSize--;
break;
}
let v1 = this.dir;
v1.x = oldDirX;
v1.y = oldDirY;
v1.z = oldDirZ;
}
isValidTetrahedron() {
let e10 = this.s[2].x - this.s[0].x;
let e11 = this.s[2].y - this.s[0].y;
let e12 = this.s[2].z - this.s[0].z;
let e20 = this.s[3].x - this.s[0].x;
let e21 = this.s[3].y - this.s[0].y;
let e22 = this.s[3].z - this.s[0].z;
let det = (this.s[1].x - this.s[0].x) * (e11 * e22 - e12 * e21) - (this.s[1].y - this.s[0].y) * (e10 * e22 - e12 * e20) + (this.s[1].z - this.s[0].z) * (e10 * e21 - e11 * e20);
if(!(det > 1e-12)) {
return det < -1e-12;
} else {
return true;
}
}
computeDepth(convex1,convex2,tf1,tf2,initialPolyhedron,initialPolyhedron1,initialPolyhedron2) {
let _this = this.polyhedron;
while(_this._numTriangles > 0) {
let t = _this._triangleList;
_this._numTriangles--;
let prev = t._prev;
let next = t._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(t == _this._triangleList) {
_this._triangleList = _this._triangleList._next;
}
if(t == _this._triangleListLast) {
_this._triangleListLast = _this._triangleListLast._prev;
}
t._next = null;
t._prev = null;
t.removeReferences();
t._next = _this._trianglePool;
_this._trianglePool = t;
}
while(_this._numVertices > 0) {
let v = _this._vertices[--_this._numVertices];
v.removeReferences();
v._next = _this._vertexPool;
_this._vertexPool = v;
}
let tmp = this.polyhedron;
let _this1 = this.polyhedron;
let first = _this1._vertexPool;
if(first != null) {
_this1._vertexPool = first._next;
first._next = null;
} else {
first = new oimo.collision.narrowphase.detector.gjkepa.EpaVertex();
}
let tmp1 = first.init(initialPolyhedron[0],initialPolyhedron1[0],initialPolyhedron2[0]);
let _this2 = this.polyhedron;
let first1 = _this2._vertexPool;
if(first1 != null) {
_this2._vertexPool = first1._next;
first1._next = null;
} else {
first1 = new oimo.collision.narrowphase.detector.gjkepa.EpaVertex();
}
let tmp2 = first1.init(initialPolyhedron[1],initialPolyhedron1[1],initialPolyhedron2[1]);
let _this3 = this.polyhedron;
let first2 = _this3._vertexPool;
if(first2 != null) {
_this3._vertexPool = first2._next;
first2._next = null;
} else {
first2 = new oimo.collision.narrowphase.detector.gjkepa.EpaVertex();
}
let tmp3 = first2.init(initialPolyhedron[2],initialPolyhedron1[2],initialPolyhedron2[2]);
let _this4 = this.polyhedron;
let first3 = _this4._vertexPool;
if(first3 != null) {
_this4._vertexPool = first3._next;
first3._next = null;
} else {
first3 = new oimo.collision.narrowphase.detector.gjkepa.EpaVertex();
}
if(!tmp._init(tmp1,tmp2,tmp3,first3.init(initialPolyhedron[3],initialPolyhedron1[3],initialPolyhedron2[3]))) {
return oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.EPA_FAILED_TO_INIT;
}
this.simplexSize = 0;
let supportingVertex = this.s[0];
let witness1 = this.w1[0];
let witness2 = this.w2[0];
let count = 0;
while(count < 40) {
let f = this.polyhedron._triangleList;
let mind = 1e65536;
let minf = null;
while(f != null) {
if(f._distanceSq < mind) {
mind = f._distanceSq;
minf = f;
}
f = f._next;
}
let face = minf;
let _this = this.dir;
let v = face._normal;
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let invLen = Math.sqrt(_this.x * _this.x + _this.y * _this.y + _this.z * _this.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this.x *= invLen;
_this.y *= invLen;
_this.z *= invLen;
this.computeWitnessPoint1(false);
this.computeWitnessPoint2(false);
let _this1 = this.s[this.simplexSize];
let v1 = this.w1[this.simplexSize];
_this1.x = v1.x;
_this1.y = v1.y;
_this1.z = v1.z;
let v2 = this.w2[this.simplexSize];
_this1.x -= v2.x;
_this1.y -= v2.y;
_this1.z -= v2.z;
let v0 = face._vertices[0];
let v11 = face._vertices[1];
let v21 = face._vertices[2];
let _this2 = v0.v;
let v3 = this.dir;
let v4 = this.dir;
if(supportingVertex.x * v4.x + supportingVertex.y * v4.y + supportingVertex.z * v4.z - (_this2.x * v3.x + _this2.y * v3.y + _this2.z * v3.z) < 1e-6 || count == 39) {
let _this = this.closest;
let v = this.dir;
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let _this1 = this.dir;
let v1 = v0.v;
let _this2 = this.dir;
let s = (_this1.x * v1.x + _this1.y * v1.y + _this1.z * v1.z) / (_this2.x * _this2.x + _this2.y * _this2.y + _this2.z * _this2.z);
_this.x *= s;
_this.y *= s;
_this.z *= s;
let cX;
let cY;
let cZ;
let v2 = this.closest;
cX = v2.x;
cY = v2.y;
cZ = v2.z;
let s0X;
let s0Y;
let s0Z;
let w10X;
let w10Y;
let w10Z;
let w20X;
let w20Y;
let w20Z;
let s1X;
let s1Y;
let s1Z;
let w11X;
let w11Y;
let w11Z;
let w21X;
let w21Y;
let w21Z;
let s2X;
let s2Y;
let s2Z;
let w12X;
let w12Y;
let w12Z;
let w22X;
let w22Y;
let w22Z;
let v3 = v0.v;
s0X = v3.x;
s0Y = v3.y;
s0Z = v3.z;
let v4 = v0.w1;
w10X = v4.x;
w10Y = v4.y;
w10Z = v4.z;
let v5 = v0.w2;
w20X = v5.x;
w20Y = v5.y;
w20Z = v5.z;
let v6 = v11.v;
s1X = v6.x;
s1Y = v6.y;
s1Z = v6.z;
let v7 = v11.w1;
w11X = v7.x;
w11Y = v7.y;
w11Z = v7.z;
let v8 = v11.w2;
w21X = v8.x;
w21Y = v8.y;
w21Z = v8.z;
let v9 = v21.v;
s2X = v9.x;
s2Y = v9.y;
s2Z = v9.z;
let v10 = v21.w1;
w12X = v10.x;
w12Y = v10.y;
w12Z = v10.z;
let v12 = v21.w2;
w22X = v12.x;
w22Y = v12.y;
w22Z = v12.z;
let s01X;
let s01Y;
let s01Z;
let s02X;
let s02Y;
let s02Z;
let s0cX;
let s0cY;
let s0cZ;
s01X = s1X - s0X;
s01Y = s1Y - s0Y;
s01Z = s1Z - s0Z;
s02X = s2X - s0X;
s02Y = s2Y - s0Y;
s02Z = s2Z - s0Z;
s0cX = cX - s0X;
s0cY = cY - s0Y;
s0cZ = cZ - s0Z;
let d11 = s01X * s01X + s01Y * s01Y + s01Z * s01Z;
let d12 = s01X * s02X + s01Y * s02Y + s01Z * s02Z;
let d22 = s02X * s02X + s02Y * s02Y + s02Z * s02Z;
let d1c = s01X * s0cX + s01Y * s0cY + s01Z * s0cZ;
let d2c = s02X * s0cX + s02Y * s0cY + s02Z * s0cZ;
let invDet = d11 * d22 - d12 * d12;
if(invDet != 0) {
invDet = 1 / invDet;
}
let s1 = (d1c * d22 - d2c * d12) * invDet;
let t = (-d1c * d12 + d2c * d11) * invDet;
let diffX;
let diffY;
let diffZ;
let cp1X;
let cp1Y;
let cp1Z;
let cp2X;
let cp2Y;
let cp2Z;
diffX = w11X - w10X;
diffY = w11Y - w10Y;
diffZ = w11Z - w10Z;
cp1X = w10X + diffX * s1;
cp1Y = w10Y + diffY * s1;
cp1Z = w10Z + diffZ * s1;
diffX = w12X - w10X;
diffY = w12Y - w10Y;
diffZ = w12Z - w10Z;
cp1X += diffX * t;
cp1Y += diffY * t;
cp1Z += diffZ * t;
diffX = w21X - w20X;
diffY = w21Y - w20Y;
diffZ = w21Z - w20Z;
cp2X = w20X + diffX * s1;
cp2Y = w20Y + diffY * s1;
cp2Z = w20Z + diffZ * s1;
diffX = w22X - w20X;
diffY = w22Y - w20Y;
diffZ = w22Z - w20Z;
cp2X += diffX * t;
cp2Y += diffY * t;
cp2Z += diffZ * t;
let v13 = this.closestPoint1;
v13.x = cp1X;
v13.y = cp1Y;
v13.z = cp1Z;
let v14 = this.closestPoint2;
v14.x = cp2X;
v14.y = cp2Y;
v14.z = cp2Z;
let _this3 = this.closest;
this.depth = Math.sqrt(_this3.x * _this3.x + _this3.y * _this3.y + _this3.z * _this3.z);
return oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.SUCCEEDED;
}
let _this3 = this.polyhedron;
let first = _this3._vertexPool;
if(first != null) {
_this3._vertexPool = first._next;
first._next = null;
} else {
first = new oimo.collision.narrowphase.detector.gjkepa.EpaVertex();
}
let epaVertex = first.init(supportingVertex,witness1,witness2);
if(!this.polyhedron._addVertex(epaVertex,face)) {
return oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.EPA_FAILED_TO_ADD_VERTEX;
}
++count;
}
return oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.EPA_DID_NOT_CONVERGE;
}
computeClosestPoints(c1,c2,tf1,tf2,cache) {
return this.computeClosestPointsImpl(c1,c2,tf1,tf2,cache,true);
}
computeDistance(c1,c2,tf1,tf2,cache) {
return this.computeClosestPointsImpl(c1,c2,tf1,tf2,cache,false);
}
convexCast(c1,c2,tf1,tf2,tl1,tl2,hit) {
return this.convexCastImpl(c1,c2,tf1,tf2,tl1,tl2,hit);
}
rayCast(c,tf,begin,end,hit) {
let tf1 = this.tempTransform;
tf1._positionX = begin.x;
tf1._positionY = begin.y;
tf1._positionZ = begin.z;
let tl1 = this.tl1;
let tl2 = this.tl2;
tl1.x = end.x;
tl1.y = end.y;
tl1.z = end.z;
tl1.x -= begin.x;
tl1.y -= begin.y;
tl1.z -= begin.z;
tl2.zero();
return this.convexCastImpl(null,c,tf1,tf,tl1,tl2,hit);
}
static getInstance() {
return oimo.collision.narrowphase.detector.gjkepa.GjkEpa.instance;
}
}
oimo.collision.narrowphase.detector.gjkepa.GjkEpaLog = class oimo_collision_narrowphase_detector_gjkepa_GjkEpaLog {
}
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState = class oimo_collision_narrowphase_detector_gjkepa_GjkEpaResultState {
}
oimo.collision.narrowphase.detector.gjkepa.SimplexUtil = class oimo_collision_narrowphase_detector_gjkepa_SimplexUtil {
static projectOrigin2(vec1,vec2,out) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
return 1;
}
if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
return 2;
}
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
return 3;
}
static projectOrigin3(vec1,vec2,vec3,out) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
v3X = vec3.x;
v3Y = vec3.y;
v3Z = vec3.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
mini = b;
mind = out.x * out.x + out.y * out.y + out.z * out.z;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 2) << 1;
mind = d;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
}
if(mind > 0) {
out.x = minvX;
out.y = minvY;
out.z = minvZ;
return mini;
}
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX = nX * l2;
minvY = nY * l2;
minvZ = nZ * l2;
out.x = minvX;
out.y = minvY;
out.z = minvZ;
return 7;
}
static projectOrigin4(vec1,vec2,vec3,vec4,out) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v4X;
let v4Y;
let v4Z;
let v12X;
let v12Y;
let v12Z;
let v13X;
let v13Y;
let v13Z;
let v14X;
let v14Y;
let v14Z;
let v23X;
let v23Y;
let v23Z;
let v24X;
let v24Y;
let v24Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
v3X = vec3.x;
v3Y = vec3.y;
v3Z = vec3.z;
v4X = vec4.x;
v4Y = vec4.y;
v4Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v13X = v3X - v1X;
v13Y = v3Y - v1Y;
v13Z = v3Z - v1Z;
v14X = v4X - v1X;
v14Y = v4Y - v1Y;
v14Z = v4Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v24X = v4X - v2X;
v24Y = v4Y - v2Y;
v24Z = v4Z - v2Z;
let n123X;
let n123Y;
let n123Z;
let n134X;
let n134Y;
let n134Z;
let n142X;
let n142Y;
let n142Z;
let n243X;
let n243Y;
let n243Z;
n123X = v12Y * v13Z - v12Z * v13Y;
n123Y = v12Z * v13X - v12X * v13Z;
n123Z = v12X * v13Y - v12Y * v13X;
n134X = v13Y * v14Z - v13Z * v14Y;
n134Y = v13Z * v14X - v13X * v14Z;
n134Z = v13X * v14Y - v13Y * v14X;
n142X = v14Y * v12Z - v14Z * v12Y;
n142Y = v14Z * v12X - v14X * v12Z;
n142Z = v14X * v12Y - v14Y * v12X;
n243X = v24Y * v23Z - v24Z * v23Y;
n243Y = v24Z * v23X - v24X * v23Z;
n243Z = v24X * v23Y - v24Y * v23X;
let sign = v12X * n243X + v12Y * n243Y + v12Z * n243Z > 0 ? 1 : -1;
let mind = -1;
let minvX;
let minvY;
let minvZ;
let mini = 0;
minvX = 0;
minvY = 0;
minvZ = 0;
if((v1X * n123X + v1Y * n123Y + v1Z * n123Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
v3X = vec3.x;
v3Y = vec3.y;
v3Z = vec3.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind1 = -1;
let minvX1;
let minvY1;
let minvZ1;
let mini1 = 0;
minvX1 = 0;
minvY1 = 0;
minvZ1 = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
mini1 = b;
mind1 = out.x * out.x + out.y * out.y + out.z * out.z;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind1 < 0 || d < mind1) {
mini1 = b << 1;
mind1 = d;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind1 < 0 || d < mind1) {
mini1 = b & 1 | (b & 2) << 1;
mind1 = d;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
}
let b;
if(mind1 > 0) {
out.x = minvX1;
out.y = minvY1;
out.z = minvZ1;
b = mini1;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX1 = nX * l2;
minvY1 = nY * l2;
minvZ1 = nZ * l2;
out.x = minvX1;
out.y = minvY1;
out.z = minvZ1;
b = 7;
}
mini = b;
mind = out.x * out.x + out.y * out.y + out.z * out.z;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
if((v1X * n134X + v1Y * n134Y + v1Z * n134Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind1 = -1;
let minvX1;
let minvY1;
let minvZ1;
let mini1 = 0;
minvX1 = 0;
minvY1 = 0;
minvZ1 = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
mini1 = b;
mind1 = out.x * out.x + out.y * out.y + out.z * out.z;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec3.x;
v1Y = vec3.y;
v1Z = vec3.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind1 < 0 || d < mind1) {
mini1 = b << 1;
mind1 = d;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind1 < 0 || d < mind1) {
mini1 = b & 1 | (b & 2) << 1;
mind1 = d;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
}
let b;
if(mind1 > 0) {
out.x = minvX1;
out.y = minvY1;
out.z = minvZ1;
b = mini1;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX1 = nX * l2;
minvY1 = nY * l2;
minvZ1 = nZ * l2;
out.x = minvX1;
out.y = minvY1;
out.z = minvZ1;
b = 7;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind < 0 || d < mind) {
mini = b & 1 | (b & 6) << 1;
mind = d;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
}
if((v1X * n142X + v1Y * n142Y + v1Z * n142Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind1 = -1;
let minvX1;
let minvY1;
let minvZ1;
let mini1 = 0;
minvX1 = 0;
minvY1 = 0;
minvZ1 = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec2.x;
v2Y = vec2.y;
v2Z = vec2.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
mini1 = b;
mind1 = out.x * out.x + out.y * out.y + out.z * out.z;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind1 < 0 || d < mind1) {
mini1 = b << 1;
mind1 = d;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec1.x;
v1Y = vec1.y;
v1Z = vec1.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind1 < 0 || d < mind1) {
mini1 = b & 1 | (b & 2) << 1;
mind1 = d;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
}
let b;
if(mind1 > 0) {
out.x = minvX1;
out.y = minvY1;
out.z = minvZ1;
b = mini1;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX1 = nX * l2;
minvY1 = nY * l2;
minvZ1 = nZ * l2;
out.x = minvX1;
out.y = minvY1;
out.z = minvZ1;
b = 7;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind < 0 || d < mind) {
mini = b & 3 | (b & 4) << 1;
mind = d;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
}
if((v2X * n243X + v2Y * n243Y + v2Z * n243Z) * sign < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
let v3X;
let v3Y;
let v3Z;
let v12X;
let v12Y;
let v12Z;
let v23X;
let v23Y;
let v23Z;
let v31X;
let v31Y;
let v31Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
v3X = vec4.x;
v3Y = vec4.y;
v3Z = vec4.z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
v23X = v3X - v2X;
v23Y = v3Y - v2Y;
v23Z = v3Z - v2Z;
v31X = v1X - v3X;
v31Y = v1Y - v3Y;
v31Z = v1Z - v3Z;
let nX;
let nY;
let nZ;
nX = v12Y * v23Z - v12Z * v23Y;
nY = v12Z * v23X - v12X * v23Z;
nZ = v12X * v23Y - v12Y * v23X;
let n12X;
let n12Y;
let n12Z;
let n23X;
let n23Y;
let n23Z;
let n31X;
let n31Y;
let n31Z;
n12X = v12Y * nZ - v12Z * nY;
n12Y = v12Z * nX - v12X * nZ;
n12Z = v12X * nY - v12Y * nX;
n23X = v23Y * nZ - v23Z * nY;
n23Y = v23Z * nX - v23X * nZ;
n23Z = v23X * nY - v23Y * nX;
n31X = v31Y * nZ - v31Z * nY;
n31Y = v31Z * nX - v31X * nZ;
n31Z = v31X * nY - v31Y * nX;
let mind1 = -1;
let minvX1;
let minvY1;
let minvZ1;
let mini1 = 0;
minvX1 = 0;
minvY1 = 0;
minvZ1 = 0;
if(v1X * n12X + v1Y * n12Y + v1Z * n12Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec3.x;
v2Y = vec3.y;
v2Z = vec3.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
mini1 = b;
mind1 = out.x * out.x + out.y * out.y + out.z * out.z;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
if(v2X * n23X + v2Y * n23Y + v2Z * n23Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec3.x;
v1Y = vec3.y;
v1Z = vec3.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind1 < 0 || d < mind1) {
mini1 = b << 1;
mind1 = d;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
}
if(v3X * n31X + v3Y * n31Y + v3Z * n31Z < 0) {
let v1X;
let v1Y;
let v1Z;
let v2X;
let v2Y;
let v2Z;
v1X = vec2.x;
v1Y = vec2.y;
v1Z = vec2.z;
v2X = vec4.x;
v2Y = vec4.y;
v2Z = vec4.z;
let v12X;
let v12Y;
let v12Z;
v12X = v2X - v1X;
v12Y = v2Y - v1Y;
v12Z = v2Z - v1Z;
let t = v12X * v1X + v12Y * v1Y + v12Z * v1Z;
t = -t / (v12X * v12X + v12Y * v12Y + v12Z * v12Z);
let b;
if(t < 0) {
out.x = v1X;
out.y = v1Y;
out.z = v1Z;
b = 1;
} else if(t > 1) {
out.x = v2X;
out.y = v2Y;
out.z = v2Z;
b = 2;
} else {
let pX;
let pY;
let pZ;
pX = v1X + v12X * t;
pY = v1Y + v12Y * t;
pZ = v1Z + v12Z * t;
out.x = pX;
out.y = pY;
out.z = pZ;
b = 3;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind1 < 0 || d < mind1) {
mini1 = b & 1 | (b & 2) << 1;
mind1 = d;
minvX1 = out.x;
minvY1 = out.y;
minvZ1 = out.z;
}
}
let b;
if(mind1 > 0) {
out.x = minvX1;
out.y = minvY1;
out.z = minvZ1;
b = mini1;
} else {
let l = nX * nX + nY * nY + nZ * nZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
nX *= l;
nY *= l;
nZ *= l;
let l2 = nX * nX + nY * nY + nZ * nZ;
l2 = (v1X * nX + v1Y * nY + v1Z * nZ) / l2;
minvX1 = nX * l2;
minvY1 = nY * l2;
minvZ1 = nZ * l2;
out.x = minvX1;
out.y = minvY1;
out.z = minvZ1;
b = 7;
}
let d = out.x * out.x + out.y * out.y + out.z * out.z;
if(mind < 0 || d < mind) {
mini = b << 1;
mind = d;
minvX = out.x;
minvY = out.y;
minvZ = out.z;
}
}
if(mind > 0) {
out.x = minvX;
out.y = minvY;
out.z = minvZ;
return mini;
}
out.zero();
return 15;
}
}
oimo.common.Mat3 = class oimo_common_Mat3 {
constructor(e00,e01,e02,e10,e11,e12,e20,e21,e22) {
if(e22 == null) {
e22 = 1;
}
if(e21 == null) {
e21 = 0;
}
if(e20 == null) {
e20 = 0;
}
if(e12 == null) {
e12 = 0;
}
if(e11 == null) {
e11 = 1;
}
if(e10 == null) {
e10 = 0;
}
if(e02 == null) {
e02 = 0;
}
if(e01 == null) {
e01 = 0;
}
if(e00 == null) {
e00 = 1;
}
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
oimo.common.Mat3.numCreations++;
}
init(e00,e01,e02,e10,e11,e12,e20,e21,e22) {
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
return this;
}
identity() {
this.e00 = 1;
this.e01 = 0;
this.e02 = 0;
this.e10 = 0;
this.e11 = 1;
this.e12 = 0;
this.e20 = 0;
this.e21 = 0;
this.e22 = 1;
return this;
}
add(m) {
return new oimo.common.Mat3(this.e00 + m.e00,this.e01 + m.e01,this.e02 + m.e02,this.e10 + m.e10,this.e11 + m.e11,this.e12 + m.e12,this.e20 + m.e20,this.e21 + m.e21,this.e22 + m.e22);
}
sub(m) {
return new oimo.common.Mat3(this.e00 - m.e00,this.e01 - m.e01,this.e02 - m.e02,this.e10 - m.e10,this.e11 - m.e11,this.e12 - m.e12,this.e20 - m.e20,this.e21 - m.e21,this.e22 - m.e22);
}
scale(s) {
return new oimo.common.Mat3(this.e00 * s,this.e01 * s,this.e02 * s,this.e10 * s,this.e11 * s,this.e12 * s,this.e20 * s,this.e21 * s,this.e22 * s);
}
mul(m) {
return new oimo.common.Mat3(this.e00 * m.e00 + this.e01 * m.e10 + this.e02 * m.e20,this.e00 * m.e01 + this.e01 * m.e11 + this.e02 * m.e21,this.e00 * m.e02 + this.e01 * m.e12 + this.e02 * m.e22,this.e10 * m.e00 + this.e11 * m.e10 + this.e12 * m.e20,this.e10 * m.e01 + this.e11 * m.e11 + this.e12 * m.e21,this.e10 * m.e02 + this.e11 * m.e12 + this.e12 * m.e22,this.e20 * m.e00 + this.e21 * m.e10 + this.e22 * m.e20,this.e20 * m.e01 + this.e21 * m.e11 + this.e22 * m.e21,this.e20 * m.e02 + this.e21 * m.e12 + this.e22 * m.e22);
}
addEq(m) {
this.e00 += m.e00;
this.e01 += m.e01;
this.e02 += m.e02;
this.e10 += m.e10;
this.e11 += m.e11;
this.e12 += m.e12;
this.e20 += m.e20;
this.e21 += m.e21;
this.e22 += m.e22;
return this;
}
subEq(m) {
this.e00 -= m.e00;
this.e01 -= m.e01;
this.e02 -= m.e02;
this.e10 -= m.e10;
this.e11 -= m.e11;
this.e12 -= m.e12;
this.e20 -= m.e20;
this.e21 -= m.e21;
this.e22 -= m.e22;
return this;
}
scaleEq(s) {
this.e00 *= s;
this.e01 *= s;
this.e02 *= s;
this.e10 *= s;
this.e11 *= s;
this.e12 *= s;
this.e20 *= s;
this.e21 *= s;
this.e22 *= s;
return this;
}
mulEq(m) {
let e01 = this.e00 * m.e01 + this.e01 * m.e11 + this.e02 * m.e21;
let e02 = this.e00 * m.e02 + this.e01 * m.e12 + this.e02 * m.e22;
let e10 = this.e10 * m.e00 + this.e11 * m.e10 + this.e12 * m.e20;
let e11 = this.e10 * m.e01 + this.e11 * m.e11 + this.e12 * m.e21;
let e12 = this.e10 * m.e02 + this.e11 * m.e12 + this.e12 * m.e22;
let e20 = this.e20 * m.e00 + this.e21 * m.e10 + this.e22 * m.e20;
let e21 = this.e20 * m.e01 + this.e21 * m.e11 + this.e22 * m.e21;
let e22 = this.e20 * m.e02 + this.e21 * m.e12 + this.e22 * m.e22;
this.e00 = this.e00 * m.e00 + this.e01 * m.e10 + this.e02 * m.e20;
this.e01 = e01;
this.e02 = e02;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
return this;
}
prependScale(sx,sy,sz) {
return new oimo.common.Mat3(this.e00 * sx,this.e01 * sx,this.e02 * sx,this.e10 * sy,this.e11 * sy,this.e12 * sy,this.e20 * sz,this.e21 * sz,this.e22 * sz);
}
appendScale(sx,sy,sz) {
return new oimo.common.Mat3(this.e00 * sx,this.e01 * sy,this.e02 * sz,this.e10 * sx,this.e11 * sy,this.e12 * sz,this.e20 * sx,this.e21 * sy,this.e22 * sz);
}
prependRotation(rad,axisX,axisY,axisZ) {
let s = Math.sin(rad);
let c = Math.cos(rad);
let c1 = 1 - c;
let r00 = axisX * axisX * c1 + c;
let r01 = axisX * axisY * c1 - axisZ * s;
let r02 = axisX * axisZ * c1 + axisY * s;
let r10 = axisY * axisX * c1 + axisZ * s;
let r11 = axisY * axisY * c1 + c;
let r12 = axisY * axisZ * c1 - axisX * s;
let r20 = axisZ * axisX * c1 - axisY * s;
let r21 = axisZ * axisY * c1 + axisX * s;
let r22 = axisZ * axisZ * c1 + c;
return new oimo.common.Mat3(r00 * this.e00 + r01 * this.e10 + r02 * this.e20,r00 * this.e01 + r01 * this.e11 + r02 * this.e21,r00 * this.e02 + r01 * this.e12 + r02 * this.e22,r10 * this.e00 + r11 * this.e10 + r12 * this.e20,r10 * this.e01 + r11 * this.e11 + r12 * this.e21,r10 * this.e02 + r11 * this.e12 + r12 * this.e22,r20 * this.e00 + r21 * this.e10 + r22 * this.e20,r20 * this.e01 + r21 * this.e11 + r22 * this.e21,r20 * this.e02 + r21 * this.e12 + r22 * this.e22);
}
appendRotation(rad,axisX,axisY,axisZ) {
let s = Math.sin(rad);
let c = Math.cos(rad);
let c1 = 1 - c;
let r00 = axisX * axisX * c1 + c;
let r01 = axisX * axisY * c1 - axisZ * s;
let r02 = axisX * axisZ * c1 + axisY * s;
let r10 = axisY * axisX * c1 + axisZ * s;
let r11 = axisY * axisY * c1 + c;
let r12 = axisY * axisZ * c1 - axisX * s;
let r20 = axisZ * axisX * c1 - axisY * s;
let r21 = axisZ * axisY * c1 + axisX * s;
let r22 = axisZ * axisZ * c1 + c;
return new oimo.common.Mat3(this.e00 * r00 + this.e01 * r10 + this.e02 * r20,this.e00 * r01 + this.e01 * r11 + this.e02 * r21,this.e00 * r02 + this.e01 * r12 + this.e02 * r22,this.e10 * r00 + this.e11 * r10 + this.e12 * r20,this.e10 * r01 + this.e11 * r11 + this.e12 * r21,this.e10 * r02 + this.e11 * r12 + this.e12 * r22,this.e20 * r00 + this.e21 * r10 + this.e22 * r20,this.e20 * r01 + this.e21 * r11 + this.e22 * r21,this.e20 * r02 + this.e21 * r12 + this.e22 * r22);
}
prependScaleEq(sx,sy,sz) {
this.e00 *= sx;
this.e01 *= sx;
this.e02 *= sx;
this.e10 *= sy;
this.e11 *= sy;
this.e12 *= sy;
this.e20 *= sz;
this.e21 *= sz;
this.e22 *= sz;
return this;
}
appendScaleEq(sx,sy,sz) {
this.e00 *= sx;
this.e01 *= sy;
this.e02 *= sz;
this.e10 *= sx;
this.e11 *= sy;
this.e12 *= sz;
this.e20 *= sx;
this.e21 *= sy;
this.e22 *= sz;
return this;
}
prependRotationEq(rad,axisX,axisY,axisZ) {
let s = Math.sin(rad);
let c = Math.cos(rad);
let c1 = 1 - c;
let r00 = axisX * axisX * c1 + c;
let r01 = axisX * axisY * c1 - axisZ * s;
let r02 = axisX * axisZ * c1 + axisY * s;
let r10 = axisY * axisX * c1 + axisZ * s;
let r11 = axisY * axisY * c1 + c;
let r12 = axisY * axisZ * c1 - axisX * s;
let r20 = axisZ * axisX * c1 - axisY * s;
let r21 = axisZ * axisY * c1 + axisX * s;
let r22 = axisZ * axisZ * c1 + c;
let e10 = r10 * this.e00 + r11 * this.e10 + r12 * this.e20;
let e11 = r10 * this.e01 + r11 * this.e11 + r12 * this.e21;
let e12 = r10 * this.e02 + r11 * this.e12 + r12 * this.e22;
let e20 = r20 * this.e00 + r21 * this.e10 + r22 * this.e20;
let e21 = r20 * this.e01 + r21 * this.e11 + r22 * this.e21;
let e22 = r20 * this.e02 + r21 * this.e12 + r22 * this.e22;
this.e00 = r00 * this.e00 + r01 * this.e10 + r02 * this.e20;
this.e01 = r00 * this.e01 + r01 * this.e11 + r02 * this.e21;
this.e02 = r00 * this.e02 + r01 * this.e12 + r02 * this.e22;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
return this;
}
appendRotationEq(rad,axisX,axisY,axisZ) {
let s = Math.sin(rad);
let c = Math.cos(rad);
let c1 = 1 - c;
let r00 = axisX * axisX * c1 + c;
let r01 = axisX * axisY * c1 - axisZ * s;
let r02 = axisX * axisZ * c1 + axisY * s;
let r10 = axisY * axisX * c1 + axisZ * s;
let r11 = axisY * axisY * c1 + c;
let r12 = axisY * axisZ * c1 - axisX * s;
let r20 = axisZ * axisX * c1 - axisY * s;
let r21 = axisZ * axisY * c1 + axisX * s;
let r22 = axisZ * axisZ * c1 + c;
let e01 = this.e00 * r01 + this.e01 * r11 + this.e02 * r21;
let e02 = this.e00 * r02 + this.e01 * r12 + this.e02 * r22;
let e11 = this.e10 * r01 + this.e11 * r11 + this.e12 * r21;
let e12 = this.e10 * r02 + this.e11 * r12 + this.e12 * r22;
let e21 = this.e20 * r01 + this.e21 * r11 + this.e22 * r21;
let e22 = this.e20 * r02 + this.e21 * r12 + this.e22 * r22;
this.e00 = this.e00 * r00 + this.e01 * r10 + this.e02 * r20;
this.e01 = e01;
this.e02 = e02;
this.e10 = this.e10 * r00 + this.e11 * r10 + this.e12 * r20;
this.e11 = e11;
this.e12 = e12;
this.e20 = this.e20 * r00 + this.e21 * r10 + this.e22 * r20;
this.e21 = e21;
this.e22 = e22;
return this;
}
transpose() {
return new oimo.common.Mat3(this.e00,this.e10,this.e20,this.e01,this.e11,this.e21,this.e02,this.e12,this.e22);
}
transposeEq() {
let e10 = this.e01;
let e20 = this.e02;
let e21 = this.e12;
this.e01 = this.e10;
this.e02 = this.e20;
this.e10 = e10;
this.e12 = this.e21;
this.e20 = e20;
this.e21 = e21;
return this;
}
determinant() {
return this.e00 * (this.e11 * this.e22 - this.e12 * this.e21) - this.e01 * (this.e10 * this.e22 - this.e12 * this.e20) + this.e02 * (this.e10 * this.e21 - this.e11 * this.e20);
}
trace() {
return this.e00 + this.e11 + this.e22;
}
inverse() {
let d00 = this.e11 * this.e22 - this.e12 * this.e21;
let d01 = this.e10 * this.e22 - this.e12 * this.e20;
let d02 = this.e10 * this.e21 - this.e11 * this.e20;
let invDet = this.e00 * d00 - this.e01 * d01 + this.e02 * d02;
if(invDet != 0) {
invDet = 1 / invDet;
}
return new oimo.common.Mat3(d00 * invDet,-(this.e01 * this.e22 - this.e02 * this.e21) * invDet,(this.e01 * this.e12 - this.e02 * this.e11) * invDet,-d01 * invDet,(this.e00 * this.e22 - this.e02 * this.e20) * invDet,-(this.e00 * this.e12 - this.e02 * this.e10) * invDet,d02 * invDet,-(this.e00 * this.e21 - this.e01 * this.e20) * invDet,(this.e00 * this.e11 - this.e01 * this.e10) * invDet);
}
inverseEq() {
let d00 = this.e11 * this.e22 - this.e12 * this.e21;
let d01 = this.e10 * this.e22 - this.e12 * this.e20;
let d02 = this.e10 * this.e21 - this.e11 * this.e20;
let invDet = this.e00 * d00 - this.e01 * d01 + this.e02 * d02;
if(invDet != 0) {
invDet = 1 / invDet;
}
let t02 = (this.e01 * this.e12 - this.e02 * this.e11) * invDet;
let t11 = (this.e00 * this.e22 - this.e02 * this.e20) * invDet;
let t12 = -(this.e00 * this.e12 - this.e02 * this.e10) * invDet;
let t21 = -(this.e00 * this.e21 - this.e01 * this.e20) * invDet;
let t22 = (this.e00 * this.e11 - this.e01 * this.e10) * invDet;
this.e00 = d00 * invDet;
this.e01 = -(this.e01 * this.e22 - this.e02 * this.e21) * invDet;
this.e02 = t02;
this.e10 = -d01 * invDet;
this.e11 = t11;
this.e12 = t12;
this.e20 = d02 * invDet;
this.e21 = t21;
this.e22 = t22;
return this;
}
toArray(columnMajor) {
if(columnMajor == null) {
columnMajor = false;
}
if(columnMajor) {
return [this.e00,this.e10,this.e20,this.e01,this.e11,this.e21,this.e02,this.e12,this.e22];
} else {
return [this.e00,this.e01,this.e02,this.e10,this.e11,this.e12,this.e20,this.e21,this.e22];
}
}
copyFrom(m) {
this.e00 = m.e00;
this.e01 = m.e01;
this.e02 = m.e02;
this.e10 = m.e10;
this.e11 = m.e11;
this.e12 = m.e12;
this.e20 = m.e20;
this.e21 = m.e21;
this.e22 = m.e22;
return this;
}
clone() {
return new oimo.common.Mat3(this.e00,this.e01,this.e02,this.e10,this.e11,this.e12,this.e20,this.e21,this.e22);
}
fromQuat(q) {
let x = q.x;
let y = q.y;
let z = q.z;
let w = q.w;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
this.e00 = 1 - yy - zz;
this.e01 = xy - wz;
this.e02 = xz + wy;
this.e10 = xy + wz;
this.e11 = 1 - xx - zz;
this.e12 = yz - wx;
this.e20 = xz - wy;
this.e21 = yz + wx;
this.e22 = 1 - xx - yy;
return this;
}
toQuat() {
let _this = new oimo.common.Quat();
let e00 = this.e00;
let e11 = this.e11;
let e22 = this.e22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
_this.w = 0.5 * s;
s = 0.5 / s;
_this.x = (this.e21 - this.e12) * s;
_this.y = (this.e02 - this.e20) * s;
_this.z = (this.e10 - this.e01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
_this.x = 0.5 * s;
s = 0.5 / s;
_this.y = (this.e01 + this.e10) * s;
_this.z = (this.e02 + this.e20) * s;
_this.w = (this.e21 - this.e12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
_this.z = 0.5 * s;
s = 0.5 / s;
_this.x = (this.e02 + this.e20) * s;
_this.y = (this.e12 + this.e21) * s;
_this.w = (this.e10 - this.e01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
_this.y = 0.5 * s;
s = 0.5 / s;
_this.x = (this.e01 + this.e10) * s;
_this.z = (this.e12 + this.e21) * s;
_this.w = (this.e02 - this.e20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
_this.z = 0.5 * s;
s = 0.5 / s;
_this.x = (this.e02 + this.e20) * s;
_this.y = (this.e12 + this.e21) * s;
_this.w = (this.e10 - this.e01) * s;
}
return _this;
}
fromEulerXyz(eulerAngles) {
let sx = Math.sin(eulerAngles.x);
let sy = Math.sin(eulerAngles.y);
let sz = Math.sin(eulerAngles.z);
let cx = Math.cos(eulerAngles.x);
let cy = Math.cos(eulerAngles.y);
let cz = Math.cos(eulerAngles.z);
this.e00 = cy * cz;
this.e01 = -cy * sz;
this.e02 = sy;
this.e10 = cx * sz + cz * sx * sy;
this.e11 = cx * cz - sx * sy * sz;
this.e12 = -cy * sx;
this.e20 = sx * sz - cx * cz * sy;
this.e21 = cz * sx + cx * sy * sz;
this.e22 = cx * cy;
return this;
}
toEulerXyz() {
let sy = this.e02;
if(sy <= -1) {
let xSubZ = Math.atan2(this.e21,this.e11);
return new oimo.common.Vec3(xSubZ * 0.5,-1.570796326794895,-xSubZ * 0.5);
}
if(sy >= 1) {
let xAddZ = Math.atan2(this.e21,this.e11);
return new oimo.common.Vec3(xAddZ * 0.5,1.570796326794895,xAddZ * 0.5);
}
return new oimo.common.Vec3(Math.atan2(-this.e12,this.e22),Math.asin(sy),Math.atan2(-this.e01,this.e00));
}
getRow(index) {
if(index == 0) {
return new oimo.common.Vec3(this.e00,this.e01,this.e02);
} else if(index == 1) {
return new oimo.common.Vec3(this.e10,this.e11,this.e12);
} else if(index == 2) {
return new oimo.common.Vec3(this.e20,this.e21,this.e22);
} else {
return null;
}
}
getCol(index) {
if(index == 0) {
return new oimo.common.Vec3(this.e00,this.e10,this.e20);
} else if(index == 1) {
return new oimo.common.Vec3(this.e01,this.e11,this.e21);
} else if(index == 2) {
return new oimo.common.Vec3(this.e02,this.e12,this.e22);
} else {
return null;
}
}
getRowTo(index,dst) {
if(index == 0) {
dst.init(this.e00,this.e01,this.e02);
} else if(index == 1) {
dst.init(this.e10,this.e11,this.e12);
} else if(index == 2) {
dst.init(this.e20,this.e21,this.e22);
} else {
dst.zero();
}
}
getColTo(index,dst) {
if(index == 0) {
dst.init(this.e00,this.e10,this.e20);
} else if(index == 1) {
dst.init(this.e01,this.e11,this.e21);
} else if(index == 2) {
dst.init(this.e02,this.e12,this.e22);
} else {
dst.zero();
}
}
fromRows(row0,row1,row2) {
this.e00 = row0.x;
this.e01 = row0.y;
this.e02 = row0.z;
this.e10 = row1.x;
this.e11 = row1.y;
this.e12 = row1.z;
this.e20 = row2.x;
this.e21 = row2.y;
this.e22 = row2.z;
return this;
}
fromCols(col0,col1,col2) {
this.e00 = col0.x;
this.e01 = col1.x;
this.e02 = col2.x;
this.e10 = col0.y;
this.e11 = col1.y;
this.e12 = col2.y;
this.e20 = col0.z;
this.e21 = col1.z;
this.e22 = col2.z;
return this;
}
toString() {
return "Mat3[" + (this.e00 > 0 ? (this.e00 * 10000000 + 0.5 | 0) / 10000000 : (this.e00 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e01 > 0 ? (this.e01 * 10000000 + 0.5 | 0) / 10000000 : (this.e01 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e02 > 0 ? (this.e02 * 10000000 + 0.5 | 0) / 10000000 : (this.e02 * 10000000 - 0.5 | 0) / 10000000) + ",\n" + " " + (this.e10 > 0 ? (this.e10 * 10000000 + 0.5 | 0) / 10000000 : (this.e10 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e11 > 0 ? (this.e11 * 10000000 + 0.5 | 0) / 10000000 : (this.e11 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e12 > 0 ? (this.e12 * 10000000 + 0.5 | 0) / 10000000 : (this.e12 * 10000000 - 0.5 | 0) / 10000000) + ",\n" + " " + (this.e20 > 0 ? (this.e20 * 10000000 + 0.5 | 0) / 10000000 : (this.e20 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e21 > 0 ? (this.e21 * 10000000 + 0.5 | 0) / 10000000 : (this.e21 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e22 > 0 ? (this.e22 * 10000000 + 0.5 | 0) / 10000000 : (this.e22 * 10000000 - 0.5 | 0) / 10000000) + "]";
}
}
oimo.common.Mat4 = class oimo_common_Mat4 {
constructor(e00,e01,e02,e03,e10,e11,e12,e13,e20,e21,e22,e23,e30,e31,e32,e33) {
if(e33 == null) {
e33 = 1;
}
if(e32 == null) {
e32 = 0;
}
if(e31 == null) {
e31 = 0;
}
if(e30 == null) {
e30 = 0;
}
if(e23 == null) {
e23 = 0;
}
if(e22 == null) {
e22 = 1;
}
if(e21 == null) {
e21 = 0;
}
if(e20 == null) {
e20 = 0;
}
if(e13 == null) {
e13 = 0;
}
if(e12 == null) {
e12 = 0;
}
if(e11 == null) {
e11 = 1;
}
if(e10 == null) {
e10 = 0;
}
if(e03 == null) {
e03 = 0;
}
if(e02 == null) {
e02 = 0;
}
if(e01 == null) {
e01 = 0;
}
if(e00 == null) {
e00 = 1;
}
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
oimo.common.Mat4.numCreations++;
}
init(e00,e01,e02,e03,e10,e11,e12,e13,e20,e21,e22,e23,e30,e31,e32,e33) {
this.e00 = e00;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
return this;
}
identity() {
this.e00 = 1;
this.e01 = 0;
this.e02 = 0;
this.e03 = 0;
this.e10 = 0;
this.e11 = 1;
this.e12 = 0;
this.e13 = 0;
this.e20 = 0;
this.e21 = 0;
this.e22 = 1;
this.e23 = 0;
this.e30 = 0;
this.e31 = 0;
this.e32 = 0;
this.e33 = 1;
return this;
}
add(m) {
return new oimo.common.Mat4(this.e00 + m.e00,this.e01 + m.e01,this.e02 + m.e02,this.e03 + m.e03,this.e10 + m.e10,this.e11 + m.e11,this.e12 + m.e12,this.e13 + m.e13,this.e20 + m.e20,this.e21 + m.e21,this.e22 + m.e22,this.e23 + m.e23,this.e30 + m.e30,this.e31 + m.e31,this.e32 + m.e32,this.e33 + m.e33);
}
sub(m) {
return new oimo.common.Mat4(this.e00 - m.e00,this.e01 - m.e01,this.e02 - m.e02,this.e03 - m.e03,this.e10 - m.e10,this.e11 - m.e11,this.e12 - m.e12,this.e13 - m.e13,this.e20 - m.e20,this.e21 - m.e21,this.e22 - m.e22,this.e23 - m.e23,this.e30 - m.e30,this.e31 - m.e31,this.e32 - m.e32,this.e33 - m.e33);
}
scale(s) {
return new oimo.common.Mat4(this.e00 * s,this.e01 * s,this.e02 * s,this.e03 * s,this.e10 * s,this.e11 * s,this.e12 * s,this.e13 * s,this.e20 * s,this.e21 * s,this.e22 * s,this.e23 * s,this.e30 * s,this.e31 * s,this.e32 * s,this.e33 * s);
}
mul(m) {
return new oimo.common.Mat4(this.e00 * m.e00 + this.e01 * m.e10 + this.e02 * m.e20 + this.e03 * m.e30,this.e00 * m.e01 + this.e01 * m.e11 + this.e02 * m.e21 + this.e03 * m.e31,this.e00 * m.e02 + this.e01 * m.e12 + this.e02 * m.e22 + this.e03 * m.e32,this.e00 * m.e03 + this.e01 * m.e13 + this.e02 * m.e23 + this.e03 * m.e33,this.e10 * m.e00 + this.e11 * m.e10 + this.e12 * m.e20 + this.e13 * m.e30,this.e10 * m.e01 + this.e11 * m.e11 + this.e12 * m.e21 + this.e13 * m.e31,this.e10 * m.e02 + this.e11 * m.e12 + this.e12 * m.e22 + this.e13 * m.e32,this.e10 * m.e03 + this.e11 * m.e13 + this.e12 * m.e23 + this.e13 * m.e33,this.e20 * m.e00 + this.e21 * m.e10 + this.e22 * m.e20 + this.e23 * m.e30,this.e20 * m.e01 + this.e21 * m.e11 + this.e22 * m.e21 + this.e23 * m.e31,this.e20 * m.e02 + this.e21 * m.e12 + this.e22 * m.e22 + this.e23 * m.e32,this.e20 * m.e03 + this.e21 * m.e13 + this.e22 * m.e23 + this.e23 * m.e33,this.e30 * m.e00 + this.e31 * m.e10 + this.e32 * m.e20 + this.e33 * m.e30,this.e30 * m.e01 + this.e31 * m.e11 + this.e32 * m.e21 + this.e33 * m.e31,this.e30 * m.e02 + this.e31 * m.e12 + this.e32 * m.e22 + this.e33 * m.e32,this.e30 * m.e03 + this.e31 * m.e13 + this.e32 * m.e23 + this.e33 * m.e33);
}
addEq(m) {
this.e00 += m.e00;
this.e01 += m.e01;
this.e02 += m.e02;
this.e03 += m.e03;
this.e10 += m.e10;
this.e11 += m.e11;
this.e12 += m.e12;
this.e13 += m.e13;
this.e20 += m.e20;
this.e21 += m.e21;
this.e22 += m.e22;
this.e23 += m.e23;
this.e30 += m.e30;
this.e31 += m.e31;
this.e32 += m.e32;
this.e33 += m.e33;
return this;
}
subEq(m) {
this.e00 -= m.e00;
this.e01 -= m.e01;
this.e02 -= m.e02;
this.e03 -= m.e03;
this.e10 -= m.e10;
this.e11 -= m.e11;
this.e12 -= m.e12;
this.e13 -= m.e13;
this.e20 -= m.e20;
this.e21 -= m.e21;
this.e22 -= m.e22;
this.e23 -= m.e23;
this.e30 -= m.e30;
this.e31 -= m.e31;
this.e32 -= m.e32;
this.e33 -= m.e33;
return this;
}
scaleEq(s) {
this.e00 *= s;
this.e01 *= s;
this.e02 *= s;
this.e03 *= s;
this.e10 *= s;
this.e11 *= s;
this.e12 *= s;
this.e13 *= s;
this.e20 *= s;
this.e21 *= s;
this.e22 *= s;
this.e23 *= s;
this.e30 *= s;
this.e31 *= s;
this.e32 *= s;
this.e33 *= s;
return this;
}
mulEq(m) {
let e01 = this.e00 * m.e01 + this.e01 * m.e11 + this.e02 * m.e21 + this.e03 * m.e31;
let e02 = this.e00 * m.e02 + this.e01 * m.e12 + this.e02 * m.e22 + this.e03 * m.e32;
let e03 = this.e00 * m.e03 + this.e01 * m.e13 + this.e02 * m.e23 + this.e03 * m.e33;
let e10 = this.e10 * m.e00 + this.e11 * m.e10 + this.e12 * m.e20 + this.e13 * m.e30;
let e11 = this.e10 * m.e01 + this.e11 * m.e11 + this.e12 * m.e21 + this.e13 * m.e31;
let e12 = this.e10 * m.e02 + this.e11 * m.e12 + this.e12 * m.e22 + this.e13 * m.e32;
let e13 = this.e10 * m.e03 + this.e11 * m.e13 + this.e12 * m.e23 + this.e13 * m.e33;
let e20 = this.e20 * m.e00 + this.e21 * m.e10 + this.e22 * m.e20 + this.e23 * m.e30;
let e21 = this.e20 * m.e01 + this.e21 * m.e11 + this.e22 * m.e21 + this.e23 * m.e31;
let e22 = this.e20 * m.e02 + this.e21 * m.e12 + this.e22 * m.e22 + this.e23 * m.e32;
let e23 = this.e20 * m.e03 + this.e21 * m.e13 + this.e22 * m.e23 + this.e23 * m.e33;
let e30 = this.e30 * m.e00 + this.e31 * m.e10 + this.e32 * m.e20 + this.e33 * m.e30;
let e31 = this.e30 * m.e01 + this.e31 * m.e11 + this.e32 * m.e21 + this.e33 * m.e31;
let e32 = this.e30 * m.e02 + this.e31 * m.e12 + this.e32 * m.e22 + this.e33 * m.e32;
let e33 = this.e30 * m.e03 + this.e31 * m.e13 + this.e32 * m.e23 + this.e33 * m.e33;
this.e00 = this.e00 * m.e00 + this.e01 * m.e10 + this.e02 * m.e20 + this.e03 * m.e30;
this.e01 = e01;
this.e02 = e02;
this.e03 = e03;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
this.e33 = e33;
return this;
}
prependScale(sx,sy,sz) {
return new oimo.common.Mat4(this.e00 * sx,this.e01 * sx,this.e02 * sx,this.e03 * sx,this.e10 * sy,this.e11 * sy,this.e12 * sy,this.e13 * sy,this.e20 * sz,this.e21 * sz,this.e22 * sz,this.e23 * sz,this.e30,this.e31,this.e32,this.e33);
}
appendScale(sx,sy,sz) {
return new oimo.common.Mat4(this.e00 * sx,this.e01 * sy,this.e02 * sz,this.e03,this.e10 * sx,this.e11 * sy,this.e12 * sz,this.e13,this.e20 * sx,this.e21 * sy,this.e22 * sz,this.e23,this.e30 * sx,this.e31 * sy,this.e32 * sz,this.e33);
}
prependRotation(rad,axisX,axisY,axisZ) {
let s = Math.sin(rad);
let c = Math.cos(rad);
let c1 = 1 - c;
let r00 = axisX * axisX * c1 + c;
let r01 = axisX * axisY * c1 - axisZ * s;
let r02 = axisX * axisZ * c1 + axisY * s;
let r10 = axisY * axisX * c1 + axisZ * s;
let r11 = axisY * axisY * c1 + c;
let r12 = axisY * axisZ * c1 - axisX * s;
let r20 = axisZ * axisX * c1 - axisY * s;
let r21 = axisZ * axisY * c1 + axisX * s;
let r22 = axisZ * axisZ * c1 + c;
return new oimo.common.Mat4(r00 * this.e00 + r01 * this.e10 + r02 * this.e20,r00 * this.e01 + r01 * this.e11 + r02 * this.e21,r00 * this.e02 + r01 * this.e12 + r02 * this.e22,r00 * this.e03 + r01 * this.e13 + r02 * this.e23,r10 * this.e00 + r11 * this.e10 + r12 * this.e20,r10 * this.e01 + r11 * this.e11 + r12 * this.e21,r10 * this.e02 + r11 * this.e12 + r12 * this.e22,r10 * this.e03 + r11 * this.e13 + r12 * this.e23,r20 * this.e00 + r21 * this.e10 + r22 * this.e20,r20 * this.e01 + r21 * this.e11 + r22 * this.e21,r20 * this.e02 + r21 * this.e12 + r22 * this.e22,r20 * this.e03 + r21 * this.e13 + r22 * this.e23,this.e30,this.e31,this.e32,this.e33);
}
appendRotation(rad,axisX,axisY,axisZ) {
let s = Math.sin(rad);
let c = Math.cos(rad);
let c1 = 1 - c;
let r00 = axisX * axisX * c1 + c;
let r01 = axisX * axisY * c1 - axisZ * s;
let r02 = axisX * axisZ * c1 + axisY * s;
let r10 = axisY * axisX * c1 + axisZ * s;
let r11 = axisY * axisY * c1 + c;
let r12 = axisY * axisZ * c1 - axisX * s;
let r20 = axisZ * axisX * c1 - axisY * s;
let r21 = axisZ * axisY * c1 + axisX * s;
let r22 = axisZ * axisZ * c1 + c;
return new oimo.common.Mat4(this.e00 * r00 + this.e01 * r10 + this.e02 * r20,this.e00 * r01 + this.e01 * r11 + this.e02 * r21,this.e00 * r02 + this.e01 * r12 + this.e02 * r22,this.e03,this.e10 * r00 + this.e11 * r10 + this.e12 * r20,this.e10 * r01 + this.e11 * r11 + this.e12 * r21,this.e10 * r02 + this.e11 * r12 + this.e12 * r22,this.e13,this.e20 * r00 + this.e21 * r10 + this.e22 * r20,this.e20 * r01 + this.e21 * r11 + this.e22 * r21,this.e20 * r02 + this.e21 * r12 + this.e22 * r22,this.e23,this.e30 * r00 + this.e31 * r10 + this.e32 * r20,this.e30 * r01 + this.e31 * r11 + this.e32 * r21,this.e30 * r02 + this.e31 * r12 + this.e32 * r22,this.e33);
}
prependTranslation(tx,ty,tz) {
return new oimo.common.Mat4(this.e00 + tx * this.e30,this.e01 + tx * this.e31,this.e02 + tx * this.e32,this.e03 + tx * this.e33,this.e10 + ty * this.e30,this.e11 + ty * this.e31,this.e12 + ty * this.e32,this.e13 + ty * this.e33,this.e20 + tz * this.e30,this.e21 + tz * this.e31,this.e22 + tz * this.e32,this.e23 + tz * this.e33,this.e30,this.e31,this.e32,this.e33);
}
appendTranslation(tx,ty,tz) {
return new oimo.common.Mat4(this.e00,this.e01,this.e02,this.e00 * tx + this.e01 * ty + this.e02 * tz + this.e03,this.e10,this.e11,this.e12,this.e10 * tx + this.e11 * ty + this.e12 * tz + this.e13,this.e20,this.e21,this.e22,this.e20 * tx + this.e21 * ty + this.e22 * tz + this.e23,this.e30,this.e31,this.e32,this.e30 * tx + this.e31 * ty + this.e32 * tz + this.e33);
}
prependScaleEq(sx,sy,sz) {
this.e00 *= sx;
this.e01 *= sx;
this.e02 *= sx;
this.e03 *= sx;
this.e10 *= sy;
this.e11 *= sy;
this.e12 *= sy;
this.e13 *= sy;
this.e20 *= sz;
this.e21 *= sz;
this.e22 *= sz;
this.e23 *= sz;
return this;
}
appendScaleEq(sx,sy,sz) {
this.e00 *= sx;
this.e01 *= sy;
this.e02 *= sz;
this.e10 *= sx;
this.e11 *= sy;
this.e12 *= sz;
this.e20 *= sx;
this.e21 *= sy;
this.e22 *= sz;
this.e30 *= sx;
this.e31 *= sy;
this.e32 *= sz;
return this;
}
prependRotationEq(rad,axisX,axisY,axisZ) {
let s = Math.sin(rad);
let c = Math.cos(rad);
let c1 = 1 - c;
let r00 = axisX * axisX * c1 + c;
let r01 = axisX * axisY * c1 - axisZ * s;
let r02 = axisX * axisZ * c1 + axisY * s;
let r10 = axisY * axisX * c1 + axisZ * s;
let r11 = axisY * axisY * c1 + c;
let r12 = axisY * axisZ * c1 - axisX * s;
let r20 = axisZ * axisX * c1 - axisY * s;
let r21 = axisZ * axisY * c1 + axisX * s;
let r22 = axisZ * axisZ * c1 + c;
let e10 = r10 * this.e00 + r11 * this.e10 + r12 * this.e20;
let e11 = r10 * this.e01 + r11 * this.e11 + r12 * this.e21;
let e12 = r10 * this.e02 + r11 * this.e12 + r12 * this.e22;
let e13 = r10 * this.e03 + r11 * this.e13 + r12 * this.e23;
let e20 = r20 * this.e00 + r21 * this.e10 + r22 * this.e20;
let e21 = r20 * this.e01 + r21 * this.e11 + r22 * this.e21;
let e22 = r20 * this.e02 + r21 * this.e12 + r22 * this.e22;
let e23 = r20 * this.e03 + r21 * this.e13 + r22 * this.e23;
this.e00 = r00 * this.e00 + r01 * this.e10 + r02 * this.e20;
this.e01 = r00 * this.e01 + r01 * this.e11 + r02 * this.e21;
this.e02 = r00 * this.e02 + r01 * this.e12 + r02 * this.e22;
this.e03 = r00 * this.e03 + r01 * this.e13 + r02 * this.e23;
this.e10 = e10;
this.e11 = e11;
this.e12 = e12;
this.e13 = e13;
this.e20 = e20;
this.e21 = e21;
this.e22 = e22;
this.e23 = e23;
return this;
}
appendRotationEq(rad,axisX,axisY,axisZ) {
let s = Math.sin(rad);
let c = Math.cos(rad);
let c1 = 1 - c;
let r00 = axisX * axisX * c1 + c;
let r01 = axisX * axisY * c1 - axisZ * s;
let r02 = axisX * axisZ * c1 + axisY * s;
let r10 = axisY * axisX * c1 + axisZ * s;
let r11 = axisY * axisY * c1 + c;
let r12 = axisY * axisZ * c1 - axisX * s;
let r20 = axisZ * axisX * c1 - axisY * s;
let r21 = axisZ * axisY * c1 + axisX * s;
let r22 = axisZ * axisZ * c1 + c;
let e01 = this.e00 * r01 + this.e01 * r11 + this.e02 * r21;
let e02 = this.e00 * r02 + this.e01 * r12 + this.e02 * r22;
let e11 = this.e10 * r01 + this.e11 * r11 + this.e12 * r21;
let e12 = this.e10 * r02 + this.e11 * r12 + this.e12 * r22;
let e21 = this.e20 * r01 + this.e21 * r11 + this.e22 * r21;
let e22 = this.e20 * r02 + this.e21 * r12 + this.e22 * r22;
let e31 = this.e30 * r01 + this.e31 * r11 + this.e32 * r21;
let e32 = this.e30 * r02 + this.e31 * r12 + this.e32 * r22;
this.e00 = this.e00 * r00 + this.e01 * r10 + this.e02 * r20;
this.e01 = e01;
this.e02 = e02;
this.e10 = this.e10 * r00 + this.e11 * r10 + this.e12 * r20;
this.e11 = e11;
this.e12 = e12;
this.e20 = this.e20 * r00 + this.e21 * r10 + this.e22 * r20;
this.e21 = e21;
this.e22 = e22;
this.e30 = this.e30 * r00 + this.e31 * r10 + this.e32 * r20;
this.e31 = e31;
this.e32 = e32;
return this;
}
prependTranslationEq(tx,ty,tz) {
this.e00 += tx * this.e30;
this.e01 += tx * this.e31;
this.e02 += tx * this.e32;
this.e03 += tx * this.e33;
this.e10 += ty * this.e30;
this.e11 += ty * this.e31;
this.e12 += ty * this.e32;
this.e13 += ty * this.e33;
this.e20 += tz * this.e30;
this.e21 += tz * this.e31;
this.e22 += tz * this.e32;
this.e23 += tz * this.e33;
return this;
}
appendTranslationEq(tx,ty,tz) {
let e03 = this.e00 * tx + this.e01 * ty + this.e02 * tz + this.e03;
let e13 = this.e10 * tx + this.e11 * ty + this.e12 * tz + this.e13;
let e23 = this.e20 * tx + this.e21 * ty + this.e22 * tz + this.e23;
let e33 = this.e30 * tx + this.e31 * ty + this.e32 * tz + this.e33;
this.e03 = e03;
this.e13 = e13;
this.e23 = e23;
this.e33 = e33;
return this;
}
transpose() {
return new oimo.common.Mat4(this.e00,this.e10,this.e20,this.e30,this.e01,this.e11,this.e21,this.e31,this.e02,this.e12,this.e22,this.e32,this.e03,this.e13,this.e23,this.e33);
}
transposeEq() {
let e10 = this.e01;
let e20 = this.e02;
let e21 = this.e12;
let e30 = this.e03;
let e31 = this.e13;
let e32 = this.e23;
this.e01 = this.e10;
this.e02 = this.e20;
this.e03 = this.e30;
this.e10 = e10;
this.e12 = this.e21;
this.e13 = this.e31;
this.e20 = e20;
this.e21 = e21;
this.e23 = this.e32;
this.e30 = e30;
this.e31 = e31;
this.e32 = e32;
return this;
}
determinant() {
let d23_01 = this.e20 * this.e31 - this.e21 * this.e30;
let d23_02 = this.e20 * this.e32 - this.e22 * this.e30;
let d23_03 = this.e20 * this.e33 - this.e23 * this.e30;
let d23_12 = this.e21 * this.e32 - this.e22 * this.e31;
let d23_13 = this.e21 * this.e33 - this.e23 * this.e31;
let d23_23 = this.e22 * this.e33 - this.e23 * this.e32;
return this.e00 * (this.e11 * d23_23 - this.e12 * d23_13 + this.e13 * d23_12) - this.e01 * (this.e10 * d23_23 - this.e12 * d23_03 + this.e13 * d23_02) + this.e02 * (this.e10 * d23_13 - this.e11 * d23_03 + this.e13 * d23_01) - this.e03 * (this.e10 * d23_12 - this.e11 * d23_02 + this.e12 * d23_01);
}
trace() {
return this.e00 + this.e11 + this.e22 + this.e33;
}
inverse() {
let d01_01 = this.e00 * this.e11 - this.e01 * this.e10;
let d01_02 = this.e00 * this.e12 - this.e02 * this.e10;
let d01_03 = this.e00 * this.e13 - this.e03 * this.e10;
let d01_12 = this.e01 * this.e12 - this.e02 * this.e11;
let d01_13 = this.e01 * this.e13 - this.e03 * this.e11;
let d01_23 = this.e02 * this.e13 - this.e03 * this.e12;
let d23_01 = this.e20 * this.e31 - this.e21 * this.e30;
let d23_02 = this.e20 * this.e32 - this.e22 * this.e30;
let d23_03 = this.e20 * this.e33 - this.e23 * this.e30;
let d23_12 = this.e21 * this.e32 - this.e22 * this.e31;
let d23_13 = this.e21 * this.e33 - this.e23 * this.e31;
let d23_23 = this.e22 * this.e33 - this.e23 * this.e32;
let d00 = this.e11 * d23_23 - this.e12 * d23_13 + this.e13 * d23_12;
let d01 = this.e10 * d23_23 - this.e12 * d23_03 + this.e13 * d23_02;
let d02 = this.e10 * d23_13 - this.e11 * d23_03 + this.e13 * d23_01;
let d03 = this.e10 * d23_12 - this.e11 * d23_02 + this.e12 * d23_01;
let invDet = this.e00 * d00 - this.e01 * d01 + this.e02 * d02 - this.e03 * d03;
if(invDet != 0) {
invDet = 1 / invDet;
}
return new oimo.common.Mat4(d00 * invDet,-(this.e01 * d23_23 - this.e02 * d23_13 + this.e03 * d23_12) * invDet,(this.e31 * d01_23 - this.e32 * d01_13 + this.e33 * d01_12) * invDet,-(this.e21 * d01_23 - this.e22 * d01_13 + this.e23 * d01_12) * invDet,-d01 * invDet,(this.e00 * d23_23 - this.e02 * d23_03 + this.e03 * d23_02) * invDet,-(this.e30 * d01_23 - this.e32 * d01_03 + this.e33 * d01_02) * invDet,(this.e20 * d01_23 - this.e22 * d01_03 + this.e23 * d01_02) * invDet,d02 * invDet,-(this.e00 * d23_13 - this.e01 * d23_03 + this.e03 * d23_01) * invDet,(this.e30 * d01_13 - this.e31 * d01_03 + this.e33 * d01_01) * invDet,-(this.e20 * d01_13 - this.e21 * d01_03 + this.e23 * d01_01) * invDet,-d03 * invDet,(this.e00 * d23_12 - this.e01 * d23_02 + this.e02 * d23_01) * invDet,-(this.e30 * d01_12 - this.e31 * d01_02 + this.e32 * d01_01) * invDet,(this.e20 * d01_12 - this.e21 * d01_02 + this.e22 * d01_01) * invDet);
}
inverseEq() {
let d01_01 = this.e00 * this.e11 - this.e01 * this.e10;
let d01_02 = this.e00 * this.e12 - this.e02 * this.e10;
let d01_03 = this.e00 * this.e13 - this.e03 * this.e10;
let d01_12 = this.e01 * this.e12 - this.e02 * this.e11;
let d01_13 = this.e01 * this.e13 - this.e03 * this.e11;
let d01_23 = this.e02 * this.e13 - this.e03 * this.e12;
let d23_01 = this.e20 * this.e31 - this.e21 * this.e30;
let d23_02 = this.e20 * this.e32 - this.e22 * this.e30;
let d23_03 = this.e20 * this.e33 - this.e23 * this.e30;
let d23_12 = this.e21 * this.e32 - this.e22 * this.e31;
let d23_13 = this.e21 * this.e33 - this.e23 * this.e31;
let d23_23 = this.e22 * this.e33 - this.e23 * this.e32;
let d00 = this.e11 * d23_23 - this.e12 * d23_13 + this.e13 * d23_12;
let d01 = this.e10 * d23_23 - this.e12 * d23_03 + this.e13 * d23_02;
let d02 = this.e10 * d23_13 - this.e11 * d23_03 + this.e13 * d23_01;
let d03 = this.e10 * d23_12 - this.e11 * d23_02 + this.e12 * d23_01;
let invDet = this.e00 * d00 - this.e01 * d01 + this.e02 * d02 - this.e03 * d03;
if(invDet != 0) {
invDet = 1 / invDet;
}
let t11 = (this.e00 * d23_23 - this.e02 * d23_03 + this.e03 * d23_02) * invDet;
let t21 = -(this.e00 * d23_13 - this.e01 * d23_03 + this.e03 * d23_01) * invDet;
let t23 = -(this.e20 * d01_13 - this.e21 * d01_03 + this.e23 * d01_01) * invDet;
let t31 = (this.e00 * d23_12 - this.e01 * d23_02 + this.e02 * d23_01) * invDet;
let t32 = -(this.e30 * d01_12 - this.e31 * d01_02 + this.e32 * d01_01) * invDet;
let t33 = (this.e20 * d01_12 - this.e21 * d01_02 + this.e22 * d01_01) * invDet;
this.e00 = d00 * invDet;
this.e01 = -(this.e01 * d23_23 - this.e02 * d23_13 + this.e03 * d23_12) * invDet;
this.e02 = (this.e31 * d01_23 - this.e32 * d01_13 + this.e33 * d01_12) * invDet;
this.e03 = -(this.e21 * d01_23 - this.e22 * d01_13 + this.e23 * d01_12) * invDet;
this.e10 = -d01 * invDet;
this.e11 = t11;
this.e12 = -(this.e30 * d01_23 - this.e32 * d01_03 + this.e33 * d01_02) * invDet;
this.e13 = (this.e20 * d01_23 - this.e22 * d01_03 + this.e23 * d01_02) * invDet;
this.e20 = d02 * invDet;
this.e21 = t21;
this.e22 = (this.e30 * d01_13 - this.e31 * d01_03 + this.e33 * d01_01) * invDet;
this.e23 = t23;
this.e30 = -d03 * invDet;
this.e31 = t31;
this.e32 = t32;
this.e33 = t33;
return this;
}
lookAt(eyeX,eyeY,eyeZ,atX,atY,atZ,upX,upY,upZ) {
let zx = eyeX - atX;
let zy = eyeY - atY;
let zz = eyeZ - atZ;
let tmp = 1 / Math.sqrt(zx * zx + zy * zy + zz * zz);
zx *= tmp;
zy *= tmp;
zz *= tmp;
let xx = upY * zz - upZ * zy;
let xy = upZ * zx - upX * zz;
let xz = upX * zy - upY * zx;
tmp = 1 / Math.sqrt(xx * xx + xy * xy + xz * xz);
xx *= tmp;
xy *= tmp;
xz *= tmp;
let yx = zy * xz - zz * xy;
let yy = zz * xx - zx * xz;
let yz = zx * xy - zy * xx;
this.e00 = xx;
this.e01 = xy;
this.e02 = xz;
this.e03 = -(xx * eyeX + xy * eyeY + xz * eyeZ);
this.e10 = yx;
this.e11 = yy;
this.e12 = yz;
this.e13 = -(yx * eyeX + yy * eyeY + yz * eyeZ);
this.e20 = zx;
this.e21 = zy;
this.e22 = zz;
this.e23 = -(zx * eyeX + zy * eyeY + zz * eyeZ);
this.e30 = 0;
this.e31 = 0;
this.e32 = 0;
this.e33 = 1;
return this;
}
perspective(fovY,aspect,near,far) {
let h = 1 / Math.tan(fovY * 0.5);
let fnf = far / (near - far);
this.e00 = h / aspect;
this.e01 = 0;
this.e02 = 0;
this.e03 = 0;
this.e10 = 0;
this.e11 = h;
this.e12 = 0;
this.e13 = 0;
this.e20 = 0;
this.e21 = 0;
this.e22 = fnf;
this.e23 = near * fnf;
this.e30 = 0;
this.e31 = 0;
this.e32 = -1;
this.e33 = 0;
return this;
}
ortho(width,height,near,far) {
let nf = 1 / (near - far);
this.e00 = 2 / width;
this.e01 = 0;
this.e02 = 0;
this.e03 = 0;
this.e10 = 0;
this.e11 = 2 / height;
this.e12 = 0;
this.e13 = 0;
this.e20 = 0;
this.e21 = 0;
this.e22 = nf;
this.e23 = near * nf;
this.e30 = 0;
this.e31 = 0;
this.e32 = 0;
this.e33 = 1;
return this;
}
toArray(columnMajor) {
if(columnMajor == null) {
columnMajor = false;
}
if(columnMajor) {
return [this.e00,this.e10,this.e20,this.e30,this.e01,this.e11,this.e21,this.e31,this.e02,this.e12,this.e22,this.e32,this.e03,this.e13,this.e23,this.e33];
} else {
return [this.e00,this.e01,this.e02,this.e03,this.e10,this.e11,this.e12,this.e13,this.e20,this.e21,this.e22,this.e23,this.e30,this.e31,this.e32,this.e33];
}
}
copyFrom(m) {
this.e00 = m.e00;
this.e01 = m.e01;
this.e02 = m.e02;
this.e03 = m.e03;
this.e10 = m.e10;
this.e11 = m.e11;
this.e12 = m.e12;
this.e13 = m.e13;
this.e20 = m.e20;
this.e21 = m.e21;
this.e22 = m.e22;
this.e23 = m.e23;
this.e30 = m.e30;
this.e31 = m.e31;
this.e32 = m.e32;
this.e33 = m.e33;
return this;
}
fromMat3(m) {
this.e00 = m.e00;
this.e01 = m.e01;
this.e02 = m.e02;
this.e03 = 0;
this.e10 = m.e10;
this.e11 = m.e11;
this.e12 = m.e12;
this.e13 = 0;
this.e20 = m.e20;
this.e21 = m.e21;
this.e22 = m.e22;
this.e23 = 0;
this.e30 = 0;
this.e31 = 0;
this.e32 = 0;
this.e33 = 1;
return this;
}
fromTransform(transform) {
this.e00 = transform._rotation00;
this.e01 = transform._rotation01;
this.e02 = transform._rotation02;
this.e10 = transform._rotation10;
this.e11 = transform._rotation11;
this.e12 = transform._rotation12;
this.e20 = transform._rotation20;
this.e21 = transform._rotation21;
this.e22 = transform._rotation22;
this.e03 = transform._positionX;
this.e13 = transform._positionY;
this.e23 = transform._positionZ;
this.e30 = 0;
this.e31 = 0;
this.e32 = 0;
this.e33 = 1;
return this;
}
clone() {
return new oimo.common.Mat4(this.e00,this.e01,this.e02,this.e03,this.e10,this.e11,this.e12,this.e13,this.e20,this.e21,this.e22,this.e23,this.e30,this.e31,this.e32,this.e33);
}
toString() {
return "Mat4[" + (this.e00 > 0 ? (this.e00 * 10000000 + 0.5 | 0) / 10000000 : (this.e00 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e01 > 0 ? (this.e01 * 10000000 + 0.5 | 0) / 10000000 : (this.e01 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e02 > 0 ? (this.e02 * 10000000 + 0.5 | 0) / 10000000 : (this.e02 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e03 > 0 ? (this.e03 * 10000000 + 0.5 | 0) / 10000000 : (this.e03 * 10000000 - 0.5 | 0) / 10000000) + ",\n" + " " + (this.e10 > 0 ? (this.e10 * 10000000 + 0.5 | 0) / 10000000 : (this.e10 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e11 > 0 ? (this.e11 * 10000000 + 0.5 | 0) / 10000000 : (this.e11 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e12 > 0 ? (this.e12 * 10000000 + 0.5 | 0) / 10000000 : (this.e12 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e13 > 0 ? (this.e13 * 10000000 + 0.5 | 0) / 10000000 : (this.e13 * 10000000 - 0.5 | 0) / 10000000) + ",\n" + " " + (this.e20 > 0 ? (this.e20 * 10000000 + 0.5 | 0) / 10000000 : (this.e20 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e21 > 0 ? (this.e21 * 10000000 + 0.5 | 0) / 10000000 : (this.e21 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e22 > 0 ? (this.e22 * 10000000 + 0.5 | 0) / 10000000 : (this.e22 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e23 > 0 ? (this.e23 * 10000000 + 0.5 | 0) / 10000000 : (this.e23 * 10000000 - 0.5 | 0) / 10000000) + ",\n" + " " + (this.e30 > 0 ? (this.e30 * 10000000 + 0.5 | 0) / 10000000 : (this.e30 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e31 > 0 ? (this.e31 * 10000000 + 0.5 | 0) / 10000000 : (this.e31 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e32 > 0 ? (this.e32 * 10000000 + 0.5 | 0) / 10000000 : (this.e32 * 10000000 - 0.5 | 0) / 10000000) + ", " + (this.e33 > 0 ? (this.e33 * 10000000 + 0.5 | 0) / 10000000 : (this.e33 * 10000000 - 0.5 | 0) / 10000000) + "]";
}
}
oimo.common.MathUtil = class oimo_common_MathUtil {
static abs(x) {
if(x > 0) {
return x;
} else {
return -x;
}
}
static sin(x) {
return Math.sin(x);
}
static cos(x) {
return Math.cos(x);
}
static tan(x) {
return Math.tan(x);
}
static asin(x) {
return Math.asin(x);
}
static acos(x) {
return Math.acos(x);
}
static atan(x) {
return Math.atan(x);
}
static safeAsin(x) {
if(x <= -1) {
return -1.570796326794895;
}
if(x >= 1) {
return 1.570796326794895;
}
return Math.asin(x);
}
static safeAcos(x) {
if(x <= -1) {
return 3.14159265358979;
}
if(x >= 1) {
return 0;
}
return Math.acos(x);
}
static atan2(y,x) {
return Math.atan2(y,x);
}
static sqrt(x) {
return Math.sqrt(x);
}
static clamp(x,min,max) {
if(x < min) {
return min;
} else if(x > max) {
return max;
} else {
return x;
}
}
static rand() {
return Math.random();
}
static randIn(min,max) {
return min + Math.random() * (max - min);
}
static randVec3In(min,max) {
return new oimo.common.Vec3(min + Math.random() * (max - min),min + Math.random() * (max - min),min + Math.random() * (max - min));
}
static randVec3() {
return new oimo.common.Vec3(-1 + Math.random() * 2,-1 + Math.random() * 2,-1 + Math.random() * 2);
}
}
oimo.common.Pool = class oimo_common_Pool {
constructor() {
this.stackVec3 = new Array(256);
this.sizeVec3 = 0;
this.stackMat3 = new Array(256);
this.sizeMat3 = 0;
this.stackMat4 = new Array(256);
this.sizeMat4 = 0;
this.stackQuat = new Array(256);
this.sizeQuat = 0;
}
vec3() {
if(this.sizeVec3 == 0) {
return new oimo.common.Vec3();
} else {
return this.stackVec3[--this.sizeVec3];
}
}
mat3() {
if(this.sizeMat3 == 0) {
return new oimo.common.Mat3();
} else {
return this.stackMat3[--this.sizeMat3];
}
}
mat4() {
if(this.sizeMat4 == 0) {
return new oimo.common.Mat4();
} else {
return this.stackMat4[--this.sizeMat4];
}
}
quat() {
if(this.sizeQuat == 0) {
return new oimo.common.Quat();
} else {
return this.stackQuat[--this.sizeQuat];
}
}
dispose(vec3,mat3,mat4,quat) {
if(vec3 != null) {
vec3.zero();
if(this.sizeVec3 == this.stackVec3.length) {
let newArray = new Array(this.sizeVec3 << 1);
let _g = 0;
let _g1 = this.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.stackVec3[i];
this.stackVec3[i] = null;
}
this.stackVec3 = newArray;
}
this.stackVec3[this.sizeVec3++] = vec3;
}
if(mat3 != null) {
mat3.e00 = 1;
mat3.e01 = 0;
mat3.e02 = 0;
mat3.e10 = 0;
mat3.e11 = 1;
mat3.e12 = 0;
mat3.e20 = 0;
mat3.e21 = 0;
mat3.e22 = 1;
if(this.sizeMat3 == this.stackMat3.length) {
let newArray = new Array(this.sizeMat3 << 1);
let _g = 0;
let _g1 = this.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.stackMat3[i];
this.stackMat3[i] = null;
}
this.stackMat3 = newArray;
}
this.stackMat3[this.sizeMat3++] = mat3;
}
if(mat4 != null) {
mat4.e00 = 1;
mat4.e01 = 0;
mat4.e02 = 0;
mat4.e03 = 0;
mat4.e10 = 0;
mat4.e11 = 1;
mat4.e12 = 0;
mat4.e13 = 0;
mat4.e20 = 0;
mat4.e21 = 0;
mat4.e22 = 1;
mat4.e23 = 0;
mat4.e30 = 0;
mat4.e31 = 0;
mat4.e32 = 0;
mat4.e33 = 1;
if(this.sizeMat4 == this.stackMat4.length) {
let newArray = new Array(this.sizeMat4 << 1);
let _g = 0;
let _g1 = this.sizeMat4;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.stackMat4[i];
this.stackMat4[i] = null;
}
this.stackMat4 = newArray;
}
this.stackMat4[this.sizeMat4++] = mat4;
}
if(quat != null) {
quat.x = 0;
quat.y = 0;
quat.z = 0;
quat.w = 1;
if(this.sizeQuat == this.stackQuat.length) {
let newArray = new Array(this.sizeQuat << 1);
let _g = 0;
let _g1 = this.sizeQuat;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.stackQuat[i];
this.stackQuat[i] = null;
}
this.stackQuat = newArray;
}
this.stackQuat[this.sizeQuat++] = quat;
}
}
disposeVec3(v) {
v.zero();
if(this.sizeVec3 == this.stackVec3.length) {
let newArray = new Array(this.sizeVec3 << 1);
let _g = 0;
let _g1 = this.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.stackVec3[i];
this.stackVec3[i] = null;
}
this.stackVec3 = newArray;
}
this.stackVec3[this.sizeVec3++] = v;
}
disposeMat3(m) {
m.e00 = 1;
m.e01 = 0;
m.e02 = 0;
m.e10 = 0;
m.e11 = 1;
m.e12 = 0;
m.e20 = 0;
m.e21 = 0;
m.e22 = 1;
if(this.sizeMat3 == this.stackMat3.length) {
let newArray = new Array(this.sizeMat3 << 1);
let _g = 0;
let _g1 = this.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.stackMat3[i];
this.stackMat3[i] = null;
}
this.stackMat3 = newArray;
}
this.stackMat3[this.sizeMat3++] = m;
}
disposeMat4(m) {
m.e00 = 1;
m.e01 = 0;
m.e02 = 0;
m.e03 = 0;
m.e10 = 0;
m.e11 = 1;
m.e12 = 0;
m.e13 = 0;
m.e20 = 0;
m.e21 = 0;
m.e22 = 1;
m.e23 = 0;
m.e30 = 0;
m.e31 = 0;
m.e32 = 0;
m.e33 = 1;
if(this.sizeMat4 == this.stackMat4.length) {
let newArray = new Array(this.sizeMat4 << 1);
let _g = 0;
let _g1 = this.sizeMat4;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.stackMat4[i];
this.stackMat4[i] = null;
}
this.stackMat4 = newArray;
}
this.stackMat4[this.sizeMat4++] = m;
}
disposeQuat(q) {
q.x = 0;
q.y = 0;
q.z = 0;
q.w = 1;
if(this.sizeQuat == this.stackQuat.length) {
let newArray = new Array(this.sizeQuat << 1);
let _g = 0;
let _g1 = this.sizeQuat;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.stackQuat[i];
this.stackQuat[i] = null;
}
this.stackQuat = newArray;
}
this.stackQuat[this.sizeQuat++] = q;
}
}
oimo.common.Quat = class oimo_common_Quat {
constructor(x,y,z,w) {
if(w == null) {
w = 1;
}
if(z == null) {
z = 0;
}
if(y == null) {
y = 0;
}
if(x == null) {
x = 0;
}
this.x = x;
this.y = y;
this.z = z;
this.w = w;
oimo.common.Quat.numCreations++;
}
identity() {
this.x = 0;
this.y = 0;
this.z = 0;
this.w = 1;
return this;
}
init(x,y,z,w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
return this;
}
add(q) {
return new oimo.common.Quat(this.x + q.x,this.y + q.y,this.z + q.z,this.w + q.w);
}
sub(q) {
return new oimo.common.Quat(this.x - q.x,this.y - q.y,this.z - q.z,this.w - q.w);
}
scale(s) {
return new oimo.common.Quat(this.x * s,this.y * s,this.z * s,this.w * s);
}
addEq(q) {
this.x += q.x;
this.y += q.y;
this.z += q.z;
this.w += q.w;
return this;
}
subEq(q) {
this.x -= q.x;
this.y -= q.y;
this.z -= q.z;
this.w -= q.w;
return this;
}
scaleEq(s) {
this.x *= s;
this.y *= s;
this.z *= s;
this.w *= s;
return this;
}
length() {
return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
}
lengthSq() {
return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
}
dot(q) {
return this.x * q.x + this.y * q.y + this.z * q.z + this.w * q.w;
}
normalized() {
let invLen = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
if(invLen > 0) {
invLen = 1 / invLen;
}
return new oimo.common.Quat(this.x * invLen,this.y * invLen,this.z * invLen,this.w * invLen);
}
normalize() {
let invLen = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
if(invLen > 0) {
invLen = 1 / invLen;
}
this.x *= invLen;
this.y *= invLen;
this.z *= invLen;
this.w *= invLen;
return this;
}
setArc(v1,v2) {
let x1 = v1.x;
let y1 = v1.y;
let z1 = v1.z;
let x2 = v2.x;
let y2 = v2.y;
let z2 = v2.z;
let d = x1 * x2 + y1 * y2 + z1 * z2;
this.w = Math.sqrt((1 + d) * 0.5);
if(this.w == 0) {
x2 = x1 * x1;
y2 = y1 * y1;
z2 = z1 * z1;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
this.x = 0;
this.y = z1 * d;
this.z = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
this.z = 0;
this.x = y1 * d;
this.y = -x1 * d;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
this.y = 0;
this.z = x1 * d;
this.x = -z1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
this.z = 0;
this.x = y1 * d;
this.y = -x1 * d;
}
return this;
}
d = 0.5 / this.w;
this.x = (y1 * z2 - z1 * y2) * d;
this.y = (z1 * x2 - x1 * z2) * d;
this.z = (x1 * y2 - y1 * x2) * d;
return this;
}
slerp(q,t) {
let qx;
let qy;
let qz;
let qw;
let d = this.x * q.x + this.y * q.y + this.z * q.z + this.w * q.w;
if(d < 0) {
d = -d;
qx = -q.x;
qy = -q.y;
qz = -q.z;
qw = -q.w;
} else {
qx = q.x;
qy = q.y;
qz = q.z;
qw = q.w;
}
if(d > 0.999999) {
let _this = new oimo.common.Quat(this.x + (qx - this.x) * t,this.y + (qy - this.y) * t,this.z + (qz - this.z) * t,this.w + (qw - this.w) * t);
let invLen = Math.sqrt(_this.x * _this.x + _this.y * _this.y + _this.z * _this.z + _this.w * _this.w);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this.x *= invLen;
_this.y *= invLen;
_this.z *= invLen;
_this.w *= invLen;
return _this;
}
let theta = t * Math.acos(d);
qx -= this.x * d;
qy -= this.y * d;
qz -= this.z * d;
qw -= this.w * d;
let invLen = 1 / Math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
qx *= invLen;
qy *= invLen;
qz *= invLen;
qw *= invLen;
let sin = Math.sin(theta);
let cos = Math.cos(theta);
return new oimo.common.Quat(this.x * cos + qx * sin,this.y * cos + qy * sin,this.z * cos + qz * sin,this.w * cos + qw * sin);
}
copyFrom(q) {
this.x = q.x;
this.y = q.y;
this.z = q.z;
this.w = q.w;
return this;
}
clone() {
return new oimo.common.Quat(this.x,this.y,this.z,this.w);
}
fromMat3(m) {
let e00 = m.e00;
let e11 = m.e11;
let e22 = m.e22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
this.w = 0.5 * s;
s = 0.5 / s;
this.x = (m.e21 - m.e12) * s;
this.y = (m.e02 - m.e20) * s;
this.z = (m.e10 - m.e01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
this.x = 0.5 * s;
s = 0.5 / s;
this.y = (m.e01 + m.e10) * s;
this.z = (m.e02 + m.e20) * s;
this.w = (m.e21 - m.e12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
this.z = 0.5 * s;
s = 0.5 / s;
this.x = (m.e02 + m.e20) * s;
this.y = (m.e12 + m.e21) * s;
this.w = (m.e10 - m.e01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
this.y = 0.5 * s;
s = 0.5 / s;
this.x = (m.e01 + m.e10) * s;
this.z = (m.e12 + m.e21) * s;
this.w = (m.e02 - m.e20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
this.z = 0.5 * s;
s = 0.5 / s;
this.x = (m.e02 + m.e20) * s;
this.y = (m.e12 + m.e21) * s;
this.w = (m.e10 - m.e01) * s;
}
return this;
}
toMat3() {
let _this = new oimo.common.Mat3();
let x = this.x;
let y = this.y;
let z = this.z;
let w = this.w;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
_this.e00 = 1 - yy - zz;
_this.e01 = xy - wz;
_this.e02 = xz + wy;
_this.e10 = xy + wz;
_this.e11 = 1 - xx - zz;
_this.e12 = yz - wx;
_this.e20 = xz - wy;
_this.e21 = yz + wx;
_this.e22 = 1 - xx - yy;
return _this;
}
toString() {
return "Quat[" + (this.x > 0 ? (this.x * 10000000 + 0.5 | 0) / 10000000 : (this.x * 10000000 - 0.5 | 0) / 10000000) + " i,\n" + " " + (this.y > 0 ? (this.y * 10000000 + 0.5 | 0) / 10000000 : (this.y * 10000000 - 0.5 | 0) / 10000000) + " j,\n" + " " + (this.z > 0 ? (this.z * 10000000 + 0.5 | 0) / 10000000 : (this.z * 10000000 - 0.5 | 0) / 10000000) + " k,\n" + " " + (this.w > 0 ? (this.w * 10000000 + 0.5 | 0) / 10000000 : (this.w * 10000000 - 0.5 | 0) / 10000000) + "]";
}
}
if(!oimo.dynamics) oimo.dynamics = {};
oimo.dynamics.Contact = class oimo_dynamics_Contact {
constructor() {
this._next = null;
this._prev = null;
this._link1 = new oimo.dynamics.ContactLink();
this._link2 = new oimo.dynamics.ContactLink();
this._s1 = null;
this._s2 = null;
this._b1 = null;
this._b2 = null;
this._detector = null;
this._cachedDetectorData = new oimo.collision.narrowphase.detector.CachedDetectorData();
this._detectorResult = new oimo.collision.narrowphase.DetectorResult();
this._latest = false;
this._shouldBeSkipped = false;
this._manifold = new oimo.dynamics.constraint.contact.Manifold();
this._updater = new oimo.dynamics.constraint.contact.ManifoldUpdater(this._manifold);
this._contactConstraint = new oimo.dynamics.constraint.contact.ContactConstraint(this._manifold);
this._touching = false;
}
_updateManifold() {
if(this._detector == null) {
return;
}
let ptouching = this._touching;
let result = this._detectorResult;
this._detector.detect(result,this._s1._geom,this._s2._geom,this._s1._transform,this._s2._transform,this._cachedDetectorData);
this._touching = result.numPoints > 0;
if(this._touching) {
this._manifold._buildBasis(result.normal);
if(result.getMaxDepth() > oimo.common.Setting.contactUseAlternativePositionCorrectionAlgorithmDepthThreshold) {
this._contactConstraint._positionCorrectionAlgorithm = oimo.common.Setting.alternativeContactPositionCorrectionAlgorithm;
} else {
this._contactConstraint._positionCorrectionAlgorithm = oimo.common.Setting.defaultContactPositionCorrectionAlgorithm;
}
if(result.incremental) {
this._updater.incrementalUpdate(result,this._b1._transform,this._b2._transform);
} else {
this._updater.totalUpdate(result,this._b1._transform,this._b2._transform);
}
} else {
this._manifold._clear();
}
if(this._touching && !ptouching) {
let cc1 = this._s1._contactCallback;
let cc2 = this._s2._contactCallback;
if(cc1 == cc2) {
cc2 = null;
}
if(cc1 != null) {
cc1.beginContact(this);
}
if(cc2 != null) {
cc2.beginContact(this);
}
}
if(!this._touching && ptouching) {
let cc1 = this._s1._contactCallback;
let cc2 = this._s2._contactCallback;
if(cc1 == cc2) {
cc2 = null;
}
if(cc1 != null) {
cc1.endContact(this);
}
if(cc2 != null) {
cc2.endContact(this);
}
}
if(this._touching) {
let cc1 = this._s1._contactCallback;
let cc2 = this._s2._contactCallback;
if(cc1 == cc2) {
cc2 = null;
}
if(cc1 != null) {
cc1.preSolve(this);
}
if(cc2 != null) {
cc2.preSolve(this);
}
}
}
_postSolve() {
let cc1 = this._s1._contactCallback;
let cc2 = this._s2._contactCallback;
if(cc1 == cc2) {
cc2 = null;
}
if(cc1 != null) {
cc1.postSolve(this);
}
if(cc2 != null) {
cc2.postSolve(this);
}
}
getShape1() {
return this._s1;
}
getShape2() {
return this._s2;
}
isTouching() {
return this._touching;
}
getManifold() {
return this._manifold;
}
getContactConstraint() {
return this._contactConstraint;
}
getPrev() {
return this._prev;
}
getNext() {
return this._next;
}
}
oimo.dynamics.ContactLink = class oimo_dynamics_ContactLink {
constructor() {
this._prev = null;
this._next = null;
this._contact = null;
this._other = null;
}
getContact() {
return this._contact;
}
getOther() {
return this._other;
}
getPrev() {
return this._prev;
}
getNext() {
return this._next;
}
}
oimo.dynamics.ContactManager = class oimo_dynamics_ContactManager {
constructor(broadPhase) {
this._broadPhase = broadPhase;
this._collisionMatrix = new oimo.collision.narrowphase.CollisionMatrix();
this._numContacts = 0;
}
createContacts() {
let pp = this._broadPhase._proxyPairList;
while(pp != null) {
let n = pp._next;
while(true) {
let s1;
let s2;
if(pp._p1._id < pp._p2._id) {
s1 = pp._p1.userData;
s2 = pp._p2.userData;
} else {
s1 = pp._p2.userData;
s2 = pp._p1.userData;
}
if(!this.shouldCollide(s1,s2)) {
break;
}
let b1 = s1._rigidBody;
let b2 = s2._rigidBody;
let l;
if(b1._numContactLinks < b2._numContactLinks) {
l = b1._contactLinkList;
} else {
l = b2._contactLinkList;
}
let id1 = s1._id;
let id2 = s2._id;
let found = false;
while(l != null) {
let c = l._contact;
if(c._s1._id == id1 && c._s2._id == id2) {
c._latest = true;
found = true;
break;
}
l = l._next;
}
if(!found) {
let first = this._contactPool;
if(first != null) {
this._contactPool = first._next;
first._next = null;
} else {
first = new oimo.dynamics.Contact();
}
let c = first;
if(this._contactList == null) {
this._contactList = c;
this._contactListLast = c;
} else {
this._contactListLast._next = c;
c._prev = this._contactListLast;
this._contactListLast = c;
}
c._latest = true;
let detector = this._collisionMatrix.detectors[s1._geom._type][s2._geom._type];
c._s1 = s1;
c._s2 = s2;
c._b1 = s1._rigidBody;
c._b2 = s2._rigidBody;
c._touching = false;
if(c._b1._contactLinkList == null) {
c._b1._contactLinkList = c._link1;
c._b1._contactLinkListLast = c._link1;
} else {
c._b1._contactLinkListLast._next = c._link1;
c._link1._prev = c._b1._contactLinkListLast;
c._b1._contactLinkListLast = c._link1;
}
if(c._b2._contactLinkList == null) {
c._b2._contactLinkList = c._link2;
c._b2._contactLinkListLast = c._link2;
} else {
c._b2._contactLinkListLast._next = c._link2;
c._link2._prev = c._b2._contactLinkListLast;
c._b2._contactLinkListLast = c._link2;
}
c._b1._numContactLinks++;
c._b2._numContactLinks++;
c._link1._other = c._b2;
c._link2._other = c._b1;
c._link1._contact = c;
c._link2._contact = c;
c._detector = detector;
let _this = c._contactConstraint;
_this._s1 = s1;
_this._s2 = s2;
_this._b1 = _this._s1._rigidBody;
_this._b2 = _this._s2._rigidBody;
_this._tf1 = _this._b1._transform;
_this._tf2 = _this._b2._transform;
this._numContacts++;
}
break;
}
pp = n;
}
}
destroyOutdatedContacts() {
let incremental = this._broadPhase._incremental;
let c = this._contactList;
while(c != null) {
let n = c._next;
while(true) {
if(c._latest) {
c._latest = false;
c._shouldBeSkipped = false;
break;
}
if(!incremental) {
let prev = c._prev;
let next = c._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(c == this._contactList) {
this._contactList = this._contactList._next;
}
if(c == this._contactListLast) {
this._contactListLast = this._contactListLast._prev;
}
c._next = null;
c._prev = null;
if(c._touching) {
let cc1 = c._s1._contactCallback;
let cc2 = c._s2._contactCallback;
if(cc1 == cc2) {
cc2 = null;
}
if(cc1 != null) {
cc1.endContact(c);
}
if(cc2 != null) {
cc2.endContact(c);
}
}
let prev1 = c._link1._prev;
let next1 = c._link1._next;
if(prev1 != null) {
prev1._next = next1;
}
if(next1 != null) {
next1._prev = prev1;
}
if(c._link1 == c._b1._contactLinkList) {
c._b1._contactLinkList = c._b1._contactLinkList._next;
}
if(c._link1 == c._b1._contactLinkListLast) {
c._b1._contactLinkListLast = c._b1._contactLinkListLast._prev;
}
c._link1._next = null;
c._link1._prev = null;
let prev2 = c._link2._prev;
let next2 = c._link2._next;
if(prev2 != null) {
prev2._next = next2;
}
if(next2 != null) {
next2._prev = prev2;
}
if(c._link2 == c._b2._contactLinkList) {
c._b2._contactLinkList = c._b2._contactLinkList._next;
}
if(c._link2 == c._b2._contactLinkListLast) {
c._b2._contactLinkListLast = c._b2._contactLinkListLast._prev;
}
c._link2._next = null;
c._link2._prev = null;
c._b1._numContactLinks--;
c._b2._numContactLinks--;
c._link1._other = null;
c._link2._other = null;
c._link1._contact = null;
c._link2._contact = null;
c._s1 = null;
c._s2 = null;
c._b1 = null;
c._b2 = null;
c._touching = false;
c._cachedDetectorData._clear();
c._manifold._clear();
c._detector = null;
let _this = c._contactConstraint;
_this._s1 = null;
_this._s2 = null;
_this._b1 = null;
_this._b2 = null;
_this._tf1 = null;
_this._tf2 = null;
c._next = this._contactPool;
this._contactPool = c;
this._numContacts--;
break;
}
let s1 = c._s1;
let s2 = c._s2;
let r1 = s1._rigidBody;
let r2 = s2._rigidBody;
if(!(!r1._sleeping && r1._type != 1) && !(!r2._sleeping && r2._type != 1)) {
c._shouldBeSkipped = true;
break;
}
let aabb1 = s1._aabb;
let aabb2 = s2._aabb;
let proxy1 = s1._proxy;
let proxy2 = s2._proxy;
if(!(proxy1._aabbMinX < proxy2._aabbMaxX && proxy1._aabbMaxX > proxy2._aabbMinX && proxy1._aabbMinY < proxy2._aabbMaxY && proxy1._aabbMaxY > proxy2._aabbMinY && proxy1._aabbMinZ < proxy2._aabbMaxZ && proxy1._aabbMaxZ > proxy2._aabbMinZ) || !this.shouldCollide(s1,s2)) {
let prev = c._prev;
let next = c._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(c == this._contactList) {
this._contactList = this._contactList._next;
}
if(c == this._contactListLast) {
this._contactListLast = this._contactListLast._prev;
}
c._next = null;
c._prev = null;
if(c._touching) {
let cc1 = c._s1._contactCallback;
let cc2 = c._s2._contactCallback;
if(cc1 == cc2) {
cc2 = null;
}
if(cc1 != null) {
cc1.endContact(c);
}
if(cc2 != null) {
cc2.endContact(c);
}
}
let prev1 = c._link1._prev;
let next1 = c._link1._next;
if(prev1 != null) {
prev1._next = next1;
}
if(next1 != null) {
next1._prev = prev1;
}
if(c._link1 == c._b1._contactLinkList) {
c._b1._contactLinkList = c._b1._contactLinkList._next;
}
if(c._link1 == c._b1._contactLinkListLast) {
c._b1._contactLinkListLast = c._b1._contactLinkListLast._prev;
}
c._link1._next = null;
c._link1._prev = null;
let prev2 = c._link2._prev;
let next2 = c._link2._next;
if(prev2 != null) {
prev2._next = next2;
}
if(next2 != null) {
next2._prev = prev2;
}
if(c._link2 == c._b2._contactLinkList) {
c._b2._contactLinkList = c._b2._contactLinkList._next;
}
if(c._link2 == c._b2._contactLinkListLast) {
c._b2._contactLinkListLast = c._b2._contactLinkListLast._prev;
}
c._link2._next = null;
c._link2._prev = null;
c._b1._numContactLinks--;
c._b2._numContactLinks--;
c._link1._other = null;
c._link2._other = null;
c._link1._contact = null;
c._link2._contact = null;
c._s1 = null;
c._s2 = null;
c._b1 = null;
c._b2 = null;
c._touching = false;
c._cachedDetectorData._clear();
c._manifold._clear();
c._detector = null;
let _this = c._contactConstraint;
_this._s1 = null;
_this._s2 = null;
_this._b1 = null;
_this._b2 = null;
_this._tf1 = null;
_this._tf2 = null;
c._next = this._contactPool;
this._contactPool = c;
this._numContacts--;
break;
}
c._shouldBeSkipped = !(aabb1._minX < aabb2._maxX && aabb1._maxX > aabb2._minX && aabb1._minY < aabb2._maxY && aabb1._maxY > aabb2._minY && aabb1._minZ < aabb2._maxZ && aabb1._maxZ > aabb2._minZ);
break;
}
c = n;
}
}
shouldCollide(s1,s2) {
let r1 = s1._rigidBody;
let r2 = s2._rigidBody;
if(r1 == r2) {
return false;
}
if(r1._type != 0 && r2._type != 0) {
return false;
}
if((s1._collisionGroup & s2._collisionMask) == 0 || (s2._collisionGroup & s1._collisionMask) == 0) {
return false;
}
let jl;
let other;
if(r1._numJointLinks < r2._numJointLinks) {
jl = r1._jointLinkList;
other = r2;
} else {
jl = r2._jointLinkList;
other = r1;
}
while(jl != null) {
if(jl._other == other && !jl._joint._allowCollision) {
return false;
}
jl = jl._next;
}
return true;
}
_updateContacts() {
this._broadPhase.collectPairs();
this.createContacts();
this.destroyOutdatedContacts();
}
_postSolve() {
let c = this._contactList;
while(c != null) {
let n = c._next;
if(c._touching) {
c._postSolve();
}
c = n;
}
}
getNumContacts() {
return this._numContacts;
}
getContactList() {
return this._contactList;
}
}
oimo.dynamics.Island = class oimo_dynamics_Island {
constructor() {
this.rigidBodies = new Array(oimo.common.Setting.islandInitialRigidBodyArraySize);
this.solvers = new Array(oimo.common.Setting.islandInitialConstraintArraySize);
this.solversSi = new Array(oimo.common.Setting.islandInitialConstraintArraySize);
this.solversNgs = new Array(oimo.common.Setting.islandInitialConstraintArraySize);
this.numRigidBodies = 0;
this.numSolvers = 0;
this.numSolversSi = 0;
this.numSolversNgs = 0;
}
_clear() {
while(this.numRigidBodies > 0) this.rigidBodies[--this.numRigidBodies] = null;
while(this.numSolvers > 0) this.solvers[--this.numSolvers] = null;
while(this.numSolversSi > 0) this.solversSi[--this.numSolversSi] = null;
while(this.numSolversNgs > 0) this.solversNgs[--this.numSolversNgs] = null;
}
_addRigidBody(rigidBody) {
if(this.numRigidBodies == this.rigidBodies.length) {
let newArray = new Array(this.numRigidBodies << 1);
let _g = 0;
let _g1 = this.numRigidBodies;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.rigidBodies[i];
this.rigidBodies[i] = null;
}
this.rigidBodies = newArray;
}
rigidBody._addedToIsland = true;
this.rigidBodies[this.numRigidBodies++] = rigidBody;
}
_addConstraintSolver(solver,positionCorrection) {
if(this.numSolvers == this.solvers.length) {
let newArray = new Array(this.numSolvers << 1);
let _g = 0;
let _g1 = this.numSolvers;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.solvers[i];
this.solvers[i] = null;
}
this.solvers = newArray;
}
solver._addedToIsland = true;
this.solvers[this.numSolvers++] = solver;
if(positionCorrection == oimo.dynamics.constraint.PositionCorrectionAlgorithm.SPLIT_IMPULSE) {
if(this.numSolversSi == this.solversSi.length) {
let newArray = new Array(this.numSolversSi << 1);
let _g = 0;
let _g1 = this.numSolversSi;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.solversSi[i];
this.solversSi[i] = null;
}
this.solversSi = newArray;
}
this.solversSi[this.numSolversSi++] = solver;
}
if(positionCorrection == oimo.dynamics.constraint.PositionCorrectionAlgorithm.NGS) {
if(this.numSolversNgs == this.solversNgs.length) {
let newArray = new Array(this.numSolversNgs << 1);
let _g = 0;
let _g1 = this.numSolversNgs;
while(_g < _g1) {
let i = _g++;
newArray[i] = this.solversNgs[i];
this.solversNgs[i] = null;
}
this.solversNgs = newArray;
}
this.solversNgs[this.numSolversNgs++] = solver;
}
}
_stepSingleRigidBody(timeStep,rb) {
let dt = timeStep.dt;
let dst = rb._ptransform;
let src = rb._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
rb._linearContactImpulseX = 0;
rb._linearContactImpulseY = 0;
rb._linearContactImpulseZ = 0;
rb._angularContactImpulseX = 0;
rb._angularContactImpulseY = 0;
rb._angularContactImpulseZ = 0;
if(rb._autoSleep && rb._velX * rb._velX + rb._velY * rb._velY + rb._velZ * rb._velZ < oimo.common.Setting.sleepingVelocityThreshold * oimo.common.Setting.sleepingVelocityThreshold && rb._angVelX * rb._angVelX + rb._angVelY * rb._angVelY + rb._angVelZ * rb._angVelZ < oimo.common.Setting.sleepingAngularVelocityThreshold * oimo.common.Setting.sleepingAngularVelocityThreshold) {
rb._sleepTime += dt;
if(rb._sleepTime > oimo.common.Setting.sleepingTimeThreshold) {
rb._sleeping = true;
rb._sleepTime = 0;
}
} else {
rb._sleepTime = 0;
}
if(!rb._sleeping) {
if(rb._type == 0) {
let x = dt * rb._linearDamping;
let x2 = x * x;
let linScale = 1 / (1 + x + x2 * (0.5 + x * 0.16666666666666666 + x2 * 0.041666666666666664));
let x1 = dt * rb._angularDamping;
let x21 = x1 * x1;
let angScale = 1 / (1 + x1 + x21 * (0.5 + x1 * 0.16666666666666666 + x21 * 0.041666666666666664));
let linAccX;
let linAccY;
let linAccZ;
let angAccX;
let angAccY;
let angAccZ;
linAccX = this.gravityX * rb._gravityScale;
linAccY = this.gravityY * rb._gravityScale;
linAccZ = this.gravityZ * rb._gravityScale;
linAccX += rb._forceX * rb._invMass;
linAccY += rb._forceY * rb._invMass;
linAccZ += rb._forceZ * rb._invMass;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = rb._invInertia00 * rb._torqueX + rb._invInertia01 * rb._torqueY + rb._invInertia02 * rb._torqueZ;
__tmp__Y = rb._invInertia10 * rb._torqueX + rb._invInertia11 * rb._torqueY + rb._invInertia12 * rb._torqueZ;
__tmp__Z = rb._invInertia20 * rb._torqueX + rb._invInertia21 * rb._torqueY + rb._invInertia22 * rb._torqueZ;
angAccX = __tmp__X;
angAccY = __tmp__Y;
angAccZ = __tmp__Z;
rb._velX += linAccX * dt;
rb._velY += linAccY * dt;
rb._velZ += linAccZ * dt;
rb._velX *= linScale;
rb._velY *= linScale;
rb._velZ *= linScale;
rb._angVelX += angAccX * dt;
rb._angVelY += angAccY * dt;
rb._angVelZ += angAccZ * dt;
rb._angVelX *= angScale;
rb._angVelY *= angScale;
rb._angVelZ *= angScale;
}
rb._integrate(dt);
let s = rb._shapeList;
while(s != null) {
let n = s._next;
let tf1 = rb._ptransform;
let tf2 = rb._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
}
}
_step(timeStep,numVelocityIterations,numPositionIterations) {
let dt = timeStep.dt;
let sleepIsland = true;
let _g = 0;
let _g1 = this.numRigidBodies;
while(_g < _g1) {
let rb = this.rigidBodies[_g++];
let dst = rb._ptransform;
let src = rb._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
rb._linearContactImpulseX = 0;
rb._linearContactImpulseY = 0;
rb._linearContactImpulseZ = 0;
rb._angularContactImpulseX = 0;
rb._angularContactImpulseY = 0;
rb._angularContactImpulseZ = 0;
rb._sleeping = false;
if(rb._autoSleep && rb._velX * rb._velX + rb._velY * rb._velY + rb._velZ * rb._velZ < oimo.common.Setting.sleepingVelocityThreshold * oimo.common.Setting.sleepingVelocityThreshold && rb._angVelX * rb._angVelX + rb._angVelY * rb._angVelY + rb._angVelZ * rb._angVelZ < oimo.common.Setting.sleepingAngularVelocityThreshold * oimo.common.Setting.sleepingAngularVelocityThreshold) {
rb._sleepTime += dt;
} else {
rb._sleepTime = 0;
}
if(rb._sleepTime < oimo.common.Setting.sleepingTimeThreshold) {
sleepIsland = false;
}
if(rb._type == 0) {
let x = dt * rb._linearDamping;
let x2 = x * x;
let linScale = 1 / (1 + x + x2 * (0.5 + x * 0.16666666666666666 + x2 * 0.041666666666666664));
let x1 = dt * rb._angularDamping;
let x21 = x1 * x1;
let angScale = 1 / (1 + x1 + x21 * (0.5 + x1 * 0.16666666666666666 + x21 * 0.041666666666666664));
let linAccX;
let linAccY;
let linAccZ;
let angAccX;
let angAccY;
let angAccZ;
linAccX = this.gravityX * rb._gravityScale;
linAccY = this.gravityY * rb._gravityScale;
linAccZ = this.gravityZ * rb._gravityScale;
linAccX += rb._forceX * rb._invMass;
linAccY += rb._forceY * rb._invMass;
linAccZ += rb._forceZ * rb._invMass;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = rb._invInertia00 * rb._torqueX + rb._invInertia01 * rb._torqueY + rb._invInertia02 * rb._torqueZ;
__tmp__Y = rb._invInertia10 * rb._torqueX + rb._invInertia11 * rb._torqueY + rb._invInertia12 * rb._torqueZ;
__tmp__Z = rb._invInertia20 * rb._torqueX + rb._invInertia21 * rb._torqueY + rb._invInertia22 * rb._torqueZ;
angAccX = __tmp__X;
angAccY = __tmp__Y;
angAccZ = __tmp__Z;
rb._velX += linAccX * dt;
rb._velY += linAccY * dt;
rb._velZ += linAccZ * dt;
rb._velX *= linScale;
rb._velY *= linScale;
rb._velZ *= linScale;
rb._angVelX += angAccX * dt;
rb._angVelY += angAccY * dt;
rb._angVelZ += angAccZ * dt;
rb._angVelX *= angScale;
rb._angVelY *= angScale;
rb._angVelZ *= angScale;
}
}
if(sleepIsland) {
let _g = 0;
let _g1 = this.numRigidBodies;
while(_g < _g1) {
let rb = this.rigidBodies[_g++];
rb._sleeping = true;
rb._sleepTime = 0;
}
return;
}
let _g2 = 0;
let _g3 = this.numSolvers;
while(_g2 < _g3) this.solvers[_g2++].preSolveVelocity(timeStep);
let _g4 = 0;
let _g5 = this.numSolvers;
while(_g4 < _g5) this.solvers[_g4++].warmStart(timeStep);
let _g6 = 0;
while(_g6 < numVelocityIterations) {
++_g6;
let _g = 0;
let _g1 = this.numSolvers;
while(_g < _g1) this.solvers[_g++].solveVelocity();
}
let _g7 = 0;
let _g8 = this.numSolvers;
while(_g7 < _g8) this.solvers[_g7++].postSolveVelocity(timeStep);
let _g9 = 0;
let _g10 = this.numRigidBodies;
while(_g9 < _g10) this.rigidBodies[_g9++]._integrate(dt);
let _g11 = 0;
let _g12 = this.numSolversSi;
while(_g11 < _g12) this.solversSi[_g11++].preSolvePosition(timeStep);
let _g13 = 0;
while(_g13 < numPositionIterations) {
++_g13;
let _g = 0;
let _g1 = this.numSolversSi;
while(_g < _g1) this.solversSi[_g++].solvePositionSplitImpulse();
}
let _g14 = 0;
let _g15 = this.numRigidBodies;
while(_g14 < _g15) this.rigidBodies[_g14++]._integratePseudoVelocity();
let _g16 = 0;
let _g17 = this.numSolversNgs;
while(_g16 < _g17) this.solversNgs[_g16++].preSolvePosition(timeStep);
let _g18 = 0;
while(_g18 < numPositionIterations) {
++_g18;
let _g = 0;
let _g1 = this.numSolversNgs;
while(_g < _g1) this.solversNgs[_g++].solvePositionNgs(timeStep);
}
let _g19 = 0;
let _g20 = this.numSolvers;
while(_g19 < _g20) this.solvers[_g19++].postSolve();
let _g21 = 0;
let _g22 = this.numRigidBodies;
while(_g21 < _g22) {
let rb = this.rigidBodies[_g21++];
let s = rb._shapeList;
while(s != null) {
let n = s._next;
let tf1 = rb._ptransform;
let tf2 = rb._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
}
}
}
oimo.dynamics.TimeStep = class oimo_dynamics_TimeStep {
constructor() {
this.dt = 0;
this.invDt = 0;
this.dtRatio = 1;
}
}
oimo.dynamics.World = class oimo_dynamics_World {
constructor(broadPhaseType,gravity) {
if(broadPhaseType == null) {
broadPhaseType = 2;
}
switch(broadPhaseType) {
case 1:
this._broadPhase = new oimo.collision.broadphase.bruteforce.BruteForceBroadPhase();
break;
case 2:
this._broadPhase = new oimo.collision.broadphase.bvh.BvhBroadPhase();
break;
}
this._contactManager = new oimo.dynamics.ContactManager(this._broadPhase);
if(gravity == null) {
gravity = new oimo.common.Vec3(0,-9.80665,0);
}
this._gravity = new oimo.common.Vec3(gravity.x,gravity.y,gravity.z);
this._rigidBodyList = null;
this._rigidBodyListLast = null;
this._jointList = null;
this._jointListLast = null;
this._numRigidBodies = 0;
this._numShapes = 0;
this._numJoints = 0;
this._numIslands = 0;
this._numVelocityIterations = 10;
this._numPositionIterations = 5;
this._rayCastWrapper = new oimo.dynamics._World.RayCastWrapper();
this._convexCastWrapper = new oimo.dynamics._World.ConvexCastWrapper();
this._aabbTestWrapper = new oimo.dynamics._World.AabbTestWrapper();
this._island = new oimo.dynamics.Island();
this._solversInIslands = new Array(oimo.common.Setting.islandInitialConstraintArraySize);
this._rigidBodyStack = new Array(oimo.common.Setting.islandInitialRigidBodyArraySize);
this._timeStep = new oimo.dynamics.TimeStep();
this._pool = new oimo.common.Pool();
this._shapeIdCount = 0;
}
_updateContacts() {
let st = Date.now() / 1000;
this._contactManager._updateContacts();
oimo.dynamics.common.Performance.broadPhaseCollisionTime = (Date.now() / 1000 - st) * 1000;
let st1 = Date.now() / 1000;
let c = this._contactManager._contactList;
while(c != null) {
let n = c._next;
if(!c._shouldBeSkipped) {
c._updateManifold();
}
c = n;
}
oimo.dynamics.common.Performance.narrowPhaseCollisionTime = (Date.now() / 1000 - st1) * 1000;
}
_solveIslands() {
let st = Date.now() / 1000;
if(oimo.common.Setting.disableSleeping) {
let b = this._rigidBodyList;
while(b != null) {
b._sleeping = false;
b._sleepTime = 0;
b = b._next;
}
}
if(this._rigidBodyStack.length < this._numRigidBodies) {
let newStackSize = this._rigidBodyStack.length << 1;
while(newStackSize < this._numRigidBodies) newStackSize <<= 1;
this._rigidBodyStack = new Array(newStackSize);
}
this._numIslands = 0;
let _this = this._island;
let gravity = this._gravity;
_this.gravityX = gravity.x;
_this.gravityY = gravity.y;
_this.gravityZ = gravity.z;
let b = this._rigidBodyList;
this._numSolversInIslands = 0;
while(b != null) {
let n = b._next;
while(!(b._addedToIsland || b._sleeping || b._type == 1)) {
if(b._numContactLinks == 0 && b._numJointLinks == 0) {
this._island._stepSingleRigidBody(this._timeStep,b);
this._numIslands++;
break;
}
this.buildIsland(b);
this._island._step(this._timeStep,this._numVelocityIterations,this._numPositionIterations);
this._island._clear();
this._numIslands++;
break;
}
b = n;
}
this._contactManager._postSolve();
b = this._rigidBodyList;
while(b != null) {
b._addedToIsland = false;
b = b._next;
}
b = this._rigidBodyList;
while(b != null) {
b._forceX = 0;
b._forceY = 0;
b._forceZ = 0;
b._torqueX = 0;
b._torqueY = 0;
b._torqueZ = 0;
b = b._next;
}
while(this._numSolversInIslands > 0) {
this._solversInIslands[--this._numSolversInIslands]._addedToIsland = false;
this._solversInIslands[this._numSolversInIslands] = null;
}
oimo.dynamics.common.Performance.dynamicsTime = (Date.now() / 1000 - st) * 1000;
}
buildIsland(base) {
let stackCount = 1;
this._island._addRigidBody(base);
this._rigidBodyStack[0] = base;
while(stackCount > 0) {
let rb = this._rigidBodyStack[--stackCount];
this._rigidBodyStack[stackCount] = null;
if(rb._type == 1) {
continue;
}
let cl = rb._contactLinkList;
while(cl != null) {
let n = cl._next;
let cc = cl._contact._contactConstraint;
let ccs = cl._contact._contactConstraint._solver;
if(cc.isTouching() && !ccs._addedToIsland) {
if(this._solversInIslands.length == this._numSolversInIslands) {
let newArray = new Array(this._numSolversInIslands << 1);
let _g = 0;
let _g1 = this._numSolversInIslands;
while(_g < _g1) {
let i = _g++;
newArray[i] = this._solversInIslands[i];
this._solversInIslands[i] = null;
}
this._solversInIslands = newArray;
}
this._solversInIslands[this._numSolversInIslands++] = ccs;
this._island._addConstraintSolver(ccs,cc._positionCorrectionAlgorithm);
let other = cl._other;
if(!other._addedToIsland) {
this._island._addRigidBody(other);
this._rigidBodyStack[stackCount++] = other;
}
}
cl = n;
}
let jl = rb._jointLinkList;
while(jl != null) {
let n = jl._next;
let j = jl._joint;
let js1 = j._solver;
if(!js1._addedToIsland) {
if(this._solversInIslands.length == this._numSolversInIslands) {
let newArray = new Array(this._numSolversInIslands << 1);
let _g = 0;
let _g1 = this._numSolversInIslands;
while(_g < _g1) {
let i = _g++;
newArray[i] = this._solversInIslands[i];
this._solversInIslands[i] = null;
}
this._solversInIslands = newArray;
}
this._solversInIslands[this._numSolversInIslands++] = js1;
this._island._addConstraintSolver(js1,j._positionCorrectionAlgorithm);
let other = jl._other;
if(!other._addedToIsland) {
this._island._addRigidBody(other);
this._rigidBodyStack[stackCount++] = other;
}
}
jl = n;
}
}
}
_drawBvh(d,tree) {
if(d.drawBvh) {
this._drawBvhNode(d,tree._root,0,d.style.bvhNodeColor);
}
}
_drawBvhNode(d,node,level,color) {
if(node == null) {
return;
}
if(level >= d.drawBvhMinLevel && level <= d.drawBvhMaxLevel) {
let _this = this._pool;
let min = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this._pool;
let max = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let v = min;
v.x = node._aabbMinX;
v.y = node._aabbMinY;
v.z = node._aabbMinZ;
let v1 = max;
v1.x = node._aabbMaxX;
v1.y = node._aabbMaxY;
v1.z = node._aabbMaxZ;
d.aabb(min,max,color);
let _this2 = this._pool;
if(min != null) {
min.zero();
if(_this2.sizeVec3 == _this2.stackVec3.length) {
let newArray = new Array(_this2.sizeVec3 << 1);
let _g = 0;
let _g1 = _this2.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this2.stackVec3[i];
_this2.stackVec3[i] = null;
}
_this2.stackVec3 = newArray;
}
_this2.stackVec3[_this2.sizeVec3++] = min;
}
let _this3 = this._pool;
if(max != null) {
max.zero();
if(_this3.sizeVec3 == _this3.stackVec3.length) {
let newArray = new Array(_this3.sizeVec3 << 1);
let _g = 0;
let _g1 = _this3.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this3.stackVec3[i];
_this3.stackVec3[i] = null;
}
_this3.stackVec3 = newArray;
}
_this3.stackVec3[_this3.sizeVec3++] = max;
}
}
this._drawBvhNode(d,node._children[0],level + 1,color);
this._drawBvhNode(d,node._children[1],level + 1,color);
}
_drawRigidBodies(d) {
let style = d.style;
let r = this._rigidBodyList;
while(r != null) {
let n = r._next;
if(d.drawBases) {
let style = d.style;
d.basis(r._transform,style.basisLength,style.basisColorX,style.basisColorY,style.basisColorZ);
}
let shapeColor = null;
let isDynamic = r._type == 0;
if(!isDynamic) {
shapeColor = r._type == 2 ? style.kinematicShapeColor : style.staticShapeColor;
}
let s = r._shapeList;
while(s != null) {
let n = s._next;
if(isDynamic) {
if((s._id & 1) == 0) {
shapeColor = r._sleeping ? style.sleepingShapeColor1 : r._sleepTime > oimo.common.Setting.sleepingTimeThreshold ? style.sleepyShapeColor1 : style.shapeColor1;
} else {
shapeColor = r._sleeping ? style.sleepingShapeColor2 : r._sleepTime > oimo.common.Setting.sleepingTimeThreshold ? style.sleepyShapeColor2 : style.shapeColor2;
}
}
if(d.drawShapes) {
let geom = s._geom;
let tf = s._transform;
switch(geom._type) {
case 0:
d.sphere(tf,geom._radius,shapeColor);
break;
case 1:
let g = geom;
let _this = this._pool;
let hx = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let v = hx;
v.x = g._halfExtentsX;
v.y = g._halfExtentsY;
v.z = g._halfExtentsZ;
d.box(tf,hx,shapeColor);
let _this1 = this._pool;
if(hx != null) {
hx.zero();
if(_this1.sizeVec3 == _this1.stackVec3.length) {
let newArray = new Array(_this1.sizeVec3 << 1);
let _g = 0;
let _g1 = _this1.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this1.stackVec3[i];
_this1.stackVec3[i] = null;
}
_this1.stackVec3 = newArray;
}
_this1.stackVec3[_this1.sizeVec3++] = hx;
}
break;
case 2:
let g1 = geom;
d.cylinder(tf,g1._radius,g1._halfHeight,shapeColor);
break;
case 3:
let g2 = geom;
d.cone(tf,g2._radius,g2._halfHeight,shapeColor);
break;
case 4:
let g3 = geom;
d.capsule(tf,g3._radius,g3._halfHeight,shapeColor);
break;
case 5:
let g4 = geom;
let n = g4._numVertices;
let _this2 = this._pool;
let v1 = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
let _this3 = this._pool;
let v2 = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _this4 = this._pool;
let v3 = _this4.sizeVec3 == 0 ? new oimo.common.Vec3() : _this4.stackVec3[--_this4.sizeVec3];
let _this5 = this._pool;
let v12 = _this5.sizeVec3 == 0 ? new oimo.common.Vec3() : _this5.stackVec3[--_this5.sizeVec3];
let _this6 = this._pool;
let v13 = _this6.sizeVec3 == 0 ? new oimo.common.Vec3() : _this6.stackVec3[--_this6.sizeVec3];
let _this7 = this._pool;
let normal = _this7.sizeVec3 == 0 ? new oimo.common.Vec3() : _this7.stackVec3[--_this7.sizeVec3];
let _this8 = this._pool;
let m = _this8.sizeMat3 == 0 ? new oimo.common.Mat3() : _this8.stackMat3[--_this8.sizeMat3];
let _this9 = this._pool;
let o = _this9.sizeVec3 == 0 ? new oimo.common.Vec3() : _this9.stackVec3[--_this9.sizeVec3];
let m1 = m;
m1.e00 = tf._rotation00;
m1.e01 = tf._rotation01;
m1.e02 = tf._rotation02;
m1.e10 = tf._rotation10;
m1.e11 = tf._rotation11;
m1.e12 = tf._rotation12;
m1.e20 = tf._rotation20;
m1.e21 = tf._rotation21;
m1.e22 = tf._rotation22;
let v4 = o;
v4.x = tf._positionX;
v4.y = tf._positionY;
v4.z = tf._positionZ;
let _g = 0;
while(_g < n) {
let i = _g++;
let _this = g4._tmpVertices[i];
let v = g4._vertices[i];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let y = _this.x * m.e10 + _this.y * m.e11 + _this.z * m.e12;
let z = _this.x * m.e20 + _this.y * m.e21 + _this.z * m.e22;
_this.x = _this.x * m.e00 + _this.y * m.e01 + _this.z * m.e02;
_this.y = y;
_this.z = z;
_this.x += o.x;
_this.y += o.y;
_this.z += o.z;
}
if(n > 30) {
let _g = 0;
while(_g < n) {
let i = _g++;
let v = g4._tmpVertices[i];
v1.x = v.x;
v1.y = v.y;
v1.z = v.z;
let v3 = g4._tmpVertices[(i + 1) % n];
v2.x = v3.x;
v2.y = v3.y;
v2.z = v3.z;
d.line(v1,v2,shapeColor);
}
} else if(this._debugDraw.wireframe || n > 10) {
let _g = 0;
while(_g < n) {
let i = _g++;
let v = g4._tmpVertices[i];
v1.x = v.x;
v1.y = v.y;
v1.z = v.z;
let _g1 = 0;
while(_g1 < i) {
let v = g4._tmpVertices[_g1++];
v2.x = v.x;
v2.y = v.y;
v2.z = v.z;
d.line(v1,v2,shapeColor);
}
}
} else {
let _g = 0;
while(_g < n) {
let i = _g++;
let v = g4._tmpVertices[i];
v1.x = v.x;
v1.y = v.y;
v1.z = v.z;
let _g1 = 0;
while(_g1 < i) {
let j = _g1++;
let v = g4._tmpVertices[j];
v2.x = v.x;
v2.y = v.y;
v2.z = v.z;
let _g = 0;
while(_g < j) {
let v = g4._tmpVertices[_g++];
v3.x = v.x;
v3.y = v.y;
v3.z = v.z;
v12.x = v2.x;
v12.y = v2.y;
v12.z = v2.z;
let _this = v12;
_this.x -= v1.x;
_this.y -= v1.y;
_this.z -= v1.z;
v13.x = v3.x;
v13.y = v3.y;
v13.z = v3.z;
let _this1 = v13;
_this1.x -= v1.x;
_this1.y -= v1.y;
_this1.z -= v1.z;
normal.x = v12.x;
normal.y = v12.y;
normal.z = v12.z;
let _this2 = normal;
let y = _this2.z * v13.x - _this2.x * v13.z;
let z = _this2.x * v13.y - _this2.y * v13.x;
_this2.x = _this2.y * v13.z - _this2.z * v13.y;
_this2.y = y;
_this2.z = z;
let invLen = Math.sqrt(_this2.x * _this2.x + _this2.y * _this2.y + _this2.z * _this2.z);
if(invLen > 0) {
invLen = 1 / invLen;
}
_this2.x *= invLen;
_this2.y *= invLen;
_this2.z *= invLen;
d.triangle(v1,v2,v3,normal,normal,normal,shapeColor);
normal.x = -normal.x;
normal.y = -normal.y;
normal.z = -normal.z;
d.triangle(v1,v3,v2,normal,normal,normal,shapeColor);
}
}
}
}
let _this10 = this._pool;
if(v1 != null) {
v1.zero();
if(_this10.sizeVec3 == _this10.stackVec3.length) {
let newArray = new Array(_this10.sizeVec3 << 1);
let _g = 0;
let _g1 = _this10.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this10.stackVec3[i];
_this10.stackVec3[i] = null;
}
_this10.stackVec3 = newArray;
}
_this10.stackVec3[_this10.sizeVec3++] = v1;
}
let _this11 = this._pool;
if(v2 != null) {
v2.zero();
if(_this11.sizeVec3 == _this11.stackVec3.length) {
let newArray = new Array(_this11.sizeVec3 << 1);
let _g = 0;
let _g1 = _this11.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this11.stackVec3[i];
_this11.stackVec3[i] = null;
}
_this11.stackVec3 = newArray;
}
_this11.stackVec3[_this11.sizeVec3++] = v2;
}
let _this12 = this._pool;
if(v3 != null) {
v3.zero();
if(_this12.sizeVec3 == _this12.stackVec3.length) {
let newArray = new Array(_this12.sizeVec3 << 1);
let _g = 0;
let _g1 = _this12.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this12.stackVec3[i];
_this12.stackVec3[i] = null;
}
_this12.stackVec3 = newArray;
}
_this12.stackVec3[_this12.sizeVec3++] = v3;
}
let _this13 = this._pool;
if(v12 != null) {
v12.zero();
if(_this13.sizeVec3 == _this13.stackVec3.length) {
let newArray = new Array(_this13.sizeVec3 << 1);
let _g = 0;
let _g1 = _this13.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this13.stackVec3[i];
_this13.stackVec3[i] = null;
}
_this13.stackVec3 = newArray;
}
_this13.stackVec3[_this13.sizeVec3++] = v12;
}
let _this14 = this._pool;
if(v13 != null) {
v13.zero();
if(_this14.sizeVec3 == _this14.stackVec3.length) {
let newArray = new Array(_this14.sizeVec3 << 1);
let _g = 0;
let _g1 = _this14.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this14.stackVec3[i];
_this14.stackVec3[i] = null;
}
_this14.stackVec3 = newArray;
}
_this14.stackVec3[_this14.sizeVec3++] = v13;
}
let _this15 = this._pool;
if(normal != null) {
normal.zero();
if(_this15.sizeVec3 == _this15.stackVec3.length) {
let newArray = new Array(_this15.sizeVec3 << 1);
let _g = 0;
let _g1 = _this15.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this15.stackVec3[i];
_this15.stackVec3[i] = null;
}
_this15.stackVec3 = newArray;
}
_this15.stackVec3[_this15.sizeVec3++] = normal;
}
let _this16 = this._pool;
if(m != null) {
m.e00 = 1;
m.e01 = 0;
m.e02 = 0;
m.e10 = 0;
m.e11 = 1;
m.e12 = 0;
m.e20 = 0;
m.e21 = 0;
m.e22 = 1;
if(_this16.sizeMat3 == _this16.stackMat3.length) {
let newArray = new Array(_this16.sizeMat3 << 1);
let _g = 0;
let _g1 = _this16.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this16.stackMat3[i];
_this16.stackMat3[i] = null;
}
_this16.stackMat3 = newArray;
}
_this16.stackMat3[_this16.sizeMat3++] = m;
}
let _this17 = this._pool;
if(o != null) {
o.zero();
if(_this17.sizeVec3 == _this17.stackVec3.length) {
let newArray = new Array(_this17.sizeVec3 << 1);
let _g = 0;
let _g1 = _this17.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this17.stackVec3[i];
_this17.stackVec3[i] = null;
}
_this17.stackVec3 = newArray;
}
_this17.stackVec3[_this17.sizeVec3++] = o;
}
break;
}
}
if(d.drawAabbs) {
let aabb = s._aabb;
let color = style.aabbColor;
let _this = this._pool;
let min = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this._pool;
let max = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let v = min;
v.x = aabb._minX;
v.y = aabb._minY;
v.z = aabb._minZ;
let v1 = max;
v1.x = aabb._maxX;
v1.y = aabb._maxY;
v1.z = aabb._maxZ;
d.aabb(min,max,color);
let _this2 = this._pool;
if(min != null) {
min.zero();
if(_this2.sizeVec3 == _this2.stackVec3.length) {
let newArray = new Array(_this2.sizeVec3 << 1);
let _g = 0;
let _g1 = _this2.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this2.stackVec3[i];
_this2.stackVec3[i] = null;
}
_this2.stackVec3 = newArray;
}
_this2.stackVec3[_this2.sizeVec3++] = min;
}
let _this3 = this._pool;
if(max != null) {
max.zero();
if(_this3.sizeVec3 == _this3.stackVec3.length) {
let newArray = new Array(_this3.sizeVec3 << 1);
let _g = 0;
let _g1 = _this3.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this3.stackVec3[i];
_this3.stackVec3[i] = null;
}
_this3.stackVec3 = newArray;
}
_this3.stackVec3[_this3.sizeVec3++] = max;
}
}
s = n;
}
r = n;
}
}
_drawConstraints(d) {
let style = d.style;
if(d.drawPairs || d.drawContacts) {
let c = this._contactManager._contactList;
while(c != null) {
let n = c._next;
if(d.drawPairs) {
let color = style.pairColor;
let _this = this._pool;
let v1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this._pool;
let v2 = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let v = v1;
v.x = c._s1._transform._positionX;
v.y = c._s1._transform._positionY;
v.z = c._s1._transform._positionZ;
let v3 = v2;
v3.x = c._s2._transform._positionX;
v3.y = c._s2._transform._positionY;
v3.z = c._s2._transform._positionZ;
d.line(v1,v2,color);
let _this2 = this._pool;
if(v1 != null) {
v1.zero();
if(_this2.sizeVec3 == _this2.stackVec3.length) {
let newArray = new Array(_this2.sizeVec3 << 1);
let _g = 0;
let _g1 = _this2.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this2.stackVec3[i];
_this2.stackVec3[i] = null;
}
_this2.stackVec3 = newArray;
}
_this2.stackVec3[_this2.sizeVec3++] = v1;
}
let _this3 = this._pool;
if(v2 != null) {
v2.zero();
if(_this3.sizeVec3 == _this3.stackVec3.length) {
let newArray = new Array(_this3.sizeVec3 << 1);
let _g = 0;
let _g1 = _this3.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this3.stackVec3[i];
_this3.stackVec3[i] = null;
}
_this3.stackVec3 = newArray;
}
_this3.stackVec3[_this3.sizeVec3++] = v2;
}
}
if(d.drawContacts) {
let cc = c._contactConstraint;
let ps = c._contactConstraint._manifold._points;
let _g = 0;
let _g1 = c._contactConstraint._manifold._numPoints;
while(_g < _g1) {
let p = ps[_g++];
let style = d.style;
let _this = this._pool;
let pos1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this._pool;
let pos2 = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let _this2 = this._pool;
let normal = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
let _this3 = this._pool;
let tangent = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _this4 = this._pool;
let binormal = _this4.sizeVec3 == 0 ? new oimo.common.Vec3() : _this4.stackVec3[--_this4.sizeVec3];
let v = pos1;
v.x = p._pos1X;
v.y = p._pos1Y;
v.z = p._pos1Z;
let v1 = pos2;
v1.x = p._pos2X;
v1.y = p._pos2Y;
v1.z = p._pos2Z;
let v2 = normal;
v2.x = cc._manifold._normalX;
v2.y = cc._manifold._normalY;
v2.z = cc._manifold._normalZ;
let v3 = tangent;
v3.x = cc._manifold._tangentX;
v3.y = cc._manifold._tangentY;
v3.z = cc._manifold._tangentZ;
let v4 = binormal;
v4.x = cc._manifold._binormalX;
v4.y = cc._manifold._binormalY;
v4.z = cc._manifold._binormalZ;
if(p._disabled) {
d.point(pos1,style.disabledContactColor);
d.point(pos2,style.disabledContactColor);
d.line(pos1,pos2,style.disabledContactColor);
} else if(p._warmStarted) {
let color;
switch(p._id & 3) {
case 0:
color = style.contactColor;
break;
case 1:
color = style.contactColor2;
break;
case 2:
color = style.contactColor3;
break;
default:
color = style.contactColor4;
}
d.point(pos1,color);
d.point(pos2,color);
d.line(pos1,pos2,style.contactColor);
} else {
d.point(pos1,style.newContactColor);
d.point(pos2,style.newContactColor);
d.line(pos1,pos2,style.newContactColor);
}
pos2.x = pos1.x;
pos2.y = pos1.y;
pos2.z = pos1.z;
let _this5 = pos2;
let s = style.contactNormalLength;
_this5.x += normal.x * s;
_this5.y += normal.y * s;
_this5.z += normal.z * s;
d.line(pos1,pos2,style.contactNormalColor);
if(d.drawContactBases) {
pos2.x = pos1.x;
pos2.y = pos1.y;
pos2.z = pos1.z;
let _this = pos2;
let s = style.contactTangentLength;
_this.x += tangent.x * s;
_this.y += tangent.y * s;
_this.z += tangent.z * s;
d.line(pos1,pos2,style.contactTangentColor);
pos2.x = pos1.x;
pos2.y = pos1.y;
pos2.z = pos1.z;
let _this1 = pos2;
let s1 = style.contactBinormalLength;
_this1.x += binormal.x * s1;
_this1.y += binormal.y * s1;
_this1.z += binormal.z * s1;
d.line(pos1,pos2,style.contactBinormalColor);
}
let _this6 = this._pool;
if(pos1 != null) {
pos1.zero();
if(_this6.sizeVec3 == _this6.stackVec3.length) {
let newArray = new Array(_this6.sizeVec3 << 1);
let _g = 0;
let _g1 = _this6.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this6.stackVec3[i];
_this6.stackVec3[i] = null;
}
_this6.stackVec3 = newArray;
}
_this6.stackVec3[_this6.sizeVec3++] = pos1;
}
let _this7 = this._pool;
if(pos2 != null) {
pos2.zero();
if(_this7.sizeVec3 == _this7.stackVec3.length) {
let newArray = new Array(_this7.sizeVec3 << 1);
let _g = 0;
let _g1 = _this7.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this7.stackVec3[i];
_this7.stackVec3[i] = null;
}
_this7.stackVec3 = newArray;
}
_this7.stackVec3[_this7.sizeVec3++] = pos2;
}
let _this8 = this._pool;
if(normal != null) {
normal.zero();
if(_this8.sizeVec3 == _this8.stackVec3.length) {
let newArray = new Array(_this8.sizeVec3 << 1);
let _g = 0;
let _g1 = _this8.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this8.stackVec3[i];
_this8.stackVec3[i] = null;
}
_this8.stackVec3 = newArray;
}
_this8.stackVec3[_this8.sizeVec3++] = normal;
}
let _this9 = this._pool;
if(tangent != null) {
tangent.zero();
if(_this9.sizeVec3 == _this9.stackVec3.length) {
let newArray = new Array(_this9.sizeVec3 << 1);
let _g = 0;
let _g1 = _this9.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this9.stackVec3[i];
_this9.stackVec3[i] = null;
}
_this9.stackVec3 = newArray;
}
_this9.stackVec3[_this9.sizeVec3++] = tangent;
}
let _this10 = this._pool;
if(binormal != null) {
binormal.zero();
if(_this10.sizeVec3 == _this10.stackVec3.length) {
let newArray = new Array(_this10.sizeVec3 << 1);
let _g = 0;
let _g1 = _this10.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this10.stackVec3[i];
_this10.stackVec3[i] = null;
}
_this10.stackVec3 = newArray;
}
_this10.stackVec3[_this10.sizeVec3++] = binormal;
}
}
}
c = n;
}
}
if(d.drawJoints) {
let j = this._jointList;
while(j != null) {
let n = j._next;
let _this = this._pool;
let p1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this._pool;
let p2 = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let v = p1;
v.x = j._b1._transform._positionX;
v.y = j._b1._transform._positionY;
v.z = j._b1._transform._positionZ;
let v1 = p2;
v1.x = j._b2._transform._positionX;
v1.y = j._b2._transform._positionY;
v1.z = j._b2._transform._positionZ;
let _this2 = this._pool;
let anchor1 = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
let _this3 = this._pool;
let anchor2 = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _this4 = this._pool;
let basisX1 = _this4.sizeVec3 == 0 ? new oimo.common.Vec3() : _this4.stackVec3[--_this4.sizeVec3];
let _this5 = this._pool;
let basisY1 = _this5.sizeVec3 == 0 ? new oimo.common.Vec3() : _this5.stackVec3[--_this5.sizeVec3];
let _this6 = this._pool;
let basisZ1 = _this6.sizeVec3 == 0 ? new oimo.common.Vec3() : _this6.stackVec3[--_this6.sizeVec3];
let _this7 = this._pool;
let basisX2 = _this7.sizeVec3 == 0 ? new oimo.common.Vec3() : _this7.stackVec3[--_this7.sizeVec3];
let _this8 = this._pool;
let basisY2 = _this8.sizeVec3 == 0 ? new oimo.common.Vec3() : _this8.stackVec3[--_this8.sizeVec3];
let _this9 = this._pool;
let basisZ2 = _this9.sizeVec3 == 0 ? new oimo.common.Vec3() : _this9.stackVec3[--_this9.sizeVec3];
let v2 = anchor1;
v2.x = j._anchor1X;
v2.y = j._anchor1Y;
v2.z = j._anchor1Z;
let v3 = anchor2;
v3.x = j._anchor2X;
v3.y = j._anchor2Y;
v3.z = j._anchor2Z;
let v4 = basisX1;
v4.x = j._basisX1X;
v4.y = j._basisX1Y;
v4.z = j._basisX1Z;
let v5 = basisY1;
v5.x = j._basisY1X;
v5.y = j._basisY1Y;
v5.z = j._basisY1Z;
let v6 = basisZ1;
v6.x = j._basisZ1X;
v6.y = j._basisZ1Y;
v6.z = j._basisZ1Z;
let v7 = basisX2;
v7.x = j._basisX2X;
v7.y = j._basisX2Y;
v7.z = j._basisX2Z;
let v8 = basisY2;
v8.x = j._basisY2X;
v8.y = j._basisY2Y;
v8.z = j._basisY2Z;
let v9 = basisZ2;
v9.x = j._basisZ2X;
v9.y = j._basisZ2Y;
v9.z = j._basisZ2Z;
d.line(p1,anchor1,d.style.jointLineColor);
d.line(p2,anchor2,d.style.jointLineColor);
if(d.drawJointLimits) {
switch(j._type) {
case 0:
break;
case 1:
let lm = j._lm;
this._drawRotationalLimit(d,anchor1,basisY1,basisZ1,basisY2,d.style.jointRotationalConstraintRadius,lm.lowerLimit,lm.upperLimit,d.style.jointLineColor);
break;
case 2:
let j1 = j;
let color = d.style.jointLineColor;
let rlm = j1._rotLm;
let tlm = j1._translLm;
this._drawRotationalLimit(d,anchor2,basisY1,basisZ1,basisY2,d.style.jointRotationalConstraintRadius,rlm.lowerLimit,rlm.upperLimit,color);
this._drawTranslationalLimit(d,anchor1,basisX1,tlm.lowerLimit,tlm.upperLimit,color);
break;
case 3:
let lm1 = j._lm;
this._drawTranslationalLimit(d,anchor1,basisX1,lm1.lowerLimit,lm1.upperLimit,d.style.jointLineColor);
break;
case 4:
let j2 = j;
let radius = d.style.jointRotationalConstraintRadius;
let color1 = d.style.jointLineColor;
let lm11 = j2._lm1;
let lm2 = j2._lm2;
this._drawRotationalLimit(d,anchor1,basisY1,basisZ1,basisY1,radius,j2._angleX - lm11.upperLimit,j2._angleX - lm11.lowerLimit,color1);
this._drawRotationalLimit(d,anchor2,basisX2,basisY2,basisX2,radius,lm2.lowerLimit - j2._angleZ,lm2.upperLimit - j2._angleZ,color1);
break;
case 5:
let j3 = j;
let radius1 = d.style.jointRotationalConstraintRadius;
let color2 = d.style.jointLineColor;
let lm3 = j3._twistLm;
this._drawRotationalLimit(d,anchor2,basisY2,basisZ2,basisY2,radius1,lm3.lowerLimit - j3._twistAngle,lm3.upperLimit - j3._twistAngle,color2);
this._drawEllipseOnSphere(d,anchor1,basisX1,basisY1,basisZ1,j3._maxSwingAngle1,j3._maxSwingAngle2,radius1,color2);
let _this10 = this._pool;
let _this11 = _this10.sizeVec3 == 0 ? new oimo.common.Vec3() : _this10.stackVec3[--_this10.sizeVec3];
_this11.x = anchor2.x;
_this11.y = anchor2.y;
_this11.z = anchor2.z;
let _this12 = _this11;
_this12.x += basisX2.x * radius1;
_this12.y += basisX2.y * radius1;
_this12.z += basisX2.z * radius1;
d.line(anchor2,_this12,color2);
let _this13 = this._pool;
if(_this12 != null) {
_this12.zero();
if(_this13.sizeVec3 == _this13.stackVec3.length) {
let newArray = new Array(_this13.sizeVec3 << 1);
let _g = 0;
let _g1 = _this13.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this13.stackVec3[i];
_this13.stackVec3[i] = null;
}
_this13.stackVec3 = newArray;
}
_this13.stackVec3[_this13.sizeVec3++] = _this12;
}
break;
case 6:
let j4 = j;
let radius2 = d.style.jointRotationalConstraintRadius;
let color3 = d.style.jointLineColor;
let rxlm = j4._rotLms[0];
let rylm = j4._rotLms[1];
let rzlm = j4._rotLms[2];
this._drawTranslationalLimit3D(d,anchor1,basisX1,basisY1,basisZ1,j4._translLms[0],j4._translLms[1],j4._translLms[2],color3);
let _this14 = this._pool;
let rotYAxis = _this14.sizeVec3 == 0 ? new oimo.common.Vec3() : _this14.stackVec3[--_this14.sizeVec3];
let v10 = rotYAxis;
v10.x = j4._axisYX;
v10.y = j4._axisYY;
v10.z = j4._axisYZ;
let _this15 = this._pool;
let _this16 = _this15.sizeVec3 == 0 ? new oimo.common.Vec3() : _this15.stackVec3[--_this15.sizeVec3];
_this16.x = basisX1.x;
_this16.y = basisX1.y;
_this16.z = basisX1.z;
let rotYBasisX = _this16;
let _this17 = this._pool;
let _this18 = _this17.sizeVec3 == 0 ? new oimo.common.Vec3() : _this17.stackVec3[--_this17.sizeVec3];
_this18.x = basisX1.x;
_this18.y = basisX1.y;
_this18.z = basisX1.z;
let _this19 = _this18;
let y = _this19.z * rotYAxis.x - _this19.x * rotYAxis.z;
let z = _this19.x * rotYAxis.y - _this19.y * rotYAxis.x;
_this19.x = _this19.y * rotYAxis.z - _this19.z * rotYAxis.y;
_this19.y = y;
_this19.z = z;
this._drawRotationalLimit(d,anchor2,basisY1,basisZ1,basisY1,radius2,j4._angleX - rxlm.upperLimit,j4._angleX - rxlm.lowerLimit,color3);
this._drawRotationalLimit(d,anchor2,rotYBasisX,_this19,rotYBasisX,radius2,rylm.lowerLimit - j4._angleY,rylm.upperLimit - j4._angleY,color3);
this._drawRotationalLimit(d,anchor2,basisX2,basisY2,basisX2,radius2,rzlm.lowerLimit - j4._angleZ,rzlm.upperLimit - j4._angleZ,color3);
break;
}
}
d.line(anchor1,anchor2,d.style.jointErrorColor);
let _this20 = this._pool;
if(p1 != null) {
p1.zero();
if(_this20.sizeVec3 == _this20.stackVec3.length) {
let newArray = new Array(_this20.sizeVec3 << 1);
let _g = 0;
let _g1 = _this20.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this20.stackVec3[i];
_this20.stackVec3[i] = null;
}
_this20.stackVec3 = newArray;
}
_this20.stackVec3[_this20.sizeVec3++] = p1;
}
let _this21 = this._pool;
if(p2 != null) {
p2.zero();
if(_this21.sizeVec3 == _this21.stackVec3.length) {
let newArray = new Array(_this21.sizeVec3 << 1);
let _g = 0;
let _g1 = _this21.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this21.stackVec3[i];
_this21.stackVec3[i] = null;
}
_this21.stackVec3 = newArray;
}
_this21.stackVec3[_this21.sizeVec3++] = p2;
}
let _this22 = this._pool;
if(anchor1 != null) {
anchor1.zero();
if(_this22.sizeVec3 == _this22.stackVec3.length) {
let newArray = new Array(_this22.sizeVec3 << 1);
let _g = 0;
let _g1 = _this22.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this22.stackVec3[i];
_this22.stackVec3[i] = null;
}
_this22.stackVec3 = newArray;
}
_this22.stackVec3[_this22.sizeVec3++] = anchor1;
}
let _this23 = this._pool;
if(anchor2 != null) {
anchor2.zero();
if(_this23.sizeVec3 == _this23.stackVec3.length) {
let newArray = new Array(_this23.sizeVec3 << 1);
let _g = 0;
let _g1 = _this23.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this23.stackVec3[i];
_this23.stackVec3[i] = null;
}
_this23.stackVec3 = newArray;
}
_this23.stackVec3[_this23.sizeVec3++] = anchor2;
}
let _this24 = this._pool;
if(basisX1 != null) {
basisX1.zero();
if(_this24.sizeVec3 == _this24.stackVec3.length) {
let newArray = new Array(_this24.sizeVec3 << 1);
let _g = 0;
let _g1 = _this24.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this24.stackVec3[i];
_this24.stackVec3[i] = null;
}
_this24.stackVec3 = newArray;
}
_this24.stackVec3[_this24.sizeVec3++] = basisX1;
}
let _this25 = this._pool;
if(basisY1 != null) {
basisY1.zero();
if(_this25.sizeVec3 == _this25.stackVec3.length) {
let newArray = new Array(_this25.sizeVec3 << 1);
let _g = 0;
let _g1 = _this25.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this25.stackVec3[i];
_this25.stackVec3[i] = null;
}
_this25.stackVec3 = newArray;
}
_this25.stackVec3[_this25.sizeVec3++] = basisY1;
}
let _this26 = this._pool;
if(basisZ1 != null) {
basisZ1.zero();
if(_this26.sizeVec3 == _this26.stackVec3.length) {
let newArray = new Array(_this26.sizeVec3 << 1);
let _g = 0;
let _g1 = _this26.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this26.stackVec3[i];
_this26.stackVec3[i] = null;
}
_this26.stackVec3 = newArray;
}
_this26.stackVec3[_this26.sizeVec3++] = basisZ1;
}
let _this27 = this._pool;
if(basisX2 != null) {
basisX2.zero();
if(_this27.sizeVec3 == _this27.stackVec3.length) {
let newArray = new Array(_this27.sizeVec3 << 1);
let _g = 0;
let _g1 = _this27.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this27.stackVec3[i];
_this27.stackVec3[i] = null;
}
_this27.stackVec3 = newArray;
}
_this27.stackVec3[_this27.sizeVec3++] = basisX2;
}
let _this28 = this._pool;
if(basisY2 != null) {
basisY2.zero();
if(_this28.sizeVec3 == _this28.stackVec3.length) {
let newArray = new Array(_this28.sizeVec3 << 1);
let _g = 0;
let _g1 = _this28.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this28.stackVec3[i];
_this28.stackVec3[i] = null;
}
_this28.stackVec3 = newArray;
}
_this28.stackVec3[_this28.sizeVec3++] = basisY2;
}
let _this29 = this._pool;
if(basisZ2 != null) {
basisZ2.zero();
if(_this29.sizeVec3 == _this29.stackVec3.length) {
let newArray = new Array(_this29.sizeVec3 << 1);
let _g = 0;
let _g1 = _this29.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this29.stackVec3[i];
_this29.stackVec3[i] = null;
}
_this29.stackVec3 = newArray;
}
_this29.stackVec3[_this29.sizeVec3++] = basisZ2;
}
j = n;
}
}
}
_drawRotationalLimit(d,center,ex,ey,needle,radius,min,max,color) {
if(min != max) {
let _this = this._pool;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = center.x;
_this1.y = center.y;
_this1.z = center.z;
let _this2 = _this1;
_this2.x += needle.x * radius;
_this2.y += needle.y * radius;
_this2.z += needle.z * radius;
d.line(center,_this2,color);
let _this3 = this._pool;
if(_this2 != null) {
_this2.zero();
if(_this3.sizeVec3 == _this3.stackVec3.length) {
let newArray = new Array(_this3.sizeVec3 << 1);
let _g = 0;
let _g1 = _this3.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this3.stackVec3[i];
_this3.stackVec3[i] = null;
}
_this3.stackVec3 = newArray;
}
_this3.stackVec3[_this3.sizeVec3++] = _this2;
}
if(min > max) {
d.ellipse(center,ex,ey,radius,radius,color);
} else {
d.arc(center,ex,ey,radius,radius,min,max,true,color);
}
}
}
_drawTranslationalLimit(d,center,ex,min,max,color) {
if(min < max) {
let _this = this._pool;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = center.x;
_this1.y = center.y;
_this1.z = center.z;
let _this2 = _this1;
_this2.x += ex.x * min;
_this2.y += ex.y * min;
_this2.z += ex.z * min;
let _this3 = this._pool;
let _this4 = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
_this4.x = center.x;
_this4.y = center.y;
_this4.z = center.z;
let _this5 = _this4;
_this5.x += ex.x * max;
_this5.y += ex.y * max;
_this5.z += ex.z * max;
d.line(_this2,_this5,color);
let _this6 = this._pool;
if(_this2 != null) {
_this2.zero();
if(_this6.sizeVec3 == _this6.stackVec3.length) {
let newArray = new Array(_this6.sizeVec3 << 1);
let _g = 0;
let _g1 = _this6.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this6.stackVec3[i];
_this6.stackVec3[i] = null;
}
_this6.stackVec3 = newArray;
}
_this6.stackVec3[_this6.sizeVec3++] = _this2;
}
let _this7 = this._pool;
if(_this5 != null) {
_this5.zero();
if(_this7.sizeVec3 == _this7.stackVec3.length) {
let newArray = new Array(_this7.sizeVec3 << 1);
let _g = 0;
let _g1 = _this7.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this7.stackVec3[i];
_this7.stackVec3[i] = null;
}
_this7.stackVec3 = newArray;
}
_this7.stackVec3[_this7.sizeVec3++] = _this5;
}
}
}
_drawTranslationalLimit3D(d,center,ex,ey,ez,xlm,ylm,zlm,color) {
let minx = xlm.lowerLimit;
let maxx = xlm.upperLimit;
let miny = ylm.lowerLimit;
let maxy = ylm.upperLimit;
let minz = zlm.lowerLimit;
let maxz = zlm.upperLimit;
let _this = this._pool;
if(_this.sizeVec3 == 0) {
new oimo.common.Vec3();
} else {
--_this.sizeVec3;
}
let _this1 = this._pool;
if(_this1.sizeVec3 == 0) {
new oimo.common.Vec3();
} else {
--_this1.sizeVec3;
}
let _this2 = this._pool;
let _this3 = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
_this3.x = center.x;
_this3.y = center.y;
_this3.z = center.z;
let _this4 = _this3;
_this4.x += ex.x * minx;
_this4.y += ex.y * minx;
_this4.z += ex.z * minx;
_this4.x += ey.x * miny;
_this4.y += ey.y * miny;
_this4.z += ey.z * miny;
_this4.x += ez.x * minz;
_this4.y += ez.y * minz;
_this4.z += ez.z * minz;
let _this5 = this._pool;
let _this6 = _this5.sizeVec3 == 0 ? new oimo.common.Vec3() : _this5.stackVec3[--_this5.sizeVec3];
_this6.x = center.x;
_this6.y = center.y;
_this6.z = center.z;
let _this7 = _this6;
_this7.x += ex.x * minx;
_this7.y += ex.y * minx;
_this7.z += ex.z * minx;
_this7.x += ey.x * miny;
_this7.y += ey.y * miny;
_this7.z += ey.z * miny;
_this7.x += ez.x * maxz;
_this7.y += ez.y * maxz;
_this7.z += ez.z * maxz;
let _this8 = this._pool;
let _this9 = _this8.sizeVec3 == 0 ? new oimo.common.Vec3() : _this8.stackVec3[--_this8.sizeVec3];
_this9.x = center.x;
_this9.y = center.y;
_this9.z = center.z;
let _this10 = _this9;
_this10.x += ex.x * minx;
_this10.y += ex.y * minx;
_this10.z += ex.z * minx;
_this10.x += ey.x * maxy;
_this10.y += ey.y * maxy;
_this10.z += ey.z * maxy;
_this10.x += ez.x * minz;
_this10.y += ez.y * minz;
_this10.z += ez.z * minz;
let _this11 = this._pool;
let _this12 = _this11.sizeVec3 == 0 ? new oimo.common.Vec3() : _this11.stackVec3[--_this11.sizeVec3];
_this12.x = center.x;
_this12.y = center.y;
_this12.z = center.z;
let _this13 = _this12;
_this13.x += ex.x * minx;
_this13.y += ex.y * minx;
_this13.z += ex.z * minx;
_this13.x += ey.x * maxy;
_this13.y += ey.y * maxy;
_this13.z += ey.z * maxy;
_this13.x += ez.x * maxz;
_this13.y += ez.y * maxz;
_this13.z += ez.z * maxz;
let _this14 = this._pool;
let _this15 = _this14.sizeVec3 == 0 ? new oimo.common.Vec3() : _this14.stackVec3[--_this14.sizeVec3];
_this15.x = center.x;
_this15.y = center.y;
_this15.z = center.z;
let _this16 = _this15;
_this16.x += ex.x * maxx;
_this16.y += ex.y * maxx;
_this16.z += ex.z * maxx;
_this16.x += ey.x * miny;
_this16.y += ey.y * miny;
_this16.z += ey.z * miny;
_this16.x += ez.x * minz;
_this16.y += ez.y * minz;
_this16.z += ez.z * minz;
let _this17 = this._pool;
let _this18 = _this17.sizeVec3 == 0 ? new oimo.common.Vec3() : _this17.stackVec3[--_this17.sizeVec3];
_this18.x = center.x;
_this18.y = center.y;
_this18.z = center.z;
let _this19 = _this18;
_this19.x += ex.x * maxx;
_this19.y += ex.y * maxx;
_this19.z += ex.z * maxx;
_this19.x += ey.x * miny;
_this19.y += ey.y * miny;
_this19.z += ey.z * miny;
_this19.x += ez.x * maxz;
_this19.y += ez.y * maxz;
_this19.z += ez.z * maxz;
let _this20 = this._pool;
let _this21 = _this20.sizeVec3 == 0 ? new oimo.common.Vec3() : _this20.stackVec3[--_this20.sizeVec3];
_this21.x = center.x;
_this21.y = center.y;
_this21.z = center.z;
let _this22 = _this21;
_this22.x += ex.x * maxx;
_this22.y += ex.y * maxx;
_this22.z += ex.z * maxx;
_this22.x += ey.x * maxy;
_this22.y += ey.y * maxy;
_this22.z += ey.z * maxy;
_this22.x += ez.x * minz;
_this22.y += ez.y * minz;
_this22.z += ez.z * minz;
let _this23 = this._pool;
let _this24 = _this23.sizeVec3 == 0 ? new oimo.common.Vec3() : _this23.stackVec3[--_this23.sizeVec3];
_this24.x = center.x;
_this24.y = center.y;
_this24.z = center.z;
let _this25 = _this24;
_this25.x += ex.x * maxx;
_this25.y += ex.y * maxx;
_this25.z += ex.z * maxx;
_this25.x += ey.x * maxy;
_this25.y += ey.y * maxy;
_this25.z += ey.z * maxy;
_this25.x += ez.x * maxz;
_this25.y += ez.y * maxz;
_this25.z += ez.z * maxz;
d.line(_this4,_this16,color);
d.line(_this10,_this22,color);
d.line(_this7,_this19,color);
d.line(_this13,_this25,color);
d.line(_this4,_this10,color);
d.line(_this16,_this22,color);
d.line(_this7,_this13,color);
d.line(_this19,_this25,color);
d.line(_this4,_this7,color);
d.line(_this16,_this19,color);
d.line(_this10,_this13,color);
d.line(_this22,_this25,color);
let _this26 = this._pool;
if(_this4 != null) {
_this4.zero();
if(_this26.sizeVec3 == _this26.stackVec3.length) {
let newArray = new Array(_this26.sizeVec3 << 1);
let _g = 0;
let _g1 = _this26.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this26.stackVec3[i];
_this26.stackVec3[i] = null;
}
_this26.stackVec3 = newArray;
}
_this26.stackVec3[_this26.sizeVec3++] = _this4;
}
let _this27 = this._pool;
if(_this7 != null) {
_this7.zero();
if(_this27.sizeVec3 == _this27.stackVec3.length) {
let newArray = new Array(_this27.sizeVec3 << 1);
let _g = 0;
let _g1 = _this27.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this27.stackVec3[i];
_this27.stackVec3[i] = null;
}
_this27.stackVec3 = newArray;
}
_this27.stackVec3[_this27.sizeVec3++] = _this7;
}
let _this28 = this._pool;
if(_this10 != null) {
_this10.zero();
if(_this28.sizeVec3 == _this28.stackVec3.length) {
let newArray = new Array(_this28.sizeVec3 << 1);
let _g = 0;
let _g1 = _this28.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this28.stackVec3[i];
_this28.stackVec3[i] = null;
}
_this28.stackVec3 = newArray;
}
_this28.stackVec3[_this28.sizeVec3++] = _this10;
}
let _this29 = this._pool;
if(_this13 != null) {
_this13.zero();
if(_this29.sizeVec3 == _this29.stackVec3.length) {
let newArray = new Array(_this29.sizeVec3 << 1);
let _g = 0;
let _g1 = _this29.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this29.stackVec3[i];
_this29.stackVec3[i] = null;
}
_this29.stackVec3 = newArray;
}
_this29.stackVec3[_this29.sizeVec3++] = _this13;
}
let _this30 = this._pool;
if(_this16 != null) {
_this16.zero();
if(_this30.sizeVec3 == _this30.stackVec3.length) {
let newArray = new Array(_this30.sizeVec3 << 1);
let _g = 0;
let _g1 = _this30.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this30.stackVec3[i];
_this30.stackVec3[i] = null;
}
_this30.stackVec3 = newArray;
}
_this30.stackVec3[_this30.sizeVec3++] = _this16;
}
let _this31 = this._pool;
if(_this19 != null) {
_this19.zero();
if(_this31.sizeVec3 == _this31.stackVec3.length) {
let newArray = new Array(_this31.sizeVec3 << 1);
let _g = 0;
let _g1 = _this31.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this31.stackVec3[i];
_this31.stackVec3[i] = null;
}
_this31.stackVec3 = newArray;
}
_this31.stackVec3[_this31.sizeVec3++] = _this19;
}
let _this32 = this._pool;
if(_this22 != null) {
_this22.zero();
if(_this32.sizeVec3 == _this32.stackVec3.length) {
let newArray = new Array(_this32.sizeVec3 << 1);
let _g = 0;
let _g1 = _this32.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this32.stackVec3[i];
_this32.stackVec3[i] = null;
}
_this32.stackVec3 = newArray;
}
_this32.stackVec3[_this32.sizeVec3++] = _this22;
}
let _this33 = this._pool;
if(_this25 != null) {
_this25.zero();
if(_this33.sizeVec3 == _this33.stackVec3.length) {
let newArray = new Array(_this33.sizeVec3 << 1);
let _g = 0;
let _g1 = _this33.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this33.stackVec3[i];
_this33.stackVec3[i] = null;
}
_this33.stackVec3 = newArray;
}
_this33.stackVec3[_this33.sizeVec3++] = _this25;
}
}
_drawEllipseOnSphere(d,center,normal,x,y,radiansX,radiansY,radius,color) {
let theta = 0;
let _this = this._pool;
let rotVec = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this._pool;
let rotQ = _this1.sizeQuat == 0 ? new oimo.common.Quat() : _this1.stackQuat[--_this1.sizeQuat];
let _this2 = this._pool;
let rotM = _this2.sizeMat3 == 0 ? new oimo.common.Mat3() : _this2.stackMat3[--_this2.sizeMat3];
let _this3 = this._pool;
let prevV = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _g = 0;
while(_g < 17) {
let i = _g++;
let rx = Math.cos(theta) * radiansX;
let ry = Math.sin(theta) * radiansY;
let halfRotAng = Math.sqrt(rx * rx + ry * ry);
let rotSin = Math.sin(halfRotAng * 0.5);
let rotCos = Math.cos(halfRotAng * 0.5);
let _this = rotVec.zero();
_this.x += x.x * rx;
_this.y += x.y * rx;
_this.z += x.z * rx;
_this.x += y.x * ry;
_this.y += y.y * ry;
_this.z += y.z * ry;
let s = 1 / halfRotAng * rotSin;
rotVec.x *= s;
rotVec.y *= s;
rotVec.z *= s;
rotQ.x = rotVec.x;
rotQ.y = rotVec.y;
rotQ.z = rotVec.z;
rotQ.w = rotCos;
let x1 = rotQ.x;
let y1 = rotQ.y;
let z = rotQ.z;
let w = rotQ.w;
let x2 = 2 * x1;
let y2 = 2 * y1;
let z2 = 2 * z;
let xx = x1 * x2;
let yy = y1 * y2;
let zz = z * z2;
let xy = x1 * y2;
let yz = y1 * z2;
let xz = x1 * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
rotM.e00 = 1 - yy - zz;
rotM.e01 = xy - wz;
rotM.e02 = xz + wy;
rotM.e10 = xy + wz;
rotM.e11 = 1 - xx - zz;
rotM.e12 = yz - wx;
rotM.e20 = xz - wy;
rotM.e21 = yz + wx;
rotM.e22 = 1 - xx - yy;
let _this1 = this._pool;
let _this2 = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
_this2.x += normal.x * radius;
_this2.y += normal.y * radius;
_this2.z += normal.z * radius;
let v = _this2;
let y3 = v.x * rotM.e10 + v.y * rotM.e11 + v.z * rotM.e12;
let z1 = v.x * rotM.e20 + v.y * rotM.e21 + v.z * rotM.e22;
v.x = v.x * rotM.e00 + v.y * rotM.e01 + v.z * rotM.e02;
v.y = y3;
v.z = z1;
v.x += center.x;
v.y += center.y;
v.z += center.z;
if(i >= 1) {
d.line(prevV,v,color);
}
let _this3 = this._pool;
if(prevV != null) {
prevV.zero();
if(_this3.sizeVec3 == _this3.stackVec3.length) {
let newArray = new Array(_this3.sizeVec3 << 1);
let _g = 0;
let _g1 = _this3.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this3.stackVec3[i];
_this3.stackVec3[i] = null;
}
_this3.stackVec3 = newArray;
}
_this3.stackVec3[_this3.sizeVec3++] = prevV;
}
prevV = v;
theta += 0.39269908169872375;
}
let _this4 = this._pool;
if(rotVec != null) {
rotVec.zero();
if(_this4.sizeVec3 == _this4.stackVec3.length) {
let newArray = new Array(_this4.sizeVec3 << 1);
let _g = 0;
let _g1 = _this4.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this4.stackVec3[i];
_this4.stackVec3[i] = null;
}
_this4.stackVec3 = newArray;
}
_this4.stackVec3[_this4.sizeVec3++] = rotVec;
}
let _this5 = this._pool;
if(rotQ != null) {
rotQ.x = 0;
rotQ.y = 0;
rotQ.z = 0;
rotQ.w = 1;
if(_this5.sizeQuat == _this5.stackQuat.length) {
let newArray = new Array(_this5.sizeQuat << 1);
let _g = 0;
let _g1 = _this5.sizeQuat;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this5.stackQuat[i];
_this5.stackQuat[i] = null;
}
_this5.stackQuat = newArray;
}
_this5.stackQuat[_this5.sizeQuat++] = rotQ;
}
let _this6 = this._pool;
if(rotM != null) {
rotM.e00 = 1;
rotM.e01 = 0;
rotM.e02 = 0;
rotM.e10 = 0;
rotM.e11 = 1;
rotM.e12 = 0;
rotM.e20 = 0;
rotM.e21 = 0;
rotM.e22 = 1;
if(_this6.sizeMat3 == _this6.stackMat3.length) {
let newArray = new Array(_this6.sizeMat3 << 1);
let _g = 0;
let _g1 = _this6.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this6.stackMat3[i];
_this6.stackMat3[i] = null;
}
_this6.stackMat3 = newArray;
}
_this6.stackMat3[_this6.sizeMat3++] = rotM;
}
let _this7 = this._pool;
if(prevV != null) {
prevV.zero();
if(_this7.sizeVec3 == _this7.stackVec3.length) {
let newArray = new Array(_this7.sizeVec3 << 1);
let _g = 0;
let _g1 = _this7.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this7.stackVec3[i];
_this7.stackVec3[i] = null;
}
_this7.stackVec3 = newArray;
}
_this7.stackVec3[_this7.sizeVec3++] = prevV;
}
}
step(timeStep) {
if(this._timeStep.dt > 0) {
this._timeStep.dtRatio = timeStep / this._timeStep.dt;
}
this._timeStep.dt = timeStep;
this._timeStep.invDt = 1 / timeStep;
let st = Date.now() / 1000;
this._updateContacts();
this._solveIslands();
oimo.dynamics.common.Performance.totalTime = (Date.now() / 1000 - st) * 1000;
}
addRigidBody(rigidBody) {
if(rigidBody._world != null) {
throw new Error("A rigid body cannot belong to multiple worlds.");
}
if(this._rigidBodyList == null) {
this._rigidBodyList = rigidBody;
this._rigidBodyListLast = rigidBody;
} else {
this._rigidBodyListLast._next = rigidBody;
rigidBody._prev = this._rigidBodyListLast;
this._rigidBodyListLast = rigidBody;
}
rigidBody._world = this;
let s = rigidBody._shapeList;
while(s != null) {
let n = s._next;
s._proxy = this._broadPhase.createProxy(s,s._aabb);
s._id = this._shapeIdCount++;
this._numShapes++;
s = n;
}
this._numRigidBodies++;
}
removeRigidBody(rigidBody) {
if(rigidBody._world != this) {
throw new Error("The rigid body doesn't belong to the world.");
}
let prev = rigidBody._prev;
let next = rigidBody._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(rigidBody == this._rigidBodyList) {
this._rigidBodyList = this._rigidBodyList._next;
}
if(rigidBody == this._rigidBodyListLast) {
this._rigidBodyListLast = this._rigidBodyListLast._prev;
}
rigidBody._next = null;
rigidBody._prev = null;
rigidBody._world = null;
let s = rigidBody._shapeList;
while(s != null) {
let n = s._next;
this._broadPhase.destroyProxy(s._proxy);
s._proxy = null;
s._id = -1;
let cl = s._rigidBody._contactLinkList;
while(cl != null) {
let n = cl._next;
let c = cl._contact;
if(c._s1 == s || c._s2 == s) {
let _this = cl._other;
_this._sleeping = false;
_this._sleepTime = 0;
let _this1 = this._contactManager;
let prev = c._prev;
let next = c._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(c == _this1._contactList) {
_this1._contactList = _this1._contactList._next;
}
if(c == _this1._contactListLast) {
_this1._contactListLast = _this1._contactListLast._prev;
}
c._next = null;
c._prev = null;
if(c._touching) {
let cc1 = c._s1._contactCallback;
let cc2 = c._s2._contactCallback;
if(cc1 == cc2) {
cc2 = null;
}
if(cc1 != null) {
cc1.endContact(c);
}
if(cc2 != null) {
cc2.endContact(c);
}
}
let prev1 = c._link1._prev;
let next1 = c._link1._next;
if(prev1 != null) {
prev1._next = next1;
}
if(next1 != null) {
next1._prev = prev1;
}
if(c._link1 == c._b1._contactLinkList) {
c._b1._contactLinkList = c._b1._contactLinkList._next;
}
if(c._link1 == c._b1._contactLinkListLast) {
c._b1._contactLinkListLast = c._b1._contactLinkListLast._prev;
}
c._link1._next = null;
c._link1._prev = null;
let prev2 = c._link2._prev;
let next2 = c._link2._next;
if(prev2 != null) {
prev2._next = next2;
}
if(next2 != null) {
next2._prev = prev2;
}
if(c._link2 == c._b2._contactLinkList) {
c._b2._contactLinkList = c._b2._contactLinkList._next;
}
if(c._link2 == c._b2._contactLinkListLast) {
c._b2._contactLinkListLast = c._b2._contactLinkListLast._prev;
}
c._link2._next = null;
c._link2._prev = null;
c._b1._numContactLinks--;
c._b2._numContactLinks--;
c._link1._other = null;
c._link2._other = null;
c._link1._contact = null;
c._link2._contact = null;
c._s1 = null;
c._s2 = null;
c._b1 = null;
c._b2 = null;
c._touching = false;
c._cachedDetectorData._clear();
c._manifold._clear();
c._detector = null;
let _this2 = c._contactConstraint;
_this2._s1 = null;
_this2._s2 = null;
_this2._b1 = null;
_this2._b2 = null;
_this2._tf1 = null;
_this2._tf2 = null;
c._next = _this1._contactPool;
_this1._contactPool = c;
_this1._numContacts--;
}
cl = n;
}
this._numShapes--;
s = n;
}
this._numRigidBodies--;
}
addJoint(joint) {
if(joint._world != null) {
throw new Error("A joint cannot belong to multiple worlds.");
}
if(this._jointList == null) {
this._jointList = joint;
this._jointListLast = joint;
} else {
this._jointListLast._next = joint;
joint._prev = this._jointListLast;
this._jointListLast = joint;
}
joint._world = this;
joint._link1._other = joint._b2;
joint._link2._other = joint._b1;
if(joint._b1._jointLinkList == null) {
joint._b1._jointLinkList = joint._link1;
joint._b1._jointLinkListLast = joint._link1;
} else {
joint._b1._jointLinkListLast._next = joint._link1;
joint._link1._prev = joint._b1._jointLinkListLast;
joint._b1._jointLinkListLast = joint._link1;
}
if(joint._b2._jointLinkList == null) {
joint._b2._jointLinkList = joint._link2;
joint._b2._jointLinkListLast = joint._link2;
} else {
joint._b2._jointLinkListLast._next = joint._link2;
joint._link2._prev = joint._b2._jointLinkListLast;
joint._b2._jointLinkListLast = joint._link2;
}
joint._b1._numJointLinks++;
joint._b2._numJointLinks++;
let _this = joint._b1;
_this._sleeping = false;
_this._sleepTime = 0;
let _this1 = joint._b2;
_this1._sleeping = false;
_this1._sleepTime = 0;
joint._syncAnchors();
this._numJoints++;
}
removeJoint(joint) {
if(joint._world != this) {
throw new Error("The joint doesn't belong to the world.");
}
let prev = joint._prev;
let next = joint._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(joint == this._jointList) {
this._jointList = this._jointList._next;
}
if(joint == this._jointListLast) {
this._jointListLast = this._jointListLast._prev;
}
joint._next = null;
joint._prev = null;
joint._world = null;
let prev1 = joint._link1._prev;
let next1 = joint._link1._next;
if(prev1 != null) {
prev1._next = next1;
}
if(next1 != null) {
next1._prev = prev1;
}
if(joint._link1 == joint._b1._jointLinkList) {
joint._b1._jointLinkList = joint._b1._jointLinkList._next;
}
if(joint._link1 == joint._b1._jointLinkListLast) {
joint._b1._jointLinkListLast = joint._b1._jointLinkListLast._prev;
}
joint._link1._next = null;
joint._link1._prev = null;
let prev2 = joint._link2._prev;
let next2 = joint._link2._next;
if(prev2 != null) {
prev2._next = next2;
}
if(next2 != null) {
next2._prev = prev2;
}
if(joint._link2 == joint._b2._jointLinkList) {
joint._b2._jointLinkList = joint._b2._jointLinkList._next;
}
if(joint._link2 == joint._b2._jointLinkListLast) {
joint._b2._jointLinkListLast = joint._b2._jointLinkListLast._prev;
}
joint._link2._next = null;
joint._link2._prev = null;
joint._link1._other = null;
joint._link2._other = null;
joint._b1._numJointLinks--;
joint._b2._numJointLinks--;
let _this = joint._b1;
_this._sleeping = false;
_this._sleepTime = 0;
let _this1 = joint._b2;
_this1._sleeping = false;
_this1._sleepTime = 0;
this._numJoints--;
}
setDebugDraw(debugDraw) {
this._debugDraw = debugDraw;
}
getDebugDraw() {
return this._debugDraw;
}
debugDraw() {
if(this._debugDraw != null) {
if(this._broadPhase._type == 2) {
this._drawBvh(this._debugDraw,this._broadPhase._tree);
}
this._drawRigidBodies(this._debugDraw);
this._drawConstraints(this._debugDraw);
}
}
rayCast(begin,end,callback) {
let _this = this._rayCastWrapper.begin;
_this.x = begin.x;
_this.y = begin.y;
_this.z = begin.z;
let _this1 = this._rayCastWrapper.end;
_this1.x = end.x;
_this1.y = end.y;
_this1.z = end.z;
this._rayCastWrapper.callback = callback;
this._broadPhase.rayCast(begin,end,this._rayCastWrapper);
}
convexCast(convex,begin,translation,callback) {
this._convexCastWrapper.convex = convex;
let _this = this._convexCastWrapper.begin;
_this._positionX = begin._positionX;
_this._positionY = begin._positionY;
_this._positionZ = begin._positionZ;
_this._rotation00 = begin._rotation00;
_this._rotation01 = begin._rotation01;
_this._rotation02 = begin._rotation02;
_this._rotation10 = begin._rotation10;
_this._rotation11 = begin._rotation11;
_this._rotation12 = begin._rotation12;
_this._rotation20 = begin._rotation20;
_this._rotation21 = begin._rotation21;
_this._rotation22 = begin._rotation22;
let _this1 = this._convexCastWrapper.translation;
_this1.x = translation.x;
_this1.y = translation.y;
_this1.z = translation.z;
this._convexCastWrapper.callback = callback;
this._broadPhase.convexCast(convex,begin,translation,this._convexCastWrapper);
}
aabbTest(aabb,callback) {
this._aabbTestWrapper._aabb.copyFrom(aabb);
this._aabbTestWrapper._callback = callback;
this._broadPhase.aabbTest(aabb,this._aabbTestWrapper);
}
getRigidBodyList() {
return this._rigidBodyList;
}
getJointList() {
return this._jointList;
}
getBroadPhase() {
return this._broadPhase;
}
getContactManager() {
return this._contactManager;
}
getNumRigidBodies() {
return this._numRigidBodies;
}
getNumJoints() {
return this._numJoints;
}
getNumShapes() {
return this._numShapes;
}
getNumIslands() {
return this._numIslands;
}
getNumVelocityIterations() {
return this._numVelocityIterations;
}
setNumVelocityIterations(numVelocityIterations) {
this._numVelocityIterations = numVelocityIterations;
}
getNumPositionIterations() {
return this._numPositionIterations;
}
setNumPositionIterations(numPositionIterations) {
this._numPositionIterations = numPositionIterations;
}
getGravity() {
return this._gravity;
}
setGravity(gravity) {
let _this = this._gravity;
_this.x = gravity.x;
_this.y = gravity.y;
_this.z = gravity.z;
}
}
if(!oimo.dynamics._World) oimo.dynamics._World = {};
oimo.dynamics._World.RayCastWrapper = class oimo_dynamics__$World_RayCastWrapper extends oimo.collision.broadphase.BroadPhaseProxyCallback {
constructor() {
super();
this.rayCastHit = new oimo.collision.geometry.RayCastHit();
this.begin = new oimo.common.Vec3();
this.end = new oimo.common.Vec3();
this.callback = null;
}
process(proxy) {
let shape = proxy.userData;
if(shape._geom.rayCast(this.begin,this.end,shape._transform,this.rayCastHit)) {
this.callback.process(shape,this.rayCastHit);
}
}
}
oimo.dynamics._World.ConvexCastWrapper = class oimo_dynamics__$World_ConvexCastWrapper extends oimo.collision.broadphase.BroadPhaseProxyCallback {
constructor() {
super();
this.rayCastHit = new oimo.collision.geometry.RayCastHit();
this.begin = new oimo.common.Transform();
this.translation = new oimo.common.Vec3();
this.zero = new oimo.common.Vec3();
this.callback = null;
this.convex = null;
}
process(proxy) {
let shape = proxy.userData;
let type = shape._geom._type;
if(type < 0 || type > 5) {
return;
}
if(oimo.collision.narrowphase.detector.gjkepa.GjkEpa.instance.convexCast(this.convex,shape._geom,this.begin,shape._transform,this.translation,this.zero,this.rayCastHit)) {
this.callback.process(shape,this.rayCastHit);
}
}
}
oimo.dynamics._World.AabbTestWrapper = class oimo_dynamics__$World_AabbTestWrapper extends oimo.collision.broadphase.BroadPhaseProxyCallback {
constructor() {
super();
this._aabb = new oimo.collision.geometry.Aabb();
this._callback = null;
}
process(proxy) {
let shape = proxy.userData;
let shapeAabb = shape._aabb;
if(shapeAabb._minX < this._aabb._maxX && shapeAabb._maxX > this._aabb._minX && shapeAabb._minY < this._aabb._maxY && shapeAabb._maxY > this._aabb._minY && shapeAabb._minZ < this._aabb._maxZ && shapeAabb._maxZ > this._aabb._minZ) {
this._callback.process(shape);
}
}
}
if(!oimo.dynamics.callback) oimo.dynamics.callback = {};
oimo.dynamics.callback.AabbTestCallback = class oimo_dynamics_callback_AabbTestCallback {
constructor() {
}
process(shape) {
}
}
oimo.dynamics.callback.ContactCallback = class oimo_dynamics_callback_ContactCallback {
constructor() {
}
beginContact(c) {
}
preSolve(c) {
}
postSolve(c) {
}
endContact(c) {
}
}
oimo.dynamics.callback.RayCastCallback = class oimo_dynamics_callback_RayCastCallback {
constructor() {
}
process(shape,hit) {
}
}
oimo.dynamics.callback.RayCastClosest = class oimo_dynamics_callback_RayCastClosest extends oimo.dynamics.callback.RayCastCallback {
constructor() {
super();
this.position = new oimo.common.Vec3();
this.normal = new oimo.common.Vec3();
this.shape = null;
this.fraction = 1;
this.position.zero();
this.normal.zero();
this.hit = false;
}
clear() {
this.shape = null;
this.fraction = 1;
this.position.zero();
this.normal.zero();
this.hit = false;
}
process(shape,hit) {
if(hit.fraction < this.fraction) {
this.shape = shape;
this.hit = true;
this.fraction = hit.fraction;
let _this = this.position;
let v = hit.position;
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let _this1 = this.normal;
let v1 = hit.normal;
_this1.x = v1.x;
_this1.y = v1.y;
_this1.z = v1.z;
}
}
}
if(!oimo.dynamics.common) oimo.dynamics.common = {};
oimo.dynamics.common.DebugDraw = class oimo_dynamics_common_DebugDraw {
constructor() {
this.p = new oimo.common.Pool();
this.wireframe = false;
this.drawShapes = true;
this.drawBvh = false;
this.drawBvhMinLevel = 0;
this.drawBvhMaxLevel = 65536;
this.drawAabbs = false;
this.drawBases = false;
this.drawPairs = false;
this.drawContacts = false;
this.drawJoints = true;
this.drawJointLimits = false;
this.sphereCoords = new Array(5);
this.tmpSphereVerts = new Array(5);
this.tmpSphereNorms = new Array(5);
let _g = 0;
while(_g < 5) {
let i = _g++;
let num = i == 0 || i == 4 ? 1 : 8;
this.sphereCoords[i] = new Array(num);
this.tmpSphereVerts[i] = new Array(num);
this.tmpSphereNorms[i] = new Array(num);
let _g1 = 0;
while(_g1 < 8) {
let j = _g1++;
let theta = i * 0.7853981633974475;
let phi = j * 0.7853981633974475;
this.sphereCoords[i][j] = new oimo.common.Vec3(Math.sin(theta) * Math.cos(phi),Math.cos(theta),-Math.sin(theta) * Math.sin(phi));
this.tmpSphereVerts[i][j] = new oimo.common.Vec3();
this.tmpSphereNorms[i][j] = new oimo.common.Vec3();
}
}
this.circleCoords = new Array(8);
this.circleCoordsShift = new Array(8);
this.tmpCircleVerts1 = new Array(8);
this.tmpCircleVerts2 = new Array(8);
this.tmpCircleNorms = new Array(8);
let _g1 = 0;
while(_g1 < 8) {
let i = _g1++;
this.circleCoords[i] = new oimo.common.Vec3(Math.cos(i * 0.7853981633974475),0,-Math.sin(i * 0.7853981633974475));
this.circleCoordsShift[i] = new oimo.common.Vec3(Math.cos((i + 0.5) * 0.7853981633974475),0,-Math.sin((i + 0.5) * 0.7853981633974475));
this.tmpCircleVerts1[i] = new oimo.common.Vec3();
this.tmpCircleVerts2[i] = new oimo.common.Vec3();
this.tmpCircleNorms[i] = new oimo.common.Vec3();
}
this.style = new oimo.dynamics.common.DebugDrawStyle();
}
aabb(min,max,color) {
let _this = this.p;
let v1 = (_this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3]).init(min.x,min.y,min.z);
let _this1 = this.p;
let v2 = (_this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3]).init(min.x,min.y,max.z);
let _this2 = this.p;
let v3 = (_this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3]).init(min.x,max.y,min.z);
let _this3 = this.p;
let v4 = (_this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3]).init(min.x,max.y,max.z);
let _this4 = this.p;
let v5 = (_this4.sizeVec3 == 0 ? new oimo.common.Vec3() : _this4.stackVec3[--_this4.sizeVec3]).init(max.x,min.y,min.z);
let _this5 = this.p;
let v6 = (_this5.sizeVec3 == 0 ? new oimo.common.Vec3() : _this5.stackVec3[--_this5.sizeVec3]).init(max.x,min.y,max.z);
let _this6 = this.p;
let v7 = (_this6.sizeVec3 == 0 ? new oimo.common.Vec3() : _this6.stackVec3[--_this6.sizeVec3]).init(max.x,max.y,min.z);
let _this7 = this.p;
let v8 = (_this7.sizeVec3 == 0 ? new oimo.common.Vec3() : _this7.stackVec3[--_this7.sizeVec3]).init(max.x,max.y,max.z);
this.line(v1,v2,color);
this.line(v3,v4,color);
this.line(v5,v6,color);
this.line(v7,v8,color);
this.line(v1,v3,color);
this.line(v2,v4,color);
this.line(v5,v7,color);
this.line(v6,v8,color);
this.line(v1,v5,color);
this.line(v2,v6,color);
this.line(v3,v7,color);
this.line(v4,v8,color);
let _this8 = this.p;
if(v1 != null) {
v1.zero();
if(_this8.sizeVec3 == _this8.stackVec3.length) {
let newArray = new Array(_this8.sizeVec3 << 1);
let _g = 0;
let _g1 = _this8.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this8.stackVec3[i];
_this8.stackVec3[i] = null;
}
_this8.stackVec3 = newArray;
}
_this8.stackVec3[_this8.sizeVec3++] = v1;
}
let _this9 = this.p;
if(v2 != null) {
v2.zero();
if(_this9.sizeVec3 == _this9.stackVec3.length) {
let newArray = new Array(_this9.sizeVec3 << 1);
let _g = 0;
let _g1 = _this9.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this9.stackVec3[i];
_this9.stackVec3[i] = null;
}
_this9.stackVec3 = newArray;
}
_this9.stackVec3[_this9.sizeVec3++] = v2;
}
let _this10 = this.p;
if(v3 != null) {
v3.zero();
if(_this10.sizeVec3 == _this10.stackVec3.length) {
let newArray = new Array(_this10.sizeVec3 << 1);
let _g = 0;
let _g1 = _this10.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this10.stackVec3[i];
_this10.stackVec3[i] = null;
}
_this10.stackVec3 = newArray;
}
_this10.stackVec3[_this10.sizeVec3++] = v3;
}
let _this11 = this.p;
if(v4 != null) {
v4.zero();
if(_this11.sizeVec3 == _this11.stackVec3.length) {
let newArray = new Array(_this11.sizeVec3 << 1);
let _g = 0;
let _g1 = _this11.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this11.stackVec3[i];
_this11.stackVec3[i] = null;
}
_this11.stackVec3 = newArray;
}
_this11.stackVec3[_this11.sizeVec3++] = v4;
}
let _this12 = this.p;
if(v5 != null) {
v5.zero();
if(_this12.sizeVec3 == _this12.stackVec3.length) {
let newArray = new Array(_this12.sizeVec3 << 1);
let _g = 0;
let _g1 = _this12.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this12.stackVec3[i];
_this12.stackVec3[i] = null;
}
_this12.stackVec3 = newArray;
}
_this12.stackVec3[_this12.sizeVec3++] = v5;
}
let _this13 = this.p;
if(v6 != null) {
v6.zero();
if(_this13.sizeVec3 == _this13.stackVec3.length) {
let newArray = new Array(_this13.sizeVec3 << 1);
let _g = 0;
let _g1 = _this13.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this13.stackVec3[i];
_this13.stackVec3[i] = null;
}
_this13.stackVec3 = newArray;
}
_this13.stackVec3[_this13.sizeVec3++] = v6;
}
let _this14 = this.p;
if(v7 != null) {
v7.zero();
if(_this14.sizeVec3 == _this14.stackVec3.length) {
let newArray = new Array(_this14.sizeVec3 << 1);
let _g = 0;
let _g1 = _this14.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this14.stackVec3[i];
_this14.stackVec3[i] = null;
}
_this14.stackVec3 = newArray;
}
_this14.stackVec3[_this14.sizeVec3++] = v7;
}
let _this15 = this.p;
if(v8 != null) {
v8.zero();
if(_this15.sizeVec3 == _this15.stackVec3.length) {
let newArray = new Array(_this15.sizeVec3 << 1);
let _g = 0;
let _g1 = _this15.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this15.stackVec3[i];
_this15.stackVec3[i] = null;
}
_this15.stackVec3 = newArray;
}
_this15.stackVec3[_this15.sizeVec3++] = v8;
}
}
basis(transform,length,colorX,colorY,colorZ) {
let _this = this.p;
let pos = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this.p;
let rot = _this1.sizeMat3 == 0 ? new oimo.common.Mat3() : _this1.stackMat3[--_this1.sizeMat3];
let _this2 = this.p;
let ex = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
let _this3 = this.p;
let ey = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _this4 = this.p;
let ez = _this4.sizeVec3 == 0 ? new oimo.common.Vec3() : _this4.stackVec3[--_this4.sizeVec3];
let v = pos;
v.x = transform._positionX;
v.y = transform._positionY;
v.z = transform._positionZ;
let m = rot;
m.e00 = transform._rotation00;
m.e01 = transform._rotation01;
m.e02 = transform._rotation02;
m.e10 = transform._rotation10;
m.e11 = transform._rotation11;
m.e12 = transform._rotation12;
m.e20 = transform._rotation20;
m.e21 = transform._rotation21;
m.e22 = transform._rotation22;
ex.init(rot.e00,rot.e10,rot.e20);
ey.init(rot.e01,rot.e11,rot.e21);
ez.init(rot.e02,rot.e12,rot.e22);
ex.x *= length;
ex.y *= length;
ex.z *= length;
let _this5 = ex;
_this5.x += pos.x;
_this5.y += pos.y;
_this5.z += pos.z;
ey.x *= length;
ey.y *= length;
ey.z *= length;
let _this6 = ey;
_this6.x += pos.x;
_this6.y += pos.y;
_this6.z += pos.z;
ez.x *= length;
ez.y *= length;
ez.z *= length;
let _this7 = ez;
_this7.x += pos.x;
_this7.y += pos.y;
_this7.z += pos.z;
this.line(pos,ex,colorX);
this.line(pos,ey,colorY);
this.line(pos,ez,colorZ);
let _this8 = this.p;
if(pos != null) {
pos.zero();
if(_this8.sizeVec3 == _this8.stackVec3.length) {
let newArray = new Array(_this8.sizeVec3 << 1);
let _g = 0;
let _g1 = _this8.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this8.stackVec3[i];
_this8.stackVec3[i] = null;
}
_this8.stackVec3 = newArray;
}
_this8.stackVec3[_this8.sizeVec3++] = pos;
}
let _this9 = this.p;
if(rot != null) {
rot.e00 = 1;
rot.e01 = 0;
rot.e02 = 0;
rot.e10 = 0;
rot.e11 = 1;
rot.e12 = 0;
rot.e20 = 0;
rot.e21 = 0;
rot.e22 = 1;
if(_this9.sizeMat3 == _this9.stackMat3.length) {
let newArray = new Array(_this9.sizeMat3 << 1);
let _g = 0;
let _g1 = _this9.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this9.stackMat3[i];
_this9.stackMat3[i] = null;
}
_this9.stackMat3 = newArray;
}
_this9.stackMat3[_this9.sizeMat3++] = rot;
}
let _this10 = this.p;
if(ex != null) {
ex.zero();
if(_this10.sizeVec3 == _this10.stackVec3.length) {
let newArray = new Array(_this10.sizeVec3 << 1);
let _g = 0;
let _g1 = _this10.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this10.stackVec3[i];
_this10.stackVec3[i] = null;
}
_this10.stackVec3 = newArray;
}
_this10.stackVec3[_this10.sizeVec3++] = ex;
}
let _this11 = this.p;
if(ey != null) {
ey.zero();
if(_this11.sizeVec3 == _this11.stackVec3.length) {
let newArray = new Array(_this11.sizeVec3 << 1);
let _g = 0;
let _g1 = _this11.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this11.stackVec3[i];
_this11.stackVec3[i] = null;
}
_this11.stackVec3 = newArray;
}
_this11.stackVec3[_this11.sizeVec3++] = ey;
}
let _this12 = this.p;
if(ez != null) {
ez.zero();
if(_this12.sizeVec3 == _this12.stackVec3.length) {
let newArray = new Array(_this12.sizeVec3 << 1);
let _g = 0;
let _g1 = _this12.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this12.stackVec3[i];
_this12.stackVec3[i] = null;
}
_this12.stackVec3 = newArray;
}
_this12.stackVec3[_this12.sizeVec3++] = ez;
}
}
ellipse(center,ex,ey,radiusX,radiusY,color) {
this.arc(center,ex,ey,radiusX,radiusY,0,6.28318530717958,false,color);
}
arc(center,ex,ey,radiusX,radiusY,startAngle,endAngle,drawSector,color) {
let _this = this.p;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = ex.x;
_this1.y = ex.y;
_this1.z = ex.z;
let _this2 = _this1;
_this2.x *= radiusX;
_this2.y *= radiusX;
_this2.z *= radiusX;
let _this3 = this.p;
let _this4 = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
_this4.x = ey.x;
_this4.y = ey.y;
_this4.z = ey.z;
let _this5 = _this4;
_this5.x *= radiusY;
_this5.y *= radiusY;
_this5.z *= radiusY;
let angDiff = endAngle - startAngle;
if(angDiff < 0) {
angDiff = -angDiff;
}
let n = angDiff / 0.52359877559829837 + 0.5 | 0;
if(n == 0) {
n = 1;
}
let theta = startAngle;
let dt = (endAngle - startAngle) / n;
let _this6 = this.p;
let _this7 = _this6.sizeVec3 == 0 ? new oimo.common.Vec3() : _this6.stackVec3[--_this6.sizeVec3];
_this7.x = center.x;
_this7.y = center.y;
_this7.z = center.z;
let _this8 = _this7;
let s = Math.cos(startAngle);
_this8.x += _this2.x * s;
_this8.y += _this2.y * s;
_this8.z += _this2.z * s;
let s1 = Math.sin(startAngle);
_this8.x += _this5.x * s1;
_this8.y += _this5.y * s1;
_this8.z += _this5.z * s1;
let prevV = _this8;
if(drawSector) {
this.line(center,_this8,color);
}
let _g = 0;
let _g1 = n;
while(_g < _g1) {
++_g;
theta += dt;
let _this = this.p;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = center.x;
_this1.y = center.y;
_this1.z = center.z;
let _this3 = _this1;
let s = Math.cos(theta);
_this3.x += _this2.x * s;
_this3.y += _this2.y * s;
_this3.z += _this2.z * s;
let s1 = Math.sin(theta);
_this3.x += _this5.x * s1;
_this3.y += _this5.y * s1;
_this3.z += _this5.z * s1;
this.line(prevV,_this3,color);
let _this4 = this.p;
if(prevV != null) {
prevV.zero();
if(_this4.sizeVec3 == _this4.stackVec3.length) {
let newArray = new Array(_this4.sizeVec3 << 1);
let _g = 0;
let _g1 = _this4.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this4.stackVec3[i];
_this4.stackVec3[i] = null;
}
_this4.stackVec3 = newArray;
}
_this4.stackVec3[_this4.sizeVec3++] = prevV;
}
prevV = _this3;
}
if(drawSector) {
this.line(center,prevV,color);
}
let _this9 = this.p;
if(prevV != null) {
prevV.zero();
if(_this9.sizeVec3 == _this9.stackVec3.length) {
let newArray = new Array(_this9.sizeVec3 << 1);
let _g = 0;
let _g1 = _this9.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this9.stackVec3[i];
_this9.stackVec3[i] = null;
}
_this9.stackVec3 = newArray;
}
_this9.stackVec3[_this9.sizeVec3++] = prevV;
}
let _this10 = this.p;
if(_this2 != null) {
_this2.zero();
if(_this10.sizeVec3 == _this10.stackVec3.length) {
let newArray = new Array(_this10.sizeVec3 << 1);
let _g = 0;
let _g1 = _this10.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this10.stackVec3[i];
_this10.stackVec3[i] = null;
}
_this10.stackVec3 = newArray;
}
_this10.stackVec3[_this10.sizeVec3++] = _this2;
}
let _this11 = this.p;
if(_this5 != null) {
_this5.zero();
if(_this11.sizeVec3 == _this11.stackVec3.length) {
let newArray = new Array(_this11.sizeVec3 << 1);
let _g = 0;
let _g1 = _this11.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this11.stackVec3[i];
_this11.stackVec3[i] = null;
}
_this11.stackVec3 = newArray;
}
_this11.stackVec3[_this11.sizeVec3++] = _this5;
}
}
cone(tf,radius,halfHeight,color) {
let _this = this.p;
let ex = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this.p;
let ey = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let _this2 = this.p;
let ez = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
let _this3 = this.p;
let o = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _this4 = this.p;
let m = _this4.sizeMat3 == 0 ? new oimo.common.Mat3() : _this4.stackMat3[--_this4.sizeMat3];
let v = o;
v.x = tf._positionX;
v.y = tf._positionY;
v.z = tf._positionZ;
let m1 = m;
m1.e00 = tf._rotation00;
m1.e01 = tf._rotation01;
m1.e02 = tf._rotation02;
m1.e10 = tf._rotation10;
m1.e11 = tf._rotation11;
m1.e12 = tf._rotation12;
m1.e20 = tf._rotation20;
m1.e21 = tf._rotation21;
m1.e22 = tf._rotation22;
ex.init(m.e00,m.e10,m.e20);
ey.init(m.e01,m.e11,m.e21);
ez.init(m.e02,m.e12,m.e22);
let _this5 = this.p;
let _this6 = _this5.sizeVec3 == 0 ? new oimo.common.Vec3() : _this5.stackVec3[--_this5.sizeVec3];
_this6.x = o.x;
_this6.y = o.y;
_this6.z = o.z;
let _this7 = _this6;
_this7.x += ey.x * halfHeight;
_this7.y += ey.y * halfHeight;
_this7.z += ey.z * halfHeight;
let _this8 = this.p;
let _this9 = _this8.sizeVec3 == 0 ? new oimo.common.Vec3() : _this8.stackVec3[--_this8.sizeVec3];
_this9.x = o.x;
_this9.y = o.y;
_this9.z = o.z;
let _this10 = _this9;
let s = -halfHeight;
_this10.x += ey.x * s;
_this10.y += ey.y * s;
_this10.z += ey.z * s;
if(this.wireframe) {
let _this = this.p;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = _this10.x;
_this1.y = _this10.y;
_this1.z = _this10.z;
let _this2 = _this1;
let s = -radius;
_this2.x += ex.x * s;
_this2.y += ex.y * s;
_this2.z += ex.z * s;
_this2.x += ez.x * 0;
_this2.y += ez.y * 0;
_this2.z += ez.z * 0;
let _this3 = this.p;
let _this4 = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
_this4.x = _this10.x;
_this4.y = _this10.y;
_this4.z = _this10.z;
let _this5 = _this4;
_this5.x += ex.x * radius;
_this5.y += ex.y * radius;
_this5.z += ex.z * radius;
_this5.x += ez.x * 0;
_this5.y += ez.y * 0;
_this5.z += ez.z * 0;
let _this6 = this.p;
let _this8 = _this6.sizeVec3 == 0 ? new oimo.common.Vec3() : _this6.stackVec3[--_this6.sizeVec3];
_this8.x = _this10.x;
_this8.y = _this10.y;
_this8.z = _this10.z;
let _this9 = _this8;
_this9.x += ex.x * 0;
_this9.y += ex.y * 0;
_this9.z += ex.z * 0;
let s1 = -radius;
_this9.x += ez.x * s1;
_this9.y += ez.y * s1;
_this9.z += ez.z * s1;
let _this11 = this.p;
let _this12 = _this11.sizeVec3 == 0 ? new oimo.common.Vec3() : _this11.stackVec3[--_this11.sizeVec3];
_this12.x = _this10.x;
_this12.y = _this10.y;
_this12.z = _this10.z;
let _this13 = _this12;
_this13.x += ex.x * 0;
_this13.y += ex.y * 0;
_this13.z += ex.z * 0;
_this13.x += ez.x * radius;
_this13.y += ez.y * radius;
_this13.z += ez.z * radius;
this.ellipse(_this10,ex,ez,radius,radius,color);
this.line(_this7,_this2,color);
this.line(_this7,_this5,color);
this.line(_this7,_this9,color);
this.line(_this7,_this13,color);
let _this14 = this.p;
if(_this2 != null) {
_this2.zero();
if(_this14.sizeVec3 == _this14.stackVec3.length) {
let newArray = new Array(_this14.sizeVec3 << 1);
let _g = 0;
let _g1 = _this14.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this14.stackVec3[i];
_this14.stackVec3[i] = null;
}
_this14.stackVec3 = newArray;
}
_this14.stackVec3[_this14.sizeVec3++] = _this2;
}
let _this15 = this.p;
if(_this5 != null) {
_this5.zero();
if(_this15.sizeVec3 == _this15.stackVec3.length) {
let newArray = new Array(_this15.sizeVec3 << 1);
let _g = 0;
let _g1 = _this15.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this15.stackVec3[i];
_this15.stackVec3[i] = null;
}
_this15.stackVec3 = newArray;
}
_this15.stackVec3[_this15.sizeVec3++] = _this5;
}
let _this16 = this.p;
if(_this9 != null) {
_this9.zero();
if(_this16.sizeVec3 == _this16.stackVec3.length) {
let newArray = new Array(_this16.sizeVec3 << 1);
let _g = 0;
let _g1 = _this16.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this16.stackVec3[i];
_this16.stackVec3[i] = null;
}
_this16.stackVec3 = newArray;
}
_this16.stackVec3[_this16.sizeVec3++] = _this9;
}
let _this17 = this.p;
if(_this13 != null) {
_this13.zero();
if(_this17.sizeVec3 == _this17.stackVec3.length) {
let newArray = new Array(_this17.sizeVec3 << 1);
let _g = 0;
let _g1 = _this17.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this17.stackVec3[i];
_this17.stackVec3[i] = null;
}
_this17.stackVec3 = newArray;
}
_this17.stackVec3[_this17.sizeVec3++] = _this13;
}
} else {
let invDenom = 1 / Math.sqrt(radius * radius + 4 * halfHeight * halfHeight);
let cos = 2 * halfHeight * invDenom;
let sin = radius * invDenom;
let _g = 0;
while(_g < 8) {
let i = _g++;
let _this = this.tmpCircleNorms[i];
let v = this.circleCoords[i];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
_this.x *= cos;
_this.y *= cos;
_this.z *= cos;
_this.y += sin;
let _this1 = this.tmpCircleNorms[i];
let y = _this1.x * m.e10 + _this1.y * m.e11 + _this1.z * m.e12;
let z = _this1.x * m.e20 + _this1.y * m.e21 + _this1.z * m.e22;
_this1.x = _this1.x * m.e00 + _this1.y * m.e01 + _this1.z * m.e02;
_this1.y = y;
_this1.z = z;
let _this2 = this.tmpCircleVerts1[i];
let v1 = this.circleCoordsShift[i];
_this2.x = v1.x;
_this2.y = v1.y;
_this2.z = v1.z;
_this2.x *= cos;
_this2.y *= cos;
_this2.z *= cos;
_this2.y += sin;
let _this3 = this.tmpCircleVerts1[i];
let y1 = _this3.x * m.e10 + _this3.y * m.e11 + _this3.z * m.e12;
let z1 = _this3.x * m.e20 + _this3.y * m.e21 + _this3.z * m.e22;
_this3.x = _this3.x * m.e00 + _this3.y * m.e01 + _this3.z * m.e02;
_this3.y = y1;
_this3.z = z1;
let _this4 = this.tmpCircleVerts2[i];
let v2 = this.circleCoords[i];
_this4.x = v2.x;
_this4.y = v2.y;
_this4.z = v2.z;
let y2 = _this4.x * m.e10 + _this4.y * m.e11 + _this4.z * m.e12;
let z2 = _this4.x * m.e20 + _this4.y * m.e21 + _this4.z * m.e22;
_this4.x = _this4.x * m.e00 + _this4.y * m.e01 + _this4.z * m.e02;
_this4.y = y2;
_this4.z = z2;
_this4.x *= radius;
_this4.y *= radius;
_this4.z *= radius;
_this4.x += o.x;
_this4.y += o.y;
_this4.z += o.z;
let _this5 = this.tmpCircleVerts2[i];
let s = -halfHeight;
_this5.x += ey.x * s;
_this5.y += ey.y * s;
_this5.z += ey.z * s;
}
let _g1 = 0;
while(_g1 < 8) {
let i = _g1++;
let v2 = this.tmpCircleVerts2[i];
let v3 = this.tmpCircleVerts2[(i + 1) % 8];
let n1 = this.tmpCircleVerts1[i];
this.triangle(_this7,v2,v3,n1,this.tmpCircleNorms[i],this.tmpCircleNorms[(i + 1) % 8],color);
v2 = this.tmpCircleVerts2[(i + 1) % 8];
v3 = this.tmpCircleVerts2[i];
let _this = this.p;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = ey.x;
_this1.y = ey.y;
_this1.z = ey.z;
let _this2 = _this1;
_this2.x = -_this2.x;
_this2.y = -_this2.y;
_this2.z = -_this2.z;
this.triangle(_this10,v2,v3,_this2,_this2,_this2,color);
let _this3 = this.p;
if(_this2 != null) {
_this2.zero();
if(_this3.sizeVec3 == _this3.stackVec3.length) {
let newArray = new Array(_this3.sizeVec3 << 1);
let _g = 0;
let _g1 = _this3.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this3.stackVec3[i];
_this3.stackVec3[i] = null;
}
_this3.stackVec3 = newArray;
}
_this3.stackVec3[_this3.sizeVec3++] = _this2;
}
}
}
let _this11 = this.p;
if(_this7 != null) {
_this7.zero();
if(_this11.sizeVec3 == _this11.stackVec3.length) {
let newArray = new Array(_this11.sizeVec3 << 1);
let _g = 0;
let _g1 = _this11.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this11.stackVec3[i];
_this11.stackVec3[i] = null;
}
_this11.stackVec3 = newArray;
}
_this11.stackVec3[_this11.sizeVec3++] = _this7;
}
let _this12 = this.p;
if(_this10 != null) {
_this10.zero();
if(_this12.sizeVec3 == _this12.stackVec3.length) {
let newArray = new Array(_this12.sizeVec3 << 1);
let _g = 0;
let _g1 = _this12.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this12.stackVec3[i];
_this12.stackVec3[i] = null;
}
_this12.stackVec3 = newArray;
}
_this12.stackVec3[_this12.sizeVec3++] = _this10;
}
let _this13 = this.p;
if(o != null) {
o.zero();
if(_this13.sizeVec3 == _this13.stackVec3.length) {
let newArray = new Array(_this13.sizeVec3 << 1);
let _g = 0;
let _g1 = _this13.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this13.stackVec3[i];
_this13.stackVec3[i] = null;
}
_this13.stackVec3 = newArray;
}
_this13.stackVec3[_this13.sizeVec3++] = o;
}
let _this14 = this.p;
if(m != null) {
m.e00 = 1;
m.e01 = 0;
m.e02 = 0;
m.e10 = 0;
m.e11 = 1;
m.e12 = 0;
m.e20 = 0;
m.e21 = 0;
m.e22 = 1;
if(_this14.sizeMat3 == _this14.stackMat3.length) {
let newArray = new Array(_this14.sizeMat3 << 1);
let _g = 0;
let _g1 = _this14.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this14.stackMat3[i];
_this14.stackMat3[i] = null;
}
_this14.stackMat3 = newArray;
}
_this14.stackMat3[_this14.sizeMat3++] = m;
}
let _this15 = this.p;
if(ex != null) {
ex.zero();
if(_this15.sizeVec3 == _this15.stackVec3.length) {
let newArray = new Array(_this15.sizeVec3 << 1);
let _g = 0;
let _g1 = _this15.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this15.stackVec3[i];
_this15.stackVec3[i] = null;
}
_this15.stackVec3 = newArray;
}
_this15.stackVec3[_this15.sizeVec3++] = ex;
}
let _this16 = this.p;
if(ey != null) {
ey.zero();
if(_this16.sizeVec3 == _this16.stackVec3.length) {
let newArray = new Array(_this16.sizeVec3 << 1);
let _g = 0;
let _g1 = _this16.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this16.stackVec3[i];
_this16.stackVec3[i] = null;
}
_this16.stackVec3 = newArray;
}
_this16.stackVec3[_this16.sizeVec3++] = ey;
}
let _this17 = this.p;
if(ez != null) {
ez.zero();
if(_this17.sizeVec3 == _this17.stackVec3.length) {
let newArray = new Array(_this17.sizeVec3 << 1);
let _g = 0;
let _g1 = _this17.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this17.stackVec3[i];
_this17.stackVec3[i] = null;
}
_this17.stackVec3 = newArray;
}
_this17.stackVec3[_this17.sizeVec3++] = ez;
}
}
cylinder(tf,radius,halfHeight,color) {
let _this = this.p;
let ex = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this.p;
let ey = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let _this2 = this.p;
let ez = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
let _this3 = this.p;
let o = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _this4 = this.p;
let m = _this4.sizeMat3 == 0 ? new oimo.common.Mat3() : _this4.stackMat3[--_this4.sizeMat3];
let v = o;
v.x = tf._positionX;
v.y = tf._positionY;
v.z = tf._positionZ;
let m1 = m;
m1.e00 = tf._rotation00;
m1.e01 = tf._rotation01;
m1.e02 = tf._rotation02;
m1.e10 = tf._rotation10;
m1.e11 = tf._rotation11;
m1.e12 = tf._rotation12;
m1.e20 = tf._rotation20;
m1.e21 = tf._rotation21;
m1.e22 = tf._rotation22;
ex.init(m.e00,m.e10,m.e20);
ey.init(m.e01,m.e11,m.e21);
ez.init(m.e02,m.e12,m.e22);
let _this5 = this.p;
let _this6 = _this5.sizeVec3 == 0 ? new oimo.common.Vec3() : _this5.stackVec3[--_this5.sizeVec3];
_this6.x = o.x;
_this6.y = o.y;
_this6.z = o.z;
let _this7 = _this6;
_this7.x += ey.x * halfHeight;
_this7.y += ey.y * halfHeight;
_this7.z += ey.z * halfHeight;
let _this8 = this.p;
let _this9 = _this8.sizeVec3 == 0 ? new oimo.common.Vec3() : _this8.stackVec3[--_this8.sizeVec3];
_this9.x = o.x;
_this9.y = o.y;
_this9.z = o.z;
let _this10 = _this9;
let s = -halfHeight;
_this10.x += ey.x * s;
_this10.y += ey.y * s;
_this10.z += ey.z * s;
if(this.wireframe) {
let _this = this.p;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = _this7.x;
_this1.y = _this7.y;
_this1.z = _this7.z;
let _this2 = _this1;
let s = -radius;
_this2.x += ex.x * s;
_this2.y += ex.y * s;
_this2.z += ex.z * s;
_this2.x += ez.x * 0;
_this2.y += ez.y * 0;
_this2.z += ez.z * 0;
let _this3 = this.p;
let _this4 = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
_this4.x = _this7.x;
_this4.y = _this7.y;
_this4.z = _this7.z;
let _this5 = _this4;
_this5.x += ex.x * radius;
_this5.y += ex.y * radius;
_this5.z += ex.z * radius;
_this5.x += ez.x * 0;
_this5.y += ez.y * 0;
_this5.z += ez.z * 0;
let _this6 = this.p;
let _this8 = _this6.sizeVec3 == 0 ? new oimo.common.Vec3() : _this6.stackVec3[--_this6.sizeVec3];
_this8.x = _this7.x;
_this8.y = _this7.y;
_this8.z = _this7.z;
let _this9 = _this8;
_this9.x += ex.x * 0;
_this9.y += ex.y * 0;
_this9.z += ex.z * 0;
let s1 = -radius;
_this9.x += ez.x * s1;
_this9.y += ez.y * s1;
_this9.z += ez.z * s1;
let _this11 = this.p;
let _this12 = _this11.sizeVec3 == 0 ? new oimo.common.Vec3() : _this11.stackVec3[--_this11.sizeVec3];
_this12.x = _this7.x;
_this12.y = _this7.y;
_this12.z = _this7.z;
let _this13 = _this12;
_this13.x += ex.x * 0;
_this13.y += ex.y * 0;
_this13.z += ex.z * 0;
_this13.x += ez.x * radius;
_this13.y += ez.y * radius;
_this13.z += ez.z * radius;
let _this14 = this.p;
let _this15 = _this14.sizeVec3 == 0 ? new oimo.common.Vec3() : _this14.stackVec3[--_this14.sizeVec3];
_this15.x = _this10.x;
_this15.y = _this10.y;
_this15.z = _this10.z;
let _this16 = _this15;
let s2 = -radius;
_this16.x += ex.x * s2;
_this16.y += ex.y * s2;
_this16.z += ex.z * s2;
_this16.x += ez.x * 0;
_this16.y += ez.y * 0;
_this16.z += ez.z * 0;
let _this17 = this.p;
let _this18 = _this17.sizeVec3 == 0 ? new oimo.common.Vec3() : _this17.stackVec3[--_this17.sizeVec3];
_this18.x = _this10.x;
_this18.y = _this10.y;
_this18.z = _this10.z;
let _this19 = _this18;
_this19.x += ex.x * radius;
_this19.y += ex.y * radius;
_this19.z += ex.z * radius;
_this19.x += ez.x * 0;
_this19.y += ez.y * 0;
_this19.z += ez.z * 0;
let _this20 = this.p;
let _this21 = _this20.sizeVec3 == 0 ? new oimo.common.Vec3() : _this20.stackVec3[--_this20.sizeVec3];
_this21.x = _this10.x;
_this21.y = _this10.y;
_this21.z = _this10.z;
let _this22 = _this21;
_this22.x += ex.x * 0;
_this22.y += ex.y * 0;
_this22.z += ex.z * 0;
let s3 = -radius;
_this22.x += ez.x * s3;
_this22.y += ez.y * s3;
_this22.z += ez.z * s3;
let _this23 = this.p;
let _this24 = _this23.sizeVec3 == 0 ? new oimo.common.Vec3() : _this23.stackVec3[--_this23.sizeVec3];
_this24.x = _this10.x;
_this24.y = _this10.y;
_this24.z = _this10.z;
let _this25 = _this24;
_this25.x += ex.x * 0;
_this25.y += ex.y * 0;
_this25.z += ex.z * 0;
_this25.x += ez.x * radius;
_this25.y += ez.y * radius;
_this25.z += ez.z * radius;
this.ellipse(_this7,ex,ez,radius,radius,color);
this.ellipse(_this10,ex,ez,radius,radius,color);
this.line(_this2,_this16,color);
this.line(_this5,_this19,color);
this.line(_this9,_this22,color);
this.line(_this13,_this25,color);
let _this26 = this.p;
if(_this2 != null) {
_this2.zero();
if(_this26.sizeVec3 == _this26.stackVec3.length) {
let newArray = new Array(_this26.sizeVec3 << 1);
let _g = 0;
let _g1 = _this26.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this26.stackVec3[i];
_this26.stackVec3[i] = null;
}
_this26.stackVec3 = newArray;
}
_this26.stackVec3[_this26.sizeVec3++] = _this2;
}
let _this27 = this.p;
if(_this5 != null) {
_this5.zero();
if(_this27.sizeVec3 == _this27.stackVec3.length) {
let newArray = new Array(_this27.sizeVec3 << 1);
let _g = 0;
let _g1 = _this27.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this27.stackVec3[i];
_this27.stackVec3[i] = null;
}
_this27.stackVec3 = newArray;
}
_this27.stackVec3[_this27.sizeVec3++] = _this5;
}
let _this28 = this.p;
if(_this9 != null) {
_this9.zero();
if(_this28.sizeVec3 == _this28.stackVec3.length) {
let newArray = new Array(_this28.sizeVec3 << 1);
let _g = 0;
let _g1 = _this28.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this28.stackVec3[i];
_this28.stackVec3[i] = null;
}
_this28.stackVec3 = newArray;
}
_this28.stackVec3[_this28.sizeVec3++] = _this9;
}
let _this29 = this.p;
if(_this13 != null) {
_this13.zero();
if(_this29.sizeVec3 == _this29.stackVec3.length) {
let newArray = new Array(_this29.sizeVec3 << 1);
let _g = 0;
let _g1 = _this29.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this29.stackVec3[i];
_this29.stackVec3[i] = null;
}
_this29.stackVec3 = newArray;
}
_this29.stackVec3[_this29.sizeVec3++] = _this13;
}
let _this30 = this.p;
if(_this16 != null) {
_this16.zero();
if(_this30.sizeVec3 == _this30.stackVec3.length) {
let newArray = new Array(_this30.sizeVec3 << 1);
let _g = 0;
let _g1 = _this30.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this30.stackVec3[i];
_this30.stackVec3[i] = null;
}
_this30.stackVec3 = newArray;
}
_this30.stackVec3[_this30.sizeVec3++] = _this16;
}
let _this31 = this.p;
if(_this19 != null) {
_this19.zero();
if(_this31.sizeVec3 == _this31.stackVec3.length) {
let newArray = new Array(_this31.sizeVec3 << 1);
let _g = 0;
let _g1 = _this31.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this31.stackVec3[i];
_this31.stackVec3[i] = null;
}
_this31.stackVec3 = newArray;
}
_this31.stackVec3[_this31.sizeVec3++] = _this19;
}
let _this32 = this.p;
if(_this22 != null) {
_this22.zero();
if(_this32.sizeVec3 == _this32.stackVec3.length) {
let newArray = new Array(_this32.sizeVec3 << 1);
let _g = 0;
let _g1 = _this32.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this32.stackVec3[i];
_this32.stackVec3[i] = null;
}
_this32.stackVec3 = newArray;
}
_this32.stackVec3[_this32.sizeVec3++] = _this22;
}
let _this33 = this.p;
if(_this25 != null) {
_this25.zero();
if(_this33.sizeVec3 == _this33.stackVec3.length) {
let newArray = new Array(_this33.sizeVec3 << 1);
let _g = 0;
let _g1 = _this33.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this33.stackVec3[i];
_this33.stackVec3[i] = null;
}
_this33.stackVec3 = newArray;
}
_this33.stackVec3[_this33.sizeVec3++] = _this25;
}
} else {
let _g = 0;
while(_g < 8) {
let i = _g++;
let _this = this.tmpCircleNorms[i];
let v = this.circleCoords[i];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let y = _this.x * m.e10 + _this.y * m.e11 + _this.z * m.e12;
let z = _this.x * m.e20 + _this.y * m.e21 + _this.z * m.e22;
_this.x = _this.x * m.e00 + _this.y * m.e01 + _this.z * m.e02;
_this.y = y;
_this.z = z;
let _this1 = this.tmpCircleVerts1[i];
let v1 = this.tmpCircleNorms[i];
_this1.x = v1.x;
_this1.y = v1.y;
_this1.z = v1.z;
_this1.x *= radius;
_this1.y *= radius;
_this1.z *= radius;
_this1.x += o.x;
_this1.y += o.y;
_this1.z += o.z;
let _this2 = this.tmpCircleVerts2[i];
let v2 = this.tmpCircleVerts1[i];
_this2.x = v2.x;
_this2.y = v2.y;
_this2.z = v2.z;
let _this3 = this.tmpCircleVerts1[i];
_this3.x += ey.x * halfHeight;
_this3.y += ey.y * halfHeight;
_this3.z += ey.z * halfHeight;
let _this4 = this.tmpCircleVerts2[i];
let s = -halfHeight;
_this4.x += ey.x * s;
_this4.y += ey.y * s;
_this4.z += ey.z * s;
}
let _g1 = 0;
while(_g1 < 8) {
let i = _g1++;
let v1;
let v2 = this.tmpCircleVerts1[i];
let v3 = this.tmpCircleVerts1[(i + 1) % 8];
let n1 = ey;
this.triangle(_this7,v2,v3,n1,n1,n1,color);
v2 = this.tmpCircleVerts2[(i + 1) % 8];
v3 = this.tmpCircleVerts2[i];
let _this = this.p;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = ey.x;
_this1.y = ey.y;
_this1.z = ey.z;
let _this2 = _this1;
_this2.x = -_this2.x;
_this2.y = -_this2.y;
_this2.z = -_this2.z;
this.triangle(_this10,v2,v3,_this2,_this2,_this2,color);
let _this3 = this.p;
if(_this2 != null) {
_this2.zero();
if(_this3.sizeVec3 == _this3.stackVec3.length) {
let newArray = new Array(_this3.sizeVec3 << 1);
let _g = 0;
let _g1 = _this3.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this3.stackVec3[i];
_this3.stackVec3[i] = null;
}
_this3.stackVec3 = newArray;
}
_this3.stackVec3[_this3.sizeVec3++] = _this2;
}
v1 = this.tmpCircleVerts1[i];
v2 = this.tmpCircleVerts2[i];
v3 = this.tmpCircleVerts2[(i + 1) % 8];
n1 = this.tmpCircleNorms[i];
let n2 = this.tmpCircleNorms[(i + 1) % 8];
this.rect(v1,v2,v3,this.tmpCircleVerts1[(i + 1) % 8],n1,n1,n2,n2,color);
}
}
let _this11 = this.p;
if(_this7 != null) {
_this7.zero();
if(_this11.sizeVec3 == _this11.stackVec3.length) {
let newArray = new Array(_this11.sizeVec3 << 1);
let _g = 0;
let _g1 = _this11.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this11.stackVec3[i];
_this11.stackVec3[i] = null;
}
_this11.stackVec3 = newArray;
}
_this11.stackVec3[_this11.sizeVec3++] = _this7;
}
let _this12 = this.p;
if(_this10 != null) {
_this10.zero();
if(_this12.sizeVec3 == _this12.stackVec3.length) {
let newArray = new Array(_this12.sizeVec3 << 1);
let _g = 0;
let _g1 = _this12.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this12.stackVec3[i];
_this12.stackVec3[i] = null;
}
_this12.stackVec3 = newArray;
}
_this12.stackVec3[_this12.sizeVec3++] = _this10;
}
let _this13 = this.p;
if(o != null) {
o.zero();
if(_this13.sizeVec3 == _this13.stackVec3.length) {
let newArray = new Array(_this13.sizeVec3 << 1);
let _g = 0;
let _g1 = _this13.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this13.stackVec3[i];
_this13.stackVec3[i] = null;
}
_this13.stackVec3 = newArray;
}
_this13.stackVec3[_this13.sizeVec3++] = o;
}
let _this14 = this.p;
if(m != null) {
m.e00 = 1;
m.e01 = 0;
m.e02 = 0;
m.e10 = 0;
m.e11 = 1;
m.e12 = 0;
m.e20 = 0;
m.e21 = 0;
m.e22 = 1;
if(_this14.sizeMat3 == _this14.stackMat3.length) {
let newArray = new Array(_this14.sizeMat3 << 1);
let _g = 0;
let _g1 = _this14.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this14.stackMat3[i];
_this14.stackMat3[i] = null;
}
_this14.stackMat3 = newArray;
}
_this14.stackMat3[_this14.sizeMat3++] = m;
}
let _this15 = this.p;
if(ex != null) {
ex.zero();
if(_this15.sizeVec3 == _this15.stackVec3.length) {
let newArray = new Array(_this15.sizeVec3 << 1);
let _g = 0;
let _g1 = _this15.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this15.stackVec3[i];
_this15.stackVec3[i] = null;
}
_this15.stackVec3 = newArray;
}
_this15.stackVec3[_this15.sizeVec3++] = ex;
}
let _this16 = this.p;
if(ey != null) {
ey.zero();
if(_this16.sizeVec3 == _this16.stackVec3.length) {
let newArray = new Array(_this16.sizeVec3 << 1);
let _g = 0;
let _g1 = _this16.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this16.stackVec3[i];
_this16.stackVec3[i] = null;
}
_this16.stackVec3 = newArray;
}
_this16.stackVec3[_this16.sizeVec3++] = ey;
}
let _this17 = this.p;
if(ez != null) {
ez.zero();
if(_this17.sizeVec3 == _this17.stackVec3.length) {
let newArray = new Array(_this17.sizeVec3 << 1);
let _g = 0;
let _g1 = _this17.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this17.stackVec3[i];
_this17.stackVec3[i] = null;
}
_this17.stackVec3 = newArray;
}
_this17.stackVec3[_this17.sizeVec3++] = ez;
}
}
capsule(tf,radius,halfHeight,color) {
let _this = this.p;
let ex = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this.p;
let ey = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let _this2 = this.p;
let ez = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
let _this3 = this.p;
let o = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _this4 = this.p;
let m = _this4.sizeMat3 == 0 ? new oimo.common.Mat3() : _this4.stackMat3[--_this4.sizeMat3];
let v = o;
v.x = tf._positionX;
v.y = tf._positionY;
v.z = tf._positionZ;
let m1 = m;
m1.e00 = tf._rotation00;
m1.e01 = tf._rotation01;
m1.e02 = tf._rotation02;
m1.e10 = tf._rotation10;
m1.e11 = tf._rotation11;
m1.e12 = tf._rotation12;
m1.e20 = tf._rotation20;
m1.e21 = tf._rotation21;
m1.e22 = tf._rotation22;
ex.init(m.e00,m.e10,m.e20);
ey.init(m.e01,m.e11,m.e21);
ez.init(m.e02,m.e12,m.e22);
let vs = this.tmpSphereVerts;
let ns = this.tmpSphereNorms;
let _g = 0;
while(_g < 5) {
let i2 = _g++;
let n = this.tmpSphereVerts[i2].length;
let _g1 = 0;
while(_g1 < n) {
let j2 = _g1++;
let _this = ns[i2][j2];
let v = this.sphereCoords[i2][j2];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let y = _this.x * m.e10 + _this.y * m.e11 + _this.z * m.e12;
let z = _this.x * m.e20 + _this.y * m.e21 + _this.z * m.e22;
_this.x = _this.x * m.e00 + _this.y * m.e01 + _this.z * m.e02;
_this.y = y;
_this.z = z;
}
}
let _g1 = 0;
while(_g1 < 4) {
let i = _g1++;
if(i == 0) {
let _g = 0;
while(_g < 3) {
let i2 = _g++;
let n = this.tmpSphereVerts[i2].length;
let _g1 = 0;
while(_g1 < n) {
let j2 = _g1++;
let _this = vs[i2][j2];
let v = ns[i2][j2];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
_this.x *= radius;
_this.y *= radius;
_this.z *= radius;
_this.x += o.x;
_this.y += o.y;
_this.z += o.z;
_this.x += ey.x * halfHeight;
_this.y += ey.y * halfHeight;
_this.z += ey.z * halfHeight;
}
}
}
if(i == 2) {
let _g = 2;
while(_g < 5) {
let i2 = _g++;
let n = this.tmpSphereVerts[i2].length;
let _g1 = 0;
while(_g1 < n) {
let j2 = _g1++;
let _this = vs[i2][j2];
let v = ns[i2][j2];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
_this.x *= radius;
_this.y *= radius;
_this.z *= radius;
_this.x += o.x;
_this.y += o.y;
_this.z += o.z;
let s = -halfHeight;
_this.x += ey.x * s;
_this.y += ey.y * s;
_this.z += ey.z * s;
}
}
}
let _g = 0;
while(_g < 8) {
let j = _g++;
let v1;
let v2;
let v3;
let v4;
let n1;
let n2;
let n3;
let n4;
if(i == 0) {
if(this.wireframe) {
v1 = vs[0][0];
v2 = vs[1][j];
this.line(v1,v2,color);
} else {
v1 = vs[0][0];
v2 = vs[1][j];
v3 = vs[1][(j + 1) % 8];
n1 = ns[0][0];
n2 = ns[1][j];
n3 = ns[1][(j + 1) % 8];
this.triangle(v1,v2,v3,n1,n2,n3,color);
}
} else if(i == 3) {
if(this.wireframe) {
v1 = vs[4][0];
v2 = vs[i][(j + 1) % 8];
v3 = vs[i][j];
this.line(v1,v2,color);
this.line(v2,v3,color);
} else {
v1 = vs[4][0];
v2 = vs[i][(j + 1) % 8];
v3 = vs[i][j];
n1 = ns[4][0];
n2 = ns[i][(j + 1) % 8];
n3 = ns[i][j];
this.triangle(v1,v2,v3,n1,n2,n3,color);
}
} else if(this.wireframe) {
v1 = vs[i][j];
v2 = vs[i][(j + 1) % 8];
v3 = vs[i + 1][j];
this.line(v1,v2,color);
this.line(v1,v3,color);
} else {
v1 = vs[i][j];
v2 = vs[i][(j + 1) % 8];
v3 = vs[i + 1][j];
v4 = vs[i + 1][(j + 1) % 8];
n1 = ns[i][j];
n2 = ns[i][(j + 1) % 8];
n3 = ns[i + 1][j];
n4 = ns[i + 1][(j + 1) % 8];
this.rect(v1,v3,v4,v2,n1,n3,n4,n2,color);
}
}
}
let _this5 = this.p;
let _this6 = _this5.sizeVec3 == 0 ? new oimo.common.Vec3() : _this5.stackVec3[--_this5.sizeVec3];
_this6.x = o.x;
_this6.y = o.y;
_this6.z = o.z;
let _this7 = _this6;
_this7.x += ey.x * halfHeight;
_this7.y += ey.y * halfHeight;
_this7.z += ey.z * halfHeight;
let _this8 = this.p;
let _this9 = _this8.sizeVec3 == 0 ? new oimo.common.Vec3() : _this8.stackVec3[--_this8.sizeVec3];
_this9.x = o.x;
_this9.y = o.y;
_this9.z = o.z;
let _this10 = _this9;
let s = -halfHeight;
_this10.x += ey.x * s;
_this10.y += ey.y * s;
_this10.z += ey.z * s;
if(this.wireframe) {
let _this = this.p;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = _this7.x;
_this1.y = _this7.y;
_this1.z = _this7.z;
let _this2 = _this1;
let s = -radius;
_this2.x += ex.x * s;
_this2.y += ex.y * s;
_this2.z += ex.z * s;
_this2.x += ez.x * 0;
_this2.y += ez.y * 0;
_this2.z += ez.z * 0;
let _this3 = this.p;
let _this4 = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
_this4.x = _this7.x;
_this4.y = _this7.y;
_this4.z = _this7.z;
let _this5 = _this4;
_this5.x += ex.x * radius;
_this5.y += ex.y * radius;
_this5.z += ex.z * radius;
_this5.x += ez.x * 0;
_this5.y += ez.y * 0;
_this5.z += ez.z * 0;
let _this6 = this.p;
let _this8 = _this6.sizeVec3 == 0 ? new oimo.common.Vec3() : _this6.stackVec3[--_this6.sizeVec3];
_this8.x = _this7.x;
_this8.y = _this7.y;
_this8.z = _this7.z;
let _this9 = _this8;
_this9.x += ex.x * 0;
_this9.y += ex.y * 0;
_this9.z += ex.z * 0;
let s1 = -radius;
_this9.x += ez.x * s1;
_this9.y += ez.y * s1;
_this9.z += ez.z * s1;
let _this11 = this.p;
let _this12 = _this11.sizeVec3 == 0 ? new oimo.common.Vec3() : _this11.stackVec3[--_this11.sizeVec3];
_this12.x = _this7.x;
_this12.y = _this7.y;
_this12.z = _this7.z;
let _this13 = _this12;
_this13.x += ex.x * 0;
_this13.y += ex.y * 0;
_this13.z += ex.z * 0;
_this13.x += ez.x * radius;
_this13.y += ez.y * radius;
_this13.z += ez.z * radius;
let _this14 = this.p;
let _this15 = _this14.sizeVec3 == 0 ? new oimo.common.Vec3() : _this14.stackVec3[--_this14.sizeVec3];
_this15.x = _this10.x;
_this15.y = _this10.y;
_this15.z = _this10.z;
let _this16 = _this15;
let s2 = -radius;
_this16.x += ex.x * s2;
_this16.y += ex.y * s2;
_this16.z += ex.z * s2;
_this16.x += ez.x * 0;
_this16.y += ez.y * 0;
_this16.z += ez.z * 0;
let _this17 = this.p;
let _this18 = _this17.sizeVec3 == 0 ? new oimo.common.Vec3() : _this17.stackVec3[--_this17.sizeVec3];
_this18.x = _this10.x;
_this18.y = _this10.y;
_this18.z = _this10.z;
let _this19 = _this18;
_this19.x += ex.x * radius;
_this19.y += ex.y * radius;
_this19.z += ex.z * radius;
_this19.x += ez.x * 0;
_this19.y += ez.y * 0;
_this19.z += ez.z * 0;
let _this20 = this.p;
let _this21 = _this20.sizeVec3 == 0 ? new oimo.common.Vec3() : _this20.stackVec3[--_this20.sizeVec3];
_this21.x = _this10.x;
_this21.y = _this10.y;
_this21.z = _this10.z;
let _this22 = _this21;
_this22.x += ex.x * 0;
_this22.y += ex.y * 0;
_this22.z += ex.z * 0;
let s3 = -radius;
_this22.x += ez.x * s3;
_this22.y += ez.y * s3;
_this22.z += ez.z * s3;
let _this23 = this.p;
let _this24 = _this23.sizeVec3 == 0 ? new oimo.common.Vec3() : _this23.stackVec3[--_this23.sizeVec3];
_this24.x = _this10.x;
_this24.y = _this10.y;
_this24.z = _this10.z;
let _this25 = _this24;
_this25.x += ex.x * 0;
_this25.y += ex.y * 0;
_this25.z += ex.z * 0;
_this25.x += ez.x * radius;
_this25.y += ez.y * radius;
_this25.z += ez.z * radius;
this.ellipse(_this7,ex,ez,radius,radius,color);
this.ellipse(_this10,ex,ez,radius,radius,color);
this.line(_this2,_this16,color);
this.line(_this5,_this19,color);
this.line(_this9,_this22,color);
this.line(_this13,_this25,color);
let _this26 = this.p;
if(_this2 != null) {
_this2.zero();
if(_this26.sizeVec3 == _this26.stackVec3.length) {
let newArray = new Array(_this26.sizeVec3 << 1);
let _g = 0;
let _g1 = _this26.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this26.stackVec3[i];
_this26.stackVec3[i] = null;
}
_this26.stackVec3 = newArray;
}
_this26.stackVec3[_this26.sizeVec3++] = _this2;
}
let _this27 = this.p;
if(_this5 != null) {
_this5.zero();
if(_this27.sizeVec3 == _this27.stackVec3.length) {
let newArray = new Array(_this27.sizeVec3 << 1);
let _g = 0;
let _g1 = _this27.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this27.stackVec3[i];
_this27.stackVec3[i] = null;
}
_this27.stackVec3 = newArray;
}
_this27.stackVec3[_this27.sizeVec3++] = _this5;
}
let _this28 = this.p;
if(_this9 != null) {
_this9.zero();
if(_this28.sizeVec3 == _this28.stackVec3.length) {
let newArray = new Array(_this28.sizeVec3 << 1);
let _g = 0;
let _g1 = _this28.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this28.stackVec3[i];
_this28.stackVec3[i] = null;
}
_this28.stackVec3 = newArray;
}
_this28.stackVec3[_this28.sizeVec3++] = _this9;
}
let _this29 = this.p;
if(_this13 != null) {
_this13.zero();
if(_this29.sizeVec3 == _this29.stackVec3.length) {
let newArray = new Array(_this29.sizeVec3 << 1);
let _g = 0;
let _g1 = _this29.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this29.stackVec3[i];
_this29.stackVec3[i] = null;
}
_this29.stackVec3 = newArray;
}
_this29.stackVec3[_this29.sizeVec3++] = _this13;
}
let _this30 = this.p;
if(_this16 != null) {
_this16.zero();
if(_this30.sizeVec3 == _this30.stackVec3.length) {
let newArray = new Array(_this30.sizeVec3 << 1);
let _g = 0;
let _g1 = _this30.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this30.stackVec3[i];
_this30.stackVec3[i] = null;
}
_this30.stackVec3 = newArray;
}
_this30.stackVec3[_this30.sizeVec3++] = _this16;
}
let _this31 = this.p;
if(_this19 != null) {
_this19.zero();
if(_this31.sizeVec3 == _this31.stackVec3.length) {
let newArray = new Array(_this31.sizeVec3 << 1);
let _g = 0;
let _g1 = _this31.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this31.stackVec3[i];
_this31.stackVec3[i] = null;
}
_this31.stackVec3 = newArray;
}
_this31.stackVec3[_this31.sizeVec3++] = _this19;
}
let _this32 = this.p;
if(_this22 != null) {
_this22.zero();
if(_this32.sizeVec3 == _this32.stackVec3.length) {
let newArray = new Array(_this32.sizeVec3 << 1);
let _g = 0;
let _g1 = _this32.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this32.stackVec3[i];
_this32.stackVec3[i] = null;
}
_this32.stackVec3 = newArray;
}
_this32.stackVec3[_this32.sizeVec3++] = _this22;
}
let _this33 = this.p;
if(_this25 != null) {
_this25.zero();
if(_this33.sizeVec3 == _this33.stackVec3.length) {
let newArray = new Array(_this33.sizeVec3 << 1);
let _g = 0;
let _g1 = _this33.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this33.stackVec3[i];
_this33.stackVec3[i] = null;
}
_this33.stackVec3 = newArray;
}
_this33.stackVec3[_this33.sizeVec3++] = _this25;
}
} else {
let _g = 0;
while(_g < 8) {
let i = _g++;
let _this = this.tmpCircleNorms[i];
let v = this.circleCoords[i];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let y = _this.x * m.e10 + _this.y * m.e11 + _this.z * m.e12;
let z = _this.x * m.e20 + _this.y * m.e21 + _this.z * m.e22;
_this.x = _this.x * m.e00 + _this.y * m.e01 + _this.z * m.e02;
_this.y = y;
_this.z = z;
let _this1 = this.tmpCircleVerts1[i];
let v1 = this.tmpCircleNorms[i];
_this1.x = v1.x;
_this1.y = v1.y;
_this1.z = v1.z;
_this1.x *= radius;
_this1.y *= radius;
_this1.z *= radius;
_this1.x += o.x;
_this1.y += o.y;
_this1.z += o.z;
let _this2 = this.tmpCircleVerts2[i];
let v2 = this.tmpCircleVerts1[i];
_this2.x = v2.x;
_this2.y = v2.y;
_this2.z = v2.z;
let _this3 = this.tmpCircleVerts1[i];
_this3.x += ey.x * halfHeight;
_this3.y += ey.y * halfHeight;
_this3.z += ey.z * halfHeight;
let _this4 = this.tmpCircleVerts2[i];
let s = -halfHeight;
_this4.x += ey.x * s;
_this4.y += ey.y * s;
_this4.z += ey.z * s;
}
let _g1 = 0;
while(_g1 < 8) {
let i = _g1++;
let n1 = this.tmpCircleNorms[i];
let n2 = this.tmpCircleNorms[(i + 1) % 8];
this.rect(this.tmpCircleVerts1[i],this.tmpCircleVerts2[i],this.tmpCircleVerts2[(i + 1) % 8],this.tmpCircleVerts1[(i + 1) % 8],n1,n1,n2,n2,color);
}
}
let _this11 = this.p;
if(_this7 != null) {
_this7.zero();
if(_this11.sizeVec3 == _this11.stackVec3.length) {
let newArray = new Array(_this11.sizeVec3 << 1);
let _g = 0;
let _g1 = _this11.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this11.stackVec3[i];
_this11.stackVec3[i] = null;
}
_this11.stackVec3 = newArray;
}
_this11.stackVec3[_this11.sizeVec3++] = _this7;
}
let _this12 = this.p;
if(_this10 != null) {
_this10.zero();
if(_this12.sizeVec3 == _this12.stackVec3.length) {
let newArray = new Array(_this12.sizeVec3 << 1);
let _g = 0;
let _g1 = _this12.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this12.stackVec3[i];
_this12.stackVec3[i] = null;
}
_this12.stackVec3 = newArray;
}
_this12.stackVec3[_this12.sizeVec3++] = _this10;
}
let _this13 = this.p;
if(o != null) {
o.zero();
if(_this13.sizeVec3 == _this13.stackVec3.length) {
let newArray = new Array(_this13.sizeVec3 << 1);
let _g = 0;
let _g1 = _this13.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this13.stackVec3[i];
_this13.stackVec3[i] = null;
}
_this13.stackVec3 = newArray;
}
_this13.stackVec3[_this13.sizeVec3++] = o;
}
let _this14 = this.p;
if(m != null) {
m.e00 = 1;
m.e01 = 0;
m.e02 = 0;
m.e10 = 0;
m.e11 = 1;
m.e12 = 0;
m.e20 = 0;
m.e21 = 0;
m.e22 = 1;
if(_this14.sizeMat3 == _this14.stackMat3.length) {
let newArray = new Array(_this14.sizeMat3 << 1);
let _g = 0;
let _g1 = _this14.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this14.stackMat3[i];
_this14.stackMat3[i] = null;
}
_this14.stackMat3 = newArray;
}
_this14.stackMat3[_this14.sizeMat3++] = m;
}
let _this15 = this.p;
if(ex != null) {
ex.zero();
if(_this15.sizeVec3 == _this15.stackVec3.length) {
let newArray = new Array(_this15.sizeVec3 << 1);
let _g = 0;
let _g1 = _this15.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this15.stackVec3[i];
_this15.stackVec3[i] = null;
}
_this15.stackVec3 = newArray;
}
_this15.stackVec3[_this15.sizeVec3++] = ex;
}
let _this16 = this.p;
if(ey != null) {
ey.zero();
if(_this16.sizeVec3 == _this16.stackVec3.length) {
let newArray = new Array(_this16.sizeVec3 << 1);
let _g = 0;
let _g1 = _this16.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this16.stackVec3[i];
_this16.stackVec3[i] = null;
}
_this16.stackVec3 = newArray;
}
_this16.stackVec3[_this16.sizeVec3++] = ey;
}
let _this17 = this.p;
if(ez != null) {
ez.zero();
if(_this17.sizeVec3 == _this17.stackVec3.length) {
let newArray = new Array(_this17.sizeVec3 << 1);
let _g = 0;
let _g1 = _this17.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this17.stackVec3[i];
_this17.stackVec3[i] = null;
}
_this17.stackVec3 = newArray;
}
_this17.stackVec3[_this17.sizeVec3++] = ez;
}
}
sphere(tf,radius,color) {
let _this = this.p;
let o = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this.p;
let m = _this1.sizeMat3 == 0 ? new oimo.common.Mat3() : _this1.stackMat3[--_this1.sizeMat3];
let v = o;
v.x = tf._positionX;
v.y = tf._positionY;
v.z = tf._positionZ;
let m1 = m;
m1.e00 = tf._rotation00;
m1.e01 = tf._rotation01;
m1.e02 = tf._rotation02;
m1.e10 = tf._rotation10;
m1.e11 = tf._rotation11;
m1.e12 = tf._rotation12;
m1.e20 = tf._rotation20;
m1.e21 = tf._rotation21;
m1.e22 = tf._rotation22;
let vs = this.tmpSphereVerts;
let ns = this.tmpSphereNorms;
let _g = 0;
while(_g < 5) {
let i = _g++;
let n = this.tmpSphereVerts[i].length;
let _g1 = 0;
while(_g1 < n) {
let j = _g1++;
let _this = ns[i][j];
let v = this.sphereCoords[i][j];
_this.x = v.x;
_this.y = v.y;
_this.z = v.z;
let y = _this.x * m.e10 + _this.y * m.e11 + _this.z * m.e12;
let z = _this.x * m.e20 + _this.y * m.e21 + _this.z * m.e22;
_this.x = _this.x * m.e00 + _this.y * m.e01 + _this.z * m.e02;
_this.y = y;
_this.z = z;
let _this1 = vs[i][j];
let v1 = ns[i][j];
_this1.x = v1.x;
_this1.y = v1.y;
_this1.z = v1.z;
_this1.x *= radius;
_this1.y *= radius;
_this1.z *= radius;
_this1.x += o.x;
_this1.y += o.y;
_this1.z += o.z;
}
}
let _g1 = 0;
while(_g1 < 4) {
let i = _g1++;
let _g = 0;
while(_g < 8) {
let j = _g++;
let v1;
let v2;
let v3;
let v4;
let n1;
let n2;
let n3;
let n4;
if(i == 0) {
if(this.wireframe) {
v1 = vs[0][0];
v2 = vs[1][j];
this.line(v1,v2,color);
} else {
v1 = vs[0][0];
v2 = vs[1][j];
v3 = vs[1][(j + 1) % 8];
n1 = ns[0][0];
n2 = ns[1][j];
n3 = ns[1][(j + 1) % 8];
this.triangle(v1,v2,v3,n1,n2,n3,color);
}
} else if(i == 3) {
if(this.wireframe) {
v1 = vs[4][0];
v2 = vs[i][(j + 1) % 8];
v3 = vs[i][j];
this.line(v1,v2,color);
this.line(v2,v3,color);
} else {
v1 = vs[4][0];
v2 = vs[i][(j + 1) % 8];
v3 = vs[i][j];
n1 = ns[4][0];
n2 = ns[i][(j + 1) % 8];
n3 = ns[i][j];
this.triangle(v1,v2,v3,n1,n2,n3,color);
}
} else if(this.wireframe) {
v1 = vs[i][j];
v2 = vs[i][(j + 1) % 8];
v3 = vs[i + 1][j];
this.line(v1,v2,color);
this.line(v1,v3,color);
} else {
v1 = vs[i][j];
v2 = vs[i][(j + 1) % 8];
v3 = vs[i + 1][j];
v4 = vs[i + 1][(j + 1) % 8];
n1 = ns[i][j];
n2 = ns[i][(j + 1) % 8];
n3 = ns[i + 1][j];
n4 = ns[i + 1][(j + 1) % 8];
this.rect(v1,v3,v4,v2,n1,n3,n4,n2,color);
}
}
}
let _this2 = this.p;
if(o != null) {
o.zero();
if(_this2.sizeVec3 == _this2.stackVec3.length) {
let newArray = new Array(_this2.sizeVec3 << 1);
let _g = 0;
let _g1 = _this2.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this2.stackVec3[i];
_this2.stackVec3[i] = null;
}
_this2.stackVec3 = newArray;
}
_this2.stackVec3[_this2.sizeVec3++] = o;
}
let _this3 = this.p;
if(m != null) {
m.e00 = 1;
m.e01 = 0;
m.e02 = 0;
m.e10 = 0;
m.e11 = 1;
m.e12 = 0;
m.e20 = 0;
m.e21 = 0;
m.e22 = 1;
if(_this3.sizeMat3 == _this3.stackMat3.length) {
let newArray = new Array(_this3.sizeMat3 << 1);
let _g = 0;
let _g1 = _this3.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this3.stackMat3[i];
_this3.stackMat3[i] = null;
}
_this3.stackMat3 = newArray;
}
_this3.stackMat3[_this3.sizeMat3++] = m;
}
}
box(tf,halfExtents,color) {
let _this = this.p;
let ex = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
let _this1 = this.p;
let ey = _this1.sizeVec3 == 0 ? new oimo.common.Vec3() : _this1.stackVec3[--_this1.sizeVec3];
let _this2 = this.p;
let ez = _this2.sizeVec3 == 0 ? new oimo.common.Vec3() : _this2.stackVec3[--_this2.sizeVec3];
let _this3 = this.p;
let o = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
let _this4 = this.p;
let m = _this4.sizeMat3 == 0 ? new oimo.common.Mat3() : _this4.stackMat3[--_this4.sizeMat3];
let v = o;
v.x = tf._positionX;
v.y = tf._positionY;
v.z = tf._positionZ;
let m1 = m;
m1.e00 = tf._rotation00;
m1.e01 = tf._rotation01;
m1.e02 = tf._rotation02;
m1.e10 = tf._rotation10;
m1.e11 = tf._rotation11;
m1.e12 = tf._rotation12;
m1.e20 = tf._rotation20;
m1.e21 = tf._rotation21;
m1.e22 = tf._rotation22;
ex.init(m.e00,m.e10,m.e20);
ey.init(m.e01,m.e11,m.e21);
ez.init(m.e02,m.e12,m.e22);
let hx = halfExtents.x;
let hy = halfExtents.y;
let hz = halfExtents.z;
let _this5 = this.p;
let _this6 = _this5.sizeVec3 == 0 ? new oimo.common.Vec3() : _this5.stackVec3[--_this5.sizeVec3];
_this6.x = o.x;
_this6.y = o.y;
_this6.z = o.z;
let _this7 = _this6;
let s = -hx;
_this7.x += ex.x * s;
_this7.y += ex.y * s;
_this7.z += ex.z * s;
let s1 = -hy;
_this7.x += ey.x * s1;
_this7.y += ey.y * s1;
_this7.z += ey.z * s1;
let s2 = -hz;
_this7.x += ez.x * s2;
_this7.y += ez.y * s2;
_this7.z += ez.z * s2;
let _this8 = this.p;
let _this9 = _this8.sizeVec3 == 0 ? new oimo.common.Vec3() : _this8.stackVec3[--_this8.sizeVec3];
_this9.x = o.x;
_this9.y = o.y;
_this9.z = o.z;
let _this10 = _this9;
let s3 = -hx;
_this10.x += ex.x * s3;
_this10.y += ex.y * s3;
_this10.z += ex.z * s3;
let s4 = -hy;
_this10.x += ey.x * s4;
_this10.y += ey.y * s4;
_this10.z += ey.z * s4;
_this10.x += ez.x * hz;
_this10.y += ez.y * hz;
_this10.z += ez.z * hz;
let _this11 = this.p;
let _this12 = _this11.sizeVec3 == 0 ? new oimo.common.Vec3() : _this11.stackVec3[--_this11.sizeVec3];
_this12.x = o.x;
_this12.y = o.y;
_this12.z = o.z;
let _this13 = _this12;
let s5 = -hx;
_this13.x += ex.x * s5;
_this13.y += ex.y * s5;
_this13.z += ex.z * s5;
_this13.x += ey.x * hy;
_this13.y += ey.y * hy;
_this13.z += ey.z * hy;
let s6 = -hz;
_this13.x += ez.x * s6;
_this13.y += ez.y * s6;
_this13.z += ez.z * s6;
let _this14 = this.p;
let _this15 = _this14.sizeVec3 == 0 ? new oimo.common.Vec3() : _this14.stackVec3[--_this14.sizeVec3];
_this15.x = o.x;
_this15.y = o.y;
_this15.z = o.z;
let _this16 = _this15;
let s7 = -hx;
_this16.x += ex.x * s7;
_this16.y += ex.y * s7;
_this16.z += ex.z * s7;
_this16.x += ey.x * hy;
_this16.y += ey.y * hy;
_this16.z += ey.z * hy;
_this16.x += ez.x * hz;
_this16.y += ez.y * hz;
_this16.z += ez.z * hz;
let _this17 = this.p;
let _this18 = _this17.sizeVec3 == 0 ? new oimo.common.Vec3() : _this17.stackVec3[--_this17.sizeVec3];
_this18.x = o.x;
_this18.y = o.y;
_this18.z = o.z;
let _this19 = _this18;
_this19.x += ex.x * hx;
_this19.y += ex.y * hx;
_this19.z += ex.z * hx;
let s8 = -hy;
_this19.x += ey.x * s8;
_this19.y += ey.y * s8;
_this19.z += ey.z * s8;
let s9 = -hz;
_this19.x += ez.x * s9;
_this19.y += ez.y * s9;
_this19.z += ez.z * s9;
let _this20 = this.p;
let _this21 = _this20.sizeVec3 == 0 ? new oimo.common.Vec3() : _this20.stackVec3[--_this20.sizeVec3];
_this21.x = o.x;
_this21.y = o.y;
_this21.z = o.z;
let _this22 = _this21;
_this22.x += ex.x * hx;
_this22.y += ex.y * hx;
_this22.z += ex.z * hx;
let s10 = -hy;
_this22.x += ey.x * s10;
_this22.y += ey.y * s10;
_this22.z += ey.z * s10;
_this22.x += ez.x * hz;
_this22.y += ez.y * hz;
_this22.z += ez.z * hz;
let _this23 = this.p;
let _this24 = _this23.sizeVec3 == 0 ? new oimo.common.Vec3() : _this23.stackVec3[--_this23.sizeVec3];
_this24.x = o.x;
_this24.y = o.y;
_this24.z = o.z;
let _this25 = _this24;
_this25.x += ex.x * hx;
_this25.y += ex.y * hx;
_this25.z += ex.z * hx;
_this25.x += ey.x * hy;
_this25.y += ey.y * hy;
_this25.z += ey.z * hy;
let s11 = -hz;
_this25.x += ez.x * s11;
_this25.y += ez.y * s11;
_this25.z += ez.z * s11;
let _this26 = this.p;
let _this27 = _this26.sizeVec3 == 0 ? new oimo.common.Vec3() : _this26.stackVec3[--_this26.sizeVec3];
_this27.x = o.x;
_this27.y = o.y;
_this27.z = o.z;
let _this28 = _this27;
_this28.x += ex.x * hx;
_this28.y += ex.y * hx;
_this28.z += ex.z * hx;
_this28.x += ey.x * hy;
_this28.y += ey.y * hy;
_this28.z += ey.z * hy;
_this28.x += ez.x * hz;
_this28.y += ez.y * hz;
_this28.z += ez.z * hz;
if(this.wireframe) {
this.line(_this7,_this10,color);
this.line(_this13,_this16,color);
this.line(_this19,_this22,color);
this.line(_this25,_this28,color);
this.line(_this7,_this13,color);
this.line(_this10,_this16,color);
this.line(_this19,_this25,color);
this.line(_this22,_this28,color);
this.line(_this7,_this19,color);
this.line(_this10,_this22,color);
this.line(_this13,_this25,color);
this.line(_this16,_this28,color);
} else {
let _this = this.p;
let _this1 = _this.sizeVec3 == 0 ? new oimo.common.Vec3() : _this.stackVec3[--_this.sizeVec3];
_this1.x = ex.x;
_this1.y = ex.y;
_this1.z = ex.z;
let _this2 = _this1;
_this2.x = -_this2.x;
_this2.y = -_this2.y;
_this2.z = -_this2.z;
let _this3 = this.p;
let _this4 = _this3.sizeVec3 == 0 ? new oimo.common.Vec3() : _this3.stackVec3[--_this3.sizeVec3];
_this4.x = ey.x;
_this4.y = ey.y;
_this4.z = ey.z;
let _this5 = _this4;
_this5.x = -_this5.x;
_this5.y = -_this5.y;
_this5.z = -_this5.z;
let _this6 = this.p;
let _this8 = _this6.sizeVec3 == 0 ? new oimo.common.Vec3() : _this6.stackVec3[--_this6.sizeVec3];
_this8.x = ez.x;
_this8.y = ez.y;
_this8.z = ez.z;
let _this9 = _this8;
_this9.x = -_this9.x;
_this9.y = -_this9.y;
_this9.z = -_this9.z;
this.rect(_this7,_this10,_this16,_this13,_this2,_this2,_this2,_this2,color);
this.rect(_this19,_this25,_this28,_this22,ex,ex,ex,ex,color);
this.rect(_this7,_this19,_this22,_this10,_this5,_this5,_this5,_this5,color);
this.rect(_this13,_this16,_this28,_this25,ey,ey,ey,ey,color);
this.rect(_this7,_this13,_this25,_this19,_this9,_this9,_this9,_this9,color);
this.rect(_this10,_this22,_this28,_this16,ez,ez,ez,ez,color);
let _this11 = this.p;
if(_this2 != null) {
_this2.zero();
if(_this11.sizeVec3 == _this11.stackVec3.length) {
let newArray = new Array(_this11.sizeVec3 << 1);
let _g = 0;
let _g1 = _this11.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this11.stackVec3[i];
_this11.stackVec3[i] = null;
}
_this11.stackVec3 = newArray;
}
_this11.stackVec3[_this11.sizeVec3++] = _this2;
}
let _this12 = this.p;
if(_this5 != null) {
_this5.zero();
if(_this12.sizeVec3 == _this12.stackVec3.length) {
let newArray = new Array(_this12.sizeVec3 << 1);
let _g = 0;
let _g1 = _this12.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this12.stackVec3[i];
_this12.stackVec3[i] = null;
}
_this12.stackVec3 = newArray;
}
_this12.stackVec3[_this12.sizeVec3++] = _this5;
}
let _this14 = this.p;
if(_this9 != null) {
_this9.zero();
if(_this14.sizeVec3 == _this14.stackVec3.length) {
let newArray = new Array(_this14.sizeVec3 << 1);
let _g = 0;
let _g1 = _this14.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this14.stackVec3[i];
_this14.stackVec3[i] = null;
}
_this14.stackVec3 = newArray;
}
_this14.stackVec3[_this14.sizeVec3++] = _this9;
}
}
let _this29 = this.p;
if(_this7 != null) {
_this7.zero();
if(_this29.sizeVec3 == _this29.stackVec3.length) {
let newArray = new Array(_this29.sizeVec3 << 1);
let _g = 0;
let _g1 = _this29.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this29.stackVec3[i];
_this29.stackVec3[i] = null;
}
_this29.stackVec3 = newArray;
}
_this29.stackVec3[_this29.sizeVec3++] = _this7;
}
let _this30 = this.p;
if(_this10 != null) {
_this10.zero();
if(_this30.sizeVec3 == _this30.stackVec3.length) {
let newArray = new Array(_this30.sizeVec3 << 1);
let _g = 0;
let _g1 = _this30.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this30.stackVec3[i];
_this30.stackVec3[i] = null;
}
_this30.stackVec3 = newArray;
}
_this30.stackVec3[_this30.sizeVec3++] = _this10;
}
let _this31 = this.p;
if(_this13 != null) {
_this13.zero();
if(_this31.sizeVec3 == _this31.stackVec3.length) {
let newArray = new Array(_this31.sizeVec3 << 1);
let _g = 0;
let _g1 = _this31.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this31.stackVec3[i];
_this31.stackVec3[i] = null;
}
_this31.stackVec3 = newArray;
}
_this31.stackVec3[_this31.sizeVec3++] = _this13;
}
let _this32 = this.p;
if(_this16 != null) {
_this16.zero();
if(_this32.sizeVec3 == _this32.stackVec3.length) {
let newArray = new Array(_this32.sizeVec3 << 1);
let _g = 0;
let _g1 = _this32.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this32.stackVec3[i];
_this32.stackVec3[i] = null;
}
_this32.stackVec3 = newArray;
}
_this32.stackVec3[_this32.sizeVec3++] = _this16;
}
let _this33 = this.p;
if(_this19 != null) {
_this19.zero();
if(_this33.sizeVec3 == _this33.stackVec3.length) {
let newArray = new Array(_this33.sizeVec3 << 1);
let _g = 0;
let _g1 = _this33.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this33.stackVec3[i];
_this33.stackVec3[i] = null;
}
_this33.stackVec3 = newArray;
}
_this33.stackVec3[_this33.sizeVec3++] = _this19;
}
let _this34 = this.p;
if(_this22 != null) {
_this22.zero();
if(_this34.sizeVec3 == _this34.stackVec3.length) {
let newArray = new Array(_this34.sizeVec3 << 1);
let _g = 0;
let _g1 = _this34.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this34.stackVec3[i];
_this34.stackVec3[i] = null;
}
_this34.stackVec3 = newArray;
}
_this34.stackVec3[_this34.sizeVec3++] = _this22;
}
let _this35 = this.p;
if(_this25 != null) {
_this25.zero();
if(_this35.sizeVec3 == _this35.stackVec3.length) {
let newArray = new Array(_this35.sizeVec3 << 1);
let _g = 0;
let _g1 = _this35.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this35.stackVec3[i];
_this35.stackVec3[i] = null;
}
_this35.stackVec3 = newArray;
}
_this35.stackVec3[_this35.sizeVec3++] = _this25;
}
let _this36 = this.p;
if(_this28 != null) {
_this28.zero();
if(_this36.sizeVec3 == _this36.stackVec3.length) {
let newArray = new Array(_this36.sizeVec3 << 1);
let _g = 0;
let _g1 = _this36.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this36.stackVec3[i];
_this36.stackVec3[i] = null;
}
_this36.stackVec3 = newArray;
}
_this36.stackVec3[_this36.sizeVec3++] = _this28;
}
let _this37 = this.p;
if(o != null) {
o.zero();
if(_this37.sizeVec3 == _this37.stackVec3.length) {
let newArray = new Array(_this37.sizeVec3 << 1);
let _g = 0;
let _g1 = _this37.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this37.stackVec3[i];
_this37.stackVec3[i] = null;
}
_this37.stackVec3 = newArray;
}
_this37.stackVec3[_this37.sizeVec3++] = o;
}
let _this38 = this.p;
if(m != null) {
m.e00 = 1;
m.e01 = 0;
m.e02 = 0;
m.e10 = 0;
m.e11 = 1;
m.e12 = 0;
m.e20 = 0;
m.e21 = 0;
m.e22 = 1;
if(_this38.sizeMat3 == _this38.stackMat3.length) {
let newArray = new Array(_this38.sizeMat3 << 1);
let _g = 0;
let _g1 = _this38.sizeMat3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this38.stackMat3[i];
_this38.stackMat3[i] = null;
}
_this38.stackMat3 = newArray;
}
_this38.stackMat3[_this38.sizeMat3++] = m;
}
let _this39 = this.p;
if(ex != null) {
ex.zero();
if(_this39.sizeVec3 == _this39.stackVec3.length) {
let newArray = new Array(_this39.sizeVec3 << 1);
let _g = 0;
let _g1 = _this39.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this39.stackVec3[i];
_this39.stackVec3[i] = null;
}
_this39.stackVec3 = newArray;
}
_this39.stackVec3[_this39.sizeVec3++] = ex;
}
let _this40 = this.p;
if(ey != null) {
ey.zero();
if(_this40.sizeVec3 == _this40.stackVec3.length) {
let newArray = new Array(_this40.sizeVec3 << 1);
let _g = 0;
let _g1 = _this40.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this40.stackVec3[i];
_this40.stackVec3[i] = null;
}
_this40.stackVec3 = newArray;
}
_this40.stackVec3[_this40.sizeVec3++] = ey;
}
let _this41 = this.p;
if(ez != null) {
ez.zero();
if(_this41.sizeVec3 == _this41.stackVec3.length) {
let newArray = new Array(_this41.sizeVec3 << 1);
let _g = 0;
let _g1 = _this41.sizeVec3;
while(_g < _g1) {
let i = _g++;
newArray[i] = _this41.stackVec3[i];
_this41.stackVec3[i] = null;
}
_this41.stackVec3 = newArray;
}
_this41.stackVec3[_this41.sizeVec3++] = ez;
}
}
rect(v1,v2,v3,v4,n1,n2,n3,n4,color) {
this.triangle(v1,v2,v3,n1,n2,n3,color);
this.triangle(v1,v3,v4,n1,n3,n4,color);
}
point(v,color) {
}
triangle(v1,v2,v3,n1,n2,n3,color) {
}
line(v1,v2,color) {
}
}
oimo.dynamics.common.DebugDrawStyle = class oimo_dynamics_common_DebugDrawStyle {
constructor() {
this.basisColorZ = new oimo.common.Vec3(0.0,0.0,1.0);
this.basisColorY = new oimo.common.Vec3(0.0,1.0,0.0);
this.basisColorX = new oimo.common.Vec3(1.0,0.0,0.0);
this.basisLength = 0.5;
this.jointRotationalConstraintRadius = 0.3;
this.jointErrorColor = new oimo.common.Vec3(1.0,0.1,0.1);
this.jointLineColor = new oimo.common.Vec3(0.8,0.8,0.8);
this.contactBinormalLength = 0.5;
this.contactTangentLength = 0.5;
this.contactNormalLength = 0.5;
this.contactBinormalColor = new oimo.common.Vec3(0.2,0.2,1.0);
this.contactTangentColor = new oimo.common.Vec3(0.1,0.8,0.1);
this.contactNormalColor = new oimo.common.Vec3(1.0,0.1,0.1);
this.disabledContactColor = new oimo.common.Vec3(0.5,0.1,0.1);
this.newContactColor = new oimo.common.Vec3(1.0,1.0,0.1);
this.contactColor4 = new oimo.common.Vec3(0.8,0.1,1.0);
this.contactColor3 = new oimo.common.Vec3(0.1,0.8,0.6);
this.contactColor2 = new oimo.common.Vec3(1.0,0.6,0.1);
this.contactColor = new oimo.common.Vec3(1.0,0.1,0.1);
this.pairColor = new oimo.common.Vec3(1.0,1.0,0.1);
this.bvhNodeColor = new oimo.common.Vec3(0.4,0.4,0.4);
this.aabbColor = new oimo.common.Vec3(1.0,0.1,0.1);
this.kinematicShapeColor = new oimo.common.Vec3(1.0,0.5,0.1);
this.staticShapeColor = new oimo.common.Vec3(0.7,0.7,0.7);
this.sleepingShapeColor2 = new oimo.common.Vec3(0.2,0.8,0.5);
this.sleepingShapeColor1 = new oimo.common.Vec3(0.3,0.3,0.8);
this.sleepyShapeColor2 = new oimo.common.Vec3(0.6,0.8,0.3);
this.sleepyShapeColor1 = new oimo.common.Vec3(0.5,0.25,0.6);
this.shapeColor2 = new oimo.common.Vec3(1.0,0.8,0.1);
this.shapeColor1 = new oimo.common.Vec3(0.7,0.2,0.4);
}
}
oimo.dynamics.common.Performance = class oimo_dynamics_common_Performance {
}
if(!oimo.dynamics.constraint) oimo.dynamics.constraint = {};
oimo.dynamics.constraint.ConstraintSolver = class oimo_dynamics_constraint_ConstraintSolver {
constructor() {
this._b1 = null;
this._b2 = null;
this._addedToIsland = false;
}
preSolveVelocity(timeStep) {
}
warmStart(timeStep) {
}
solveVelocity() {
}
postSolveVelocity(timeStep) {
}
preSolvePosition(timeStep) {
}
solvePositionSplitImpulse() {
}
solvePositionNgs(timeStep) {
}
postSolve() {
}
}
oimo.dynamics.constraint.PositionCorrectionAlgorithm = class oimo_dynamics_constraint_PositionCorrectionAlgorithm {
}
if(!oimo.dynamics.constraint.contact) oimo.dynamics.constraint.contact = {};
oimo.dynamics.constraint.contact.ContactConstraint = class oimo_dynamics_constraint_contact_ContactConstraint {
constructor(manifold) {
this._solver = new oimo.dynamics.constraint.solver.pgs.PgsContactConstraintSolver(this);
this._manifold = manifold;
}
_getVelocitySolverInfo(timeStep,info) {
info.b1 = this._b1;
info.b2 = this._b2;
let normalX;
let normalY;
let normalZ;
let tangentX;
let tangentY;
let tangentZ;
let binormalX;
let binormalY;
let binormalZ;
normalX = this._manifold._normalX;
normalY = this._manifold._normalY;
normalZ = this._manifold._normalZ;
tangentX = this._manifold._tangentX;
tangentY = this._manifold._tangentY;
tangentZ = this._manifold._tangentZ;
binormalX = this._manifold._binormalX;
binormalY = this._manifold._binormalY;
binormalZ = this._manifold._binormalZ;
let friction = Math.sqrt(this._s1._friction * this._s2._friction);
let restitution = Math.sqrt(this._s1._restitution * this._s2._restitution);
let num = this._manifold._numPoints;
info.numRows = 0;
let _g = 0;
while(_g < num) {
let p = this._manifold._points[_g++];
if(p._depth < 0) {
p._disabled = true;
let _this = p._impulse;
_this.impulseN = 0;
_this.impulseT = 0;
_this.impulseB = 0;
_this.impulseP = 0;
_this.impulseLX = 0;
_this.impulseLY = 0;
_this.impulseLZ = 0;
continue;
} else {
p._disabled = false;
}
let row = info.rows[info.numRows++];
row.friction = friction;
row.cfm = 0;
let j = row.jacobianN;
j.lin1X = normalX;
j.lin1Y = normalY;
j.lin1Z = normalZ;
j.lin2X = normalX;
j.lin2Y = normalY;
j.lin2Z = normalZ;
j.ang1X = p._relPos1Y * normalZ - p._relPos1Z * normalY;
j.ang1Y = p._relPos1Z * normalX - p._relPos1X * normalZ;
j.ang1Z = p._relPos1X * normalY - p._relPos1Y * normalX;
j.ang2X = p._relPos2Y * normalZ - p._relPos2Z * normalY;
j.ang2Y = p._relPos2Z * normalX - p._relPos2X * normalZ;
j.ang2Z = p._relPos2X * normalY - p._relPos2Y * normalX;
j = row.jacobianT;
j.lin1X = tangentX;
j.lin1Y = tangentY;
j.lin1Z = tangentZ;
j.lin2X = tangentX;
j.lin2Y = tangentY;
j.lin2Z = tangentZ;
j.ang1X = p._relPos1Y * tangentZ - p._relPos1Z * tangentY;
j.ang1Y = p._relPos1Z * tangentX - p._relPos1X * tangentZ;
j.ang1Z = p._relPos1X * tangentY - p._relPos1Y * tangentX;
j.ang2X = p._relPos2Y * tangentZ - p._relPos2Z * tangentY;
j.ang2Y = p._relPos2Z * tangentX - p._relPos2X * tangentZ;
j.ang2Z = p._relPos2X * tangentY - p._relPos2Y * tangentX;
j = row.jacobianB;
j.lin1X = binormalX;
j.lin1Y = binormalY;
j.lin1Z = binormalZ;
j.lin2X = binormalX;
j.lin2Y = binormalY;
j.lin2Z = binormalZ;
j.ang1X = p._relPos1Y * binormalZ - p._relPos1Z * binormalY;
j.ang1Y = p._relPos1Z * binormalX - p._relPos1X * binormalZ;
j.ang1Z = p._relPos1X * binormalY - p._relPos1Y * binormalX;
j.ang2X = p._relPos2Y * binormalZ - p._relPos2Z * binormalY;
j.ang2Y = p._relPos2Z * binormalX - p._relPos2X * binormalZ;
j.ang2Z = p._relPos2X * binormalY - p._relPos2Y * binormalX;
j = row.jacobianN;
let rvn = j.lin1X * this._b1._velX + j.lin1Y * this._b1._velY + j.lin1Z * this._b1._velZ + (j.ang1X * this._b1._angVelX + j.ang1Y * this._b1._angVelY + j.ang1Z * this._b1._angVelZ) - (j.lin2X * this._b2._velX + j.lin2Y * this._b2._velY + j.lin2Z * this._b2._velZ + (j.ang2X * this._b2._angVelX + j.ang2Y * this._b2._angVelY + j.ang2Z * this._b2._angVelZ));
if(rvn < -oimo.common.Setting.contactEnableBounceThreshold && !p._warmStarted) {
row.rhs = -rvn * restitution;
} else {
row.rhs = 0;
}
if(this._positionCorrectionAlgorithm == oimo.dynamics.constraint.PositionCorrectionAlgorithm.BAUMGARTE) {
if(p._depth > oimo.common.Setting.linearSlop) {
let minRhs = (p._depth - oimo.common.Setting.linearSlop) * oimo.common.Setting.velocityBaumgarte * timeStep.invDt;
if(row.rhs < minRhs) {
row.rhs = minRhs;
}
}
}
if(!p._warmStarted) {
let _this = p._impulse;
_this.impulseN = 0;
_this.impulseT = 0;
_this.impulseB = 0;
_this.impulseP = 0;
_this.impulseLX = 0;
_this.impulseLY = 0;
_this.impulseLZ = 0;
}
row.impulse = p._impulse;
}
}
_getPositionSolverInfo(info) {
info.b1 = this._b1;
info.b2 = this._b2;
let normalX;
let normalY;
let normalZ;
normalX = this._manifold._normalX;
normalY = this._manifold._normalY;
normalZ = this._manifold._normalZ;
let num = this._manifold._numPoints;
info.numRows = 0;
let _g = 0;
while(_g < num) {
let p = this._manifold._points[_g++];
if(p._disabled) {
continue;
}
let row = info.rows[info.numRows++];
let j = row.jacobianN;
j.lin1X = normalX;
j.lin1Y = normalY;
j.lin1Z = normalZ;
j.lin2X = normalX;
j.lin2Y = normalY;
j.lin2Z = normalZ;
j.ang1X = p._relPos1Y * normalZ - p._relPos1Z * normalY;
j.ang1Y = p._relPos1Z * normalX - p._relPos1X * normalZ;
j.ang1Z = p._relPos1X * normalY - p._relPos1Y * normalX;
j.ang2X = p._relPos2Y * normalZ - p._relPos2Z * normalY;
j.ang2Y = p._relPos2Z * normalX - p._relPos2X * normalZ;
j.ang2Z = p._relPos2X * normalY - p._relPos2Y * normalX;
row.rhs = p._depth - oimo.common.Setting.linearSlop;
if(row.rhs < 0) {
row.rhs = 0;
}
row.impulse = p._impulse;
}
}
_syncManifold() {
this._manifold._updateDepthsAndPositions(this._tf1,this._tf2);
}
getShape1() {
return this._s1;
}
getShape2() {
return this._s2;
}
getManifold() {
return this._manifold;
}
isTouching() {
let _g = 0;
let _g1 = this._manifold._numPoints;
while(_g < _g1) if(this._manifold._points[_g++]._depth >= 0) {
return true;
}
return false;
}
}
oimo.dynamics.constraint.contact.ContactImpulse = class oimo_dynamics_constraint_contact_ContactImpulse {
constructor() {
this.impulseN = 0;
this.impulseT = 0;
this.impulseB = 0;
this.impulseP = 0;
this.impulseLX = 0;
this.impulseLY = 0;
this.impulseLZ = 0;
}
copyFrom(imp) {
this.impulseN = imp.impulseN;
this.impulseT = imp.impulseT;
this.impulseB = imp.impulseB;
this.impulseLX = imp.impulseLX;
this.impulseLY = imp.impulseLY;
this.impulseLZ = imp.impulseLZ;
}
}
oimo.dynamics.constraint.contact.Manifold = class oimo_dynamics_constraint_contact_Manifold {
constructor() {
this._normalX = 0;
this._normalY = 0;
this._normalZ = 0;
this._tangentX = 0;
this._tangentY = 0;
this._tangentZ = 0;
this._binormalX = 0;
this._binormalY = 0;
this._binormalZ = 0;
this._numPoints = 0;
this._points = new Array(oimo.common.Setting.maxManifoldPoints);
let _g = 0;
let _g1 = oimo.common.Setting.maxManifoldPoints;
while(_g < _g1) this._points[_g++] = new oimo.dynamics.constraint.contact.ManifoldPoint();
}
_clear() {
let _g = 0;
let _g1 = this._numPoints;
while(_g < _g1) {
let _this = this._points[_g++];
_this._localPos1X = 0;
_this._localPos1Y = 0;
_this._localPos1Z = 0;
_this._localPos2X = 0;
_this._localPos2Y = 0;
_this._localPos2Z = 0;
_this._relPos1X = 0;
_this._relPos1Y = 0;
_this._relPos1Z = 0;
_this._relPos2X = 0;
_this._relPos2Y = 0;
_this._relPos2Z = 0;
_this._pos1X = 0;
_this._pos1Y = 0;
_this._pos1Z = 0;
_this._pos2X = 0;
_this._pos2Y = 0;
_this._pos2Z = 0;
_this._depth = 0;
let _this1 = _this._impulse;
_this1.impulseN = 0;
_this1.impulseT = 0;
_this1.impulseB = 0;
_this1.impulseP = 0;
_this1.impulseLX = 0;
_this1.impulseLY = 0;
_this1.impulseLZ = 0;
_this._warmStarted = false;
_this._disabled = false;
_this._id = -1;
}
this._numPoints = 0;
}
_buildBasis(normal) {
this._normalX = normal.x;
this._normalY = normal.y;
this._normalZ = normal.z;
let nx = normal.x;
let ny = normal.y;
let nz = normal.z;
let nx2 = nx * nx;
let ny2 = ny * ny;
let nz2 = nz * nz;
let tx;
let ty;
let tz;
let bx;
let by;
let bz;
if(nx2 < ny2) {
if(nx2 < nz2) {
let invL = 1 / Math.sqrt(ny2 + nz2);
tx = 0;
ty = -nz * invL;
tz = ny * invL;
bx = ny * tz - nz * ty;
by = -nx * tz;
bz = nx * ty;
} else {
let invL = 1 / Math.sqrt(nx2 + ny2);
tx = -ny * invL;
ty = nx * invL;
tz = 0;
bx = -nz * ty;
by = nz * tx;
bz = nx * ty - ny * tx;
}
} else if(ny2 < nz2) {
let invL = 1 / Math.sqrt(nx2 + nz2);
tx = nz * invL;
ty = 0;
tz = -nx * invL;
bx = ny * tz;
by = nz * tx - nx * tz;
bz = -ny * tx;
} else {
let invL = 1 / Math.sqrt(nx2 + ny2);
tx = -ny * invL;
ty = nx * invL;
tz = 0;
bx = -nz * ty;
by = nz * tx;
bz = nx * ty - ny * tx;
}
this._tangentX = tx;
this._tangentY = ty;
this._tangentZ = tz;
this._binormalX = bx;
this._binormalY = by;
this._binormalZ = bz;
}
_updateDepthsAndPositions(tf1,tf2) {
let _g = 0;
let _g1 = this._numPoints;
while(_g < _g1) {
let p = this._points[_g++];
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * p._localPos1X + tf1._rotation01 * p._localPos1Y + tf1._rotation02 * p._localPos1Z;
__tmp__Y = tf1._rotation10 * p._localPos1X + tf1._rotation11 * p._localPos1Y + tf1._rotation12 * p._localPos1Z;
__tmp__Z = tf1._rotation20 * p._localPos1X + tf1._rotation21 * p._localPos1Y + tf1._rotation22 * p._localPos1Z;
p._relPos1X = __tmp__X;
p._relPos1Y = __tmp__Y;
p._relPos1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * p._localPos2X + tf2._rotation01 * p._localPos2Y + tf2._rotation02 * p._localPos2Z;
__tmp__Y1 = tf2._rotation10 * p._localPos2X + tf2._rotation11 * p._localPos2Y + tf2._rotation12 * p._localPos2Z;
__tmp__Z1 = tf2._rotation20 * p._localPos2X + tf2._rotation21 * p._localPos2Y + tf2._rotation22 * p._localPos2Z;
p._relPos2X = __tmp__X1;
p._relPos2Y = __tmp__Y1;
p._relPos2Z = __tmp__Z1;
p._pos1X = p._relPos1X + tf1._positionX;
p._pos1Y = p._relPos1Y + tf1._positionY;
p._pos1Z = p._relPos1Z + tf1._positionZ;
p._pos2X = p._relPos2X + tf2._positionX;
p._pos2Y = p._relPos2Y + tf2._positionY;
p._pos2Z = p._relPos2Z + tf2._positionZ;
let diffX;
let diffY;
let diffZ;
diffX = p._pos1X - p._pos2X;
diffY = p._pos1Y - p._pos2Y;
diffZ = p._pos1Z - p._pos2Z;
p._depth = -(diffX * this._normalX + diffY * this._normalY + diffZ * this._normalZ);
}
}
getNormal() {
let v = new oimo.common.Vec3();
v.x = this._normalX;
v.y = this._normalY;
v.z = this._normalZ;
return v;
}
getNormalTo(normal) {
normal.x = this._normalX;
normal.y = this._normalY;
normal.z = this._normalZ;
}
getTangent() {
let v = new oimo.common.Vec3();
v.x = this._tangentX;
v.y = this._tangentY;
v.z = this._tangentZ;
return v;
}
getTangentTo(tangent) {
tangent.x = this._tangentX;
tangent.y = this._tangentY;
tangent.z = this._tangentZ;
}
getBinormal() {
let v = new oimo.common.Vec3();
v.x = this._binormalX;
v.y = this._binormalY;
v.z = this._binormalZ;
return v;
}
getBinormalTo(binormal) {
binormal.x = this._binormalX;
binormal.y = this._binormalY;
binormal.z = this._binormalZ;
}
getPoints() {
return this._points;
}
getNumPoints() {
return this._numPoints;
}
}
oimo.dynamics.constraint.contact.ManifoldPoint = class oimo_dynamics_constraint_contact_ManifoldPoint {
constructor() {
this._localPos1X = 0;
this._localPos1Y = 0;
this._localPos1Z = 0;
this._localPos2X = 0;
this._localPos2Y = 0;
this._localPos2Z = 0;
this._relPos1X = 0;
this._relPos1Y = 0;
this._relPos1Z = 0;
this._relPos2X = 0;
this._relPos2Y = 0;
this._relPos2Z = 0;
this._pos1X = 0;
this._pos1Y = 0;
this._pos1Z = 0;
this._pos2X = 0;
this._pos2Y = 0;
this._pos2Z = 0;
this._depth = 0;
this._impulse = new oimo.dynamics.constraint.contact.ContactImpulse();
this._warmStarted = false;
this._disabled = false;
this._id = -1;
}
getPosition1() {
let v = new oimo.common.Vec3();
v.x = this._pos1X;
v.y = this._pos1Y;
v.z = this._pos1Z;
return v;
}
getPosition1To(position) {
position.x = this._pos1X;
position.y = this._pos1Y;
position.z = this._pos1Z;
}
getPosition2() {
let v = new oimo.common.Vec3();
v.x = this._pos2X;
v.y = this._pos2Y;
v.z = this._pos2Z;
return v;
}
getPosition2To(position) {
position.x = this._pos2X;
position.y = this._pos2Y;
position.z = this._pos2Z;
}
getDepth() {
return this._depth;
}
isWarmStarted() {
return this._warmStarted;
}
getNormalImpulse() {
return this._impulse.impulseN;
}
getTangentImpulse() {
return this._impulse.impulseT;
}
getBinormalImpulse() {
return this._impulse.impulseB;
}
isEnabled() {
return !this._disabled;
}
}
oimo.dynamics.constraint.contact.ManifoldUpdater = class oimo_dynamics_constraint_contact_ManifoldUpdater {
constructor(manifold) {
this._manifold = manifold;
this.numOldPoints = 0;
this.oldPoints = new Array(oimo.common.Setting.maxManifoldPoints);
let _g = 0;
let _g1 = oimo.common.Setting.maxManifoldPoints;
while(_g < _g1) this.oldPoints[_g++] = new oimo.dynamics.constraint.contact.ManifoldPoint();
}
removeOutdatedPoints() {
let index = this._manifold._numPoints;
while(--index >= 0) {
let p = this._manifold._points[index];
let diffX;
let diffY;
let diffZ;
diffX = p._pos1X - p._pos2X;
diffY = p._pos1Y - p._pos2Y;
diffZ = p._pos1Z - p._pos2Z;
let dotN = this._manifold._normalX * diffX + this._manifold._normalY * diffY + this._manifold._normalZ * diffZ;
if(dotN > oimo.common.Setting.contactPersistenceThreshold) {
this.removeManifoldPoint(index);
continue;
}
diffX += this._manifold._normalX * -dotN;
diffY += this._manifold._normalY * -dotN;
diffZ += this._manifold._normalZ * -dotN;
if(diffX * diffX + diffY * diffY + diffZ * diffZ > oimo.common.Setting.contactPersistenceThreshold * oimo.common.Setting.contactPersistenceThreshold) {
this.removeManifoldPoint(index);
continue;
}
}
}
removeManifoldPoint(index) {
let lastIndex = --this._manifold._numPoints;
if(index != lastIndex) {
let tmp = this._manifold._points[index];
this._manifold._points[index] = this._manifold._points[lastIndex];
this._manifold._points[lastIndex] = tmp;
}
let _this = this._manifold._points[lastIndex];
_this._localPos1X = 0;
_this._localPos1Y = 0;
_this._localPos1Z = 0;
_this._localPos2X = 0;
_this._localPos2Y = 0;
_this._localPos2Z = 0;
_this._relPos1X = 0;
_this._relPos1Y = 0;
_this._relPos1Z = 0;
_this._relPos2X = 0;
_this._relPos2Y = 0;
_this._relPos2Z = 0;
_this._pos1X = 0;
_this._pos1Y = 0;
_this._pos1Z = 0;
_this._pos2X = 0;
_this._pos2Y = 0;
_this._pos2Z = 0;
_this._depth = 0;
let _this1 = _this._impulse;
_this1.impulseN = 0;
_this1.impulseT = 0;
_this1.impulseB = 0;
_this1.impulseP = 0;
_this1.impulseLX = 0;
_this1.impulseLY = 0;
_this1.impulseLZ = 0;
_this._warmStarted = false;
_this._disabled = false;
_this._id = -1;
}
addManifoldPoint(point,tf1,tf2) {
let num = this._manifold._numPoints;
if(num == oimo.common.Setting.maxManifoldPoints) {
let targetIndex = this.computeTargetIndex(point,tf1,tf2);
let _this = this._manifold._points[targetIndex];
let v = point.position1;
_this._pos1X = v.x;
_this._pos1Y = v.y;
_this._pos1Z = v.z;
let v1 = point.position2;
_this._pos2X = v1.x;
_this._pos2Y = v1.y;
_this._pos2Z = v1.z;
_this._relPos1X = _this._pos1X - tf1._positionX;
_this._relPos1Y = _this._pos1Y - tf1._positionY;
_this._relPos1Z = _this._pos1Z - tf1._positionZ;
_this._relPos2X = _this._pos2X - tf2._positionX;
_this._relPos2Y = _this._pos2Y - tf2._positionY;
_this._relPos2Z = _this._pos2Z - tf2._positionZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * _this._relPos1X + tf1._rotation10 * _this._relPos1Y + tf1._rotation20 * _this._relPos1Z;
__tmp__Y = tf1._rotation01 * _this._relPos1X + tf1._rotation11 * _this._relPos1Y + tf1._rotation21 * _this._relPos1Z;
__tmp__Z = tf1._rotation02 * _this._relPos1X + tf1._rotation12 * _this._relPos1Y + tf1._rotation22 * _this._relPos1Z;
_this._localPos1X = __tmp__X;
_this._localPos1Y = __tmp__Y;
_this._localPos1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * _this._relPos2X + tf2._rotation10 * _this._relPos2Y + tf2._rotation20 * _this._relPos2Z;
__tmp__Y1 = tf2._rotation01 * _this._relPos2X + tf2._rotation11 * _this._relPos2Y + tf2._rotation21 * _this._relPos2Z;
__tmp__Z1 = tf2._rotation02 * _this._relPos2X + tf2._rotation12 * _this._relPos2Y + tf2._rotation22 * _this._relPos2Z;
_this._localPos2X = __tmp__X1;
_this._localPos2Y = __tmp__Y1;
_this._localPos2Z = __tmp__Z1;
_this._depth = point.depth;
let _this1 = _this._impulse;
_this1.impulseN = 0;
_this1.impulseT = 0;
_this1.impulseB = 0;
_this1.impulseP = 0;
_this1.impulseLX = 0;
_this1.impulseLY = 0;
_this1.impulseLZ = 0;
_this._id = point.id;
_this._warmStarted = false;
_this._disabled = false;
return;
}
let _this = this._manifold._points[num];
let v = point.position1;
_this._pos1X = v.x;
_this._pos1Y = v.y;
_this._pos1Z = v.z;
let v1 = point.position2;
_this._pos2X = v1.x;
_this._pos2Y = v1.y;
_this._pos2Z = v1.z;
_this._relPos1X = _this._pos1X - tf1._positionX;
_this._relPos1Y = _this._pos1Y - tf1._positionY;
_this._relPos1Z = _this._pos1Z - tf1._positionZ;
_this._relPos2X = _this._pos2X - tf2._positionX;
_this._relPos2Y = _this._pos2Y - tf2._positionY;
_this._relPos2Z = _this._pos2Z - tf2._positionZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * _this._relPos1X + tf1._rotation10 * _this._relPos1Y + tf1._rotation20 * _this._relPos1Z;
__tmp__Y = tf1._rotation01 * _this._relPos1X + tf1._rotation11 * _this._relPos1Y + tf1._rotation21 * _this._relPos1Z;
__tmp__Z = tf1._rotation02 * _this._relPos1X + tf1._rotation12 * _this._relPos1Y + tf1._rotation22 * _this._relPos1Z;
_this._localPos1X = __tmp__X;
_this._localPos1Y = __tmp__Y;
_this._localPos1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * _this._relPos2X + tf2._rotation10 * _this._relPos2Y + tf2._rotation20 * _this._relPos2Z;
__tmp__Y1 = tf2._rotation01 * _this._relPos2X + tf2._rotation11 * _this._relPos2Y + tf2._rotation21 * _this._relPos2Z;
__tmp__Z1 = tf2._rotation02 * _this._relPos2X + tf2._rotation12 * _this._relPos2Y + tf2._rotation22 * _this._relPos2Z;
_this._localPos2X = __tmp__X1;
_this._localPos2Y = __tmp__Y1;
_this._localPos2Z = __tmp__Z1;
_this._depth = point.depth;
let _this1 = _this._impulse;
_this1.impulseN = 0;
_this1.impulseT = 0;
_this1.impulseB = 0;
_this1.impulseP = 0;
_this1.impulseLX = 0;
_this1.impulseLY = 0;
_this1.impulseLZ = 0;
_this._id = point.id;
_this._warmStarted = false;
_this._disabled = false;
this._manifold._numPoints++;
}
computeTargetIndex(newPoint,tf1,tf2) {
let p1 = this._manifold._points[0];
let p2 = this._manifold._points[1];
let p3 = this._manifold._points[2];
let p4 = this._manifold._points[3];
let maxDepth = p1._depth;
let maxDepthIndex = 0;
if(p2._depth > maxDepth) {
maxDepth = p2._depth;
maxDepthIndex = 1;
}
if(p3._depth > maxDepth) {
maxDepth = p3._depth;
maxDepthIndex = 2;
}
if(p4._depth > maxDepth) {
maxDepthIndex = 3;
}
let rp1X;
let rp1Y;
let rp1Z;
let v = newPoint.position1;
rp1X = v.x;
rp1Y = v.y;
rp1Z = v.z;
rp1X -= tf1._positionX;
rp1Y -= tf1._positionY;
rp1Z -= tf1._positionZ;
let p1X = p2._relPos1X;
let p1Y = p2._relPos1Y;
let p1Z = p2._relPos1Z;
let p2X = p3._relPos1X;
let p2Y = p3._relPos1Y;
let p2Z = p3._relPos1Z;
let p3X = p4._relPos1X;
let p3Y = p4._relPos1Y;
let p3Z = p4._relPos1Z;
let v12X;
let v12Y;
let v12Z;
let v34X;
let v34Y;
let v34Z;
let v13X;
let v13Y;
let v13Z;
let v24X;
let v24Y;
let v24Z;
let v14X;
let v14Y;
let v14Z;
let v23X;
let v23Y;
let v23Z;
v12X = p2X - p1X;
v12Y = p2Y - p1Y;
v12Z = p2Z - p1Z;
v34X = rp1X - p3X;
v34Y = rp1Y - p3Y;
v34Z = rp1Z - p3Z;
v13X = p3X - p1X;
v13Y = p3Y - p1Y;
v13Z = p3Z - p1Z;
v24X = rp1X - p2X;
v24Y = rp1Y - p2Y;
v24Z = rp1Z - p2Z;
v14X = rp1X - p1X;
v14Y = rp1Y - p1Y;
v14Z = rp1Z - p1Z;
v23X = p3X - p2X;
v23Y = p3Y - p2Y;
v23Z = p3Z - p2Z;
let cross1X;
let cross1Y;
let cross1Z;
let cross2X;
let cross2Y;
let cross2Z;
let cross3X;
let cross3Y;
let cross3Z;
cross1X = v12Y * v34Z - v12Z * v34Y;
cross1Y = v12Z * v34X - v12X * v34Z;
cross1Z = v12X * v34Y - v12Y * v34X;
cross2X = v13Y * v24Z - v13Z * v24Y;
cross2Y = v13Z * v24X - v13X * v24Z;
cross2Z = v13X * v24Y - v13Y * v24X;
cross3X = v14Y * v23Z - v14Z * v23Y;
cross3Y = v14Z * v23X - v14X * v23Z;
cross3Z = v14X * v23Y - v14Y * v23X;
let a1 = cross1X * cross1X + cross1Y * cross1Y + cross1Z * cross1Z;
let a2 = cross2X * cross2X + cross2Y * cross2Y + cross2Z * cross2Z;
let a3 = cross3X * cross3X + cross3Y * cross3Y + cross3Z * cross3Z;
let p1X1 = p1._relPos1X;
let p1Y1 = p1._relPos1Y;
let p1Z1 = p1._relPos1Z;
let p2X1 = p3._relPos1X;
let p2Y1 = p3._relPos1Y;
let p2Z1 = p3._relPos1Z;
let p3X1 = p4._relPos1X;
let p3Y1 = p4._relPos1Y;
let p3Z1 = p4._relPos1Z;
let v12X1;
let v12Y1;
let v12Z1;
let v34X1;
let v34Y1;
let v34Z1;
let v13X1;
let v13Y1;
let v13Z1;
let v24X1;
let v24Y1;
let v24Z1;
let v14X1;
let v14Y1;
let v14Z1;
let v23X1;
let v23Y1;
let v23Z1;
v12X1 = p2X1 - p1X1;
v12Y1 = p2Y1 - p1Y1;
v12Z1 = p2Z1 - p1Z1;
v34X1 = rp1X - p3X1;
v34Y1 = rp1Y - p3Y1;
v34Z1 = rp1Z - p3Z1;
v13X1 = p3X1 - p1X1;
v13Y1 = p3Y1 - p1Y1;
v13Z1 = p3Z1 - p1Z1;
v24X1 = rp1X - p2X1;
v24Y1 = rp1Y - p2Y1;
v24Z1 = rp1Z - p2Z1;
v14X1 = rp1X - p1X1;
v14Y1 = rp1Y - p1Y1;
v14Z1 = rp1Z - p1Z1;
v23X1 = p3X1 - p2X1;
v23Y1 = p3Y1 - p2Y1;
v23Z1 = p3Z1 - p2Z1;
let cross1X1;
let cross1Y1;
let cross1Z1;
let cross2X1;
let cross2Y1;
let cross2Z1;
let cross3X1;
let cross3Y1;
let cross3Z1;
cross1X1 = v12Y1 * v34Z1 - v12Z1 * v34Y1;
cross1Y1 = v12Z1 * v34X1 - v12X1 * v34Z1;
cross1Z1 = v12X1 * v34Y1 - v12Y1 * v34X1;
cross2X1 = v13Y1 * v24Z1 - v13Z1 * v24Y1;
cross2Y1 = v13Z1 * v24X1 - v13X1 * v24Z1;
cross2Z1 = v13X1 * v24Y1 - v13Y1 * v24X1;
cross3X1 = v14Y1 * v23Z1 - v14Z1 * v23Y1;
cross3Y1 = v14Z1 * v23X1 - v14X1 * v23Z1;
cross3Z1 = v14X1 * v23Y1 - v14Y1 * v23X1;
let a11 = cross1X1 * cross1X1 + cross1Y1 * cross1Y1 + cross1Z1 * cross1Z1;
let a21 = cross2X1 * cross2X1 + cross2Y1 * cross2Y1 + cross2Z1 * cross2Z1;
let a31 = cross3X1 * cross3X1 + cross3Y1 * cross3Y1 + cross3Z1 * cross3Z1;
let a22 = a11 > a21 ? a11 > a31 ? a11 : a31 : a21 > a31 ? a21 : a31;
let p1X2 = p1._relPos1X;
let p1Y2 = p1._relPos1Y;
let p1Z2 = p1._relPos1Z;
let p2X2 = p2._relPos1X;
let p2Y2 = p2._relPos1Y;
let p2Z2 = p2._relPos1Z;
let p3X2 = p4._relPos1X;
let p3Y2 = p4._relPos1Y;
let p3Z2 = p4._relPos1Z;
let v12X2;
let v12Y2;
let v12Z2;
let v34X2;
let v34Y2;
let v34Z2;
let v13X2;
let v13Y2;
let v13Z2;
let v24X2;
let v24Y2;
let v24Z2;
let v14X2;
let v14Y2;
let v14Z2;
let v23X2;
let v23Y2;
let v23Z2;
v12X2 = p2X2 - p1X2;
v12Y2 = p2Y2 - p1Y2;
v12Z2 = p2Z2 - p1Z2;
v34X2 = rp1X - p3X2;
v34Y2 = rp1Y - p3Y2;
v34Z2 = rp1Z - p3Z2;
v13X2 = p3X2 - p1X2;
v13Y2 = p3Y2 - p1Y2;
v13Z2 = p3Z2 - p1Z2;
v24X2 = rp1X - p2X2;
v24Y2 = rp1Y - p2Y2;
v24Z2 = rp1Z - p2Z2;
v14X2 = rp1X - p1X2;
v14Y2 = rp1Y - p1Y2;
v14Z2 = rp1Z - p1Z2;
v23X2 = p3X2 - p2X2;
v23Y2 = p3Y2 - p2Y2;
v23Z2 = p3Z2 - p2Z2;
let cross1X2;
let cross1Y2;
let cross1Z2;
let cross2X2;
let cross2Y2;
let cross2Z2;
let cross3X2;
let cross3Y2;
let cross3Z2;
cross1X2 = v12Y2 * v34Z2 - v12Z2 * v34Y2;
cross1Y2 = v12Z2 * v34X2 - v12X2 * v34Z2;
cross1Z2 = v12X2 * v34Y2 - v12Y2 * v34X2;
cross2X2 = v13Y2 * v24Z2 - v13Z2 * v24Y2;
cross2Y2 = v13Z2 * v24X2 - v13X2 * v24Z2;
cross2Z2 = v13X2 * v24Y2 - v13Y2 * v24X2;
cross3X2 = v14Y2 * v23Z2 - v14Z2 * v23Y2;
cross3Y2 = v14Z2 * v23X2 - v14X2 * v23Z2;
cross3Z2 = v14X2 * v23Y2 - v14Y2 * v23X2;
let a12 = cross1X2 * cross1X2 + cross1Y2 * cross1Y2 + cross1Z2 * cross1Z2;
let a23 = cross2X2 * cross2X2 + cross2Y2 * cross2Y2 + cross2Z2 * cross2Z2;
let a32 = cross3X2 * cross3X2 + cross3Y2 * cross3Y2 + cross3Z2 * cross3Z2;
let a33 = a12 > a23 ? a12 > a32 ? a12 : a32 : a23 > a32 ? a23 : a32;
let p1X3 = p1._relPos1X;
let p1Y3 = p1._relPos1Y;
let p1Z3 = p1._relPos1Z;
let p2X3 = p2._relPos1X;
let p2Y3 = p2._relPos1Y;
let p2Z3 = p2._relPos1Z;
let p3X3 = p3._relPos1X;
let p3Y3 = p3._relPos1Y;
let p3Z3 = p3._relPos1Z;
let v12X3;
let v12Y3;
let v12Z3;
let v34X3;
let v34Y3;
let v34Z3;
let v13X3;
let v13Y3;
let v13Z3;
let v24X3;
let v24Y3;
let v24Z3;
let v14X3;
let v14Y3;
let v14Z3;
let v23X3;
let v23Y3;
let v23Z3;
v12X3 = p2X3 - p1X3;
v12Y3 = p2Y3 - p1Y3;
v12Z3 = p2Z3 - p1Z3;
v34X3 = rp1X - p3X3;
v34Y3 = rp1Y - p3Y3;
v34Z3 = rp1Z - p3Z3;
v13X3 = p3X3 - p1X3;
v13Y3 = p3Y3 - p1Y3;
v13Z3 = p3Z3 - p1Z3;
v24X3 = rp1X - p2X3;
v24Y3 = rp1Y - p2Y3;
v24Z3 = rp1Z - p2Z3;
v14X3 = rp1X - p1X3;
v14Y3 = rp1Y - p1Y3;
v14Z3 = rp1Z - p1Z3;
v23X3 = p3X3 - p2X3;
v23Y3 = p3Y3 - p2Y3;
v23Z3 = p3Z3 - p2Z3;
let cross1X3;
let cross1Y3;
let cross1Z3;
let cross2X3;
let cross2Y3;
let cross2Z3;
let cross3X3;
let cross3Y3;
let cross3Z3;
cross1X3 = v12Y3 * v34Z3 - v12Z3 * v34Y3;
cross1Y3 = v12Z3 * v34X3 - v12X3 * v34Z3;
cross1Z3 = v12X3 * v34Y3 - v12Y3 * v34X3;
cross2X3 = v13Y3 * v24Z3 - v13Z3 * v24Y3;
cross2Y3 = v13Z3 * v24X3 - v13X3 * v24Z3;
cross2Z3 = v13X3 * v24Y3 - v13Y3 * v24X3;
cross3X3 = v14Y3 * v23Z3 - v14Z3 * v23Y3;
cross3Y3 = v14Z3 * v23X3 - v14X3 * v23Z3;
cross3Z3 = v14X3 * v23Y3 - v14Y3 * v23X3;
let a13 = cross1X3 * cross1X3 + cross1Y3 * cross1Y3 + cross1Z3 * cross1Z3;
let a24 = cross2X3 * cross2X3 + cross2Y3 * cross2Y3 + cross2Z3 * cross2Z3;
let a34 = cross3X3 * cross3X3 + cross3Y3 * cross3Y3 + cross3Z3 * cross3Z3;
let a4 = a13 > a24 ? a13 > a34 ? a13 : a34 : a24 > a34 ? a24 : a34;
let max = a1 > a2 ? a1 > a3 ? a1 : a3 : a2 > a3 ? a2 : a3;
let target = 0;
if(a22 > max && maxDepthIndex != 1 || maxDepthIndex == 0) {
max = a22;
target = 1;
}
if(a33 > max && maxDepthIndex != 2) {
max = a33;
target = 2;
}
if(a4 > max && maxDepthIndex != 3) {
target = 3;
}
return target;
}
computeRelativePositions(tf1,tf2) {
let num = this._manifold._numPoints;
let _g = 0;
while(_g < num) {
let p = this._manifold._points[_g++];
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * p._localPos1X + tf1._rotation01 * p._localPos1Y + tf1._rotation02 * p._localPos1Z;
__tmp__Y = tf1._rotation10 * p._localPos1X + tf1._rotation11 * p._localPos1Y + tf1._rotation12 * p._localPos1Z;
__tmp__Z = tf1._rotation20 * p._localPos1X + tf1._rotation21 * p._localPos1Y + tf1._rotation22 * p._localPos1Z;
p._relPos1X = __tmp__X;
p._relPos1Y = __tmp__Y;
p._relPos1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * p._localPos2X + tf2._rotation01 * p._localPos2Y + tf2._rotation02 * p._localPos2Z;
__tmp__Y1 = tf2._rotation10 * p._localPos2X + tf2._rotation11 * p._localPos2Y + tf2._rotation12 * p._localPos2Z;
__tmp__Z1 = tf2._rotation20 * p._localPos2X + tf2._rotation21 * p._localPos2Y + tf2._rotation22 * p._localPos2Z;
p._relPos2X = __tmp__X1;
p._relPos2Y = __tmp__Y1;
p._relPos2Z = __tmp__Z1;
p._warmStarted = true;
}
}
findNearestContactPointIndex(target,tf1,tf2) {
let nearestSq = oimo.common.Setting.contactPersistenceThreshold * oimo.common.Setting.contactPersistenceThreshold;
let idx = -1;
let _g = 0;
let _g1 = this._manifold._numPoints;
while(_g < _g1) {
let i = _g++;
let mp = this._manifold._points[i];
let rp1X;
let rp1Y;
let rp1Z;
let rp2X;
let rp2Y;
let rp2Z;
let v = target.position1;
rp1X = v.x;
rp1Y = v.y;
rp1Z = v.z;
let v1 = target.position2;
rp2X = v1.x;
rp2Y = v1.y;
rp2Z = v1.z;
rp1X -= tf1._positionX;
rp1Y -= tf1._positionY;
rp1Z -= tf1._positionZ;
rp2X -= tf2._positionX;
rp2Y -= tf2._positionY;
rp2Z -= tf2._positionZ;
let diff1X;
let diff1Y;
let diff1Z;
let diff2X;
let diff2Y;
let diff2Z;
diff1X = mp._relPos1X - rp1X;
diff1Y = mp._relPos1Y - rp1Y;
diff1Z = mp._relPos1Z - rp1Z;
diff2X = mp._relPos2X - rp2X;
diff2Y = mp._relPos2Y - rp2Y;
diff2Z = mp._relPos2Z - rp2Z;
let sq1 = diff1X * diff1X + diff1Y * diff1Y + diff1Z * diff1Z;
let sq2 = diff2X * diff2X + diff2Y * diff2Y + diff2Z * diff2Z;
let d = sq1 < sq2 ? sq1 : sq2;
if(d < nearestSq) {
nearestSq = d;
idx = i;
}
}
return idx;
}
totalUpdate(result,tf1,tf2) {
this.numOldPoints = this._manifold._numPoints;
let _g = 0;
let _g1 = this.numOldPoints;
while(_g < _g1) {
let i = _g++;
let _this = this.oldPoints[i];
let cp = this._manifold._points[i];
_this._localPos1X = cp._localPos1X;
_this._localPos1Y = cp._localPos1Y;
_this._localPos1Z = cp._localPos1Z;
_this._localPos2X = cp._localPos2X;
_this._localPos2Y = cp._localPos2Y;
_this._localPos2Z = cp._localPos2Z;
_this._relPos1X = cp._relPos1X;
_this._relPos1Y = cp._relPos1Y;
_this._relPos1Z = cp._relPos1Z;
_this._relPos2X = cp._relPos2X;
_this._relPos2Y = cp._relPos2Y;
_this._relPos2Z = cp._relPos2Z;
_this._pos1X = cp._pos1X;
_this._pos1Y = cp._pos1Y;
_this._pos1Z = cp._pos1Z;
_this._pos2X = cp._pos2X;
_this._pos2Y = cp._pos2Y;
_this._pos2Z = cp._pos2Z;
_this._depth = cp._depth;
_this._impulse.copyFrom(cp._impulse);
_this._id = cp._id;
_this._warmStarted = cp._warmStarted;
_this._disabled = false;
}
let num = result.numPoints;
this._manifold._numPoints = num;
let _g2 = 0;
while(_g2 < num) {
let i = _g2++;
let p = this._manifold._points[i];
let ref = result.points[i];
let v = ref.position1;
p._pos1X = v.x;
p._pos1Y = v.y;
p._pos1Z = v.z;
let v1 = ref.position2;
p._pos2X = v1.x;
p._pos2Y = v1.y;
p._pos2Z = v1.z;
p._relPos1X = p._pos1X - tf1._positionX;
p._relPos1Y = p._pos1Y - tf1._positionY;
p._relPos1Z = p._pos1Z - tf1._positionZ;
p._relPos2X = p._pos2X - tf2._positionX;
p._relPos2Y = p._pos2Y - tf2._positionY;
p._relPos2Z = p._pos2Z - tf2._positionZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * p._relPos1X + tf1._rotation10 * p._relPos1Y + tf1._rotation20 * p._relPos1Z;
__tmp__Y = tf1._rotation01 * p._relPos1X + tf1._rotation11 * p._relPos1Y + tf1._rotation21 * p._relPos1Z;
__tmp__Z = tf1._rotation02 * p._relPos1X + tf1._rotation12 * p._relPos1Y + tf1._rotation22 * p._relPos1Z;
p._localPos1X = __tmp__X;
p._localPos1Y = __tmp__Y;
p._localPos1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * p._relPos2X + tf2._rotation10 * p._relPos2Y + tf2._rotation20 * p._relPos2Z;
__tmp__Y1 = tf2._rotation01 * p._relPos2X + tf2._rotation11 * p._relPos2Y + tf2._rotation21 * p._relPos2Z;
__tmp__Z1 = tf2._rotation02 * p._relPos2X + tf2._rotation12 * p._relPos2Y + tf2._rotation22 * p._relPos2Z;
p._localPos2X = __tmp__X1;
p._localPos2Y = __tmp__Y1;
p._localPos2Z = __tmp__Z1;
p._depth = ref.depth;
let _this = p._impulse;
_this.impulseN = 0;
_this.impulseT = 0;
_this.impulseB = 0;
_this.impulseP = 0;
_this.impulseLX = 0;
_this.impulseLY = 0;
_this.impulseLZ = 0;
p._id = ref.id;
p._warmStarted = false;
p._disabled = false;
let _g = 0;
let _g1 = this.numOldPoints;
while(_g < _g1) {
let ocp = this.oldPoints[_g++];
if(p._id == ocp._id) {
p._impulse.copyFrom(ocp._impulse);
p._warmStarted = true;
break;
}
}
}
}
incrementalUpdate(result,tf1,tf2) {
this._manifold._updateDepthsAndPositions(tf1,tf2);
let _g = 0;
let _g1 = this._manifold._numPoints;
while(_g < _g1) this._manifold._points[_g++]._warmStarted = true;
let newPoint = result.points[0];
let index = this.findNearestContactPointIndex(newPoint,tf1,tf2);
if(index == -1) {
this.addManifoldPoint(newPoint,tf1,tf2);
} else {
let cp = this._manifold._points[index];
let v = newPoint.position1;
cp._pos1X = v.x;
cp._pos1Y = v.y;
cp._pos1Z = v.z;
let v1 = newPoint.position2;
cp._pos2X = v1.x;
cp._pos2Y = v1.y;
cp._pos2Z = v1.z;
cp._relPos1X = cp._pos1X - tf1._positionX;
cp._relPos1Y = cp._pos1Y - tf1._positionY;
cp._relPos1Z = cp._pos1Z - tf1._positionZ;
cp._relPos2X = cp._pos2X - tf2._positionX;
cp._relPos2Y = cp._pos2Y - tf2._positionY;
cp._relPos2Z = cp._pos2Z - tf2._positionZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * cp._relPos1X + tf1._rotation10 * cp._relPos1Y + tf1._rotation20 * cp._relPos1Z;
__tmp__Y = tf1._rotation01 * cp._relPos1X + tf1._rotation11 * cp._relPos1Y + tf1._rotation21 * cp._relPos1Z;
__tmp__Z = tf1._rotation02 * cp._relPos1X + tf1._rotation12 * cp._relPos1Y + tf1._rotation22 * cp._relPos1Z;
cp._localPos1X = __tmp__X;
cp._localPos1Y = __tmp__Y;
cp._localPos1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * cp._relPos2X + tf2._rotation10 * cp._relPos2Y + tf2._rotation20 * cp._relPos2Z;
__tmp__Y1 = tf2._rotation01 * cp._relPos2X + tf2._rotation11 * cp._relPos2Y + tf2._rotation21 * cp._relPos2Z;
__tmp__Z1 = tf2._rotation02 * cp._relPos2X + tf2._rotation12 * cp._relPos2Y + tf2._rotation22 * cp._relPos2Z;
cp._localPos2X = __tmp__X1;
cp._localPos2Y = __tmp__Y1;
cp._localPos2Z = __tmp__Z1;
cp._depth = newPoint.depth;
}
this.removeOutdatedPoints();
}
}
if(!oimo.dynamics.constraint.info) oimo.dynamics.constraint.info = {};
oimo.dynamics.constraint.info.JacobianRow = class oimo_dynamics_constraint_info_JacobianRow {
constructor() {
this.lin1X = 0;
this.lin1Y = 0;
this.lin1Z = 0;
this.lin2X = 0;
this.lin2Y = 0;
this.lin2Z = 0;
this.ang1X = 0;
this.ang1Y = 0;
this.ang1Z = 0;
this.ang2X = 0;
this.ang2Y = 0;
this.ang2Z = 0;
this.flag = 0;
}
updateSparsity() {
this.flag = 0;
if(!(this.lin1X == 0 && this.lin1Y == 0 && this.lin1Z == 0) || !(this.lin2X == 0 && this.lin2Y == 0 && this.lin2Z == 0)) {
this.flag |= 1;
}
if(!(this.ang1X == 0 && this.ang1Y == 0 && this.ang1Z == 0) || !(this.ang2X == 0 && this.ang2Y == 0 && this.ang2Z == 0)) {
this.flag |= 2;
}
}
}
if(!oimo.dynamics.constraint.info.contact) oimo.dynamics.constraint.info.contact = {};
oimo.dynamics.constraint.info.contact.ContactSolverInfo = class oimo_dynamics_constraint_info_contact_ContactSolverInfo {
constructor() {
this.b1 = null;
this.b2 = null;
this.numRows = 0;
this.rows = new Array(oimo.common.Setting.maxManifoldPoints);
let _g = 0;
let _g1 = this.rows.length;
while(_g < _g1) this.rows[_g++] = new oimo.dynamics.constraint.info.contact.ContactSolverInfoRow();
}
}
oimo.dynamics.constraint.info.contact.ContactSolverInfoRow = class oimo_dynamics_constraint_info_contact_ContactSolverInfoRow {
constructor() {
this.jacobianN = new oimo.dynamics.constraint.info.JacobianRow();
this.jacobianT = new oimo.dynamics.constraint.info.JacobianRow();
this.jacobianB = new oimo.dynamics.constraint.info.JacobianRow();
this.rhs = 0;
this.cfm = 0;
this.friction = 0;
this.impulse = null;
}
}
if(!oimo.dynamics.constraint.info.joint) oimo.dynamics.constraint.info.joint = {};
oimo.dynamics.constraint.info.joint.JointSolverInfo = class oimo_dynamics_constraint_info_joint_JointSolverInfo {
constructor() {
this.b1 = null;
this.b2 = null;
this.numRows = 0;
this.rows = new Array(oimo.common.Setting.maxJacobianRows);
let _g = 0;
let _g1 = this.rows.length;
while(_g < _g1) this.rows[_g++] = new oimo.dynamics.constraint.info.joint.JointSolverInfoRow();
}
}
oimo.dynamics.constraint.info.joint.JointSolverInfoRow = class oimo_dynamics_constraint_info_joint_JointSolverInfoRow {
constructor() {
this.jacobian = new oimo.dynamics.constraint.info.JacobianRow();
this.rhs = 0;
this.cfm = 0;
this.minImpulse = 0;
this.maxImpulse = 0;
this.motorSpeed = 0;
this.motorMaxImpulse = 0;
this.impulse = null;
}
}
if(!oimo.dynamics.constraint.joint) oimo.dynamics.constraint.joint = {};
oimo.dynamics.constraint.joint.BasisTracker = class oimo_dynamics_constraint_joint_BasisTracker {
constructor(joint) {
this.joint = joint;
this.xX = 0;
this.xY = 0;
this.xZ = 0;
this.yX = 0;
this.yY = 0;
this.yZ = 0;
this.zX = 0;
this.zY = 0;
this.zZ = 0;
}
}
oimo.dynamics.constraint.joint.Joint = class oimo_dynamics_constraint_joint_Joint {
constructor(config,type) {
this._link1 = new oimo.dynamics.constraint.joint.JointLink(this);
this._link2 = new oimo.dynamics.constraint.joint.JointLink(this);
this._positionCorrectionAlgorithm = oimo.common.Setting.defaultJointPositionCorrectionAlgorithm;
this._type = type;
this._world = null;
this._b1 = config.rigidBody1;
this._b2 = config.rigidBody2;
this._allowCollision = config.allowCollision;
this._breakForce = config.breakForce;
this._breakTorque = config.breakTorque;
switch(config.solverType) {
case 0:
this._solver = new oimo.dynamics.constraint.solver.pgs.PgsJointConstraintSolver(this);
break;
case 1:
this._solver = new oimo.dynamics.constraint.solver.direct.DirectJointConstraintSolver(this);
break;
}
let v = config.localAnchor1;
this._localAnchor1X = v.x;
this._localAnchor1Y = v.y;
this._localAnchor1Z = v.z;
let v1 = config.localAnchor2;
this._localAnchor2X = v1.x;
this._localAnchor2Y = v1.y;
this._localAnchor2Z = v1.z;
this._relativeAnchor1X = 0;
this._relativeAnchor1Y = 0;
this._relativeAnchor1Z = 0;
this._relativeAnchor2X = 0;
this._relativeAnchor2Y = 0;
this._relativeAnchor2Z = 0;
this._anchor1X = 0;
this._anchor1Y = 0;
this._anchor1Z = 0;
this._anchor2X = 0;
this._anchor2Y = 0;
this._anchor2Z = 0;
this._localBasisX1X = 0;
this._localBasisX1Y = 0;
this._localBasisX1Z = 0;
this._localBasisY1X = 0;
this._localBasisY1Y = 0;
this._localBasisY1Z = 0;
this._localBasisZ1X = 0;
this._localBasisZ1Y = 0;
this._localBasisZ1Z = 0;
this._localBasisX2X = 0;
this._localBasisX2Y = 0;
this._localBasisX2Z = 0;
this._localBasisY2X = 0;
this._localBasisY2Y = 0;
this._localBasisY2Z = 0;
this._localBasisZ2X = 0;
this._localBasisZ2Y = 0;
this._localBasisZ2Z = 0;
this._impulses = new Array(oimo.common.Setting.maxJacobianRows);
let _g = 0;
let _g1 = oimo.common.Setting.maxJacobianRows;
while(_g < _g1) this._impulses[_g++] = new oimo.dynamics.constraint.joint.JointImpulse();
}
buildLocalBasesFromX() {
if(this._localBasisX1X * this._localBasisX1X + this._localBasisX1Y * this._localBasisX1Y + this._localBasisX1Z * this._localBasisX1Z == 0) {
this._localBasisX1X = 1;
this._localBasisX1Y = 0;
this._localBasisX1Z = 0;
} else {
let l = this._localBasisX1X * this._localBasisX1X + this._localBasisX1Y * this._localBasisX1Y + this._localBasisX1Z * this._localBasisX1Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisX1X *= l;
this._localBasisX1Y *= l;
this._localBasisX1Z *= l;
}
if(this._localBasisX2X * this._localBasisX2X + this._localBasisX2Y * this._localBasisX2Y + this._localBasisX2Z * this._localBasisX2Z == 0) {
this._localBasisX2X = 1;
this._localBasisX2Y = 0;
this._localBasisX2Z = 0;
} else {
let l = this._localBasisX2X * this._localBasisX2X + this._localBasisX2Y * this._localBasisX2Y + this._localBasisX2Z * this._localBasisX2Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisX2X *= l;
this._localBasisX2Y *= l;
this._localBasisX2Z *= l;
}
let slerpQX;
let slerpQY;
let slerpQZ;
let slerpQW;
let slerpM00;
let slerpM01;
let slerpM02;
let slerpM10;
let slerpM11;
let slerpM12;
let slerpM20;
let slerpM21;
let slerpM22;
let d = this._localBasisX1X * this._localBasisX2X + this._localBasisX1Y * this._localBasisX2Y + this._localBasisX1Z * this._localBasisX2Z;
if(d < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = this._localBasisX1X;
let y1 = this._localBasisX1Y;
let z1 = this._localBasisX1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
slerpQX = vX;
slerpQY = vY;
slerpQZ = vZ;
slerpQW = 0;
} else {
let cX;
let cY;
let cZ;
cX = this._localBasisX1Y * this._localBasisX2Z - this._localBasisX1Z * this._localBasisX2Y;
cY = this._localBasisX1Z * this._localBasisX2X - this._localBasisX1X * this._localBasisX2Z;
cZ = this._localBasisX1X * this._localBasisX2Y - this._localBasisX1Y * this._localBasisX2X;
let w = Math.sqrt((1 + d) * 0.5);
d = 0.5 / w;
cX *= d;
cY *= d;
cZ *= d;
slerpQX = cX;
slerpQY = cY;
slerpQZ = cZ;
slerpQW = w;
}
let x = slerpQX;
let y = slerpQY;
let z = slerpQZ;
let w = slerpQW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
slerpM00 = 1 - yy - zz;
slerpM01 = xy - wz;
slerpM02 = xz + wy;
slerpM10 = xy + wz;
slerpM11 = 1 - xx - zz;
slerpM12 = yz - wx;
slerpM20 = xz - wy;
slerpM21 = yz + wx;
slerpM22 = 1 - xx - yy;
let x1 = this._localBasisX1X;
let y1 = this._localBasisX1Y;
let z1 = this._localBasisX1Z;
let x21 = x1 * x1;
let y21 = y1 * y1;
let z21 = z1 * z1;
let d1;
if(x21 < y21) {
if(x21 < z21) {
d1 = 1 / Math.sqrt(y21 + z21);
this._localBasisY1X = 0;
this._localBasisY1Y = z1 * d1;
this._localBasisY1Z = -y1 * d1;
} else {
d1 = 1 / Math.sqrt(x21 + y21);
this._localBasisY1X = y1 * d1;
this._localBasisY1Y = -x1 * d1;
this._localBasisY1Z = 0;
}
} else if(y21 < z21) {
d1 = 1 / Math.sqrt(z21 + x21);
this._localBasisY1X = -z1 * d1;
this._localBasisY1Y = 0;
this._localBasisY1Z = x1 * d1;
} else {
d1 = 1 / Math.sqrt(x21 + y21);
this._localBasisY1X = y1 * d1;
this._localBasisY1Y = -x1 * d1;
this._localBasisY1Z = 0;
}
this._localBasisZ1X = this._localBasisX1Y * this._localBasisY1Z - this._localBasisX1Z * this._localBasisY1Y;
this._localBasisZ1Y = this._localBasisX1Z * this._localBasisY1X - this._localBasisX1X * this._localBasisY1Z;
this._localBasisZ1Z = this._localBasisX1X * this._localBasisY1Y - this._localBasisX1Y * this._localBasisY1X;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = slerpM00 * this._localBasisX1X + slerpM01 * this._localBasisX1Y + slerpM02 * this._localBasisX1Z;
__tmp__Y = slerpM10 * this._localBasisX1X + slerpM11 * this._localBasisX1Y + slerpM12 * this._localBasisX1Z;
__tmp__Z = slerpM20 * this._localBasisX1X + slerpM21 * this._localBasisX1Y + slerpM22 * this._localBasisX1Z;
this._localBasisX2X = __tmp__X;
this._localBasisX2Y = __tmp__Y;
this._localBasisX2Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = slerpM00 * this._localBasisY1X + slerpM01 * this._localBasisY1Y + slerpM02 * this._localBasisY1Z;
__tmp__Y1 = slerpM10 * this._localBasisY1X + slerpM11 * this._localBasisY1Y + slerpM12 * this._localBasisY1Z;
__tmp__Z1 = slerpM20 * this._localBasisY1X + slerpM21 * this._localBasisY1Y + slerpM22 * this._localBasisY1Z;
this._localBasisY2X = __tmp__X1;
this._localBasisY2Y = __tmp__Y1;
this._localBasisY2Z = __tmp__Z1;
let __tmp__X2;
let __tmp__Y2;
let __tmp__Z2;
__tmp__X2 = slerpM00 * this._localBasisZ1X + slerpM01 * this._localBasisZ1Y + slerpM02 * this._localBasisZ1Z;
__tmp__Y2 = slerpM10 * this._localBasisZ1X + slerpM11 * this._localBasisZ1Y + slerpM12 * this._localBasisZ1Z;
__tmp__Z2 = slerpM20 * this._localBasisZ1X + slerpM21 * this._localBasisZ1Y + slerpM22 * this._localBasisZ1Z;
this._localBasisZ2X = __tmp__X2;
this._localBasisZ2Y = __tmp__Y2;
this._localBasisZ2Z = __tmp__Z2;
}
buildLocalBasesFromXY() {
if(this._localBasisX1X * this._localBasisX1X + this._localBasisX1Y * this._localBasisX1Y + this._localBasisX1Z * this._localBasisX1Z == 0) {
this._localBasisX1X = 1;
this._localBasisX1Y = 0;
this._localBasisX1Z = 0;
} else {
let l = this._localBasisX1X * this._localBasisX1X + this._localBasisX1Y * this._localBasisX1Y + this._localBasisX1Z * this._localBasisX1Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisX1X *= l;
this._localBasisX1Y *= l;
this._localBasisX1Z *= l;
}
if(this._localBasisX2X * this._localBasisX2X + this._localBasisX2Y * this._localBasisX2Y + this._localBasisX2Z * this._localBasisX2Z == 0) {
this._localBasisX2X = 1;
this._localBasisX2Y = 0;
this._localBasisX2Z = 0;
} else {
let l = this._localBasisX2X * this._localBasisX2X + this._localBasisX2Y * this._localBasisX2Y + this._localBasisX2Z * this._localBasisX2Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisX2X *= l;
this._localBasisX2Y *= l;
this._localBasisX2Z *= l;
}
this._localBasisZ1X = this._localBasisX1Y * this._localBasisY1Z - this._localBasisX1Z * this._localBasisY1Y;
this._localBasisZ1Y = this._localBasisX1Z * this._localBasisY1X - this._localBasisX1X * this._localBasisY1Z;
this._localBasisZ1Z = this._localBasisX1X * this._localBasisY1Y - this._localBasisX1Y * this._localBasisY1X;
this._localBasisZ2X = this._localBasisX2Y * this._localBasisY2Z - this._localBasisX2Z * this._localBasisY2Y;
this._localBasisZ2Y = this._localBasisX2Z * this._localBasisY2X - this._localBasisX2X * this._localBasisY2Z;
this._localBasisZ2Z = this._localBasisX2X * this._localBasisY2Y - this._localBasisX2Y * this._localBasisY2X;
if(this._localBasisZ1X * this._localBasisZ1X + this._localBasisZ1Y * this._localBasisZ1Y + this._localBasisZ1Z * this._localBasisZ1Z == 0) {
let x1 = this._localBasisX1X;
let y1 = this._localBasisX1Y;
let z1 = this._localBasisX1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
this._localBasisY1X = 0;
this._localBasisY1Y = z1 * d;
this._localBasisY1Z = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
this._localBasisY1X = y1 * d;
this._localBasisY1Y = -x1 * d;
this._localBasisY1Z = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
this._localBasisY1X = -z1 * d;
this._localBasisY1Y = 0;
this._localBasisY1Z = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
this._localBasisY1X = y1 * d;
this._localBasisY1Y = -x1 * d;
this._localBasisY1Z = 0;
}
this._localBasisZ1X = this._localBasisX1Y * this._localBasisY1Z - this._localBasisX1Z * this._localBasisY1Y;
this._localBasisZ1Y = this._localBasisX1Z * this._localBasisY1X - this._localBasisX1X * this._localBasisY1Z;
this._localBasisZ1Z = this._localBasisX1X * this._localBasisY1Y - this._localBasisX1Y * this._localBasisY1X;
} else {
let l = this._localBasisZ1X * this._localBasisZ1X + this._localBasisZ1Y * this._localBasisZ1Y + this._localBasisZ1Z * this._localBasisZ1Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisZ1X *= l;
this._localBasisZ1Y *= l;
this._localBasisZ1Z *= l;
this._localBasisY1X = this._localBasisZ1Y * this._localBasisX1Z - this._localBasisZ1Z * this._localBasisX1Y;
this._localBasisY1Y = this._localBasisZ1Z * this._localBasisX1X - this._localBasisZ1X * this._localBasisX1Z;
this._localBasisY1Z = this._localBasisZ1X * this._localBasisX1Y - this._localBasisZ1Y * this._localBasisX1X;
}
if(this._localBasisZ2X * this._localBasisZ2X + this._localBasisZ2Y * this._localBasisZ2Y + this._localBasisZ2Z * this._localBasisZ2Z == 0) {
let x1 = this._localBasisX2X;
let y1 = this._localBasisX2Y;
let z1 = this._localBasisX2Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
this._localBasisY2X = 0;
this._localBasisY2Y = z1 * d;
this._localBasisY2Z = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
this._localBasisY2X = y1 * d;
this._localBasisY2Y = -x1 * d;
this._localBasisY2Z = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
this._localBasisY2X = -z1 * d;
this._localBasisY2Y = 0;
this._localBasisY2Z = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
this._localBasisY2X = y1 * d;
this._localBasisY2Y = -x1 * d;
this._localBasisY2Z = 0;
}
this._localBasisZ2X = this._localBasisX2Y * this._localBasisY2Z - this._localBasisX2Z * this._localBasisY2Y;
this._localBasisZ2Y = this._localBasisX2Z * this._localBasisY2X - this._localBasisX2X * this._localBasisY2Z;
this._localBasisZ2Z = this._localBasisX2X * this._localBasisY2Y - this._localBasisX2Y * this._localBasisY2X;
} else {
let l = this._localBasisZ2X * this._localBasisZ2X + this._localBasisZ2Y * this._localBasisZ2Y + this._localBasisZ2Z * this._localBasisZ2Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisZ2X *= l;
this._localBasisZ2Y *= l;
this._localBasisZ2Z *= l;
this._localBasisY2X = this._localBasisZ2Y * this._localBasisX2Z - this._localBasisZ2Z * this._localBasisX2Y;
this._localBasisY2Y = this._localBasisZ2Z * this._localBasisX2X - this._localBasisZ2X * this._localBasisX2Z;
this._localBasisY2Z = this._localBasisZ2X * this._localBasisX2Y - this._localBasisZ2Y * this._localBasisX2X;
}
}
buildLocalBasesFromX1Z2() {
if(this._localBasisX1X * this._localBasisX1X + this._localBasisX1Y * this._localBasisX1Y + this._localBasisX1Z * this._localBasisX1Z == 0) {
this._localBasisX1X = 1;
this._localBasisX1Y = 0;
this._localBasisX1Z = 0;
} else {
let l = this._localBasisX1X * this._localBasisX1X + this._localBasisX1Y * this._localBasisX1Y + this._localBasisX1Z * this._localBasisX1Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisX1X *= l;
this._localBasisX1Y *= l;
this._localBasisX1Z *= l;
}
if(this._localBasisZ2X * this._localBasisZ2X + this._localBasisZ2Y * this._localBasisZ2Y + this._localBasisZ2Z * this._localBasisZ2Z == 0) {
this._localBasisZ2X = 0;
this._localBasisZ2Y = 0;
this._localBasisZ2Z = 1;
} else {
let l = this._localBasisZ2X * this._localBasisZ2X + this._localBasisZ2Y * this._localBasisZ2Y + this._localBasisZ2Z * this._localBasisZ2Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisZ2X *= l;
this._localBasisZ2Y *= l;
this._localBasisZ2Z *= l;
}
let tf1 = this._b1._transform;
let tf2 = this._b2._transform;
let worldX1X;
let worldX1Y;
let worldX1Z;
let worldZ1X;
let worldZ1Y;
let worldZ1Z;
let worldYX;
let worldYY;
let worldYZ;
let worldX2X;
let worldX2Y;
let worldX2Z;
let worldZ2X;
let worldZ2Y;
let worldZ2Z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * this._localBasisX1X + tf1._rotation01 * this._localBasisX1Y + tf1._rotation02 * this._localBasisX1Z;
__tmp__Y = tf1._rotation10 * this._localBasisX1X + tf1._rotation11 * this._localBasisX1Y + tf1._rotation12 * this._localBasisX1Z;
__tmp__Z = tf1._rotation20 * this._localBasisX1X + tf1._rotation21 * this._localBasisX1Y + tf1._rotation22 * this._localBasisX1Z;
worldX1X = __tmp__X;
worldX1Y = __tmp__Y;
worldX1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * this._localBasisZ2X + tf2._rotation01 * this._localBasisZ2Y + tf2._rotation02 * this._localBasisZ2Z;
__tmp__Y1 = tf2._rotation10 * this._localBasisZ2X + tf2._rotation11 * this._localBasisZ2Y + tf2._rotation12 * this._localBasisZ2Z;
__tmp__Z1 = tf2._rotation20 * this._localBasisZ2X + tf2._rotation21 * this._localBasisZ2Y + tf2._rotation22 * this._localBasisZ2Z;
worldZ2X = __tmp__X1;
worldZ2Y = __tmp__Y1;
worldZ2Z = __tmp__Z1;
worldYX = worldZ2Y * worldX1Z - worldZ2Z * worldX1Y;
worldYY = worldZ2Z * worldX1X - worldZ2X * worldX1Z;
worldYZ = worldZ2X * worldX1Y - worldZ2Y * worldX1X;
if(worldYX * worldYX + worldYY * worldYY + worldYZ * worldYZ == 0) {
let x1 = worldX1X;
let y1 = worldX1Y;
let z1 = worldX1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
worldYX = 0;
worldYY = z1 * d;
worldYZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
worldYX = y1 * d;
worldYY = -x1 * d;
worldYZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
worldYX = -z1 * d;
worldYY = 0;
worldYZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
worldYX = y1 * d;
worldYY = -x1 * d;
worldYZ = 0;
}
}
worldZ1X = worldX1Y * worldYZ - worldX1Z * worldYY;
worldZ1Y = worldX1Z * worldYX - worldX1X * worldYZ;
worldZ1Z = worldX1X * worldYY - worldX1Y * worldYX;
worldX2X = worldYY * worldZ2Z - worldYZ * worldZ2Y;
worldX2Y = worldYZ * worldZ2X - worldYX * worldZ2Z;
worldX2Z = worldYX * worldZ2Y - worldYY * worldZ2X;
let __tmp__X2;
let __tmp__Y2;
let __tmp__Z2;
__tmp__X2 = tf1._rotation00 * worldX1X + tf1._rotation10 * worldX1Y + tf1._rotation20 * worldX1Z;
__tmp__Y2 = tf1._rotation01 * worldX1X + tf1._rotation11 * worldX1Y + tf1._rotation21 * worldX1Z;
__tmp__Z2 = tf1._rotation02 * worldX1X + tf1._rotation12 * worldX1Y + tf1._rotation22 * worldX1Z;
this._localBasisX1X = __tmp__X2;
this._localBasisX1Y = __tmp__Y2;
this._localBasisX1Z = __tmp__Z2;
let __tmp__X3;
let __tmp__Y3;
let __tmp__Z3;
__tmp__X3 = tf1._rotation00 * worldYX + tf1._rotation10 * worldYY + tf1._rotation20 * worldYZ;
__tmp__Y3 = tf1._rotation01 * worldYX + tf1._rotation11 * worldYY + tf1._rotation21 * worldYZ;
__tmp__Z3 = tf1._rotation02 * worldYX + tf1._rotation12 * worldYY + tf1._rotation22 * worldYZ;
this._localBasisY1X = __tmp__X3;
this._localBasisY1Y = __tmp__Y3;
this._localBasisY1Z = __tmp__Z3;
let __tmp__X4;
let __tmp__Y4;
let __tmp__Z4;
__tmp__X4 = tf1._rotation00 * worldZ1X + tf1._rotation10 * worldZ1Y + tf1._rotation20 * worldZ1Z;
__tmp__Y4 = tf1._rotation01 * worldZ1X + tf1._rotation11 * worldZ1Y + tf1._rotation21 * worldZ1Z;
__tmp__Z4 = tf1._rotation02 * worldZ1X + tf1._rotation12 * worldZ1Y + tf1._rotation22 * worldZ1Z;
this._localBasisZ1X = __tmp__X4;
this._localBasisZ1Y = __tmp__Y4;
this._localBasisZ1Z = __tmp__Z4;
let __tmp__X5;
let __tmp__Y5;
let __tmp__Z5;
__tmp__X5 = tf2._rotation00 * worldX2X + tf2._rotation10 * worldX2Y + tf2._rotation20 * worldX2Z;
__tmp__Y5 = tf2._rotation01 * worldX2X + tf2._rotation11 * worldX2Y + tf2._rotation21 * worldX2Z;
__tmp__Z5 = tf2._rotation02 * worldX2X + tf2._rotation12 * worldX2Y + tf2._rotation22 * worldX2Z;
this._localBasisX2X = __tmp__X5;
this._localBasisX2Y = __tmp__Y5;
this._localBasisX2Z = __tmp__Z5;
let __tmp__X6;
let __tmp__Y6;
let __tmp__Z6;
__tmp__X6 = tf2._rotation00 * worldYX + tf2._rotation10 * worldYY + tf2._rotation20 * worldYZ;
__tmp__Y6 = tf2._rotation01 * worldYX + tf2._rotation11 * worldYY + tf2._rotation21 * worldYZ;
__tmp__Z6 = tf2._rotation02 * worldYX + tf2._rotation12 * worldYY + tf2._rotation22 * worldYZ;
this._localBasisY2X = __tmp__X6;
this._localBasisY2Y = __tmp__Y6;
this._localBasisY2Z = __tmp__Z6;
let __tmp__X7;
let __tmp__Y7;
let __tmp__Z7;
__tmp__X7 = tf2._rotation00 * worldZ2X + tf2._rotation10 * worldZ2Y + tf2._rotation20 * worldZ2Z;
__tmp__Y7 = tf2._rotation01 * worldZ2X + tf2._rotation11 * worldZ2Y + tf2._rotation21 * worldZ2Z;
__tmp__Z7 = tf2._rotation02 * worldZ2X + tf2._rotation12 * worldZ2Y + tf2._rotation22 * worldZ2Z;
this._localBasisZ2X = __tmp__X7;
this._localBasisZ2Y = __tmp__Y7;
this._localBasisZ2Z = __tmp__Z7;
}
buildLocalBasesFromXY1X2() {
if(this._localBasisX1X * this._localBasisX1X + this._localBasisX1Y * this._localBasisX1Y + this._localBasisX1Z * this._localBasisX1Z == 0) {
this._localBasisX1X = 1;
this._localBasisX1Y = 0;
this._localBasisX1Z = 0;
} else {
let l = this._localBasisX1X * this._localBasisX1X + this._localBasisX1Y * this._localBasisX1Y + this._localBasisX1Z * this._localBasisX1Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisX1X *= l;
this._localBasisX1Y *= l;
this._localBasisX1Z *= l;
}
this._localBasisZ1X = this._localBasisX1Y * this._localBasisY1Z - this._localBasisX1Z * this._localBasisY1Y;
this._localBasisZ1Y = this._localBasisX1Z * this._localBasisY1X - this._localBasisX1X * this._localBasisY1Z;
this._localBasisZ1Z = this._localBasisX1X * this._localBasisY1Y - this._localBasisX1Y * this._localBasisY1X;
if(this._localBasisZ1X * this._localBasisZ1X + this._localBasisZ1Y * this._localBasisZ1Y + this._localBasisZ1Z * this._localBasisZ1Z == 0) {
let x1 = this._localBasisX1X;
let y1 = this._localBasisX1Y;
let z1 = this._localBasisX1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
this._localBasisY1X = 0;
this._localBasisY1Y = z1 * d;
this._localBasisY1Z = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
this._localBasisY1X = y1 * d;
this._localBasisY1Y = -x1 * d;
this._localBasisY1Z = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
this._localBasisY1X = -z1 * d;
this._localBasisY1Y = 0;
this._localBasisY1Z = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
this._localBasisY1X = y1 * d;
this._localBasisY1Y = -x1 * d;
this._localBasisY1Z = 0;
}
this._localBasisZ1X = this._localBasisX1Y * this._localBasisY1Z - this._localBasisX1Z * this._localBasisY1Y;
this._localBasisZ1Y = this._localBasisX1Z * this._localBasisY1X - this._localBasisX1X * this._localBasisY1Z;
this._localBasisZ1Z = this._localBasisX1X * this._localBasisY1Y - this._localBasisX1Y * this._localBasisY1X;
} else {
let l = this._localBasisZ1X * this._localBasisZ1X + this._localBasisZ1Y * this._localBasisZ1Y + this._localBasisZ1Z * this._localBasisZ1Z;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._localBasisZ1X *= l;
this._localBasisZ1Y *= l;
this._localBasisZ1Z *= l;
this._localBasisY1X = this._localBasisZ1Y * this._localBasisX1Z - this._localBasisZ1Z * this._localBasisX1Y;
this._localBasisY1Y = this._localBasisZ1Z * this._localBasisX1X - this._localBasisZ1X * this._localBasisX1Z;
this._localBasisY1Z = this._localBasisZ1X * this._localBasisX1Y - this._localBasisZ1Y * this._localBasisX1X;
}
let slerpQX;
let slerpQY;
let slerpQZ;
let slerpQW;
let slerpM00;
let slerpM01;
let slerpM02;
let slerpM10;
let slerpM11;
let slerpM12;
let slerpM20;
let slerpM21;
let slerpM22;
let d = this._localBasisX1X * this._localBasisX2X + this._localBasisX1Y * this._localBasisX2Y + this._localBasisX1Z * this._localBasisX2Z;
if(d < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = this._localBasisX1X;
let y1 = this._localBasisX1Y;
let z1 = this._localBasisX1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
slerpQX = vX;
slerpQY = vY;
slerpQZ = vZ;
slerpQW = 0;
} else {
let cX;
let cY;
let cZ;
cX = this._localBasisX1Y * this._localBasisX2Z - this._localBasisX1Z * this._localBasisX2Y;
cY = this._localBasisX1Z * this._localBasisX2X - this._localBasisX1X * this._localBasisX2Z;
cZ = this._localBasisX1X * this._localBasisX2Y - this._localBasisX1Y * this._localBasisX2X;
let w = Math.sqrt((1 + d) * 0.5);
d = 0.5 / w;
cX *= d;
cY *= d;
cZ *= d;
slerpQX = cX;
slerpQY = cY;
slerpQZ = cZ;
slerpQW = w;
}
let x = slerpQX;
let y = slerpQY;
let z = slerpQZ;
let w = slerpQW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
slerpM00 = 1 - yy - zz;
slerpM01 = xy - wz;
slerpM02 = xz + wy;
slerpM10 = xy + wz;
slerpM11 = 1 - xx - zz;
slerpM12 = yz - wx;
slerpM20 = xz - wy;
slerpM21 = yz + wx;
slerpM22 = 1 - xx - yy;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = slerpM00 * this._localBasisX1X + slerpM01 * this._localBasisX1Y + slerpM02 * this._localBasisX1Z;
__tmp__Y = slerpM10 * this._localBasisX1X + slerpM11 * this._localBasisX1Y + slerpM12 * this._localBasisX1Z;
__tmp__Z = slerpM20 * this._localBasisX1X + slerpM21 * this._localBasisX1Y + slerpM22 * this._localBasisX1Z;
this._localBasisX2X = __tmp__X;
this._localBasisX2Y = __tmp__Y;
this._localBasisX2Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = slerpM00 * this._localBasisY1X + slerpM01 * this._localBasisY1Y + slerpM02 * this._localBasisY1Z;
__tmp__Y1 = slerpM10 * this._localBasisY1X + slerpM11 * this._localBasisY1Y + slerpM12 * this._localBasisY1Z;
__tmp__Z1 = slerpM20 * this._localBasisY1X + slerpM21 * this._localBasisY1Y + slerpM22 * this._localBasisY1Z;
this._localBasisY2X = __tmp__X1;
this._localBasisY2Y = __tmp__Y1;
this._localBasisY2Z = __tmp__Z1;
let __tmp__X2;
let __tmp__Y2;
let __tmp__Z2;
__tmp__X2 = slerpM00 * this._localBasisZ1X + slerpM01 * this._localBasisZ1Y + slerpM02 * this._localBasisZ1Z;
__tmp__Y2 = slerpM10 * this._localBasisZ1X + slerpM11 * this._localBasisZ1Y + slerpM12 * this._localBasisZ1Z;
__tmp__Z2 = slerpM20 * this._localBasisZ1X + slerpM21 * this._localBasisZ1Y + slerpM22 * this._localBasisZ1Z;
this._localBasisZ2X = __tmp__X2;
this._localBasisZ2Y = __tmp__Y2;
this._localBasisZ2Z = __tmp__Z2;
}
setSolverInfoRowLinear(row,diff,lm,mass,sd,timeStep,isPositionPart) {
let cfmFactor;
let erp;
let slop = oimo.common.Setting.linearSlop;
if(isPositionPart) {
cfmFactor = 0;
erp = 1;
} else {
if(sd.frequency > 0) {
slop = 0;
let omega = 6.28318530717958 * sd.frequency;
let zeta = sd.dampingRatio;
if(zeta < oimo.common.Setting.minSpringDamperDampingRatio) {
zeta = oimo.common.Setting.minSpringDamperDampingRatio;
}
let h = timeStep.dt;
let c = 2 * zeta * omega;
let k = omega * omega;
if(sd.useSymplecticEuler) {
cfmFactor = 1 / (h * c);
erp = k / c;
} else {
cfmFactor = 1 / (h * (h * k + c));
erp = k / (h * k + c);
}
} else {
cfmFactor = 0;
erp = this.getErp(timeStep,false);
}
if(lm.motorForce > 0) {
row.motorSpeed = lm.motorSpeed;
row.motorMaxImpulse = lm.motorForce * timeStep.dt;
} else {
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
}
}
let lower = lm.lowerLimit;
let upper = lm.upperLimit;
let minImp;
let maxImp;
let error;
if(lower > upper) {
minImp = 0;
maxImp = 0;
error = 0;
} else if(lower == upper) {
minImp = -1e65536;
maxImp = 1e65536;
error = diff - lower;
} else if(diff < lower) {
minImp = -1e65536;
maxImp = 0;
error = diff - lower + slop;
if(error > 0) {
error = 0;
}
} else if(diff > upper) {
minImp = 0;
maxImp = 1e65536;
error = diff - upper - slop;
if(error < 0) {
error = 0;
}
} else {
minImp = 0;
maxImp = 0;
error = 0;
}
row.minImpulse = minImp;
row.maxImpulse = maxImp;
row.cfm = cfmFactor * (mass == 0 ? 0 : 1 / mass);
row.rhs = error * erp;
}
setSolverInfoRowAngular(row,diff,lm,mass,sd,timeStep,isPositionPart) {
let cfmFactor;
let erp;
let slop = oimo.common.Setting.angularSlop;
if(isPositionPart) {
cfmFactor = 0;
erp = 1;
} else {
if(sd.frequency > 0) {
slop = 0;
let omega = 6.28318530717958 * sd.frequency;
let zeta = sd.dampingRatio;
if(zeta < oimo.common.Setting.minSpringDamperDampingRatio) {
zeta = oimo.common.Setting.minSpringDamperDampingRatio;
}
let h = timeStep.dt;
let c = 2 * zeta * omega;
let k = omega * omega;
if(sd.useSymplecticEuler) {
cfmFactor = 1 / (h * c);
erp = k / c;
} else {
cfmFactor = 1 / (h * (h * k + c));
erp = k / (h * k + c);
}
} else {
cfmFactor = 0;
erp = this.getErp(timeStep,false);
}
if(lm.motorTorque > 0) {
row.motorSpeed = lm.motorSpeed;
row.motorMaxImpulse = lm.motorTorque * timeStep.dt;
} else {
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
}
}
let lower = lm.lowerLimit;
let upper = lm.upperLimit;
let mid = (lower + upper) * 0.5;
diff -= mid;
diff = ((diff + 3.14159265358979) % 6.28318530717958 + 6.28318530717958) % 6.28318530717958 - 3.14159265358979;
diff += mid;
let minImp;
let maxImp;
let error;
if(lower > upper) {
minImp = 0;
maxImp = 0;
error = 0;
} else if(lower == upper) {
minImp = -1e65536;
maxImp = 1e65536;
error = diff - lower;
} else if(diff < lower) {
minImp = -1e65536;
maxImp = 0;
error = diff - lower + slop;
if(error > 0) {
error = 0;
}
} else if(diff > upper) {
minImp = 0;
maxImp = 1e65536;
error = diff - upper - slop;
if(error < 0) {
error = 0;
}
} else {
minImp = 0;
maxImp = 0;
error = 0;
}
row.minImpulse = minImp;
row.maxImpulse = maxImp;
row.cfm = cfmFactor * (mass == 0 ? 0 : 1 / mass);
row.rhs = error * erp;
}
getErp(timeStep,isPositionPart) {
if(isPositionPart) {
return 1;
} else if(this._positionCorrectionAlgorithm == oimo.dynamics.constraint.PositionCorrectionAlgorithm.BAUMGARTE) {
return timeStep.invDt * oimo.common.Setting.velocityBaumgarte;
} else {
return 0;
}
}
computeEffectiveInertiaMoment(axisX,axisY,axisZ) {
let ia1X;
let ia1Y;
let ia1Z;
let ia2X;
let ia2Y;
let ia2Z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._b1._invInertia00 * axisX + this._b1._invInertia01 * axisY + this._b1._invInertia02 * axisZ;
__tmp__Y = this._b1._invInertia10 * axisX + this._b1._invInertia11 * axisY + this._b1._invInertia12 * axisZ;
__tmp__Z = this._b1._invInertia20 * axisX + this._b1._invInertia21 * axisY + this._b1._invInertia22 * axisZ;
ia1X = __tmp__X;
ia1Y = __tmp__Y;
ia1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = this._b2._invInertia00 * axisX + this._b2._invInertia01 * axisY + this._b2._invInertia02 * axisZ;
__tmp__Y1 = this._b2._invInertia10 * axisX + this._b2._invInertia11 * axisY + this._b2._invInertia12 * axisZ;
__tmp__Z1 = this._b2._invInertia20 * axisX + this._b2._invInertia21 * axisY + this._b2._invInertia22 * axisZ;
ia2X = __tmp__X1;
ia2Y = __tmp__Y1;
ia2Z = __tmp__Z1;
let invI1 = ia1X * axisX + ia1Y * axisY + ia1Z * axisZ;
let invI2 = ia2X * axisX + ia2Y * axisY + ia2Z * axisZ;
if(invI1 > 0) {
let dot = axisX * this._relativeAnchor1X + axisY * this._relativeAnchor1Y + axisZ * this._relativeAnchor1Z;
let projsq = this._relativeAnchor1X * this._relativeAnchor1X + this._relativeAnchor1Y * this._relativeAnchor1Y + this._relativeAnchor1Z * this._relativeAnchor1Z - dot * dot;
if(projsq > 0) {
if(this._b1._invMass > 0) {
invI1 = 1 / (1 / invI1 + this._b1._mass * projsq);
} else {
invI1 = 0;
}
}
}
if(invI2 > 0) {
let dot = axisX * this._relativeAnchor2X + axisY * this._relativeAnchor2Y + axisZ * this._relativeAnchor2Z;
let projsq = this._relativeAnchor2X * this._relativeAnchor2X + this._relativeAnchor2Y * this._relativeAnchor2Y + this._relativeAnchor2Z * this._relativeAnchor2Z - dot * dot;
if(projsq > 0) {
if(this._b2._invMass > 0) {
invI2 = 1 / (1 / invI2 + this._b2._mass * projsq);
} else {
invI2 = 0;
}
}
}
if(invI1 + invI2 == 0) {
return 0;
} else {
return 1 / (invI1 + invI2);
}
}
computeEffectiveInertiaMoment2(axis1X,axis1Y,axis1Z,axis2X,axis2Y,axis2Z) {
let ia1X;
let ia1Y;
let ia1Z;
let ia2X;
let ia2Y;
let ia2Z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._b1._invInertia00 * axis1X + this._b1._invInertia01 * axis1Y + this._b1._invInertia02 * axis1Z;
__tmp__Y = this._b1._invInertia10 * axis1X + this._b1._invInertia11 * axis1Y + this._b1._invInertia12 * axis1Z;
__tmp__Z = this._b1._invInertia20 * axis1X + this._b1._invInertia21 * axis1Y + this._b1._invInertia22 * axis1Z;
ia1X = __tmp__X;
ia1Y = __tmp__Y;
ia1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = this._b2._invInertia00 * axis2X + this._b2._invInertia01 * axis2Y + this._b2._invInertia02 * axis2Z;
__tmp__Y1 = this._b2._invInertia10 * axis2X + this._b2._invInertia11 * axis2Y + this._b2._invInertia12 * axis2Z;
__tmp__Z1 = this._b2._invInertia20 * axis2X + this._b2._invInertia21 * axis2Y + this._b2._invInertia22 * axis2Z;
ia2X = __tmp__X1;
ia2Y = __tmp__Y1;
ia2Z = __tmp__Z1;
let invI1 = ia1X * axis1X + ia1Y * axis1Y + ia1Z * axis1Z;
let invI2 = ia2X * axis2X + ia2Y * axis2Y + ia2Z * axis2Z;
if(invI1 > 0) {
let rsq = this._relativeAnchor1X * this._relativeAnchor1X + this._relativeAnchor1Y * this._relativeAnchor1Y + this._relativeAnchor1Z * this._relativeAnchor1Z;
let dot = axis1X * this._relativeAnchor1X + axis1Y * this._relativeAnchor1Y + axis1Z * this._relativeAnchor1Z;
let projsq = rsq * rsq - dot * dot;
if(projsq > 0) {
if(this._b1._invMass > 0) {
invI1 = 1 / (1 / invI1 + this._b1._mass * projsq);
} else {
invI1 = 0;
}
}
}
if(invI2 > 0) {
let rsq = this._relativeAnchor2X * this._relativeAnchor2X + this._relativeAnchor2Y * this._relativeAnchor2Y + this._relativeAnchor2Z * this._relativeAnchor2Z;
let dot = axis2X * this._relativeAnchor2X + axis2Y * this._relativeAnchor2Y + axis2Z * this._relativeAnchor2Z;
let projsq = rsq * rsq - dot * dot;
if(projsq > 0) {
if(this._b2._invMass > 0) {
invI2 = 1 / (1 / invI2 + this._b2._mass * projsq);
} else {
invI2 = 0;
}
}
}
if(invI1 + invI2 == 0) {
return 0;
} else {
return 1 / (invI1 + invI2);
}
}
_syncAnchors() {
let tf1 = this._b1._transform;
let tf2 = this._b2._transform;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * this._localAnchor1X + tf1._rotation01 * this._localAnchor1Y + tf1._rotation02 * this._localAnchor1Z;
__tmp__Y = tf1._rotation10 * this._localAnchor1X + tf1._rotation11 * this._localAnchor1Y + tf1._rotation12 * this._localAnchor1Z;
__tmp__Z = tf1._rotation20 * this._localAnchor1X + tf1._rotation21 * this._localAnchor1Y + tf1._rotation22 * this._localAnchor1Z;
this._relativeAnchor1X = __tmp__X;
this._relativeAnchor1Y = __tmp__Y;
this._relativeAnchor1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * this._localAnchor2X + tf2._rotation01 * this._localAnchor2Y + tf2._rotation02 * this._localAnchor2Z;
__tmp__Y1 = tf2._rotation10 * this._localAnchor2X + tf2._rotation11 * this._localAnchor2Y + tf2._rotation12 * this._localAnchor2Z;
__tmp__Z1 = tf2._rotation20 * this._localAnchor2X + tf2._rotation21 * this._localAnchor2Y + tf2._rotation22 * this._localAnchor2Z;
this._relativeAnchor2X = __tmp__X1;
this._relativeAnchor2Y = __tmp__Y1;
this._relativeAnchor2Z = __tmp__Z1;
this._anchor1X = this._relativeAnchor1X + tf1._positionX;
this._anchor1Y = this._relativeAnchor1Y + tf1._positionY;
this._anchor1Z = this._relativeAnchor1Z + tf1._positionZ;
this._anchor2X = this._relativeAnchor2X + tf2._positionX;
this._anchor2Y = this._relativeAnchor2Y + tf2._positionY;
this._anchor2Z = this._relativeAnchor2Z + tf2._positionZ;
let __tmp__X2;
let __tmp__Y2;
let __tmp__Z2;
__tmp__X2 = tf1._rotation00 * this._localBasisX1X + tf1._rotation01 * this._localBasisX1Y + tf1._rotation02 * this._localBasisX1Z;
__tmp__Y2 = tf1._rotation10 * this._localBasisX1X + tf1._rotation11 * this._localBasisX1Y + tf1._rotation12 * this._localBasisX1Z;
__tmp__Z2 = tf1._rotation20 * this._localBasisX1X + tf1._rotation21 * this._localBasisX1Y + tf1._rotation22 * this._localBasisX1Z;
this._basisX1X = __tmp__X2;
this._basisX1Y = __tmp__Y2;
this._basisX1Z = __tmp__Z2;
let __tmp__X3;
let __tmp__Y3;
let __tmp__Z3;
__tmp__X3 = tf1._rotation00 * this._localBasisY1X + tf1._rotation01 * this._localBasisY1Y + tf1._rotation02 * this._localBasisY1Z;
__tmp__Y3 = tf1._rotation10 * this._localBasisY1X + tf1._rotation11 * this._localBasisY1Y + tf1._rotation12 * this._localBasisY1Z;
__tmp__Z3 = tf1._rotation20 * this._localBasisY1X + tf1._rotation21 * this._localBasisY1Y + tf1._rotation22 * this._localBasisY1Z;
this._basisY1X = __tmp__X3;
this._basisY1Y = __tmp__Y3;
this._basisY1Z = __tmp__Z3;
let __tmp__X4;
let __tmp__Y4;
let __tmp__Z4;
__tmp__X4 = tf1._rotation00 * this._localBasisZ1X + tf1._rotation01 * this._localBasisZ1Y + tf1._rotation02 * this._localBasisZ1Z;
__tmp__Y4 = tf1._rotation10 * this._localBasisZ1X + tf1._rotation11 * this._localBasisZ1Y + tf1._rotation12 * this._localBasisZ1Z;
__tmp__Z4 = tf1._rotation20 * this._localBasisZ1X + tf1._rotation21 * this._localBasisZ1Y + tf1._rotation22 * this._localBasisZ1Z;
this._basisZ1X = __tmp__X4;
this._basisZ1Y = __tmp__Y4;
this._basisZ1Z = __tmp__Z4;
let __tmp__X5;
let __tmp__Y5;
let __tmp__Z5;
__tmp__X5 = tf2._rotation00 * this._localBasisX2X + tf2._rotation01 * this._localBasisX2Y + tf2._rotation02 * this._localBasisX2Z;
__tmp__Y5 = tf2._rotation10 * this._localBasisX2X + tf2._rotation11 * this._localBasisX2Y + tf2._rotation12 * this._localBasisX2Z;
__tmp__Z5 = tf2._rotation20 * this._localBasisX2X + tf2._rotation21 * this._localBasisX2Y + tf2._rotation22 * this._localBasisX2Z;
this._basisX2X = __tmp__X5;
this._basisX2Y = __tmp__Y5;
this._basisX2Z = __tmp__Z5;
let __tmp__X6;
let __tmp__Y6;
let __tmp__Z6;
__tmp__X6 = tf2._rotation00 * this._localBasisY2X + tf2._rotation01 * this._localBasisY2Y + tf2._rotation02 * this._localBasisY2Z;
__tmp__Y6 = tf2._rotation10 * this._localBasisY2X + tf2._rotation11 * this._localBasisY2Y + tf2._rotation12 * this._localBasisY2Z;
__tmp__Z6 = tf2._rotation20 * this._localBasisY2X + tf2._rotation21 * this._localBasisY2Y + tf2._rotation22 * this._localBasisY2Z;
this._basisY2X = __tmp__X6;
this._basisY2Y = __tmp__Y6;
this._basisY2Z = __tmp__Z6;
let __tmp__X7;
let __tmp__Y7;
let __tmp__Z7;
__tmp__X7 = tf2._rotation00 * this._localBasisZ2X + tf2._rotation01 * this._localBasisZ2Y + tf2._rotation02 * this._localBasisZ2Z;
__tmp__Y7 = tf2._rotation10 * this._localBasisZ2X + tf2._rotation11 * this._localBasisZ2Y + tf2._rotation12 * this._localBasisZ2Z;
__tmp__Z7 = tf2._rotation20 * this._localBasisZ2X + tf2._rotation21 * this._localBasisZ2Y + tf2._rotation22 * this._localBasisZ2Z;
this._basisZ2X = __tmp__X7;
this._basisZ2Y = __tmp__Y7;
this._basisZ2Z = __tmp__Z7;
}
_getVelocitySolverInfo(timeStep,info) {
info.b1 = this._b1;
info.b2 = this._b2;
info.numRows = 0;
}
_getPositionSolverInfo(info) {
info.b1 = this._b1;
info.b2 = this._b2;
info.numRows = 0;
}
_checkDestruction() {
let torqueSq = this._appliedTorqueX * this._appliedTorqueX + this._appliedTorqueY * this._appliedTorqueY + this._appliedTorqueZ * this._appliedTorqueZ;
if(this._breakForce > 0 && this._appliedForceX * this._appliedForceX + this._appliedForceY * this._appliedForceY + this._appliedForceZ * this._appliedForceZ > this._breakForce * this._breakForce) {
this._world.removeJoint(this);
return;
}
if(this._breakTorque > 0 && torqueSq > this._breakTorque * this._breakTorque) {
this._world.removeJoint(this);
return;
}
}
getRigidBody1() {
return this._b1;
}
getRigidBody2() {
return this._b2;
}
getType() {
return this._type;
}
getAnchor1() {
let v = new oimo.common.Vec3();
v.x = this._anchor1X;
v.y = this._anchor1Y;
v.z = this._anchor1Z;
return v;
}
getAnchor2() {
let v = new oimo.common.Vec3();
v.x = this._anchor2X;
v.y = this._anchor2Y;
v.z = this._anchor2Z;
return v;
}
getAnchor1To(anchor) {
anchor.x = this._anchor1X;
anchor.y = this._anchor1Y;
anchor.z = this._anchor1Z;
}
getAnchor2To(anchor) {
anchor.x = this._anchor2X;
anchor.y = this._anchor2Y;
anchor.z = this._anchor2Z;
}
getLocalAnchor1() {
let v = new oimo.common.Vec3();
v.x = this._localAnchor1X;
v.y = this._localAnchor1Y;
v.z = this._localAnchor1Z;
return v;
}
getLocalAnchor2() {
let v = new oimo.common.Vec3();
v.x = this._localAnchor2X;
v.y = this._localAnchor2Y;
v.z = this._localAnchor2Z;
return v;
}
getLocalAnchor1To(localAnchor) {
localAnchor.x = this._localAnchor1X;
localAnchor.y = this._localAnchor1Y;
localAnchor.z = this._localAnchor1Z;
}
getLocalAnchor2To(localAnchor) {
localAnchor.x = this._localAnchor2X;
localAnchor.y = this._localAnchor2Y;
localAnchor.z = this._localAnchor2Z;
}
getBasis1() {
let m = new oimo.common.Mat3();
let b00;
let b01;
let b02;
let b10;
let b11;
let b12;
let b20;
let b21;
let b22;
b00 = this._basisX1X;
b01 = this._basisY1X;
b02 = this._basisZ1X;
b10 = this._basisX1Y;
b11 = this._basisY1Y;
b12 = this._basisZ1Y;
b20 = this._basisX1Z;
b21 = this._basisY1Z;
b22 = this._basisZ1Z;
m.e00 = b00;
m.e01 = b01;
m.e02 = b02;
m.e10 = b10;
m.e11 = b11;
m.e12 = b12;
m.e20 = b20;
m.e21 = b21;
m.e22 = b22;
return m;
}
getBasis2() {
let m = new oimo.common.Mat3();
let b00;
let b01;
let b02;
let b10;
let b11;
let b12;
let b20;
let b21;
let b22;
b00 = this._basisX2X;
b01 = this._basisY2X;
b02 = this._basisZ2X;
b10 = this._basisX2Y;
b11 = this._basisY2Y;
b12 = this._basisZ2Y;
b20 = this._basisX2Z;
b21 = this._basisY2Z;
b22 = this._basisZ2Z;
m.e00 = b00;
m.e01 = b01;
m.e02 = b02;
m.e10 = b10;
m.e11 = b11;
m.e12 = b12;
m.e20 = b20;
m.e21 = b21;
m.e22 = b22;
return m;
}
getBasis1To(basis) {
let b00;
let b01;
let b02;
let b10;
let b11;
let b12;
let b20;
let b21;
let b22;
b00 = this._basisX1X;
b01 = this._basisY1X;
b02 = this._basisZ1X;
b10 = this._basisX1Y;
b11 = this._basisY1Y;
b12 = this._basisZ1Y;
b20 = this._basisX1Z;
b21 = this._basisY1Z;
b22 = this._basisZ1Z;
basis.e00 = b00;
basis.e01 = b01;
basis.e02 = b02;
basis.e10 = b10;
basis.e11 = b11;
basis.e12 = b12;
basis.e20 = b20;
basis.e21 = b21;
basis.e22 = b22;
}
getBasis2To(basis) {
let b00;
let b01;
let b02;
let b10;
let b11;
let b12;
let b20;
let b21;
let b22;
b00 = this._basisX2X;
b01 = this._basisY2X;
b02 = this._basisZ2X;
b10 = this._basisX2Y;
b11 = this._basisY2Y;
b12 = this._basisZ2Y;
b20 = this._basisX2Z;
b21 = this._basisY2Z;
b22 = this._basisZ2Z;
basis.e00 = b00;
basis.e01 = b01;
basis.e02 = b02;
basis.e10 = b10;
basis.e11 = b11;
basis.e12 = b12;
basis.e20 = b20;
basis.e21 = b21;
basis.e22 = b22;
}
getAllowCollision() {
return this._allowCollision;
}
setAllowCollision(allowCollision) {
this._allowCollision = allowCollision;
}
getBreakForce() {
return this._breakForce;
}
setBreakForce(breakForce) {
this._breakForce = breakForce;
}
getBreakTorque() {
return this._breakTorque;
}
setBreakTorque(breakTorque) {
this._breakTorque = breakTorque;
}
getPositionCorrectionAlgorithm() {
return this._positionCorrectionAlgorithm;
}
setPositionCorrectionAlgorithm(positionCorrectionAlgorithm) {
switch(positionCorrectionAlgorithm) {
case 0:case 1:case 2:
break;
default:
throw new Error("invalid position correction algorithm id: " + positionCorrectionAlgorithm);
}
this._positionCorrectionAlgorithm = positionCorrectionAlgorithm;
}
getAppliedForce() {
let v = new oimo.common.Vec3();
v.x = this._appliedForceX;
v.y = this._appliedForceY;
v.z = this._appliedForceZ;
return v;
}
getAppliedForceTo(appliedForce) {
appliedForce.x = this._appliedForceX;
appliedForce.y = this._appliedForceY;
appliedForce.z = this._appliedForceZ;
}
getAppliedTorque() {
let v = new oimo.common.Vec3();
v.x = this._appliedTorqueX;
v.y = this._appliedTorqueY;
v.z = this._appliedTorqueZ;
return v;
}
getAppliedTorqueTo(appliedTorque) {
appliedTorque.x = this._appliedTorqueX;
appliedTorque.y = this._appliedTorqueY;
appliedTorque.z = this._appliedTorqueZ;
}
getPrev() {
return this._prev;
}
getNext() {
return this._next;
}
}
oimo.dynamics.constraint.joint.CylindricalJoint = class oimo_dynamics_constraint_joint_CylindricalJoint extends oimo.dynamics.constraint.joint.Joint {
constructor(config) {
super(config,2);
let v = config.localAxis1;
this._localBasisX1X = v.x;
this._localBasisX1Y = v.y;
this._localBasisX1Z = v.z;
let v1 = config.localAxis2;
this._localBasisX2X = v1.x;
this._localBasisX2Y = v1.y;
this._localBasisX2Z = v1.z;
this.buildLocalBasesFromX();
this.angle = 0;
this.angularErrorY = 0;
this.angularErrorZ = 0;
this.translation = 0;
this.linearErrorY = 0;
this.linearErrorZ = 0;
this._basis = new oimo.dynamics.constraint.joint.BasisTracker(this);
this._translSd = config.translationalSpringDamper.clone();
this._translLm = config.translationalLimitMotor.clone();
this._rotSd = config.rotationalSpringDamper.clone();
this._rotLm = config.rotationalLimitMotor.clone();
}
getInfo(info,timeStep,isPositionPart) {
let erp = this.getErp(timeStep,isPositionPart);
let linRhsY = this.linearErrorY * erp;
let linRhsZ = this.linearErrorZ * erp;
let angRhsY = this.angularErrorY * erp;
let angRhsZ = this.angularErrorZ * erp;
let j;
let translationalMotorMass = 1 / (this._b1._invMass + this._b2._invMass);
let rotationalMotorMass = this.computeEffectiveInertiaMoment(this._basis.xX,this._basis.xY,this._basis.xZ);
if(this._translSd.frequency <= 0 || !isPositionPart) {
let impulse = this._impulses[0];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowLinear(row,this.translation,this._translLm,translationalMotorMass,this._translSd,timeStep,isPositionPart);
j = row.jacobian;
j.lin1X = this._basis.xX;
j.lin1Y = this._basis.xY;
j.lin1Z = this._basis.xZ;
j.lin2X = this._basis.xX;
j.lin2Y = this._basis.xY;
j.lin2Z = this._basis.xZ;
j.ang1X = this._relativeAnchor1Y * this._basis.xZ - this._relativeAnchor1Z * this._basis.xY;
j.ang1Y = this._relativeAnchor1Z * this._basis.xX - this._relativeAnchor1X * this._basis.xZ;
j.ang1Z = this._relativeAnchor1X * this._basis.xY - this._relativeAnchor1Y * this._basis.xX;
j.ang2X = this._relativeAnchor2Y * this._basis.xZ - this._relativeAnchor2Z * this._basis.xY;
j.ang2Y = this._relativeAnchor2Z * this._basis.xX - this._relativeAnchor2X * this._basis.xZ;
j.ang2Z = this._relativeAnchor2X * this._basis.xY - this._relativeAnchor2Y * this._basis.xX;
}
let impulse = this._impulses[1];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
row.rhs = linRhsY;
row.cfm = 0;
row.minImpulse = -1e65536;
row.maxImpulse = 1e65536;
j = row.jacobian;
j.lin1X = this._basis.yX;
j.lin1Y = this._basis.yY;
j.lin1Z = this._basis.yZ;
j.lin2X = this._basis.yX;
j.lin2Y = this._basis.yY;
j.lin2Z = this._basis.yZ;
j.ang1X = this._relativeAnchor1Y * this._basis.yZ - this._relativeAnchor1Z * this._basis.yY;
j.ang1Y = this._relativeAnchor1Z * this._basis.yX - this._relativeAnchor1X * this._basis.yZ;
j.ang1Z = this._relativeAnchor1X * this._basis.yY - this._relativeAnchor1Y * this._basis.yX;
j.ang2X = this._relativeAnchor2Y * this._basis.yZ - this._relativeAnchor2Z * this._basis.yY;
j.ang2Y = this._relativeAnchor2Z * this._basis.yX - this._relativeAnchor2X * this._basis.yZ;
j.ang2Z = this._relativeAnchor2X * this._basis.yY - this._relativeAnchor2Y * this._basis.yX;
let impulse1 = this._impulses[2];
let row1 = info.rows[info.numRows++];
let _this1 = row1.jacobian;
_this1.lin1X = 0;
_this1.lin1Y = 0;
_this1.lin1Z = 0;
_this1.lin2X = 0;
_this1.lin2Y = 0;
_this1.lin2Z = 0;
_this1.ang1X = 0;
_this1.ang1Y = 0;
_this1.ang1Z = 0;
_this1.ang2X = 0;
_this1.ang2Y = 0;
_this1.ang2Z = 0;
row1.rhs = 0;
row1.cfm = 0;
row1.minImpulse = 0;
row1.maxImpulse = 0;
row1.motorSpeed = 0;
row1.motorMaxImpulse = 0;
row1.impulse = null;
row1.impulse = impulse1;
row1.rhs = linRhsZ;
row1.cfm = 0;
row1.minImpulse = -1e65536;
row1.maxImpulse = 1e65536;
j = row1.jacobian;
j.lin1X = this._basis.zX;
j.lin1Y = this._basis.zY;
j.lin1Z = this._basis.zZ;
j.lin2X = this._basis.zX;
j.lin2Y = this._basis.zY;
j.lin2Z = this._basis.zZ;
j.ang1X = this._relativeAnchor1Y * this._basis.zZ - this._relativeAnchor1Z * this._basis.zY;
j.ang1Y = this._relativeAnchor1Z * this._basis.zX - this._relativeAnchor1X * this._basis.zZ;
j.ang1Z = this._relativeAnchor1X * this._basis.zY - this._relativeAnchor1Y * this._basis.zX;
j.ang2X = this._relativeAnchor2Y * this._basis.zZ - this._relativeAnchor2Z * this._basis.zY;
j.ang2Y = this._relativeAnchor2Z * this._basis.zX - this._relativeAnchor2X * this._basis.zZ;
j.ang2Z = this._relativeAnchor2X * this._basis.zY - this._relativeAnchor2Y * this._basis.zX;
if(this._rotSd.frequency <= 0 || !isPositionPart) {
let impulse = this._impulses[3];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this.angle,this._rotLm,rotationalMotorMass,this._rotSd,timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this._basis.xX;
j.ang1Y = this._basis.xY;
j.ang1Z = this._basis.xZ;
j.ang2X = this._basis.xX;
j.ang2Y = this._basis.xY;
j.ang2Z = this._basis.xZ;
}
let impulse2 = this._impulses[4];
let row2 = info.rows[info.numRows++];
let _this2 = row2.jacobian;
_this2.lin1X = 0;
_this2.lin1Y = 0;
_this2.lin1Z = 0;
_this2.lin2X = 0;
_this2.lin2Y = 0;
_this2.lin2Z = 0;
_this2.ang1X = 0;
_this2.ang1Y = 0;
_this2.ang1Z = 0;
_this2.ang2X = 0;
_this2.ang2Y = 0;
_this2.ang2Z = 0;
row2.rhs = 0;
row2.cfm = 0;
row2.minImpulse = 0;
row2.maxImpulse = 0;
row2.motorSpeed = 0;
row2.motorMaxImpulse = 0;
row2.impulse = null;
row2.impulse = impulse2;
row2.rhs = angRhsY;
row2.cfm = 0;
row2.minImpulse = -1e65536;
row2.maxImpulse = 1e65536;
j = row2.jacobian;
j.ang1X = this._basis.yX;
j.ang1Y = this._basis.yY;
j.ang1Z = this._basis.yZ;
j.ang2X = this._basis.yX;
j.ang2Y = this._basis.yY;
j.ang2Z = this._basis.yZ;
let impulse3 = this._impulses[5];
let row3 = info.rows[info.numRows++];
let _this3 = row3.jacobian;
_this3.lin1X = 0;
_this3.lin1Y = 0;
_this3.lin1Z = 0;
_this3.lin2X = 0;
_this3.lin2Y = 0;
_this3.lin2Z = 0;
_this3.ang1X = 0;
_this3.ang1Y = 0;
_this3.ang1Z = 0;
_this3.ang2X = 0;
_this3.ang2Y = 0;
_this3.ang2Z = 0;
row3.rhs = 0;
row3.cfm = 0;
row3.minImpulse = 0;
row3.maxImpulse = 0;
row3.motorSpeed = 0;
row3.motorMaxImpulse = 0;
row3.impulse = null;
row3.impulse = impulse3;
row3.rhs = angRhsZ;
row3.cfm = 0;
row3.minImpulse = -1e65536;
row3.maxImpulse = 1e65536;
j = row3.jacobian;
j.ang1X = this._basis.zX;
j.ang1Y = this._basis.zY;
j.ang1Z = this._basis.zZ;
j.ang2X = this._basis.zX;
j.ang2Y = this._basis.zY;
j.ang2Z = this._basis.zZ;
}
_syncAnchors() {
super._syncAnchors();
let _this = this._basis;
let invM1 = _this.joint._b1._invMass;
let invM2 = _this.joint._b2._invMass;
let qX;
let qY;
let qZ;
let qW;
let idQX;
let idQY;
let idQZ;
let idQW;
let slerpQX;
let slerpQY;
let slerpQZ;
let slerpQW;
let slerpM00;
let slerpM01;
let slerpM02;
let slerpM10;
let slerpM11;
let slerpM12;
let slerpM20;
let slerpM21;
let slerpM22;
let newXX;
let newXY;
let newXZ;
let newYX;
let newYY;
let newYZ;
let newZX;
let newZY;
let newZZ;
let prevXX;
let prevXY;
let prevXZ;
let prevYX;
let prevYY;
let prevYZ;
let d = _this.joint._basisX1X * _this.joint._basisX2X + _this.joint._basisX1Y * _this.joint._basisX2Y + _this.joint._basisX1Z * _this.joint._basisX2Z;
if(d < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = _this.joint._basisX1X;
let y1 = _this.joint._basisX1Y;
let z1 = _this.joint._basisX1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
qX = vX;
qY = vY;
qZ = vZ;
qW = 0;
} else {
let cX;
let cY;
let cZ;
cX = _this.joint._basisX1Y * _this.joint._basisX2Z - _this.joint._basisX1Z * _this.joint._basisX2Y;
cY = _this.joint._basisX1Z * _this.joint._basisX2X - _this.joint._basisX1X * _this.joint._basisX2Z;
cZ = _this.joint._basisX1X * _this.joint._basisX2Y - _this.joint._basisX1Y * _this.joint._basisX2X;
let w = Math.sqrt((1 + d) * 0.5);
d = 0.5 / w;
cX *= d;
cY *= d;
cZ *= d;
qX = cX;
qY = cY;
qZ = cZ;
qW = w;
}
idQX = 0;
idQY = 0;
idQZ = 0;
idQW = 1;
let q1X;
let q1Y;
let q1Z;
let q1W;
let q2X;
let q2Y;
let q2Z;
let q2W;
q1X = idQX;
q1Y = idQY;
q1Z = idQZ;
q1W = idQW;
q2X = qX;
q2Y = qY;
q2Z = qZ;
q2W = qW;
let d1 = q1X * q2X + q1Y * q2Y + q1Z * q2Z + q1W * q2W;
if(d1 < 0) {
d1 = -d1;
q2X = -q2X;
q2Y = -q2Y;
q2Z = -q2Z;
q2W = -q2W;
}
if(d1 > 0.999999) {
let dqX;
let dqY;
let dqZ;
let dqW;
dqX = q2X - q1X;
dqY = q2Y - q1Y;
dqZ = q2Z - q1Z;
dqW = q2W - q1W;
q2X = q1X + dqX * (invM1 / (invM1 + invM2));
q2Y = q1Y + dqY * (invM1 / (invM1 + invM2));
q2Z = q1Z + dqZ * (invM1 / (invM1 + invM2));
q2W = q1W + dqW * (invM1 / (invM1 + invM2));
let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
slerpQX = q2X * l;
slerpQY = q2Y * l;
slerpQZ = q2Z * l;
slerpQW = q2W * l;
} else {
let theta = invM1 / (invM1 + invM2) * Math.acos(d1);
q2X += q1X * -d1;
q2Y += q1Y * -d1;
q2Z += q1Z * -d1;
q2W += q1W * -d1;
let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
q2X *= l;
q2Y *= l;
q2Z *= l;
q2W *= l;
let sin = Math.sin(theta);
let cos = Math.cos(theta);
q1X *= cos;
q1Y *= cos;
q1Z *= cos;
q1W *= cos;
slerpQX = q1X + q2X * sin;
slerpQY = q1Y + q2Y * sin;
slerpQZ = q1Z + q2Z * sin;
slerpQW = q1W + q2W * sin;
}
let x = slerpQX;
let y = slerpQY;
let z = slerpQZ;
let w = slerpQW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
slerpM00 = 1 - yy - zz;
slerpM01 = xy - wz;
slerpM02 = xz + wy;
slerpM10 = xy + wz;
slerpM11 = 1 - xx - zz;
slerpM12 = yz - wx;
slerpM20 = xz - wy;
slerpM21 = yz + wx;
slerpM22 = 1 - xx - yy;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = slerpM00 * _this.joint._basisX1X + slerpM01 * _this.joint._basisX1Y + slerpM02 * _this.joint._basisX1Z;
__tmp__Y = slerpM10 * _this.joint._basisX1X + slerpM11 * _this.joint._basisX1Y + slerpM12 * _this.joint._basisX1Z;
__tmp__Z = slerpM20 * _this.joint._basisX1X + slerpM21 * _this.joint._basisX1Y + slerpM22 * _this.joint._basisX1Z;
newXX = __tmp__X;
newXY = __tmp__Y;
newXZ = __tmp__Z;
prevXX = _this.xX;
prevXY = _this.xY;
prevXZ = _this.xZ;
prevYX = _this.yX;
prevYY = _this.yY;
prevYZ = _this.yZ;
let d2 = prevXX * newXX + prevXY * newXY + prevXZ * newXZ;
if(d2 < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = prevXX;
let y1 = prevXY;
let z1 = prevXZ;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
slerpQX = vX;
slerpQY = vY;
slerpQZ = vZ;
slerpQW = 0;
} else {
let cX;
let cY;
let cZ;
cX = prevXY * newXZ - prevXZ * newXY;
cY = prevXZ * newXX - prevXX * newXZ;
cZ = prevXX * newXY - prevXY * newXX;
let w = Math.sqrt((1 + d2) * 0.5);
d2 = 0.5 / w;
cX *= d2;
cY *= d2;
cZ *= d2;
slerpQX = cX;
slerpQY = cY;
slerpQZ = cZ;
slerpQW = w;
}
let x1 = slerpQX;
let y1 = slerpQY;
let z1 = slerpQZ;
let w1 = slerpQW;
let x21 = 2 * x1;
let y21 = 2 * y1;
let z21 = 2 * z1;
let xx1 = x1 * x21;
let yy1 = y1 * y21;
let zz1 = z1 * z21;
let xy1 = x1 * y21;
let yz1 = y1 * z21;
let xz1 = x1 * z21;
let wx1 = w1 * x21;
let wy1 = w1 * y21;
let wz1 = w1 * z21;
slerpM00 = 1 - yy1 - zz1;
slerpM01 = xy1 - wz1;
slerpM02 = xz1 + wy1;
slerpM10 = xy1 + wz1;
slerpM11 = 1 - xx1 - zz1;
slerpM12 = yz1 - wx1;
slerpM20 = xz1 - wy1;
slerpM21 = yz1 + wx1;
slerpM22 = 1 - xx1 - yy1;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = slerpM00 * prevYX + slerpM01 * prevYY + slerpM02 * prevYZ;
__tmp__Y1 = slerpM10 * prevYX + slerpM11 * prevYY + slerpM12 * prevYZ;
__tmp__Z1 = slerpM20 * prevYX + slerpM21 * prevYY + slerpM22 * prevYZ;
newYX = __tmp__X1;
newYY = __tmp__Y1;
newYZ = __tmp__Z1;
newZX = newXY * newYZ - newXZ * newYY;
newZY = newXZ * newYX - newXX * newYZ;
newZZ = newXX * newYY - newXY * newYX;
if(newZX * newZX + newZY * newZY + newZZ * newZZ > 1e-6) {
let l = newZX * newZX + newZY * newZY + newZZ * newZZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
newZX *= l;
newZY *= l;
newZZ *= l;
} else {
let x1 = newXX;
let y1 = newXY;
let z1 = newXZ;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
newZX = 0;
newZY = z1 * d;
newZZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
newZX = y1 * d;
newZY = -x1 * d;
newZZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
newZX = -z1 * d;
newZY = 0;
newZZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
newZX = y1 * d;
newZY = -x1 * d;
newZZ = 0;
}
}
newYX = newZY * newXZ - newZZ * newXY;
newYY = newZZ * newXX - newZX * newXZ;
newYZ = newZX * newXY - newZY * newXX;
_this.xX = newXX;
_this.xY = newXY;
_this.xZ = newXZ;
_this.yX = newYX;
_this.yY = newYY;
_this.yZ = newYZ;
_this.zX = newZX;
_this.zY = newZY;
_this.zZ = newZZ;
let angErrorX;
let angErrorY;
let angErrorZ;
angErrorX = this._basisX1Y * this._basisX2Z - this._basisX1Z * this._basisX2Y;
angErrorY = this._basisX1Z * this._basisX2X - this._basisX1X * this._basisX2Z;
angErrorZ = this._basisX1X * this._basisX2Y - this._basisX1Y * this._basisX2X;
let cos = this._basisX1X * this._basisX2X + this._basisX1Y * this._basisX2Y + this._basisX1Z * this._basisX2Z;
let theta = cos <= -1 ? 3.14159265358979 : cos >= 1 ? 0 : Math.acos(cos);
let l = angErrorX * angErrorX + angErrorY * angErrorY + angErrorZ * angErrorZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
angErrorX *= l;
angErrorY *= l;
angErrorZ *= l;
angErrorX *= theta;
angErrorY *= theta;
angErrorZ *= theta;
this.angularErrorY = angErrorX * this._basis.yX + angErrorY * this._basis.yY + angErrorZ * this._basis.yZ;
this.angularErrorZ = angErrorX * this._basis.zX + angErrorY * this._basis.zY + angErrorZ * this._basis.zZ;
let perpCrossX;
let perpCrossY;
let perpCrossZ;
perpCrossX = this._basisY1Y * this._basisY2Z - this._basisY1Z * this._basisY2Y;
perpCrossY = this._basisY1Z * this._basisY2X - this._basisY1X * this._basisY2Z;
perpCrossZ = this._basisY1X * this._basisY2Y - this._basisY1Y * this._basisY2X;
cos = this._basisY1X * this._basisY2X + this._basisY1Y * this._basisY2Y + this._basisY1Z * this._basisY2Z;
this.angle = cos <= -1 ? 3.14159265358979 : cos >= 1 ? 0 : Math.acos(cos);
if(perpCrossX * this._basis.xX + perpCrossY * this._basis.xY + perpCrossZ * this._basis.xZ < 0) {
this.angle = -this.angle;
}
let anchorDiffX;
let anchorDiffY;
let anchorDiffZ;
anchorDiffX = this._anchor2X - this._anchor1X;
anchorDiffY = this._anchor2Y - this._anchor1Y;
anchorDiffZ = this._anchor2Z - this._anchor1Z;
this.translation = anchorDiffX * this._basis.xX + anchorDiffY * this._basis.xY + anchorDiffZ * this._basis.xZ;
this.linearErrorY = anchorDiffX * this._basis.yX + anchorDiffY * this._basis.yY + anchorDiffZ * this._basis.yZ;
this.linearErrorZ = anchorDiffX * this._basis.zX + anchorDiffY * this._basis.zY + anchorDiffZ * this._basis.zZ;
}
_getVelocitySolverInfo(timeStep,info) {
super._getVelocitySolverInfo(timeStep,info);
this.getInfo(info,timeStep,false);
}
_getPositionSolverInfo(info) {
super._getPositionSolverInfo(info);
this.getInfo(info,null,true);
}
getAxis1() {
let v = new oimo.common.Vec3();
v.x = this._basisX1X;
v.y = this._basisX1Y;
v.z = this._basisX1Z;
return v;
}
getAxis2() {
let v = new oimo.common.Vec3();
v.x = this._basisX2X;
v.y = this._basisX2Y;
v.z = this._basisX2Z;
return v;
}
getAxis1To(axis) {
axis.x = this._basisX1X;
axis.y = this._basisX1Y;
axis.z = this._basisX1Z;
}
getAxis2To(axis) {
axis.x = this._basisX2X;
axis.y = this._basisX2Y;
axis.z = this._basisX2Z;
}
getLocalAxis1() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX1X;
v.y = this._localBasisX1Y;
v.z = this._localBasisX1Z;
return v;
}
getLocalAxis2() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX2X;
v.y = this._localBasisX2Y;
v.z = this._localBasisX2Z;
return v;
}
getLocalAxis1To(axis) {
axis.x = this._localBasisX1X;
axis.y = this._localBasisX1Y;
axis.z = this._localBasisX1Z;
}
getLocalAxis2To(axis) {
axis.x = this._localBasisX2X;
axis.y = this._localBasisX2Y;
axis.z = this._localBasisX2Z;
}
getTranslationalSpringDamper() {
return this._translSd;
}
getRotationalSpringDamper() {
return this._rotSd;
}
getTranslationalLimitMotor() {
return this._translLm;
}
getRotationalLimitMotor() {
return this._rotLm;
}
getAngle() {
return this.angle;
}
getTranslation() {
return this.translation;
}
}
oimo.dynamics.constraint.joint.JointConfig = class oimo_dynamics_constraint_joint_JointConfig {
constructor() {
this.rigidBody1 = null;
this.rigidBody2 = null;
this.localAnchor1 = new oimo.common.Vec3();
this.localAnchor2 = new oimo.common.Vec3();
this.allowCollision = false;
this.solverType = oimo.common.Setting.defaultJointConstraintSolverType;
this.positionCorrectionAlgorithm = oimo.common.Setting.defaultJointPositionCorrectionAlgorithm;
this.breakForce = 0;
this.breakTorque = 0;
}
_init(rb1,rb2,worldAnchor) {
this.rigidBody1 = rb1;
this.rigidBody2 = rb2;
let _this = this.rigidBody1;
let localPoint = this.localAnchor1;
let vX;
let vY;
let vZ;
vX = worldAnchor.x;
vY = worldAnchor.y;
vZ = worldAnchor.z;
vX -= _this._transform._positionX;
vY -= _this._transform._positionY;
vZ -= _this._transform._positionZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = _this._transform._rotation00 * vX + _this._transform._rotation10 * vY + _this._transform._rotation20 * vZ;
__tmp__Y = _this._transform._rotation01 * vX + _this._transform._rotation11 * vY + _this._transform._rotation21 * vZ;
__tmp__Z = _this._transform._rotation02 * vX + _this._transform._rotation12 * vY + _this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
localPoint.x = vX;
localPoint.y = vY;
localPoint.z = vZ;
let _this1 = this.rigidBody2;
let localPoint1 = this.localAnchor2;
let vX1;
let vY1;
let vZ1;
vX1 = worldAnchor.x;
vY1 = worldAnchor.y;
vZ1 = worldAnchor.z;
vX1 -= _this1._transform._positionX;
vY1 -= _this1._transform._positionY;
vZ1 -= _this1._transform._positionZ;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = _this1._transform._rotation00 * vX1 + _this1._transform._rotation10 * vY1 + _this1._transform._rotation20 * vZ1;
__tmp__Y1 = _this1._transform._rotation01 * vX1 + _this1._transform._rotation11 * vY1 + _this1._transform._rotation21 * vZ1;
__tmp__Z1 = _this1._transform._rotation02 * vX1 + _this1._transform._rotation12 * vY1 + _this1._transform._rotation22 * vZ1;
vX1 = __tmp__X1;
vY1 = __tmp__Y1;
vZ1 = __tmp__Z1;
localPoint1.x = vX1;
localPoint1.y = vY1;
localPoint1.z = vZ1;
}
}
oimo.dynamics.constraint.joint.CylindricalJointConfig = class oimo_dynamics_constraint_joint_CylindricalJointConfig extends oimo.dynamics.constraint.joint.JointConfig {
constructor() {
super();
this.localAxis1 = new oimo.common.Vec3(1,0,0);
this.localAxis2 = new oimo.common.Vec3(1,0,0);
this.translationalLimitMotor = new oimo.dynamics.constraint.joint.TranslationalLimitMotor();
this.translationalSpringDamper = new oimo.dynamics.constraint.joint.SpringDamper();
this.rotationalLimitMotor = new oimo.dynamics.constraint.joint.RotationalLimitMotor();
this.rotationalSpringDamper = new oimo.dynamics.constraint.joint.SpringDamper();
}
init(rigidBody1,rigidBody2,worldAnchor,worldAxis) {
this._init(rigidBody1,rigidBody2,worldAnchor);
let localVector = this.localAxis1;
let vX;
let vY;
let vZ;
vX = worldAxis.x;
vY = worldAxis.y;
vZ = worldAxis.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = rigidBody1._transform._rotation00 * vX + rigidBody1._transform._rotation10 * vY + rigidBody1._transform._rotation20 * vZ;
__tmp__Y = rigidBody1._transform._rotation01 * vX + rigidBody1._transform._rotation11 * vY + rigidBody1._transform._rotation21 * vZ;
__tmp__Z = rigidBody1._transform._rotation02 * vX + rigidBody1._transform._rotation12 * vY + rigidBody1._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
localVector.x = vX;
localVector.y = vY;
localVector.z = vZ;
let localVector1 = this.localAxis2;
let vX1;
let vY1;
let vZ1;
vX1 = worldAxis.x;
vY1 = worldAxis.y;
vZ1 = worldAxis.z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = rigidBody2._transform._rotation00 * vX1 + rigidBody2._transform._rotation10 * vY1 + rigidBody2._transform._rotation20 * vZ1;
__tmp__Y1 = rigidBody2._transform._rotation01 * vX1 + rigidBody2._transform._rotation11 * vY1 + rigidBody2._transform._rotation21 * vZ1;
__tmp__Z1 = rigidBody2._transform._rotation02 * vX1 + rigidBody2._transform._rotation12 * vY1 + rigidBody2._transform._rotation22 * vZ1;
vX1 = __tmp__X1;
vY1 = __tmp__Y1;
vZ1 = __tmp__Z1;
localVector1.x = vX1;
localVector1.y = vY1;
localVector1.z = vZ1;
return this;
}
}
oimo.dynamics.constraint.joint.GenericJoint = class oimo_dynamics_constraint_joint_GenericJoint extends oimo.dynamics.constraint.joint.Joint {
constructor(config) {
super(config,oimo.dynamics.constraint.joint.JointType.GENERIC);
let tmp;
let _this = config.localBasis1;
if(!(_this.e00 * (_this.e11 * _this.e22 - _this.e12 * _this.e21) - _this.e01 * (_this.e10 * _this.e22 - _this.e12 * _this.e20) + _this.e02 * (_this.e10 * _this.e21 - _this.e11 * _this.e20) < 0)) {
let _this = config.localBasis2;
tmp = _this.e00 * (_this.e11 * _this.e22 - _this.e12 * _this.e21) - _this.e01 * (_this.e10 * _this.e22 - _this.e12 * _this.e20) + _this.e02 * (_this.e10 * _this.e21 - _this.e11 * _this.e20) < 0;
} else {
tmp = true;
}
if(tmp) {
console.log("src/oimo/dynamics/constraint/joint/GenericJoint.hx:50:","[warning] joint basis must be right handed");
}
let lb100;
let lb101;
let lb102;
let lb110;
let lb111;
let lb112;
let lb120;
let lb121;
let lb122;
let lb200;
let lb201;
let lb202;
let lb210;
let lb211;
let lb212;
let lb220;
let lb221;
let lb222;
let m = config.localBasis1;
lb100 = m.e00;
lb101 = m.e01;
lb102 = m.e02;
lb110 = m.e10;
lb111 = m.e11;
lb112 = m.e12;
lb120 = m.e20;
lb121 = m.e21;
lb122 = m.e22;
let m1 = config.localBasis2;
lb200 = m1.e00;
lb201 = m1.e01;
lb202 = m1.e02;
lb210 = m1.e10;
lb211 = m1.e11;
lb212 = m1.e12;
lb220 = m1.e20;
lb221 = m1.e21;
lb222 = m1.e22;
this._localBasisX1X = lb100;
this._localBasisX1Y = lb110;
this._localBasisX1Z = lb120;
this._localBasisY1X = lb101;
this._localBasisY1Y = lb111;
this._localBasisY1Z = lb121;
this._localBasisZ1X = lb102;
this._localBasisZ1Y = lb112;
this._localBasisZ1Z = lb122;
this._localBasisX2X = lb200;
this._localBasisX2Y = lb210;
this._localBasisX2Z = lb220;
this._localBasisY2X = lb201;
this._localBasisY2Y = lb211;
this._localBasisY2Z = lb221;
this._localBasisZ2X = lb202;
this._localBasisZ2Y = lb212;
this._localBasisZ2Z = lb222;
this._angleX = 0;
this._angleY = 0;
this._angleZ = 0;
this.translationX = 0;
this.translationY = 0;
this.translationZ = 0;
this.xSingular = false;
this.ySingular = false;
this.zSingular = false;
this._translLms = new Array(3);
this._translSds = new Array(3);
this._rotLms = new Array(3);
this._rotSds = new Array(3);
this._translLms[0] = config.translationalLimitMotors[0].clone();
this._translLms[1] = config.translationalLimitMotors[1].clone();
this._translLms[2] = config.translationalLimitMotors[2].clone();
this._translSds[0] = config.translationalSpringDampers[0].clone();
this._translSds[1] = config.translationalSpringDampers[1].clone();
this._translSds[2] = config.translationalSpringDampers[2].clone();
this._rotLms[0] = config.rotationalLimitMotors[0].clone();
this._rotLms[1] = config.rotationalLimitMotors[1].clone();
this._rotLms[2] = config.rotationalLimitMotors[2].clone();
this._rotSds[0] = config.rotationalSpringDampers[0].clone();
this._rotSds[1] = config.rotationalSpringDampers[1].clone();
this._rotSds[2] = config.rotationalSpringDampers[2].clone();
}
getInfo(info,timeStep,isPositionPart) {
let j;
let translMotorMass = 1 / (this._b1._invMass + this._b2._invMass);
let motorMassX = this.computeEffectiveInertiaMoment(this._axisXX,this._axisXY,this._axisXZ);
let motorMassY = this.computeEffectiveInertiaMoment(this._axisYX,this._axisYY,this._axisYZ);
let motorMassZ = this.computeEffectiveInertiaMoment(this._axisZX,this._axisZY,this._axisZZ);
if(this._translSds[0].frequency <= 0 || !isPositionPart) {
let impulse = this._impulses[0];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowLinear(row,this.translationX,this._translLms[0],translMotorMass,this._translSds[0],timeStep,isPositionPart);
j = row.jacobian;
j.lin1X = this._basisX1X;
j.lin1Y = this._basisX1Y;
j.lin1Z = this._basisX1Z;
j.lin2X = this._basisX1X;
j.lin2Y = this._basisX1Y;
j.lin2Z = this._basisX1Z;
j.ang1X = this._relativeAnchor1Y * this._basisX1Z - this._relativeAnchor1Z * this._basisX1Y;
j.ang1Y = this._relativeAnchor1Z * this._basisX1X - this._relativeAnchor1X * this._basisX1Z;
j.ang1Z = this._relativeAnchor1X * this._basisX1Y - this._relativeAnchor1Y * this._basisX1X;
j.ang2X = this._relativeAnchor2Y * this._basisX1Z - this._relativeAnchor2Z * this._basisX1Y;
j.ang2Y = this._relativeAnchor2Z * this._basisX1X - this._relativeAnchor2X * this._basisX1Z;
j.ang2Z = this._relativeAnchor2X * this._basisX1Y - this._relativeAnchor2Y * this._basisX1X;
}
if(this._translSds[1].frequency <= 0 || !isPositionPart) {
let impulse = this._impulses[1];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowLinear(row,this.translationY,this._translLms[1],translMotorMass,this._translSds[1],timeStep,isPositionPart);
j = row.jacobian;
j.lin1X = this._basisY1X;
j.lin1Y = this._basisY1Y;
j.lin1Z = this._basisY1Z;
j.lin2X = this._basisY1X;
j.lin2Y = this._basisY1Y;
j.lin2Z = this._basisY1Z;
j.ang1X = this._relativeAnchor1Y * this._basisY1Z - this._relativeAnchor1Z * this._basisY1Y;
j.ang1Y = this._relativeAnchor1Z * this._basisY1X - this._relativeAnchor1X * this._basisY1Z;
j.ang1Z = this._relativeAnchor1X * this._basisY1Y - this._relativeAnchor1Y * this._basisY1X;
j.ang2X = this._relativeAnchor2Y * this._basisY1Z - this._relativeAnchor2Z * this._basisY1Y;
j.ang2Y = this._relativeAnchor2Z * this._basisY1X - this._relativeAnchor2X * this._basisY1Z;
j.ang2Z = this._relativeAnchor2X * this._basisY1Y - this._relativeAnchor2Y * this._basisY1X;
}
if(this._translSds[2].frequency <= 0 || !isPositionPart) {
let impulse = this._impulses[2];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowLinear(row,this.translationZ,this._translLms[2],translMotorMass,this._translSds[2],timeStep,isPositionPart);
j = row.jacobian;
j.lin1X = this._basisZ1X;
j.lin1Y = this._basisZ1Y;
j.lin1Z = this._basisZ1Z;
j.lin2X = this._basisZ1X;
j.lin2Y = this._basisZ1Y;
j.lin2Z = this._basisZ1Z;
j.ang1X = this._relativeAnchor1Y * this._basisZ1Z - this._relativeAnchor1Z * this._basisZ1Y;
j.ang1Y = this._relativeAnchor1Z * this._basisZ1X - this._relativeAnchor1X * this._basisZ1Z;
j.ang1Z = this._relativeAnchor1X * this._basisZ1Y - this._relativeAnchor1Y * this._basisZ1X;
j.ang2X = this._relativeAnchor2Y * this._basisZ1Z - this._relativeAnchor2Z * this._basisZ1Y;
j.ang2Y = this._relativeAnchor2Z * this._basisZ1X - this._relativeAnchor2X * this._basisZ1Z;
j.ang2Z = this._relativeAnchor2X * this._basisZ1Y - this._relativeAnchor2Y * this._basisZ1X;
}
if(!this.xSingular && (this._rotSds[0].frequency <= 0 || !isPositionPart)) {
let impulse = this._impulses[3];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this._angleX,this._rotLms[0],motorMassX,this._rotSds[0],timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this._axisXX;
j.ang1Y = this._axisXY;
j.ang1Z = this._axisXZ;
j.ang2X = this._axisXX;
j.ang2Y = this._axisXY;
j.ang2Z = this._axisXZ;
}
if(!this.ySingular && (this._rotSds[1].frequency <= 0 || !isPositionPart)) {
let impulse = this._impulses[4];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this._angleY,this._rotLms[1],motorMassY,this._rotSds[1],timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this._axisYX;
j.ang1Y = this._axisYY;
j.ang1Z = this._axisYZ;
j.ang2X = this._axisYX;
j.ang2Y = this._axisYY;
j.ang2Z = this._axisYZ;
}
if(!this.zSingular && (this._rotSds[2].frequency <= 0 || !isPositionPart)) {
let impulse = this._impulses[5];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this._angleZ,this._rotLms[2],motorMassZ,this._rotSds[2],timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this._axisZX;
j.ang1Y = this._axisZY;
j.ang1Z = this._axisZZ;
j.ang2X = this._axisZX;
j.ang2Y = this._axisZY;
j.ang2Z = this._axisZZ;
}
}
_syncAnchors() {
super._syncAnchors();
let angleAxisXX;
let angleAxisXY;
let angleAxisXZ;
let angleAxisYX;
let angleAxisYY;
let angleAxisYZ;
let angleAxisZX;
let angleAxisZY;
let angleAxisZZ;
angleAxisXX = this._basisX1X;
angleAxisXY = this._basisX1Y;
angleAxisXZ = this._basisX1Z;
angleAxisZX = this._basisZ2X;
angleAxisZY = this._basisZ2Y;
angleAxisZZ = this._basisZ2Z;
angleAxisYX = angleAxisZY * angleAxisXZ - angleAxisZZ * angleAxisXY;
angleAxisYY = angleAxisZZ * angleAxisXX - angleAxisZX * angleAxisXZ;
angleAxisYZ = angleAxisZX * angleAxisXY - angleAxisZY * angleAxisXX;
this._axisXX = angleAxisYY * angleAxisZZ - angleAxisYZ * angleAxisZY;
this._axisXY = angleAxisYZ * angleAxisZX - angleAxisYX * angleAxisZZ;
this._axisXZ = angleAxisYX * angleAxisZY - angleAxisYY * angleAxisZX;
this._axisYX = angleAxisYX;
this._axisYY = angleAxisYY;
this._axisYZ = angleAxisYZ;
this._axisZX = angleAxisXY * angleAxisYZ - angleAxisXZ * angleAxisYY;
this._axisZY = angleAxisXZ * angleAxisYX - angleAxisXX * angleAxisYZ;
this._axisZZ = angleAxisXX * angleAxisYY - angleAxisXY * angleAxisYX;
let l = this._axisXX * this._axisXX + this._axisXY * this._axisXY + this._axisXZ * this._axisXZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._axisXX *= l;
this._axisXY *= l;
this._axisXZ *= l;
let l1 = this._axisYX * this._axisYX + this._axisYY * this._axisYY + this._axisYZ * this._axisYZ;
if(l1 > 0) {
l1 = 1 / Math.sqrt(l1);
}
this._axisYX *= l1;
this._axisYY *= l1;
this._axisYZ *= l1;
let l2 = this._axisZX * this._axisZX + this._axisZY * this._axisZY + this._axisZZ * this._axisZZ;
if(l2 > 0) {
l2 = 1 / Math.sqrt(l2);
}
this._axisZX *= l2;
this._axisZY *= l2;
this._axisZZ *= l2;
this.xSingular = this._axisXX * this._axisXX + this._axisXY * this._axisXY + this._axisXZ * this._axisXZ == 0;
this.ySingular = this._axisYX * this._axisYX + this._axisYY * this._axisYY + this._axisYZ * this._axisYZ == 0;
this.zSingular = this._axisZX * this._axisZX + this._axisZY * this._axisZY + this._axisZZ * this._axisZZ == 0;
let rot100;
let rot101;
let rot102;
let rot110;
let rot111;
let rot112;
let rot120;
let rot121;
let rot122;
let rot200;
let rot201;
let rot202;
let rot210;
let rot211;
let rot212;
let rot220;
let rot221;
let rot222;
rot100 = this._basisX1X;
rot101 = this._basisY1X;
rot102 = this._basisZ1X;
rot110 = this._basisX1Y;
rot111 = this._basisY1Y;
rot112 = this._basisZ1Y;
rot120 = this._basisX1Z;
rot121 = this._basisY1Z;
rot122 = this._basisZ1Z;
rot200 = this._basisX2X;
rot201 = this._basisY2X;
rot202 = this._basisZ2X;
rot210 = this._basisX2Y;
rot211 = this._basisY2Y;
rot212 = this._basisZ2Y;
rot220 = this._basisX2Z;
rot221 = this._basisY2Z;
rot222 = this._basisZ2Z;
let relRot00;
let relRot01;
let relRot02;
let relRot11;
let relRot12;
let relRot21;
let relRot22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__11;
let __tmp__12;
let __tmp__21;
let __tmp__22;
__tmp__00 = rot100 * rot200 + rot110 * rot210 + rot120 * rot220;
__tmp__01 = rot100 * rot201 + rot110 * rot211 + rot120 * rot221;
__tmp__02 = rot100 * rot202 + rot110 * rot212 + rot120 * rot222;
__tmp__11 = rot101 * rot201 + rot111 * rot211 + rot121 * rot221;
__tmp__12 = rot101 * rot202 + rot111 * rot212 + rot121 * rot222;
__tmp__21 = rot102 * rot201 + rot112 * rot211 + rot122 * rot221;
__tmp__22 = rot102 * rot202 + rot112 * rot212 + rot122 * rot222;
relRot00 = __tmp__00;
relRot01 = __tmp__01;
relRot02 = __tmp__02;
relRot11 = __tmp__11;
relRot12 = __tmp__12;
relRot21 = __tmp__21;
relRot22 = __tmp__22;
let anglesX;
let anglesY;
let anglesZ;
let sy = relRot02;
if(sy <= -1) {
let xSubZ = Math.atan2(relRot21,relRot11);
anglesX = xSubZ * 0.5;
anglesY = -1.570796326794895;
anglesZ = -xSubZ * 0.5;
} else if(sy >= 1) {
let xAddZ = Math.atan2(relRot21,relRot11);
anglesX = xAddZ * 0.5;
anglesY = 1.570796326794895;
anglesZ = xAddZ * 0.5;
} else {
anglesX = Math.atan2(-relRot12,relRot22);
anglesY = Math.asin(sy);
anglesZ = Math.atan2(-relRot01,relRot00);
}
this._angleX = anglesX;
this._angleY = anglesY;
this._angleZ = anglesZ;
let anchorDiffX;
let anchorDiffY;
let anchorDiffZ;
anchorDiffX = this._anchor2X - this._anchor1X;
anchorDiffY = this._anchor2Y - this._anchor1Y;
anchorDiffZ = this._anchor2Z - this._anchor1Z;
this.translationX = anchorDiffX * this._basisX1X + anchorDiffY * this._basisX1Y + anchorDiffZ * this._basisX1Z;
this.translationY = anchorDiffX * this._basisY1X + anchorDiffY * this._basisY1Y + anchorDiffZ * this._basisY1Z;
this.translationZ = anchorDiffX * this._basisZ1X + anchorDiffY * this._basisZ1Y + anchorDiffZ * this._basisZ1Z;
}
_getVelocitySolverInfo(timeStep,info) {
super._getVelocitySolverInfo(timeStep,info);
this.getInfo(info,timeStep,false);
}
_getPositionSolverInfo(info) {
super._getPositionSolverInfo(info);
this.getInfo(info,null,true);
}
getAxisX() {
let v = new oimo.common.Vec3();
v.x = this._basisX1X;
v.y = this._basisX1Y;
v.z = this._basisX1Z;
return v;
}
getAxisY() {
let v = new oimo.common.Vec3();
v.x = this._axisYX;
v.y = this._axisYY;
v.z = this._axisYZ;
return v;
}
getAxisZ() {
let v = new oimo.common.Vec3();
v.x = this._basisZ2X;
v.y = this._basisZ2Y;
v.z = this._basisZ2Z;
return v;
}
getTranslationalSpringDampers() {
return this._translSds.slice(0);
}
getRotationalSpringDampers() {
return this._translSds.slice(0);
}
getTranslationalLimitMotors() {
return this._translLms.slice(0);
}
getRotationalLimitMotors() {
return this._rotLms.slice(0);
}
getAngles() {
return new oimo.common.Vec3(this._angleX,this._angleY,this._angleZ);
}
getTranslations() {
return new oimo.common.Vec3(this.translationX,this.translationY,this.translationZ);
}
}
oimo.dynamics.constraint.joint.GenericJointConfig = class oimo_dynamics_constraint_joint_GenericJointConfig extends oimo.dynamics.constraint.joint.JointConfig {
constructor() {
super();
this.localBasis1 = new oimo.common.Mat3();
this.localBasis2 = new oimo.common.Mat3();
let _g = [];
_g.push(new oimo.dynamics.constraint.joint.TranslationalLimitMotor().setLimits(0,0));
_g.push(new oimo.dynamics.constraint.joint.TranslationalLimitMotor().setLimits(0,0));
_g.push(new oimo.dynamics.constraint.joint.TranslationalLimitMotor().setLimits(0,0));
this.translationalLimitMotors = _g;
let _g1 = [];
_g1.push(new oimo.dynamics.constraint.joint.RotationalLimitMotor().setLimits(0,0));
_g1.push(new oimo.dynamics.constraint.joint.RotationalLimitMotor().setLimits(0,0));
_g1.push(new oimo.dynamics.constraint.joint.RotationalLimitMotor().setLimits(0,0));
this.rotationalLimitMotors = _g1;
this.translationalSpringDampers = [new oimo.dynamics.constraint.joint.SpringDamper(),new oimo.dynamics.constraint.joint.SpringDamper(),new oimo.dynamics.constraint.joint.SpringDamper()];
this.rotationalSpringDampers = [new oimo.dynamics.constraint.joint.SpringDamper(),new oimo.dynamics.constraint.joint.SpringDamper(),new oimo.dynamics.constraint.joint.SpringDamper()];
}
init(rigidBody1,rigidBody2,worldAnchor,worldBasis1,worldBasis2) {
this._init(rigidBody1,rigidBody2,worldAnchor);
let tf1 = rigidBody1._transform;
let tf2 = rigidBody2._transform;
let wb100;
let wb101;
let wb102;
let wb110;
let wb111;
let wb112;
let wb120;
let wb121;
let wb122;
let wb200;
let wb201;
let wb202;
let wb210;
let wb211;
let wb212;
let wb220;
let wb221;
let wb222;
let lb100;
let lb101;
let lb102;
let lb110;
let lb111;
let lb112;
let lb120;
let lb121;
let lb122;
let lb200;
let lb201;
let lb202;
let lb210;
let lb211;
let lb212;
let lb220;
let lb221;
let lb222;
wb100 = worldBasis1.e00;
wb101 = worldBasis1.e01;
wb102 = worldBasis1.e02;
wb110 = worldBasis1.e10;
wb111 = worldBasis1.e11;
wb112 = worldBasis1.e12;
wb120 = worldBasis1.e20;
wb121 = worldBasis1.e21;
wb122 = worldBasis1.e22;
wb200 = worldBasis2.e00;
wb201 = worldBasis2.e01;
wb202 = worldBasis2.e02;
wb210 = worldBasis2.e10;
wb211 = worldBasis2.e11;
wb212 = worldBasis2.e12;
wb220 = worldBasis2.e20;
wb221 = worldBasis2.e21;
wb222 = worldBasis2.e22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * wb100 + tf1._rotation10 * wb110 + tf1._rotation20 * wb120;
__tmp__01 = tf1._rotation00 * wb101 + tf1._rotation10 * wb111 + tf1._rotation20 * wb121;
__tmp__02 = tf1._rotation00 * wb102 + tf1._rotation10 * wb112 + tf1._rotation20 * wb122;
__tmp__10 = tf1._rotation01 * wb100 + tf1._rotation11 * wb110 + tf1._rotation21 * wb120;
__tmp__11 = tf1._rotation01 * wb101 + tf1._rotation11 * wb111 + tf1._rotation21 * wb121;
__tmp__12 = tf1._rotation01 * wb102 + tf1._rotation11 * wb112 + tf1._rotation21 * wb122;
__tmp__20 = tf1._rotation02 * wb100 + tf1._rotation12 * wb110 + tf1._rotation22 * wb120;
__tmp__21 = tf1._rotation02 * wb101 + tf1._rotation12 * wb111 + tf1._rotation22 * wb121;
__tmp__22 = tf1._rotation02 * wb102 + tf1._rotation12 * wb112 + tf1._rotation22 * wb122;
lb100 = __tmp__00;
lb101 = __tmp__01;
lb102 = __tmp__02;
lb110 = __tmp__10;
lb111 = __tmp__11;
lb112 = __tmp__12;
lb120 = __tmp__20;
lb121 = __tmp__21;
lb122 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * wb200 + tf2._rotation10 * wb210 + tf2._rotation20 * wb220;
__tmp__011 = tf2._rotation00 * wb201 + tf2._rotation10 * wb211 + tf2._rotation20 * wb221;
__tmp__021 = tf2._rotation00 * wb202 + tf2._rotation10 * wb212 + tf2._rotation20 * wb222;
__tmp__101 = tf2._rotation01 * wb200 + tf2._rotation11 * wb210 + tf2._rotation21 * wb220;
__tmp__111 = tf2._rotation01 * wb201 + tf2._rotation11 * wb211 + tf2._rotation21 * wb221;
__tmp__121 = tf2._rotation01 * wb202 + tf2._rotation11 * wb212 + tf2._rotation21 * wb222;
__tmp__201 = tf2._rotation02 * wb200 + tf2._rotation12 * wb210 + tf2._rotation22 * wb220;
__tmp__211 = tf2._rotation02 * wb201 + tf2._rotation12 * wb211 + tf2._rotation22 * wb221;
__tmp__221 = tf2._rotation02 * wb202 + tf2._rotation12 * wb212 + tf2._rotation22 * wb222;
lb200 = __tmp__001;
lb201 = __tmp__011;
lb202 = __tmp__021;
lb210 = __tmp__101;
lb211 = __tmp__111;
lb212 = __tmp__121;
lb220 = __tmp__201;
lb221 = __tmp__211;
lb222 = __tmp__221;
let m = this.localBasis1;
m.e00 = lb100;
m.e01 = lb101;
m.e02 = lb102;
m.e10 = lb110;
m.e11 = lb111;
m.e12 = lb112;
m.e20 = lb120;
m.e21 = lb121;
m.e22 = lb122;
let m1 = this.localBasis2;
m1.e00 = lb200;
m1.e01 = lb201;
m1.e02 = lb202;
m1.e10 = lb210;
m1.e11 = lb211;
m1.e12 = lb212;
m1.e20 = lb220;
m1.e21 = lb221;
m1.e22 = lb222;
return this;
}
}
oimo.dynamics.constraint.joint.JointImpulse = class oimo_dynamics_constraint_joint_JointImpulse {
constructor() {
this.impulse = 0;
this.impulseM = 0;
this.impulseP = 0;
}
}
oimo.dynamics.constraint.joint.JointLink = class oimo_dynamics_constraint_joint_JointLink {
constructor(joint) {
this._joint = joint;
}
getContact() {
return this._joint;
}
getOther() {
return this._other;
}
getPrev() {
return this._prev;
}
getNext() {
return this._next;
}
}
oimo.dynamics.constraint.joint.JointMacro = class oimo_dynamics_constraint_joint_JointMacro {
}
oimo.dynamics.constraint.joint.JointType = class oimo_dynamics_constraint_joint_JointType {
}
oimo.dynamics.constraint.joint.PrismaticJoint = class oimo_dynamics_constraint_joint_PrismaticJoint extends oimo.dynamics.constraint.joint.Joint {
constructor(config) {
super(config,oimo.dynamics.constraint.joint.JointType.PRISMATIC);
let v = config.localAxis1;
this._localBasisX1X = v.x;
this._localBasisX1Y = v.y;
this._localBasisX1Z = v.z;
let v1 = config.localAxis2;
this._localBasisX2X = v1.x;
this._localBasisX2Y = v1.y;
this._localBasisX2Z = v1.z;
this.buildLocalBasesFromX();
this._basis = new oimo.dynamics.constraint.joint.BasisTracker(this);
this.translation = 0;
this.linearErrorY = 0;
this.linearErrorZ = 0;
this.angularErrorX = 0;
this.angularErrorY = 0;
this.angularErrorZ = 0;
this._sd = config.springDamper.clone();
this._lm = config.limitMotor.clone();
}
getInfo(info,timeStep,isPositionPart) {
let erp = this.getErp(timeStep,isPositionPart);
let linRhsY = this.linearErrorY * erp;
let linRhsZ = this.linearErrorZ * erp;
let angRhsX = this.angularErrorX * erp;
let angRhsY = this.angularErrorY * erp;
let angRhsZ = this.angularErrorZ * erp;
let j;
if(this._sd.frequency <= 0 || !isPositionPart) {
let impulse = this._impulses[0];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowLinear(row,this.translation,this._lm,1 / (this._b1._invMass + this._b2._invMass),this._sd,timeStep,isPositionPart);
j = row.jacobian;
j.lin1X = this._basis.xX;
j.lin1Y = this._basis.xY;
j.lin1Z = this._basis.xZ;
j.lin2X = this._basis.xX;
j.lin2Y = this._basis.xY;
j.lin2Z = this._basis.xZ;
j.ang1X = this._relativeAnchor1Y * this._basis.xZ - this._relativeAnchor1Z * this._basis.xY;
j.ang1Y = this._relativeAnchor1Z * this._basis.xX - this._relativeAnchor1X * this._basis.xZ;
j.ang1Z = this._relativeAnchor1X * this._basis.xY - this._relativeAnchor1Y * this._basis.xX;
j.ang2X = this._relativeAnchor2Y * this._basis.xZ - this._relativeAnchor2Z * this._basis.xY;
j.ang2Y = this._relativeAnchor2Z * this._basis.xX - this._relativeAnchor2X * this._basis.xZ;
j.ang2Z = this._relativeAnchor2X * this._basis.xY - this._relativeAnchor2Y * this._basis.xX;
}
let impulse = this._impulses[1];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
row.rhs = linRhsY;
row.cfm = 0;
row.minImpulse = -1e65536;
row.maxImpulse = 1e65536;
j = row.jacobian;
j.lin1X = this._basis.yX;
j.lin1Y = this._basis.yY;
j.lin1Z = this._basis.yZ;
j.lin2X = this._basis.yX;
j.lin2Y = this._basis.yY;
j.lin2Z = this._basis.yZ;
j.ang1X = this._relativeAnchor1Y * this._basis.yZ - this._relativeAnchor1Z * this._basis.yY;
j.ang1Y = this._relativeAnchor1Z * this._basis.yX - this._relativeAnchor1X * this._basis.yZ;
j.ang1Z = this._relativeAnchor1X * this._basis.yY - this._relativeAnchor1Y * this._basis.yX;
j.ang2X = this._relativeAnchor2Y * this._basis.yZ - this._relativeAnchor2Z * this._basis.yY;
j.ang2Y = this._relativeAnchor2Z * this._basis.yX - this._relativeAnchor2X * this._basis.yZ;
j.ang2Z = this._relativeAnchor2X * this._basis.yY - this._relativeAnchor2Y * this._basis.yX;
let impulse1 = this._impulses[2];
let row1 = info.rows[info.numRows++];
let _this1 = row1.jacobian;
_this1.lin1X = 0;
_this1.lin1Y = 0;
_this1.lin1Z = 0;
_this1.lin2X = 0;
_this1.lin2Y = 0;
_this1.lin2Z = 0;
_this1.ang1X = 0;
_this1.ang1Y = 0;
_this1.ang1Z = 0;
_this1.ang2X = 0;
_this1.ang2Y = 0;
_this1.ang2Z = 0;
row1.rhs = 0;
row1.cfm = 0;
row1.minImpulse = 0;
row1.maxImpulse = 0;
row1.motorSpeed = 0;
row1.motorMaxImpulse = 0;
row1.impulse = null;
row1.impulse = impulse1;
row1.rhs = linRhsZ;
row1.cfm = 0;
row1.minImpulse = -1e65536;
row1.maxImpulse = 1e65536;
j = row1.jacobian;
j.lin1X = this._basis.zX;
j.lin1Y = this._basis.zY;
j.lin1Z = this._basis.zZ;
j.lin2X = this._basis.zX;
j.lin2Y = this._basis.zY;
j.lin2Z = this._basis.zZ;
j.ang1X = this._relativeAnchor1Y * this._basis.zZ - this._relativeAnchor1Z * this._basis.zY;
j.ang1Y = this._relativeAnchor1Z * this._basis.zX - this._relativeAnchor1X * this._basis.zZ;
j.ang1Z = this._relativeAnchor1X * this._basis.zY - this._relativeAnchor1Y * this._basis.zX;
j.ang2X = this._relativeAnchor2Y * this._basis.zZ - this._relativeAnchor2Z * this._basis.zY;
j.ang2Y = this._relativeAnchor2Z * this._basis.zX - this._relativeAnchor2X * this._basis.zZ;
j.ang2Z = this._relativeAnchor2X * this._basis.zY - this._relativeAnchor2Y * this._basis.zX;
let impulse2 = this._impulses[3];
let row2 = info.rows[info.numRows++];
let _this2 = row2.jacobian;
_this2.lin1X = 0;
_this2.lin1Y = 0;
_this2.lin1Z = 0;
_this2.lin2X = 0;
_this2.lin2Y = 0;
_this2.lin2Z = 0;
_this2.ang1X = 0;
_this2.ang1Y = 0;
_this2.ang1Z = 0;
_this2.ang2X = 0;
_this2.ang2Y = 0;
_this2.ang2Z = 0;
row2.rhs = 0;
row2.cfm = 0;
row2.minImpulse = 0;
row2.maxImpulse = 0;
row2.motorSpeed = 0;
row2.motorMaxImpulse = 0;
row2.impulse = null;
row2.impulse = impulse2;
row2.rhs = angRhsX;
row2.cfm = 0;
row2.minImpulse = -1e65536;
row2.maxImpulse = 1e65536;
j = row2.jacobian;
j.ang1X = 1;
j.ang1Y = 0;
j.ang1Z = 0;
j.ang2X = 1;
j.ang2Y = 0;
j.ang2Z = 0;
let impulse3 = this._impulses[4];
let row3 = info.rows[info.numRows++];
let _this3 = row3.jacobian;
_this3.lin1X = 0;
_this3.lin1Y = 0;
_this3.lin1Z = 0;
_this3.lin2X = 0;
_this3.lin2Y = 0;
_this3.lin2Z = 0;
_this3.ang1X = 0;
_this3.ang1Y = 0;
_this3.ang1Z = 0;
_this3.ang2X = 0;
_this3.ang2Y = 0;
_this3.ang2Z = 0;
row3.rhs = 0;
row3.cfm = 0;
row3.minImpulse = 0;
row3.maxImpulse = 0;
row3.motorSpeed = 0;
row3.motorMaxImpulse = 0;
row3.impulse = null;
row3.impulse = impulse3;
row3.rhs = angRhsY;
row3.cfm = 0;
row3.minImpulse = -1e65536;
row3.maxImpulse = 1e65536;
j = row3.jacobian;
j.ang1X = 0;
j.ang1Y = 1;
j.ang1Z = 0;
j.ang2X = 0;
j.ang2Y = 1;
j.ang2Z = 0;
let impulse4 = this._impulses[5];
let row4 = info.rows[info.numRows++];
let _this4 = row4.jacobian;
_this4.lin1X = 0;
_this4.lin1Y = 0;
_this4.lin1Z = 0;
_this4.lin2X = 0;
_this4.lin2Y = 0;
_this4.lin2Z = 0;
_this4.ang1X = 0;
_this4.ang1Y = 0;
_this4.ang1Z = 0;
_this4.ang2X = 0;
_this4.ang2Y = 0;
_this4.ang2Z = 0;
row4.rhs = 0;
row4.cfm = 0;
row4.minImpulse = 0;
row4.maxImpulse = 0;
row4.motorSpeed = 0;
row4.motorMaxImpulse = 0;
row4.impulse = null;
row4.impulse = impulse4;
row4.rhs = angRhsZ;
row4.cfm = 0;
row4.minImpulse = -1e65536;
row4.maxImpulse = 1e65536;
j = row4.jacobian;
j.ang1X = 0;
j.ang1Y = 0;
j.ang1Z = 1;
j.ang2X = 0;
j.ang2Y = 0;
j.ang2Z = 1;
}
_syncAnchors() {
super._syncAnchors();
let _this = this._basis;
let invM1 = _this.joint._b1._invMass;
let invM2 = _this.joint._b2._invMass;
let qX;
let qY;
let qZ;
let qW;
let idQX;
let idQY;
let idQZ;
let idQW;
let slerpQX;
let slerpQY;
let slerpQZ;
let slerpQW;
let slerpM00;
let slerpM01;
let slerpM02;
let slerpM10;
let slerpM11;
let slerpM12;
let slerpM20;
let slerpM21;
let slerpM22;
let newXX;
let newXY;
let newXZ;
let newYX;
let newYY;
let newYZ;
let newZX;
let newZY;
let newZZ;
let prevXX;
let prevXY;
let prevXZ;
let prevYX;
let prevYY;
let prevYZ;
let d = _this.joint._basisX1X * _this.joint._basisX2X + _this.joint._basisX1Y * _this.joint._basisX2Y + _this.joint._basisX1Z * _this.joint._basisX2Z;
if(d < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = _this.joint._basisX1X;
let y1 = _this.joint._basisX1Y;
let z1 = _this.joint._basisX1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
qX = vX;
qY = vY;
qZ = vZ;
qW = 0;
} else {
let cX;
let cY;
let cZ;
cX = _this.joint._basisX1Y * _this.joint._basisX2Z - _this.joint._basisX1Z * _this.joint._basisX2Y;
cY = _this.joint._basisX1Z * _this.joint._basisX2X - _this.joint._basisX1X * _this.joint._basisX2Z;
cZ = _this.joint._basisX1X * _this.joint._basisX2Y - _this.joint._basisX1Y * _this.joint._basisX2X;
let w = Math.sqrt((1 + d) * 0.5);
d = 0.5 / w;
cX *= d;
cY *= d;
cZ *= d;
qX = cX;
qY = cY;
qZ = cZ;
qW = w;
}
idQX = 0;
idQY = 0;
idQZ = 0;
idQW = 1;
let q1X;
let q1Y;
let q1Z;
let q1W;
let q2X;
let q2Y;
let q2Z;
let q2W;
q1X = idQX;
q1Y = idQY;
q1Z = idQZ;
q1W = idQW;
q2X = qX;
q2Y = qY;
q2Z = qZ;
q2W = qW;
let d1 = q1X * q2X + q1Y * q2Y + q1Z * q2Z + q1W * q2W;
if(d1 < 0) {
d1 = -d1;
q2X = -q2X;
q2Y = -q2Y;
q2Z = -q2Z;
q2W = -q2W;
}
if(d1 > 0.999999) {
let dqX;
let dqY;
let dqZ;
let dqW;
dqX = q2X - q1X;
dqY = q2Y - q1Y;
dqZ = q2Z - q1Z;
dqW = q2W - q1W;
q2X = q1X + dqX * (invM1 / (invM1 + invM2));
q2Y = q1Y + dqY * (invM1 / (invM1 + invM2));
q2Z = q1Z + dqZ * (invM1 / (invM1 + invM2));
q2W = q1W + dqW * (invM1 / (invM1 + invM2));
let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
slerpQX = q2X * l;
slerpQY = q2Y * l;
slerpQZ = q2Z * l;
slerpQW = q2W * l;
} else {
let theta = invM1 / (invM1 + invM2) * Math.acos(d1);
q2X += q1X * -d1;
q2Y += q1Y * -d1;
q2Z += q1Z * -d1;
q2W += q1W * -d1;
let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
q2X *= l;
q2Y *= l;
q2Z *= l;
q2W *= l;
let sin = Math.sin(theta);
let cos = Math.cos(theta);
q1X *= cos;
q1Y *= cos;
q1Z *= cos;
q1W *= cos;
slerpQX = q1X + q2X * sin;
slerpQY = q1Y + q2Y * sin;
slerpQZ = q1Z + q2Z * sin;
slerpQW = q1W + q2W * sin;
}
let x = slerpQX;
let y = slerpQY;
let z = slerpQZ;
let w = slerpQW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
slerpM00 = 1 - yy - zz;
slerpM01 = xy - wz;
slerpM02 = xz + wy;
slerpM10 = xy + wz;
slerpM11 = 1 - xx - zz;
slerpM12 = yz - wx;
slerpM20 = xz - wy;
slerpM21 = yz + wx;
slerpM22 = 1 - xx - yy;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = slerpM00 * _this.joint._basisX1X + slerpM01 * _this.joint._basisX1Y + slerpM02 * _this.joint._basisX1Z;
__tmp__Y = slerpM10 * _this.joint._basisX1X + slerpM11 * _this.joint._basisX1Y + slerpM12 * _this.joint._basisX1Z;
__tmp__Z = slerpM20 * _this.joint._basisX1X + slerpM21 * _this.joint._basisX1Y + slerpM22 * _this.joint._basisX1Z;
newXX = __tmp__X;
newXY = __tmp__Y;
newXZ = __tmp__Z;
prevXX = _this.xX;
prevXY = _this.xY;
prevXZ = _this.xZ;
prevYX = _this.yX;
prevYY = _this.yY;
prevYZ = _this.yZ;
let d2 = prevXX * newXX + prevXY * newXY + prevXZ * newXZ;
if(d2 < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = prevXX;
let y1 = prevXY;
let z1 = prevXZ;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
slerpQX = vX;
slerpQY = vY;
slerpQZ = vZ;
slerpQW = 0;
} else {
let cX;
let cY;
let cZ;
cX = prevXY * newXZ - prevXZ * newXY;
cY = prevXZ * newXX - prevXX * newXZ;
cZ = prevXX * newXY - prevXY * newXX;
let w = Math.sqrt((1 + d2) * 0.5);
d2 = 0.5 / w;
cX *= d2;
cY *= d2;
cZ *= d2;
slerpQX = cX;
slerpQY = cY;
slerpQZ = cZ;
slerpQW = w;
}
let x1 = slerpQX;
let y1 = slerpQY;
let z1 = slerpQZ;
let w1 = slerpQW;
let x21 = 2 * x1;
let y21 = 2 * y1;
let z21 = 2 * z1;
let xx1 = x1 * x21;
let yy1 = y1 * y21;
let zz1 = z1 * z21;
let xy1 = x1 * y21;
let yz1 = y1 * z21;
let xz1 = x1 * z21;
let wx1 = w1 * x21;
let wy1 = w1 * y21;
let wz1 = w1 * z21;
slerpM00 = 1 - yy1 - zz1;
slerpM01 = xy1 - wz1;
slerpM02 = xz1 + wy1;
slerpM10 = xy1 + wz1;
slerpM11 = 1 - xx1 - zz1;
slerpM12 = yz1 - wx1;
slerpM20 = xz1 - wy1;
slerpM21 = yz1 + wx1;
slerpM22 = 1 - xx1 - yy1;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = slerpM00 * prevYX + slerpM01 * prevYY + slerpM02 * prevYZ;
__tmp__Y1 = slerpM10 * prevYX + slerpM11 * prevYY + slerpM12 * prevYZ;
__tmp__Z1 = slerpM20 * prevYX + slerpM21 * prevYY + slerpM22 * prevYZ;
newYX = __tmp__X1;
newYY = __tmp__Y1;
newYZ = __tmp__Z1;
newZX = newXY * newYZ - newXZ * newYY;
newZY = newXZ * newYX - newXX * newYZ;
newZZ = newXX * newYY - newXY * newYX;
if(newZX * newZX + newZY * newZY + newZZ * newZZ > 1e-6) {
let l = newZX * newZX + newZY * newZY + newZZ * newZZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
newZX *= l;
newZY *= l;
newZZ *= l;
} else {
let x1 = newXX;
let y1 = newXY;
let z1 = newXZ;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
newZX = 0;
newZY = z1 * d;
newZZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
newZX = y1 * d;
newZY = -x1 * d;
newZZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
newZX = -z1 * d;
newZY = 0;
newZZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
newZX = y1 * d;
newZY = -x1 * d;
newZZ = 0;
}
}
newYX = newZY * newXZ - newZZ * newXY;
newYY = newZZ * newXX - newZX * newXZ;
newYZ = newZX * newXY - newZY * newXX;
_this.xX = newXX;
_this.xY = newXY;
_this.xZ = newXZ;
_this.yX = newYX;
_this.yY = newYY;
_this.yZ = newYZ;
_this.zX = newZX;
_this.zY = newZY;
_this.zZ = newZZ;
let rot100;
let rot101;
let rot102;
let rot110;
let rot111;
let rot112;
let rot120;
let rot121;
let rot122;
let rot200;
let rot201;
let rot202;
let rot210;
let rot211;
let rot212;
let rot220;
let rot221;
let rot222;
rot100 = this._basisX1X;
rot101 = this._basisY1X;
rot102 = this._basisZ1X;
rot110 = this._basisX1Y;
rot111 = this._basisY1Y;
rot112 = this._basisZ1Y;
rot120 = this._basisX1Z;
rot121 = this._basisY1Z;
rot122 = this._basisZ1Z;
rot200 = this._basisX2X;
rot201 = this._basisY2X;
rot202 = this._basisZ2X;
rot210 = this._basisX2Y;
rot211 = this._basisY2Y;
rot212 = this._basisZ2Y;
rot220 = this._basisX2Z;
rot221 = this._basisY2Z;
rot222 = this._basisZ2Z;
let relRot00;
let relRot01;
let relRot02;
let relRot10;
let relRot11;
let relRot12;
let relRot20;
let relRot21;
let relRot22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = rot200 * rot100 + rot201 * rot101 + rot202 * rot102;
__tmp__01 = rot200 * rot110 + rot201 * rot111 + rot202 * rot112;
__tmp__02 = rot200 * rot120 + rot201 * rot121 + rot202 * rot122;
__tmp__10 = rot210 * rot100 + rot211 * rot101 + rot212 * rot102;
__tmp__11 = rot210 * rot110 + rot211 * rot111 + rot212 * rot112;
__tmp__12 = rot210 * rot120 + rot211 * rot121 + rot212 * rot122;
__tmp__20 = rot220 * rot100 + rot221 * rot101 + rot222 * rot102;
__tmp__21 = rot220 * rot110 + rot221 * rot111 + rot222 * rot112;
__tmp__22 = rot220 * rot120 + rot221 * rot121 + rot222 * rot122;
relRot00 = __tmp__00;
relRot01 = __tmp__01;
relRot02 = __tmp__02;
relRot10 = __tmp__10;
relRot11 = __tmp__11;
relRot12 = __tmp__12;
relRot20 = __tmp__20;
relRot21 = __tmp__21;
relRot22 = __tmp__22;
let relQX;
let relQY;
let relQZ;
let relQW;
let e00 = relRot00;
let e11 = relRot11;
let e22 = relRot22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
relQW = 0.5 * s;
s = 0.5 / s;
relQX = (relRot21 - relRot12) * s;
relQY = (relRot02 - relRot20) * s;
relQZ = (relRot10 - relRot01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
relQX = 0.5 * s;
s = 0.5 / s;
relQY = (relRot01 + relRot10) * s;
relQZ = (relRot02 + relRot20) * s;
relQW = (relRot21 - relRot12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
relQZ = 0.5 * s;
s = 0.5 / s;
relQX = (relRot02 + relRot20) * s;
relQY = (relRot12 + relRot21) * s;
relQW = (relRot10 - relRot01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
relQY = 0.5 * s;
s = 0.5 / s;
relQX = (relRot01 + relRot10) * s;
relQZ = (relRot12 + relRot21) * s;
relQW = (relRot02 - relRot20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
relQZ = 0.5 * s;
s = 0.5 / s;
relQX = (relRot02 + relRot20) * s;
relQY = (relRot12 + relRot21) * s;
relQW = (relRot10 - relRot01) * s;
}
let cosHalfTheta = relQW;
let theta = (cosHalfTheta <= -1 ? 3.14159265358979 : cosHalfTheta >= 1 ? 0 : Math.acos(cosHalfTheta)) * 2;
this.angularErrorX = relQX;
this.angularErrorY = relQY;
this.angularErrorZ = relQZ;
let l = this.angularErrorX * this.angularErrorX + this.angularErrorY * this.angularErrorY + this.angularErrorZ * this.angularErrorZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this.angularErrorX *= l;
this.angularErrorY *= l;
this.angularErrorZ *= l;
this.angularErrorX *= theta;
this.angularErrorY *= theta;
this.angularErrorZ *= theta;
let anchorDiffX;
let anchorDiffY;
let anchorDiffZ;
anchorDiffX = this._anchor2X - this._anchor1X;
anchorDiffY = this._anchor2Y - this._anchor1Y;
anchorDiffZ = this._anchor2Z - this._anchor1Z;
this.translation = anchorDiffX * this._basis.xX + anchorDiffY * this._basis.xY + anchorDiffZ * this._basis.xZ;
this.linearErrorY = anchorDiffX * this._basis.yX + anchorDiffY * this._basis.yY + anchorDiffZ * this._basis.yZ;
this.linearErrorZ = anchorDiffX * this._basis.zX + anchorDiffY * this._basis.zY + anchorDiffZ * this._basis.zZ;
}
_getVelocitySolverInfo(timeStep,info) {
super._getVelocitySolverInfo(timeStep,info);
this.getInfo(info,timeStep,false);
}
_getPositionSolverInfo(info) {
super._getPositionSolverInfo(info);
this.getInfo(info,null,true);
}
getAxis1() {
let v = new oimo.common.Vec3();
v.x = this._basisX1X;
v.y = this._basisX1Y;
v.z = this._basisX1Z;
return v;
}
getAxis2() {
let v = new oimo.common.Vec3();
v.x = this._basisX2X;
v.y = this._basisX2Y;
v.z = this._basisX2Z;
return v;
}
getAxis1To(axis) {
axis.x = this._basisX1X;
axis.y = this._basisX1Y;
axis.z = this._basisX1Z;
}
getAxis2To(axis) {
axis.x = this._basisX2X;
axis.y = this._basisX2Y;
axis.z = this._basisX2Z;
}
getLocalAxis1() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX1X;
v.y = this._localBasisX1Y;
v.z = this._localBasisX1Z;
return v;
}
getLocalAxis2() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX2X;
v.y = this._localBasisX2Y;
v.z = this._localBasisX2Z;
return v;
}
getLocalAxis1To(axis) {
axis.x = this._localBasisX1X;
axis.y = this._localBasisX1Y;
axis.z = this._localBasisX1Z;
}
getLocalAxis2To(axis) {
axis.x = this._localBasisX2X;
axis.y = this._localBasisX2Y;
axis.z = this._localBasisX2Z;
}
getSpringDamper() {
return this._sd;
}
getLimitMotor() {
return this._lm;
}
getTranslation() {
return this.translation;
}
}
oimo.dynamics.constraint.joint.PrismaticJointConfig = class oimo_dynamics_constraint_joint_PrismaticJointConfig extends oimo.dynamics.constraint.joint.JointConfig {
constructor() {
super();
this.localAxis1 = new oimo.common.Vec3(1,0,0);
this.localAxis2 = new oimo.common.Vec3(1,0,0);
this.limitMotor = new oimo.dynamics.constraint.joint.TranslationalLimitMotor();
this.springDamper = new oimo.dynamics.constraint.joint.SpringDamper();
}
init(rigidBody1,rigidBody2,worldAnchor,worldAxis) {
this._init(rigidBody1,rigidBody2,worldAnchor);
let localVector = this.localAxis1;
let vX;
let vY;
let vZ;
vX = worldAxis.x;
vY = worldAxis.y;
vZ = worldAxis.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = rigidBody1._transform._rotation00 * vX + rigidBody1._transform._rotation10 * vY + rigidBody1._transform._rotation20 * vZ;
__tmp__Y = rigidBody1._transform._rotation01 * vX + rigidBody1._transform._rotation11 * vY + rigidBody1._transform._rotation21 * vZ;
__tmp__Z = rigidBody1._transform._rotation02 * vX + rigidBody1._transform._rotation12 * vY + rigidBody1._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
localVector.x = vX;
localVector.y = vY;
localVector.z = vZ;
let localVector1 = this.localAxis2;
let vX1;
let vY1;
let vZ1;
vX1 = worldAxis.x;
vY1 = worldAxis.y;
vZ1 = worldAxis.z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = rigidBody2._transform._rotation00 * vX1 + rigidBody2._transform._rotation10 * vY1 + rigidBody2._transform._rotation20 * vZ1;
__tmp__Y1 = rigidBody2._transform._rotation01 * vX1 + rigidBody2._transform._rotation11 * vY1 + rigidBody2._transform._rotation21 * vZ1;
__tmp__Z1 = rigidBody2._transform._rotation02 * vX1 + rigidBody2._transform._rotation12 * vY1 + rigidBody2._transform._rotation22 * vZ1;
vX1 = __tmp__X1;
vY1 = __tmp__Y1;
vZ1 = __tmp__Z1;
localVector1.x = vX1;
localVector1.y = vY1;
localVector1.z = vZ1;
return this;
}
}
oimo.dynamics.constraint.joint.RagdollJoint = class oimo_dynamics_constraint_joint_RagdollJoint extends oimo.dynamics.constraint.joint.Joint {
constructor(config) {
super(config,oimo.dynamics.constraint.joint.JointType.RAGDOLL);
let v = config.localTwistAxis1;
this._localBasisX1X = v.x;
this._localBasisX1Y = v.y;
this._localBasisX1Z = v.z;
let v1 = config.localSwingAxis1;
this._localBasisY1X = v1.x;
this._localBasisY1Y = v1.y;
this._localBasisY1Z = v1.z;
let v2 = config.localTwistAxis2;
this._localBasisX2X = v2.x;
this._localBasisX2Y = v2.y;
this._localBasisX2Z = v2.z;
this.buildLocalBasesFromXY1X2();
this._twistSd = config.twistSpringDamper.clone();
this._twistLm = config.twistLimitMotor.clone();
this._swingSd = config.swingSpringDamper.clone();
this._maxSwingAngle1 = config.maxSwingAngle1;
this._maxSwingAngle2 = config.maxSwingAngle2;
if(this._maxSwingAngle1 < oimo.common.Setting.minRagdollMaxSwingAngle) {
this._maxSwingAngle1 = oimo.common.Setting.minRagdollMaxSwingAngle;
}
if(this._maxSwingAngle2 < oimo.common.Setting.minRagdollMaxSwingAngle) {
this._maxSwingAngle2 = oimo.common.Setting.minRagdollMaxSwingAngle;
}
this.dummySwingLm = new oimo.dynamics.constraint.joint.RotationalLimitMotor();
this.dummySwingLm.lowerLimit = -1;
this.dummySwingLm.upperLimit = 0;
this._swingAngle = 0;
this._twistAngle = 0;
this.swingError = 0;
this.swingAxisX = 0;
this.swingAxisY = 0;
this.swingAxisZ = 0;
this.twistAxisX = 0;
this.twistAxisY = 0;
this.twistAxisZ = 0;
}
getInfo(info,timeStep,isPositionPart) {
let erp = this.getErp(timeStep,isPositionPart);
let linearRhsX;
let linearRhsY;
let linearRhsZ;
linearRhsX = this.linearErrorX * erp;
linearRhsY = this.linearErrorY * erp;
linearRhsZ = this.linearErrorZ * erp;
let crossR100;
let crossR101;
let crossR102;
let crossR110;
let crossR111;
let crossR112;
let crossR120;
let crossR121;
let crossR122;
let crossR200;
let crossR201;
let crossR202;
let crossR210;
let crossR211;
let crossR212;
let crossR220;
let crossR221;
let crossR222;
crossR100 = 0;
crossR101 = -this._relativeAnchor1Z;
crossR102 = this._relativeAnchor1Y;
crossR110 = this._relativeAnchor1Z;
crossR111 = 0;
crossR112 = -this._relativeAnchor1X;
crossR120 = -this._relativeAnchor1Y;
crossR121 = this._relativeAnchor1X;
crossR122 = 0;
crossR200 = 0;
crossR201 = -this._relativeAnchor2Z;
crossR202 = this._relativeAnchor2Y;
crossR210 = this._relativeAnchor2Z;
crossR211 = 0;
crossR212 = -this._relativeAnchor2X;
crossR220 = -this._relativeAnchor2Y;
crossR221 = this._relativeAnchor2X;
crossR222 = 0;
crossR100 = -crossR100;
crossR101 = -crossR101;
crossR102 = -crossR102;
crossR110 = -crossR110;
crossR111 = -crossR111;
crossR112 = -crossR112;
crossR120 = -crossR120;
crossR121 = -crossR121;
crossR122 = -crossR122;
crossR200 = -crossR200;
crossR201 = -crossR201;
crossR202 = -crossR202;
crossR210 = -crossR210;
crossR211 = -crossR211;
crossR212 = -crossR212;
crossR220 = -crossR220;
crossR221 = -crossR221;
crossR222 = -crossR222;
let swingMass = this.computeEffectiveInertiaMoment(this.swingAxisX,this.swingAxisY,this.swingAxisZ);
let twistMass = this.computeEffectiveInertiaMoment(this._basisX2X,this._basisX2Y,this._basisX2Z);
let impulse = this._impulses[0];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
row.rhs = linearRhsX;
row.cfm = 0;
row.minImpulse = -1e65536;
row.maxImpulse = 1e65536;
let j = row.jacobian;
j.lin1X = 1;
j.lin1Y = 0;
j.lin1Z = 0;
j.lin2X = 1;
j.lin2Y = 0;
j.lin2Z = 0;
j.ang1X = crossR100;
j.ang1Y = crossR101;
j.ang1Z = crossR102;
j.ang2X = crossR200;
j.ang2Y = crossR201;
j.ang2Z = crossR202;
let impulse1 = this._impulses[1];
let row1 = info.rows[info.numRows++];
let _this1 = row1.jacobian;
_this1.lin1X = 0;
_this1.lin1Y = 0;
_this1.lin1Z = 0;
_this1.lin2X = 0;
_this1.lin2Y = 0;
_this1.lin2Z = 0;
_this1.ang1X = 0;
_this1.ang1Y = 0;
_this1.ang1Z = 0;
_this1.ang2X = 0;
_this1.ang2Y = 0;
_this1.ang2Z = 0;
row1.rhs = 0;
row1.cfm = 0;
row1.minImpulse = 0;
row1.maxImpulse = 0;
row1.motorSpeed = 0;
row1.motorMaxImpulse = 0;
row1.impulse = null;
row1.impulse = impulse1;
row1.rhs = linearRhsY;
row1.cfm = 0;
row1.minImpulse = -1e65536;
row1.maxImpulse = 1e65536;
j = row1.jacobian;
j.lin1X = 0;
j.lin1Y = 1;
j.lin1Z = 0;
j.lin2X = 0;
j.lin2Y = 1;
j.lin2Z = 0;
j.ang1X = crossR110;
j.ang1Y = crossR111;
j.ang1Z = crossR112;
j.ang2X = crossR210;
j.ang2Y = crossR211;
j.ang2Z = crossR212;
let impulse2 = this._impulses[2];
let row2 = info.rows[info.numRows++];
let _this2 = row2.jacobian;
_this2.lin1X = 0;
_this2.lin1Y = 0;
_this2.lin1Z = 0;
_this2.lin2X = 0;
_this2.lin2Y = 0;
_this2.lin2Z = 0;
_this2.ang1X = 0;
_this2.ang1Y = 0;
_this2.ang1Z = 0;
_this2.ang2X = 0;
_this2.ang2Y = 0;
_this2.ang2Z = 0;
row2.rhs = 0;
row2.cfm = 0;
row2.minImpulse = 0;
row2.maxImpulse = 0;
row2.motorSpeed = 0;
row2.motorMaxImpulse = 0;
row2.impulse = null;
row2.impulse = impulse2;
row2.rhs = linearRhsZ;
row2.cfm = 0;
row2.minImpulse = -1e65536;
row2.maxImpulse = 1e65536;
j = row2.jacobian;
j.lin1X = 0;
j.lin1Y = 0;
j.lin1Z = 1;
j.lin2X = 0;
j.lin2Y = 0;
j.lin2Z = 1;
j.ang1X = crossR120;
j.ang1Y = crossR121;
j.ang1Z = crossR122;
j.ang2X = crossR220;
j.ang2Y = crossR221;
j.ang2Z = crossR222;
if(this.swingError > 0 && (this._swingSd.frequency <= 0 || !isPositionPart)) {
let impulse = this._impulses[3];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this.swingError,this.dummySwingLm,swingMass,this._swingSd,timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this.swingAxisX;
j.ang1Y = this.swingAxisY;
j.ang1Z = this.swingAxisZ;
j.ang2X = this.swingAxisX;
j.ang2Y = this.swingAxisY;
j.ang2Z = this.swingAxisZ;
}
if(this._twistSd.frequency <= 0 || !isPositionPart) {
let impulse = this._impulses[4];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this._twistAngle,this._twistLm,twistMass,this._twistSd,timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this.twistAxisX;
j.ang1Y = this.twistAxisY;
j.ang1Z = this.twistAxisZ;
j.ang2X = this.twistAxisX;
j.ang2Y = this.twistAxisY;
j.ang2Z = this.twistAxisZ;
}
}
_syncAnchors() {
super._syncAnchors();
let axis1X;
let axis1Y;
let axis1Z;
let axis2X;
let axis2Y;
let axis2Z;
axis1X = this._basisX1X;
axis1Y = this._basisX1Y;
axis1Z = this._basisX1Z;
axis2X = this._basisX2X;
axis2Y = this._basisX2Y;
axis2Z = this._basisX2Z;
let basis1Mat00;
let basis1Mat01;
let basis1Mat02;
let basis1Mat10;
let basis1Mat11;
let basis1Mat12;
let basis1Mat20;
let basis1Mat21;
let basis1Mat22;
basis1Mat00 = this._basisX1X;
basis1Mat01 = this._basisY1X;
basis1Mat02 = this._basisZ1X;
basis1Mat10 = this._basisX1Y;
basis1Mat11 = this._basisY1Y;
basis1Mat12 = this._basisZ1Y;
basis1Mat20 = this._basisX1Z;
basis1Mat21 = this._basisY1Z;
basis1Mat22 = this._basisZ1Z;
let swingQX;
let swingQY;
let swingQZ;
let swingQW;
let swingM00;
let swingM01;
let swingM02;
let swingM10;
let swingM11;
let swingM12;
let swingM20;
let swingM21;
let swingM22;
let swingVX;
let swingVY;
let swingVZ;
let d = axis1X * axis2X + axis1Y * axis2Y + axis1Z * axis2Z;
if(d < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = axis1X;
let y1 = axis1Y;
let z1 = axis1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
swingQX = vX;
swingQY = vY;
swingQZ = vZ;
swingQW = 0;
} else {
let cX;
let cY;
let cZ;
cX = axis1Y * axis2Z - axis1Z * axis2Y;
cY = axis1Z * axis2X - axis1X * axis2Z;
cZ = axis1X * axis2Y - axis1Y * axis2X;
let w = Math.sqrt((1 + d) * 0.5);
d = 0.5 / w;
cX *= d;
cY *= d;
cZ *= d;
swingQX = cX;
swingQY = cY;
swingQZ = cZ;
swingQW = w;
}
let x = swingQX;
let y = swingQY;
let z = swingQZ;
let w = swingQW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
swingM00 = 1 - yy - zz;
swingM01 = xy - wz;
swingM02 = xz + wy;
swingM10 = xy + wz;
swingM11 = 1 - xx - zz;
swingM12 = yz - wx;
swingM20 = xz - wy;
swingM21 = yz + wx;
swingM22 = 1 - xx - yy;
this._swingAngle = (swingQW <= -1 ? 3.14159265358979 : swingQW >= 1 ? 0 : Math.acos(swingQW)) * 2;
swingVX = swingQX;
swingVY = swingQY;
swingVZ = swingQZ;
let basisY2In1X;
let basisY2In1Y;
let basisY2In1Z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = swingM00 * this._basisY2X + swingM10 * this._basisY2Y + swingM20 * this._basisY2Z;
__tmp__Y = swingM01 * this._basisY2X + swingM11 * this._basisY2Y + swingM21 * this._basisY2Z;
__tmp__Z = swingM02 * this._basisY2X + swingM12 * this._basisY2Y + swingM22 * this._basisY2Z;
basisY2In1X = __tmp__X;
basisY2In1Y = __tmp__Y;
basisY2In1Z = __tmp__Z;
this._twistAngle = Math.atan2(this._basisZ1X * basisY2In1X + this._basisZ1Y * basisY2In1Y + this._basisZ1Z * basisY2In1Z,this._basisY1X * basisY2In1X + this._basisY1Y * basisY2In1Y + this._basisY1Z * basisY2In1Z);
this.twistAxisX = this._basisX1X + this._basisX2X;
this.twistAxisY = this._basisX1Y + this._basisX2Y;
this.twistAxisZ = this._basisX1Z + this._basisX2Z;
let l = this.twistAxisX * this.twistAxisX + this.twistAxisY * this.twistAxisY + this.twistAxisZ * this.twistAxisZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this.twistAxisX *= l;
this.twistAxisY *= l;
this.twistAxisZ *= l;
let invLen = Math.sqrt(swingVX * swingVX + swingVY * swingVY + swingVZ * swingVZ);
if(invLen > 0) {
invLen = 1 / invLen;
}
swingVX *= invLen * this._swingAngle;
swingVY *= invLen * this._swingAngle;
swingVZ *= invLen * this._swingAngle;
let __tmp__Y1;
let __tmp__Z1;
__tmp__Y1 = basis1Mat01 * swingVX + basis1Mat11 * swingVY + basis1Mat21 * swingVZ;
__tmp__Z1 = basis1Mat02 * swingVX + basis1Mat12 * swingVY + basis1Mat22 * swingVZ;
swingVY = __tmp__Y1;
swingVZ = __tmp__Z1;
let x1 = swingVY;
let y1 = swingVZ;
let a = this._maxSwingAngle1;
let b = this._maxSwingAngle2;
let invA2 = 1 / (a * a);
let invB2 = 1 / (b * b);
let w1 = x1 * x1 * invA2 + y1 * y1 * invB2;
if(w1 == 0) {
this.swingAxisX = 0;
this.swingAxisY = 0;
this.swingAxisZ = 0;
this.swingError = 0;
} else {
let t = Math.sqrt(1 / w1);
let x0 = x1 * t;
let y0 = y1 * t;
let nx = x0 * invA2;
let ny = y0 * invB2;
invLen = 1 / Math.sqrt(nx * nx + ny * ny);
nx *= invLen;
ny *= invLen;
let depth = (x1 - x0) * nx + (y1 - y0) * ny;
if(depth > 0) {
this.swingError = depth;
this.swingAxisX = 0;
this.swingAxisY = nx;
this.swingAxisZ = ny;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = basis1Mat00 * this.swingAxisX + basis1Mat01 * this.swingAxisY + basis1Mat02 * this.swingAxisZ;
__tmp__Y = basis1Mat10 * this.swingAxisX + basis1Mat11 * this.swingAxisY + basis1Mat12 * this.swingAxisZ;
__tmp__Z = basis1Mat20 * this.swingAxisX + basis1Mat21 * this.swingAxisY + basis1Mat22 * this.swingAxisZ;
this.swingAxisX = __tmp__X;
this.swingAxisY = __tmp__Y;
this.swingAxisZ = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = swingM00 * this.swingAxisX + swingM01 * this.swingAxisY + swingM02 * this.swingAxisZ;
__tmp__Y1 = swingM10 * this.swingAxisX + swingM11 * this.swingAxisY + swingM12 * this.swingAxisZ;
__tmp__Z1 = swingM20 * this.swingAxisX + swingM21 * this.swingAxisY + swingM22 * this.swingAxisZ;
this.swingAxisX = __tmp__X1;
this.swingAxisY = __tmp__Y1;
this.swingAxisZ = __tmp__Z1;
} else {
this.swingError = 0;
}
}
this.linearErrorX = this._anchor2X - this._anchor1X;
this.linearErrorY = this._anchor2Y - this._anchor1Y;
this.linearErrorZ = this._anchor2Z - this._anchor1Z;
}
_getVelocitySolverInfo(timeStep,info) {
super._getVelocitySolverInfo(timeStep,info);
this.getInfo(info,timeStep,false);
}
_getPositionSolverInfo(info) {
super._getPositionSolverInfo(info);
this.getInfo(info,null,true);
}
getAxis1() {
let v = new oimo.common.Vec3();
v.x = this._basisX1X;
v.y = this._basisX1Y;
v.z = this._basisX1Z;
return v;
}
getAxis2() {
let v = new oimo.common.Vec3();
v.x = this._basisX2X;
v.y = this._basisX2Y;
v.z = this._basisX2Z;
return v;
}
getAxis1To(axis) {
axis.x = this._basisX1X;
axis.y = this._basisX1Y;
axis.z = this._basisX1Z;
}
getAxis2To(axis) {
axis.x = this._basisX2X;
axis.y = this._basisX2Y;
axis.z = this._basisX2Z;
}
getLocalAxis1() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX1X;
v.y = this._localBasisX1Y;
v.z = this._localBasisX1Z;
return v;
}
getLocalAxis2() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX2X;
v.y = this._localBasisX2Y;
v.z = this._localBasisX2Z;
return v;
}
getLocalAxis1To(axis) {
axis.x = this._localBasisX1X;
axis.y = this._localBasisX1Y;
axis.z = this._localBasisX1Z;
}
getLocalAxis2To(axis) {
axis.x = this._localBasisX2X;
axis.y = this._localBasisX2Y;
axis.z = this._localBasisX2Z;
}
getTwistSpringDamper() {
return this._twistSd;
}
getTwistLimitMotor() {
return this._twistLm;
}
getSwingSpringDamper() {
return this._swingSd;
}
getSwingAxis() {
let v = new oimo.common.Vec3();
v.x = this.swingAxisX;
v.y = this.swingAxisY;
v.z = this.swingAxisZ;
return v;
}
getSwingAxisTo(axis) {
axis.x = this.swingAxisX;
axis.y = this.swingAxisY;
axis.z = this.swingAxisZ;
}
getSwingAngle() {
return this._swingAngle;
}
getTwistAngle() {
return this._twistAngle;
}
}
oimo.dynamics.constraint.joint.RagdollJointConfig = class oimo_dynamics_constraint_joint_RagdollJointConfig extends oimo.dynamics.constraint.joint.JointConfig {
constructor() {
super();
this.localTwistAxis1 = new oimo.common.Vec3(1,0,0);
this.localTwistAxis2 = new oimo.common.Vec3(1,0,0);
this.localSwingAxis1 = new oimo.common.Vec3(0,1,0);
this.twistSpringDamper = new oimo.dynamics.constraint.joint.SpringDamper();
this.swingSpringDamper = new oimo.dynamics.constraint.joint.SpringDamper();
this.twistLimitMotor = new oimo.dynamics.constraint.joint.RotationalLimitMotor();
this.maxSwingAngle1 = 3.14159265358979;
this.maxSwingAngle2 = 3.14159265358979;
}
init(rigidBody1,rigidBody2,worldAnchor,worldTwistAxis,worldSwingAxis) {
this._init(rigidBody1,rigidBody2,worldAnchor);
let localVector = this.localTwistAxis1;
let vX;
let vY;
let vZ;
vX = worldTwistAxis.x;
vY = worldTwistAxis.y;
vZ = worldTwistAxis.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = rigidBody1._transform._rotation00 * vX + rigidBody1._transform._rotation10 * vY + rigidBody1._transform._rotation20 * vZ;
__tmp__Y = rigidBody1._transform._rotation01 * vX + rigidBody1._transform._rotation11 * vY + rigidBody1._transform._rotation21 * vZ;
__tmp__Z = rigidBody1._transform._rotation02 * vX + rigidBody1._transform._rotation12 * vY + rigidBody1._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
localVector.x = vX;
localVector.y = vY;
localVector.z = vZ;
let localVector1 = this.localTwistAxis2;
let vX1;
let vY1;
let vZ1;
vX1 = worldTwistAxis.x;
vY1 = worldTwistAxis.y;
vZ1 = worldTwistAxis.z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = rigidBody2._transform._rotation00 * vX1 + rigidBody2._transform._rotation10 * vY1 + rigidBody2._transform._rotation20 * vZ1;
__tmp__Y1 = rigidBody2._transform._rotation01 * vX1 + rigidBody2._transform._rotation11 * vY1 + rigidBody2._transform._rotation21 * vZ1;
__tmp__Z1 = rigidBody2._transform._rotation02 * vX1 + rigidBody2._transform._rotation12 * vY1 + rigidBody2._transform._rotation22 * vZ1;
vX1 = __tmp__X1;
vY1 = __tmp__Y1;
vZ1 = __tmp__Z1;
localVector1.x = vX1;
localVector1.y = vY1;
localVector1.z = vZ1;
let localVector2 = this.localSwingAxis1;
let vX2;
let vY2;
let vZ2;
vX2 = worldSwingAxis.x;
vY2 = worldSwingAxis.y;
vZ2 = worldSwingAxis.z;
let __tmp__X2;
let __tmp__Y2;
let __tmp__Z2;
__tmp__X2 = rigidBody1._transform._rotation00 * vX2 + rigidBody1._transform._rotation10 * vY2 + rigidBody1._transform._rotation20 * vZ2;
__tmp__Y2 = rigidBody1._transform._rotation01 * vX2 + rigidBody1._transform._rotation11 * vY2 + rigidBody1._transform._rotation21 * vZ2;
__tmp__Z2 = rigidBody1._transform._rotation02 * vX2 + rigidBody1._transform._rotation12 * vY2 + rigidBody1._transform._rotation22 * vZ2;
vX2 = __tmp__X2;
vY2 = __tmp__Y2;
vZ2 = __tmp__Z2;
localVector2.x = vX2;
localVector2.y = vY2;
localVector2.z = vZ2;
return this;
}
}
oimo.dynamics.constraint.joint.RevoluteJoint = class oimo_dynamics_constraint_joint_RevoluteJoint extends oimo.dynamics.constraint.joint.Joint {
constructor(config) {
super(config,1);
let v = config.localAxis1;
this._localBasisX1X = v.x;
this._localBasisX1Y = v.y;
this._localBasisX1Z = v.z;
let v1 = config.localAxis2;
this._localBasisX2X = v1.x;
this._localBasisX2Y = v1.y;
this._localBasisX2Z = v1.z;
this.buildLocalBasesFromX();
this.angle = 0;
this.angularErrorY = 0;
this.angularErrorZ = 0;
this._basis = new oimo.dynamics.constraint.joint.BasisTracker(this);
this._sd = config.springDamper.clone();
this._lm = config.limitMotor.clone();
}
getInfo(info,timeStep,isPositionPart) {
let erp = this.getErp(timeStep,isPositionPart);
let linearRhsX;
let linearRhsY;
let linearRhsZ;
linearRhsX = this.linearErrorX * erp;
linearRhsY = this.linearErrorY * erp;
linearRhsZ = this.linearErrorZ * erp;
let angRhsY = this.angularErrorY * erp;
let angRhsZ = this.angularErrorZ * erp;
let crossR100;
let crossR101;
let crossR102;
let crossR110;
let crossR111;
let crossR112;
let crossR120;
let crossR121;
let crossR122;
let crossR200;
let crossR201;
let crossR202;
let crossR210;
let crossR211;
let crossR212;
let crossR220;
let crossR221;
let crossR222;
crossR100 = 0;
crossR101 = -this._relativeAnchor1Z;
crossR102 = this._relativeAnchor1Y;
crossR110 = this._relativeAnchor1Z;
crossR111 = 0;
crossR112 = -this._relativeAnchor1X;
crossR120 = -this._relativeAnchor1Y;
crossR121 = this._relativeAnchor1X;
crossR122 = 0;
crossR200 = 0;
crossR201 = -this._relativeAnchor2Z;
crossR202 = this._relativeAnchor2Y;
crossR210 = this._relativeAnchor2Z;
crossR211 = 0;
crossR212 = -this._relativeAnchor2X;
crossR220 = -this._relativeAnchor2Y;
crossR221 = this._relativeAnchor2X;
crossR222 = 0;
crossR100 = -crossR100;
crossR101 = -crossR101;
crossR102 = -crossR102;
crossR110 = -crossR110;
crossR111 = -crossR111;
crossR112 = -crossR112;
crossR120 = -crossR120;
crossR121 = -crossR121;
crossR122 = -crossR122;
crossR200 = -crossR200;
crossR201 = -crossR201;
crossR202 = -crossR202;
crossR210 = -crossR210;
crossR211 = -crossR211;
crossR212 = -crossR212;
crossR220 = -crossR220;
crossR221 = -crossR221;
crossR222 = -crossR222;
let motorMass = this.computeEffectiveInertiaMoment(this._basis.xX,this._basis.xY,this._basis.xZ);
let impulse = this._impulses[0];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
row.rhs = linearRhsX;
row.cfm = 0;
row.minImpulse = -1e65536;
row.maxImpulse = 1e65536;
let j = row.jacobian;
j.lin1X = 1;
j.lin1Y = 0;
j.lin1Z = 0;
j.lin2X = 1;
j.lin2Y = 0;
j.lin2Z = 0;
j.ang1X = crossR100;
j.ang1Y = crossR101;
j.ang1Z = crossR102;
j.ang2X = crossR200;
j.ang2Y = crossR201;
j.ang2Z = crossR202;
let impulse1 = this._impulses[1];
let row1 = info.rows[info.numRows++];
let _this1 = row1.jacobian;
_this1.lin1X = 0;
_this1.lin1Y = 0;
_this1.lin1Z = 0;
_this1.lin2X = 0;
_this1.lin2Y = 0;
_this1.lin2Z = 0;
_this1.ang1X = 0;
_this1.ang1Y = 0;
_this1.ang1Z = 0;
_this1.ang2X = 0;
_this1.ang2Y = 0;
_this1.ang2Z = 0;
row1.rhs = 0;
row1.cfm = 0;
row1.minImpulse = 0;
row1.maxImpulse = 0;
row1.motorSpeed = 0;
row1.motorMaxImpulse = 0;
row1.impulse = null;
row1.impulse = impulse1;
row1.rhs = linearRhsY;
row1.cfm = 0;
row1.minImpulse = -1e65536;
row1.maxImpulse = 1e65536;
j = row1.jacobian;
j.lin1X = 0;
j.lin1Y = 1;
j.lin1Z = 0;
j.lin2X = 0;
j.lin2Y = 1;
j.lin2Z = 0;
j.ang1X = crossR110;
j.ang1Y = crossR111;
j.ang1Z = crossR112;
j.ang2X = crossR210;
j.ang2Y = crossR211;
j.ang2Z = crossR212;
let impulse2 = this._impulses[2];
let row2 = info.rows[info.numRows++];
let _this2 = row2.jacobian;
_this2.lin1X = 0;
_this2.lin1Y = 0;
_this2.lin1Z = 0;
_this2.lin2X = 0;
_this2.lin2Y = 0;
_this2.lin2Z = 0;
_this2.ang1X = 0;
_this2.ang1Y = 0;
_this2.ang1Z = 0;
_this2.ang2X = 0;
_this2.ang2Y = 0;
_this2.ang2Z = 0;
row2.rhs = 0;
row2.cfm = 0;
row2.minImpulse = 0;
row2.maxImpulse = 0;
row2.motorSpeed = 0;
row2.motorMaxImpulse = 0;
row2.impulse = null;
row2.impulse = impulse2;
row2.rhs = linearRhsZ;
row2.cfm = 0;
row2.minImpulse = -1e65536;
row2.maxImpulse = 1e65536;
j = row2.jacobian;
j.lin1X = 0;
j.lin1Y = 0;
j.lin1Z = 1;
j.lin2X = 0;
j.lin2Y = 0;
j.lin2Z = 1;
j.ang1X = crossR120;
j.ang1Y = crossR121;
j.ang1Z = crossR122;
j.ang2X = crossR220;
j.ang2Y = crossR221;
j.ang2Z = crossR222;
if(this._sd.frequency <= 0 || !isPositionPart) {
let impulse = this._impulses[3];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this.angle,this._lm,motorMass,this._sd,timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this._basis.xX;
j.ang1Y = this._basis.xY;
j.ang1Z = this._basis.xZ;
j.ang2X = this._basis.xX;
j.ang2Y = this._basis.xY;
j.ang2Z = this._basis.xZ;
}
let impulse3 = this._impulses[4];
let row3 = info.rows[info.numRows++];
let _this3 = row3.jacobian;
_this3.lin1X = 0;
_this3.lin1Y = 0;
_this3.lin1Z = 0;
_this3.lin2X = 0;
_this3.lin2Y = 0;
_this3.lin2Z = 0;
_this3.ang1X = 0;
_this3.ang1Y = 0;
_this3.ang1Z = 0;
_this3.ang2X = 0;
_this3.ang2Y = 0;
_this3.ang2Z = 0;
row3.rhs = 0;
row3.cfm = 0;
row3.minImpulse = 0;
row3.maxImpulse = 0;
row3.motorSpeed = 0;
row3.motorMaxImpulse = 0;
row3.impulse = null;
row3.impulse = impulse3;
row3.rhs = angRhsY;
row3.cfm = 0;
row3.minImpulse = -1e65536;
row3.maxImpulse = 1e65536;
j = row3.jacobian;
j.ang1X = this._basis.yX;
j.ang1Y = this._basis.yY;
j.ang1Z = this._basis.yZ;
j.ang2X = this._basis.yX;
j.ang2Y = this._basis.yY;
j.ang2Z = this._basis.yZ;
let impulse4 = this._impulses[5];
let row4 = info.rows[info.numRows++];
let _this4 = row4.jacobian;
_this4.lin1X = 0;
_this4.lin1Y = 0;
_this4.lin1Z = 0;
_this4.lin2X = 0;
_this4.lin2Y = 0;
_this4.lin2Z = 0;
_this4.ang1X = 0;
_this4.ang1Y = 0;
_this4.ang1Z = 0;
_this4.ang2X = 0;
_this4.ang2Y = 0;
_this4.ang2Z = 0;
row4.rhs = 0;
row4.cfm = 0;
row4.minImpulse = 0;
row4.maxImpulse = 0;
row4.motorSpeed = 0;
row4.motorMaxImpulse = 0;
row4.impulse = null;
row4.impulse = impulse4;
row4.rhs = angRhsZ;
row4.cfm = 0;
row4.minImpulse = -1e65536;
row4.maxImpulse = 1e65536;
j = row4.jacobian;
j.ang1X = this._basis.zX;
j.ang1Y = this._basis.zY;
j.ang1Z = this._basis.zZ;
j.ang2X = this._basis.zX;
j.ang2Y = this._basis.zY;
j.ang2Z = this._basis.zZ;
}
_syncAnchors() {
super._syncAnchors();
let _this = this._basis;
let invM1 = _this.joint._b1._invMass;
let invM2 = _this.joint._b2._invMass;
let qX;
let qY;
let qZ;
let qW;
let idQX;
let idQY;
let idQZ;
let idQW;
let slerpQX;
let slerpQY;
let slerpQZ;
let slerpQW;
let slerpM00;
let slerpM01;
let slerpM02;
let slerpM10;
let slerpM11;
let slerpM12;
let slerpM20;
let slerpM21;
let slerpM22;
let newXX;
let newXY;
let newXZ;
let newYX;
let newYY;
let newYZ;
let newZX;
let newZY;
let newZZ;
let prevXX;
let prevXY;
let prevXZ;
let prevYX;
let prevYY;
let prevYZ;
let d = _this.joint._basisX1X * _this.joint._basisX2X + _this.joint._basisX1Y * _this.joint._basisX2Y + _this.joint._basisX1Z * _this.joint._basisX2Z;
if(d < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = _this.joint._basisX1X;
let y1 = _this.joint._basisX1Y;
let z1 = _this.joint._basisX1Z;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
qX = vX;
qY = vY;
qZ = vZ;
qW = 0;
} else {
let cX;
let cY;
let cZ;
cX = _this.joint._basisX1Y * _this.joint._basisX2Z - _this.joint._basisX1Z * _this.joint._basisX2Y;
cY = _this.joint._basisX1Z * _this.joint._basisX2X - _this.joint._basisX1X * _this.joint._basisX2Z;
cZ = _this.joint._basisX1X * _this.joint._basisX2Y - _this.joint._basisX1Y * _this.joint._basisX2X;
let w = Math.sqrt((1 + d) * 0.5);
d = 0.5 / w;
cX *= d;
cY *= d;
cZ *= d;
qX = cX;
qY = cY;
qZ = cZ;
qW = w;
}
idQX = 0;
idQY = 0;
idQZ = 0;
idQW = 1;
let q1X;
let q1Y;
let q1Z;
let q1W;
let q2X;
let q2Y;
let q2Z;
let q2W;
q1X = idQX;
q1Y = idQY;
q1Z = idQZ;
q1W = idQW;
q2X = qX;
q2Y = qY;
q2Z = qZ;
q2W = qW;
let d1 = q1X * q2X + q1Y * q2Y + q1Z * q2Z + q1W * q2W;
if(d1 < 0) {
d1 = -d1;
q2X = -q2X;
q2Y = -q2Y;
q2Z = -q2Z;
q2W = -q2W;
}
if(d1 > 0.999999) {
let dqX;
let dqY;
let dqZ;
let dqW;
dqX = q2X - q1X;
dqY = q2Y - q1Y;
dqZ = q2Z - q1Z;
dqW = q2W - q1W;
q2X = q1X + dqX * (invM1 / (invM1 + invM2));
q2Y = q1Y + dqY * (invM1 / (invM1 + invM2));
q2Z = q1Z + dqZ * (invM1 / (invM1 + invM2));
q2W = q1W + dqW * (invM1 / (invM1 + invM2));
let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
slerpQX = q2X * l;
slerpQY = q2Y * l;
slerpQZ = q2Z * l;
slerpQW = q2W * l;
} else {
let theta = invM1 / (invM1 + invM2) * Math.acos(d1);
q2X += q1X * -d1;
q2Y += q1Y * -d1;
q2Z += q1Z * -d1;
q2W += q1W * -d1;
let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
q2X *= l;
q2Y *= l;
q2Z *= l;
q2W *= l;
let sin = Math.sin(theta);
let cos = Math.cos(theta);
q1X *= cos;
q1Y *= cos;
q1Z *= cos;
q1W *= cos;
slerpQX = q1X + q2X * sin;
slerpQY = q1Y + q2Y * sin;
slerpQZ = q1Z + q2Z * sin;
slerpQW = q1W + q2W * sin;
}
let x = slerpQX;
let y = slerpQY;
let z = slerpQZ;
let w = slerpQW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
slerpM00 = 1 - yy - zz;
slerpM01 = xy - wz;
slerpM02 = xz + wy;
slerpM10 = xy + wz;
slerpM11 = 1 - xx - zz;
slerpM12 = yz - wx;
slerpM20 = xz - wy;
slerpM21 = yz + wx;
slerpM22 = 1 - xx - yy;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = slerpM00 * _this.joint._basisX1X + slerpM01 * _this.joint._basisX1Y + slerpM02 * _this.joint._basisX1Z;
__tmp__Y = slerpM10 * _this.joint._basisX1X + slerpM11 * _this.joint._basisX1Y + slerpM12 * _this.joint._basisX1Z;
__tmp__Z = slerpM20 * _this.joint._basisX1X + slerpM21 * _this.joint._basisX1Y + slerpM22 * _this.joint._basisX1Z;
newXX = __tmp__X;
newXY = __tmp__Y;
newXZ = __tmp__Z;
prevXX = _this.xX;
prevXY = _this.xY;
prevXZ = _this.xZ;
prevYX = _this.yX;
prevYY = _this.yY;
prevYZ = _this.yZ;
let d2 = prevXX * newXX + prevXY * newXY + prevXZ * newXZ;
if(d2 < -0.999999999) {
let vX;
let vY;
let vZ;
let x1 = prevXX;
let y1 = prevXY;
let z1 = prevXZ;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
vX = 0;
vY = z1 * d;
vZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
vX = -z1 * d;
vY = 0;
vZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
vX = y1 * d;
vY = -x1 * d;
vZ = 0;
}
slerpQX = vX;
slerpQY = vY;
slerpQZ = vZ;
slerpQW = 0;
} else {
let cX;
let cY;
let cZ;
cX = prevXY * newXZ - prevXZ * newXY;
cY = prevXZ * newXX - prevXX * newXZ;
cZ = prevXX * newXY - prevXY * newXX;
let w = Math.sqrt((1 + d2) * 0.5);
d2 = 0.5 / w;
cX *= d2;
cY *= d2;
cZ *= d2;
slerpQX = cX;
slerpQY = cY;
slerpQZ = cZ;
slerpQW = w;
}
let x1 = slerpQX;
let y1 = slerpQY;
let z1 = slerpQZ;
let w1 = slerpQW;
let x21 = 2 * x1;
let y21 = 2 * y1;
let z21 = 2 * z1;
let xx1 = x1 * x21;
let yy1 = y1 * y21;
let zz1 = z1 * z21;
let xy1 = x1 * y21;
let yz1 = y1 * z21;
let xz1 = x1 * z21;
let wx1 = w1 * x21;
let wy1 = w1 * y21;
let wz1 = w1 * z21;
slerpM00 = 1 - yy1 - zz1;
slerpM01 = xy1 - wz1;
slerpM02 = xz1 + wy1;
slerpM10 = xy1 + wz1;
slerpM11 = 1 - xx1 - zz1;
slerpM12 = yz1 - wx1;
slerpM20 = xz1 - wy1;
slerpM21 = yz1 + wx1;
slerpM22 = 1 - xx1 - yy1;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = slerpM00 * prevYX + slerpM01 * prevYY + slerpM02 * prevYZ;
__tmp__Y1 = slerpM10 * prevYX + slerpM11 * prevYY + slerpM12 * prevYZ;
__tmp__Z1 = slerpM20 * prevYX + slerpM21 * prevYY + slerpM22 * prevYZ;
newYX = __tmp__X1;
newYY = __tmp__Y1;
newYZ = __tmp__Z1;
newZX = newXY * newYZ - newXZ * newYY;
newZY = newXZ * newYX - newXX * newYZ;
newZZ = newXX * newYY - newXY * newYX;
if(newZX * newZX + newZY * newZY + newZZ * newZZ > 1e-6) {
let l = newZX * newZX + newZY * newZY + newZZ * newZZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
newZX *= l;
newZY *= l;
newZZ *= l;
} else {
let x1 = newXX;
let y1 = newXY;
let z1 = newXZ;
let x2 = x1 * x1;
let y2 = y1 * y1;
let z2 = z1 * z1;
let d;
if(x2 < y2) {
if(x2 < z2) {
d = 1 / Math.sqrt(y2 + z2);
newZX = 0;
newZY = z1 * d;
newZZ = -y1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
newZX = y1 * d;
newZY = -x1 * d;
newZZ = 0;
}
} else if(y2 < z2) {
d = 1 / Math.sqrt(z2 + x2);
newZX = -z1 * d;
newZY = 0;
newZZ = x1 * d;
} else {
d = 1 / Math.sqrt(x2 + y2);
newZX = y1 * d;
newZY = -x1 * d;
newZZ = 0;
}
}
newYX = newZY * newXZ - newZZ * newXY;
newYY = newZZ * newXX - newZX * newXZ;
newYZ = newZX * newXY - newZY * newXX;
_this.xX = newXX;
_this.xY = newXY;
_this.xZ = newXZ;
_this.yX = newYX;
_this.yY = newYY;
_this.yZ = newYZ;
_this.zX = newZX;
_this.zY = newZY;
_this.zZ = newZZ;
let angErrorX;
let angErrorY;
let angErrorZ;
angErrorX = this._basisX1Y * this._basisX2Z - this._basisX1Z * this._basisX2Y;
angErrorY = this._basisX1Z * this._basisX2X - this._basisX1X * this._basisX2Z;
angErrorZ = this._basisX1X * this._basisX2Y - this._basisX1Y * this._basisX2X;
let cos = this._basisX1X * this._basisX2X + this._basisX1Y * this._basisX2Y + this._basisX1Z * this._basisX2Z;
let theta = cos <= -1 ? 3.14159265358979 : cos >= 1 ? 0 : Math.acos(cos);
let l = angErrorX * angErrorX + angErrorY * angErrorY + angErrorZ * angErrorZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
angErrorX *= l;
angErrorY *= l;
angErrorZ *= l;
angErrorX *= theta;
angErrorY *= theta;
angErrorZ *= theta;
this.angularErrorY = angErrorX * this._basis.yX + angErrorY * this._basis.yY + angErrorZ * this._basis.yZ;
this.angularErrorZ = angErrorX * this._basis.zX + angErrorY * this._basis.zY + angErrorZ * this._basis.zZ;
let perpCrossX;
let perpCrossY;
let perpCrossZ;
perpCrossX = this._basisY1Y * this._basisY2Z - this._basisY1Z * this._basisY2Y;
perpCrossY = this._basisY1Z * this._basisY2X - this._basisY1X * this._basisY2Z;
perpCrossZ = this._basisY1X * this._basisY2Y - this._basisY1Y * this._basisY2X;
cos = this._basisY1X * this._basisY2X + this._basisY1Y * this._basisY2Y + this._basisY1Z * this._basisY2Z;
this.angle = cos <= -1 ? 3.14159265358979 : cos >= 1 ? 0 : Math.acos(cos);
if(perpCrossX * this._basis.xX + perpCrossY * this._basis.xY + perpCrossZ * this._basis.xZ < 0) {
this.angle = -this.angle;
}
this.linearErrorX = this._anchor2X - this._anchor1X;
this.linearErrorY = this._anchor2Y - this._anchor1Y;
this.linearErrorZ = this._anchor2Z - this._anchor1Z;
}
_getVelocitySolverInfo(timeStep,info) {
super._getVelocitySolverInfo(timeStep,info);
this.getInfo(info,timeStep,false);
}
_getPositionSolverInfo(info) {
super._getPositionSolverInfo(info);
this.getInfo(info,null,true);
}
getAxis1() {
let v = new oimo.common.Vec3();
v.x = this._basisX1X;
v.y = this._basisX1Y;
v.z = this._basisX1Z;
return v;
}
getAxis2() {
let v = new oimo.common.Vec3();
v.x = this._basisX2X;
v.y = this._basisX2Y;
v.z = this._basisX2Z;
return v;
}
getAxis1To(axis) {
axis.x = this._basisX1X;
axis.y = this._basisX1Y;
axis.z = this._basisX1Z;
}
getAxis2To(axis) {
axis.x = this._basisX2X;
axis.y = this._basisX2Y;
axis.z = this._basisX2Z;
}
getLocalAxis1() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX1X;
v.y = this._localBasisX1Y;
v.z = this._localBasisX1Z;
return v;
}
getLocalAxis2() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX2X;
v.y = this._localBasisX2Y;
v.z = this._localBasisX2Z;
return v;
}
getLocalAxis1To(axis) {
axis.x = this._localBasisX1X;
axis.y = this._localBasisX1Y;
axis.z = this._localBasisX1Z;
}
getLocalAxis2To(axis) {
axis.x = this._localBasisX2X;
axis.y = this._localBasisX2Y;
axis.z = this._localBasisX2Z;
}
getSpringDamper() {
return this._sd;
}
getLimitMotor() {
return this._lm;
}
getAngle() {
return this.angle;
}
}
oimo.dynamics.constraint.joint.RevoluteJointConfig = class oimo_dynamics_constraint_joint_RevoluteJointConfig extends oimo.dynamics.constraint.joint.JointConfig {
constructor() {
super();
this.localAxis1 = new oimo.common.Vec3(1,0,0);
this.localAxis2 = new oimo.common.Vec3(1,0,0);
this.springDamper = new oimo.dynamics.constraint.joint.SpringDamper();
this.limitMotor = new oimo.dynamics.constraint.joint.RotationalLimitMotor();
}
init(rigidBody1,rigidBody2,worldAnchor,worldAxis) {
this._init(rigidBody1,rigidBody2,worldAnchor);
let localVector = this.localAxis1;
let vX;
let vY;
let vZ;
vX = worldAxis.x;
vY = worldAxis.y;
vZ = worldAxis.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = rigidBody1._transform._rotation00 * vX + rigidBody1._transform._rotation10 * vY + rigidBody1._transform._rotation20 * vZ;
__tmp__Y = rigidBody1._transform._rotation01 * vX + rigidBody1._transform._rotation11 * vY + rigidBody1._transform._rotation21 * vZ;
__tmp__Z = rigidBody1._transform._rotation02 * vX + rigidBody1._transform._rotation12 * vY + rigidBody1._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
localVector.x = vX;
localVector.y = vY;
localVector.z = vZ;
let localVector1 = this.localAxis2;
let vX1;
let vY1;
let vZ1;
vX1 = worldAxis.x;
vY1 = worldAxis.y;
vZ1 = worldAxis.z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = rigidBody2._transform._rotation00 * vX1 + rigidBody2._transform._rotation10 * vY1 + rigidBody2._transform._rotation20 * vZ1;
__tmp__Y1 = rigidBody2._transform._rotation01 * vX1 + rigidBody2._transform._rotation11 * vY1 + rigidBody2._transform._rotation21 * vZ1;
__tmp__Z1 = rigidBody2._transform._rotation02 * vX1 + rigidBody2._transform._rotation12 * vY1 + rigidBody2._transform._rotation22 * vZ1;
vX1 = __tmp__X1;
vY1 = __tmp__Y1;
vZ1 = __tmp__Z1;
localVector1.x = vX1;
localVector1.y = vY1;
localVector1.z = vZ1;
return this;
}
}
oimo.dynamics.constraint.joint.RotationalLimitMotor = class oimo_dynamics_constraint_joint_RotationalLimitMotor {
constructor() {
this.lowerLimit = 1;
this.upperLimit = 0;
this.motorTorque = 0;
}
setLimits(lower,upper) {
this.lowerLimit = lower;
this.upperLimit = upper;
return this;
}
setMotor(speed,torque) {
this.motorSpeed = speed;
this.motorTorque = torque;
return this;
}
clone() {
let lm = new oimo.dynamics.constraint.joint.RotationalLimitMotor();
lm.lowerLimit = this.lowerLimit;
lm.upperLimit = this.upperLimit;
lm.motorSpeed = this.motorSpeed;
lm.motorTorque = this.motorTorque;
return lm;
}
}
oimo.dynamics.constraint.joint.SphericalJoint = class oimo_dynamics_constraint_joint_SphericalJoint extends oimo.dynamics.constraint.joint.Joint {
constructor(config) {
super(config,0);
this._sd = config.springDamper.clone();
}
getInfo(info,timeStep,isPositionPart) {
if(this._sd.frequency > 0 && isPositionPart) {
return;
}
let errorX;
let errorY;
let errorZ;
errorX = this._anchor2X - this._anchor1X;
errorY = this._anchor2Y - this._anchor1Y;
errorZ = this._anchor2Z - this._anchor1Z;
let cfm;
let erp;
if(this._sd.frequency > 0) {
let omega = 6.28318530717958 * this._sd.frequency;
let zeta = this._sd.dampingRatio;
if(zeta < oimo.common.Setting.minSpringDamperDampingRatio) {
zeta = oimo.common.Setting.minSpringDamperDampingRatio;
}
let h = timeStep.dt;
let c = 2 * zeta * omega;
let k = omega * omega;
if(this._sd.useSymplecticEuler) {
cfm = 1 / (h * c);
erp = k / c;
} else {
cfm = 1 / (h * (h * k + c));
erp = k / (h * k + c);
}
cfm *= this._b1._invMass + this._b2._invMass;
} else {
cfm = 0;
erp = this.getErp(timeStep,isPositionPart);
}
let linearRhsX;
let linearRhsY;
let linearRhsZ;
linearRhsX = errorX * erp;
linearRhsY = errorY * erp;
linearRhsZ = errorZ * erp;
let crossR100;
let crossR101;
let crossR102;
let crossR110;
let crossR111;
let crossR112;
let crossR120;
let crossR121;
let crossR122;
let crossR200;
let crossR201;
let crossR202;
let crossR210;
let crossR211;
let crossR212;
let crossR220;
let crossR221;
let crossR222;
crossR100 = 0;
crossR101 = -this._relativeAnchor1Z;
crossR102 = this._relativeAnchor1Y;
crossR110 = this._relativeAnchor1Z;
crossR111 = 0;
crossR112 = -this._relativeAnchor1X;
crossR120 = -this._relativeAnchor1Y;
crossR121 = this._relativeAnchor1X;
crossR122 = 0;
crossR200 = 0;
crossR201 = -this._relativeAnchor2Z;
crossR202 = this._relativeAnchor2Y;
crossR210 = this._relativeAnchor2Z;
crossR211 = 0;
crossR212 = -this._relativeAnchor2X;
crossR220 = -this._relativeAnchor2Y;
crossR221 = this._relativeAnchor2X;
crossR222 = 0;
crossR100 = -crossR100;
crossR101 = -crossR101;
crossR102 = -crossR102;
crossR110 = -crossR110;
crossR111 = -crossR111;
crossR112 = -crossR112;
crossR120 = -crossR120;
crossR121 = -crossR121;
crossR122 = -crossR122;
crossR200 = -crossR200;
crossR201 = -crossR201;
crossR202 = -crossR202;
crossR210 = -crossR210;
crossR211 = -crossR211;
crossR212 = -crossR212;
crossR220 = -crossR220;
crossR221 = -crossR221;
crossR222 = -crossR222;
let impulse = this._impulses[0];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
row.rhs = linearRhsX;
row.cfm = cfm;
row.minImpulse = -1e65536;
row.maxImpulse = 1e65536;
let j = row.jacobian;
j.lin1X = 1;
j.lin1Y = 0;
j.lin1Z = 0;
j.lin2X = 1;
j.lin2Y = 0;
j.lin2Z = 0;
j.ang1X = crossR100;
j.ang1Y = crossR101;
j.ang1Z = crossR102;
j.ang2X = crossR200;
j.ang2Y = crossR201;
j.ang2Z = crossR202;
let impulse1 = this._impulses[1];
let row1 = info.rows[info.numRows++];
let _this1 = row1.jacobian;
_this1.lin1X = 0;
_this1.lin1Y = 0;
_this1.lin1Z = 0;
_this1.lin2X = 0;
_this1.lin2Y = 0;
_this1.lin2Z = 0;
_this1.ang1X = 0;
_this1.ang1Y = 0;
_this1.ang1Z = 0;
_this1.ang2X = 0;
_this1.ang2Y = 0;
_this1.ang2Z = 0;
row1.rhs = 0;
row1.cfm = 0;
row1.minImpulse = 0;
row1.maxImpulse = 0;
row1.motorSpeed = 0;
row1.motorMaxImpulse = 0;
row1.impulse = null;
row1.impulse = impulse1;
row1.rhs = linearRhsY;
row1.cfm = cfm;
row1.minImpulse = -1e65536;
row1.maxImpulse = 1e65536;
j = row1.jacobian;
j.lin1X = 0;
j.lin1Y = 1;
j.lin1Z = 0;
j.lin2X = 0;
j.lin2Y = 1;
j.lin2Z = 0;
j.ang1X = crossR110;
j.ang1Y = crossR111;
j.ang1Z = crossR112;
j.ang2X = crossR210;
j.ang2Y = crossR211;
j.ang2Z = crossR212;
let impulse2 = this._impulses[2];
let row2 = info.rows[info.numRows++];
let _this2 = row2.jacobian;
_this2.lin1X = 0;
_this2.lin1Y = 0;
_this2.lin1Z = 0;
_this2.lin2X = 0;
_this2.lin2Y = 0;
_this2.lin2Z = 0;
_this2.ang1X = 0;
_this2.ang1Y = 0;
_this2.ang1Z = 0;
_this2.ang2X = 0;
_this2.ang2Y = 0;
_this2.ang2Z = 0;
row2.rhs = 0;
row2.cfm = 0;
row2.minImpulse = 0;
row2.maxImpulse = 0;
row2.motorSpeed = 0;
row2.motorMaxImpulse = 0;
row2.impulse = null;
row2.impulse = impulse2;
row2.rhs = linearRhsZ;
row2.cfm = cfm;
row2.minImpulse = -1e65536;
row2.maxImpulse = 1e65536;
j = row2.jacobian;
j.lin1X = 0;
j.lin1Y = 0;
j.lin1Z = 1;
j.lin2X = 0;
j.lin2Y = 0;
j.lin2Z = 1;
j.ang1X = crossR120;
j.ang1Y = crossR121;
j.ang1Z = crossR122;
j.ang2X = crossR220;
j.ang2Y = crossR221;
j.ang2Z = crossR222;
}
_getVelocitySolverInfo(timeStep,info) {
super._getVelocitySolverInfo(timeStep,info);
this.getInfo(info,timeStep,false);
}
_getPositionSolverInfo(info) {
super._getPositionSolverInfo(info);
this.getInfo(info,null,true);
}
getSpringDamper() {
return this._sd;
}
}
oimo.dynamics.constraint.joint.SphericalJointConfig = class oimo_dynamics_constraint_joint_SphericalJointConfig extends oimo.dynamics.constraint.joint.JointConfig {
constructor() {
super();
this.springDamper = new oimo.dynamics.constraint.joint.SpringDamper();
}
init(rigidBody1,rigidBody2,worldAnchor) {
this._init(rigidBody1,rigidBody2,worldAnchor);
return this;
}
}
oimo.dynamics.constraint.joint.SpringDamper = class oimo_dynamics_constraint_joint_SpringDamper {
constructor() {
this.frequency = 0;
this.dampingRatio = 0;
this.useSymplecticEuler = false;
}
setSpring(frequency,dampingRatio) {
this.frequency = frequency;
this.dampingRatio = dampingRatio;
return this;
}
setSymplecticEuler(useSymplecticEuler) {
this.useSymplecticEuler = useSymplecticEuler;
return this;
}
clone() {
let sd = new oimo.dynamics.constraint.joint.SpringDamper();
sd.frequency = this.frequency;
sd.dampingRatio = this.dampingRatio;
sd.useSymplecticEuler = this.useSymplecticEuler;
return sd;
}
}
oimo.dynamics.constraint.joint.TranslationalLimitMotor = class oimo_dynamics_constraint_joint_TranslationalLimitMotor {
constructor() {
this.lowerLimit = 1;
this.upperLimit = 0;
this.motorForce = 0;
}
setLimits(lower,upper) {
this.lowerLimit = lower;
this.upperLimit = upper;
return this;
}
setMotor(speed,force) {
this.motorSpeed = speed;
this.motorForce = force;
return this;
}
clone() {
let lm = new oimo.dynamics.constraint.joint.TranslationalLimitMotor();
lm.lowerLimit = this.lowerLimit;
lm.upperLimit = this.upperLimit;
lm.motorSpeed = this.motorSpeed;
lm.motorForce = this.motorForce;
return lm;
}
}
oimo.dynamics.constraint.joint.UniversalJoint = class oimo_dynamics_constraint_joint_UniversalJoint extends oimo.dynamics.constraint.joint.Joint {
constructor(config) {
super(config,oimo.dynamics.constraint.joint.JointType.UNIVERSAL);
let v = config.localAxis1;
this._localBasisX1X = v.x;
this._localBasisX1Y = v.y;
this._localBasisX1Z = v.z;
let v1 = config.localAxis2;
this._localBasisZ2X = v1.x;
this._localBasisZ2Y = v1.y;
this._localBasisZ2Z = v1.z;
this.buildLocalBasesFromX1Z2();
this._angleX = 0;
this._angleY = 0;
this._angleZ = 0;
this.xSingular = false;
this.ySingular = false;
this.zSingular = false;
this._sd1 = config.springDamper1.clone();
this._sd2 = config.springDamper2.clone();
this._lm1 = config.limitMotor1.clone();
this._lm2 = config.limitMotor2.clone();
}
getInfo(info,timeStep,isPositionPart) {
let erp = this.getErp(timeStep,isPositionPart);
let linearRhsX;
let linearRhsY;
let linearRhsZ;
linearRhsX = this.linearErrorX * erp;
linearRhsY = this.linearErrorY * erp;
linearRhsZ = this.linearErrorZ * erp;
let angRhsY = this._angleY * erp;
let crossR100;
let crossR101;
let crossR102;
let crossR110;
let crossR111;
let crossR112;
let crossR120;
let crossR121;
let crossR122;
let crossR200;
let crossR201;
let crossR202;
let crossR210;
let crossR211;
let crossR212;
let crossR220;
let crossR221;
let crossR222;
crossR100 = 0;
crossR101 = -this._relativeAnchor1Z;
crossR102 = this._relativeAnchor1Y;
crossR110 = this._relativeAnchor1Z;
crossR111 = 0;
crossR112 = -this._relativeAnchor1X;
crossR120 = -this._relativeAnchor1Y;
crossR121 = this._relativeAnchor1X;
crossR122 = 0;
crossR200 = 0;
crossR201 = -this._relativeAnchor2Z;
crossR202 = this._relativeAnchor2Y;
crossR210 = this._relativeAnchor2Z;
crossR211 = 0;
crossR212 = -this._relativeAnchor2X;
crossR220 = -this._relativeAnchor2Y;
crossR221 = this._relativeAnchor2X;
crossR222 = 0;
crossR100 = -crossR100;
crossR101 = -crossR101;
crossR102 = -crossR102;
crossR110 = -crossR110;
crossR111 = -crossR111;
crossR112 = -crossR112;
crossR120 = -crossR120;
crossR121 = -crossR121;
crossR122 = -crossR122;
crossR200 = -crossR200;
crossR201 = -crossR201;
crossR202 = -crossR202;
crossR210 = -crossR210;
crossR211 = -crossR211;
crossR212 = -crossR212;
crossR220 = -crossR220;
crossR221 = -crossR221;
crossR222 = -crossR222;
let motorMassX = this.computeEffectiveInertiaMoment(this._axisXX,this._axisXY,this._axisXZ);
let motorMassZ = this.computeEffectiveInertiaMoment(this._axisZX,this._axisZY,this._axisZZ);
let impulse = this._impulses[0];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
row.rhs = linearRhsX;
row.cfm = 0;
row.minImpulse = -1e65536;
row.maxImpulse = 1e65536;
let j = row.jacobian;
j.lin1X = 1;
j.lin1Y = 0;
j.lin1Z = 0;
j.lin2X = 1;
j.lin2Y = 0;
j.lin2Z = 0;
j.ang1X = crossR100;
j.ang1Y = crossR101;
j.ang1Z = crossR102;
j.ang2X = crossR200;
j.ang2Y = crossR201;
j.ang2Z = crossR202;
let impulse1 = this._impulses[1];
let row1 = info.rows[info.numRows++];
let _this1 = row1.jacobian;
_this1.lin1X = 0;
_this1.lin1Y = 0;
_this1.lin1Z = 0;
_this1.lin2X = 0;
_this1.lin2Y = 0;
_this1.lin2Z = 0;
_this1.ang1X = 0;
_this1.ang1Y = 0;
_this1.ang1Z = 0;
_this1.ang2X = 0;
_this1.ang2Y = 0;
_this1.ang2Z = 0;
row1.rhs = 0;
row1.cfm = 0;
row1.minImpulse = 0;
row1.maxImpulse = 0;
row1.motorSpeed = 0;
row1.motorMaxImpulse = 0;
row1.impulse = null;
row1.impulse = impulse1;
row1.rhs = linearRhsY;
row1.cfm = 0;
row1.minImpulse = -1e65536;
row1.maxImpulse = 1e65536;
j = row1.jacobian;
j.lin1X = 0;
j.lin1Y = 1;
j.lin1Z = 0;
j.lin2X = 0;
j.lin2Y = 1;
j.lin2Z = 0;
j.ang1X = crossR110;
j.ang1Y = crossR111;
j.ang1Z = crossR112;
j.ang2X = crossR210;
j.ang2Y = crossR211;
j.ang2Z = crossR212;
let impulse2 = this._impulses[2];
let row2 = info.rows[info.numRows++];
let _this2 = row2.jacobian;
_this2.lin1X = 0;
_this2.lin1Y = 0;
_this2.lin1Z = 0;
_this2.lin2X = 0;
_this2.lin2Y = 0;
_this2.lin2Z = 0;
_this2.ang1X = 0;
_this2.ang1Y = 0;
_this2.ang1Z = 0;
_this2.ang2X = 0;
_this2.ang2Y = 0;
_this2.ang2Z = 0;
row2.rhs = 0;
row2.cfm = 0;
row2.minImpulse = 0;
row2.maxImpulse = 0;
row2.motorSpeed = 0;
row2.motorMaxImpulse = 0;
row2.impulse = null;
row2.impulse = impulse2;
row2.rhs = linearRhsZ;
row2.cfm = 0;
row2.minImpulse = -1e65536;
row2.maxImpulse = 1e65536;
j = row2.jacobian;
j.lin1X = 0;
j.lin1Y = 0;
j.lin1Z = 1;
j.lin2X = 0;
j.lin2Y = 0;
j.lin2Z = 1;
j.ang1X = crossR120;
j.ang1Y = crossR121;
j.ang1Z = crossR122;
j.ang2X = crossR220;
j.ang2Y = crossR221;
j.ang2Z = crossR222;
if(!this.xSingular && (this._sd1.frequency <= 0 || !isPositionPart)) {
let impulse = this._impulses[3];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this._angleX,this._lm1,motorMassX,this._sd1,timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this._axisXX;
j.ang1Y = this._axisXY;
j.ang1Z = this._axisXZ;
j.ang2X = this._axisXX;
j.ang2Y = this._axisXY;
j.ang2Z = this._axisXZ;
}
if(!this.ySingular) {
let impulse = this._impulses[4];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
row.rhs = angRhsY;
row.cfm = 0;
row.minImpulse = -1e65536;
row.maxImpulse = 1e65536;
j = row.jacobian;
j.ang1X = this._axisYX;
j.ang1Y = this._axisYY;
j.ang1Z = this._axisYZ;
j.ang2X = this._axisYX;
j.ang2Y = this._axisYY;
j.ang2Z = this._axisYZ;
}
if(!this.zSingular && (this._sd2.frequency <= 0 || !isPositionPart)) {
let impulse = this._impulses[5];
let row = info.rows[info.numRows++];
let _this = row.jacobian;
_this.lin1X = 0;
_this.lin1Y = 0;
_this.lin1Z = 0;
_this.lin2X = 0;
_this.lin2Y = 0;
_this.lin2Z = 0;
_this.ang1X = 0;
_this.ang1Y = 0;
_this.ang1Z = 0;
_this.ang2X = 0;
_this.ang2Y = 0;
_this.ang2Z = 0;
row.rhs = 0;
row.cfm = 0;
row.minImpulse = 0;
row.maxImpulse = 0;
row.motorSpeed = 0;
row.motorMaxImpulse = 0;
row.impulse = null;
row.impulse = impulse;
this.setSolverInfoRowAngular(row,this._angleZ,this._lm2,motorMassZ,this._sd2,timeStep,isPositionPart);
j = row.jacobian;
j.ang1X = this._axisZX;
j.ang1Y = this._axisZY;
j.ang1Z = this._axisZZ;
j.ang2X = this._axisZX;
j.ang2Y = this._axisZY;
j.ang2Z = this._axisZZ;
}
}
_syncAnchors() {
super._syncAnchors();
let angleAxisXX;
let angleAxisXY;
let angleAxisXZ;
let angleAxisYX;
let angleAxisYY;
let angleAxisYZ;
let angleAxisZX;
let angleAxisZY;
let angleAxisZZ;
angleAxisXX = this._basisX1X;
angleAxisXY = this._basisX1Y;
angleAxisXZ = this._basisX1Z;
angleAxisZX = this._basisZ2X;
angleAxisZY = this._basisZ2Y;
angleAxisZZ = this._basisZ2Z;
angleAxisYX = angleAxisZY * angleAxisXZ - angleAxisZZ * angleAxisXY;
angleAxisYY = angleAxisZZ * angleAxisXX - angleAxisZX * angleAxisXZ;
angleAxisYZ = angleAxisZX * angleAxisXY - angleAxisZY * angleAxisXX;
this._axisXX = angleAxisYY * angleAxisZZ - angleAxisYZ * angleAxisZY;
this._axisXY = angleAxisYZ * angleAxisZX - angleAxisYX * angleAxisZZ;
this._axisXZ = angleAxisYX * angleAxisZY - angleAxisYY * angleAxisZX;
this._axisYX = angleAxisYX;
this._axisYY = angleAxisYY;
this._axisYZ = angleAxisYZ;
this._axisZX = angleAxisXY * angleAxisYZ - angleAxisXZ * angleAxisYY;
this._axisZY = angleAxisXZ * angleAxisYX - angleAxisXX * angleAxisYZ;
this._axisZZ = angleAxisXX * angleAxisYY - angleAxisXY * angleAxisYX;
let l = this._axisXX * this._axisXX + this._axisXY * this._axisXY + this._axisXZ * this._axisXZ;
if(l > 0) {
l = 1 / Math.sqrt(l);
}
this._axisXX *= l;
this._axisXY *= l;
this._axisXZ *= l;
let l1 = this._axisYX * this._axisYX + this._axisYY * this._axisYY + this._axisYZ * this._axisYZ;
if(l1 > 0) {
l1 = 1 / Math.sqrt(l1);
}
this._axisYX *= l1;
this._axisYY *= l1;
this._axisYZ *= l1;
let l2 = this._axisZX * this._axisZX + this._axisZY * this._axisZY + this._axisZZ * this._axisZZ;
if(l2 > 0) {
l2 = 1 / Math.sqrt(l2);
}
this._axisZX *= l2;
this._axisZY *= l2;
this._axisZZ *= l2;
this.xSingular = this._axisXX * this._axisXX + this._axisXY * this._axisXY + this._axisXZ * this._axisXZ == 0;
this.ySingular = this._axisYX * this._axisYX + this._axisYY * this._axisYY + this._axisYZ * this._axisYZ == 0;
this.zSingular = this._axisZX * this._axisZX + this._axisZY * this._axisZY + this._axisZZ * this._axisZZ == 0;
let rot100;
let rot101;
let rot102;
let rot110;
let rot111;
let rot112;
let rot120;
let rot121;
let rot122;
let rot200;
let rot201;
let rot202;
let rot210;
let rot211;
let rot212;
let rot220;
let rot221;
let rot222;
rot100 = this._basisX1X;
rot101 = this._basisY1X;
rot102 = this._basisZ1X;
rot110 = this._basisX1Y;
rot111 = this._basisY1Y;
rot112 = this._basisZ1Y;
rot120 = this._basisX1Z;
rot121 = this._basisY1Z;
rot122 = this._basisZ1Z;
rot200 = this._basisX2X;
rot201 = this._basisY2X;
rot202 = this._basisZ2X;
rot210 = this._basisX2Y;
rot211 = this._basisY2Y;
rot212 = this._basisZ2Y;
rot220 = this._basisX2Z;
rot221 = this._basisY2Z;
rot222 = this._basisZ2Z;
let relRot00;
let relRot01;
let relRot02;
let relRot11;
let relRot12;
let relRot21;
let relRot22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__11;
let __tmp__12;
let __tmp__21;
let __tmp__22;
__tmp__00 = rot100 * rot200 + rot110 * rot210 + rot120 * rot220;
__tmp__01 = rot100 * rot201 + rot110 * rot211 + rot120 * rot221;
__tmp__02 = rot100 * rot202 + rot110 * rot212 + rot120 * rot222;
__tmp__11 = rot101 * rot201 + rot111 * rot211 + rot121 * rot221;
__tmp__12 = rot101 * rot202 + rot111 * rot212 + rot121 * rot222;
__tmp__21 = rot102 * rot201 + rot112 * rot211 + rot122 * rot221;
__tmp__22 = rot102 * rot202 + rot112 * rot212 + rot122 * rot222;
relRot00 = __tmp__00;
relRot01 = __tmp__01;
relRot02 = __tmp__02;
relRot11 = __tmp__11;
relRot12 = __tmp__12;
relRot21 = __tmp__21;
relRot22 = __tmp__22;
let anglesX;
let anglesY;
let anglesZ;
let sy = relRot02;
if(sy <= -1) {
let xSubZ = Math.atan2(relRot21,relRot11);
anglesX = xSubZ * 0.5;
anglesY = -1.570796326794895;
anglesZ = -xSubZ * 0.5;
} else if(sy >= 1) {
let xAddZ = Math.atan2(relRot21,relRot11);
anglesX = xAddZ * 0.5;
anglesY = 1.570796326794895;
anglesZ = xAddZ * 0.5;
} else {
anglesX = Math.atan2(-relRot12,relRot22);
anglesY = Math.asin(sy);
anglesZ = Math.atan2(-relRot01,relRot00);
}
this._angleX = anglesX;
this._angleY = anglesY;
this._angleZ = anglesZ;
this.linearErrorX = this._anchor2X - this._anchor1X;
this.linearErrorY = this._anchor2Y - this._anchor1Y;
this.linearErrorZ = this._anchor2Z - this._anchor1Z;
}
_getVelocitySolverInfo(timeStep,info) {
super._getVelocitySolverInfo(timeStep,info);
this.getInfo(info,timeStep,false);
}
_getPositionSolverInfo(info) {
super._getPositionSolverInfo(info);
this.getInfo(info,null,true);
}
getAxis1() {
let v = new oimo.common.Vec3();
v.x = this._basisX1X;
v.y = this._basisX1Y;
v.z = this._basisX1Z;
return v;
}
getAxis2() {
let v = new oimo.common.Vec3();
v.x = this._basisZ2X;
v.y = this._basisZ2Y;
v.z = this._basisZ2Z;
return v;
}
getAxis1To(axis) {
axis.x = this._basisX1X;
axis.y = this._basisX1Y;
axis.z = this._basisX1Z;
}
getAxis2To(axis) {
axis.x = this._basisZ2X;
axis.y = this._basisZ2Y;
axis.z = this._basisZ2Z;
}
getLocalAxis1() {
let v = new oimo.common.Vec3();
v.x = this._localBasisX1X;
v.y = this._localBasisX1Y;
v.z = this._localBasisX1Z;
return v;
}
getLocalAxis2() {
let v = new oimo.common.Vec3();
v.x = this._localBasisZ2X;
v.y = this._localBasisZ2Y;
v.z = this._localBasisZ2Z;
return v;
}
getLocalAxis1To(axis) {
axis.x = this._localBasisX1X;
axis.y = this._localBasisX1Y;
axis.z = this._localBasisX1Z;
}
getLocalAxis2To(axis) {
axis.x = this._localBasisZ2X;
axis.y = this._localBasisZ2Y;
axis.z = this._localBasisZ2Z;
}
getSpringDamper1() {
return this._sd1;
}
getSpringDamper2() {
return this._sd2;
}
getLimitMotor1() {
return this._lm1;
}
getLimitMotor2() {
return this._lm2;
}
getAngle1() {
return this._angleX;
}
getAngle2() {
return this._angleZ;
}
}
oimo.dynamics.constraint.joint.UniversalJointConfig = class oimo_dynamics_constraint_joint_UniversalJointConfig extends oimo.dynamics.constraint.joint.JointConfig {
constructor() {
super();
this.localAxis1 = new oimo.common.Vec3(1,0,0);
this.localAxis2 = new oimo.common.Vec3(1,0,0);
this.springDamper1 = new oimo.dynamics.constraint.joint.SpringDamper();
this.springDamper2 = new oimo.dynamics.constraint.joint.SpringDamper();
this.limitMotor1 = new oimo.dynamics.constraint.joint.RotationalLimitMotor();
this.limitMotor2 = new oimo.dynamics.constraint.joint.RotationalLimitMotor();
}
init(rigidBody1,rigidBody2,worldAnchor,worldAxis1,worldAxis2) {
this._init(rigidBody1,rigidBody2,worldAnchor);
let localVector = this.localAxis1;
let vX;
let vY;
let vZ;
vX = worldAxis1.x;
vY = worldAxis1.y;
vZ = worldAxis1.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = rigidBody1._transform._rotation00 * vX + rigidBody1._transform._rotation10 * vY + rigidBody1._transform._rotation20 * vZ;
__tmp__Y = rigidBody1._transform._rotation01 * vX + rigidBody1._transform._rotation11 * vY + rigidBody1._transform._rotation21 * vZ;
__tmp__Z = rigidBody1._transform._rotation02 * vX + rigidBody1._transform._rotation12 * vY + rigidBody1._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
localVector.x = vX;
localVector.y = vY;
localVector.z = vZ;
let localVector1 = this.localAxis2;
let vX1;
let vY1;
let vZ1;
vX1 = worldAxis2.x;
vY1 = worldAxis2.y;
vZ1 = worldAxis2.z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = rigidBody2._transform._rotation00 * vX1 + rigidBody2._transform._rotation10 * vY1 + rigidBody2._transform._rotation20 * vZ1;
__tmp__Y1 = rigidBody2._transform._rotation01 * vX1 + rigidBody2._transform._rotation11 * vY1 + rigidBody2._transform._rotation21 * vZ1;
__tmp__Z1 = rigidBody2._transform._rotation02 * vX1 + rigidBody2._transform._rotation12 * vY1 + rigidBody2._transform._rotation22 * vZ1;
vX1 = __tmp__X1;
vY1 = __tmp__Y1;
vZ1 = __tmp__Z1;
localVector1.x = vX1;
localVector1.y = vY1;
localVector1.z = vZ1;
return this;
}
}
if(!oimo.dynamics.constraint.solver) oimo.dynamics.constraint.solver = {};
oimo.dynamics.constraint.solver.ConstraintSolverType = class oimo_dynamics_constraint_solver_ConstraintSolverType {
}
if(!oimo.dynamics.constraint.solver.common) oimo.dynamics.constraint.solver.common = {};
oimo.dynamics.constraint.solver.common.ContactSolverMassDataRow = class oimo_dynamics_constraint_solver_common_ContactSolverMassDataRow {
constructor() {
this.invMLinN1X = 0;
this.invMLinN1Y = 0;
this.invMLinN1Z = 0;
this.invMLinN2X = 0;
this.invMLinN2Y = 0;
this.invMLinN2Z = 0;
this.invMAngN1X = 0;
this.invMAngN1Y = 0;
this.invMAngN1Z = 0;
this.invMAngN2X = 0;
this.invMAngN2Y = 0;
this.invMAngN2Z = 0;
this.invMLinT1X = 0;
this.invMLinT1Y = 0;
this.invMLinT1Z = 0;
this.invMLinT2X = 0;
this.invMLinT2Y = 0;
this.invMLinT2Z = 0;
this.invMAngT1X = 0;
this.invMAngT1Y = 0;
this.invMAngT1Z = 0;
this.invMAngT2X = 0;
this.invMAngT2Y = 0;
this.invMAngT2Z = 0;
this.invMLinB1X = 0;
this.invMLinB1Y = 0;
this.invMLinB1Z = 0;
this.invMLinB2X = 0;
this.invMLinB2Y = 0;
this.invMLinB2Z = 0;
this.invMAngB1X = 0;
this.invMAngB1Y = 0;
this.invMAngB1Z = 0;
this.invMAngB2X = 0;
this.invMAngB2Y = 0;
this.invMAngB2Z = 0;
this.massN = 0;
this.massTB00 = 0;
this.massTB01 = 0;
this.massTB10 = 0;
this.massTB11 = 0;
}
}
oimo.dynamics.constraint.solver.common.JointSolverMassDataRow = class oimo_dynamics_constraint_solver_common_JointSolverMassDataRow {
constructor() {
this.invMLin1X = 0;
this.invMLin1Y = 0;
this.invMLin1Z = 0;
this.invMLin2X = 0;
this.invMLin2Y = 0;
this.invMLin2Z = 0;
this.invMAng1X = 0;
this.invMAng1Y = 0;
this.invMAng1Z = 0;
this.invMAng2X = 0;
this.invMAng2Y = 0;
this.invMAng2Z = 0;
this.mass = 0;
this.massWithoutCfm = 0;
}
}
if(!oimo.dynamics.constraint.solver.direct) oimo.dynamics.constraint.solver.direct = {};
oimo.dynamics.constraint.solver.direct.Boundary = class oimo_dynamics_constraint_solver_direct_Boundary {
constructor(maxRows) {
this.iBounded = new Array(maxRows);
this.iUnbounded = new Array(maxRows);
this.signs = new Array(maxRows);
this.b = new Array(maxRows);
this.numBounded = 0;
this.numUnbounded = 0;
this.matrixId = 0;
}
init(buildInfo) {
this.numBounded = buildInfo.numBounded;
let _g = 0;
let _g1 = this.numBounded;
while(_g < _g1) {
let i = _g++;
this.iBounded[i] = buildInfo.iBounded[i];
this.signs[i] = buildInfo.signs[i];
}
this.numUnbounded = buildInfo.numUnbounded;
this.matrixId = 0;
let _g2 = 0;
let _g3 = this.numUnbounded;
while(_g2 < _g3) {
let i = _g2++;
let idx = buildInfo.iUnbounded[i];
this.iUnbounded[i] = idx;
this.matrixId |= 1 << idx;
}
}
computeImpulses(info,mass,relVels,impulses,dImpulses,impulseFactor,noCheck) {
let _g = 0;
let _g1 = this.numUnbounded;
while(_g < _g1) {
let idx = this.iUnbounded[_g++];
let row = info.rows[idx];
this.b[idx] = row.rhs * impulseFactor - relVels[idx] - row.cfm * impulses[idx];
}
let invMassWithoutCfm = mass._invMassWithoutCfm;
let _g2 = 0;
let _g3 = this.numBounded;
while(_g2 < _g3) {
let i = _g2++;
let idx = this.iBounded[i];
let sign = this.signs[i];
let row = info.rows[idx];
let dImpulse = (sign < 0 ? row.minImpulse : sign > 0 ? row.maxImpulse : 0) - impulses[idx];
dImpulses[idx] = dImpulse;
if(dImpulse != 0) {
let _g = 0;
let _g1 = this.numUnbounded;
while(_g < _g1) {
let idx2 = this.iUnbounded[_g++];
this.b[idx2] -= invMassWithoutCfm[idx][idx2] * dImpulse;
}
}
}
let indices = this.iUnbounded;
let n = this.numUnbounded;
let id = 0;
let _g4 = 0;
while(_g4 < n) id |= 1 << indices[_g4++];
let massMatrix;
if(mass._cacheComputed[id]) {
massMatrix = mass._cachedSubmatrices[id];
} else {
mass.computeSubmatrix(id,indices,n);
mass._cacheComputed[id] = true;
massMatrix = mass._cachedSubmatrices[id];
}
let ok = true;
let _g5 = 0;
let _g6 = this.numUnbounded;
while(_g5 < _g6) {
let i = _g5++;
let idx = this.iUnbounded[i];
let row = info.rows[idx];
let oldImpulse = impulses[idx];
let impulse = oldImpulse;
let _g = 0;
let _g1 = this.numUnbounded;
while(_g < _g1) {
let j = _g++;
impulse += this.b[this.iUnbounded[j]] * massMatrix[i][j];
}
if(impulse < row.minImpulse - oimo.common.Setting.directMlcpSolverEps || impulse > row.maxImpulse + oimo.common.Setting.directMlcpSolverEps) {
ok = false;
break;
}
dImpulses[idx] = impulse - oldImpulse;
}
if(noCheck) {
return true;
}
if(!ok) {
return false;
}
let _g7 = 0;
let _g8 = this.numBounded;
while(_g7 < _g8) {
let i = _g7++;
let idx = this.iBounded[i];
let row = info.rows[idx];
let sign = this.signs[i];
let error = 0;
let newImpulse = impulses[idx] + dImpulses[idx];
let relVel = relVels[idx];
let _g = 0;
let _g1 = info.numRows;
while(_g < _g1) {
let j = _g++;
relVel += invMassWithoutCfm[idx][j] * dImpulses[j];
}
error = row.rhs * impulseFactor - relVel - row.cfm * newImpulse;
if(sign < 0 && error > oimo.common.Setting.directMlcpSolverEps || sign > 0 && error < -oimo.common.Setting.directMlcpSolverEps) {
ok = false;
break;
}
}
return ok;
}
}
oimo.dynamics.constraint.solver.direct.BoundaryBuildInfo = class oimo_dynamics_constraint_solver_direct_BoundaryBuildInfo {
constructor(size) {
this.size = size;
this.numBounded = 0;
this.iBounded = new Array(size);
this.signs = new Array(size);
this.numUnbounded = 0;
this.iUnbounded = new Array(size);
}
}
oimo.dynamics.constraint.solver.direct.BoundaryBuilder = class oimo_dynamics_constraint_solver_direct_BoundaryBuilder {
constructor(maxRows) {
this.maxRows = maxRows;
this.numBoundaries = 0;
this.boundaries = new Array(1 << maxRows);
this.bbInfo = new oimo.dynamics.constraint.solver.direct.BoundaryBuildInfo(maxRows);
}
buildBoundariesRecursive(info,i) {
if(i == info.numRows) {
if(this.boundaries[this.numBoundaries] == null) {
this.boundaries[this.numBoundaries] = new oimo.dynamics.constraint.solver.direct.Boundary(this.maxRows);
}
this.boundaries[this.numBoundaries++].init(this.bbInfo);
return;
}
let row = info.rows[i];
let lowerLimitEnabled = row.minImpulse > -1e65536;
let upperLimitEnabled = row.maxImpulse < 1e65536;
if(row.minImpulse == 0 && row.maxImpulse == 0) {
let _this = this.bbInfo;
_this.iBounded[_this.numBounded] = i;
_this.signs[_this.numBounded] = 0;
_this.numBounded++;
this.buildBoundariesRecursive(info,i + 1);
this.bbInfo.numBounded--;
return;
}
let _this = this.bbInfo;
_this.iUnbounded[_this.numUnbounded] = i;
_this.numUnbounded++;
this.buildBoundariesRecursive(info,i + 1);
this.bbInfo.numUnbounded--;
if(lowerLimitEnabled) {
let _this = this.bbInfo;
_this.iBounded[_this.numBounded] = i;
_this.signs[_this.numBounded] = -1;
_this.numBounded++;
this.buildBoundariesRecursive(info,i + 1);
this.bbInfo.numBounded--;
}
if(upperLimitEnabled) {
let _this = this.bbInfo;
_this.iBounded[_this.numBounded] = i;
_this.signs[_this.numBounded] = 1;
_this.numBounded++;
this.buildBoundariesRecursive(info,i + 1);
this.bbInfo.numBounded--;
}
}
buildBoundaries(info) {
this.numBoundaries = 0;
let _this = this.bbInfo;
_this.numBounded = 0;
_this.numUnbounded = 0;
this.buildBoundariesRecursive(info,0);
}
}
oimo.dynamics.constraint.solver.direct.BoundarySelector = class oimo_dynamics_constraint_solver_direct_BoundarySelector {
constructor(n) {
this.n = n;
this.indices = new Array(n);
this.tmpIndices = new Array(n);
let _g = 0;
while(_g < n) {
let i = _g++;
this.indices[i] = i;
}
}
getIndex(i) {
return this.indices[i];
}
select(index) {
let i = 0;
while(this.indices[i] != index) ++i;
while(i > 0) {
let tmp = this.indices[i];
this.indices[i] = this.indices[i - 1];
this.indices[i - 1] = tmp;
--i;
}
}
setSize(size) {
let numSmaller = 0;
let numGreater = 0;
let _g = 0;
let _g1 = this.n;
while(_g < _g1) {
let idx = this.indices[_g++];
if(idx < size) {
this.tmpIndices[numSmaller] = idx;
++numSmaller;
} else {
this.tmpIndices[size + numGreater] = idx;
++numGreater;
}
}
let tmp = this.indices;
this.indices = this.tmpIndices;
this.tmpIndices = tmp;
}
}
oimo.dynamics.constraint.solver.direct.DirectJointConstraintSolver = class oimo_dynamics_constraint_solver_direct_DirectJointConstraintSolver extends oimo.dynamics.constraint.ConstraintSolver {
constructor(joint) {
super();
this.joint = joint;
this.info = new oimo.dynamics.constraint.info.joint.JointSolverInfo();
let maxRows = oimo.common.Setting.maxJacobianRows;
this.massMatrix = new oimo.dynamics.constraint.solver.direct.MassMatrix(maxRows);
this.boundaryBuilder = new oimo.dynamics.constraint.solver.direct.BoundaryBuilder(maxRows);
this.massData = new Array(maxRows);
let _g = 0;
let _g1 = this.massData.length;
while(_g < _g1) this.massData[_g++] = new oimo.dynamics.constraint.solver.common.JointSolverMassDataRow();
let numMaxBoundaries = this.boundaryBuilder.boundaries.length;
this.velBoundarySelector = new oimo.dynamics.constraint.solver.direct.BoundarySelector(numMaxBoundaries);
this.posBoundarySelector = new oimo.dynamics.constraint.solver.direct.BoundarySelector(numMaxBoundaries);
this.relVels = new Array(maxRows);
this.impulses = new Array(maxRows);
this.dImpulses = new Array(maxRows);
this.dTotalImpulses = new Array(maxRows);
let _g2 = 0;
while(_g2 < maxRows) {
let i = _g2++;
this.relVels[i] = 0;
this.impulses[i] = 0;
this.dImpulses[i] = 0;
this.dTotalImpulses[i] = 0;
}
}
preSolveVelocity(timeStep) {
this.joint._syncAnchors();
this.joint._getVelocitySolverInfo(timeStep,this.info);
this._b1 = this.info.b1;
this._b2 = this.info.b2;
this.massMatrix.computeInvMass(this.info,this.massData);
let _this = this.boundaryBuilder;
_this.numBoundaries = 0;
let _this1 = _this.bbInfo;
_this1.numBounded = 0;
_this1.numUnbounded = 0;
_this.buildBoundariesRecursive(this.info,0);
let _this2 = this.velBoundarySelector;
let size = this.boundaryBuilder.numBoundaries;
let numSmaller = 0;
let numGreater = 0;
let _g = 0;
let _g1 = _this2.n;
while(_g < _g1) {
let idx = _this2.indices[_g++];
if(idx < size) {
_this2.tmpIndices[numSmaller] = idx;
++numSmaller;
} else {
_this2.tmpIndices[size + numGreater] = idx;
++numGreater;
}
}
let tmp = _this2.indices;
_this2.indices = _this2.tmpIndices;
_this2.tmpIndices = tmp;
}
warmStart(timeStep) {
let factor = this.joint._positionCorrectionAlgorithm == oimo.dynamics.constraint.PositionCorrectionAlgorithm.BAUMGARTE ? oimo.common.Setting.jointWarmStartingFactorForBaungarte : oimo.common.Setting.jointWarmStartingFactor;
factor *= timeStep.dtRatio;
if(factor <= 0) {
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let _this = this.info.rows[_g++].impulse;
_this.impulse = 0;
_this.impulseM = 0;
_this.impulseP = 0;
}
return;
}
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let row = this.info.rows[i];
let imp = row.impulse;
let impulse = imp.impulse * factor;
if(impulse < row.minImpulse) {
impulse = row.minImpulse;
} else if(impulse > row.maxImpulse) {
impulse = row.maxImpulse;
}
imp.impulse = impulse;
if(row.motorMaxImpulse > 0) {
let impulseM = imp.impulseM * factor;
let max = row.motorMaxImpulse;
if(impulseM < -max) {
impulseM = -max;
} else if(impulseM > max) {
impulseM = max;
}
imp.impulseM = impulseM;
} else {
imp.impulseM = 0;
}
this.dImpulses[i] = imp.impulse + imp.impulseM;
}
let impulses = this.dImpulses;
let linearSet = false;
let angularSet = false;
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._velX;
lv1Y = this._b1._velY;
lv1Z = this._b1._velZ;
lv2X = this._b2._velX;
lv2Y = this._b2._velY;
lv2Z = this._b2._velZ;
av1X = this._b1._angVelX;
av1Y = this._b1._angVelY;
av1Z = this._b1._angVelZ;
av2X = this._b2._angVelX;
av2Y = this._b2._angVelY;
av2Z = this._b2._angVelZ;
let _g2 = 0;
let _g3 = this.info.numRows;
while(_g2 < _g3) {
let i = _g2++;
let j = this.info.rows[i].jacobian;
let md = this.massData[i];
let imp = impulses[i];
if((j.flag & 1) != 0) {
lv1X += md.invMLin1X * imp;
lv1Y += md.invMLin1Y * imp;
lv1Z += md.invMLin1Z * imp;
lv2X += md.invMLin2X * -imp;
lv2Y += md.invMLin2Y * -imp;
lv2Z += md.invMLin2Z * -imp;
linearSet = true;
}
if((j.flag & 2) != 0) {
av1X += md.invMAng1X * imp;
av1Y += md.invMAng1Y * imp;
av1Z += md.invMAng1Z * imp;
av2X += md.invMAng2X * -imp;
av2Y += md.invMAng2Y * -imp;
av2Z += md.invMAng2Z * -imp;
angularSet = true;
}
}
if(linearSet) {
this._b1._velX = lv1X;
this._b1._velY = lv1Y;
this._b1._velZ = lv1Z;
this._b2._velX = lv2X;
this._b2._velY = lv2Y;
this._b2._velZ = lv2Z;
}
if(angularSet) {
this._b1._angVelX = av1X;
this._b1._angVelY = av1Y;
this._b1._angVelZ = av1Z;
this._b2._angVelX = av2X;
this._b2._angVelY = av2Y;
this._b2._angVelZ = av2Z;
}
}
solveVelocity() {
let numRows = this.info.numRows;
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._velX;
lv1Y = this._b1._velY;
lv1Z = this._b1._velZ;
lv2X = this._b2._velX;
lv2Y = this._b2._velY;
lv2Z = this._b2._velZ;
av1X = this._b1._angVelX;
av1Y = this._b1._angVelY;
av1Z = this._b1._angVelZ;
av2X = this._b2._angVelX;
av2Y = this._b2._angVelY;
av2Z = this._b2._angVelZ;
let _g = 0;
while(_g < numRows) {
let i = _g++;
let row = this.info.rows[i];
let j = row.jacobian;
let relVel = 0;
relVel += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
relVel -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
relVel += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
relVel -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
this.relVels[i] = relVel;
this.impulses[i] = row.impulse.impulse;
this.dTotalImpulses[i] = 0;
}
let invMass = this.massMatrix._invMassWithoutCfm;
let _g1 = 0;
while(_g1 < numRows) {
let i = _g1++;
let row = this.info.rows[i];
let imp = row.impulse;
if(row.motorMaxImpulse > 0) {
let oldImpulseM = imp.impulseM;
let impulseM = oldImpulseM + this.massData[i].massWithoutCfm * (-row.motorSpeed - this.relVels[i]);
let maxImpulseM = row.motorMaxImpulse;
if(impulseM < -maxImpulseM) {
impulseM = -maxImpulseM;
} else if(impulseM > maxImpulseM) {
impulseM = maxImpulseM;
}
imp.impulseM = impulseM;
let dImpulseM = impulseM - oldImpulseM;
this.dTotalImpulses[i] = dImpulseM;
let _g = 0;
while(_g < numRows) {
let j = _g++;
this.relVels[j] += dImpulseM * invMass[i][j];
}
}
}
let solved = false;
let _g2 = 0;
let _g3 = this.boundaryBuilder.numBoundaries;
while(_g2 < _g3) {
let idx = this.velBoundarySelector.indices[_g2++];
if(this.boundaryBuilder.boundaries[idx].computeImpulses(this.info,this.massMatrix,this.relVels,this.impulses,this.dImpulses,1,false)) {
let _g = 0;
while(_g < numRows) {
let j = _g++;
let dimp = this.dImpulses[j];
this.info.rows[j].impulse.impulse += dimp;
this.dTotalImpulses[j] += dimp;
}
let impulses = this.dTotalImpulses;
let linearSet = false;
let angularSet = false;
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._velX;
lv1Y = this._b1._velY;
lv1Z = this._b1._velZ;
lv2X = this._b2._velX;
lv2Y = this._b2._velY;
lv2Z = this._b2._velZ;
av1X = this._b1._angVelX;
av1Y = this._b1._angVelY;
av1Z = this._b1._angVelZ;
av2X = this._b2._angVelX;
av2Y = this._b2._angVelY;
av2Z = this._b2._angVelZ;
let _g1 = 0;
let _g2 = this.info.numRows;
while(_g1 < _g2) {
let i = _g1++;
let j = this.info.rows[i].jacobian;
let md = this.massData[i];
let imp = impulses[i];
if((j.flag & 1) != 0) {
lv1X += md.invMLin1X * imp;
lv1Y += md.invMLin1Y * imp;
lv1Z += md.invMLin1Z * imp;
lv2X += md.invMLin2X * -imp;
lv2Y += md.invMLin2Y * -imp;
lv2Z += md.invMLin2Z * -imp;
linearSet = true;
}
if((j.flag & 2) != 0) {
av1X += md.invMAng1X * imp;
av1Y += md.invMAng1Y * imp;
av1Z += md.invMAng1Z * imp;
av2X += md.invMAng2X * -imp;
av2Y += md.invMAng2Y * -imp;
av2Z += md.invMAng2Z * -imp;
angularSet = true;
}
}
if(linearSet) {
this._b1._velX = lv1X;
this._b1._velY = lv1Y;
this._b1._velZ = lv1Z;
this._b2._velX = lv2X;
this._b2._velY = lv2Y;
this._b2._velZ = lv2Z;
}
if(angularSet) {
this._b1._angVelX = av1X;
this._b1._angVelY = av1Y;
this._b1._angVelZ = av1Z;
this._b2._angVelX = av2X;
this._b2._angVelY = av2Y;
this._b2._angVelZ = av2Z;
}
let _this = this.velBoundarySelector;
let i = 0;
while(_this.indices[i] != idx) ++i;
while(i > 0) {
let tmp = _this.indices[i];
_this.indices[i] = _this.indices[i - 1];
_this.indices[i - 1] = tmp;
--i;
}
solved = true;
break;
}
}
if(!solved) {
console.log("src/oimo/dynamics/constraint/solver/direct/DirectJointConstraintSolver.hx:335:","could not find solution. (velocity)");
return;
}
}
postSolveVelocity(timeStep) {
let linX;
let linY;
let linZ;
let angX;
let angY;
let angZ;
linX = 0;
linY = 0;
linZ = 0;
angX = 0;
angY = 0;
angZ = 0;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let row = this.info.rows[_g++];
let imp = row.impulse;
let j = row.jacobian;
if((j.flag & 1) != 0) {
linX += j.lin1X * imp.impulse;
linY += j.lin1Y * imp.impulse;
linZ += j.lin1Z * imp.impulse;
} else if((j.flag & 2) != 0) {
angX += j.ang1X * imp.impulse;
angY += j.ang1Y * imp.impulse;
angZ += j.ang1Z * imp.impulse;
}
}
this.joint._appliedForceX = linX * timeStep.invDt;
this.joint._appliedForceY = linY * timeStep.invDt;
this.joint._appliedForceZ = linZ * timeStep.invDt;
this.joint._appliedTorqueX = angX * timeStep.invDt;
this.joint._appliedTorqueY = angY * timeStep.invDt;
this.joint._appliedTorqueZ = angZ * timeStep.invDt;
}
preSolvePosition(timeStep) {
this.joint._syncAnchors();
this.joint._getPositionSolverInfo(this.info);
this._b1 = this.info.b1;
this._b2 = this.info.b2;
this.massMatrix.computeInvMass(this.info,this.massData);
let _this = this.boundaryBuilder;
_this.numBoundaries = 0;
let _this1 = _this.bbInfo;
_this1.numBounded = 0;
_this1.numUnbounded = 0;
_this.buildBoundariesRecursive(this.info,0);
let _this2 = this.posBoundarySelector;
let size = this.boundaryBuilder.numBoundaries;
let numSmaller = 0;
let numGreater = 0;
let _g = 0;
let _g1 = _this2.n;
while(_g < _g1) {
let idx = _this2.indices[_g++];
if(idx < size) {
_this2.tmpIndices[numSmaller] = idx;
++numSmaller;
} else {
_this2.tmpIndices[size + numGreater] = idx;
++numGreater;
}
}
let tmp = _this2.indices;
_this2.indices = _this2.tmpIndices;
_this2.tmpIndices = tmp;
let _g2 = 0;
let _g3 = this.info.numRows;
while(_g2 < _g3) this.info.rows[_g2++].impulse.impulseP = 0;
}
solvePositionSplitImpulse() {
let numRows = this.info.numRows;
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._pseudoVelX;
lv1Y = this._b1._pseudoVelY;
lv1Z = this._b1._pseudoVelZ;
lv2X = this._b2._pseudoVelX;
lv2Y = this._b2._pseudoVelY;
lv2Z = this._b2._pseudoVelZ;
av1X = this._b1._angPseudoVelX;
av1Y = this._b1._angPseudoVelY;
av1Z = this._b1._angPseudoVelZ;
av2X = this._b2._angPseudoVelX;
av2Y = this._b2._angPseudoVelY;
av2Z = this._b2._angPseudoVelZ;
let _g = 0;
while(_g < numRows) {
let i = _g++;
let row = this.info.rows[i];
let j = row.jacobian;
let relVel = 0;
relVel += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
relVel -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
relVel += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
relVel -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
this.relVels[i] = relVel;
this.impulses[i] = row.impulse.impulseP;
}
let solved = false;
let _g1 = 0;
let _g2 = this.boundaryBuilder.numBoundaries;
while(_g1 < _g2) {
let idx = this.posBoundarySelector.indices[_g1++];
if(this.boundaryBuilder.boundaries[idx].computeImpulses(this.info,this.massMatrix,this.relVels,this.impulses,this.dImpulses,oimo.common.Setting.positionSplitImpulseBaumgarte,false)) {
let _g = 0;
while(_g < numRows) {
let j = _g++;
this.info.rows[j].impulse.impulseP += this.dImpulses[j];
}
let impulses = this.dImpulses;
let linearSet = false;
let angularSet = false;
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._pseudoVelX;
lv1Y = this._b1._pseudoVelY;
lv1Z = this._b1._pseudoVelZ;
lv2X = this._b2._pseudoVelX;
lv2Y = this._b2._pseudoVelY;
lv2Z = this._b2._pseudoVelZ;
av1X = this._b1._angPseudoVelX;
av1Y = this._b1._angPseudoVelY;
av1Z = this._b1._angPseudoVelZ;
av2X = this._b2._angPseudoVelX;
av2Y = this._b2._angPseudoVelY;
av2Z = this._b2._angPseudoVelZ;
let _g1 = 0;
let _g2 = this.info.numRows;
while(_g1 < _g2) {
let i = _g1++;
let j = this.info.rows[i].jacobian;
let md = this.massData[i];
let imp = impulses[i];
if((j.flag & 1) != 0) {
lv1X += md.invMLin1X * imp;
lv1Y += md.invMLin1Y * imp;
lv1Z += md.invMLin1Z * imp;
lv2X += md.invMLin2X * -imp;
lv2Y += md.invMLin2Y * -imp;
lv2Z += md.invMLin2Z * -imp;
linearSet = true;
}
if((j.flag & 2) != 0) {
av1X += md.invMAng1X * imp;
av1Y += md.invMAng1Y * imp;
av1Z += md.invMAng1Z * imp;
av2X += md.invMAng2X * -imp;
av2Y += md.invMAng2Y * -imp;
av2Z += md.invMAng2Z * -imp;
angularSet = true;
}
}
if(linearSet) {
this._b1._pseudoVelX = lv1X;
this._b1._pseudoVelY = lv1Y;
this._b1._pseudoVelZ = lv1Z;
this._b2._pseudoVelX = lv2X;
this._b2._pseudoVelY = lv2Y;
this._b2._pseudoVelZ = lv2Z;
}
if(angularSet) {
this._b1._angPseudoVelX = av1X;
this._b1._angPseudoVelY = av1Y;
this._b1._angPseudoVelZ = av1Z;
this._b2._angPseudoVelX = av2X;
this._b2._angPseudoVelY = av2Y;
this._b2._angPseudoVelZ = av2Z;
}
let _this = this.posBoundarySelector;
let i = 0;
while(_this.indices[i] != idx) ++i;
while(i > 0) {
let tmp = _this.indices[i];
_this.indices[i] = _this.indices[i - 1];
_this.indices[i - 1] = tmp;
--i;
}
solved = true;
break;
}
}
if(!solved) {
console.log("src/oimo/dynamics/constraint/solver/direct/DirectJointConstraintSolver.hx:450:","could not find solution. (split impulse)");
return;
}
}
solvePositionNgs(timeStep) {
this.joint._syncAnchors();
this.joint._getPositionSolverInfo(this.info);
this._b1 = this.info.b1;
this._b2 = this.info.b2;
this.massMatrix.computeInvMass(this.info,this.massData);
let _this = this.boundaryBuilder;
_this.numBoundaries = 0;
let _this1 = _this.bbInfo;
_this1.numBounded = 0;
_this1.numUnbounded = 0;
_this.buildBoundariesRecursive(this.info,0);
let _this2 = this.posBoundarySelector;
let size = this.boundaryBuilder.numBoundaries;
let numSmaller = 0;
let numGreater = 0;
let _g = 0;
let _g1 = _this2.n;
while(_g < _g1) {
let idx = _this2.indices[_g++];
if(idx < size) {
_this2.tmpIndices[numSmaller] = idx;
++numSmaller;
} else {
_this2.tmpIndices[size + numGreater] = idx;
++numGreater;
}
}
let tmp = _this2.indices;
_this2.indices = _this2.tmpIndices;
_this2.tmpIndices = tmp;
let numRows = this.info.numRows;
let _g2 = 0;
while(_g2 < numRows) {
let i = _g2++;
let imp = this.info.rows[i].impulse;
this.relVels[i] = 0;
this.impulses[i] = imp.impulseP;
}
let solved = false;
let _g3 = 0;
let _g4 = this.boundaryBuilder.numBoundaries;
while(_g3 < _g4) {
let idx = this.posBoundarySelector.indices[_g3++];
if(this.boundaryBuilder.boundaries[idx].computeImpulses(this.info,this.massMatrix,this.relVels,this.impulses,this.dImpulses,oimo.common.Setting.positionNgsBaumgarte,false)) {
let _g = 0;
while(_g < numRows) {
let j = _g++;
this.info.rows[j].impulse.impulseP += this.dImpulses[j];
}
let impulses = this.dImpulses;
let linearSet = false;
let angularSet = false;
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = 0;
lv1Y = 0;
lv1Z = 0;
lv2X = 0;
lv2Y = 0;
lv2Z = 0;
av1X = 0;
av1Y = 0;
av1Z = 0;
av2X = 0;
av2Y = 0;
av2Z = 0;
let _g1 = 0;
let _g2 = this.info.numRows;
while(_g1 < _g2) {
let i = _g1++;
let j = this.info.rows[i].jacobian;
let md = this.massData[i];
let imp = impulses[i];
if((j.flag & 1) != 0) {
lv1X += md.invMLin1X * imp;
lv1Y += md.invMLin1Y * imp;
lv1Z += md.invMLin1Z * imp;
lv2X += md.invMLin2X * -imp;
lv2Y += md.invMLin2Y * -imp;
lv2Z += md.invMLin2Z * -imp;
linearSet = true;
}
if((j.flag & 2) != 0) {
av1X += md.invMAng1X * imp;
av1Y += md.invMAng1Y * imp;
av1Z += md.invMAng1Z * imp;
av2X += md.invMAng2X * -imp;
av2Y += md.invMAng2Y * -imp;
av2Z += md.invMAng2Z * -imp;
angularSet = true;
}
}
if(linearSet) {
let _this = this._b1;
_this._transform._positionX += lv1X;
_this._transform._positionY += lv1Y;
_this._transform._positionZ += lv1Z;
let _this1 = this._b2;
_this1._transform._positionX += lv2X;
_this1._transform._positionY += lv2Y;
_this1._transform._positionZ += lv2Z;
}
if(angularSet) {
let _this = this._b1;
let theta = Math.sqrt(av1X * av1X + av1Y * av1Y + av1Z * av1Z);
let halfTheta = theta * 0.5;
let rotationToSinAxisFactor;
let cosHalfTheta;
if(halfTheta < 0.5) {
let ht2 = halfTheta * halfTheta;
rotationToSinAxisFactor = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
cosHalfTheta = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
} else {
rotationToSinAxisFactor = Math.sin(halfTheta) / theta;
cosHalfTheta = Math.cos(halfTheta);
}
let sinAxisX;
let sinAxisY;
let sinAxisZ;
sinAxisX = av1X * rotationToSinAxisFactor;
sinAxisY = av1Y * rotationToSinAxisFactor;
sinAxisZ = av1Z * rotationToSinAxisFactor;
let dqX;
let dqY;
let dqZ;
let dqW;
dqX = sinAxisX;
dqY = sinAxisY;
dqZ = sinAxisZ;
dqW = cosHalfTheta;
let qX;
let qY;
let qZ;
let qW;
let e00 = _this._transform._rotation00;
let e11 = _this._transform._rotation11;
let e22 = _this._transform._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
qW = 0.5 * s;
s = 0.5 / s;
qX = (_this._transform._rotation21 - _this._transform._rotation12) * s;
qY = (_this._transform._rotation02 - _this._transform._rotation20) * s;
qZ = (_this._transform._rotation10 - _this._transform._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
qX = 0.5 * s;
s = 0.5 / s;
qY = (_this._transform._rotation01 + _this._transform._rotation10) * s;
qZ = (_this._transform._rotation02 + _this._transform._rotation20) * s;
qW = (_this._transform._rotation21 - _this._transform._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (_this._transform._rotation02 + _this._transform._rotation20) * s;
qY = (_this._transform._rotation12 + _this._transform._rotation21) * s;
qW = (_this._transform._rotation10 - _this._transform._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
qY = 0.5 * s;
s = 0.5 / s;
qX = (_this._transform._rotation01 + _this._transform._rotation10) * s;
qZ = (_this._transform._rotation12 + _this._transform._rotation21) * s;
qW = (_this._transform._rotation02 - _this._transform._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (_this._transform._rotation02 + _this._transform._rotation20) * s;
qY = (_this._transform._rotation12 + _this._transform._rotation21) * s;
qW = (_this._transform._rotation10 - _this._transform._rotation01) * s;
}
qX = dqW * qX + dqX * qW + dqY * qZ - dqZ * qY;
qY = dqW * qY - dqX * qZ + dqY * qW + dqZ * qX;
qZ = dqW * qZ + dqX * qY - dqY * qX + dqZ * qW;
qW = dqW * qW - dqX * qX - dqY * qY - dqZ * qZ;
let l = qX * qX + qY * qY + qZ * qZ + qW * qW;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
qX *= l;
qY *= l;
qZ *= l;
qW *= l;
let x = qX;
let y = qY;
let z = qZ;
let w = qW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
_this._transform._rotation00 = 1 - yy - zz;
_this._transform._rotation01 = xy - wz;
_this._transform._rotation02 = xz + wy;
_this._transform._rotation10 = xy + wz;
_this._transform._rotation11 = 1 - xx - zz;
_this._transform._rotation12 = yz - wx;
_this._transform._rotation20 = xz - wy;
_this._transform._rotation21 = yz + wx;
_this._transform._rotation22 = 1 - xx - yy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = _this._transform._rotation00 * _this._invLocalInertia00 + _this._transform._rotation01 * _this._invLocalInertia10 + _this._transform._rotation02 * _this._invLocalInertia20;
__tmp__01 = _this._transform._rotation00 * _this._invLocalInertia01 + _this._transform._rotation01 * _this._invLocalInertia11 + _this._transform._rotation02 * _this._invLocalInertia21;
__tmp__02 = _this._transform._rotation00 * _this._invLocalInertia02 + _this._transform._rotation01 * _this._invLocalInertia12 + _this._transform._rotation02 * _this._invLocalInertia22;
__tmp__10 = _this._transform._rotation10 * _this._invLocalInertia00 + _this._transform._rotation11 * _this._invLocalInertia10 + _this._transform._rotation12 * _this._invLocalInertia20;
__tmp__11 = _this._transform._rotation10 * _this._invLocalInertia01 + _this._transform._rotation11 * _this._invLocalInertia11 + _this._transform._rotation12 * _this._invLocalInertia21;
__tmp__12 = _this._transform._rotation10 * _this._invLocalInertia02 + _this._transform._rotation11 * _this._invLocalInertia12 + _this._transform._rotation12 * _this._invLocalInertia22;
__tmp__20 = _this._transform._rotation20 * _this._invLocalInertia00 + _this._transform._rotation21 * _this._invLocalInertia10 + _this._transform._rotation22 * _this._invLocalInertia20;
__tmp__21 = _this._transform._rotation20 * _this._invLocalInertia01 + _this._transform._rotation21 * _this._invLocalInertia11 + _this._transform._rotation22 * _this._invLocalInertia21;
__tmp__22 = _this._transform._rotation20 * _this._invLocalInertia02 + _this._transform._rotation21 * _this._invLocalInertia12 + _this._transform._rotation22 * _this._invLocalInertia22;
_this._invInertia00 = __tmp__00;
_this._invInertia01 = __tmp__01;
_this._invInertia02 = __tmp__02;
_this._invInertia10 = __tmp__10;
_this._invInertia11 = __tmp__11;
_this._invInertia12 = __tmp__12;
_this._invInertia20 = __tmp__20;
_this._invInertia21 = __tmp__21;
_this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = _this._invInertia00 * _this._transform._rotation00 + _this._invInertia01 * _this._transform._rotation01 + _this._invInertia02 * _this._transform._rotation02;
__tmp__011 = _this._invInertia00 * _this._transform._rotation10 + _this._invInertia01 * _this._transform._rotation11 + _this._invInertia02 * _this._transform._rotation12;
__tmp__021 = _this._invInertia00 * _this._transform._rotation20 + _this._invInertia01 * _this._transform._rotation21 + _this._invInertia02 * _this._transform._rotation22;
__tmp__101 = _this._invInertia10 * _this._transform._rotation00 + _this._invInertia11 * _this._transform._rotation01 + _this._invInertia12 * _this._transform._rotation02;
__tmp__111 = _this._invInertia10 * _this._transform._rotation10 + _this._invInertia11 * _this._transform._rotation11 + _this._invInertia12 * _this._transform._rotation12;
__tmp__121 = _this._invInertia10 * _this._transform._rotation20 + _this._invInertia11 * _this._transform._rotation21 + _this._invInertia12 * _this._transform._rotation22;
__tmp__201 = _this._invInertia20 * _this._transform._rotation00 + _this._invInertia21 * _this._transform._rotation01 + _this._invInertia22 * _this._transform._rotation02;
__tmp__211 = _this._invInertia20 * _this._transform._rotation10 + _this._invInertia21 * _this._transform._rotation11 + _this._invInertia22 * _this._transform._rotation12;
__tmp__221 = _this._invInertia20 * _this._transform._rotation20 + _this._invInertia21 * _this._transform._rotation21 + _this._invInertia22 * _this._transform._rotation22;
_this._invInertia00 = __tmp__001;
_this._invInertia01 = __tmp__011;
_this._invInertia02 = __tmp__021;
_this._invInertia10 = __tmp__101;
_this._invInertia11 = __tmp__111;
_this._invInertia12 = __tmp__121;
_this._invInertia20 = __tmp__201;
_this._invInertia21 = __tmp__211;
_this._invInertia22 = __tmp__221;
_this._invInertia00 *= _this._rotFactor.x;
_this._invInertia01 *= _this._rotFactor.x;
_this._invInertia02 *= _this._rotFactor.x;
_this._invInertia10 *= _this._rotFactor.y;
_this._invInertia11 *= _this._rotFactor.y;
_this._invInertia12 *= _this._rotFactor.y;
_this._invInertia20 *= _this._rotFactor.z;
_this._invInertia21 *= _this._rotFactor.z;
_this._invInertia22 *= _this._rotFactor.z;
let _this1 = this._b2;
let theta1 = Math.sqrt(av2X * av2X + av2Y * av2Y + av2Z * av2Z);
let halfTheta1 = theta1 * 0.5;
let rotationToSinAxisFactor1;
let cosHalfTheta1;
if(halfTheta1 < 0.5) {
let ht2 = halfTheta1 * halfTheta1;
rotationToSinAxisFactor1 = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
cosHalfTheta1 = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
} else {
rotationToSinAxisFactor1 = Math.sin(halfTheta1) / theta1;
cosHalfTheta1 = Math.cos(halfTheta1);
}
let sinAxisX1;
let sinAxisY1;
let sinAxisZ1;
sinAxisX1 = av2X * rotationToSinAxisFactor1;
sinAxisY1 = av2Y * rotationToSinAxisFactor1;
sinAxisZ1 = av2Z * rotationToSinAxisFactor1;
let dqX1;
let dqY1;
let dqZ1;
let dqW1;
dqX1 = sinAxisX1;
dqY1 = sinAxisY1;
dqZ1 = sinAxisZ1;
dqW1 = cosHalfTheta1;
let qX1;
let qY1;
let qZ1;
let qW1;
let e001 = _this1._transform._rotation00;
let e111 = _this1._transform._rotation11;
let e221 = _this1._transform._rotation22;
let t1 = e001 + e111 + e221;
let s1;
if(t1 > 0) {
s1 = Math.sqrt(t1 + 1);
qW1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this1._transform._rotation21 - _this1._transform._rotation12) * s1;
qY1 = (_this1._transform._rotation02 - _this1._transform._rotation20) * s1;
qZ1 = (_this1._transform._rotation10 - _this1._transform._rotation01) * s1;
} else if(e001 > e111) {
if(e001 > e221) {
s1 = Math.sqrt(e001 - e111 - e221 + 1);
qX1 = 0.5 * s1;
s1 = 0.5 / s1;
qY1 = (_this1._transform._rotation01 + _this1._transform._rotation10) * s1;
qZ1 = (_this1._transform._rotation02 + _this1._transform._rotation20) * s1;
qW1 = (_this1._transform._rotation21 - _this1._transform._rotation12) * s1;
} else {
s1 = Math.sqrt(e221 - e001 - e111 + 1);
qZ1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this1._transform._rotation02 + _this1._transform._rotation20) * s1;
qY1 = (_this1._transform._rotation12 + _this1._transform._rotation21) * s1;
qW1 = (_this1._transform._rotation10 - _this1._transform._rotation01) * s1;
}
} else if(e111 > e221) {
s1 = Math.sqrt(e111 - e221 - e001 + 1);
qY1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this1._transform._rotation01 + _this1._transform._rotation10) * s1;
qZ1 = (_this1._transform._rotation12 + _this1._transform._rotation21) * s1;
qW1 = (_this1._transform._rotation02 - _this1._transform._rotation20) * s1;
} else {
s1 = Math.sqrt(e221 - e001 - e111 + 1);
qZ1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this1._transform._rotation02 + _this1._transform._rotation20) * s1;
qY1 = (_this1._transform._rotation12 + _this1._transform._rotation21) * s1;
qW1 = (_this1._transform._rotation10 - _this1._transform._rotation01) * s1;
}
qX1 = dqW1 * qX1 + dqX1 * qW1 + dqY1 * qZ1 - dqZ1 * qY1;
qY1 = dqW1 * qY1 - dqX1 * qZ1 + dqY1 * qW1 + dqZ1 * qX1;
qZ1 = dqW1 * qZ1 + dqX1 * qY1 - dqY1 * qX1 + dqZ1 * qW1;
qW1 = dqW1 * qW1 - dqX1 * qX1 - dqY1 * qY1 - dqZ1 * qZ1;
let l1 = qX1 * qX1 + qY1 * qY1 + qZ1 * qZ1 + qW1 * qW1;
if(l1 > 1e-32) {
l1 = 1 / Math.sqrt(l1);
}
qX1 *= l1;
qY1 *= l1;
qZ1 *= l1;
qW1 *= l1;
let x1 = qX1;
let y1 = qY1;
let z1 = qZ1;
let w1 = qW1;
let x21 = 2 * x1;
let y21 = 2 * y1;
let z21 = 2 * z1;
let xx1 = x1 * x21;
let yy1 = y1 * y21;
let zz1 = z1 * z21;
let xy1 = x1 * y21;
let yz1 = y1 * z21;
let xz1 = x1 * z21;
let wx1 = w1 * x21;
let wy1 = w1 * y21;
let wz1 = w1 * z21;
_this1._transform._rotation00 = 1 - yy1 - zz1;
_this1._transform._rotation01 = xy1 - wz1;
_this1._transform._rotation02 = xz1 + wy1;
_this1._transform._rotation10 = xy1 + wz1;
_this1._transform._rotation11 = 1 - xx1 - zz1;
_this1._transform._rotation12 = yz1 - wx1;
_this1._transform._rotation20 = xz1 - wy1;
_this1._transform._rotation21 = yz1 + wx1;
_this1._transform._rotation22 = 1 - xx1 - yy1;
let __tmp__002;
let __tmp__012;
let __tmp__022;
let __tmp__102;
let __tmp__112;
let __tmp__122;
let __tmp__202;
let __tmp__212;
let __tmp__222;
__tmp__002 = _this1._transform._rotation00 * _this1._invLocalInertia00 + _this1._transform._rotation01 * _this1._invLocalInertia10 + _this1._transform._rotation02 * _this1._invLocalInertia20;
__tmp__012 = _this1._transform._rotation00 * _this1._invLocalInertia01 + _this1._transform._rotation01 * _this1._invLocalInertia11 + _this1._transform._rotation02 * _this1._invLocalInertia21;
__tmp__022 = _this1._transform._rotation00 * _this1._invLocalInertia02 + _this1._transform._rotation01 * _this1._invLocalInertia12 + _this1._transform._rotation02 * _this1._invLocalInertia22;
__tmp__102 = _this1._transform._rotation10 * _this1._invLocalInertia00 + _this1._transform._rotation11 * _this1._invLocalInertia10 + _this1._transform._rotation12 * _this1._invLocalInertia20;
__tmp__112 = _this1._transform._rotation10 * _this1._invLocalInertia01 + _this1._transform._rotation11 * _this1._invLocalInertia11 + _this1._transform._rotation12 * _this1._invLocalInertia21;
__tmp__122 = _this1._transform._rotation10 * _this1._invLocalInertia02 + _this1._transform._rotation11 * _this1._invLocalInertia12 + _this1._transform._rotation12 * _this1._invLocalInertia22;
__tmp__202 = _this1._transform._rotation20 * _this1._invLocalInertia00 + _this1._transform._rotation21 * _this1._invLocalInertia10 + _this1._transform._rotation22 * _this1._invLocalInertia20;
__tmp__212 = _this1._transform._rotation20 * _this1._invLocalInertia01 + _this1._transform._rotation21 * _this1._invLocalInertia11 + _this1._transform._rotation22 * _this1._invLocalInertia21;
__tmp__222 = _this1._transform._rotation20 * _this1._invLocalInertia02 + _this1._transform._rotation21 * _this1._invLocalInertia12 + _this1._transform._rotation22 * _this1._invLocalInertia22;
_this1._invInertia00 = __tmp__002;
_this1._invInertia01 = __tmp__012;
_this1._invInertia02 = __tmp__022;
_this1._invInertia10 = __tmp__102;
_this1._invInertia11 = __tmp__112;
_this1._invInertia12 = __tmp__122;
_this1._invInertia20 = __tmp__202;
_this1._invInertia21 = __tmp__212;
_this1._invInertia22 = __tmp__222;
let __tmp__003;
let __tmp__013;
let __tmp__023;
let __tmp__103;
let __tmp__113;
let __tmp__123;
let __tmp__203;
let __tmp__213;
let __tmp__223;
__tmp__003 = _this1._invInertia00 * _this1._transform._rotation00 + _this1._invInertia01 * _this1._transform._rotation01 + _this1._invInertia02 * _this1._transform._rotation02;
__tmp__013 = _this1._invInertia00 * _this1._transform._rotation10 + _this1._invInertia01 * _this1._transform._rotation11 + _this1._invInertia02 * _this1._transform._rotation12;
__tmp__023 = _this1._invInertia00 * _this1._transform._rotation20 + _this1._invInertia01 * _this1._transform._rotation21 + _this1._invInertia02 * _this1._transform._rotation22;
__tmp__103 = _this1._invInertia10 * _this1._transform._rotation00 + _this1._invInertia11 * _this1._transform._rotation01 + _this1._invInertia12 * _this1._transform._rotation02;
__tmp__113 = _this1._invInertia10 * _this1._transform._rotation10 + _this1._invInertia11 * _this1._transform._rotation11 + _this1._invInertia12 * _this1._transform._rotation12;
__tmp__123 = _this1._invInertia10 * _this1._transform._rotation20 + _this1._invInertia11 * _this1._transform._rotation21 + _this1._invInertia12 * _this1._transform._rotation22;
__tmp__203 = _this1._invInertia20 * _this1._transform._rotation00 + _this1._invInertia21 * _this1._transform._rotation01 + _this1._invInertia22 * _this1._transform._rotation02;
__tmp__213 = _this1._invInertia20 * _this1._transform._rotation10 + _this1._invInertia21 * _this1._transform._rotation11 + _this1._invInertia22 * _this1._transform._rotation12;
__tmp__223 = _this1._invInertia20 * _this1._transform._rotation20 + _this1._invInertia21 * _this1._transform._rotation21 + _this1._invInertia22 * _this1._transform._rotation22;
_this1._invInertia00 = __tmp__003;
_this1._invInertia01 = __tmp__013;
_this1._invInertia02 = __tmp__023;
_this1._invInertia10 = __tmp__103;
_this1._invInertia11 = __tmp__113;
_this1._invInertia12 = __tmp__123;
_this1._invInertia20 = __tmp__203;
_this1._invInertia21 = __tmp__213;
_this1._invInertia22 = __tmp__223;
_this1._invInertia00 *= _this1._rotFactor.x;
_this1._invInertia01 *= _this1._rotFactor.x;
_this1._invInertia02 *= _this1._rotFactor.x;
_this1._invInertia10 *= _this1._rotFactor.y;
_this1._invInertia11 *= _this1._rotFactor.y;
_this1._invInertia12 *= _this1._rotFactor.y;
_this1._invInertia20 *= _this1._rotFactor.z;
_this1._invInertia21 *= _this1._rotFactor.z;
_this1._invInertia22 *= _this1._rotFactor.z;
}
let _this = this.posBoundarySelector;
let i = 0;
while(_this.indices[i] != idx) ++i;
while(i > 0) {
let tmp = _this.indices[i];
_this.indices[i] = _this.indices[i - 1];
_this.indices[i - 1] = tmp;
--i;
}
solved = true;
break;
}
}
if(!solved) {
console.log("src/oimo/dynamics/constraint/solver/direct/DirectJointConstraintSolver.hx:502:","could not find solution. (NGS)");
return;
}
}
postSolve() {
this.joint._syncAnchors();
this.joint._checkDestruction();
}
}
oimo.dynamics.constraint.solver.direct.MassMatrix = class oimo_dynamics_constraint_solver_direct_MassMatrix {
constructor(size) {
this._size = size;
this.tmpMatrix = new Array(this._size);
this._invMass = new Array(this._size);
this._invMassWithoutCfm = new Array(this._size);
let _g = 0;
let _g1 = this._size;
while(_g < _g1) {
let i = _g++;
this.tmpMatrix[i] = new Array(this._size);
this._invMass[i] = new Array(this._size);
this._invMassWithoutCfm[i] = new Array(this._size);
let _g1 = 0;
let _g2 = this._size;
while(_g1 < _g2) {
let j = _g1++;
this.tmpMatrix[i][j] = 0;
this._invMass[i][j] = 0;
this._invMassWithoutCfm[i][j] = 0;
}
}
this._maxSubmatrixId = 1 << this._size;
this._cacheComputed = new Array(this._maxSubmatrixId);
this._cachedSubmatrices = new Array(this._maxSubmatrixId);
let _g2 = 0;
let _g3 = this._maxSubmatrixId;
while(_g2 < _g3) {
let i = _g2++;
let t;
t = (i & 85) + (i >> 1 & 85);
t = (t & 51) + (t >> 2 & 51);
t = (t & 15) + (t >> 4 & 15);
let matrixSize = t;
let subMatrix = new Array(matrixSize);
let _g = 0;
while(_g < matrixSize) {
let j = _g++;
subMatrix[j] = new Array(matrixSize);
let _g1 = 0;
while(_g1 < matrixSize) subMatrix[j][_g1++] = 0;
}
this._cacheComputed[i] = false;
this._cachedSubmatrices[i] = subMatrix;
}
}
computeSubmatrix(id,indices,size) {
let _g = 0;
while(_g < size) {
let i = _g++;
let ii = indices[i];
let _g1 = 0;
while(_g1 < size) {
let j = _g1++;
this.tmpMatrix[i][j] = this._invMass[ii][indices[j]];
}
}
let src = this.tmpMatrix;
let dst = this._cachedSubmatrices[id];
let srci;
let dsti;
let srcj;
let dstj;
let diag;
switch(size) {
case 4:
srci = src[0];
dsti = dst[0];
diag = 1 / srci[0];
dsti[0] = diag;
srci[1] *= diag;
srci[2] *= diag;
srci[3] *= diag;
srcj = src[1];
dstj = dst[1];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj = src[2];
dstj = dst[2];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj = src[3];
dstj = dst[3];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srci = src[1];
dsti = dst[1];
diag = 1 / srci[1];
dsti[1] = diag;
dsti[0] *= diag;
srci[2] *= diag;
srci[3] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srci = src[2];
dsti = dst[2];
diag = 1 / srci[2];
dsti[2] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
srci[3] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
dstj[2] = -diag * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srci = src[3];
dsti = dst[3];
diag = 1 / srci[3];
dsti[3] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
dsti[2] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[3];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
dstj[2] -= dsti[2] * srcj[3];
dsti = dst[1];
dst[0][1] = dsti[0];
dsti = dst[2];
dst[0][2] = dsti[0];
dst[1][2] = dsti[1];
dsti = dst[3];
dst[0][3] = dsti[0];
dst[1][3] = dsti[1];
dst[2][3] = dsti[2];
break;
case 5:
srci = src[0];
dsti = dst[0];
diag = 1 / srci[0];
dsti[0] = diag;
srci[1] *= diag;
srci[2] *= diag;
srci[3] *= diag;
srci[4] *= diag;
srcj = src[1];
dstj = dst[1];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srcj = src[2];
dstj = dst[2];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srcj = src[3];
dstj = dst[3];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srcj = src[4];
dstj = dst[4];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srci = src[1];
dsti = dst[1];
diag = 1 / srci[1];
dsti[1] = diag;
dsti[0] *= diag;
srci[2] *= diag;
srci[3] *= diag;
srci[4] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srcj = src[4];
dstj = dst[4];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srci = src[2];
dsti = dst[2];
diag = 1 / srci[2];
dsti[2] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
srci[3] *= diag;
srci[4] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
dstj[2] = -diag * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srcj = src[4];
dstj = dst[4];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
dstj[2] = -diag * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srci = src[3];
dsti = dst[3];
diag = 1 / srci[3];
dsti[3] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
dsti[2] *= diag;
srci[4] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
dstj[2] -= dsti[2] * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srcj = src[4];
dstj = dst[4];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
dstj[2] -= dsti[2] * srcj[3];
dstj[3] = -diag * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srci = src[4];
dsti = dst[4];
diag = 1 / srci[4];
dsti[4] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
dsti[2] *= diag;
dsti[3] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[4];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[4];
dstj[1] -= dsti[1] * srcj[4];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[4];
dstj[1] -= dsti[1] * srcj[4];
dstj[2] -= dsti[2] * srcj[4];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[4];
dstj[1] -= dsti[1] * srcj[4];
dstj[2] -= dsti[2] * srcj[4];
dstj[3] -= dsti[3] * srcj[4];
dsti = dst[1];
dst[0][1] = dsti[0];
dsti = dst[2];
dst[0][2] = dsti[0];
dst[1][2] = dsti[1];
dsti = dst[3];
dst[0][3] = dsti[0];
dst[1][3] = dsti[1];
dst[2][3] = dsti[2];
dsti = dst[4];
dst[0][4] = dsti[0];
dst[1][4] = dsti[1];
dst[2][4] = dsti[2];
dst[3][4] = dsti[3];
break;
case 6:
srci = src[0];
dsti = dst[0];
diag = 1 / srci[0];
dsti[0] = diag;
srci[1] *= diag;
srci[2] *= diag;
srci[3] *= diag;
srci[4] *= diag;
srci[5] *= diag;
srcj = src[1];
dstj = dst[1];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srcj[5] -= srci[5] * srcj[0];
srcj = src[2];
dstj = dst[2];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srcj[5] -= srci[5] * srcj[0];
srcj = src[3];
dstj = dst[3];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srcj[5] -= srci[5] * srcj[0];
srcj = src[4];
dstj = dst[4];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srcj[5] -= srci[5] * srcj[0];
srcj = src[5];
dstj = dst[5];
dstj[0] = -diag * srcj[0];
srcj[1] -= srci[1] * srcj[0];
srcj[2] -= srci[2] * srcj[0];
srcj[3] -= srci[3] * srcj[0];
srcj[4] -= srci[4] * srcj[0];
srcj[5] -= srci[5] * srcj[0];
srci = src[1];
dsti = dst[1];
diag = 1 / srci[1];
dsti[1] = diag;
dsti[0] *= diag;
srci[2] *= diag;
srci[3] *= diag;
srci[4] *= diag;
srci[5] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srcj[5] -= srci[5] * srcj[1];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srcj[5] -= srci[5] * srcj[1];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srcj[5] -= srci[5] * srcj[1];
srcj = src[4];
dstj = dst[4];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srcj[5] -= srci[5] * srcj[1];
srcj = src[5];
dstj = dst[5];
dstj[0] -= dsti[0] * srcj[1];
dstj[1] = -diag * srcj[1];
srcj[2] -= srci[2] * srcj[1];
srcj[3] -= srci[3] * srcj[1];
srcj[4] -= srci[4] * srcj[1];
srcj[5] -= srci[5] * srcj[1];
srci = src[2];
dsti = dst[2];
diag = 1 / srci[2];
dsti[2] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
srci[3] *= diag;
srci[4] *= diag;
srci[5] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srcj[5] -= srci[5] * srcj[2];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srcj[5] -= srci[5] * srcj[2];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
dstj[2] = -diag * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srcj[5] -= srci[5] * srcj[2];
srcj = src[4];
dstj = dst[4];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
dstj[2] = -diag * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srcj[5] -= srci[5] * srcj[2];
srcj = src[5];
dstj = dst[5];
dstj[0] -= dsti[0] * srcj[2];
dstj[1] -= dsti[1] * srcj[2];
dstj[2] = -diag * srcj[2];
srcj[3] -= srci[3] * srcj[2];
srcj[4] -= srci[4] * srcj[2];
srcj[5] -= srci[5] * srcj[2];
srci = src[3];
dsti = dst[3];
diag = 1 / srci[3];
dsti[3] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
dsti[2] *= diag;
srci[4] *= diag;
srci[5] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srcj[5] -= srci[5] * srcj[3];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srcj[5] -= srci[5] * srcj[3];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
dstj[2] -= dsti[2] * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srcj[5] -= srci[5] * srcj[3];
srcj = src[4];
dstj = dst[4];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
dstj[2] -= dsti[2] * srcj[3];
dstj[3] = -diag * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srcj[5] -= srci[5] * srcj[3];
srcj = src[5];
dstj = dst[5];
dstj[0] -= dsti[0] * srcj[3];
dstj[1] -= dsti[1] * srcj[3];
dstj[2] -= dsti[2] * srcj[3];
dstj[3] = -diag * srcj[3];
srcj[4] -= srci[4] * srcj[3];
srcj[5] -= srci[5] * srcj[3];
srci = src[4];
dsti = dst[4];
diag = 1 / srci[4];
dsti[4] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
dsti[2] *= diag;
dsti[3] *= diag;
srci[5] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[4];
srcj[5] -= srci[5] * srcj[4];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[4];
dstj[1] -= dsti[1] * srcj[4];
srcj[5] -= srci[5] * srcj[4];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[4];
dstj[1] -= dsti[1] * srcj[4];
dstj[2] -= dsti[2] * srcj[4];
srcj[5] -= srci[5] * srcj[4];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[4];
dstj[1] -= dsti[1] * srcj[4];
dstj[2] -= dsti[2] * srcj[4];
dstj[3] -= dsti[3] * srcj[4];
srcj[5] -= srci[5] * srcj[4];
srcj = src[5];
dstj = dst[5];
dstj[0] -= dsti[0] * srcj[4];
dstj[1] -= dsti[1] * srcj[4];
dstj[2] -= dsti[2] * srcj[4];
dstj[3] -= dsti[3] * srcj[4];
dstj[4] = -diag * srcj[4];
srcj[5] -= srci[5] * srcj[4];
srci = src[5];
dsti = dst[5];
diag = 1 / srci[5];
dsti[5] = diag;
dsti[0] *= diag;
dsti[1] *= diag;
dsti[2] *= diag;
dsti[3] *= diag;
dsti[4] *= diag;
srcj = src[0];
dstj = dst[0];
dstj[0] -= dsti[0] * srcj[5];
srcj = src[1];
dstj = dst[1];
dstj[0] -= dsti[0] * srcj[5];
dstj[1] -= dsti[1] * srcj[5];
srcj = src[2];
dstj = dst[2];
dstj[0] -= dsti[0] * srcj[5];
dstj[1] -= dsti[1] * srcj[5];
dstj[2] -= dsti[2] * srcj[5];
srcj = src[3];
dstj = dst[3];
dstj[0] -= dsti[0] * srcj[5];
dstj[1] -= dsti[1] * srcj[5];
dstj[2] -= dsti[2] * srcj[5];
dstj[3] -= dsti[3] * srcj[5];
srcj = src[4];
dstj = dst[4];
dstj[0] -= dsti[0] * srcj[5];
dstj[1] -= dsti[1] * srcj[5];
dstj[2] -= dsti[2] * srcj[5];
dstj[3] -= dsti[3] * srcj[5];
dstj[4] -= dsti[4] * srcj[5];
dsti = dst[1];
dst[0][1] = dsti[0];
dsti = dst[2];
dst[0][2] = dsti[0];
dst[1][2] = dsti[1];
dsti = dst[3];
dst[0][3] = dsti[0];
dst[1][3] = dsti[1];
dst[2][3] = dsti[2];
dsti = dst[4];
dst[0][4] = dsti[0];
dst[1][4] = dsti[1];
dst[2][4] = dsti[2];
dst[3][4] = dsti[3];
dsti = dst[5];
dst[0][5] = dsti[0];
dst[1][5] = dsti[1];
dst[2][5] = dsti[2];
dst[3][5] = dsti[3];
dst[4][5] = dsti[4];
break;
default:
let _g1 = 0;
while(_g1 < size) {
let i = _g1++;
srci = src[i];
dsti = dst[i];
let diag = 1 / srci[i];
dsti[i] = diag;
let _g = 0;
while(_g < i) dsti[_g++] *= diag;
let _g2 = i + 1;
while(_g2 < size) srci[_g2++] *= diag;
let _g3 = 0;
while(_g3 < i) {
let j = _g3++;
srcj = src[j];
dstj = dst[j];
let _g = 0;
let _g1 = j + 1;
while(_g < _g1) {
let k = _g++;
dstj[k] -= dsti[k] * srcj[i];
}
let _g2 = i + 1;
while(_g2 < size) {
let k = _g2++;
srcj[k] -= srci[k] * srcj[i];
}
}
let _g4 = i + 1;
while(_g4 < size) {
let j = _g4++;
srcj = src[j];
dstj = dst[j];
let _g = 0;
while(_g < i) {
let k = _g++;
dstj[k] -= dsti[k] * srcj[i];
}
dstj[i] = -diag * srcj[i];
let _g1 = i + 1;
while(_g1 < size) {
let k = _g1++;
srcj[k] -= srci[k] * srcj[i];
}
}
}
let _g2 = 1;
while(_g2 < size) {
let i = _g2++;
dsti = dst[i];
let _g = 0;
while(_g < i) {
let j = _g++;
dst[j][i] = dsti[j];
}
}
}
}
computeInvMass(info,massData) {
let invMass = this._invMass;
let invMassWithoutCfm = this._invMassWithoutCfm;
let numRows = info.numRows;
let b1 = info.b1;
let b2 = info.b2;
let invM1 = b1._invMass;
let invM2 = b2._invMass;
let invI100;
let invI101;
let invI102;
let invI110;
let invI111;
let invI112;
let invI120;
let invI121;
let invI122;
let invI200;
let invI201;
let invI202;
let invI210;
let invI211;
let invI212;
let invI220;
let invI221;
let invI222;
invI100 = b1._invInertia00;
invI101 = b1._invInertia01;
invI102 = b1._invInertia02;
invI110 = b1._invInertia10;
invI111 = b1._invInertia11;
invI112 = b1._invInertia12;
invI120 = b1._invInertia20;
invI121 = b1._invInertia21;
invI122 = b1._invInertia22;
invI200 = b2._invInertia00;
invI201 = b2._invInertia01;
invI202 = b2._invInertia02;
invI210 = b2._invInertia10;
invI211 = b2._invInertia11;
invI212 = b2._invInertia12;
invI220 = b2._invInertia20;
invI221 = b2._invInertia21;
invI222 = b2._invInertia22;
let _g = 0;
while(_g < numRows) {
let i = _g++;
let j = info.rows[i].jacobian;
let md = massData[i];
j.updateSparsity();
if((j.flag & 1) != 0) {
md.invMLin1X = j.lin1X * invM1;
md.invMLin1Y = j.lin1Y * invM1;
md.invMLin1Z = j.lin1Z * invM1;
md.invMLin2X = j.lin2X * invM2;
md.invMLin2Y = j.lin2Y * invM2;
md.invMLin2Z = j.lin2Z * invM2;
} else {
md.invMLin1X = 0;
md.invMLin1Y = 0;
md.invMLin1Z = 0;
md.invMLin2X = 0;
md.invMLin2Y = 0;
md.invMLin2Z = 0;
}
if((j.flag & 2) != 0) {
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = invI100 * j.ang1X + invI101 * j.ang1Y + invI102 * j.ang1Z;
__tmp__Y = invI110 * j.ang1X + invI111 * j.ang1Y + invI112 * j.ang1Z;
__tmp__Z = invI120 * j.ang1X + invI121 * j.ang1Y + invI122 * j.ang1Z;
md.invMAng1X = __tmp__X;
md.invMAng1Y = __tmp__Y;
md.invMAng1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = invI200 * j.ang2X + invI201 * j.ang2Y + invI202 * j.ang2Z;
__tmp__Y1 = invI210 * j.ang2X + invI211 * j.ang2Y + invI212 * j.ang2Z;
__tmp__Z1 = invI220 * j.ang2X + invI221 * j.ang2Y + invI222 * j.ang2Z;
md.invMAng2X = __tmp__X1;
md.invMAng2Y = __tmp__Y1;
md.invMAng2Z = __tmp__Z1;
} else {
md.invMAng1X = 0;
md.invMAng1Y = 0;
md.invMAng1Z = 0;
md.invMAng2X = 0;
md.invMAng2Y = 0;
md.invMAng2Z = 0;
}
}
let _g1 = 0;
while(_g1 < numRows) {
let i = _g1++;
let j1 = info.rows[i].jacobian;
let _g = i;
while(_g < numRows) {
let j = _g++;
let md2 = massData[j];
let val = j1.lin1X * md2.invMLin1X + j1.lin1Y * md2.invMLin1Y + j1.lin1Z * md2.invMLin1Z + (j1.ang1X * md2.invMAng1X + j1.ang1Y * md2.invMAng1Y + j1.ang1Z * md2.invMAng1Z) + (j1.lin2X * md2.invMLin2X + j1.lin2Y * md2.invMLin2Y + j1.lin2Z * md2.invMLin2Z) + (j1.ang2X * md2.invMAng2X + j1.ang2Y * md2.invMAng2Y + j1.ang2Z * md2.invMAng2Z);
if(i == j) {
invMass[i][j] = val + info.rows[i].cfm;
invMassWithoutCfm[i][j] = val;
md2.mass = val + info.rows[i].cfm;
md2.massWithoutCfm = val;
if(md2.mass != 0) {
md2.mass = 1 / md2.mass;
}
if(md2.massWithoutCfm != 0) {
md2.massWithoutCfm = 1 / md2.massWithoutCfm;
}
} else {
invMass[i][j] = val;
invMass[j][i] = val;
invMassWithoutCfm[i][j] = val;
invMassWithoutCfm[j][i] = val;
}
}
}
let _g2 = 0;
let _g3 = this._maxSubmatrixId;
while(_g2 < _g3) this._cacheComputed[_g2++] = false;
}
}
if(!oimo.dynamics.constraint.solver.pgs) oimo.dynamics.constraint.solver.pgs = {};
oimo.dynamics.constraint.solver.pgs.PgsContactConstraintSolver = class oimo_dynamics_constraint_solver_pgs_PgsContactConstraintSolver extends oimo.dynamics.constraint.ConstraintSolver {
constructor(constraint) {
super();
this.constraint = constraint;
this.info = new oimo.dynamics.constraint.info.contact.ContactSolverInfo();
this.massData = new Array(oimo.common.Setting.maxManifoldPoints);
let _g = 0;
let _g1 = this.massData.length;
while(_g < _g1) this.massData[_g++] = new oimo.dynamics.constraint.solver.common.ContactSolverMassDataRow();
}
preSolveVelocity(timeStep) {
this.constraint._getVelocitySolverInfo(timeStep,this.info);
this._b1 = this.info.b1;
this._b2 = this.info.b2;
let invM1 = this._b1._invMass;
let invM2 = this._b2._invMass;
let invI100;
let invI101;
let invI102;
let invI110;
let invI111;
let invI112;
let invI120;
let invI121;
let invI122;
let invI200;
let invI201;
let invI202;
let invI210;
let invI211;
let invI212;
let invI220;
let invI221;
let invI222;
invI100 = this._b1._invInertia00;
invI101 = this._b1._invInertia01;
invI102 = this._b1._invInertia02;
invI110 = this._b1._invInertia10;
invI111 = this._b1._invInertia11;
invI112 = this._b1._invInertia12;
invI120 = this._b1._invInertia20;
invI121 = this._b1._invInertia21;
invI122 = this._b1._invInertia22;
invI200 = this._b2._invInertia00;
invI201 = this._b2._invInertia01;
invI202 = this._b2._invInertia02;
invI210 = this._b2._invInertia10;
invI211 = this._b2._invInertia11;
invI212 = this._b2._invInertia12;
invI220 = this._b2._invInertia20;
invI221 = this._b2._invInertia21;
invI222 = this._b2._invInertia22;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let row = this.info.rows[i];
let md = this.massData[i];
let j = row.jacobianN;
md.invMLinN1X = j.lin1X * invM1;
md.invMLinN1Y = j.lin1Y * invM1;
md.invMLinN1Z = j.lin1Z * invM1;
md.invMLinN2X = j.lin2X * invM2;
md.invMLinN2Y = j.lin2Y * invM2;
md.invMLinN2Z = j.lin2Z * invM2;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = invI100 * j.ang1X + invI101 * j.ang1Y + invI102 * j.ang1Z;
__tmp__Y = invI110 * j.ang1X + invI111 * j.ang1Y + invI112 * j.ang1Z;
__tmp__Z = invI120 * j.ang1X + invI121 * j.ang1Y + invI122 * j.ang1Z;
md.invMAngN1X = __tmp__X;
md.invMAngN1Y = __tmp__Y;
md.invMAngN1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = invI200 * j.ang2X + invI201 * j.ang2Y + invI202 * j.ang2Z;
__tmp__Y1 = invI210 * j.ang2X + invI211 * j.ang2Y + invI212 * j.ang2Z;
__tmp__Z1 = invI220 * j.ang2X + invI221 * j.ang2Y + invI222 * j.ang2Z;
md.invMAngN2X = __tmp__X1;
md.invMAngN2Y = __tmp__Y1;
md.invMAngN2Z = __tmp__Z1;
md.massN = invM1 + invM2 + (md.invMAngN1X * j.ang1X + md.invMAngN1Y * j.ang1Y + md.invMAngN1Z * j.ang1Z) + (md.invMAngN2X * j.ang2X + md.invMAngN2Y * j.ang2Y + md.invMAngN2Z * j.ang2Z);
if(md.massN != 0) {
md.massN = 1 / md.massN;
}
let jt = row.jacobianT;
let jb = row.jacobianB;
md.invMLinT1X = jt.lin1X * invM1;
md.invMLinT1Y = jt.lin1Y * invM1;
md.invMLinT1Z = jt.lin1Z * invM1;
md.invMLinT2X = jt.lin2X * invM2;
md.invMLinT2Y = jt.lin2Y * invM2;
md.invMLinT2Z = jt.lin2Z * invM2;
md.invMLinB1X = jb.lin1X * invM1;
md.invMLinB1Y = jb.lin1Y * invM1;
md.invMLinB1Z = jb.lin1Z * invM1;
md.invMLinB2X = jb.lin2X * invM2;
md.invMLinB2Y = jb.lin2Y * invM2;
md.invMLinB2Z = jb.lin2Z * invM2;
let __tmp__X2;
let __tmp__Y2;
let __tmp__Z2;
__tmp__X2 = invI100 * jt.ang1X + invI101 * jt.ang1Y + invI102 * jt.ang1Z;
__tmp__Y2 = invI110 * jt.ang1X + invI111 * jt.ang1Y + invI112 * jt.ang1Z;
__tmp__Z2 = invI120 * jt.ang1X + invI121 * jt.ang1Y + invI122 * jt.ang1Z;
md.invMAngT1X = __tmp__X2;
md.invMAngT1Y = __tmp__Y2;
md.invMAngT1Z = __tmp__Z2;
let __tmp__X3;
let __tmp__Y3;
let __tmp__Z3;
__tmp__X3 = invI200 * jt.ang2X + invI201 * jt.ang2Y + invI202 * jt.ang2Z;
__tmp__Y3 = invI210 * jt.ang2X + invI211 * jt.ang2Y + invI212 * jt.ang2Z;
__tmp__Z3 = invI220 * jt.ang2X + invI221 * jt.ang2Y + invI222 * jt.ang2Z;
md.invMAngT2X = __tmp__X3;
md.invMAngT2Y = __tmp__Y3;
md.invMAngT2Z = __tmp__Z3;
let __tmp__X4;
let __tmp__Y4;
let __tmp__Z4;
__tmp__X4 = invI100 * jb.ang1X + invI101 * jb.ang1Y + invI102 * jb.ang1Z;
__tmp__Y4 = invI110 * jb.ang1X + invI111 * jb.ang1Y + invI112 * jb.ang1Z;
__tmp__Z4 = invI120 * jb.ang1X + invI121 * jb.ang1Y + invI122 * jb.ang1Z;
md.invMAngB1X = __tmp__X4;
md.invMAngB1Y = __tmp__Y4;
md.invMAngB1Z = __tmp__Z4;
let __tmp__X5;
let __tmp__Y5;
let __tmp__Z5;
__tmp__X5 = invI200 * jb.ang2X + invI201 * jb.ang2Y + invI202 * jb.ang2Z;
__tmp__Y5 = invI210 * jb.ang2X + invI211 * jb.ang2Y + invI212 * jb.ang2Z;
__tmp__Z5 = invI220 * jb.ang2X + invI221 * jb.ang2Y + invI222 * jb.ang2Z;
md.invMAngB2X = __tmp__X5;
md.invMAngB2Y = __tmp__Y5;
md.invMAngB2Z = __tmp__Z5;
let invMassTB00 = invM1 + invM2 + (md.invMAngT1X * jt.ang1X + md.invMAngT1Y * jt.ang1Y + md.invMAngT1Z * jt.ang1Z) + (md.invMAngT2X * jt.ang2X + md.invMAngT2Y * jt.ang2Y + md.invMAngT2Z * jt.ang2Z);
let invMassTB01 = md.invMAngT1X * jb.ang1X + md.invMAngT1Y * jb.ang1Y + md.invMAngT1Z * jb.ang1Z + (md.invMAngT2X * jb.ang2X + md.invMAngT2Y * jb.ang2Y + md.invMAngT2Z * jb.ang2Z);
let invMassTB11 = invM1 + invM2 + (md.invMAngB1X * jb.ang1X + md.invMAngB1Y * jb.ang1Y + md.invMAngB1Z * jb.ang1Z) + (md.invMAngB2X * jb.ang2X + md.invMAngB2Y * jb.ang2Y + md.invMAngB2Z * jb.ang2Z);
let invDet = invMassTB00 * invMassTB11 - invMassTB01 * invMassTB01;
if(invDet != 0) {
invDet = 1 / invDet;
}
md.massTB00 = invMassTB11 * invDet;
md.massTB01 = -invMassTB01 * invDet;
md.massTB10 = -invMassTB01 * invDet;
md.massTB11 = invMassTB00 * invDet;
}
}
warmStart(timeStep) {
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._velX;
lv1Y = this._b1._velY;
lv1Z = this._b1._velZ;
lv2X = this._b2._velX;
lv2Y = this._b2._velY;
lv2Z = this._b2._velZ;
av1X = this._b1._angVelX;
av1Y = this._b1._angVelY;
av1Z = this._b1._angVelZ;
av2X = this._b2._angVelX;
av2Y = this._b2._angVelY;
av2Z = this._b2._angVelZ;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let row = this.info.rows[i];
let imp = row.impulse;
let md = this.massData[i];
let jt = row.jacobianT;
let jb = row.jacobianB;
let impulseN = imp.impulseN;
let impulseT = imp.impulseLX * jt.lin1X + imp.impulseLY * jt.lin1Y + imp.impulseLZ * jt.lin1Z;
let impulseB = imp.impulseLX * jb.lin1X + imp.impulseLY * jb.lin1Y + imp.impulseLZ * jb.lin1Z;
imp.impulseT = impulseT;
imp.impulseB = impulseB;
imp.impulseN *= timeStep.dtRatio;
imp.impulseT *= timeStep.dtRatio;
imp.impulseB *= timeStep.dtRatio;
lv1X += md.invMLinN1X * impulseN;
lv1Y += md.invMLinN1Y * impulseN;
lv1Z += md.invMLinN1Z * impulseN;
lv1X += md.invMLinT1X * impulseT;
lv1Y += md.invMLinT1Y * impulseT;
lv1Z += md.invMLinT1Z * impulseT;
lv1X += md.invMLinB1X * impulseB;
lv1Y += md.invMLinB1Y * impulseB;
lv1Z += md.invMLinB1Z * impulseB;
lv2X += md.invMLinN2X * -impulseN;
lv2Y += md.invMLinN2Y * -impulseN;
lv2Z += md.invMLinN2Z * -impulseN;
lv2X += md.invMLinT2X * -impulseT;
lv2Y += md.invMLinT2Y * -impulseT;
lv2Z += md.invMLinT2Z * -impulseT;
lv2X += md.invMLinB2X * -impulseB;
lv2Y += md.invMLinB2Y * -impulseB;
lv2Z += md.invMLinB2Z * -impulseB;
av1X += md.invMAngN1X * impulseN;
av1Y += md.invMAngN1Y * impulseN;
av1Z += md.invMAngN1Z * impulseN;
av1X += md.invMAngT1X * impulseT;
av1Y += md.invMAngT1Y * impulseT;
av1Z += md.invMAngT1Z * impulseT;
av1X += md.invMAngB1X * impulseB;
av1Y += md.invMAngB1Y * impulseB;
av1Z += md.invMAngB1Z * impulseB;
av2X += md.invMAngN2X * -impulseN;
av2Y += md.invMAngN2Y * -impulseN;
av2Z += md.invMAngN2Z * -impulseN;
av2X += md.invMAngT2X * -impulseT;
av2Y += md.invMAngT2Y * -impulseT;
av2Z += md.invMAngT2Z * -impulseT;
av2X += md.invMAngB2X * -impulseB;
av2Y += md.invMAngB2Y * -impulseB;
av2Z += md.invMAngB2Z * -impulseB;
}
this._b1._velX = lv1X;
this._b1._velY = lv1Y;
this._b1._velZ = lv1Z;
this._b2._velX = lv2X;
this._b2._velY = lv2Y;
this._b2._velZ = lv2Z;
this._b1._angVelX = av1X;
this._b1._angVelY = av1Y;
this._b1._angVelZ = av1Z;
this._b2._angVelX = av2X;
this._b2._angVelY = av2Y;
this._b2._angVelZ = av2Z;
}
solveVelocity() {
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._velX;
lv1Y = this._b1._velY;
lv1Z = this._b1._velZ;
lv2X = this._b2._velX;
lv2Y = this._b2._velY;
lv2Z = this._b2._velZ;
av1X = this._b1._angVelX;
av1Y = this._b1._angVelY;
av1Z = this._b1._angVelZ;
av2X = this._b2._angVelX;
av2Y = this._b2._angVelY;
av2Z = this._b2._angVelZ;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let row = this.info.rows[i];
let md = this.massData[i];
let imp = row.impulse;
let rvt = 0;
let j = row.jacobianT;
rvt += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rvt -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rvt += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rvt -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let rvb = 0;
j = row.jacobianB;
rvb += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rvb -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rvb += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rvb -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let impulseT = -(rvt * md.massTB00 + rvb * md.massTB01);
let impulseB = -(rvt * md.massTB10 + rvb * md.massTB11);
let oldImpulseT = imp.impulseT;
let oldImpulseB = imp.impulseB;
imp.impulseT += impulseT;
imp.impulseB += impulseB;
let maxImpulse = row.friction * imp.impulseN;
if(maxImpulse == 0) {
imp.impulseT = 0;
imp.impulseB = 0;
} else {
let impulseLengthSq = imp.impulseT * imp.impulseT + imp.impulseB * imp.impulseB;
if(impulseLengthSq > maxImpulse * maxImpulse) {
let invL = maxImpulse / Math.sqrt(impulseLengthSq);
imp.impulseT *= invL;
imp.impulseB *= invL;
}
}
impulseT = imp.impulseT - oldImpulseT;
impulseB = imp.impulseB - oldImpulseB;
lv1X += md.invMLinT1X * impulseT;
lv1Y += md.invMLinT1Y * impulseT;
lv1Z += md.invMLinT1Z * impulseT;
lv1X += md.invMLinB1X * impulseB;
lv1Y += md.invMLinB1Y * impulseB;
lv1Z += md.invMLinB1Z * impulseB;
lv2X += md.invMLinT2X * -impulseT;
lv2Y += md.invMLinT2Y * -impulseT;
lv2Z += md.invMLinT2Z * -impulseT;
lv2X += md.invMLinB2X * -impulseB;
lv2Y += md.invMLinB2Y * -impulseB;
lv2Z += md.invMLinB2Z * -impulseB;
av1X += md.invMAngT1X * impulseT;
av1Y += md.invMAngT1Y * impulseT;
av1Z += md.invMAngT1Z * impulseT;
av1X += md.invMAngB1X * impulseB;
av1Y += md.invMAngB1Y * impulseB;
av1Z += md.invMAngB1Z * impulseB;
av2X += md.invMAngT2X * -impulseT;
av2Y += md.invMAngT2Y * -impulseT;
av2Z += md.invMAngT2Z * -impulseT;
av2X += md.invMAngB2X * -impulseB;
av2Y += md.invMAngB2Y * -impulseB;
av2Z += md.invMAngB2Z * -impulseB;
}
let _g2 = 0;
let _g3 = this.info.numRows;
while(_g2 < _g3) {
let i = _g2++;
let row = this.info.rows[i];
let md = this.massData[i];
let imp = row.impulse;
let rvn = 0;
let j = row.jacobianN;
rvn += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rvn -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rvn += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rvn -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let impulseN = (row.rhs - rvn) * md.massN;
let oldImpulseN = imp.impulseN;
imp.impulseN += impulseN;
if(imp.impulseN < 0) {
imp.impulseN = 0;
}
impulseN = imp.impulseN - oldImpulseN;
lv1X += md.invMLinN1X * impulseN;
lv1Y += md.invMLinN1Y * impulseN;
lv1Z += md.invMLinN1Z * impulseN;
lv2X += md.invMLinN2X * -impulseN;
lv2Y += md.invMLinN2Y * -impulseN;
lv2Z += md.invMLinN2Z * -impulseN;
av1X += md.invMAngN1X * impulseN;
av1Y += md.invMAngN1Y * impulseN;
av1Z += md.invMAngN1Z * impulseN;
av2X += md.invMAngN2X * -impulseN;
av2Y += md.invMAngN2Y * -impulseN;
av2Z += md.invMAngN2Z * -impulseN;
}
this._b1._velX = lv1X;
this._b1._velY = lv1Y;
this._b1._velZ = lv1Z;
this._b2._velX = lv2X;
this._b2._velY = lv2Y;
this._b2._velZ = lv2Z;
this._b1._angVelX = av1X;
this._b1._angVelY = av1Y;
this._b1._angVelZ = av1Z;
this._b2._angVelX = av2X;
this._b2._angVelY = av2Y;
this._b2._angVelZ = av2Z;
}
preSolvePosition(timeStep) {
this.constraint._syncManifold();
this.constraint._getPositionSolverInfo(this.info);
let invM1 = this._b1._invMass;
let invM2 = this._b2._invMass;
let invI100;
let invI101;
let invI102;
let invI110;
let invI111;
let invI112;
let invI120;
let invI121;
let invI122;
let invI200;
let invI201;
let invI202;
let invI210;
let invI211;
let invI212;
let invI220;
let invI221;
let invI222;
invI100 = this._b1._invInertia00;
invI101 = this._b1._invInertia01;
invI102 = this._b1._invInertia02;
invI110 = this._b1._invInertia10;
invI111 = this._b1._invInertia11;
invI112 = this._b1._invInertia12;
invI120 = this._b1._invInertia20;
invI121 = this._b1._invInertia21;
invI122 = this._b1._invInertia22;
invI200 = this._b2._invInertia00;
invI201 = this._b2._invInertia01;
invI202 = this._b2._invInertia02;
invI210 = this._b2._invInertia10;
invI211 = this._b2._invInertia11;
invI212 = this._b2._invInertia12;
invI220 = this._b2._invInertia20;
invI221 = this._b2._invInertia21;
invI222 = this._b2._invInertia22;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let md = this.massData[i];
let j = this.info.rows[i].jacobianN;
md.invMLinN1X = j.lin1X * invM1;
md.invMLinN1Y = j.lin1Y * invM1;
md.invMLinN1Z = j.lin1Z * invM1;
md.invMLinN2X = j.lin2X * invM2;
md.invMLinN2Y = j.lin2Y * invM2;
md.invMLinN2Z = j.lin2Z * invM2;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = invI100 * j.ang1X + invI101 * j.ang1Y + invI102 * j.ang1Z;
__tmp__Y = invI110 * j.ang1X + invI111 * j.ang1Y + invI112 * j.ang1Z;
__tmp__Z = invI120 * j.ang1X + invI121 * j.ang1Y + invI122 * j.ang1Z;
md.invMAngN1X = __tmp__X;
md.invMAngN1Y = __tmp__Y;
md.invMAngN1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = invI200 * j.ang2X + invI201 * j.ang2Y + invI202 * j.ang2Z;
__tmp__Y1 = invI210 * j.ang2X + invI211 * j.ang2Y + invI212 * j.ang2Z;
__tmp__Z1 = invI220 * j.ang2X + invI221 * j.ang2Y + invI222 * j.ang2Z;
md.invMAngN2X = __tmp__X1;
md.invMAngN2Y = __tmp__Y1;
md.invMAngN2Z = __tmp__Z1;
md.massN = invM1 + invM2 + (md.invMAngN1X * j.ang1X + md.invMAngN1Y * j.ang1Y + md.invMAngN1Z * j.ang1Z) + (md.invMAngN2X * j.ang2X + md.invMAngN2Y * j.ang2Y + md.invMAngN2Z * j.ang2Z);
if(md.massN != 0) {
md.massN = 1 / md.massN;
}
}
let _g2 = 0;
let _g3 = this.info.numRows;
while(_g2 < _g3) this.info.rows[_g2++].impulse.impulseP = 0;
}
solvePositionSplitImpulse() {
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._pseudoVelX;
lv1Y = this._b1._pseudoVelY;
lv1Z = this._b1._pseudoVelZ;
lv2X = this._b2._pseudoVelX;
lv2Y = this._b2._pseudoVelY;
lv2Z = this._b2._pseudoVelZ;
av1X = this._b1._angPseudoVelX;
av1Y = this._b1._angPseudoVelY;
av1Z = this._b1._angPseudoVelZ;
av2X = this._b2._angPseudoVelX;
av2Y = this._b2._angPseudoVelY;
av2Z = this._b2._angPseudoVelZ;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let row = this.info.rows[i];
let md = this.massData[i];
let imp = row.impulse;
let j = row.jacobianN;
let rvn = 0;
rvn += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rvn -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rvn += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rvn -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let impulseP = (row.rhs - rvn) * md.massN * oimo.common.Setting.positionSplitImpulseBaumgarte;
let oldImpulseP = imp.impulseP;
imp.impulseP += impulseP;
if(imp.impulseP < 0) {
imp.impulseP = 0;
}
impulseP = imp.impulseP - oldImpulseP;
lv1X += md.invMLinN1X * impulseP;
lv1Y += md.invMLinN1Y * impulseP;
lv1Z += md.invMLinN1Z * impulseP;
lv2X += md.invMLinN2X * -impulseP;
lv2Y += md.invMLinN2Y * -impulseP;
lv2Z += md.invMLinN2Z * -impulseP;
av1X += md.invMAngN1X * impulseP;
av1Y += md.invMAngN1Y * impulseP;
av1Z += md.invMAngN1Z * impulseP;
av2X += md.invMAngN2X * -impulseP;
av2Y += md.invMAngN2Y * -impulseP;
av2Z += md.invMAngN2Z * -impulseP;
}
this._b1._pseudoVelX = lv1X;
this._b1._pseudoVelY = lv1Y;
this._b1._pseudoVelZ = lv1Z;
this._b2._pseudoVelX = lv2X;
this._b2._pseudoVelY = lv2Y;
this._b2._pseudoVelZ = lv2Z;
this._b1._angPseudoVelX = av1X;
this._b1._angPseudoVelY = av1Y;
this._b1._angPseudoVelZ = av1Z;
this._b2._angPseudoVelX = av2X;
this._b2._angPseudoVelY = av2Y;
this._b2._angPseudoVelZ = av2Z;
}
solvePositionNgs(timeStep) {
this.constraint._syncManifold();
this.constraint._getPositionSolverInfo(this.info);
let invM1 = this._b1._invMass;
let invM2 = this._b2._invMass;
let invI100;
let invI101;
let invI102;
let invI110;
let invI111;
let invI112;
let invI120;
let invI121;
let invI122;
let invI200;
let invI201;
let invI202;
let invI210;
let invI211;
let invI212;
let invI220;
let invI221;
let invI222;
invI100 = this._b1._invInertia00;
invI101 = this._b1._invInertia01;
invI102 = this._b1._invInertia02;
invI110 = this._b1._invInertia10;
invI111 = this._b1._invInertia11;
invI112 = this._b1._invInertia12;
invI120 = this._b1._invInertia20;
invI121 = this._b1._invInertia21;
invI122 = this._b1._invInertia22;
invI200 = this._b2._invInertia00;
invI201 = this._b2._invInertia01;
invI202 = this._b2._invInertia02;
invI210 = this._b2._invInertia10;
invI211 = this._b2._invInertia11;
invI212 = this._b2._invInertia12;
invI220 = this._b2._invInertia20;
invI221 = this._b2._invInertia21;
invI222 = this._b2._invInertia22;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let md = this.massData[i];
let j = this.info.rows[i].jacobianN;
md.invMLinN1X = j.lin1X * invM1;
md.invMLinN1Y = j.lin1Y * invM1;
md.invMLinN1Z = j.lin1Z * invM1;
md.invMLinN2X = j.lin2X * invM2;
md.invMLinN2Y = j.lin2Y * invM2;
md.invMLinN2Z = j.lin2Z * invM2;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = invI100 * j.ang1X + invI101 * j.ang1Y + invI102 * j.ang1Z;
__tmp__Y = invI110 * j.ang1X + invI111 * j.ang1Y + invI112 * j.ang1Z;
__tmp__Z = invI120 * j.ang1X + invI121 * j.ang1Y + invI122 * j.ang1Z;
md.invMAngN1X = __tmp__X;
md.invMAngN1Y = __tmp__Y;
md.invMAngN1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = invI200 * j.ang2X + invI201 * j.ang2Y + invI202 * j.ang2Z;
__tmp__Y1 = invI210 * j.ang2X + invI211 * j.ang2Y + invI212 * j.ang2Z;
__tmp__Z1 = invI220 * j.ang2X + invI221 * j.ang2Y + invI222 * j.ang2Z;
md.invMAngN2X = __tmp__X1;
md.invMAngN2Y = __tmp__Y1;
md.invMAngN2Z = __tmp__Z1;
md.massN = invM1 + invM2 + (md.invMAngN1X * j.ang1X + md.invMAngN1Y * j.ang1Y + md.invMAngN1Z * j.ang1Z) + (md.invMAngN2X * j.ang2X + md.invMAngN2Y * j.ang2Y + md.invMAngN2Z * j.ang2Z);
if(md.massN != 0) {
md.massN = 1 / md.massN;
}
}
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = 0;
lv1Y = 0;
lv1Z = 0;
lv2X = 0;
lv2Y = 0;
lv2Z = 0;
av1X = 0;
av1Y = 0;
av1Z = 0;
av2X = 0;
av2Y = 0;
av2Z = 0;
let _g2 = 0;
let _g3 = this.info.numRows;
while(_g2 < _g3) {
let i = _g2++;
let row = this.info.rows[i];
let md = this.massData[i];
let imp = row.impulse;
let j = row.jacobianN;
let rvn = 0;
rvn += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rvn -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rvn += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rvn -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let impulseP = (row.rhs - rvn) * md.massN * oimo.common.Setting.positionNgsBaumgarte;
let oldImpulseP = imp.impulseP;
imp.impulseP += impulseP;
if(imp.impulseP < 0) {
imp.impulseP = 0;
}
impulseP = imp.impulseP - oldImpulseP;
lv1X += md.invMLinN1X * impulseP;
lv1Y += md.invMLinN1Y * impulseP;
lv1Z += md.invMLinN1Z * impulseP;
lv2X += md.invMLinN2X * -impulseP;
lv2Y += md.invMLinN2Y * -impulseP;
lv2Z += md.invMLinN2Z * -impulseP;
av1X += md.invMAngN1X * impulseP;
av1Y += md.invMAngN1Y * impulseP;
av1Z += md.invMAngN1Z * impulseP;
av2X += md.invMAngN2X * -impulseP;
av2Y += md.invMAngN2Y * -impulseP;
av2Z += md.invMAngN2Z * -impulseP;
}
let _this = this._b1;
_this._transform._positionX += lv1X;
_this._transform._positionY += lv1Y;
_this._transform._positionZ += lv1Z;
let _this1 = this._b2;
_this1._transform._positionX += lv2X;
_this1._transform._positionY += lv2Y;
_this1._transform._positionZ += lv2Z;
let _this2 = this._b1;
let theta = Math.sqrt(av1X * av1X + av1Y * av1Y + av1Z * av1Z);
let halfTheta = theta * 0.5;
let rotationToSinAxisFactor;
let cosHalfTheta;
if(halfTheta < 0.5) {
let ht2 = halfTheta * halfTheta;
rotationToSinAxisFactor = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
cosHalfTheta = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
} else {
rotationToSinAxisFactor = Math.sin(halfTheta) / theta;
cosHalfTheta = Math.cos(halfTheta);
}
let sinAxisX;
let sinAxisY;
let sinAxisZ;
sinAxisX = av1X * rotationToSinAxisFactor;
sinAxisY = av1Y * rotationToSinAxisFactor;
sinAxisZ = av1Z * rotationToSinAxisFactor;
let dqX;
let dqY;
let dqZ;
let dqW;
dqX = sinAxisX;
dqY = sinAxisY;
dqZ = sinAxisZ;
dqW = cosHalfTheta;
let qX;
let qY;
let qZ;
let qW;
let e00 = _this2._transform._rotation00;
let e11 = _this2._transform._rotation11;
let e22 = _this2._transform._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
qW = 0.5 * s;
s = 0.5 / s;
qX = (_this2._transform._rotation21 - _this2._transform._rotation12) * s;
qY = (_this2._transform._rotation02 - _this2._transform._rotation20) * s;
qZ = (_this2._transform._rotation10 - _this2._transform._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
qX = 0.5 * s;
s = 0.5 / s;
qY = (_this2._transform._rotation01 + _this2._transform._rotation10) * s;
qZ = (_this2._transform._rotation02 + _this2._transform._rotation20) * s;
qW = (_this2._transform._rotation21 - _this2._transform._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (_this2._transform._rotation02 + _this2._transform._rotation20) * s;
qY = (_this2._transform._rotation12 + _this2._transform._rotation21) * s;
qW = (_this2._transform._rotation10 - _this2._transform._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
qY = 0.5 * s;
s = 0.5 / s;
qX = (_this2._transform._rotation01 + _this2._transform._rotation10) * s;
qZ = (_this2._transform._rotation12 + _this2._transform._rotation21) * s;
qW = (_this2._transform._rotation02 - _this2._transform._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (_this2._transform._rotation02 + _this2._transform._rotation20) * s;
qY = (_this2._transform._rotation12 + _this2._transform._rotation21) * s;
qW = (_this2._transform._rotation10 - _this2._transform._rotation01) * s;
}
qX = dqW * qX + dqX * qW + dqY * qZ - dqZ * qY;
qY = dqW * qY - dqX * qZ + dqY * qW + dqZ * qX;
qZ = dqW * qZ + dqX * qY - dqY * qX + dqZ * qW;
qW = dqW * qW - dqX * qX - dqY * qY - dqZ * qZ;
let l = qX * qX + qY * qY + qZ * qZ + qW * qW;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
qX *= l;
qY *= l;
qZ *= l;
qW *= l;
let x = qX;
let y = qY;
let z = qZ;
let w = qW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
_this2._transform._rotation00 = 1 - yy - zz;
_this2._transform._rotation01 = xy - wz;
_this2._transform._rotation02 = xz + wy;
_this2._transform._rotation10 = xy + wz;
_this2._transform._rotation11 = 1 - xx - zz;
_this2._transform._rotation12 = yz - wx;
_this2._transform._rotation20 = xz - wy;
_this2._transform._rotation21 = yz + wx;
_this2._transform._rotation22 = 1 - xx - yy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = _this2._transform._rotation00 * _this2._invLocalInertia00 + _this2._transform._rotation01 * _this2._invLocalInertia10 + _this2._transform._rotation02 * _this2._invLocalInertia20;
__tmp__01 = _this2._transform._rotation00 * _this2._invLocalInertia01 + _this2._transform._rotation01 * _this2._invLocalInertia11 + _this2._transform._rotation02 * _this2._invLocalInertia21;
__tmp__02 = _this2._transform._rotation00 * _this2._invLocalInertia02 + _this2._transform._rotation01 * _this2._invLocalInertia12 + _this2._transform._rotation02 * _this2._invLocalInertia22;
__tmp__10 = _this2._transform._rotation10 * _this2._invLocalInertia00 + _this2._transform._rotation11 * _this2._invLocalInertia10 + _this2._transform._rotation12 * _this2._invLocalInertia20;
__tmp__11 = _this2._transform._rotation10 * _this2._invLocalInertia01 + _this2._transform._rotation11 * _this2._invLocalInertia11 + _this2._transform._rotation12 * _this2._invLocalInertia21;
__tmp__12 = _this2._transform._rotation10 * _this2._invLocalInertia02 + _this2._transform._rotation11 * _this2._invLocalInertia12 + _this2._transform._rotation12 * _this2._invLocalInertia22;
__tmp__20 = _this2._transform._rotation20 * _this2._invLocalInertia00 + _this2._transform._rotation21 * _this2._invLocalInertia10 + _this2._transform._rotation22 * _this2._invLocalInertia20;
__tmp__21 = _this2._transform._rotation20 * _this2._invLocalInertia01 + _this2._transform._rotation21 * _this2._invLocalInertia11 + _this2._transform._rotation22 * _this2._invLocalInertia21;
__tmp__22 = _this2._transform._rotation20 * _this2._invLocalInertia02 + _this2._transform._rotation21 * _this2._invLocalInertia12 + _this2._transform._rotation22 * _this2._invLocalInertia22;
_this2._invInertia00 = __tmp__00;
_this2._invInertia01 = __tmp__01;
_this2._invInertia02 = __tmp__02;
_this2._invInertia10 = __tmp__10;
_this2._invInertia11 = __tmp__11;
_this2._invInertia12 = __tmp__12;
_this2._invInertia20 = __tmp__20;
_this2._invInertia21 = __tmp__21;
_this2._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = _this2._invInertia00 * _this2._transform._rotation00 + _this2._invInertia01 * _this2._transform._rotation01 + _this2._invInertia02 * _this2._transform._rotation02;
__tmp__011 = _this2._invInertia00 * _this2._transform._rotation10 + _this2._invInertia01 * _this2._transform._rotation11 + _this2._invInertia02 * _this2._transform._rotation12;
__tmp__021 = _this2._invInertia00 * _this2._transform._rotation20 + _this2._invInertia01 * _this2._transform._rotation21 + _this2._invInertia02 * _this2._transform._rotation22;
__tmp__101 = _this2._invInertia10 * _this2._transform._rotation00 + _this2._invInertia11 * _this2._transform._rotation01 + _this2._invInertia12 * _this2._transform._rotation02;
__tmp__111 = _this2._invInertia10 * _this2._transform._rotation10 + _this2._invInertia11 * _this2._transform._rotation11 + _this2._invInertia12 * _this2._transform._rotation12;
__tmp__121 = _this2._invInertia10 * _this2._transform._rotation20 + _this2._invInertia11 * _this2._transform._rotation21 + _this2._invInertia12 * _this2._transform._rotation22;
__tmp__201 = _this2._invInertia20 * _this2._transform._rotation00 + _this2._invInertia21 * _this2._transform._rotation01 + _this2._invInertia22 * _this2._transform._rotation02;
__tmp__211 = _this2._invInertia20 * _this2._transform._rotation10 + _this2._invInertia21 * _this2._transform._rotation11 + _this2._invInertia22 * _this2._transform._rotation12;
__tmp__221 = _this2._invInertia20 * _this2._transform._rotation20 + _this2._invInertia21 * _this2._transform._rotation21 + _this2._invInertia22 * _this2._transform._rotation22;
_this2._invInertia00 = __tmp__001;
_this2._invInertia01 = __tmp__011;
_this2._invInertia02 = __tmp__021;
_this2._invInertia10 = __tmp__101;
_this2._invInertia11 = __tmp__111;
_this2._invInertia12 = __tmp__121;
_this2._invInertia20 = __tmp__201;
_this2._invInertia21 = __tmp__211;
_this2._invInertia22 = __tmp__221;
_this2._invInertia00 *= _this2._rotFactor.x;
_this2._invInertia01 *= _this2._rotFactor.x;
_this2._invInertia02 *= _this2._rotFactor.x;
_this2._invInertia10 *= _this2._rotFactor.y;
_this2._invInertia11 *= _this2._rotFactor.y;
_this2._invInertia12 *= _this2._rotFactor.y;
_this2._invInertia20 *= _this2._rotFactor.z;
_this2._invInertia21 *= _this2._rotFactor.z;
_this2._invInertia22 *= _this2._rotFactor.z;
let _this3 = this._b2;
let theta1 = Math.sqrt(av2X * av2X + av2Y * av2Y + av2Z * av2Z);
let halfTheta1 = theta1 * 0.5;
let rotationToSinAxisFactor1;
let cosHalfTheta1;
if(halfTheta1 < 0.5) {
let ht2 = halfTheta1 * halfTheta1;
rotationToSinAxisFactor1 = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
cosHalfTheta1 = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
} else {
rotationToSinAxisFactor1 = Math.sin(halfTheta1) / theta1;
cosHalfTheta1 = Math.cos(halfTheta1);
}
let sinAxisX1;
let sinAxisY1;
let sinAxisZ1;
sinAxisX1 = av2X * rotationToSinAxisFactor1;
sinAxisY1 = av2Y * rotationToSinAxisFactor1;
sinAxisZ1 = av2Z * rotationToSinAxisFactor1;
let dqX1;
let dqY1;
let dqZ1;
let dqW1;
dqX1 = sinAxisX1;
dqY1 = sinAxisY1;
dqZ1 = sinAxisZ1;
dqW1 = cosHalfTheta1;
let qX1;
let qY1;
let qZ1;
let qW1;
let e001 = _this3._transform._rotation00;
let e111 = _this3._transform._rotation11;
let e221 = _this3._transform._rotation22;
let t1 = e001 + e111 + e221;
let s1;
if(t1 > 0) {
s1 = Math.sqrt(t1 + 1);
qW1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this3._transform._rotation21 - _this3._transform._rotation12) * s1;
qY1 = (_this3._transform._rotation02 - _this3._transform._rotation20) * s1;
qZ1 = (_this3._transform._rotation10 - _this3._transform._rotation01) * s1;
} else if(e001 > e111) {
if(e001 > e221) {
s1 = Math.sqrt(e001 - e111 - e221 + 1);
qX1 = 0.5 * s1;
s1 = 0.5 / s1;
qY1 = (_this3._transform._rotation01 + _this3._transform._rotation10) * s1;
qZ1 = (_this3._transform._rotation02 + _this3._transform._rotation20) * s1;
qW1 = (_this3._transform._rotation21 - _this3._transform._rotation12) * s1;
} else {
s1 = Math.sqrt(e221 - e001 - e111 + 1);
qZ1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this3._transform._rotation02 + _this3._transform._rotation20) * s1;
qY1 = (_this3._transform._rotation12 + _this3._transform._rotation21) * s1;
qW1 = (_this3._transform._rotation10 - _this3._transform._rotation01) * s1;
}
} else if(e111 > e221) {
s1 = Math.sqrt(e111 - e221 - e001 + 1);
qY1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this3._transform._rotation01 + _this3._transform._rotation10) * s1;
qZ1 = (_this3._transform._rotation12 + _this3._transform._rotation21) * s1;
qW1 = (_this3._transform._rotation02 - _this3._transform._rotation20) * s1;
} else {
s1 = Math.sqrt(e221 - e001 - e111 + 1);
qZ1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this3._transform._rotation02 + _this3._transform._rotation20) * s1;
qY1 = (_this3._transform._rotation12 + _this3._transform._rotation21) * s1;
qW1 = (_this3._transform._rotation10 - _this3._transform._rotation01) * s1;
}
qX1 = dqW1 * qX1 + dqX1 * qW1 + dqY1 * qZ1 - dqZ1 * qY1;
qY1 = dqW1 * qY1 - dqX1 * qZ1 + dqY1 * qW1 + dqZ1 * qX1;
qZ1 = dqW1 * qZ1 + dqX1 * qY1 - dqY1 * qX1 + dqZ1 * qW1;
qW1 = dqW1 * qW1 - dqX1 * qX1 - dqY1 * qY1 - dqZ1 * qZ1;
let l1 = qX1 * qX1 + qY1 * qY1 + qZ1 * qZ1 + qW1 * qW1;
if(l1 > 1e-32) {
l1 = 1 / Math.sqrt(l1);
}
qX1 *= l1;
qY1 *= l1;
qZ1 *= l1;
qW1 *= l1;
let x1 = qX1;
let y1 = qY1;
let z1 = qZ1;
let w1 = qW1;
let x21 = 2 * x1;
let y21 = 2 * y1;
let z21 = 2 * z1;
let xx1 = x1 * x21;
let yy1 = y1 * y21;
let zz1 = z1 * z21;
let xy1 = x1 * y21;
let yz1 = y1 * z21;
let xz1 = x1 * z21;
let wx1 = w1 * x21;
let wy1 = w1 * y21;
let wz1 = w1 * z21;
_this3._transform._rotation00 = 1 - yy1 - zz1;
_this3._transform._rotation01 = xy1 - wz1;
_this3._transform._rotation02 = xz1 + wy1;
_this3._transform._rotation10 = xy1 + wz1;
_this3._transform._rotation11 = 1 - xx1 - zz1;
_this3._transform._rotation12 = yz1 - wx1;
_this3._transform._rotation20 = xz1 - wy1;
_this3._transform._rotation21 = yz1 + wx1;
_this3._transform._rotation22 = 1 - xx1 - yy1;
let __tmp__002;
let __tmp__012;
let __tmp__022;
let __tmp__102;
let __tmp__112;
let __tmp__122;
let __tmp__202;
let __tmp__212;
let __tmp__222;
__tmp__002 = _this3._transform._rotation00 * _this3._invLocalInertia00 + _this3._transform._rotation01 * _this3._invLocalInertia10 + _this3._transform._rotation02 * _this3._invLocalInertia20;
__tmp__012 = _this3._transform._rotation00 * _this3._invLocalInertia01 + _this3._transform._rotation01 * _this3._invLocalInertia11 + _this3._transform._rotation02 * _this3._invLocalInertia21;
__tmp__022 = _this3._transform._rotation00 * _this3._invLocalInertia02 + _this3._transform._rotation01 * _this3._invLocalInertia12 + _this3._transform._rotation02 * _this3._invLocalInertia22;
__tmp__102 = _this3._transform._rotation10 * _this3._invLocalInertia00 + _this3._transform._rotation11 * _this3._invLocalInertia10 + _this3._transform._rotation12 * _this3._invLocalInertia20;
__tmp__112 = _this3._transform._rotation10 * _this3._invLocalInertia01 + _this3._transform._rotation11 * _this3._invLocalInertia11 + _this3._transform._rotation12 * _this3._invLocalInertia21;
__tmp__122 = _this3._transform._rotation10 * _this3._invLocalInertia02 + _this3._transform._rotation11 * _this3._invLocalInertia12 + _this3._transform._rotation12 * _this3._invLocalInertia22;
__tmp__202 = _this3._transform._rotation20 * _this3._invLocalInertia00 + _this3._transform._rotation21 * _this3._invLocalInertia10 + _this3._transform._rotation22 * _this3._invLocalInertia20;
__tmp__212 = _this3._transform._rotation20 * _this3._invLocalInertia01 + _this3._transform._rotation21 * _this3._invLocalInertia11 + _this3._transform._rotation22 * _this3._invLocalInertia21;
__tmp__222 = _this3._transform._rotation20 * _this3._invLocalInertia02 + _this3._transform._rotation21 * _this3._invLocalInertia12 + _this3._transform._rotation22 * _this3._invLocalInertia22;
_this3._invInertia00 = __tmp__002;
_this3._invInertia01 = __tmp__012;
_this3._invInertia02 = __tmp__022;
_this3._invInertia10 = __tmp__102;
_this3._invInertia11 = __tmp__112;
_this3._invInertia12 = __tmp__122;
_this3._invInertia20 = __tmp__202;
_this3._invInertia21 = __tmp__212;
_this3._invInertia22 = __tmp__222;
let __tmp__003;
let __tmp__013;
let __tmp__023;
let __tmp__103;
let __tmp__113;
let __tmp__123;
let __tmp__203;
let __tmp__213;
let __tmp__223;
__tmp__003 = _this3._invInertia00 * _this3._transform._rotation00 + _this3._invInertia01 * _this3._transform._rotation01 + _this3._invInertia02 * _this3._transform._rotation02;
__tmp__013 = _this3._invInertia00 * _this3._transform._rotation10 + _this3._invInertia01 * _this3._transform._rotation11 + _this3._invInertia02 * _this3._transform._rotation12;
__tmp__023 = _this3._invInertia00 * _this3._transform._rotation20 + _this3._invInertia01 * _this3._transform._rotation21 + _this3._invInertia02 * _this3._transform._rotation22;
__tmp__103 = _this3._invInertia10 * _this3._transform._rotation00 + _this3._invInertia11 * _this3._transform._rotation01 + _this3._invInertia12 * _this3._transform._rotation02;
__tmp__113 = _this3._invInertia10 * _this3._transform._rotation10 + _this3._invInertia11 * _this3._transform._rotation11 + _this3._invInertia12 * _this3._transform._rotation12;
__tmp__123 = _this3._invInertia10 * _this3._transform._rotation20 + _this3._invInertia11 * _this3._transform._rotation21 + _this3._invInertia12 * _this3._transform._rotation22;
__tmp__203 = _this3._invInertia20 * _this3._transform._rotation00 + _this3._invInertia21 * _this3._transform._rotation01 + _this3._invInertia22 * _this3._transform._rotation02;
__tmp__213 = _this3._invInertia20 * _this3._transform._rotation10 + _this3._invInertia21 * _this3._transform._rotation11 + _this3._invInertia22 * _this3._transform._rotation12;
__tmp__223 = _this3._invInertia20 * _this3._transform._rotation20 + _this3._invInertia21 * _this3._transform._rotation21 + _this3._invInertia22 * _this3._transform._rotation22;
_this3._invInertia00 = __tmp__003;
_this3._invInertia01 = __tmp__013;
_this3._invInertia02 = __tmp__023;
_this3._invInertia10 = __tmp__103;
_this3._invInertia11 = __tmp__113;
_this3._invInertia12 = __tmp__123;
_this3._invInertia20 = __tmp__203;
_this3._invInertia21 = __tmp__213;
_this3._invInertia22 = __tmp__223;
_this3._invInertia00 *= _this3._rotFactor.x;
_this3._invInertia01 *= _this3._rotFactor.x;
_this3._invInertia02 *= _this3._rotFactor.x;
_this3._invInertia10 *= _this3._rotFactor.y;
_this3._invInertia11 *= _this3._rotFactor.y;
_this3._invInertia12 *= _this3._rotFactor.y;
_this3._invInertia20 *= _this3._rotFactor.z;
_this3._invInertia21 *= _this3._rotFactor.z;
_this3._invInertia22 *= _this3._rotFactor.z;
}
postSolve() {
let lin1X;
let lin1Y;
let lin1Z;
let ang1X;
let ang1Y;
let ang1Z;
let ang2X;
let ang2Y;
let ang2Z;
lin1X = 0;
lin1Y = 0;
lin1Z = 0;
ang1X = 0;
ang1Y = 0;
ang1Z = 0;
ang2X = 0;
ang2Y = 0;
ang2Z = 0;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let row = this.info.rows[_g++];
let imp = row.impulse;
let jn = row.jacobianN;
let jt = row.jacobianT;
let jb = row.jacobianB;
let impN = imp.impulseN;
let impT = imp.impulseT;
let impB = imp.impulseB;
let impulseLX;
let impulseLY;
let impulseLZ;
impulseLX = 0;
impulseLY = 0;
impulseLZ = 0;
impulseLX += jt.lin1X * impT;
impulseLY += jt.lin1Y * impT;
impulseLZ += jt.lin1Z * impT;
impulseLX += jb.lin1X * impB;
impulseLY += jb.lin1Y * impB;
impulseLZ += jb.lin1Z * impB;
imp.impulseLX = impulseLX;
imp.impulseLY = impulseLY;
imp.impulseLZ = impulseLZ;
lin1X += jn.lin1X * impN;
lin1Y += jn.lin1Y * impN;
lin1Z += jn.lin1Z * impN;
ang1X += jn.ang1X * impN;
ang1Y += jn.ang1Y * impN;
ang1Z += jn.ang1Z * impN;
ang2X += jn.ang2X * impN;
ang2Y += jn.ang2Y * impN;
ang2Z += jn.ang2Z * impN;
lin1X += jt.lin1X * impT;
lin1Y += jt.lin1Y * impT;
lin1Z += jt.lin1Z * impT;
ang1X += jt.ang1X * impT;
ang1Y += jt.ang1Y * impT;
ang1Z += jt.ang1Z * impT;
ang2X += jt.ang2X * impT;
ang2Y += jt.ang2Y * impT;
ang2Z += jt.ang2Z * impT;
lin1X += jb.lin1X * impB;
lin1Y += jb.lin1Y * impB;
lin1Z += jb.lin1Z * impB;
ang1X += jb.ang1X * impB;
ang1Y += jb.ang1Y * impB;
ang1Z += jb.ang1Z * impB;
ang2X += jb.ang2X * impB;
ang2Y += jb.ang2Y * impB;
ang2Z += jb.ang2Z * impB;
}
this._b1._linearContactImpulseX += lin1X;
this._b1._linearContactImpulseY += lin1Y;
this._b1._linearContactImpulseZ += lin1Z;
this._b1._angularContactImpulseX += ang1X;
this._b1._angularContactImpulseY += ang1Y;
this._b1._angularContactImpulseZ += ang1Z;
this._b2._linearContactImpulseX -= lin1X;
this._b2._linearContactImpulseY -= lin1Y;
this._b2._linearContactImpulseZ -= lin1Z;
this._b2._angularContactImpulseX -= ang2X;
this._b2._angularContactImpulseY -= ang2Y;
this._b2._angularContactImpulseZ -= ang2Z;
this.constraint._syncManifold();
}
}
oimo.dynamics.constraint.solver.pgs.PgsJointConstraintSolver = class oimo_dynamics_constraint_solver_pgs_PgsJointConstraintSolver extends oimo.dynamics.constraint.ConstraintSolver {
constructor(joint) {
super();
this.joint = joint;
this.info = new oimo.dynamics.constraint.info.joint.JointSolverInfo();
this.massData = new Array(oimo.common.Setting.maxJacobianRows);
let _g = 0;
let _g1 = this.massData.length;
while(_g < _g1) this.massData[_g++] = new oimo.dynamics.constraint.solver.common.JointSolverMassDataRow();
}
preSolveVelocity(timeStep) {
this.joint._syncAnchors();
this.joint._getVelocitySolverInfo(timeStep,this.info);
this._b1 = this.info.b1;
this._b2 = this.info.b2;
let invM1 = this._b1._invMass;
let invM2 = this._b2._invMass;
let invI100;
let invI101;
let invI102;
let invI110;
let invI111;
let invI112;
let invI120;
let invI121;
let invI122;
let invI200;
let invI201;
let invI202;
let invI210;
let invI211;
let invI212;
let invI220;
let invI221;
let invI222;
invI100 = this._b1._invInertia00;
invI101 = this._b1._invInertia01;
invI102 = this._b1._invInertia02;
invI110 = this._b1._invInertia10;
invI111 = this._b1._invInertia11;
invI112 = this._b1._invInertia12;
invI120 = this._b1._invInertia20;
invI121 = this._b1._invInertia21;
invI122 = this._b1._invInertia22;
invI200 = this._b2._invInertia00;
invI201 = this._b2._invInertia01;
invI202 = this._b2._invInertia02;
invI210 = this._b2._invInertia10;
invI211 = this._b2._invInertia11;
invI212 = this._b2._invInertia12;
invI220 = this._b2._invInertia20;
invI221 = this._b2._invInertia21;
invI222 = this._b2._invInertia22;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let row = this.info.rows[i];
let md = this.massData[i];
let j = row.jacobian;
j.updateSparsity();
if((j.flag & 1) != 0) {
md.invMLin1X = j.lin1X * invM1;
md.invMLin1Y = j.lin1Y * invM1;
md.invMLin1Z = j.lin1Z * invM1;
md.invMLin2X = j.lin2X * invM2;
md.invMLin2Y = j.lin2Y * invM2;
md.invMLin2Z = j.lin2Z * invM2;
} else {
md.invMLin1X = 0;
md.invMLin1Y = 0;
md.invMLin1Z = 0;
md.invMLin2X = 0;
md.invMLin2Y = 0;
md.invMLin2Z = 0;
}
if((j.flag & 2) != 0) {
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = invI100 * j.ang1X + invI101 * j.ang1Y + invI102 * j.ang1Z;
__tmp__Y = invI110 * j.ang1X + invI111 * j.ang1Y + invI112 * j.ang1Z;
__tmp__Z = invI120 * j.ang1X + invI121 * j.ang1Y + invI122 * j.ang1Z;
md.invMAng1X = __tmp__X;
md.invMAng1Y = __tmp__Y;
md.invMAng1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = invI200 * j.ang2X + invI201 * j.ang2Y + invI202 * j.ang2Z;
__tmp__Y1 = invI210 * j.ang2X + invI211 * j.ang2Y + invI212 * j.ang2Z;
__tmp__Z1 = invI220 * j.ang2X + invI221 * j.ang2Y + invI222 * j.ang2Z;
md.invMAng2X = __tmp__X1;
md.invMAng2Y = __tmp__Y1;
md.invMAng2Z = __tmp__Z1;
} else {
md.invMAng1X = 0;
md.invMAng1Y = 0;
md.invMAng1Z = 0;
md.invMAng2X = 0;
md.invMAng2Y = 0;
md.invMAng2Z = 0;
}
md.massWithoutCfm = md.invMLin1X * j.lin1X + md.invMLin1Y * j.lin1Y + md.invMLin1Z * j.lin1Z + (md.invMLin2X * j.lin2X + md.invMLin2Y * j.lin2Y + md.invMLin2Z * j.lin2Z) + (md.invMAng1X * j.ang1X + md.invMAng1Y * j.ang1Y + md.invMAng1Z * j.ang1Z) + (md.invMAng2X * j.ang2X + md.invMAng2Y * j.ang2Y + md.invMAng2Z * j.ang2Z);
md.mass = md.massWithoutCfm + row.cfm;
if(md.massWithoutCfm != 0) {
md.massWithoutCfm = 1 / md.massWithoutCfm;
}
if(md.mass != 0) {
md.mass = 1 / md.mass;
}
}
}
warmStart(timeStep) {
let factor = this.joint._positionCorrectionAlgorithm == oimo.dynamics.constraint.PositionCorrectionAlgorithm.BAUMGARTE ? oimo.common.Setting.jointWarmStartingFactorForBaungarte : oimo.common.Setting.jointWarmStartingFactor;
factor *= timeStep.dtRatio;
if(factor <= 0) {
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let _this = this.info.rows[_g++].impulse;
_this.impulse = 0;
_this.impulseM = 0;
_this.impulseP = 0;
}
return;
}
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._velX;
lv1Y = this._b1._velY;
lv1Z = this._b1._velZ;
lv2X = this._b2._velX;
lv2Y = this._b2._velY;
lv2Z = this._b2._velZ;
av1X = this._b1._angVelX;
av1Y = this._b1._angVelY;
av1Z = this._b1._angVelZ;
av2X = this._b2._angVelX;
av2Y = this._b2._angVelY;
av2Z = this._b2._angVelZ;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let md = this.massData[i];
let imp = this.info.rows[i].impulse;
imp.impulse *= factor;
imp.impulseM *= factor;
let impulse = imp.impulse + imp.impulseM;
lv1X += md.invMLin1X * impulse;
lv1Y += md.invMLin1Y * impulse;
lv1Z += md.invMLin1Z * impulse;
lv2X += md.invMLin2X * -impulse;
lv2Y += md.invMLin2Y * -impulse;
lv2Z += md.invMLin2Z * -impulse;
av1X += md.invMAng1X * impulse;
av1Y += md.invMAng1Y * impulse;
av1Z += md.invMAng1Z * impulse;
av2X += md.invMAng2X * -impulse;
av2Y += md.invMAng2Y * -impulse;
av2Z += md.invMAng2Z * -impulse;
}
this._b1._velX = lv1X;
this._b1._velY = lv1Y;
this._b1._velZ = lv1Z;
this._b2._velX = lv2X;
this._b2._velY = lv2Y;
this._b2._velZ = lv2Z;
this._b1._angVelX = av1X;
this._b1._angVelY = av1Y;
this._b1._angVelZ = av1Z;
this._b2._angVelX = av2X;
this._b2._angVelY = av2Y;
this._b2._angVelZ = av2Z;
}
solveVelocity() {
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._velX;
lv1Y = this._b1._velY;
lv1Z = this._b1._velZ;
lv2X = this._b2._velX;
lv2Y = this._b2._velY;
lv2Z = this._b2._velZ;
av1X = this._b1._angVelX;
av1Y = this._b1._angVelY;
av1Z = this._b1._angVelZ;
av2X = this._b2._angVelX;
av2Y = this._b2._angVelY;
av2Z = this._b2._angVelZ;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let row = this.info.rows[i];
let md = this.massData[i];
let imp = row.impulse;
let j = row.jacobian;
if(row.motorMaxImpulse == 0) {
continue;
}
let rv = 0;
rv += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rv -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rv += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rv -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let impulseM = (-row.motorSpeed - rv) * md.massWithoutCfm;
let oldImpulseM = imp.impulseM;
imp.impulseM += impulseM;
if(imp.impulseM < -row.motorMaxImpulse) {
imp.impulseM = -row.motorMaxImpulse;
} else if(imp.impulseM > row.motorMaxImpulse) {
imp.impulseM = row.motorMaxImpulse;
}
impulseM = imp.impulseM - oldImpulseM;
if((j.flag & 1) != 0) {
lv1X += md.invMLin1X * impulseM;
lv1Y += md.invMLin1Y * impulseM;
lv1Z += md.invMLin1Z * impulseM;
lv2X += md.invMLin2X * -impulseM;
lv2Y += md.invMLin2Y * -impulseM;
lv2Z += md.invMLin2Z * -impulseM;
}
if((j.flag & 2) != 0) {
av1X += md.invMAng1X * impulseM;
av1Y += md.invMAng1Y * impulseM;
av1Z += md.invMAng1Z * impulseM;
av2X += md.invMAng2X * -impulseM;
av2Y += md.invMAng2Y * -impulseM;
av2Z += md.invMAng2Z * -impulseM;
}
}
let _g2 = 0;
let _g3 = this.info.numRows;
while(_g2 < _g3) {
let i = _g2++;
let row = this.info.rows[i];
let md = this.massData[i];
let imp = row.impulse;
let j = row.jacobian;
let rv = 0;
rv += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rv -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rv += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rv -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let impulse = (row.rhs - rv - imp.impulse * row.cfm) * md.mass;
let oldImpulse = imp.impulse;
imp.impulse += impulse;
if(imp.impulse < row.minImpulse) {
imp.impulse = row.minImpulse;
} else if(imp.impulse > row.maxImpulse) {
imp.impulse = row.maxImpulse;
}
impulse = imp.impulse - oldImpulse;
if((j.flag & 1) != 0) {
lv1X += md.invMLin1X * impulse;
lv1Y += md.invMLin1Y * impulse;
lv1Z += md.invMLin1Z * impulse;
lv2X += md.invMLin2X * -impulse;
lv2Y += md.invMLin2Y * -impulse;
lv2Z += md.invMLin2Z * -impulse;
}
if((j.flag & 2) != 0) {
av1X += md.invMAng1X * impulse;
av1Y += md.invMAng1Y * impulse;
av1Z += md.invMAng1Z * impulse;
av2X += md.invMAng2X * -impulse;
av2Y += md.invMAng2Y * -impulse;
av2Z += md.invMAng2Z * -impulse;
}
}
this._b1._velX = lv1X;
this._b1._velY = lv1Y;
this._b1._velZ = lv1Z;
this._b2._velX = lv2X;
this._b2._velY = lv2Y;
this._b2._velZ = lv2Z;
this._b1._angVelX = av1X;
this._b1._angVelY = av1Y;
this._b1._angVelZ = av1Z;
this._b2._angVelX = av2X;
this._b2._angVelY = av2Y;
this._b2._angVelZ = av2Z;
}
postSolveVelocity(timeStep) {
let linX;
let linY;
let linZ;
let angX;
let angY;
let angZ;
linX = 0;
linY = 0;
linZ = 0;
angX = 0;
angY = 0;
angZ = 0;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let row = this.info.rows[_g++];
let imp = row.impulse;
let j = row.jacobian;
if((j.flag & 1) != 0) {
linX += j.lin1X * imp.impulse;
linY += j.lin1Y * imp.impulse;
linZ += j.lin1Z * imp.impulse;
} else if((j.flag & 2) != 0) {
angX += j.ang1X * imp.impulse;
angY += j.ang1Y * imp.impulse;
angZ += j.ang1Z * imp.impulse;
}
}
this.joint._appliedForceX = linX * timeStep.invDt;
this.joint._appliedForceY = linY * timeStep.invDt;
this.joint._appliedForceZ = linZ * timeStep.invDt;
this.joint._appliedTorqueX = angX * timeStep.invDt;
this.joint._appliedTorqueY = angY * timeStep.invDt;
this.joint._appliedTorqueZ = angZ * timeStep.invDt;
}
preSolvePosition(timeStep) {
this.joint._syncAnchors();
this.joint._getPositionSolverInfo(this.info);
this._b1 = this.info.b1;
this._b2 = this.info.b2;
let invM1 = this._b1._invMass;
let invM2 = this._b2._invMass;
let invI100;
let invI101;
let invI102;
let invI110;
let invI111;
let invI112;
let invI120;
let invI121;
let invI122;
let invI200;
let invI201;
let invI202;
let invI210;
let invI211;
let invI212;
let invI220;
let invI221;
let invI222;
invI100 = this._b1._invInertia00;
invI101 = this._b1._invInertia01;
invI102 = this._b1._invInertia02;
invI110 = this._b1._invInertia10;
invI111 = this._b1._invInertia11;
invI112 = this._b1._invInertia12;
invI120 = this._b1._invInertia20;
invI121 = this._b1._invInertia21;
invI122 = this._b1._invInertia22;
invI200 = this._b2._invInertia00;
invI201 = this._b2._invInertia01;
invI202 = this._b2._invInertia02;
invI210 = this._b2._invInertia10;
invI211 = this._b2._invInertia11;
invI212 = this._b2._invInertia12;
invI220 = this._b2._invInertia20;
invI221 = this._b2._invInertia21;
invI222 = this._b2._invInertia22;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let md = this.massData[i];
let j = this.info.rows[i].jacobian;
md.invMLin1X = j.lin1X * invM1;
md.invMLin1Y = j.lin1Y * invM1;
md.invMLin1Z = j.lin1Z * invM1;
md.invMLin2X = j.lin2X * invM2;
md.invMLin2Y = j.lin2Y * invM2;
md.invMLin2Z = j.lin2Z * invM2;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = invI100 * j.ang1X + invI101 * j.ang1Y + invI102 * j.ang1Z;
__tmp__Y = invI110 * j.ang1X + invI111 * j.ang1Y + invI112 * j.ang1Z;
__tmp__Z = invI120 * j.ang1X + invI121 * j.ang1Y + invI122 * j.ang1Z;
md.invMAng1X = __tmp__X;
md.invMAng1Y = __tmp__Y;
md.invMAng1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = invI200 * j.ang2X + invI201 * j.ang2Y + invI202 * j.ang2Z;
__tmp__Y1 = invI210 * j.ang2X + invI211 * j.ang2Y + invI212 * j.ang2Z;
__tmp__Z1 = invI220 * j.ang2X + invI221 * j.ang2Y + invI222 * j.ang2Z;
md.invMAng2X = __tmp__X1;
md.invMAng2Y = __tmp__Y1;
md.invMAng2Z = __tmp__Z1;
md.mass = md.invMLin1X * j.lin1X + md.invMLin1Y * j.lin1Y + md.invMLin1Z * j.lin1Z + (md.invMLin2X * j.lin2X + md.invMLin2Y * j.lin2Y + md.invMLin2Z * j.lin2Z) + (md.invMAng1X * j.ang1X + md.invMAng1Y * j.ang1Y + md.invMAng1Z * j.ang1Z) + (md.invMAng2X * j.ang2X + md.invMAng2Y * j.ang2Y + md.invMAng2Z * j.ang2Z);
if(md.mass != 0) {
md.mass = 1 / md.mass;
}
}
let _g2 = 0;
let _g3 = this.info.numRows;
while(_g2 < _g3) this.info.rows[_g2++].impulse.impulseP = 0;
}
solvePositionSplitImpulse() {
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = this._b1._pseudoVelX;
lv1Y = this._b1._pseudoVelY;
lv1Z = this._b1._pseudoVelZ;
lv2X = this._b2._pseudoVelX;
lv2Y = this._b2._pseudoVelY;
lv2Z = this._b2._pseudoVelZ;
av1X = this._b1._angPseudoVelX;
av1Y = this._b1._angPseudoVelY;
av1Z = this._b1._angPseudoVelZ;
av2X = this._b2._angPseudoVelX;
av2Y = this._b2._angPseudoVelY;
av2Z = this._b2._angPseudoVelZ;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let row = this.info.rows[i];
let md = this.massData[i];
let imp = row.impulse;
let j = row.jacobian;
let rv = 0;
rv += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rv -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rv += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rv -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let impulseP = (row.rhs * oimo.common.Setting.positionSplitImpulseBaumgarte - rv) * md.mass;
let oldImpulseP = imp.impulseP;
imp.impulseP += impulseP;
if(imp.impulseP < row.minImpulse) {
imp.impulseP = row.minImpulse;
} else if(imp.impulseP > row.maxImpulse) {
imp.impulseP = row.maxImpulse;
}
impulseP = imp.impulseP - oldImpulseP;
lv1X += md.invMLin1X * impulseP;
lv1Y += md.invMLin1Y * impulseP;
lv1Z += md.invMLin1Z * impulseP;
lv2X += md.invMLin2X * -impulseP;
lv2Y += md.invMLin2Y * -impulseP;
lv2Z += md.invMLin2Z * -impulseP;
av1X += md.invMAng1X * impulseP;
av1Y += md.invMAng1Y * impulseP;
av1Z += md.invMAng1Z * impulseP;
av2X += md.invMAng2X * -impulseP;
av2Y += md.invMAng2Y * -impulseP;
av2Z += md.invMAng2Z * -impulseP;
}
this._b1._pseudoVelX = lv1X;
this._b1._pseudoVelY = lv1Y;
this._b1._pseudoVelZ = lv1Z;
this._b2._pseudoVelX = lv2X;
this._b2._pseudoVelY = lv2Y;
this._b2._pseudoVelZ = lv2Z;
this._b1._angPseudoVelX = av1X;
this._b1._angPseudoVelY = av1Y;
this._b1._angPseudoVelZ = av1Z;
this._b2._angPseudoVelX = av2X;
this._b2._angPseudoVelY = av2Y;
this._b2._angPseudoVelZ = av2Z;
}
solvePositionNgs(timeStep) {
this.joint._syncAnchors();
this.joint._getPositionSolverInfo(this.info);
this._b1 = this.info.b1;
this._b2 = this.info.b2;
let invM1 = this._b1._invMass;
let invM2 = this._b2._invMass;
let invI100;
let invI101;
let invI102;
let invI110;
let invI111;
let invI112;
let invI120;
let invI121;
let invI122;
let invI200;
let invI201;
let invI202;
let invI210;
let invI211;
let invI212;
let invI220;
let invI221;
let invI222;
invI100 = this._b1._invInertia00;
invI101 = this._b1._invInertia01;
invI102 = this._b1._invInertia02;
invI110 = this._b1._invInertia10;
invI111 = this._b1._invInertia11;
invI112 = this._b1._invInertia12;
invI120 = this._b1._invInertia20;
invI121 = this._b1._invInertia21;
invI122 = this._b1._invInertia22;
invI200 = this._b2._invInertia00;
invI201 = this._b2._invInertia01;
invI202 = this._b2._invInertia02;
invI210 = this._b2._invInertia10;
invI211 = this._b2._invInertia11;
invI212 = this._b2._invInertia12;
invI220 = this._b2._invInertia20;
invI221 = this._b2._invInertia21;
invI222 = this._b2._invInertia22;
let _g = 0;
let _g1 = this.info.numRows;
while(_g < _g1) {
let i = _g++;
let md = this.massData[i];
let j = this.info.rows[i].jacobian;
md.invMLin1X = j.lin1X * invM1;
md.invMLin1Y = j.lin1Y * invM1;
md.invMLin1Z = j.lin1Z * invM1;
md.invMLin2X = j.lin2X * invM2;
md.invMLin2Y = j.lin2Y * invM2;
md.invMLin2Z = j.lin2Z * invM2;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = invI100 * j.ang1X + invI101 * j.ang1Y + invI102 * j.ang1Z;
__tmp__Y = invI110 * j.ang1X + invI111 * j.ang1Y + invI112 * j.ang1Z;
__tmp__Z = invI120 * j.ang1X + invI121 * j.ang1Y + invI122 * j.ang1Z;
md.invMAng1X = __tmp__X;
md.invMAng1Y = __tmp__Y;
md.invMAng1Z = __tmp__Z;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = invI200 * j.ang2X + invI201 * j.ang2Y + invI202 * j.ang2Z;
__tmp__Y1 = invI210 * j.ang2X + invI211 * j.ang2Y + invI212 * j.ang2Z;
__tmp__Z1 = invI220 * j.ang2X + invI221 * j.ang2Y + invI222 * j.ang2Z;
md.invMAng2X = __tmp__X1;
md.invMAng2Y = __tmp__Y1;
md.invMAng2Z = __tmp__Z1;
md.mass = md.invMLin1X * j.lin1X + md.invMLin1Y * j.lin1Y + md.invMLin1Z * j.lin1Z + (md.invMLin2X * j.lin2X + md.invMLin2Y * j.lin2Y + md.invMLin2Z * j.lin2Z) + (md.invMAng1X * j.ang1X + md.invMAng1Y * j.ang1Y + md.invMAng1Z * j.ang1Z) + (md.invMAng2X * j.ang2X + md.invMAng2Y * j.ang2Y + md.invMAng2Z * j.ang2Z);
if(md.mass != 0) {
md.mass = 1 / md.mass;
}
}
let lv1X;
let lv1Y;
let lv1Z;
let lv2X;
let lv2Y;
let lv2Z;
let av1X;
let av1Y;
let av1Z;
let av2X;
let av2Y;
let av2Z;
lv1X = 0;
lv1Y = 0;
lv1Z = 0;
lv2X = 0;
lv2Y = 0;
lv2Z = 0;
av1X = 0;
av1Y = 0;
av1Z = 0;
av2X = 0;
av2Y = 0;
av2Z = 0;
let _g2 = 0;
let _g3 = this.info.numRows;
while(_g2 < _g3) {
let i = _g2++;
let row = this.info.rows[i];
let md = this.massData[i];
let imp = row.impulse;
let j = row.jacobian;
let rv = 0;
rv += lv1X * j.lin1X + lv1Y * j.lin1Y + lv1Z * j.lin1Z;
rv -= lv2X * j.lin2X + lv2Y * j.lin2Y + lv2Z * j.lin2Z;
rv += av1X * j.ang1X + av1Y * j.ang1Y + av1Z * j.ang1Z;
rv -= av2X * j.ang2X + av2Y * j.ang2Y + av2Z * j.ang2Z;
let impulseP = (row.rhs * oimo.common.Setting.positionNgsBaumgarte - rv) * md.mass;
let oldImpulseP = imp.impulseP;
imp.impulseP += impulseP;
if(imp.impulseP < row.minImpulse) {
imp.impulseP = row.minImpulse;
} else if(imp.impulseP > row.maxImpulse) {
imp.impulseP = row.maxImpulse;
}
impulseP = imp.impulseP - oldImpulseP;
lv1X += md.invMLin1X * impulseP;
lv1Y += md.invMLin1Y * impulseP;
lv1Z += md.invMLin1Z * impulseP;
lv2X += md.invMLin2X * -impulseP;
lv2Y += md.invMLin2Y * -impulseP;
lv2Z += md.invMLin2Z * -impulseP;
av1X += md.invMAng1X * impulseP;
av1Y += md.invMAng1Y * impulseP;
av1Z += md.invMAng1Z * impulseP;
av2X += md.invMAng2X * -impulseP;
av2Y += md.invMAng2Y * -impulseP;
av2Z += md.invMAng2Z * -impulseP;
}
let _this = this._b1;
_this._transform._positionX += lv1X;
_this._transform._positionY += lv1Y;
_this._transform._positionZ += lv1Z;
let _this1 = this._b2;
_this1._transform._positionX += lv2X;
_this1._transform._positionY += lv2Y;
_this1._transform._positionZ += lv2Z;
let _this2 = this._b1;
let theta = Math.sqrt(av1X * av1X + av1Y * av1Y + av1Z * av1Z);
let halfTheta = theta * 0.5;
let rotationToSinAxisFactor;
let cosHalfTheta;
if(halfTheta < 0.5) {
let ht2 = halfTheta * halfTheta;
rotationToSinAxisFactor = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
cosHalfTheta = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
} else {
rotationToSinAxisFactor = Math.sin(halfTheta) / theta;
cosHalfTheta = Math.cos(halfTheta);
}
let sinAxisX;
let sinAxisY;
let sinAxisZ;
sinAxisX = av1X * rotationToSinAxisFactor;
sinAxisY = av1Y * rotationToSinAxisFactor;
sinAxisZ = av1Z * rotationToSinAxisFactor;
let dqX;
let dqY;
let dqZ;
let dqW;
dqX = sinAxisX;
dqY = sinAxisY;
dqZ = sinAxisZ;
dqW = cosHalfTheta;
let qX;
let qY;
let qZ;
let qW;
let e00 = _this2._transform._rotation00;
let e11 = _this2._transform._rotation11;
let e22 = _this2._transform._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
qW = 0.5 * s;
s = 0.5 / s;
qX = (_this2._transform._rotation21 - _this2._transform._rotation12) * s;
qY = (_this2._transform._rotation02 - _this2._transform._rotation20) * s;
qZ = (_this2._transform._rotation10 - _this2._transform._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
qX = 0.5 * s;
s = 0.5 / s;
qY = (_this2._transform._rotation01 + _this2._transform._rotation10) * s;
qZ = (_this2._transform._rotation02 + _this2._transform._rotation20) * s;
qW = (_this2._transform._rotation21 - _this2._transform._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (_this2._transform._rotation02 + _this2._transform._rotation20) * s;
qY = (_this2._transform._rotation12 + _this2._transform._rotation21) * s;
qW = (_this2._transform._rotation10 - _this2._transform._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
qY = 0.5 * s;
s = 0.5 / s;
qX = (_this2._transform._rotation01 + _this2._transform._rotation10) * s;
qZ = (_this2._transform._rotation12 + _this2._transform._rotation21) * s;
qW = (_this2._transform._rotation02 - _this2._transform._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (_this2._transform._rotation02 + _this2._transform._rotation20) * s;
qY = (_this2._transform._rotation12 + _this2._transform._rotation21) * s;
qW = (_this2._transform._rotation10 - _this2._transform._rotation01) * s;
}
qX = dqW * qX + dqX * qW + dqY * qZ - dqZ * qY;
qY = dqW * qY - dqX * qZ + dqY * qW + dqZ * qX;
qZ = dqW * qZ + dqX * qY - dqY * qX + dqZ * qW;
qW = dqW * qW - dqX * qX - dqY * qY - dqZ * qZ;
let l = qX * qX + qY * qY + qZ * qZ + qW * qW;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
qX *= l;
qY *= l;
qZ *= l;
qW *= l;
let x = qX;
let y = qY;
let z = qZ;
let w = qW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
_this2._transform._rotation00 = 1 - yy - zz;
_this2._transform._rotation01 = xy - wz;
_this2._transform._rotation02 = xz + wy;
_this2._transform._rotation10 = xy + wz;
_this2._transform._rotation11 = 1 - xx - zz;
_this2._transform._rotation12 = yz - wx;
_this2._transform._rotation20 = xz - wy;
_this2._transform._rotation21 = yz + wx;
_this2._transform._rotation22 = 1 - xx - yy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = _this2._transform._rotation00 * _this2._invLocalInertia00 + _this2._transform._rotation01 * _this2._invLocalInertia10 + _this2._transform._rotation02 * _this2._invLocalInertia20;
__tmp__01 = _this2._transform._rotation00 * _this2._invLocalInertia01 + _this2._transform._rotation01 * _this2._invLocalInertia11 + _this2._transform._rotation02 * _this2._invLocalInertia21;
__tmp__02 = _this2._transform._rotation00 * _this2._invLocalInertia02 + _this2._transform._rotation01 * _this2._invLocalInertia12 + _this2._transform._rotation02 * _this2._invLocalInertia22;
__tmp__10 = _this2._transform._rotation10 * _this2._invLocalInertia00 + _this2._transform._rotation11 * _this2._invLocalInertia10 + _this2._transform._rotation12 * _this2._invLocalInertia20;
__tmp__11 = _this2._transform._rotation10 * _this2._invLocalInertia01 + _this2._transform._rotation11 * _this2._invLocalInertia11 + _this2._transform._rotation12 * _this2._invLocalInertia21;
__tmp__12 = _this2._transform._rotation10 * _this2._invLocalInertia02 + _this2._transform._rotation11 * _this2._invLocalInertia12 + _this2._transform._rotation12 * _this2._invLocalInertia22;
__tmp__20 = _this2._transform._rotation20 * _this2._invLocalInertia00 + _this2._transform._rotation21 * _this2._invLocalInertia10 + _this2._transform._rotation22 * _this2._invLocalInertia20;
__tmp__21 = _this2._transform._rotation20 * _this2._invLocalInertia01 + _this2._transform._rotation21 * _this2._invLocalInertia11 + _this2._transform._rotation22 * _this2._invLocalInertia21;
__tmp__22 = _this2._transform._rotation20 * _this2._invLocalInertia02 + _this2._transform._rotation21 * _this2._invLocalInertia12 + _this2._transform._rotation22 * _this2._invLocalInertia22;
_this2._invInertia00 = __tmp__00;
_this2._invInertia01 = __tmp__01;
_this2._invInertia02 = __tmp__02;
_this2._invInertia10 = __tmp__10;
_this2._invInertia11 = __tmp__11;
_this2._invInertia12 = __tmp__12;
_this2._invInertia20 = __tmp__20;
_this2._invInertia21 = __tmp__21;
_this2._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = _this2._invInertia00 * _this2._transform._rotation00 + _this2._invInertia01 * _this2._transform._rotation01 + _this2._invInertia02 * _this2._transform._rotation02;
__tmp__011 = _this2._invInertia00 * _this2._transform._rotation10 + _this2._invInertia01 * _this2._transform._rotation11 + _this2._invInertia02 * _this2._transform._rotation12;
__tmp__021 = _this2._invInertia00 * _this2._transform._rotation20 + _this2._invInertia01 * _this2._transform._rotation21 + _this2._invInertia02 * _this2._transform._rotation22;
__tmp__101 = _this2._invInertia10 * _this2._transform._rotation00 + _this2._invInertia11 * _this2._transform._rotation01 + _this2._invInertia12 * _this2._transform._rotation02;
__tmp__111 = _this2._invInertia10 * _this2._transform._rotation10 + _this2._invInertia11 * _this2._transform._rotation11 + _this2._invInertia12 * _this2._transform._rotation12;
__tmp__121 = _this2._invInertia10 * _this2._transform._rotation20 + _this2._invInertia11 * _this2._transform._rotation21 + _this2._invInertia12 * _this2._transform._rotation22;
__tmp__201 = _this2._invInertia20 * _this2._transform._rotation00 + _this2._invInertia21 * _this2._transform._rotation01 + _this2._invInertia22 * _this2._transform._rotation02;
__tmp__211 = _this2._invInertia20 * _this2._transform._rotation10 + _this2._invInertia21 * _this2._transform._rotation11 + _this2._invInertia22 * _this2._transform._rotation12;
__tmp__221 = _this2._invInertia20 * _this2._transform._rotation20 + _this2._invInertia21 * _this2._transform._rotation21 + _this2._invInertia22 * _this2._transform._rotation22;
_this2._invInertia00 = __tmp__001;
_this2._invInertia01 = __tmp__011;
_this2._invInertia02 = __tmp__021;
_this2._invInertia10 = __tmp__101;
_this2._invInertia11 = __tmp__111;
_this2._invInertia12 = __tmp__121;
_this2._invInertia20 = __tmp__201;
_this2._invInertia21 = __tmp__211;
_this2._invInertia22 = __tmp__221;
_this2._invInertia00 *= _this2._rotFactor.x;
_this2._invInertia01 *= _this2._rotFactor.x;
_this2._invInertia02 *= _this2._rotFactor.x;
_this2._invInertia10 *= _this2._rotFactor.y;
_this2._invInertia11 *= _this2._rotFactor.y;
_this2._invInertia12 *= _this2._rotFactor.y;
_this2._invInertia20 *= _this2._rotFactor.z;
_this2._invInertia21 *= _this2._rotFactor.z;
_this2._invInertia22 *= _this2._rotFactor.z;
let _this3 = this._b2;
let theta1 = Math.sqrt(av2X * av2X + av2Y * av2Y + av2Z * av2Z);
let halfTheta1 = theta1 * 0.5;
let rotationToSinAxisFactor1;
let cosHalfTheta1;
if(halfTheta1 < 0.5) {
let ht2 = halfTheta1 * halfTheta1;
rotationToSinAxisFactor1 = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
cosHalfTheta1 = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
} else {
rotationToSinAxisFactor1 = Math.sin(halfTheta1) / theta1;
cosHalfTheta1 = Math.cos(halfTheta1);
}
let sinAxisX1;
let sinAxisY1;
let sinAxisZ1;
sinAxisX1 = av2X * rotationToSinAxisFactor1;
sinAxisY1 = av2Y * rotationToSinAxisFactor1;
sinAxisZ1 = av2Z * rotationToSinAxisFactor1;
let dqX1;
let dqY1;
let dqZ1;
let dqW1;
dqX1 = sinAxisX1;
dqY1 = sinAxisY1;
dqZ1 = sinAxisZ1;
dqW1 = cosHalfTheta1;
let qX1;
let qY1;
let qZ1;
let qW1;
let e001 = _this3._transform._rotation00;
let e111 = _this3._transform._rotation11;
let e221 = _this3._transform._rotation22;
let t1 = e001 + e111 + e221;
let s1;
if(t1 > 0) {
s1 = Math.sqrt(t1 + 1);
qW1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this3._transform._rotation21 - _this3._transform._rotation12) * s1;
qY1 = (_this3._transform._rotation02 - _this3._transform._rotation20) * s1;
qZ1 = (_this3._transform._rotation10 - _this3._transform._rotation01) * s1;
} else if(e001 > e111) {
if(e001 > e221) {
s1 = Math.sqrt(e001 - e111 - e221 + 1);
qX1 = 0.5 * s1;
s1 = 0.5 / s1;
qY1 = (_this3._transform._rotation01 + _this3._transform._rotation10) * s1;
qZ1 = (_this3._transform._rotation02 + _this3._transform._rotation20) * s1;
qW1 = (_this3._transform._rotation21 - _this3._transform._rotation12) * s1;
} else {
s1 = Math.sqrt(e221 - e001 - e111 + 1);
qZ1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this3._transform._rotation02 + _this3._transform._rotation20) * s1;
qY1 = (_this3._transform._rotation12 + _this3._transform._rotation21) * s1;
qW1 = (_this3._transform._rotation10 - _this3._transform._rotation01) * s1;
}
} else if(e111 > e221) {
s1 = Math.sqrt(e111 - e221 - e001 + 1);
qY1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this3._transform._rotation01 + _this3._transform._rotation10) * s1;
qZ1 = (_this3._transform._rotation12 + _this3._transform._rotation21) * s1;
qW1 = (_this3._transform._rotation02 - _this3._transform._rotation20) * s1;
} else {
s1 = Math.sqrt(e221 - e001 - e111 + 1);
qZ1 = 0.5 * s1;
s1 = 0.5 / s1;
qX1 = (_this3._transform._rotation02 + _this3._transform._rotation20) * s1;
qY1 = (_this3._transform._rotation12 + _this3._transform._rotation21) * s1;
qW1 = (_this3._transform._rotation10 - _this3._transform._rotation01) * s1;
}
qX1 = dqW1 * qX1 + dqX1 * qW1 + dqY1 * qZ1 - dqZ1 * qY1;
qY1 = dqW1 * qY1 - dqX1 * qZ1 + dqY1 * qW1 + dqZ1 * qX1;
qZ1 = dqW1 * qZ1 + dqX1 * qY1 - dqY1 * qX1 + dqZ1 * qW1;
qW1 = dqW1 * qW1 - dqX1 * qX1 - dqY1 * qY1 - dqZ1 * qZ1;
let l1 = qX1 * qX1 + qY1 * qY1 + qZ1 * qZ1 + qW1 * qW1;
if(l1 > 1e-32) {
l1 = 1 / Math.sqrt(l1);
}
qX1 *= l1;
qY1 *= l1;
qZ1 *= l1;
qW1 *= l1;
let x1 = qX1;
let y1 = qY1;
let z1 = qZ1;
let w1 = qW1;
let x21 = 2 * x1;
let y21 = 2 * y1;
let z21 = 2 * z1;
let xx1 = x1 * x21;
let yy1 = y1 * y21;
let zz1 = z1 * z21;
let xy1 = x1 * y21;
let yz1 = y1 * z21;
let xz1 = x1 * z21;
let wx1 = w1 * x21;
let wy1 = w1 * y21;
let wz1 = w1 * z21;
_this3._transform._rotation00 = 1 - yy1 - zz1;
_this3._transform._rotation01 = xy1 - wz1;
_this3._transform._rotation02 = xz1 + wy1;
_this3._transform._rotation10 = xy1 + wz1;
_this3._transform._rotation11 = 1 - xx1 - zz1;
_this3._transform._rotation12 = yz1 - wx1;
_this3._transform._rotation20 = xz1 - wy1;
_this3._transform._rotation21 = yz1 + wx1;
_this3._transform._rotation22 = 1 - xx1 - yy1;
let __tmp__002;
let __tmp__012;
let __tmp__022;
let __tmp__102;
let __tmp__112;
let __tmp__122;
let __tmp__202;
let __tmp__212;
let __tmp__222;
__tmp__002 = _this3._transform._rotation00 * _this3._invLocalInertia00 + _this3._transform._rotation01 * _this3._invLocalInertia10 + _this3._transform._rotation02 * _this3._invLocalInertia20;
__tmp__012 = _this3._transform._rotation00 * _this3._invLocalInertia01 + _this3._transform._rotation01 * _this3._invLocalInertia11 + _this3._transform._rotation02 * _this3._invLocalInertia21;
__tmp__022 = _this3._transform._rotation00 * _this3._invLocalInertia02 + _this3._transform._rotation01 * _this3._invLocalInertia12 + _this3._transform._rotation02 * _this3._invLocalInertia22;
__tmp__102 = _this3._transform._rotation10 * _this3._invLocalInertia00 + _this3._transform._rotation11 * _this3._invLocalInertia10 + _this3._transform._rotation12 * _this3._invLocalInertia20;
__tmp__112 = _this3._transform._rotation10 * _this3._invLocalInertia01 + _this3._transform._rotation11 * _this3._invLocalInertia11 + _this3._transform._rotation12 * _this3._invLocalInertia21;
__tmp__122 = _this3._transform._rotation10 * _this3._invLocalInertia02 + _this3._transform._rotation11 * _this3._invLocalInertia12 + _this3._transform._rotation12 * _this3._invLocalInertia22;
__tmp__202 = _this3._transform._rotation20 * _this3._invLocalInertia00 + _this3._transform._rotation21 * _this3._invLocalInertia10 + _this3._transform._rotation22 * _this3._invLocalInertia20;
__tmp__212 = _this3._transform._rotation20 * _this3._invLocalInertia01 + _this3._transform._rotation21 * _this3._invLocalInertia11 + _this3._transform._rotation22 * _this3._invLocalInertia21;
__tmp__222 = _this3._transform._rotation20 * _this3._invLocalInertia02 + _this3._transform._rotation21 * _this3._invLocalInertia12 + _this3._transform._rotation22 * _this3._invLocalInertia22;
_this3._invInertia00 = __tmp__002;
_this3._invInertia01 = __tmp__012;
_this3._invInertia02 = __tmp__022;
_this3._invInertia10 = __tmp__102;
_this3._invInertia11 = __tmp__112;
_this3._invInertia12 = __tmp__122;
_this3._invInertia20 = __tmp__202;
_this3._invInertia21 = __tmp__212;
_this3._invInertia22 = __tmp__222;
let __tmp__003;
let __tmp__013;
let __tmp__023;
let __tmp__103;
let __tmp__113;
let __tmp__123;
let __tmp__203;
let __tmp__213;
let __tmp__223;
__tmp__003 = _this3._invInertia00 * _this3._transform._rotation00 + _this3._invInertia01 * _this3._transform._rotation01 + _this3._invInertia02 * _this3._transform._rotation02;
__tmp__013 = _this3._invInertia00 * _this3._transform._rotation10 + _this3._invInertia01 * _this3._transform._rotation11 + _this3._invInertia02 * _this3._transform._rotation12;
__tmp__023 = _this3._invInertia00 * _this3._transform._rotation20 + _this3._invInertia01 * _this3._transform._rotation21 + _this3._invInertia02 * _this3._transform._rotation22;
__tmp__103 = _this3._invInertia10 * _this3._transform._rotation00 + _this3._invInertia11 * _this3._transform._rotation01 + _this3._invInertia12 * _this3._transform._rotation02;
__tmp__113 = _this3._invInertia10 * _this3._transform._rotation10 + _this3._invInertia11 * _this3._transform._rotation11 + _this3._invInertia12 * _this3._transform._rotation12;
__tmp__123 = _this3._invInertia10 * _this3._transform._rotation20 + _this3._invInertia11 * _this3._transform._rotation21 + _this3._invInertia12 * _this3._transform._rotation22;
__tmp__203 = _this3._invInertia20 * _this3._transform._rotation00 + _this3._invInertia21 * _this3._transform._rotation01 + _this3._invInertia22 * _this3._transform._rotation02;
__tmp__213 = _this3._invInertia20 * _this3._transform._rotation10 + _this3._invInertia21 * _this3._transform._rotation11 + _this3._invInertia22 * _this3._transform._rotation12;
__tmp__223 = _this3._invInertia20 * _this3._transform._rotation20 + _this3._invInertia21 * _this3._transform._rotation21 + _this3._invInertia22 * _this3._transform._rotation22;
_this3._invInertia00 = __tmp__003;
_this3._invInertia01 = __tmp__013;
_this3._invInertia02 = __tmp__023;
_this3._invInertia10 = __tmp__103;
_this3._invInertia11 = __tmp__113;
_this3._invInertia12 = __tmp__123;
_this3._invInertia20 = __tmp__203;
_this3._invInertia21 = __tmp__213;
_this3._invInertia22 = __tmp__223;
_this3._invInertia00 *= _this3._rotFactor.x;
_this3._invInertia01 *= _this3._rotFactor.x;
_this3._invInertia02 *= _this3._rotFactor.x;
_this3._invInertia10 *= _this3._rotFactor.y;
_this3._invInertia11 *= _this3._rotFactor.y;
_this3._invInertia12 *= _this3._rotFactor.y;
_this3._invInertia20 *= _this3._rotFactor.z;
_this3._invInertia21 *= _this3._rotFactor.z;
_this3._invInertia22 *= _this3._rotFactor.z;
}
postSolve() {
this.joint._syncAnchors();
this.joint._checkDestruction();
}
}
if(!oimo.dynamics.rigidbody) oimo.dynamics.rigidbody = {};
oimo.dynamics.rigidbody.MassData = class oimo_dynamics_rigidbody_MassData {
constructor() {
this.mass = 0;
this.localInertia = new oimo.common.Mat3();
}
}
oimo.dynamics.rigidbody.RigidBody = class oimo_dynamics_rigidbody_RigidBody {
constructor(config) {
this._next = null;
this._prev = null;
this._shapeList = null;
this._shapeListLast = null;
this._numShapes = 0;
this._contactLinkList = null;
this._contactLinkListLast = null;
this._numContactLinks = 0;
this._jointLinkList = null;
this._jointLinkListLast = null;
this._numJointLinks = 0;
let v = config.linearVelocity;
this._velX = v.x;
this._velY = v.y;
this._velZ = v.z;
let v1 = config.angularVelocity;
this._angVelX = v1.x;
this._angVelY = v1.y;
this._angVelZ = v1.z;
this._pseudoVelX = 0;
this._pseudoVelY = 0;
this._pseudoVelZ = 0;
this._angPseudoVelX = 0;
this._angPseudoVelY = 0;
this._angPseudoVelZ = 0;
this._ptransform = new oimo.common.Transform();
this._transform = new oimo.common.Transform();
let v2 = config.position;
this._ptransform._positionX = v2.x;
this._ptransform._positionY = v2.y;
this._ptransform._positionZ = v2.z;
let m = config.rotation;
this._ptransform._rotation00 = m.e00;
this._ptransform._rotation01 = m.e01;
this._ptransform._rotation02 = m.e02;
this._ptransform._rotation10 = m.e10;
this._ptransform._rotation11 = m.e11;
this._ptransform._rotation12 = m.e12;
this._ptransform._rotation20 = m.e20;
this._ptransform._rotation21 = m.e21;
this._ptransform._rotation22 = m.e22;
let dst = this._transform;
let src = this._ptransform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
this._type = config.type;
this._sleepTime = 0;
this._sleeping = false;
this._autoSleep = config.autoSleep;
this._mass = 0;
this._invMass = 0;
this._localInertia00 = 0;
this._localInertia01 = 0;
this._localInertia02 = 0;
this._localInertia10 = 0;
this._localInertia11 = 0;
this._localInertia12 = 0;
this._localInertia20 = 0;
this._localInertia21 = 0;
this._localInertia22 = 0;
this._invLocalInertia00 = 0;
this._invLocalInertia01 = 0;
this._invLocalInertia02 = 0;
this._invLocalInertia10 = 0;
this._invLocalInertia11 = 0;
this._invLocalInertia12 = 0;
this._invLocalInertia20 = 0;
this._invLocalInertia21 = 0;
this._invLocalInertia22 = 0;
this._invLocalInertiaWithoutRotFactor00 = 0;
this._invLocalInertiaWithoutRotFactor01 = 0;
this._invLocalInertiaWithoutRotFactor02 = 0;
this._invLocalInertiaWithoutRotFactor10 = 0;
this._invLocalInertiaWithoutRotFactor11 = 0;
this._invLocalInertiaWithoutRotFactor12 = 0;
this._invLocalInertiaWithoutRotFactor20 = 0;
this._invLocalInertiaWithoutRotFactor21 = 0;
this._invLocalInertiaWithoutRotFactor22 = 0;
this._invInertia00 = 0;
this._invInertia01 = 0;
this._invInertia02 = 0;
this._invInertia10 = 0;
this._invInertia11 = 0;
this._invInertia12 = 0;
this._invInertia20 = 0;
this._invInertia21 = 0;
this._invInertia22 = 0;
this._linearDamping = config.linearDamping;
this._angularDamping = config.angularDamping;
this._forceX = 0;
this._forceY = 0;
this._forceZ = 0;
this._torqueX = 0;
this._torqueY = 0;
this._torqueZ = 0;
this._linearContactImpulseX = 0;
this._linearContactImpulseY = 0;
this._linearContactImpulseZ = 0;
this._angularContactImpulseX = 0;
this._angularContactImpulseY = 0;
this._angularContactImpulseZ = 0;
this._rotFactor = new oimo.common.Vec3(1,1,1);
this._addedToIsland = false;
this._gravityScale = 1;
this._world = null;
}
_integrate(dt) {
switch(this._type) {
case 1:
this._velX = 0;
this._velY = 0;
this._velZ = 0;
this._angVelX = 0;
this._angVelY = 0;
this._angVelZ = 0;
this._pseudoVelX = 0;
this._pseudoVelY = 0;
this._pseudoVelZ = 0;
this._angPseudoVelX = 0;
this._angPseudoVelY = 0;
this._angPseudoVelZ = 0;
break;
case 0:case 2:
let translationX;
let translationY;
let translationZ;
let rotationX;
let rotationY;
let rotationZ;
translationX = this._velX * dt;
translationY = this._velY * dt;
translationZ = this._velZ * dt;
rotationX = this._angVelX * dt;
rotationY = this._angVelY * dt;
rotationZ = this._angVelZ * dt;
let translationLengthSq = translationX * translationX + translationY * translationY + translationZ * translationZ;
let rotationLengthSq = rotationX * rotationX + rotationY * rotationY + rotationZ * rotationZ;
if(translationLengthSq == 0 && rotationLengthSq == 0) {
return;
}
if(translationLengthSq > oimo.common.Setting.maxTranslationPerStep * oimo.common.Setting.maxTranslationPerStep) {
let l = oimo.common.Setting.maxTranslationPerStep / Math.sqrt(translationLengthSq);
this._velX *= l;
this._velY *= l;
this._velZ *= l;
translationX *= l;
translationY *= l;
translationZ *= l;
}
if(rotationLengthSq > oimo.common.Setting.maxRotationPerStep * oimo.common.Setting.maxRotationPerStep) {
let l = oimo.common.Setting.maxRotationPerStep / Math.sqrt(rotationLengthSq);
this._angVelX *= l;
this._angVelY *= l;
this._angVelZ *= l;
rotationX *= l;
rotationY *= l;
rotationZ *= l;
}
this._transform._positionX += translationX;
this._transform._positionY += translationY;
this._transform._positionZ += translationZ;
let theta = Math.sqrt(rotationX * rotationX + rotationY * rotationY + rotationZ * rotationZ);
let halfTheta = theta * 0.5;
let rotationToSinAxisFactor;
let cosHalfTheta;
if(halfTheta < 0.5) {
let ht2 = halfTheta * halfTheta;
rotationToSinAxisFactor = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
cosHalfTheta = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
} else {
rotationToSinAxisFactor = Math.sin(halfTheta) / theta;
cosHalfTheta = Math.cos(halfTheta);
}
let sinAxisX;
let sinAxisY;
let sinAxisZ;
sinAxisX = rotationX * rotationToSinAxisFactor;
sinAxisY = rotationY * rotationToSinAxisFactor;
sinAxisZ = rotationZ * rotationToSinAxisFactor;
let dqX;
let dqY;
let dqZ;
let dqW;
dqX = sinAxisX;
dqY = sinAxisY;
dqZ = sinAxisZ;
dqW = cosHalfTheta;
let qX;
let qY;
let qZ;
let qW;
let e00 = this._transform._rotation00;
let e11 = this._transform._rotation11;
let e22 = this._transform._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
qW = 0.5 * s;
s = 0.5 / s;
qX = (this._transform._rotation21 - this._transform._rotation12) * s;
qY = (this._transform._rotation02 - this._transform._rotation20) * s;
qZ = (this._transform._rotation10 - this._transform._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
qX = 0.5 * s;
s = 0.5 / s;
qY = (this._transform._rotation01 + this._transform._rotation10) * s;
qZ = (this._transform._rotation02 + this._transform._rotation20) * s;
qW = (this._transform._rotation21 - this._transform._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (this._transform._rotation02 + this._transform._rotation20) * s;
qY = (this._transform._rotation12 + this._transform._rotation21) * s;
qW = (this._transform._rotation10 - this._transform._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
qY = 0.5 * s;
s = 0.5 / s;
qX = (this._transform._rotation01 + this._transform._rotation10) * s;
qZ = (this._transform._rotation12 + this._transform._rotation21) * s;
qW = (this._transform._rotation02 - this._transform._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (this._transform._rotation02 + this._transform._rotation20) * s;
qY = (this._transform._rotation12 + this._transform._rotation21) * s;
qW = (this._transform._rotation10 - this._transform._rotation01) * s;
}
qX = dqW * qX + dqX * qW + dqY * qZ - dqZ * qY;
qY = dqW * qY - dqX * qZ + dqY * qW + dqZ * qX;
qZ = dqW * qZ + dqX * qY - dqY * qX + dqZ * qW;
qW = dqW * qW - dqX * qX - dqY * qY - dqZ * qZ;
let l = qX * qX + qY * qY + qZ * qZ + qW * qW;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
qX *= l;
qY *= l;
qZ *= l;
qW *= l;
let x = qX;
let y = qY;
let z = qZ;
let w = qW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
this._transform._rotation00 = 1 - yy - zz;
this._transform._rotation01 = xy - wz;
this._transform._rotation02 = xz + wy;
this._transform._rotation10 = xy + wz;
this._transform._rotation11 = 1 - xx - zz;
this._transform._rotation12 = yz - wx;
this._transform._rotation20 = xz - wy;
this._transform._rotation21 = yz + wx;
this._transform._rotation22 = 1 - xx - yy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
break;
}
}
_integratePseudoVelocity() {
if(this._pseudoVelX * this._pseudoVelX + this._pseudoVelY * this._pseudoVelY + this._pseudoVelZ * this._pseudoVelZ == 0 && this._angPseudoVelX * this._angPseudoVelX + this._angPseudoVelY * this._angPseudoVelY + this._angPseudoVelZ * this._angPseudoVelZ == 0) {
return;
}
switch(this._type) {
case 1:
this._pseudoVelX = 0;
this._pseudoVelY = 0;
this._pseudoVelZ = 0;
this._angPseudoVelX = 0;
this._angPseudoVelY = 0;
this._angPseudoVelZ = 0;
break;
case 0:case 2:
let translationX;
let translationY;
let translationZ;
let rotationX;
let rotationY;
let rotationZ;
translationX = this._pseudoVelX;
translationY = this._pseudoVelY;
translationZ = this._pseudoVelZ;
rotationX = this._angPseudoVelX;
rotationY = this._angPseudoVelY;
rotationZ = this._angPseudoVelZ;
this._pseudoVelX = 0;
this._pseudoVelY = 0;
this._pseudoVelZ = 0;
this._angPseudoVelX = 0;
this._angPseudoVelY = 0;
this._angPseudoVelZ = 0;
this._transform._positionX += translationX;
this._transform._positionY += translationY;
this._transform._positionZ += translationZ;
let theta = Math.sqrt(rotationX * rotationX + rotationY * rotationY + rotationZ * rotationZ);
let halfTheta = theta * 0.5;
let rotationToSinAxisFactor;
let cosHalfTheta;
if(halfTheta < 0.5) {
let ht2 = halfTheta * halfTheta;
rotationToSinAxisFactor = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
cosHalfTheta = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
} else {
rotationToSinAxisFactor = Math.sin(halfTheta) / theta;
cosHalfTheta = Math.cos(halfTheta);
}
let sinAxisX;
let sinAxisY;
let sinAxisZ;
sinAxisX = rotationX * rotationToSinAxisFactor;
sinAxisY = rotationY * rotationToSinAxisFactor;
sinAxisZ = rotationZ * rotationToSinAxisFactor;
let dqX;
let dqY;
let dqZ;
let dqW;
dqX = sinAxisX;
dqY = sinAxisY;
dqZ = sinAxisZ;
dqW = cosHalfTheta;
let qX;
let qY;
let qZ;
let qW;
let e00 = this._transform._rotation00;
let e11 = this._transform._rotation11;
let e22 = this._transform._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
qW = 0.5 * s;
s = 0.5 / s;
qX = (this._transform._rotation21 - this._transform._rotation12) * s;
qY = (this._transform._rotation02 - this._transform._rotation20) * s;
qZ = (this._transform._rotation10 - this._transform._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
qX = 0.5 * s;
s = 0.5 / s;
qY = (this._transform._rotation01 + this._transform._rotation10) * s;
qZ = (this._transform._rotation02 + this._transform._rotation20) * s;
qW = (this._transform._rotation21 - this._transform._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (this._transform._rotation02 + this._transform._rotation20) * s;
qY = (this._transform._rotation12 + this._transform._rotation21) * s;
qW = (this._transform._rotation10 - this._transform._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
qY = 0.5 * s;
s = 0.5 / s;
qX = (this._transform._rotation01 + this._transform._rotation10) * s;
qZ = (this._transform._rotation12 + this._transform._rotation21) * s;
qW = (this._transform._rotation02 - this._transform._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
qZ = 0.5 * s;
s = 0.5 / s;
qX = (this._transform._rotation02 + this._transform._rotation20) * s;
qY = (this._transform._rotation12 + this._transform._rotation21) * s;
qW = (this._transform._rotation10 - this._transform._rotation01) * s;
}
qX = dqW * qX + dqX * qW + dqY * qZ - dqZ * qY;
qY = dqW * qY - dqX * qZ + dqY * qW + dqZ * qX;
qZ = dqW * qZ + dqX * qY - dqY * qX + dqZ * qW;
qW = dqW * qW - dqX * qX - dqY * qY - dqZ * qZ;
let l = qX * qX + qY * qY + qZ * qZ + qW * qW;
if(l > 1e-32) {
l = 1 / Math.sqrt(l);
}
qX *= l;
qY *= l;
qZ *= l;
qW *= l;
let x = qX;
let y = qY;
let z = qZ;
let w = qW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
this._transform._rotation00 = 1 - yy - zz;
this._transform._rotation01 = xy - wz;
this._transform._rotation02 = xz + wy;
this._transform._rotation10 = xy + wz;
this._transform._rotation11 = 1 - xx - zz;
this._transform._rotation12 = yz - wx;
this._transform._rotation20 = xz - wy;
this._transform._rotation21 = yz + wx;
this._transform._rotation22 = 1 - xx - yy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
break;
}
}
updateMass() {
let totalInertia00;
let totalInertia01;
let totalInertia02;
let totalInertia10;
let totalInertia11;
let totalInertia12;
let totalInertia20;
let totalInertia21;
let totalInertia22;
totalInertia00 = 0;
totalInertia01 = 0;
totalInertia02 = 0;
totalInertia10 = 0;
totalInertia11 = 0;
totalInertia12 = 0;
totalInertia20 = 0;
totalInertia21 = 0;
totalInertia22 = 0;
let totalMass = 0;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let g = s._geom;
g._updateMass();
let mass = s._density * g._volume;
let inertia00;
let inertia01;
let inertia02;
let inertia10;
let inertia11;
let inertia12;
let inertia20;
let inertia21;
let inertia22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = s._localTransform._rotation00 * g._inertiaCoeff00 + s._localTransform._rotation01 * g._inertiaCoeff10 + s._localTransform._rotation02 * g._inertiaCoeff20;
__tmp__01 = s._localTransform._rotation00 * g._inertiaCoeff01 + s._localTransform._rotation01 * g._inertiaCoeff11 + s._localTransform._rotation02 * g._inertiaCoeff21;
__tmp__02 = s._localTransform._rotation00 * g._inertiaCoeff02 + s._localTransform._rotation01 * g._inertiaCoeff12 + s._localTransform._rotation02 * g._inertiaCoeff22;
__tmp__10 = s._localTransform._rotation10 * g._inertiaCoeff00 + s._localTransform._rotation11 * g._inertiaCoeff10 + s._localTransform._rotation12 * g._inertiaCoeff20;
__tmp__11 = s._localTransform._rotation10 * g._inertiaCoeff01 + s._localTransform._rotation11 * g._inertiaCoeff11 + s._localTransform._rotation12 * g._inertiaCoeff21;
__tmp__12 = s._localTransform._rotation10 * g._inertiaCoeff02 + s._localTransform._rotation11 * g._inertiaCoeff12 + s._localTransform._rotation12 * g._inertiaCoeff22;
__tmp__20 = s._localTransform._rotation20 * g._inertiaCoeff00 + s._localTransform._rotation21 * g._inertiaCoeff10 + s._localTransform._rotation22 * g._inertiaCoeff20;
__tmp__21 = s._localTransform._rotation20 * g._inertiaCoeff01 + s._localTransform._rotation21 * g._inertiaCoeff11 + s._localTransform._rotation22 * g._inertiaCoeff21;
__tmp__22 = s._localTransform._rotation20 * g._inertiaCoeff02 + s._localTransform._rotation21 * g._inertiaCoeff12 + s._localTransform._rotation22 * g._inertiaCoeff22;
inertia00 = __tmp__00;
inertia01 = __tmp__01;
inertia02 = __tmp__02;
inertia10 = __tmp__10;
inertia11 = __tmp__11;
inertia12 = __tmp__12;
inertia20 = __tmp__20;
inertia21 = __tmp__21;
inertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = inertia00 * s._localTransform._rotation00 + inertia01 * s._localTransform._rotation01 + inertia02 * s._localTransform._rotation02;
__tmp__011 = inertia00 * s._localTransform._rotation10 + inertia01 * s._localTransform._rotation11 + inertia02 * s._localTransform._rotation12;
__tmp__021 = inertia00 * s._localTransform._rotation20 + inertia01 * s._localTransform._rotation21 + inertia02 * s._localTransform._rotation22;
__tmp__101 = inertia10 * s._localTransform._rotation00 + inertia11 * s._localTransform._rotation01 + inertia12 * s._localTransform._rotation02;
__tmp__111 = inertia10 * s._localTransform._rotation10 + inertia11 * s._localTransform._rotation11 + inertia12 * s._localTransform._rotation12;
__tmp__121 = inertia10 * s._localTransform._rotation20 + inertia11 * s._localTransform._rotation21 + inertia12 * s._localTransform._rotation22;
__tmp__201 = inertia20 * s._localTransform._rotation00 + inertia21 * s._localTransform._rotation01 + inertia22 * s._localTransform._rotation02;
__tmp__211 = inertia20 * s._localTransform._rotation10 + inertia21 * s._localTransform._rotation11 + inertia22 * s._localTransform._rotation12;
__tmp__221 = inertia20 * s._localTransform._rotation20 + inertia21 * s._localTransform._rotation21 + inertia22 * s._localTransform._rotation22;
inertia00 = __tmp__001;
inertia01 = __tmp__011;
inertia02 = __tmp__021;
inertia10 = __tmp__101;
inertia11 = __tmp__111;
inertia12 = __tmp__121;
inertia20 = __tmp__201;
inertia21 = __tmp__211;
inertia22 = __tmp__221;
inertia00 *= mass;
inertia01 *= mass;
inertia02 *= mass;
inertia10 *= mass;
inertia11 *= mass;
inertia12 *= mass;
inertia20 *= mass;
inertia21 *= mass;
inertia22 *= mass;
let cogInertia00;
let cogInertia01;
let cogInertia02;
let cogInertia10;
let cogInertia11;
let cogInertia12;
let cogInertia20;
let cogInertia21;
let cogInertia22;
let xx = s._localTransform._positionX * s._localTransform._positionX;
let yy = s._localTransform._positionY * s._localTransform._positionY;
let zz = s._localTransform._positionZ * s._localTransform._positionZ;
let xy = -s._localTransform._positionX * s._localTransform._positionY;
let yz = -s._localTransform._positionY * s._localTransform._positionZ;
let zx = -s._localTransform._positionZ * s._localTransform._positionX;
cogInertia00 = yy + zz;
cogInertia01 = xy;
cogInertia02 = zx;
cogInertia10 = xy;
cogInertia11 = xx + zz;
cogInertia12 = yz;
cogInertia20 = zx;
cogInertia21 = yz;
cogInertia22 = xx + yy;
inertia00 += cogInertia00 * mass;
inertia01 += cogInertia01 * mass;
inertia02 += cogInertia02 * mass;
inertia10 += cogInertia10 * mass;
inertia11 += cogInertia11 * mass;
inertia12 += cogInertia12 * mass;
inertia20 += cogInertia20 * mass;
inertia21 += cogInertia21 * mass;
inertia22 += cogInertia22 * mass;
totalMass += mass;
totalInertia00 += inertia00;
totalInertia01 += inertia01;
totalInertia02 += inertia02;
totalInertia10 += inertia10;
totalInertia11 += inertia11;
totalInertia12 += inertia12;
totalInertia20 += inertia20;
totalInertia21 += inertia21;
totalInertia22 += inertia22;
s = n;
}
this._mass = totalMass;
this._localInertia00 = totalInertia00;
this._localInertia01 = totalInertia01;
this._localInertia02 = totalInertia02;
this._localInertia10 = totalInertia10;
this._localInertia11 = totalInertia11;
this._localInertia12 = totalInertia12;
this._localInertia20 = totalInertia20;
this._localInertia21 = totalInertia21;
this._localInertia22 = totalInertia22;
if(this._mass > 0 && this._localInertia00 * (this._localInertia11 * this._localInertia22 - this._localInertia12 * this._localInertia21) - this._localInertia01 * (this._localInertia10 * this._localInertia22 - this._localInertia12 * this._localInertia20) + this._localInertia02 * (this._localInertia10 * this._localInertia21 - this._localInertia11 * this._localInertia20) > 0 && this._type == 0) {
this._invMass = 1 / this._mass;
let d00 = this._localInertia11 * this._localInertia22 - this._localInertia12 * this._localInertia21;
let d01 = this._localInertia10 * this._localInertia22 - this._localInertia12 * this._localInertia20;
let d02 = this._localInertia10 * this._localInertia21 - this._localInertia11 * this._localInertia20;
let d = this._localInertia00 * d00 - this._localInertia01 * d01 + this._localInertia02 * d02;
if(d < -1e-32 || d > 1e-32) {
d = 1 / d;
}
this._invLocalInertia00 = d00 * d;
this._invLocalInertia01 = -(this._localInertia01 * this._localInertia22 - this._localInertia02 * this._localInertia21) * d;
this._invLocalInertia02 = (this._localInertia01 * this._localInertia12 - this._localInertia02 * this._localInertia11) * d;
this._invLocalInertia10 = -d01 * d;
this._invLocalInertia11 = (this._localInertia00 * this._localInertia22 - this._localInertia02 * this._localInertia20) * d;
this._invLocalInertia12 = -(this._localInertia00 * this._localInertia12 - this._localInertia02 * this._localInertia10) * d;
this._invLocalInertia20 = d02 * d;
this._invLocalInertia21 = -(this._localInertia00 * this._localInertia21 - this._localInertia01 * this._localInertia20) * d;
this._invLocalInertia22 = (this._localInertia00 * this._localInertia11 - this._localInertia01 * this._localInertia10) * d;
this._invLocalInertiaWithoutRotFactor00 = this._invLocalInertia00;
this._invLocalInertiaWithoutRotFactor01 = this._invLocalInertia01;
this._invLocalInertiaWithoutRotFactor02 = this._invLocalInertia02;
this._invLocalInertiaWithoutRotFactor10 = this._invLocalInertia10;
this._invLocalInertiaWithoutRotFactor11 = this._invLocalInertia11;
this._invLocalInertiaWithoutRotFactor12 = this._invLocalInertia12;
this._invLocalInertiaWithoutRotFactor20 = this._invLocalInertia20;
this._invLocalInertiaWithoutRotFactor21 = this._invLocalInertia21;
this._invLocalInertiaWithoutRotFactor22 = this._invLocalInertia22;
this._invLocalInertia00 = this._invLocalInertiaWithoutRotFactor00 * this._rotFactor.x;
this._invLocalInertia01 = this._invLocalInertiaWithoutRotFactor01 * this._rotFactor.x;
this._invLocalInertia02 = this._invLocalInertiaWithoutRotFactor02 * this._rotFactor.x;
this._invLocalInertia10 = this._invLocalInertiaWithoutRotFactor10 * this._rotFactor.y;
this._invLocalInertia11 = this._invLocalInertiaWithoutRotFactor11 * this._rotFactor.y;
this._invLocalInertia12 = this._invLocalInertiaWithoutRotFactor12 * this._rotFactor.y;
this._invLocalInertia20 = this._invLocalInertiaWithoutRotFactor20 * this._rotFactor.z;
this._invLocalInertia21 = this._invLocalInertiaWithoutRotFactor21 * this._rotFactor.z;
this._invLocalInertia22 = this._invLocalInertiaWithoutRotFactor22 * this._rotFactor.z;
} else {
this._invMass = 0;
this._invLocalInertia00 = 0;
this._invLocalInertia01 = 0;
this._invLocalInertia02 = 0;
this._invLocalInertia10 = 0;
this._invLocalInertia11 = 0;
this._invLocalInertia12 = 0;
this._invLocalInertia20 = 0;
this._invLocalInertia21 = 0;
this._invLocalInertia22 = 0;
this._invLocalInertiaWithoutRotFactor00 = 0;
this._invLocalInertiaWithoutRotFactor01 = 0;
this._invLocalInertiaWithoutRotFactor02 = 0;
this._invLocalInertiaWithoutRotFactor10 = 0;
this._invLocalInertiaWithoutRotFactor11 = 0;
this._invLocalInertiaWithoutRotFactor12 = 0;
this._invLocalInertiaWithoutRotFactor20 = 0;
this._invLocalInertiaWithoutRotFactor21 = 0;
this._invLocalInertiaWithoutRotFactor22 = 0;
if(this._type == 0) {
this._type = 1;
}
}
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
this._sleeping = false;
this._sleepTime = 0;
}
getPosition() {
let v = new oimo.common.Vec3();
v.x = this._transform._positionX;
v.y = this._transform._positionY;
v.z = this._transform._positionZ;
return v;
}
getPositionTo(position) {
position.x = this._transform._positionX;
position.y = this._transform._positionY;
position.z = this._transform._positionZ;
}
setPosition(position) {
this._transform._positionX = position.x;
this._transform._positionY = position.y;
this._transform._positionZ = position.z;
let dst = this._ptransform;
let src = this._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
this._sleeping = false;
this._sleepTime = 0;
}
translate(translation) {
let diffX;
let diffY;
let diffZ;
diffX = translation.x;
diffY = translation.y;
diffZ = translation.z;
this._transform._positionX += diffX;
this._transform._positionY += diffY;
this._transform._positionZ += diffZ;
let dst = this._ptransform;
let src = this._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
this._sleeping = false;
this._sleepTime = 0;
}
getRotation() {
let m = new oimo.common.Mat3();
m.e00 = this._transform._rotation00;
m.e01 = this._transform._rotation01;
m.e02 = this._transform._rotation02;
m.e10 = this._transform._rotation10;
m.e11 = this._transform._rotation11;
m.e12 = this._transform._rotation12;
m.e20 = this._transform._rotation20;
m.e21 = this._transform._rotation21;
m.e22 = this._transform._rotation22;
return m;
}
getRotationTo(rotation) {
rotation.e00 = this._transform._rotation00;
rotation.e01 = this._transform._rotation01;
rotation.e02 = this._transform._rotation02;
rotation.e10 = this._transform._rotation10;
rotation.e11 = this._transform._rotation11;
rotation.e12 = this._transform._rotation12;
rotation.e20 = this._transform._rotation20;
rotation.e21 = this._transform._rotation21;
rotation.e22 = this._transform._rotation22;
}
setRotation(rotation) {
this._transform._rotation00 = rotation.e00;
this._transform._rotation01 = rotation.e01;
this._transform._rotation02 = rotation.e02;
this._transform._rotation10 = rotation.e10;
this._transform._rotation11 = rotation.e11;
this._transform._rotation12 = rotation.e12;
this._transform._rotation20 = rotation.e20;
this._transform._rotation21 = rotation.e21;
this._transform._rotation22 = rotation.e22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
let dst = this._ptransform;
let src = this._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
this._sleeping = false;
this._sleepTime = 0;
}
setRotationXyz(eulerAngles) {
let xyzX;
let xyzY;
let xyzZ;
xyzX = eulerAngles.x;
xyzY = eulerAngles.y;
xyzZ = eulerAngles.z;
let sx = Math.sin(xyzX);
let sy = Math.sin(xyzY);
let sz = Math.sin(xyzZ);
let cx = Math.cos(xyzX);
let cy = Math.cos(xyzY);
let cz = Math.cos(xyzZ);
this._transform._rotation00 = cy * cz;
this._transform._rotation01 = -cy * sz;
this._transform._rotation02 = sy;
this._transform._rotation10 = cx * sz + cz * sx * sy;
this._transform._rotation11 = cx * cz - sx * sy * sz;
this._transform._rotation12 = -cy * sx;
this._transform._rotation20 = sx * sz - cx * cz * sy;
this._transform._rotation21 = cz * sx + cx * sy * sz;
this._transform._rotation22 = cx * cy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
let dst = this._ptransform;
let src = this._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
this._sleeping = false;
this._sleepTime = 0;
}
rotate(rotation) {
let rot00;
let rot01;
let rot02;
let rot10;
let rot11;
let rot12;
let rot20;
let rot21;
let rot22;
rot00 = rotation.e00;
rot01 = rotation.e01;
rot02 = rotation.e02;
rot10 = rotation.e10;
rot11 = rotation.e11;
rot12 = rotation.e12;
rot20 = rotation.e20;
rot21 = rotation.e21;
rot22 = rotation.e22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = rot00 * this._transform._rotation00 + rot01 * this._transform._rotation10 + rot02 * this._transform._rotation20;
__tmp__01 = rot00 * this._transform._rotation01 + rot01 * this._transform._rotation11 + rot02 * this._transform._rotation21;
__tmp__02 = rot00 * this._transform._rotation02 + rot01 * this._transform._rotation12 + rot02 * this._transform._rotation22;
__tmp__10 = rot10 * this._transform._rotation00 + rot11 * this._transform._rotation10 + rot12 * this._transform._rotation20;
__tmp__11 = rot10 * this._transform._rotation01 + rot11 * this._transform._rotation11 + rot12 * this._transform._rotation21;
__tmp__12 = rot10 * this._transform._rotation02 + rot11 * this._transform._rotation12 + rot12 * this._transform._rotation22;
__tmp__20 = rot20 * this._transform._rotation00 + rot21 * this._transform._rotation10 + rot22 * this._transform._rotation20;
__tmp__21 = rot20 * this._transform._rotation01 + rot21 * this._transform._rotation11 + rot22 * this._transform._rotation21;
__tmp__22 = rot20 * this._transform._rotation02 + rot21 * this._transform._rotation12 + rot22 * this._transform._rotation22;
this._transform._rotation00 = __tmp__00;
this._transform._rotation01 = __tmp__01;
this._transform._rotation02 = __tmp__02;
this._transform._rotation10 = __tmp__10;
this._transform._rotation11 = __tmp__11;
this._transform._rotation12 = __tmp__12;
this._transform._rotation20 = __tmp__20;
this._transform._rotation21 = __tmp__21;
this._transform._rotation22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__011 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__021 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__101 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__111 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__121 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__201 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__211 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__221 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
let __tmp__002;
let __tmp__012;
let __tmp__022;
let __tmp__102;
let __tmp__112;
let __tmp__122;
let __tmp__202;
let __tmp__212;
let __tmp__222;
__tmp__002 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__012 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__022 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__102 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__112 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__122 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__202 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__212 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__222 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__002;
this._invInertia01 = __tmp__012;
this._invInertia02 = __tmp__022;
this._invInertia10 = __tmp__102;
this._invInertia11 = __tmp__112;
this._invInertia12 = __tmp__122;
this._invInertia20 = __tmp__202;
this._invInertia21 = __tmp__212;
this._invInertia22 = __tmp__222;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
let dst = this._ptransform;
let src = this._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
this._sleeping = false;
this._sleepTime = 0;
}
rotateXyz(eulerAngles) {
let xyzX;
let xyzY;
let xyzZ;
let rot00;
let rot01;
let rot02;
let rot10;
let rot11;
let rot12;
let rot20;
let rot21;
let rot22;
xyzX = eulerAngles.x;
xyzY = eulerAngles.y;
xyzZ = eulerAngles.z;
let sx = Math.sin(xyzX);
let sy = Math.sin(xyzY);
let sz = Math.sin(xyzZ);
let cx = Math.cos(xyzX);
let cy = Math.cos(xyzY);
let cz = Math.cos(xyzZ);
rot00 = cy * cz;
rot01 = -cy * sz;
rot02 = sy;
rot10 = cx * sz + cz * sx * sy;
rot11 = cx * cz - sx * sy * sz;
rot12 = -cy * sx;
rot20 = sx * sz - cx * cz * sy;
rot21 = cz * sx + cx * sy * sz;
rot22 = cx * cy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = rot00 * this._transform._rotation00 + rot01 * this._transform._rotation10 + rot02 * this._transform._rotation20;
__tmp__01 = rot00 * this._transform._rotation01 + rot01 * this._transform._rotation11 + rot02 * this._transform._rotation21;
__tmp__02 = rot00 * this._transform._rotation02 + rot01 * this._transform._rotation12 + rot02 * this._transform._rotation22;
__tmp__10 = rot10 * this._transform._rotation00 + rot11 * this._transform._rotation10 + rot12 * this._transform._rotation20;
__tmp__11 = rot10 * this._transform._rotation01 + rot11 * this._transform._rotation11 + rot12 * this._transform._rotation21;
__tmp__12 = rot10 * this._transform._rotation02 + rot11 * this._transform._rotation12 + rot12 * this._transform._rotation22;
__tmp__20 = rot20 * this._transform._rotation00 + rot21 * this._transform._rotation10 + rot22 * this._transform._rotation20;
__tmp__21 = rot20 * this._transform._rotation01 + rot21 * this._transform._rotation11 + rot22 * this._transform._rotation21;
__tmp__22 = rot20 * this._transform._rotation02 + rot21 * this._transform._rotation12 + rot22 * this._transform._rotation22;
this._transform._rotation00 = __tmp__00;
this._transform._rotation01 = __tmp__01;
this._transform._rotation02 = __tmp__02;
this._transform._rotation10 = __tmp__10;
this._transform._rotation11 = __tmp__11;
this._transform._rotation12 = __tmp__12;
this._transform._rotation20 = __tmp__20;
this._transform._rotation21 = __tmp__21;
this._transform._rotation22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__011 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__021 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__101 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__111 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__121 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__201 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__211 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__221 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
let __tmp__002;
let __tmp__012;
let __tmp__022;
let __tmp__102;
let __tmp__112;
let __tmp__122;
let __tmp__202;
let __tmp__212;
let __tmp__222;
__tmp__002 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__012 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__022 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__102 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__112 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__122 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__202 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__212 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__222 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__002;
this._invInertia01 = __tmp__012;
this._invInertia02 = __tmp__022;
this._invInertia10 = __tmp__102;
this._invInertia11 = __tmp__112;
this._invInertia12 = __tmp__122;
this._invInertia20 = __tmp__202;
this._invInertia21 = __tmp__212;
this._invInertia22 = __tmp__222;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
let dst = this._ptransform;
let src = this._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
this._sleeping = false;
this._sleepTime = 0;
}
getOrientation() {
let q = new oimo.common.Quat();
let iqX;
let iqY;
let iqZ;
let iqW;
let e00 = this._transform._rotation00;
let e11 = this._transform._rotation11;
let e22 = this._transform._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
iqW = 0.5 * s;
s = 0.5 / s;
iqX = (this._transform._rotation21 - this._transform._rotation12) * s;
iqY = (this._transform._rotation02 - this._transform._rotation20) * s;
iqZ = (this._transform._rotation10 - this._transform._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
iqX = 0.5 * s;
s = 0.5 / s;
iqY = (this._transform._rotation01 + this._transform._rotation10) * s;
iqZ = (this._transform._rotation02 + this._transform._rotation20) * s;
iqW = (this._transform._rotation21 - this._transform._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
iqZ = 0.5 * s;
s = 0.5 / s;
iqX = (this._transform._rotation02 + this._transform._rotation20) * s;
iqY = (this._transform._rotation12 + this._transform._rotation21) * s;
iqW = (this._transform._rotation10 - this._transform._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
iqY = 0.5 * s;
s = 0.5 / s;
iqX = (this._transform._rotation01 + this._transform._rotation10) * s;
iqZ = (this._transform._rotation12 + this._transform._rotation21) * s;
iqW = (this._transform._rotation02 - this._transform._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
iqZ = 0.5 * s;
s = 0.5 / s;
iqX = (this._transform._rotation02 + this._transform._rotation20) * s;
iqY = (this._transform._rotation12 + this._transform._rotation21) * s;
iqW = (this._transform._rotation10 - this._transform._rotation01) * s;
}
q.x = iqX;
q.y = iqY;
q.z = iqZ;
q.w = iqW;
return q;
}
getOrientationTo(orientation) {
let iqX;
let iqY;
let iqZ;
let iqW;
let e00 = this._transform._rotation00;
let e11 = this._transform._rotation11;
let e22 = this._transform._rotation22;
let t = e00 + e11 + e22;
let s;
if(t > 0) {
s = Math.sqrt(t + 1);
iqW = 0.5 * s;
s = 0.5 / s;
iqX = (this._transform._rotation21 - this._transform._rotation12) * s;
iqY = (this._transform._rotation02 - this._transform._rotation20) * s;
iqZ = (this._transform._rotation10 - this._transform._rotation01) * s;
} else if(e00 > e11) {
if(e00 > e22) {
s = Math.sqrt(e00 - e11 - e22 + 1);
iqX = 0.5 * s;
s = 0.5 / s;
iqY = (this._transform._rotation01 + this._transform._rotation10) * s;
iqZ = (this._transform._rotation02 + this._transform._rotation20) * s;
iqW = (this._transform._rotation21 - this._transform._rotation12) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
iqZ = 0.5 * s;
s = 0.5 / s;
iqX = (this._transform._rotation02 + this._transform._rotation20) * s;
iqY = (this._transform._rotation12 + this._transform._rotation21) * s;
iqW = (this._transform._rotation10 - this._transform._rotation01) * s;
}
} else if(e11 > e22) {
s = Math.sqrt(e11 - e22 - e00 + 1);
iqY = 0.5 * s;
s = 0.5 / s;
iqX = (this._transform._rotation01 + this._transform._rotation10) * s;
iqZ = (this._transform._rotation12 + this._transform._rotation21) * s;
iqW = (this._transform._rotation02 - this._transform._rotation20) * s;
} else {
s = Math.sqrt(e22 - e00 - e11 + 1);
iqZ = 0.5 * s;
s = 0.5 / s;
iqX = (this._transform._rotation02 + this._transform._rotation20) * s;
iqY = (this._transform._rotation12 + this._transform._rotation21) * s;
iqW = (this._transform._rotation10 - this._transform._rotation01) * s;
}
orientation.x = iqX;
orientation.y = iqY;
orientation.z = iqZ;
orientation.w = iqW;
}
setOrientation(quaternion) {
let qX;
let qY;
let qZ;
let qW;
qX = quaternion.x;
qY = quaternion.y;
qZ = quaternion.z;
qW = quaternion.w;
let x = qX;
let y = qY;
let z = qZ;
let w = qW;
let x2 = 2 * x;
let y2 = 2 * y;
let z2 = 2 * z;
let xx = x * x2;
let yy = y * y2;
let zz = z * z2;
let xy = x * y2;
let yz = y * z2;
let xz = x * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
this._transform._rotation00 = 1 - yy - zz;
this._transform._rotation01 = xy - wz;
this._transform._rotation02 = xz + wy;
this._transform._rotation10 = xy + wz;
this._transform._rotation11 = 1 - xx - zz;
this._transform._rotation12 = yz - wx;
this._transform._rotation20 = xz - wy;
this._transform._rotation21 = yz + wx;
this._transform._rotation22 = 1 - xx - yy;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
let dst = this._ptransform;
let src = this._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
this._sleeping = false;
this._sleepTime = 0;
}
getTransform() {
let _this = this._transform;
let tf = new oimo.common.Transform();
tf._positionX = _this._positionX;
tf._positionY = _this._positionY;
tf._positionZ = _this._positionZ;
tf._rotation00 = _this._rotation00;
tf._rotation01 = _this._rotation01;
tf._rotation02 = _this._rotation02;
tf._rotation10 = _this._rotation10;
tf._rotation11 = _this._rotation11;
tf._rotation12 = _this._rotation12;
tf._rotation20 = _this._rotation20;
tf._rotation21 = _this._rotation21;
tf._rotation22 = _this._rotation22;
return tf;
}
getTransformTo(transform) {
let transform1 = this._transform;
transform._positionX = transform1._positionX;
transform._positionY = transform1._positionY;
transform._positionZ = transform1._positionZ;
transform._rotation00 = transform1._rotation00;
transform._rotation01 = transform1._rotation01;
transform._rotation02 = transform1._rotation02;
transform._rotation10 = transform1._rotation10;
transform._rotation11 = transform1._rotation11;
transform._rotation12 = transform1._rotation12;
transform._rotation20 = transform1._rotation20;
transform._rotation21 = transform1._rotation21;
transform._rotation22 = transform1._rotation22;
}
setTransform(transform) {
this._transform._positionX = transform._positionX;
this._transform._positionY = transform._positionY;
this._transform._positionZ = transform._positionZ;
this._transform._rotation00 = transform._rotation00;
this._transform._rotation01 = transform._rotation01;
this._transform._rotation02 = transform._rotation02;
this._transform._rotation10 = transform._rotation10;
this._transform._rotation11 = transform._rotation11;
this._transform._rotation12 = transform._rotation12;
this._transform._rotation20 = transform._rotation20;
this._transform._rotation21 = transform._rotation21;
this._transform._rotation22 = transform._rotation22;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
let dst = this._ptransform;
let src = this._transform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
this._sleeping = false;
this._sleepTime = 0;
}
getMass() {
return this._mass;
}
getLocalInertia() {
let m = new oimo.common.Mat3();
m.e00 = this._localInertia00;
m.e01 = this._localInertia01;
m.e02 = this._localInertia02;
m.e10 = this._localInertia10;
m.e11 = this._localInertia11;
m.e12 = this._localInertia12;
m.e20 = this._localInertia20;
m.e21 = this._localInertia21;
m.e22 = this._localInertia22;
return m;
}
getLocalInertiaTo(inertia) {
inertia.e00 = this._localInertia00;
inertia.e01 = this._localInertia01;
inertia.e02 = this._localInertia02;
inertia.e10 = this._localInertia10;
inertia.e11 = this._localInertia11;
inertia.e12 = this._localInertia12;
inertia.e20 = this._localInertia20;
inertia.e21 = this._localInertia21;
inertia.e22 = this._localInertia22;
}
getMassData() {
let md = new oimo.dynamics.rigidbody.MassData();
md.mass = this._mass;
let m = md.localInertia;
m.e00 = this._localInertia00;
m.e01 = this._localInertia01;
m.e02 = this._localInertia02;
m.e10 = this._localInertia10;
m.e11 = this._localInertia11;
m.e12 = this._localInertia12;
m.e20 = this._localInertia20;
m.e21 = this._localInertia21;
m.e22 = this._localInertia22;
return md;
}
getMassDataTo(massData) {
massData.mass = this._mass;
let m = massData.localInertia;
m.e00 = this._localInertia00;
m.e01 = this._localInertia01;
m.e02 = this._localInertia02;
m.e10 = this._localInertia10;
m.e11 = this._localInertia11;
m.e12 = this._localInertia12;
m.e20 = this._localInertia20;
m.e21 = this._localInertia21;
m.e22 = this._localInertia22;
}
setMassData(massData) {
this._mass = massData.mass;
let m = massData.localInertia;
this._localInertia00 = m.e00;
this._localInertia01 = m.e01;
this._localInertia02 = m.e02;
this._localInertia10 = m.e10;
this._localInertia11 = m.e11;
this._localInertia12 = m.e12;
this._localInertia20 = m.e20;
this._localInertia21 = m.e21;
this._localInertia22 = m.e22;
if(this._mass > 0 && this._localInertia00 * (this._localInertia11 * this._localInertia22 - this._localInertia12 * this._localInertia21) - this._localInertia01 * (this._localInertia10 * this._localInertia22 - this._localInertia12 * this._localInertia20) + this._localInertia02 * (this._localInertia10 * this._localInertia21 - this._localInertia11 * this._localInertia20) > 0 && this._type == 0) {
this._invMass = 1 / this._mass;
let d00 = this._localInertia11 * this._localInertia22 - this._localInertia12 * this._localInertia21;
let d01 = this._localInertia10 * this._localInertia22 - this._localInertia12 * this._localInertia20;
let d02 = this._localInertia10 * this._localInertia21 - this._localInertia11 * this._localInertia20;
let d = this._localInertia00 * d00 - this._localInertia01 * d01 + this._localInertia02 * d02;
if(d < -1e-32 || d > 1e-32) {
d = 1 / d;
}
this._invLocalInertia00 = d00 * d;
this._invLocalInertia01 = -(this._localInertia01 * this._localInertia22 - this._localInertia02 * this._localInertia21) * d;
this._invLocalInertia02 = (this._localInertia01 * this._localInertia12 - this._localInertia02 * this._localInertia11) * d;
this._invLocalInertia10 = -d01 * d;
this._invLocalInertia11 = (this._localInertia00 * this._localInertia22 - this._localInertia02 * this._localInertia20) * d;
this._invLocalInertia12 = -(this._localInertia00 * this._localInertia12 - this._localInertia02 * this._localInertia10) * d;
this._invLocalInertia20 = d02 * d;
this._invLocalInertia21 = -(this._localInertia00 * this._localInertia21 - this._localInertia01 * this._localInertia20) * d;
this._invLocalInertia22 = (this._localInertia00 * this._localInertia11 - this._localInertia01 * this._localInertia10) * d;
this._invLocalInertiaWithoutRotFactor00 = this._invLocalInertia00;
this._invLocalInertiaWithoutRotFactor01 = this._invLocalInertia01;
this._invLocalInertiaWithoutRotFactor02 = this._invLocalInertia02;
this._invLocalInertiaWithoutRotFactor10 = this._invLocalInertia10;
this._invLocalInertiaWithoutRotFactor11 = this._invLocalInertia11;
this._invLocalInertiaWithoutRotFactor12 = this._invLocalInertia12;
this._invLocalInertiaWithoutRotFactor20 = this._invLocalInertia20;
this._invLocalInertiaWithoutRotFactor21 = this._invLocalInertia21;
this._invLocalInertiaWithoutRotFactor22 = this._invLocalInertia22;
this._invLocalInertia00 = this._invLocalInertiaWithoutRotFactor00 * this._rotFactor.x;
this._invLocalInertia01 = this._invLocalInertiaWithoutRotFactor01 * this._rotFactor.x;
this._invLocalInertia02 = this._invLocalInertiaWithoutRotFactor02 * this._rotFactor.x;
this._invLocalInertia10 = this._invLocalInertiaWithoutRotFactor10 * this._rotFactor.y;
this._invLocalInertia11 = this._invLocalInertiaWithoutRotFactor11 * this._rotFactor.y;
this._invLocalInertia12 = this._invLocalInertiaWithoutRotFactor12 * this._rotFactor.y;
this._invLocalInertia20 = this._invLocalInertiaWithoutRotFactor20 * this._rotFactor.z;
this._invLocalInertia21 = this._invLocalInertiaWithoutRotFactor21 * this._rotFactor.z;
this._invLocalInertia22 = this._invLocalInertiaWithoutRotFactor22 * this._rotFactor.z;
} else {
this._invMass = 0;
this._invLocalInertia00 = 0;
this._invLocalInertia01 = 0;
this._invLocalInertia02 = 0;
this._invLocalInertia10 = 0;
this._invLocalInertia11 = 0;
this._invLocalInertia12 = 0;
this._invLocalInertia20 = 0;
this._invLocalInertia21 = 0;
this._invLocalInertia22 = 0;
this._invLocalInertiaWithoutRotFactor00 = 0;
this._invLocalInertiaWithoutRotFactor01 = 0;
this._invLocalInertiaWithoutRotFactor02 = 0;
this._invLocalInertiaWithoutRotFactor10 = 0;
this._invLocalInertiaWithoutRotFactor11 = 0;
this._invLocalInertiaWithoutRotFactor12 = 0;
this._invLocalInertiaWithoutRotFactor20 = 0;
this._invLocalInertiaWithoutRotFactor21 = 0;
this._invLocalInertiaWithoutRotFactor22 = 0;
if(this._type == 0) {
this._type = 1;
}
}
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
this._sleeping = false;
this._sleepTime = 0;
}
getRotationFactor() {
let _this = this._rotFactor;
return new oimo.common.Vec3(_this.x,_this.y,_this.z);
}
setRotationFactor(rotationFactor) {
let _this = this._rotFactor;
_this.x = rotationFactor.x;
_this.y = rotationFactor.y;
_this.z = rotationFactor.z;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = this._transform._rotation00 * this._invLocalInertia00 + this._transform._rotation01 * this._invLocalInertia10 + this._transform._rotation02 * this._invLocalInertia20;
__tmp__01 = this._transform._rotation00 * this._invLocalInertia01 + this._transform._rotation01 * this._invLocalInertia11 + this._transform._rotation02 * this._invLocalInertia21;
__tmp__02 = this._transform._rotation00 * this._invLocalInertia02 + this._transform._rotation01 * this._invLocalInertia12 + this._transform._rotation02 * this._invLocalInertia22;
__tmp__10 = this._transform._rotation10 * this._invLocalInertia00 + this._transform._rotation11 * this._invLocalInertia10 + this._transform._rotation12 * this._invLocalInertia20;
__tmp__11 = this._transform._rotation10 * this._invLocalInertia01 + this._transform._rotation11 * this._invLocalInertia11 + this._transform._rotation12 * this._invLocalInertia21;
__tmp__12 = this._transform._rotation10 * this._invLocalInertia02 + this._transform._rotation11 * this._invLocalInertia12 + this._transform._rotation12 * this._invLocalInertia22;
__tmp__20 = this._transform._rotation20 * this._invLocalInertia00 + this._transform._rotation21 * this._invLocalInertia10 + this._transform._rotation22 * this._invLocalInertia20;
__tmp__21 = this._transform._rotation20 * this._invLocalInertia01 + this._transform._rotation21 * this._invLocalInertia11 + this._transform._rotation22 * this._invLocalInertia21;
__tmp__22 = this._transform._rotation20 * this._invLocalInertia02 + this._transform._rotation21 * this._invLocalInertia12 + this._transform._rotation22 * this._invLocalInertia22;
this._invInertia00 = __tmp__00;
this._invInertia01 = __tmp__01;
this._invInertia02 = __tmp__02;
this._invInertia10 = __tmp__10;
this._invInertia11 = __tmp__11;
this._invInertia12 = __tmp__12;
this._invInertia20 = __tmp__20;
this._invInertia21 = __tmp__21;
this._invInertia22 = __tmp__22;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = this._invInertia00 * this._transform._rotation00 + this._invInertia01 * this._transform._rotation01 + this._invInertia02 * this._transform._rotation02;
__tmp__011 = this._invInertia00 * this._transform._rotation10 + this._invInertia01 * this._transform._rotation11 + this._invInertia02 * this._transform._rotation12;
__tmp__021 = this._invInertia00 * this._transform._rotation20 + this._invInertia01 * this._transform._rotation21 + this._invInertia02 * this._transform._rotation22;
__tmp__101 = this._invInertia10 * this._transform._rotation00 + this._invInertia11 * this._transform._rotation01 + this._invInertia12 * this._transform._rotation02;
__tmp__111 = this._invInertia10 * this._transform._rotation10 + this._invInertia11 * this._transform._rotation11 + this._invInertia12 * this._transform._rotation12;
__tmp__121 = this._invInertia10 * this._transform._rotation20 + this._invInertia11 * this._transform._rotation21 + this._invInertia12 * this._transform._rotation22;
__tmp__201 = this._invInertia20 * this._transform._rotation00 + this._invInertia21 * this._transform._rotation01 + this._invInertia22 * this._transform._rotation02;
__tmp__211 = this._invInertia20 * this._transform._rotation10 + this._invInertia21 * this._transform._rotation11 + this._invInertia22 * this._transform._rotation12;
__tmp__221 = this._invInertia20 * this._transform._rotation20 + this._invInertia21 * this._transform._rotation21 + this._invInertia22 * this._transform._rotation22;
this._invInertia00 = __tmp__001;
this._invInertia01 = __tmp__011;
this._invInertia02 = __tmp__021;
this._invInertia10 = __tmp__101;
this._invInertia11 = __tmp__111;
this._invInertia12 = __tmp__121;
this._invInertia20 = __tmp__201;
this._invInertia21 = __tmp__211;
this._invInertia22 = __tmp__221;
this._invInertia00 *= this._rotFactor.x;
this._invInertia01 *= this._rotFactor.x;
this._invInertia02 *= this._rotFactor.x;
this._invInertia10 *= this._rotFactor.y;
this._invInertia11 *= this._rotFactor.y;
this._invInertia12 *= this._rotFactor.y;
this._invInertia20 *= this._rotFactor.z;
this._invInertia21 *= this._rotFactor.z;
this._invInertia22 *= this._rotFactor.z;
this._sleeping = false;
this._sleepTime = 0;
}
getLinearVelocity() {
let v = new oimo.common.Vec3();
v.x = this._velX;
v.y = this._velY;
v.z = this._velZ;
return v;
}
getLinearVelocityTo(linearVelocity) {
linearVelocity.x = this._velX;
linearVelocity.y = this._velY;
linearVelocity.z = this._velZ;
}
setLinearVelocity(linearVelocity) {
if(this._type == 1) {
this._velX = 0;
this._velY = 0;
this._velZ = 0;
} else {
this._velX = linearVelocity.x;
this._velY = linearVelocity.y;
this._velZ = linearVelocity.z;
}
this._sleeping = false;
this._sleepTime = 0;
}
getAngularVelocity() {
let v = new oimo.common.Vec3();
v.x = this._angVelX;
v.y = this._angVelY;
v.z = this._angVelZ;
return v;
}
getAngularVelocityTo(angularVelocity) {
angularVelocity.x = this._velX;
angularVelocity.y = this._velY;
angularVelocity.z = this._velZ;
}
setAngularVelocity(angularVelocity) {
if(this._type == 1) {
this._angVelX = 0;
this._angVelY = 0;
this._angVelZ = 0;
} else {
this._angVelX = angularVelocity.x;
this._angVelY = angularVelocity.y;
this._angVelZ = angularVelocity.z;
}
this._sleeping = false;
this._sleepTime = 0;
}
addLinearVelocity(linearVelocityChange) {
if(this._type != 1) {
let dX;
let dY;
let dZ;
dX = linearVelocityChange.x;
dY = linearVelocityChange.y;
dZ = linearVelocityChange.z;
this._velX += dX;
this._velY += dY;
this._velZ += dZ;
}
this._sleeping = false;
this._sleepTime = 0;
}
addAngularVelocity(angularVelocityChange) {
if(this._type != 1) {
let dX;
let dY;
let dZ;
dX = angularVelocityChange.x;
dY = angularVelocityChange.y;
dZ = angularVelocityChange.z;
this._angVelX += dX;
this._angVelY += dY;
this._angVelZ += dZ;
}
this._sleeping = false;
this._sleepTime = 0;
}
applyImpulse(impulse,positionInWorld) {
let impX;
let impY;
let impZ;
impX = impulse.x;
impY = impulse.y;
impZ = impulse.z;
this._velX += impX * this._invMass;
this._velY += impY * this._invMass;
this._velZ += impZ * this._invMass;
let aimpX;
let aimpY;
let aimpZ;
let posX;
let posY;
let posZ;
posX = positionInWorld.x;
posY = positionInWorld.y;
posZ = positionInWorld.z;
posX -= this._transform._positionX;
posY -= this._transform._positionY;
posZ -= this._transform._positionZ;
aimpX = posY * impZ - posZ * impY;
aimpY = posZ * impX - posX * impZ;
aimpZ = posX * impY - posY * impX;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._invInertia00 * aimpX + this._invInertia01 * aimpY + this._invInertia02 * aimpZ;
__tmp__Y = this._invInertia10 * aimpX + this._invInertia11 * aimpY + this._invInertia12 * aimpZ;
__tmp__Z = this._invInertia20 * aimpX + this._invInertia21 * aimpY + this._invInertia22 * aimpZ;
aimpX = __tmp__X;
aimpY = __tmp__Y;
aimpZ = __tmp__Z;
this._angVelX += aimpX;
this._angVelY += aimpY;
this._angVelZ += aimpZ;
this._sleeping = false;
this._sleepTime = 0;
}
applyLinearImpulse(impulse) {
let impX;
let impY;
let impZ;
impX = impulse.x;
impY = impulse.y;
impZ = impulse.z;
this._velX += impX * this._invMass;
this._velY += impY * this._invMass;
this._velZ += impZ * this._invMass;
this._sleeping = false;
this._sleepTime = 0;
}
applyAngularImpulse(impulse) {
let impX;
let impY;
let impZ;
impX = impulse.x;
impY = impulse.y;
impZ = impulse.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._invInertia00 * impX + this._invInertia01 * impY + this._invInertia02 * impZ;
__tmp__Y = this._invInertia10 * impX + this._invInertia11 * impY + this._invInertia12 * impZ;
__tmp__Z = this._invInertia20 * impX + this._invInertia21 * impY + this._invInertia22 * impZ;
impX = __tmp__X;
impY = __tmp__Y;
impZ = __tmp__Z;
this._angVelX += impX;
this._angVelY += impY;
this._angVelZ += impZ;
this._sleeping = false;
this._sleepTime = 0;
}
applyForce(force,positionInWorld) {
let iforceX;
let iforceY;
let iforceZ;
iforceX = force.x;
iforceY = force.y;
iforceZ = force.z;
this._forceX += iforceX;
this._forceY += iforceY;
this._forceZ += iforceZ;
let itorqueX;
let itorqueY;
let itorqueZ;
let posX;
let posY;
let posZ;
posX = positionInWorld.x;
posY = positionInWorld.y;
posZ = positionInWorld.z;
posX -= this._transform._positionX;
posY -= this._transform._positionY;
posZ -= this._transform._positionZ;
itorqueX = posY * iforceZ - posZ * iforceY;
itorqueY = posZ * iforceX - posX * iforceZ;
itorqueZ = posX * iforceY - posY * iforceX;
this._torqueX += itorqueX;
this._torqueY += itorqueY;
this._torqueZ += itorqueZ;
this._sleeping = false;
this._sleepTime = 0;
}
applyForceToCenter(force) {
let iforceX;
let iforceY;
let iforceZ;
iforceX = force.x;
iforceY = force.y;
iforceZ = force.z;
this._forceX += iforceX;
this._forceY += iforceY;
this._forceZ += iforceZ;
this._sleeping = false;
this._sleepTime = 0;
}
applyTorque(torque) {
let itorqueX;
let itorqueY;
let itorqueZ;
itorqueX = torque.x;
itorqueY = torque.y;
itorqueZ = torque.z;
this._torqueX += itorqueX;
this._torqueY += itorqueY;
this._torqueZ += itorqueZ;
this._sleeping = false;
this._sleepTime = 0;
}
getLinearContactImpulse() {
let res = new oimo.common.Vec3();
res.x = this._linearContactImpulseX;
res.y = this._linearContactImpulseY;
res.z = this._linearContactImpulseZ;
return res;
}
getLinearContactImpulseTo(linearContactImpulse) {
linearContactImpulse.x = this._linearContactImpulseX;
linearContactImpulse.y = this._linearContactImpulseY;
linearContactImpulse.z = this._linearContactImpulseZ;
}
getAngularContactImpulse() {
let res = new oimo.common.Vec3();
res.x = this._angularContactImpulseX;
res.y = this._angularContactImpulseY;
res.z = this._angularContactImpulseZ;
return res;
}
getAngularContactImpulseTo(angularContactImpulse) {
angularContactImpulse.x = this._angularContactImpulseX;
angularContactImpulse.y = this._angularContactImpulseY;
angularContactImpulse.z = this._angularContactImpulseZ;
}
getGravityScale() {
return this._gravityScale;
}
setGravityScale(gravityScale) {
this._gravityScale = gravityScale;
this._sleeping = false;
this._sleepTime = 0;
}
getLocalPoint(worldPoint) {
let vX;
let vY;
let vZ;
vX = worldPoint.x;
vY = worldPoint.y;
vZ = worldPoint.z;
vX -= this._transform._positionX;
vY -= this._transform._positionY;
vZ -= this._transform._positionZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._transform._rotation00 * vX + this._transform._rotation10 * vY + this._transform._rotation20 * vZ;
__tmp__Y = this._transform._rotation01 * vX + this._transform._rotation11 * vY + this._transform._rotation21 * vZ;
__tmp__Z = this._transform._rotation02 * vX + this._transform._rotation12 * vY + this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
let res = new oimo.common.Vec3();
res.x = vX;
res.y = vY;
res.z = vZ;
return res;
}
getLocalPointTo(worldPoint,localPoint) {
let vX;
let vY;
let vZ;
vX = worldPoint.x;
vY = worldPoint.y;
vZ = worldPoint.z;
vX -= this._transform._positionX;
vY -= this._transform._positionY;
vZ -= this._transform._positionZ;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._transform._rotation00 * vX + this._transform._rotation10 * vY + this._transform._rotation20 * vZ;
__tmp__Y = this._transform._rotation01 * vX + this._transform._rotation11 * vY + this._transform._rotation21 * vZ;
__tmp__Z = this._transform._rotation02 * vX + this._transform._rotation12 * vY + this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
localPoint.x = vX;
localPoint.y = vY;
localPoint.z = vZ;
}
getLocalVector(worldVector) {
let vX;
let vY;
let vZ;
vX = worldVector.x;
vY = worldVector.y;
vZ = worldVector.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._transform._rotation00 * vX + this._transform._rotation10 * vY + this._transform._rotation20 * vZ;
__tmp__Y = this._transform._rotation01 * vX + this._transform._rotation11 * vY + this._transform._rotation21 * vZ;
__tmp__Z = this._transform._rotation02 * vX + this._transform._rotation12 * vY + this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
let res = new oimo.common.Vec3();
res.x = vX;
res.y = vY;
res.z = vZ;
return res;
}
getLocalVectorTo(worldVector,localVector) {
let vX;
let vY;
let vZ;
vX = worldVector.x;
vY = worldVector.y;
vZ = worldVector.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._transform._rotation00 * vX + this._transform._rotation10 * vY + this._transform._rotation20 * vZ;
__tmp__Y = this._transform._rotation01 * vX + this._transform._rotation11 * vY + this._transform._rotation21 * vZ;
__tmp__Z = this._transform._rotation02 * vX + this._transform._rotation12 * vY + this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
localVector.x = vX;
localVector.y = vY;
localVector.z = vZ;
}
getWorldPoint(localPoint) {
let vX;
let vY;
let vZ;
vX = localPoint.x;
vY = localPoint.y;
vZ = localPoint.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._transform._rotation00 * vX + this._transform._rotation01 * vY + this._transform._rotation02 * vZ;
__tmp__Y = this._transform._rotation10 * vX + this._transform._rotation11 * vY + this._transform._rotation12 * vZ;
__tmp__Z = this._transform._rotation20 * vX + this._transform._rotation21 * vY + this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
vX += this._transform._positionX;
vY += this._transform._positionY;
vZ += this._transform._positionZ;
let res = new oimo.common.Vec3();
res.x = vX;
res.y = vY;
res.z = vZ;
return res;
}
getWorldPointTo(localPoint,worldPoint) {
let vX;
let vY;
let vZ;
vX = localPoint.x;
vY = localPoint.y;
vZ = localPoint.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._transform._rotation00 * vX + this._transform._rotation01 * vY + this._transform._rotation02 * vZ;
__tmp__Y = this._transform._rotation10 * vX + this._transform._rotation11 * vY + this._transform._rotation12 * vZ;
__tmp__Z = this._transform._rotation20 * vX + this._transform._rotation21 * vY + this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
vX += this._transform._positionX;
vY += this._transform._positionY;
vZ += this._transform._positionZ;
worldPoint.x = vX;
worldPoint.y = vY;
worldPoint.z = vZ;
}
getWorldVector(localVector) {
let vX;
let vY;
let vZ;
vX = localVector.x;
vY = localVector.y;
vZ = localVector.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._transform._rotation00 * vX + this._transform._rotation01 * vY + this._transform._rotation02 * vZ;
__tmp__Y = this._transform._rotation10 * vX + this._transform._rotation11 * vY + this._transform._rotation12 * vZ;
__tmp__Z = this._transform._rotation20 * vX + this._transform._rotation21 * vY + this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
let res = new oimo.common.Vec3();
res.x = vX;
res.y = vY;
res.z = vZ;
return res;
}
getWorldVectorTo(localVector,worldVector) {
let vX;
let vY;
let vZ;
vX = localVector.x;
vY = localVector.y;
vZ = localVector.z;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = this._transform._rotation00 * vX + this._transform._rotation01 * vY + this._transform._rotation02 * vZ;
__tmp__Y = this._transform._rotation10 * vX + this._transform._rotation11 * vY + this._transform._rotation12 * vZ;
__tmp__Z = this._transform._rotation20 * vX + this._transform._rotation21 * vY + this._transform._rotation22 * vZ;
vX = __tmp__X;
vY = __tmp__Y;
vZ = __tmp__Z;
worldVector.x = vX;
worldVector.y = vY;
worldVector.z = vZ;
}
getNumShapes() {
return this._numShapes;
}
getShapeList() {
return this._shapeList;
}
getNumContectLinks() {
return this._numContactLinks;
}
getContactLinkList() {
return this._contactLinkList;
}
getNumJointLinks() {
return this._numJointLinks;
}
getJointLinkList() {
return this._jointLinkList;
}
addShape(shape) {
if(this._shapeList == null) {
this._shapeList = shape;
this._shapeListLast = shape;
} else {
this._shapeListLast._next = shape;
shape._prev = this._shapeListLast;
this._shapeListLast = shape;
}
this._numShapes++;
shape._rigidBody = this;
if(this._world != null) {
let _this = this._world;
shape._proxy = _this._broadPhase.createProxy(shape,shape._aabb);
shape._id = _this._shapeIdCount++;
_this._numShapes++;
}
this.updateMass();
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
}
removeShape(shape) {
let prev = shape._prev;
let next = shape._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(shape == this._shapeList) {
this._shapeList = this._shapeList._next;
}
if(shape == this._shapeListLast) {
this._shapeListLast = this._shapeListLast._prev;
}
shape._next = null;
shape._prev = null;
this._numShapes--;
shape._rigidBody = null;
if(this._world != null) {
let _this = this._world;
_this._broadPhase.destroyProxy(shape._proxy);
shape._proxy = null;
shape._id = -1;
let cl = shape._rigidBody._contactLinkList;
while(cl != null) {
let n = cl._next;
let c = cl._contact;
if(c._s1 == shape || c._s2 == shape) {
let _this1 = cl._other;
_this1._sleeping = false;
_this1._sleepTime = 0;
let _this2 = _this._contactManager;
let prev = c._prev;
let next = c._next;
if(prev != null) {
prev._next = next;
}
if(next != null) {
next._prev = prev;
}
if(c == _this2._contactList) {
_this2._contactList = _this2._contactList._next;
}
if(c == _this2._contactListLast) {
_this2._contactListLast = _this2._contactListLast._prev;
}
c._next = null;
c._prev = null;
if(c._touching) {
let cc1 = c._s1._contactCallback;
let cc2 = c._s2._contactCallback;
if(cc1 == cc2) {
cc2 = null;
}
if(cc1 != null) {
cc1.endContact(c);
}
if(cc2 != null) {
cc2.endContact(c);
}
}
let prev1 = c._link1._prev;
let next1 = c._link1._next;
if(prev1 != null) {
prev1._next = next1;
}
if(next1 != null) {
next1._prev = prev1;
}
if(c._link1 == c._b1._contactLinkList) {
c._b1._contactLinkList = c._b1._contactLinkList._next;
}
if(c._link1 == c._b1._contactLinkListLast) {
c._b1._contactLinkListLast = c._b1._contactLinkListLast._prev;
}
c._link1._next = null;
c._link1._prev = null;
let prev2 = c._link2._prev;
let next2 = c._link2._next;
if(prev2 != null) {
prev2._next = next2;
}
if(next2 != null) {
next2._prev = prev2;
}
if(c._link2 == c._b2._contactLinkList) {
c._b2._contactLinkList = c._b2._contactLinkList._next;
}
if(c._link2 == c._b2._contactLinkListLast) {
c._b2._contactLinkListLast = c._b2._contactLinkListLast._prev;
}
c._link2._next = null;
c._link2._prev = null;
c._b1._numContactLinks--;
c._b2._numContactLinks--;
c._link1._other = null;
c._link2._other = null;
c._link1._contact = null;
c._link2._contact = null;
c._s1 = null;
c._s2 = null;
c._b1 = null;
c._b2 = null;
c._touching = false;
c._cachedDetectorData._clear();
c._manifold._clear();
c._detector = null;
let _this3 = c._contactConstraint;
_this3._s1 = null;
_this3._s2 = null;
_this3._b1 = null;
_this3._b2 = null;
_this3._tf1 = null;
_this3._tf2 = null;
c._next = _this2._contactPool;
_this2._contactPool = c;
_this2._numContacts--;
}
cl = n;
}
_this._numShapes--;
}
this.updateMass();
let s = this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = this._ptransform;
let tf2 = this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
}
getType() {
return this._type;
}
setType(type) {
this._type = type;
this.updateMass();
}
wakeUp() {
this._sleeping = false;
this._sleepTime = 0;
}
sleep() {
this._sleeping = true;
this._sleepTime = 0;
}
isSleeping() {
return this._sleeping;
}
getSleepTime() {
return this._sleepTime;
}
setAutoSleep(autoSleepEnabled) {
this._autoSleep = autoSleepEnabled;
this._sleeping = false;
this._sleepTime = 0;
}
getLinearDamping() {
return this._linearDamping;
}
setLinearDamping(damping) {
this._linearDamping = damping;
}
getAngularDamping() {
return this._angularDamping;
}
setAngularDamping(damping) {
this._angularDamping = damping;
}
getPrev() {
return this._prev;
}
getNext() {
return this._next;
}
}
oimo.dynamics.rigidbody.RigidBodyConfig = class oimo_dynamics_rigidbody_RigidBodyConfig {
constructor() {
this.position = new oimo.common.Vec3();
this.rotation = new oimo.common.Mat3();
this.linearVelocity = new oimo.common.Vec3();
this.angularVelocity = new oimo.common.Vec3();
this.type = 0;
this.autoSleep = true;
this.linearDamping = 0;
this.angularDamping = 0;
}
}
oimo.dynamics.rigidbody.RigidBodyType = class oimo_dynamics_rigidbody_RigidBodyType {
}
oimo.dynamics.rigidbody.Shape = class oimo_dynamics_rigidbody_Shape {
constructor(config) {
this._id = -1;
this._localTransform = new oimo.common.Transform();
this._ptransform = new oimo.common.Transform();
this._transform = new oimo.common.Transform();
let v = config.position;
this._localTransform._positionX = v.x;
this._localTransform._positionY = v.y;
this._localTransform._positionZ = v.z;
let m = config.rotation;
this._localTransform._rotation00 = m.e00;
this._localTransform._rotation01 = m.e01;
this._localTransform._rotation02 = m.e02;
this._localTransform._rotation10 = m.e10;
this._localTransform._rotation11 = m.e11;
this._localTransform._rotation12 = m.e12;
this._localTransform._rotation20 = m.e20;
this._localTransform._rotation21 = m.e21;
this._localTransform._rotation22 = m.e22;
let dst = this._ptransform;
let src = this._localTransform;
dst._positionX = src._positionX;
dst._positionY = src._positionY;
dst._positionZ = src._positionZ;
dst._rotation00 = src._rotation00;
dst._rotation01 = src._rotation01;
dst._rotation02 = src._rotation02;
dst._rotation10 = src._rotation10;
dst._rotation11 = src._rotation11;
dst._rotation12 = src._rotation12;
dst._rotation20 = src._rotation20;
dst._rotation21 = src._rotation21;
dst._rotation22 = src._rotation22;
let dst1 = this._transform;
let src1 = this._localTransform;
dst1._positionX = src1._positionX;
dst1._positionY = src1._positionY;
dst1._positionZ = src1._positionZ;
dst1._rotation00 = src1._rotation00;
dst1._rotation01 = src1._rotation01;
dst1._rotation02 = src1._rotation02;
dst1._rotation10 = src1._rotation10;
dst1._rotation11 = src1._rotation11;
dst1._rotation12 = src1._rotation12;
dst1._rotation20 = src1._rotation20;
dst1._rotation21 = src1._rotation21;
dst1._rotation22 = src1._rotation22;
this._restitution = config.restitution;
this._friction = config.friction;
this._density = config.density;
this._geom = config.geometry;
this._collisionGroup = config.collisionGroup;
this._collisionMask = config.collisionMask;
this._contactCallback = config.contactCallback;
this._aabb = new oimo.collision.geometry.Aabb();
this._proxy = null;
this.displacement = new oimo.common.Vec3();
}
getFriction() {
return this._friction;
}
setFriction(friction) {
this._friction = friction;
}
getRestitution() {
return this._restitution;
}
setRestitution(restitution) {
this._restitution = restitution;
}
getLocalTransform() {
let _this = this._localTransform;
let tf = new oimo.common.Transform();
tf._positionX = _this._positionX;
tf._positionY = _this._positionY;
tf._positionZ = _this._positionZ;
tf._rotation00 = _this._rotation00;
tf._rotation01 = _this._rotation01;
tf._rotation02 = _this._rotation02;
tf._rotation10 = _this._rotation10;
tf._rotation11 = _this._rotation11;
tf._rotation12 = _this._rotation12;
tf._rotation20 = _this._rotation20;
tf._rotation21 = _this._rotation21;
tf._rotation22 = _this._rotation22;
return tf;
}
getLocalTransformTo(transform) {
let transform1 = this._localTransform;
transform._positionX = transform1._positionX;
transform._positionY = transform1._positionY;
transform._positionZ = transform1._positionZ;
transform._rotation00 = transform1._rotation00;
transform._rotation01 = transform1._rotation01;
transform._rotation02 = transform1._rotation02;
transform._rotation10 = transform1._rotation10;
transform._rotation11 = transform1._rotation11;
transform._rotation12 = transform1._rotation12;
transform._rotation20 = transform1._rotation20;
transform._rotation21 = transform1._rotation21;
transform._rotation22 = transform1._rotation22;
}
getTransform() {
let _this = this._transform;
let tf = new oimo.common.Transform();
tf._positionX = _this._positionX;
tf._positionY = _this._positionY;
tf._positionZ = _this._positionZ;
tf._rotation00 = _this._rotation00;
tf._rotation01 = _this._rotation01;
tf._rotation02 = _this._rotation02;
tf._rotation10 = _this._rotation10;
tf._rotation11 = _this._rotation11;
tf._rotation12 = _this._rotation12;
tf._rotation20 = _this._rotation20;
tf._rotation21 = _this._rotation21;
tf._rotation22 = _this._rotation22;
return tf;
}
getTransformTo(transform) {
let transform1 = this._transform;
transform._positionX = transform1._positionX;
transform._positionY = transform1._positionY;
transform._positionZ = transform1._positionZ;
transform._rotation00 = transform1._rotation00;
transform._rotation01 = transform1._rotation01;
transform._rotation02 = transform1._rotation02;
transform._rotation10 = transform1._rotation10;
transform._rotation11 = transform1._rotation11;
transform._rotation12 = transform1._rotation12;
transform._rotation20 = transform1._rotation20;
transform._rotation21 = transform1._rotation21;
transform._rotation22 = transform1._rotation22;
}
setLocalTransform(transform) {
let _this = this._localTransform;
_this._positionX = transform._positionX;
_this._positionY = transform._positionY;
_this._positionZ = transform._positionZ;
_this._rotation00 = transform._rotation00;
_this._rotation01 = transform._rotation01;
_this._rotation02 = transform._rotation02;
_this._rotation10 = transform._rotation10;
_this._rotation11 = transform._rotation11;
_this._rotation12 = transform._rotation12;
_this._rotation20 = transform._rotation20;
_this._rotation21 = transform._rotation21;
_this._rotation22 = transform._rotation22;
if(this._rigidBody != null) {
let _this = this._rigidBody;
_this.updateMass();
let s = _this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = _this._ptransform;
let tf2 = _this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
}
}
getDensity() {
return this._density;
}
setDensity(density) {
this._density = density;
if(this._rigidBody != null) {
let _this = this._rigidBody;
_this.updateMass();
let s = _this._shapeList;
while(s != null) {
let n = s._next;
let tf1 = _this._ptransform;
let tf2 = _this._transform;
let dst = s._ptransform;
let src1 = s._localTransform;
let __tmp__00;
let __tmp__01;
let __tmp__02;
let __tmp__10;
let __tmp__11;
let __tmp__12;
let __tmp__20;
let __tmp__21;
let __tmp__22;
__tmp__00 = tf1._rotation00 * src1._rotation00 + tf1._rotation01 * src1._rotation10 + tf1._rotation02 * src1._rotation20;
__tmp__01 = tf1._rotation00 * src1._rotation01 + tf1._rotation01 * src1._rotation11 + tf1._rotation02 * src1._rotation21;
__tmp__02 = tf1._rotation00 * src1._rotation02 + tf1._rotation01 * src1._rotation12 + tf1._rotation02 * src1._rotation22;
__tmp__10 = tf1._rotation10 * src1._rotation00 + tf1._rotation11 * src1._rotation10 + tf1._rotation12 * src1._rotation20;
__tmp__11 = tf1._rotation10 * src1._rotation01 + tf1._rotation11 * src1._rotation11 + tf1._rotation12 * src1._rotation21;
__tmp__12 = tf1._rotation10 * src1._rotation02 + tf1._rotation11 * src1._rotation12 + tf1._rotation12 * src1._rotation22;
__tmp__20 = tf1._rotation20 * src1._rotation00 + tf1._rotation21 * src1._rotation10 + tf1._rotation22 * src1._rotation20;
__tmp__21 = tf1._rotation20 * src1._rotation01 + tf1._rotation21 * src1._rotation11 + tf1._rotation22 * src1._rotation21;
__tmp__22 = tf1._rotation20 * src1._rotation02 + tf1._rotation21 * src1._rotation12 + tf1._rotation22 * src1._rotation22;
dst._rotation00 = __tmp__00;
dst._rotation01 = __tmp__01;
dst._rotation02 = __tmp__02;
dst._rotation10 = __tmp__10;
dst._rotation11 = __tmp__11;
dst._rotation12 = __tmp__12;
dst._rotation20 = __tmp__20;
dst._rotation21 = __tmp__21;
dst._rotation22 = __tmp__22;
let __tmp__X;
let __tmp__Y;
let __tmp__Z;
__tmp__X = tf1._rotation00 * src1._positionX + tf1._rotation01 * src1._positionY + tf1._rotation02 * src1._positionZ;
__tmp__Y = tf1._rotation10 * src1._positionX + tf1._rotation11 * src1._positionY + tf1._rotation12 * src1._positionZ;
__tmp__Z = tf1._rotation20 * src1._positionX + tf1._rotation21 * src1._positionY + tf1._rotation22 * src1._positionZ;
dst._positionX = __tmp__X;
dst._positionY = __tmp__Y;
dst._positionZ = __tmp__Z;
dst._positionX += tf1._positionX;
dst._positionY += tf1._positionY;
dst._positionZ += tf1._positionZ;
let dst1 = s._transform;
let src11 = s._localTransform;
let __tmp__001;
let __tmp__011;
let __tmp__021;
let __tmp__101;
let __tmp__111;
let __tmp__121;
let __tmp__201;
let __tmp__211;
let __tmp__221;
__tmp__001 = tf2._rotation00 * src11._rotation00 + tf2._rotation01 * src11._rotation10 + tf2._rotation02 * src11._rotation20;
__tmp__011 = tf2._rotation00 * src11._rotation01 + tf2._rotation01 * src11._rotation11 + tf2._rotation02 * src11._rotation21;
__tmp__021 = tf2._rotation00 * src11._rotation02 + tf2._rotation01 * src11._rotation12 + tf2._rotation02 * src11._rotation22;
__tmp__101 = tf2._rotation10 * src11._rotation00 + tf2._rotation11 * src11._rotation10 + tf2._rotation12 * src11._rotation20;
__tmp__111 = tf2._rotation10 * src11._rotation01 + tf2._rotation11 * src11._rotation11 + tf2._rotation12 * src11._rotation21;
__tmp__121 = tf2._rotation10 * src11._rotation02 + tf2._rotation11 * src11._rotation12 + tf2._rotation12 * src11._rotation22;
__tmp__201 = tf2._rotation20 * src11._rotation00 + tf2._rotation21 * src11._rotation10 + tf2._rotation22 * src11._rotation20;
__tmp__211 = tf2._rotation20 * src11._rotation01 + tf2._rotation21 * src11._rotation11 + tf2._rotation22 * src11._rotation21;
__tmp__221 = tf2._rotation20 * src11._rotation02 + tf2._rotation21 * src11._rotation12 + tf2._rotation22 * src11._rotation22;
dst1._rotation00 = __tmp__001;
dst1._rotation01 = __tmp__011;
dst1._rotation02 = __tmp__021;
dst1._rotation10 = __tmp__101;
dst1._rotation11 = __tmp__111;
dst1._rotation12 = __tmp__121;
dst1._rotation20 = __tmp__201;
dst1._rotation21 = __tmp__211;
dst1._rotation22 = __tmp__221;
let __tmp__X1;
let __tmp__Y1;
let __tmp__Z1;
__tmp__X1 = tf2._rotation00 * src11._positionX + tf2._rotation01 * src11._positionY + tf2._rotation02 * src11._positionZ;
__tmp__Y1 = tf2._rotation10 * src11._positionX + tf2._rotation11 * src11._positionY + tf2._rotation12 * src11._positionZ;
__tmp__Z1 = tf2._rotation20 * src11._positionX + tf2._rotation21 * src11._positionY + tf2._rotation22 * src11._positionZ;
dst1._positionX = __tmp__X1;
dst1._positionY = __tmp__Y1;
dst1._positionZ = __tmp__Z1;
dst1._positionX += tf2._positionX;
dst1._positionY += tf2._positionY;
dst1._positionZ += tf2._positionZ;
let minX;
let minY;
let minZ;
let maxX;
let maxY;
let maxZ;
s._geom._computeAabb(s._aabb,s._ptransform);
minX = s._aabb._minX;
minY = s._aabb._minY;
minZ = s._aabb._minZ;
maxX = s._aabb._maxX;
maxY = s._aabb._maxY;
maxZ = s._aabb._maxZ;
s._geom._computeAabb(s._aabb,s._transform);
s._aabb._minX = minX < s._aabb._minX ? minX : s._aabb._minX;
s._aabb._minY = minY < s._aabb._minY ? minY : s._aabb._minY;
s._aabb._minZ = minZ < s._aabb._minZ ? minZ : s._aabb._minZ;
s._aabb._maxX = maxX > s._aabb._maxX ? maxX : s._aabb._maxX;
s._aabb._maxY = maxY > s._aabb._maxY ? maxY : s._aabb._maxY;
s._aabb._maxZ = maxZ > s._aabb._maxZ ? maxZ : s._aabb._maxZ;
if(s._proxy != null) {
let dX;
let dY;
let dZ;
dX = s._transform._positionX - s._ptransform._positionX;
dY = s._transform._positionY - s._ptransform._positionY;
dZ = s._transform._positionZ - s._ptransform._positionZ;
let v = s.displacement;
v.x = dX;
v.y = dY;
v.z = dZ;
s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
}
s = n;
}
}
}
getAabb() {
return this._aabb.clone();
}
getAabbTo(aabb) {
aabb.copyFrom(this._aabb);
}
getGeometry() {
return this._geom;
}
getRigidBody() {
return this._rigidBody;
}
getCollisionGroup() {
return this._collisionGroup;
}
setCollisionGroup(collisionGroup) {
this._collisionGroup = collisionGroup;
}
getCollisionMask() {
return this._collisionMask;
}
setCollisionMask(collisionMask) {
this._collisionMask = collisionMask;
}
getContactCallback() {
return this._contactCallback;
}
setContactCallback(callback) {
this._contactCallback = callback;
}
getPrev() {
return this._prev;
}
getNext() {
return this._next;
}
}
oimo.dynamics.rigidbody.ShapeConfig = class oimo_dynamics_rigidbody_ShapeConfig {
constructor() {
this.position = new oimo.common.Vec3();
this.rotation = new oimo.common.Mat3();
this.friction = oimo.common.Setting.defaultFriction;
this.restitution = oimo.common.Setting.defaultRestitution;
this.density = oimo.common.Setting.defaultDensity;
this.collisionGroup = oimo.common.Setting.defaultCollisionGroup;
this.collisionMask = oimo.common.Setting.defaultCollisionMask;
this.geometry = null;
this.contactCallback = null;
}
}
if(!oimo.m) oimo.m = {};
oimo.m.M = class oimo_m_M {
}
oimo.collision.broadphase.BroadPhaseType._BRUTE_FORCE = 1;
oimo.collision.broadphase.BroadPhaseType._BVH = 2;
oimo.collision.broadphase.BroadPhaseType.BRUTE_FORCE = 1;
oimo.collision.broadphase.BroadPhaseType.BVH = 2;
oimo.collision.broadphase.bvh.BvhInsertionStrategy.SIMPLE = 0;
oimo.collision.broadphase.bvh.BvhInsertionStrategy.MINIMIZE_SURFACE_AREA = 1;
oimo.collision.geometry.GeometryType._SPHERE = 0;
oimo.collision.geometry.GeometryType._BOX = 1;
oimo.collision.geometry.GeometryType._CYLINDER = 2;
oimo.collision.geometry.GeometryType._CONE = 3;
oimo.collision.geometry.GeometryType._CAPSULE = 4;
oimo.collision.geometry.GeometryType._CONVEX_HULL = 5;
oimo.collision.geometry.GeometryType._CONVEX_MIN = 0;
oimo.collision.geometry.GeometryType._CONVEX_MAX = 5;
oimo.collision.geometry.GeometryType.SPHERE = 0;
oimo.collision.geometry.GeometryType.BOX = 1;
oimo.collision.geometry.GeometryType.CYLINDER = 2;
oimo.collision.geometry.GeometryType.CONE = 3;
oimo.collision.geometry.GeometryType.CAPSULE = 4;
oimo.collision.geometry.GeometryType.CONVEX_HULL = 5;
oimo.collision.narrowphase.detector.BoxBoxDetector.EDGE_BIAS_MULT = 1.0;
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedronState.OK = 0;
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedronState.INVALID_TRIANGLE = 1;
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedronState.NO_ADJACENT_PAIR_INDEX = 2;
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedronState.NO_ADJACENT_TRIANGLE = 3;
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedronState.EDGE_LOOP_BROKEN = 4;
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedronState.NO_OUTER_TRIANGLE = 5;
oimo.collision.narrowphase.detector.gjkepa.EpaPolyhedronState.TRIANGLE_INVISIBLE = 6;
oimo.collision.narrowphase.detector.gjkepa.EpaTriangle.count = 0;
oimo.common.Vec3.numCreations = 0;
oimo.common.Setting.defaultFriction = 0.2;
oimo.common.Setting.defaultRestitution = 0.2;
oimo.common.Setting.defaultDensity = 1;
oimo.common.Setting.defaultCollisionGroup = 1;
oimo.common.Setting.defaultCollisionMask = 1;
oimo.common.Setting.maxTranslationPerStep = 20;
oimo.common.Setting.maxRotationPerStep = 3.14159265358979;
oimo.common.Setting.bvhProxyPadding = 0.1;
oimo.common.Setting.bvhIncrementalCollisionThreshold = 0.45;
oimo.common.Setting.defaultGJKMargin = 0.05;
oimo.common.Setting.enableGJKCaching = true;
oimo.common.Setting.maxEPAVertices = 128;
oimo.common.Setting.maxEPAPolyhedronFaces = 128;
oimo.common.Setting.contactEnableBounceThreshold = 0.5;
oimo.common.Setting.velocityBaumgarte = 0.2;
oimo.common.Setting.positionSplitImpulseBaumgarte = 0.4;
oimo.common.Setting.positionNgsBaumgarte = 1.0;
oimo.common.Setting.contactUseAlternativePositionCorrectionAlgorithmDepthThreshold = 0.05;
oimo.common.Setting.defaultContactPositionCorrectionAlgorithm = 0;
oimo.common.Setting.alternativeContactPositionCorrectionAlgorithm = 1;
oimo.common.Setting.contactPersistenceThreshold = 0.05;
oimo.common.Setting.maxManifoldPoints = 4;
oimo.common.Setting.defaultJointConstraintSolverType = 0;
oimo.common.Setting.defaultJointPositionCorrectionAlgorithm = 0;
oimo.common.Setting.jointWarmStartingFactorForBaungarte = 0.8;
oimo.common.Setting.jointWarmStartingFactor = 0.95;
oimo.common.Setting.minSpringDamperDampingRatio = 1e-6;
oimo.common.Setting.minRagdollMaxSwingAngle = 1e-6;
oimo.common.Setting.maxJacobianRows = 6;
oimo.common.Setting.directMlcpSolverEps = 1e-9;
oimo.common.Setting.islandInitialRigidBodyArraySize = 128;
oimo.common.Setting.islandInitialConstraintArraySize = 128;
oimo.common.Setting.sleepingVelocityThreshold = 0.2;
oimo.common.Setting.sleepingAngularVelocityThreshold = 0.5;
oimo.common.Setting.sleepingTimeThreshold = 1.0;
oimo.common.Setting.disableSleeping = false;
oimo.common.Setting.linearSlop = 0.005;
oimo.common.Setting.angularSlop = 0.017453292519943278;
oimo.collision.narrowphase.detector.gjkepa.GjkEpa.instance = new oimo.collision.narrowphase.detector.gjkepa.GjkEpa();
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState._SUCCEEDED = 0;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState._GJK_FAILED_TO_MAKE_TETRAHEDRON = 1;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState._GJK_DID_NOT_CONVERGE = 2;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState._EPA_FAILED_TO_INIT = 257;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState._EPA_FAILED_TO_ADD_VERTEX = 258;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState._EPA_DID_NOT_CONVERGE = 259;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.SUCCEEDED = 0;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.GJK_FAILED_TO_MAKE_TETRAHEDRON = 1;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.GJK_DID_NOT_CONVERGE = 2;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.EPA_FAILED_TO_INIT = 257;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.EPA_FAILED_TO_ADD_VERTEX = 258;
oimo.collision.narrowphase.detector.gjkepa.GjkEpaResultState.EPA_DID_NOT_CONVERGE = 259;
oimo.common.Mat3.numCreations = 0;
oimo.common.Mat4.numCreations = 0;
oimo.common.MathUtil.POSITIVE_INFINITY = 1e65536;
oimo.common.MathUtil.NEGATIVE_INFINITY = -1e65536;
oimo.common.MathUtil.PI = 3.14159265358979;
oimo.common.MathUtil.TWO_PI = 6.28318530717958;
oimo.common.MathUtil.HALF_PI = 1.570796326794895;
oimo.common.MathUtil.TO_RADIANS = 0.017453292519943278;
oimo.common.MathUtil.TO_DEGREES = 57.29577951308238;
oimo.common.Quat.numCreations = 0;
oimo.dynamics.common.DebugDraw.SPHERE_PHI_DIVISION = 8;
oimo.dynamics.common.DebugDraw.SPHERE_THETA_DIVISION = 4;
oimo.dynamics.common.DebugDraw.CIRCLE_THETA_DIVISION = 8;
oimo.dynamics.common.Performance.broadPhaseCollisionTime = 0;
oimo.dynamics.common.Performance.narrowPhaseCollisionTime = 0;
oimo.dynamics.common.Performance.dynamicsTime = 0;
oimo.dynamics.common.Performance.totalTime = 0;
oimo.dynamics.constraint.PositionCorrectionAlgorithm._BAUMGARTE = 0;
oimo.dynamics.constraint.PositionCorrectionAlgorithm._SPLIT_IMPULSE = 1;
oimo.dynamics.constraint.PositionCorrectionAlgorithm._NGS = 2;
oimo.dynamics.constraint.PositionCorrectionAlgorithm.BAUMGARTE = 0;
oimo.dynamics.constraint.PositionCorrectionAlgorithm.SPLIT_IMPULSE = 1;
oimo.dynamics.constraint.PositionCorrectionAlgorithm.NGS = 2;
oimo.dynamics.constraint.info.JacobianRow.BIT_LINEAR_SET = 1;
oimo.dynamics.constraint.info.JacobianRow.BIT_ANGULAR_SET = 2;
oimo.dynamics.constraint.joint.JointType._SPHERICAL = 0;
oimo.dynamics.constraint.joint.JointType._REVOLUTE = 1;
oimo.dynamics.constraint.joint.JointType._CYLINDRICAL = 2;
oimo.dynamics.constraint.joint.JointType._PRISMATIC = 3;
oimo.dynamics.constraint.joint.JointType._UNIVERSAL = 4;
oimo.dynamics.constraint.joint.JointType._RAGDOLL = 5;
oimo.dynamics.constraint.joint.JointType._GENERIC = 6;
oimo.dynamics.constraint.joint.JointType.SPHERICAL = 0;
oimo.dynamics.constraint.joint.JointType.REVOLUTE = 1;
oimo.dynamics.constraint.joint.JointType.CYLINDRICAL = 2;
oimo.dynamics.constraint.joint.JointType.PRISMATIC = 3;
oimo.dynamics.constraint.joint.JointType.UNIVERSAL = 4;
oimo.dynamics.constraint.joint.JointType.RAGDOLL = 5;
oimo.dynamics.constraint.joint.JointType.GENERIC = 6;
oimo.dynamics.constraint.solver.ConstraintSolverType._ITERATIVE = 0;
oimo.dynamics.constraint.solver.ConstraintSolverType._DIRECT = 1;
oimo.dynamics.constraint.solver.ConstraintSolverType.ITERATIVE = 0;
oimo.dynamics.constraint.solver.ConstraintSolverType.DIRECT = 1;
oimo.dynamics.rigidbody.RigidBodyType._DYNAMIC = 0;
oimo.dynamics.rigidbody.RigidBodyType._STATIC = 1;
oimo.dynamics.rigidbody.RigidBodyType._KINEMATIC = 2;
oimo.dynamics.rigidbody.RigidBodyType.DYNAMIC = 0;
oimo.dynamics.rigidbody.RigidBodyType.STATIC = 1;
oimo.dynamics.rigidbody.RigidBodyType.KINEMATIC = 2;
export {oimo};