@ue-too/dynamics 0.9.5 → 0.10.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/README.md +538 -4
- package/collision-filter.d.ts +115 -0
- package/constraint.d.ts +70 -0
- package/dynamic-tree.d.ts +16 -0
- package/index.d.ts +189 -0
- package/index.js +2 -1947
- package/index.js.map +9 -9
- package/package.json +3 -3
- package/pair-manager.d.ts +36 -0
- package/quadtree.d.ts +4 -0
- package/rigidbody.d.ts +21 -0
- package/world.d.ts +51 -0
package/index.js
CHANGED
|
@@ -1,1948 +1,3 @@
|
|
|
1
|
-
// src/rigidbody.ts
|
|
2
|
-
import { PointCal } from "@ue-too/math";
|
|
1
|
+
import{PointCal as X}from"@ue-too/math";var r={category:1,mask:65535,group:0};function o(k,Y){if(k.group!==0&&k.group===Y.group)return k.group>0;return(k.category&Y.mask)!==0&&(Y.category&k.mask)!==0}var H0={STATIC:1,DYNAMIC:2,PLAYER:4,ENEMY:8,PROJECTILE:16,SENSOR:32,PICKUP:64,PLATFORM:128};class n{_center;_mass=50;_linearVelocity;_angularVelocity;_orientationAngle=0;linearAcceleartion;force;isStaticBody=!1;_staticFrictionCoeff=0.3;dynamicFrictionCoeff=0.3;frictionEnabled=!1;isMovingStaticBody=!1;angularDampingFactor=0.005;collisionFilter={...r};isSleeping=!1;sleepThreshold=0.01;sleepTime=0.5;timeAtRest=0;constructor(k,Y=0,z=50,w=!1,$=!1){this._center=k,this._orientationAngle=Y,this._mass=z,this.isStaticBody=w,this.frictionEnabled=$,this.force={x:0,y:0},this.linearAcceleartion={x:0,y:0},this._linearVelocity={x:0,y:0},this._angularVelocity=0}move(k){if(!this.isStatic())this._center=X.addVector(this._center,k)}rotateRadians(k){this._orientationAngle+=k}getCenter(){return this._center}getOrientationAngle(){return this._orientationAngle}get angularVelocity(){return this._angularVelocity}set angularVelocity(k){this._angularVelocity=k}get orientationAngle(){return this._orientationAngle}isStatic(){return this.isStaticBody}isMovingStatic(){return this.isMovingStaticBody}setLinearVelocity(k){this._linearVelocity=k}setMovingStatic(k){this.isMovingStaticBody=k}setOrientationAngle(k){this._orientationAngle=k}applyForce(k){if(X.magnitude(this.force)!==0)this.force=X.addVector(this.force,k);else this.force=k}applyForceInOrientation(k){let Y;if(typeof k==="number")Y=X.rotatePoint({x:k,y:0},this._orientationAngle);else Y=X.rotatePoint(k,this._orientationAngle);this.applyForce(Y)}step(k){if(this.frictionEnabled)if(this.isStatic()||this.linearVelocity.x==0&&this.linearVelocity.y==0&&X.magnitude(X.subVector({x:this.force.x,y:this.force.y},{x:0,y:0}))>=0&&X.magnitude({x:this.force.x,y:this.force.y})<this.staticFrictionCoeff*this.mass*9.81)if(this.force.z!=null)this.force={x:0,y:0,z:this.force.z};else this.force={x:0,y:0};else{let G=X.multiplyVectorByScalar(X.unitVector({x:this._linearVelocity.x,y:this._linearVelocity.y}),-1),J=X.multiplyVectorByScalar(G,this.dynamicFrictionCoeff*this.mass*9.81);this.force=X.addVector(this.force,J)}let Y=this._angularVelocity!=0?this._angularVelocity>0?-this.angularDampingFactor:this.angularDampingFactor:0;if(Math.abs(this._angularVelocity)<Math.abs(Y))this._angularVelocity=0;else this._angularVelocity+=Y;if(this._orientationAngle+=this._angularVelocity*k,X.magnitude({x:this._linearVelocity.x,y:this._linearVelocity.y})<X.magnitude(X.divideVectorByScalar(X.multiplyVectorByScalar(this.force,k),this.mass)))if(this._linearVelocity.z!=null)this._linearVelocity={x:0,y:0,z:this._linearVelocity.z};else this._linearVelocity={x:0,y:0};let z=-9.81*this._mass;this.force=X.addVector(this.force,{x:0,y:0,z});let w=X.divideVectorByScalar(X.multiplyVectorByScalar(this.force,k),this.mass);this._linearVelocity=X.addVector(this._linearVelocity,w);let $=X.multiplyVectorByScalar(this._linearVelocity,k);if(this._center=X.addVector(this._center,$),this._center.z!=null&&this._center.z<0)this._center.z=0;this.force={x:0,y:0}}get center(){return this._center}set center(k){this._center=k}get linearVelocity(){return this._linearVelocity}set linearVelocity(k){this._linearVelocity=k}get mass(){return this._mass}get staticFrictionCoeff(){return this._staticFrictionCoeff}set staticFrictionCoeff(k){this._staticFrictionCoeff=k}setSleeping(k){if(k&&!this.isSleeping)this.isSleeping=!0,this._linearVelocity={x:0,y:0},this._angularVelocity=0;else if(!k&&this.isSleeping)this.isSleeping=!1,this.timeAtRest=0}updateSleeping(k){if(this.isStatic()||this.isMovingStatic())return;let Y=X.magnitude(this._linearVelocity),z=Math.abs(this._angularVelocity);if(Y<this.sleepThreshold&&z<this.sleepThreshold){if(this.timeAtRest+=k,this.timeAtRest>=this.sleepTime)this.setSleeping(!0)}else if(this.timeAtRest=0,this.isSleeping)this.setSleeping(!1)}}class G0{_circle;_context;collisionFilter={...r};isSleeping=!1;sleepThreshold=0.01;sleepTime=0.5;timeAtRest=0;constructor(k={x:0,y:0},Y,z,w=0,$=50,G=!1,J=!0){this._circle=new Y0(k,Y,w,$,G,J),this._context=z}draw(k){k.beginPath(),k.arc(this._circle.center.x,this._circle.center.y,this._circle.radius,0,2*Math.PI),k.stroke()}step(k){this._circle.step(k),this.draw(this._context)}isStatic(){return this._circle.isStatic()}isMovingStatic(){return this._circle.isMovingStatic()}getMinMaxProjection(k){return this._circle.getMinMaxProjection(k)}getCollisionAxes(k){return this._circle.getCollisionAxes(k)}applyForce(k){this._circle.applyForce(k)}get AABB(){return this._circle.AABB}getMass(){return this._circle.mass}applyForceInOrientation(k){this._circle.applyForceInOrientation(k)}move(k){this._circle.move(k)}getSignificantVertices(k){return this._circle.getSignificantVertices(k)}get center(){return this._circle.center}set center(k){this._circle.center=k}get linearVelocity(){return this._circle.linearVelocity}set linearVelocity(k){this._circle.linearVelocity=k}get orientationAngle(){return this._circle.orientationAngle}significantVertex(k){return this._circle.significantVertex(k)}set angularVelocity(k){this._circle.angularVelocity=k}get angularVelocity(){return this._circle.angularVelocity}get mass(){return this._circle.mass}getNormalOfSignificantFace(k){return this._circle.getNormalOfSignificantFace(k)}get staticFrictionCoeff(){return this._circle.staticFrictionCoeff}set staticFrictionCoeff(k){this._circle.staticFrictionCoeff=k}getAdjacentFaces(k){return this._circle.getAdjacentFaces(k)}get momentOfInertia(){return this._circle.momentOfInertia}setSleeping(k){if(k&&!this.isSleeping)this.isSleeping=!0,this._circle.linearVelocity={x:0,y:0},this._circle.angularVelocity=0;else if(!k&&this.isSleeping)this.isSleeping=!1,this.timeAtRest=0}updateSleeping(k){if(this._circle.isStatic()||this._circle.isMovingStatic())return;let Y=X.magnitude(this._circle.linearVelocity),z=Math.abs(this._circle.angularVelocity);if(Y<this.sleepThreshold&&z<this.sleepThreshold){if(this.timeAtRest+=k,this.timeAtRest>=this.sleepTime)this.setSleeping(!0)}else if(this.timeAtRest=0,this.isSleeping)this.setSleeping(!1)}}class J0{_polygon;_context;collisionFilter={...r};isSleeping=!1;sleepThreshold=0.01;sleepTime=0.5;timeAtRest=0;constructor(k={x:0,y:0},Y,z,w=0,$=50,G=!1,J=!0){this._polygon=new k0(k,Y,w,$,G,J),this._context=z}draw(k){k.beginPath();let Y=this._polygon.getVerticesAbsCoord();k.moveTo(Y[0].x,Y[0].y),Y.forEach((z)=>{k.lineTo(z.x,z.y)}),k.lineTo(Y[0].x,Y[0].y),k.stroke()}step(k){this._polygon.step(k),this.draw(this._context)}isStatic(){return this._polygon.isStatic()}isMovingStatic(){return this._polygon.isMovingStatic()}getMinMaxProjection(k){return this._polygon.getMinMaxProjection(k)}getCollisionAxes(k){return this._polygon.getCollisionAxes(k)}applyForce(k){this._polygon.applyForce(k)}applyForceInOrientation(k){this._polygon.applyForceInOrientation(k)}setLinearVelocity(k){this._polygon.setLinearVelocity(k)}move(k){this._polygon.move(k)}get center(){return this._polygon.center}set center(k){this._polygon.center=k}get linearVelocity(){return this._polygon.linearVelocity}set linearVelocity(k){this._polygon.linearVelocity=k}get angularVelocity(){return this._polygon.angularVelocity}set angularVelocity(k){this._polygon.angularVelocity=k}get orientationAngle(){return this._polygon.orientationAngle}significantVertex(k){return this._polygon.significantVertex(k)}getSignificantVertices(k){return this._polygon.getSignificantVertices(k)}get AABB(){return this._polygon.AABB}get mass(){return this._polygon.mass}get staticFrictionCoeff(){return this._polygon.staticFrictionCoeff}set staticFrictionCoeff(k){this._polygon.staticFrictionCoeff=k}get momentOfInertia(){return this._polygon.momentOfInertia}getNormalOfSignificantFace(k){return this._polygon.getNormalOfSignificantFace(k)}getAdjacentFaces(k){return this._polygon.getAdjacentFaces(k)}setSleeping(k){if(k&&!this.isSleeping)this.isSleeping=!0,this._polygon.linearVelocity={x:0,y:0},this._polygon.angularVelocity=0;else if(!k&&this.isSleeping)this.isSleeping=!1,this.timeAtRest=0}updateSleeping(k){if(this._polygon.isStatic()||this._polygon.isMovingStatic())return;let Y=X.magnitude(this._polygon.linearVelocity),z=Math.abs(this._polygon.angularVelocity);if(Y<this.sleepThreshold&&z<this.sleepThreshold){if(this.timeAtRest+=k,this.timeAtRest>=this.sleepTime)this.setSleeping(!0)}else if(this.timeAtRest=0,this.isSleeping)this.setSleeping(!1)}}class k0 extends n{vertices;_momentOfInertia;constructor(k={x:0,y:0},Y,z=0,w=50,$=!1,G=!0){super(k,z,w,$,G);this.vertices=Y,this.step=this.step.bind(this);let J=this.vertices.reduce((Q,U,R)=>{let W=R<this.vertices.length-1?R+1:0,S=this.vertices[W],K=X.crossProduct(S,U);return Q+X.magnitude(K)*(X.dotProduct(U,U)+X.dotProduct(U,S)+X.dotProduct(S,S))},0),Z=this.vertices.reduce((Q,U,R)=>{return Q+X.magnitude(X.crossProduct(this.vertices[R<this.vertices.length-1?R+1:0],U))},0);this._momentOfInertia=this._mass*J/(6*Z)}getVerticesAbsCoord(){return this.vertices.map((k)=>{return X.addVector(this._center,X.rotatePoint(k,this._orientationAngle))})}getCollisionAxes(k){return this.getVerticesAbsCoord().map((Y,z,w)=>{let $=X.subVector(Y,w[w.length-1]);if(z>0)$=X.subVector(Y,w[z-1]);return X.unitVector(X.rotatePoint($,Math.PI/2))})}getMinMaxProjection(k){let z=this.getVerticesAbsCoord().map((w)=>{return X.dotProduct(w,k)});return{min:Math.min(...z),max:Math.max(...z)}}get AABB(){let k=this.getVerticesAbsCoord(),Y=k.map((w)=>w.x),z=k.map((w)=>w.y);return{min:{x:Math.min(...Y),y:Math.min(...z)},max:{x:Math.max(...Y),y:Math.max(...z)}}}significantVertex(k){let Y=this.getVerticesAbsCoord(),z=Y.map(($)=>X.dotProduct($,k)),w=z.indexOf(Math.max(...z));return Y[w]}getSignificantVertices(k){let Y=this.getVerticesAbsCoord(),z=Y.map((W)=>X.dotProduct(W,k)),w=z.indexOf(Math.max(...z)),$=Y[w],G=w>0?w-1:Y.length-1,J=w<Y.length-1?w+1:0,Z=Y[G],Q=Y[J],U=X.dotProduct(Z,k),R=X.dotProduct(Q,k);if(U>R)return[Z,$];else return[$,Q]}getNormalOfSignificantFace(k){let Y=this.getSignificantVertices(k),z=X.unitVectorFromA2B(Y[0],Y[1]);return X.rotatePoint(z,-Math.PI/2)}getAdjacentFaces(k){let Y=this.getVerticesAbsCoord(),z=Y.map((K)=>X.dotProduct(K,k)),w=z.indexOf(Math.max(...z)),$=Y[w],G=w>0?w-1:Y.length-1,J=w<Y.length-1?w+1:0,Z=Y[G],Q=Y[J],U=X.dotProduct(Z,k),R=X.dotProduct(Q,k),W=[],S=[];if(U>R){W.push(Z,$),S.push({startPoint:{coord:Z,index:G},endPoint:{coord:$,index:w}}),S.unshift({startPoint:{coord:$,index:w},endPoint:{coord:Q,index:J}});let K=G>0?G-1:Y.length-1;S.unshift({startPoint:{coord:Y[K],index:K},endPoint:{coord:Z,index:G}})}else{W.push($,Q),S.push({startPoint:{coord:$,index:w},endPoint:{coord:Q,index:J}});let K=J<Y.length-1?J+1:0;S.unshift({startPoint:{coord:Q,index:J},endPoint:{coord:Y[K],index:K}}),S.unshift({startPoint:{coord:Z,index:G},endPoint:{coord:$,index:w}})}return S}get momentOfInertia(){return this._momentOfInertia}}class Y0 extends n{_radius;_momentOfInertia;constructor(k={x:0,y:0},Y,z=0,w=50,$=!1,G=!0){super(k,z,w,$,G);this._radius=Y,this.step=this.step.bind(this),this._momentOfInertia=this._mass*this._radius*this._radius/2}getMinMaxProjection(k){let Y=X.addVector(this._center,X.multiplyVectorByScalar(k,this._radius)),z=X.addVector(this._center,X.multiplyVectorByScalar(k,-this._radius));return{min:X.dotProduct(z,k),max:X.dotProduct(Y,k)}}getCollisionAxes(k){return[X.unitVector(X.subVector(k.center,this._center))]}get AABB(){return{min:X.subVector(this._center,{x:this._radius,y:this._radius}),max:X.addVector(this._center,{x:this._radius,y:this._radius})}}significantVertex(k){return X.addVector(this._center,X.multiplyVectorByScalar(k,this._radius))}get radius(){return this._radius}getSignificantVertices(k){return[X.addVector(this._center,X.multiplyVectorByScalar(k,this._radius))]}getNormalOfSignificantFace(k){return X.unitVector(k)}getAdjacentFaces(k){return[]}get momentOfInertia(){return this._momentOfInertia}}class j{bottomLeft;width;height;constructor(k,Y,z){this.bottomLeft=k,this.width=Y,this.height=z}getWidth(){return this.width}getHeight(){return this.height}getbottomLeft(){return this.bottomLeft}}class M{MAX_OBJECTS=10;MAX_LEVELS=5;level;objects=[];nodes=[];bounds;constructor(k,Y){this.level=k,this.objects=[],this.bounds=Y,this.nodes=[void 0,void 0,void 0,void 0]}draw(k){k.beginPath(),k.rect(this.bounds.getbottomLeft().x,this.bounds.getbottomLeft().y,this.bounds.getWidth(),this.bounds.getHeight()),k.stroke();for(let Y=0;Y<this.nodes.length;Y++){let z=this.nodes[Y];if(z!=null)z.draw(k)}}clear(){this.objects=[];for(let k=0;k<this.nodes.length;k++){let Y=this.nodes[k];if(Y!=null)Y.clear(),Y=void 0}}split(){let k=this.bounds.getWidth()/2,Y=this.bounds.getHeight()/2,z=this.bounds.getbottomLeft();this.nodes[0]=new M(this.level+1,new j({x:z.x,y:z.y},k,Y)),this.nodes[1]=new M(this.level+1,new j({x:z.x,y:z.y+Y},k,Y)),this.nodes[2]=new M(this.level+1,new j({x:z.x+k,y:z.y+Y},k,Y)),this.nodes[3]=new M(this.level+1,new j({x:z.x+k,y:z.y},k,Y))}getIndex(k){let Y={x:this.bounds.getbottomLeft().x+this.bounds.getWidth()/2,y:this.bounds.getbottomLeft().y+this.bounds.getHeight()/2},z=k.AABB,w=z.max.y<Y.y&&z.min.y>this.bounds.getbottomLeft().y,$=z.max.x<Y.x&&z.min.x>this.bounds.getbottomLeft().x,G=z.max.x>Y.x&&z.min.x>Y.x,J=z.max.y>Y.y&&z.min.y>Y.y;if(w&&$)return 0;else if(w&&G)return 3;else if(J&&$)return 1;else if(J&&G)return 2;return-1}insert(k){let Y=this.nodes[0];if(Y!=null){let z=this.getIndex(k);if(z!==-1){Y=this.nodes[z],Y?.insert(k);return}}if(this.objects.push(k),this.objects.length>this.MAX_OBJECTS&&this.level<this.MAX_LEVELS){if(this.nodes[0]==null||this.nodes[0]==null)this.split();let z=0;while(z<this.objects.length){let w=this.getIndex(this.objects[z]),$=this.nodes[w];if(w!=-1&&$!==void 0){let G=this.objects[z];this.objects.splice(z,1),$.insert(G)}else z++}}}retrieve(k){let Y=this.getIndex(k),z=[],w=this.nodes[Y];if(Y!==-1&&w!==void 0)z.push(...w.retrieve(k));return z.push(...this.objects),z}}class t{parent=null;children=[null,null];aabb={min:{x:0,y:0},max:{x:0,y:0}};object=null;height=0;constructor(k){if(k)this.setLeaf(k)}isLeaf(){return this.children[0]===null}setLeaf(k){this.object=k,this.updateAABB(),this.children=[null,null],this.height=0}setBranch(k,Y){k.parent=this,Y.parent=this,this.children=[k,Y],this.object=null,this.updateAABB(),this.height=1+Math.max(k.height,Y.height)}updateAABB(k=0.1){if(this.isLeaf()&&this.object){let Y=this.object.AABB;this.aabb={min:{x:Y.min.x-k,y:Y.min.y-k},max:{x:Y.max.x+k,y:Y.max.y+k}}}else if(this.children[0]&&this.children[1]){let Y=this.children[0].aabb,z=this.children[1].aabb;this.aabb={min:{x:Math.min(Y.min.x,z.min.x),y:Math.min(Y.min.y,z.min.y)},max:{x:Math.max(Y.max.x,z.max.x),y:Math.max(Y.max.y,z.max.y)}}}}getSibling(){if(!this.parent)return null;return this.parent.children[0]===this?this.parent.children[1]:this.parent.children[0]}getBalance(){if(this.isLeaf())return 0;let k=this.children[0]?this.children[0].height:0,Y=this.children[1]?this.children[1].height:0;return k-Y}}class u{xEndpoints=[];objects=new Map;nextId=0;clear(){this.xEndpoints=[],this.objects.clear(),this.nextId=0}insert(k){let Y=this.nextId++,z={value:k.AABB.min.x,isMin:!0,object:k,id:Y},w={value:k.AABB.max.x,isMin:!1,object:k,id:Y};this.objects.set(k,{minEndpoint:z,maxEndpoint:w}),this.insertEndpointSorted(z),this.insertEndpointSorted(w)}update(k){let Y=this.objects.get(k);if(!Y)return;let z=k.AABB.min.x,w=k.AABB.max.x;if(Y.minEndpoint.value!==z)this.removeEndpoint(Y.minEndpoint),Y.minEndpoint.value=z,this.insertEndpointSorted(Y.minEndpoint);if(Y.maxEndpoint.value!==w)this.removeEndpoint(Y.maxEndpoint),Y.maxEndpoint.value=w,this.insertEndpointSorted(Y.maxEndpoint)}remove(k){let Y=this.objects.get(k);if(!Y)return;this.removeEndpoint(Y.minEndpoint),this.removeEndpoint(Y.maxEndpoint),this.objects.delete(k)}retrieve(k){let Y=[],z=k.AABB.min.x,w=k.AABB.max.x;for(let $ of this.xEndpoints){if($.value>w)break;if($.isMin&&$.object!==k){let G=this.objects.get($.object)?.maxEndpoint.value;if(G!==void 0&&G>=z){if(this.aabbIntersects($.object.AABB,k.AABB))Y.push($.object)}}}return Y}findAllOverlaps(){let k=[],Y=new Set;for(let z of this.xEndpoints)if(z.isMin){for(let w of Y)if(this.aabbIntersects(z.object.AABB,w.AABB))k.push({a:z.object,b:w});Y.add(z.object)}else Y.delete(z.object);return k}insertEndpointSorted(k){let Y=0,z=this.xEndpoints.length;while(Y<z){let w=Math.floor((Y+z)/2),$=this.xEndpoints[w];if($.value<k.value||$.value===k.value&&$.isMin&&!k.isMin)Y=w+1;else z=w}this.xEndpoints.splice(Y,0,k)}removeEndpoint(k){let Y=this.xEndpoints.indexOf(k);if(Y!==-1)this.xEndpoints.splice(Y,1)}aabbIntersects(k,Y){return!(k.max.x<Y.min.x||k.min.x>Y.max.x||k.max.y<Y.min.y||k.min.y>Y.max.y)}draw(k){k.strokeStyle="orange",k.lineWidth=1,k.globalAlpha=0.3;for(let Y of this.xEndpoints)k.beginPath(),k.moveTo(Y.value,-1000),k.lineTo(Y.value,1000),k.stroke(),k.fillStyle=Y.isMin?"green":"red",k.fillText(Y.isMin?"min":"max",Y.value,-950);k.globalAlpha=1}getStats(){return{endpointCount:this.xEndpoints.length,objectCount:this.objects.size}}}class C{root=null;nodeCount=0;margin=0.1;clear(){this.root=null,this.nodeCount=0}insert(k){let Y=new t(k);this.insertNode(Y)}insertNode(k){if(this.nodeCount++,!this.root){this.root=k;return}let Y=this.findBestSibling(k),z=Y.parent,w=new t;if(w.parent=z,w.setBranch(Y,k),z)if(z.children[0]===Y)z.children[0]=w;else z.children[1]=w;else this.root=w;let $=w.parent;while($)$.updateAABB(this.margin),$.height=1+Math.max($.children[0]?$.children[0].height:0,$.children[1]?$.children[1].height:0),$=this.balance($),$=$.parent}findBestSibling(k){let Y=this.root;while(!Y.isLeaf()){let z=Y.children[0],w=Y.children[1],$=this.getArea(Y.aabb),G=this.combineAABB(Y.aabb,k.aabb),J=this.getArea(G),Z=2*J,Q=2*(J-$),U,R=this.combineAABB(k.aabb,z.aabb);if(z.isLeaf())U=this.getArea(R)+Q;else{let K=this.getArea(z.aabb);U=this.getArea(R)-K+Q}let W,S=this.combineAABB(k.aabb,w.aabb);if(w.isLeaf())W=this.getArea(S)+Q;else{let K=this.getArea(w.aabb);W=this.getArea(S)-K+Q}if(Z<U&&Z<W)break;Y=U<W?z:w}return Y}balance(k){if(k.isLeaf())return k;let Y=k.getBalance();if(Y<-1)return this.rotateLeft(k);else if(Y>1)return this.rotateRight(k);return k}rotateLeft(k){let Y=k.children[1],z=Y.children[0],w=Y.children[1];if(Y.children[0]=k,Y.parent=k.parent,k.parent=Y,Y.parent)if(Y.parent.children[0]===k)Y.parent.children[0]=Y;else Y.parent.children[1]=Y;else this.root=Y;if(z.height>w.height)Y.children[1]=z,k.children[1]=w,w.parent=k,k.updateAABB(this.margin),Y.updateAABB(this.margin),k.height=1+Math.max(k.children[0].height,k.children[1].height),Y.height=1+Math.max(k.height,z.height);else Y.children[1]=w,k.children[1]=z,z.parent=k,k.updateAABB(this.margin),Y.updateAABB(this.margin),k.height=1+Math.max(k.children[0].height,k.children[1].height),Y.height=1+Math.max(k.height,w.height);return Y}rotateRight(k){let Y=k.children[0],z=Y.children[0],w=Y.children[1];if(Y.children[1]=k,Y.parent=k.parent,k.parent=Y,Y.parent)if(Y.parent.children[0]===k)Y.parent.children[0]=Y;else Y.parent.children[1]=Y;else this.root=Y;if(z.height>w.height)Y.children[0]=z,k.children[0]=w,w.parent=k,k.updateAABB(this.margin),Y.updateAABB(this.margin),k.height=1+Math.max(k.children[0].height,k.children[1].height),Y.height=1+Math.max(z.height,k.height);else Y.children[0]=w,k.children[0]=z,z.parent=k,k.updateAABB(this.margin),Y.updateAABB(this.margin),k.height=1+Math.max(k.children[0].height,k.children[1].height),Y.height=1+Math.max(w.height,k.height);return Y}retrieve(k){let Y=[];if(!this.root)return Y;let z=[this.root];while(z.length>0){let w=z.pop();if(this.aabbIntersects(w.aabb,k.AABB))if(w.isLeaf()){if(w.object&&w.object!==k)Y.push(w.object)}else{if(w.children[0])z.push(w.children[0]);if(w.children[1])z.push(w.children[1])}}return Y}draw(k){if(!this.root)return;this.drawNode(k,this.root,0)}drawNode(k,Y,z){let{min:w,max:$}=Y.aabb,G=["red","blue","green","orange","purple","brown"];if(k.strokeStyle=G[z%G.length],k.lineWidth=Math.max(1,3-z),k.beginPath(),k.rect(w.x,w.y,$.x-w.x,$.y-w.y),k.stroke(),!Y.isLeaf()){if(Y.children[0])this.drawNode(k,Y.children[0],z+1);if(Y.children[1])this.drawNode(k,Y.children[1],z+1)}}getArea(k){return(k.max.x-k.min.x)*(k.max.y-k.min.y)}combineAABB(k,Y){return{min:{x:Math.min(k.min.x,Y.min.x),y:Math.min(k.min.y,Y.min.y)},max:{x:Math.max(k.max.x,Y.max.x),y:Math.max(k.max.y,Y.max.y)}}}aabbIntersects(k,Y){return!(k.max.x<Y.min.x||k.min.x>Y.max.x||k.max.y<Y.min.y||k.min.y>Y.max.y)}getStats(){return{nodeCount:this.nodeCount,height:this.root?this.root.height:0}}}import{PointCal as H}from"@ue-too/math";function Z0(k,Y,z){if(k.isStatic()&&Y.isStatic())return;let w=0.4,$=k.isStatic()||k.isMovingStatic()?0:1/k.mass,G=Y.isStatic()||Y.isMovingStatic()?0:1/Y.mass,J=H.subVector(k.linearVelocity,Y.linearVelocity),Z=-(1+w)*H.dotProduct(J,z);Z/=$+G;let Q=H.multiplyVectorByScalar(z,Z*$),U=H.multiplyVectorByScalar(z,Z*G);k.linearVelocity=H.addVector(k.linearVelocity,Q),Y.linearVelocity=H.subVector(Y.linearVelocity,U)}function z0(k,Y,z){if(k.isStatic()&&Y.isStatic())return;let w=0.4,$=k.isStatic()||k.isMovingStatic()?0:1/k.mass,G=Y.isStatic()||Y.isMovingStatic()?0:1/Y.mass,J=k.isStatic()||k.isMovingStatic()?0:1/k.momentOfInertia,Z=Y.isStatic()||Y.isMovingStatic()?0:1/Y.momentOfInertia,Q=[];for(let U=0;U<z.contactPoints.length;U++){let R=z.contactPoints[U],W=H.subVector(R,k.center),S=H.subVector(R,Y.center),K={x:-W.y,y:W.x},_={x:-S.y,y:S.x},E=H.multiplyVectorByScalar(K,k.angularVelocity),V=H.multiplyVectorByScalar(_,Y.angularVelocity),L=H.subVector(H.addVector(k.linearVelocity,E),H.addVector(Y.linearVelocity,V)),F=H.dotProduct(L,z.normal),B=H.dotProduct(K,z.normal),O=H.dotProduct(_,z.normal),D=$+G+B*B*J+O*O*Z,g=-(1+w)*F;g/=D,g/=z.contactPoints.length,Q.push(H.multiplyVectorByScalar(z.normal,g))}Q.forEach((U,R)=>{let W=H.multiplyVectorByScalar(U,$),S=H.multiplyVectorByScalar(U,G);k.linearVelocity=H.addVector(k.linearVelocity,W);let K=H.crossProduct(H.subVector(z.contactPoints[R],k.center),U).z;K=K==null?0:K;let _=H.crossProduct(H.subVector(z.contactPoints[R],Y.center),U).z;_=_==null?0:_,k.angularVelocity+=K*J,Y.angularVelocity-=_*Z,Y.linearVelocity=H.subVector(Y.linearVelocity,S)})}function c(k,Y){if(k.min.x<=Y.max.x&&Y.min.x<=k.max.x&&(k.min.y<=Y.max.y&&Y.min.y<=k.max.y))return!0;return!1}function i(k,Y){let z=[],w=k.getCollisionAxes(Y),$=Y.getCollisionAxes(k);z.push(...w),z.push(...$);let G=!0,J=Number.MAX_VALUE,Z=z[0];if(z.forEach((Q)=>{let U=k.getMinMaxProjection(Q),R=Y.getMinMaxProjection(Q);if(U.min>=R.max||R.min>=U.max)G=!1;else{let W=Math.abs(Math.min(U.max,R.max)-Math.max(R.min,U.min));if(W<J){if(J=W,Z=Q,U.max<R.max)Z=H.multiplyVectorByScalar(Z,-1)}}}),G)return{collision:G,depth:J,normal:Z};else return{collision:!1,depth:void 0,normal:void 0}}function N0(k,Y,z){if(!z)return[];let w=[];return Y.forEach(($)=>{let{bodyA:G,bodyB:J}=$;if(G==J)return;let Z=G.center.z==null?0:G.center.z,Q=J.center.z==null?0:J.center.z;if(Math.abs(Z-Q)>0.5)return;let{collision:U,depth:R,normal:W}=i(G,J);if(U&&W!==void 0&&R!==void 0){let S=H.multiplyVectorByScalar(W,R/2),K=H.multiplyVectorByScalar(W,-R/2);if(!G.isStatic())G.move(S);if(!J.isStatic())J.move(K);if(G.isStatic())J.move(K);if(J.isStatic())G.move(S);let _=G.getNormalOfSignificantFace(H.multiplyVectorByScalar(W,-1)),E=J.getNormalOfSignificantFace(W),V=G.getSignificantVertices(H.multiplyVectorByScalar(W,-1)),L=J.getSignificantVertices(W),F=Math.abs(H.dotProduct(_,H.multiplyVectorByScalar(W,-1))),B=Math.abs(H.dotProduct(E,W));if(L.length==1||V.length==1)if(L.length==1)w.push(L[0]);else w.push(V[0]);else if(F>B){let O=G.getAdjacentFaces(H.multiplyVectorByScalar(W,-1)),D=[...L];for(let f=0;f<O.length-1;f++){let N=O[f].startPoint.coord,T=O[f].endPoint.coord,m=H.subVector(T,N),x=H.subVector(D[0],N),d=H.subVector(D[1],N),A=H.angleFromA2B(m,x)>=0,y=H.angleFromA2B(m,d)>=0;if((A?1:0)^(y?1:0)){let p=H.getLineIntersection(N,T,D[0],D[1]);if(p.intersects&&p.intersection!==void 0)if(A)D[1]=p.intersection;else D[0]=p.intersection}}let g=O[O.length-1],I=g.startPoint.coord,h=g.endPoint.coord,v=H.subVector(h,I),P=H.subVector(D[0],I),b=H.subVector(D[1],I),l=H.angleFromA2B(v,P)>=0,a=H.angleFromA2B(v,b)>=0;if(l)w.push(D[0]);if(a)w.push(D[1])}else{let O=J.getAdjacentFaces(W),D=[...V];if(D.length==0)console.log("warning");let g=0;for(let N=0;N<O.length-1;N++){let T=O[N].startPoint.coord,m=O[N].endPoint.coord,x=H.subVector(m,T),d=H.subVector(D[0],T),A=H.subVector(D[1],T),y=H.angleFromA2B(x,d)>=0,p=H.angleFromA2B(x,A)>=0;if((y?1:0)^(p?1:0)){g+=1;let s=H.getLineIntersection(T,m,D[0],D[1]);if(s.intersects&&s.intersection!==void 0)if(y)D[1]=s.intersection;else D[0]=s.intersection}}let I=O[O.length-1],h=I.startPoint.coord,v=I.endPoint.coord,P=H.subVector(v,h),b=H.subVector(D[0],h),l=H.subVector(D[1],h),a=H.angleFromA2B(P,b)>=0,f=H.angleFromA2B(P,l)>=0;if(a)w.push(D[0]);if(f)w.push(D[1])}if(z)z0(G,J,{normal:W,contactPoints:w})}}),w}function g0(k,Y,z){if(!z)return;Y.forEach((w)=>{let $=k[w.bodyAIndex],G=k[w.bodyBIndex],{collision:J,depth:Z,normal:Q}=i($,G);if(J&&Q!==void 0&&Z!==void 0){let U=H.multiplyVectorByScalar(Q,Z/2),R=H.multiplyVectorByScalar(Q,-Z/2);if(!$.isStatic())$.move(U);if(!G.isStatic())G.move(R);if($.isStatic())G.move(R);if(G.isStatic())$.move(U);if(z)Z0($,G,Q)}})}function M0(k,Y){let z=[];for(let w=0;w<=Y.length-1;w++){let $=k.retrieve(Y[w]);for(let G=0;G<=$.length-1;G++){let J=Y[w],Z=$[G];if(J.isStatic()&&Z.isStatic())continue;if(!c(J.AABB,Z.AABB))continue;z.push({bodyA:J,bodyB:Z})}}return z}function T0(k,Y){let z=[];for(let w=0;w<=Y.length-1;w++){let $=k.retrieve(Y[w]);for(let G=0;G<=$.length-1;G++){let J=Y[w],Z=$[G];if(J.isStatic()&&Z.isStatic())continue;if(!c(J.AABB,Z.AABB))continue;z.push({bodyA:J,bodyB:Z})}}return z}function j0(k,Y){let z=[];for(let w=0;w<=Y.length-1;w++){let $=k.retrieve(Y[w]);for(let G=0;G<=$.length-1;G++){let J=Y[w],Z=$[G];if(J.isStatic()&&Z.isStatic())continue;if(!c(J.AABB,Z.AABB))continue;z.push({bodyAIndex:w,bodyBIndex:G})}}return z}function w0(k,Y){let z=[];for(let w=0;w<=Y.length-1;w++){let $=k.retrieve(Y[w]);for(let G=0;G<=$.length-1;G++){let J=Y[w],Z=$[G];if(J.isStatic()&&Z.isStatic())continue;if(!o(J.collisionFilter,Z.collisionFilter))continue;if(!c(J.AABB,Z.AABB))continue;z.push({bodyA:J,bodyB:Z})}}return z}function $0(k,Y,z){let w=[],$=[];return Y.forEach((G)=>{let{bodyA:J,bodyB:Z}=G,{collision:Q,depth:U,normal:R}=i(J,Z);if(Q&&R!==void 0&&U!==void 0){let W=[],S={x:(J.center.x+Z.center.x)/2,y:(J.center.y+Z.center.y)/2};W.push(S);let K={bodyA:J,bodyB:Z,contactPoints:W,normal:R,depth:U};if($.push(K),w.push(...W),z){let _=H.multiplyVectorByScalar(R,U/2),E=H.multiplyVectorByScalar(R,-U/2);if(!J.isStatic())J.move(_);if(!Z.isStatic())Z.move(E);if(J.isStatic())Z.move(E);if(Z.isStatic())J.move(_);z0(J,Z,{normal:R,contactPoints:W})}}}),{contactPoints:w,collisions:$}}class e{pairs=new Map;frameNumber=0;maxPairAge=10;constructor(){}getPairId(k,Y){let z=this.getBodyId(k),w=this.getBodyId(Y);return z<w?`${z}:${w}`:`${w}:${z}`}getBodyId(k){return k.toString()}updatePairs(k){this.frameNumber++;let Y={created:[],updated:[],removed:[]};this.pairs.forEach((w)=>{w.isActive=!1});for(let w of k){let $=this.getPairId(w.bodyA,w.bodyB),G=this.pairs.get($);if(G)G.isActive=!0,G.frameUpdated=this.frameNumber,G.contactPoints=w.contactPoints||[],G.normal=w.normal,G.depth=w.depth,Y.updated.push(G);else{let J={bodyA:w.bodyA,bodyB:w.bodyB,id:$,isActive:!0,contactPoints:w.contactPoints||[],normal:w.normal,depth:w.depth,frameCreated:this.frameNumber,frameUpdated:this.frameNumber};this.pairs.set($,J),Y.created.push(J)}}let z=[];return this.pairs.forEach((w,$)=>{if(!w.isActive&&this.frameNumber-w.frameUpdated>this.maxPairAge)z.push($),Y.removed.push(w)}),z.forEach((w)=>{this.pairs.delete(w)}),Y}getActivePairs(){return Array.from(this.pairs.values()).filter((k)=>k.isActive)}getPair(k,Y){let z=this.getPairId(k,Y);return this.pairs.get(z)}clear(){this.pairs.clear(),this.frameNumber=0}getStats(){return{totalPairs:this.pairs.size,activePairs:this.getActivePairs().length,frameNumber:this.frameNumber}}}class X0{rigidBodyList;rigidBodyMap;_resolveCollision;maxTransWidth;maxTransHeight;bound;spatialIndex;spatialIndexType;constraints;pinJoints=[];pairManager;enableSleeping=!0;_context=null;constructor(k,Y,z="dynamictree"){if(this.maxTransHeight=Y,this.maxTransWidth=k,this.spatialIndexType=z,this.bound=new j({x:-this.maxTransWidth,y:-this.maxTransHeight},2*this.maxTransWidth,2*this.maxTransHeight),z==="dynamictree")this.spatialIndex=new C;else if(z==="sap")this.spatialIndex=new u;else this.spatialIndex=new M(0,this.bound);this.rigidBodyList=[],this.rigidBodyMap=new Map,this._resolveCollision=!0,this.constraints=[],this.pairManager=new e}addRigidBody(k,Y){if(this.rigidBodyList.push(Y),this.rigidBodyMap.set(k,Y),this.spatialIndexType==="sap")this.spatialIndex.insert(Y)}removeRigidBody(k){if(this.rigidBodyMap.has(k)){let Y=this.rigidBodyMap.get(k);if(this.rigidBodyMap.delete(k),Y&&this.spatialIndexType==="sap"&&this.spatialIndex instanceof u)this.spatialIndex.remove(Y);let z=this.rigidBodyList.findIndex((w)=>w===Y);if(z!==-1)this.rigidBodyList.splice(z,1)}}step(k){if(this.enableSleeping)this.getRigidBodyList().forEach((Y)=>{Y.updateSleeping(k)});if(this._resolveCollision){let Y=this.resolveCollisionPhase()}this.constraints.forEach((Y)=>Y.enforce(k)),this.getRigidBodyList().forEach((Y)=>{if(!Y.isSleeping)Y.step(k)})}resolveCollisionPhase(){let k=[];if(this.spatialIndexType==="sap")this.rigidBodyMap.forEach(($)=>{if(!this.enableSleeping||!$.isSleeping){if(k.push($),this.spatialIndex instanceof u)this.spatialIndex.update($)}});else this.spatialIndex.clear(),this.rigidBodyMap.forEach(($)=>{if(!this.enableSleeping||!$.isSleeping)k.push($),this.spatialIndex.insert($)});let Y=w0(this.spatialIndex,k),z=$0(k,Y,this._resolveCollision),w=this.pairManager.updatePairs(z.collisions);if(this.enableSleeping)w.created.forEach(($)=>{if($.bodyA.isSleeping)$.bodyA.setSleeping(!1);if($.bodyB.isSleeping)$.bodyB.setSleeping(!1)});return z.contactPoints}get resolveCollision(){return this._resolveCollision}set resolveCollision(k){this._resolveCollision=k}getRigidBodyList(){let k=[];return this.rigidBodyMap.forEach((Y)=>{k.push(Y)}),k}getRigidBodyMap(){return this.rigidBodyMap}setMaxTransHeight(k){this.maxTransHeight=k}setMaxTransWidth(k){this.maxTransWidth=k}addConstraint(k){this.constraints.push(k)}getConstraints(){return this.constraints}addPinJoint(k,Y,z,w){this.pinJoints.push({bodyA:k,bodyB:Y,anchorA:z,anchorB:w})}get currentSpatialIndexType(){return this.spatialIndexType}setSpatialIndexType(k){if(k===this.spatialIndexType)return;if(this.spatialIndexType=k,k==="dynamictree")this.spatialIndex=new C;else if(k==="sap")this.spatialIndex=new u;else this.spatialIndex=new M(0,this.bound)}getSpatialIndexStats(){if(this.spatialIndex instanceof C)return this.spatialIndex.getStats();else if(this.spatialIndex instanceof u)return{type:this.spatialIndexType,...this.spatialIndex.getStats()};return{type:this.spatialIndexType,objects:this.rigidBodyMap.size}}get sleepingEnabled(){return this.enableSleeping}set sleepingEnabled(k){if(this.enableSleeping=k,!k)this.rigidBodyMap.forEach((Y)=>{if(Y.isSleeping)Y.setSleeping(!1)})}getPairManager(){return this.pairManager}getCollisionStats(){return{...this.pairManager.getStats(),sleepingBodies:Array.from(this.rigidBodyMap.values()).filter((k)=>k.isSleeping).length,totalBodies:this.rigidBodyMap.size}}}import{PointCal as q}from"@ue-too/math";class Q0{anchorA;worldAnchorA;bodyA;constructor(k,Y,z){this.bodyA=k,this.anchorA=Y,this.worldAnchorA=z}enforce(k){this.solveWorldPinJointConstraint(k)}solveWorldPinJointConstraint(k){let Y=this.bodyA,z=this.anchorA,w=this.worldAnchorA,$=q.addVector(Y.center,q.rotatePoint(z,Y.orientationAngle)),G=q.subVector($,w),J=q.subVector($,Y.center),Z=q.addVector(Y.linearVelocity,q.crossProduct({x:0,y:0,z:Y.angularVelocity},J)),Q=Y.isStatic()?0:1/Y.mass,U=Y.isStatic()?0:1/Y.momentOfInertia,R={x:Q+U*J.y*J.y,y:Q+U*J.x*J.x,xy:-U*J.x*J.y},W=1,S={x:-R.x*G.x-R.xy*G.y-1*G.x/k-Z.x,y:-R.xy*G.x-R.y*G.y-1*G.y/k-Z.y};if(!Y.isStatic())Y.linearVelocity.x+=Q*S.x,Y.linearVelocity.y+=Q*S.y,Y.angularVelocity+=U*(J.x*S.y-J.y*S.x)}}class R0{anchorA;anchorB;bodyA;bodyB;constructor(k,Y,z,w){this.bodyA=k,this.bodyB=Y,this.anchorA=z,this.anchorB=w}enforce(k){this.solvePinJointConstraint(k)}solvePinJointConstraint(k){let Y=this.bodyA,z=this.bodyB,w=this.anchorA,$=this.anchorB,G=q.addVector(Y.center,q.rotatePoint(w,Y.orientationAngle)),J=q.addVector(z.center,q.rotatePoint($,z.orientationAngle)),Z=q.subVector(J,G),Q=q.subVector(G,Y.center),U=q.subVector(J,z.center),R=q.subVector(q.addVector(z.linearVelocity,q.crossProduct({x:0,y:0,z:z.angularVelocity},U)),q.addVector(Y.linearVelocity,q.crossProduct({x:0,y:0,z:Y.angularVelocity},Q))),W=Y.isStatic()?0:1/Y.mass,S=z.isStatic()?0:1/z.mass,K=Y.isStatic()?0:1/Y.momentOfInertia,_=z.isStatic()?0:1/z.momentOfInertia,E={x:W+S+K*Q.y*Q.y+_*U.y*U.y,y:W+S+K*Q.x*Q.x+_*U.x*U.x,xy:-K*Q.x*Q.y-_*U.x*U.y},V=1,L={x:-E.x*Z.x-E.xy*Z.y-1*Z.x/k-R.x,y:-E.xy*Z.x-E.y*Z.y-1*Z.y/k-R.y};if(!Y.isStatic())Y.linearVelocity.x-=W*L.x,Y.linearVelocity.y-=W*L.y,Y.angularVelocity-=K*(Q.x*L.y-Q.y*L.x);if(!z.isStatic())z.linearVelocity.x+=S*L.x,z.linearVelocity.y+=S*L.y,z.angularVelocity+=_*(U.x*L.y-U.y*L.x)}}function P0(k,Y){let{bodyA:z,bodyB:w,anchorA:$,anchorB:G}=k,J=q.addVector(z.center,q.rotatePoint($,z.orientationAngle)),Z=q.addVector(w.center,q.rotatePoint(G,w.orientationAngle)),Q=q.subVector(Z,J),U=q.subVector(J,z.center),R=q.subVector(Z,w.center),W=q.subVector(q.addVector(w.linearVelocity,q.crossProduct({x:0,y:0,z:w.angularVelocity},R)),q.addVector(z.linearVelocity,q.crossProduct({x:0,y:0,z:z.angularVelocity},U))),S=z.isStatic()?0:1/z.mass,K=w.isStatic()?0:1/w.mass,_=z.isStatic()?0:1/z.momentOfInertia,E=w.isStatic()?0:1/w.momentOfInertia,V={x:S+K+_*U.y*U.y+E*R.y*R.y,y:S+K+_*U.x*U.x+E*R.x*R.x,xy:-_*U.x*U.y-E*R.x*R.y},L=0.5,F={x:-V.x*Q.x-V.xy*Q.y-0.5*Q.x/Y-W.x,y:-V.xy*Q.x-V.y*Q.y-0.5*Q.y/Y-W.y};if(!z.isStatic())z.linearVelocity.x-=S*F.x,z.linearVelocity.y-=S*F.y,z.angularVelocity-=_*(U.x*F.y-U.y*F.x);if(!w.isStatic())w.linearVelocity.x+=K*F.x,w.linearVelocity.y+=K*F.y,w.angularVelocity+=E*(R.x*F.y-R.y*F.x)}function x0(k,Y){let{body:z,localAnchor:w,worldAnchor:$}=k,G=q.addVector(z.center,q.rotatePoint(w,z.orientationAngle)),J=q.subVector(G,$),Z=q.subVector(G,z.center),Q=q.addVector(z.linearVelocity,q.crossProduct({x:0,y:0,z:z.angularVelocity},Z)),U=z.isStatic()?0:1/z.mass,R=z.isStatic()?0:1/z.momentOfInertia,W={x:U+R*Z.y*Z.y,y:U+R*Z.x*Z.x,xy:-R*Z.x*Z.y},S=0.2,K={x:-W.x*J.x-W.xy*J.y-0.2*J.x/Y-Q.x,y:-W.xy*J.x-W.y*J.y-0.2*J.y/Y-Q.y};if(!z.isStatic())z.linearVelocity.x+=U*K.x,z.linearVelocity.y+=U*K.y,z.angularVelocity+=R*(Z.x*K.y-Z.y*K.x)}export{x0 as solveWorldPinJointConstraint,P0 as solvePinJointConstraint,z0 as resolveCollisionWithRotation,Z0 as resolveCollision,$0 as narrowPhaseWithRigidBodyAndPairs,N0 as narrowPhaseWithRigidBody,g0 as narrowPhase,i as intersects,o as canCollide,w0 as broadPhaseWithSpatialIndexFiltered,T0 as broadPhaseWithSpatialIndex,M0 as broadPhaseWithRigidBodyReturned,j0 as broadPhase,c as aabbIntersects,X0 as World,J0 as VisualPolygonBody,G0 as VisaulCircleBody,u as SweepAndPrune,j as RectangleBound,M as QuadTree,k0 as Polygon,R0 as PinJoint,e as PairManager,Q0 as FixedPinJoint,C as DynamicTree,r as DEFAULT_COLLISION_FILTER,H0 as CollisionCategory,Y0 as Circle,n as BaseRigidBody};
|
|
3
2
|
|
|
4
|
-
|
|
5
|
-
var DEFAULT_COLLISION_FILTER = {
|
|
6
|
-
category: 1,
|
|
7
|
-
mask: 65535,
|
|
8
|
-
group: 0
|
|
9
|
-
};
|
|
10
|
-
function canCollide(filterA, filterB) {
|
|
11
|
-
if (filterA.group !== 0 && filterA.group === filterB.group) {
|
|
12
|
-
return filterA.group > 0;
|
|
13
|
-
}
|
|
14
|
-
return (filterA.category & filterB.mask) !== 0 && (filterB.category & filterA.mask) !== 0;
|
|
15
|
-
}
|
|
16
|
-
var CollisionCategory = {
|
|
17
|
-
STATIC: 1,
|
|
18
|
-
DYNAMIC: 2,
|
|
19
|
-
PLAYER: 4,
|
|
20
|
-
ENEMY: 8,
|
|
21
|
-
PROJECTILE: 16,
|
|
22
|
-
SENSOR: 32,
|
|
23
|
-
PICKUP: 64,
|
|
24
|
-
PLATFORM: 128
|
|
25
|
-
};
|
|
26
|
-
|
|
27
|
-
// src/rigidbody.ts
|
|
28
|
-
class BaseRigidBody {
|
|
29
|
-
_center;
|
|
30
|
-
_mass = 50;
|
|
31
|
-
_linearVelocity;
|
|
32
|
-
_angularVelocity;
|
|
33
|
-
_orientationAngle = 0;
|
|
34
|
-
linearAcceleartion;
|
|
35
|
-
force;
|
|
36
|
-
isStaticBody = false;
|
|
37
|
-
_staticFrictionCoeff = 0.3;
|
|
38
|
-
dynamicFrictionCoeff = 0.3;
|
|
39
|
-
frictionEnabled = false;
|
|
40
|
-
isMovingStaticBody = false;
|
|
41
|
-
angularDampingFactor = 0.005;
|
|
42
|
-
collisionFilter = { ...DEFAULT_COLLISION_FILTER };
|
|
43
|
-
isSleeping = false;
|
|
44
|
-
sleepThreshold = 0.01;
|
|
45
|
-
sleepTime = 0.5;
|
|
46
|
-
timeAtRest = 0;
|
|
47
|
-
constructor(center, _orientationAngle = 0, mass = 50, isStaticBody = false, frictionEnabled = false) {
|
|
48
|
-
this._center = center;
|
|
49
|
-
this._orientationAngle = _orientationAngle;
|
|
50
|
-
this._mass = mass;
|
|
51
|
-
this.isStaticBody = isStaticBody;
|
|
52
|
-
this.frictionEnabled = frictionEnabled;
|
|
53
|
-
this.force = { x: 0, y: 0 };
|
|
54
|
-
this.linearAcceleartion = { x: 0, y: 0 };
|
|
55
|
-
this._linearVelocity = { x: 0, y: 0 };
|
|
56
|
-
this._angularVelocity = 0;
|
|
57
|
-
}
|
|
58
|
-
move(delta) {
|
|
59
|
-
if (!this.isStatic()) {
|
|
60
|
-
this._center = PointCal.addVector(this._center, delta);
|
|
61
|
-
}
|
|
62
|
-
}
|
|
63
|
-
rotateRadians(angle) {
|
|
64
|
-
this._orientationAngle += angle;
|
|
65
|
-
}
|
|
66
|
-
getCenter() {
|
|
67
|
-
return this._center;
|
|
68
|
-
}
|
|
69
|
-
getOrientationAngle() {
|
|
70
|
-
return this._orientationAngle;
|
|
71
|
-
}
|
|
72
|
-
get angularVelocity() {
|
|
73
|
-
return this._angularVelocity;
|
|
74
|
-
}
|
|
75
|
-
set angularVelocity(angularVelocity) {
|
|
76
|
-
this._angularVelocity = angularVelocity;
|
|
77
|
-
}
|
|
78
|
-
get orientationAngle() {
|
|
79
|
-
return this._orientationAngle;
|
|
80
|
-
}
|
|
81
|
-
isStatic() {
|
|
82
|
-
return this.isStaticBody;
|
|
83
|
-
}
|
|
84
|
-
isMovingStatic() {
|
|
85
|
-
return this.isMovingStaticBody;
|
|
86
|
-
}
|
|
87
|
-
setLinearVelocity(linearVelocity) {
|
|
88
|
-
this._linearVelocity = linearVelocity;
|
|
89
|
-
}
|
|
90
|
-
setMovingStatic(movingStatic) {
|
|
91
|
-
this.isMovingStaticBody = movingStatic;
|
|
92
|
-
}
|
|
93
|
-
setOrientationAngle(angle) {
|
|
94
|
-
this._orientationAngle = angle;
|
|
95
|
-
}
|
|
96
|
-
applyForce(force) {
|
|
97
|
-
if (PointCal.magnitude(this.force) !== 0) {
|
|
98
|
-
this.force = PointCal.addVector(this.force, force);
|
|
99
|
-
} else {
|
|
100
|
-
this.force = force;
|
|
101
|
-
}
|
|
102
|
-
}
|
|
103
|
-
applyForceInOrientation(force) {
|
|
104
|
-
let forceTransformed;
|
|
105
|
-
if (typeof force === "number") {
|
|
106
|
-
forceTransformed = PointCal.rotatePoint({ x: force, y: 0 }, this._orientationAngle);
|
|
107
|
-
} else {
|
|
108
|
-
forceTransformed = PointCal.rotatePoint(force, this._orientationAngle);
|
|
109
|
-
}
|
|
110
|
-
this.applyForce(forceTransformed);
|
|
111
|
-
}
|
|
112
|
-
step(deltaTime) {
|
|
113
|
-
if (this.frictionEnabled) {
|
|
114
|
-
if (this.isStatic() || this.linearVelocity.x == 0 && this.linearVelocity.y == 0 && PointCal.magnitude(PointCal.subVector({ x: this.force.x, y: this.force.y }, { x: 0, y: 0 })) >= 0 && PointCal.magnitude({ x: this.force.x, y: this.force.y }) < this.staticFrictionCoeff * this.mass * 9.81) {
|
|
115
|
-
if (this.force.z != null) {
|
|
116
|
-
this.force = { x: 0, y: 0, z: this.force.z };
|
|
117
|
-
} else {
|
|
118
|
-
this.force = { x: 0, y: 0 };
|
|
119
|
-
}
|
|
120
|
-
} else {
|
|
121
|
-
let kineticFrictionDirection = PointCal.multiplyVectorByScalar(PointCal.unitVector({ x: this._linearVelocity.x, y: this._linearVelocity.y }), -1);
|
|
122
|
-
let kineticFriction = PointCal.multiplyVectorByScalar(kineticFrictionDirection, this.dynamicFrictionCoeff * this.mass * 9.81);
|
|
123
|
-
this.force = PointCal.addVector(this.force, kineticFriction);
|
|
124
|
-
}
|
|
125
|
-
}
|
|
126
|
-
const angularDamping = this._angularVelocity != 0 ? this._angularVelocity > 0 ? -this.angularDampingFactor : this.angularDampingFactor : 0;
|
|
127
|
-
if (Math.abs(this._angularVelocity) < Math.abs(angularDamping)) {
|
|
128
|
-
this._angularVelocity = 0;
|
|
129
|
-
} else {
|
|
130
|
-
this._angularVelocity += angularDamping;
|
|
131
|
-
}
|
|
132
|
-
this._orientationAngle += this._angularVelocity * deltaTime;
|
|
133
|
-
if (PointCal.magnitude({ x: this._linearVelocity.x, y: this._linearVelocity.y }) < PointCal.magnitude(PointCal.divideVectorByScalar(PointCal.multiplyVectorByScalar(this.force, deltaTime), this.mass))) {
|
|
134
|
-
if (this._linearVelocity.z != null) {
|
|
135
|
-
this._linearVelocity = { x: 0, y: 0, z: this._linearVelocity.z };
|
|
136
|
-
} else {
|
|
137
|
-
this._linearVelocity = { x: 0, y: 0 };
|
|
138
|
-
}
|
|
139
|
-
}
|
|
140
|
-
const gravitationalForce = -9.81 * this._mass;
|
|
141
|
-
this.force = PointCal.addVector(this.force, { x: 0, y: 0, z: gravitationalForce });
|
|
142
|
-
const deltaLinearVelocity = PointCal.divideVectorByScalar(PointCal.multiplyVectorByScalar(this.force, deltaTime), this.mass);
|
|
143
|
-
this._linearVelocity = PointCal.addVector(this._linearVelocity, deltaLinearVelocity);
|
|
144
|
-
const deltaCenter = PointCal.multiplyVectorByScalar(this._linearVelocity, deltaTime);
|
|
145
|
-
this._center = PointCal.addVector(this._center, deltaCenter);
|
|
146
|
-
if (this._center.z != null && this._center.z < 0) {
|
|
147
|
-
this._center.z = 0;
|
|
148
|
-
}
|
|
149
|
-
this.force = { x: 0, y: 0 };
|
|
150
|
-
}
|
|
151
|
-
get center() {
|
|
152
|
-
return this._center;
|
|
153
|
-
}
|
|
154
|
-
set center(dest) {
|
|
155
|
-
this._center = dest;
|
|
156
|
-
}
|
|
157
|
-
get linearVelocity() {
|
|
158
|
-
return this._linearVelocity;
|
|
159
|
-
}
|
|
160
|
-
set linearVelocity(dest) {
|
|
161
|
-
this._linearVelocity = dest;
|
|
162
|
-
}
|
|
163
|
-
get mass() {
|
|
164
|
-
return this._mass;
|
|
165
|
-
}
|
|
166
|
-
get staticFrictionCoeff() {
|
|
167
|
-
return this._staticFrictionCoeff;
|
|
168
|
-
}
|
|
169
|
-
set staticFrictionCoeff(coeff) {
|
|
170
|
-
this._staticFrictionCoeff = coeff;
|
|
171
|
-
}
|
|
172
|
-
setSleeping(sleeping) {
|
|
173
|
-
if (sleeping && !this.isSleeping) {
|
|
174
|
-
this.isSleeping = true;
|
|
175
|
-
this._linearVelocity = { x: 0, y: 0 };
|
|
176
|
-
this._angularVelocity = 0;
|
|
177
|
-
} else if (!sleeping && this.isSleeping) {
|
|
178
|
-
this.isSleeping = false;
|
|
179
|
-
this.timeAtRest = 0;
|
|
180
|
-
}
|
|
181
|
-
}
|
|
182
|
-
updateSleeping(deltaTime) {
|
|
183
|
-
if (this.isStatic() || this.isMovingStatic())
|
|
184
|
-
return;
|
|
185
|
-
const speed = PointCal.magnitude(this._linearVelocity);
|
|
186
|
-
const angularSpeed = Math.abs(this._angularVelocity);
|
|
187
|
-
if (speed < this.sleepThreshold && angularSpeed < this.sleepThreshold) {
|
|
188
|
-
this.timeAtRest += deltaTime;
|
|
189
|
-
if (this.timeAtRest >= this.sleepTime) {
|
|
190
|
-
this.setSleeping(true);
|
|
191
|
-
}
|
|
192
|
-
} else {
|
|
193
|
-
this.timeAtRest = 0;
|
|
194
|
-
if (this.isSleeping) {
|
|
195
|
-
this.setSleeping(false);
|
|
196
|
-
}
|
|
197
|
-
}
|
|
198
|
-
}
|
|
199
|
-
}
|
|
200
|
-
|
|
201
|
-
class VisaulCircleBody {
|
|
202
|
-
_circle;
|
|
203
|
-
_context;
|
|
204
|
-
collisionFilter = { ...DEFAULT_COLLISION_FILTER };
|
|
205
|
-
isSleeping = false;
|
|
206
|
-
sleepThreshold = 0.01;
|
|
207
|
-
sleepTime = 0.5;
|
|
208
|
-
timeAtRest = 0;
|
|
209
|
-
constructor(center = { x: 0, y: 0 }, radius, drawingContext, _orientationAngle = 0, mass = 50, isStatic = false, frictionEnabled = true) {
|
|
210
|
-
this._circle = new Circle(center, radius, _orientationAngle, mass, isStatic, frictionEnabled);
|
|
211
|
-
this._context = drawingContext;
|
|
212
|
-
}
|
|
213
|
-
draw(ctx) {
|
|
214
|
-
ctx.beginPath();
|
|
215
|
-
ctx.arc(this._circle.center.x, this._circle.center.y, this._circle.radius, 0, 2 * Math.PI);
|
|
216
|
-
ctx.stroke();
|
|
217
|
-
}
|
|
218
|
-
step(deltaTime) {
|
|
219
|
-
this._circle.step(deltaTime);
|
|
220
|
-
this.draw(this._context);
|
|
221
|
-
}
|
|
222
|
-
isStatic() {
|
|
223
|
-
return this._circle.isStatic();
|
|
224
|
-
}
|
|
225
|
-
isMovingStatic() {
|
|
226
|
-
return this._circle.isMovingStatic();
|
|
227
|
-
}
|
|
228
|
-
getMinMaxProjection(unitvector) {
|
|
229
|
-
return this._circle.getMinMaxProjection(unitvector);
|
|
230
|
-
}
|
|
231
|
-
getCollisionAxes(relativeBody) {
|
|
232
|
-
return this._circle.getCollisionAxes(relativeBody);
|
|
233
|
-
}
|
|
234
|
-
applyForce(force) {
|
|
235
|
-
this._circle.applyForce(force);
|
|
236
|
-
}
|
|
237
|
-
get AABB() {
|
|
238
|
-
return this._circle.AABB;
|
|
239
|
-
}
|
|
240
|
-
getMass() {
|
|
241
|
-
return this._circle.mass;
|
|
242
|
-
}
|
|
243
|
-
applyForceInOrientation(force) {
|
|
244
|
-
this._circle.applyForceInOrientation(force);
|
|
245
|
-
}
|
|
246
|
-
move(delta) {
|
|
247
|
-
this._circle.move(delta);
|
|
248
|
-
}
|
|
249
|
-
getSignificantVertices(collisionNormal) {
|
|
250
|
-
return this._circle.getSignificantVertices(collisionNormal);
|
|
251
|
-
}
|
|
252
|
-
get center() {
|
|
253
|
-
return this._circle.center;
|
|
254
|
-
}
|
|
255
|
-
set center(dest) {
|
|
256
|
-
this._circle.center = dest;
|
|
257
|
-
}
|
|
258
|
-
get linearVelocity() {
|
|
259
|
-
return this._circle.linearVelocity;
|
|
260
|
-
}
|
|
261
|
-
set linearVelocity(dest) {
|
|
262
|
-
this._circle.linearVelocity = dest;
|
|
263
|
-
}
|
|
264
|
-
get orientationAngle() {
|
|
265
|
-
return this._circle.orientationAngle;
|
|
266
|
-
}
|
|
267
|
-
significantVertex(collisionNormal) {
|
|
268
|
-
return this._circle.significantVertex(collisionNormal);
|
|
269
|
-
}
|
|
270
|
-
set angularVelocity(angularVelocity) {
|
|
271
|
-
this._circle.angularVelocity = angularVelocity;
|
|
272
|
-
}
|
|
273
|
-
get angularVelocity() {
|
|
274
|
-
return this._circle.angularVelocity;
|
|
275
|
-
}
|
|
276
|
-
get mass() {
|
|
277
|
-
return this._circle.mass;
|
|
278
|
-
}
|
|
279
|
-
getNormalOfSignificantFace(collisionNormal) {
|
|
280
|
-
return this._circle.getNormalOfSignificantFace(collisionNormal);
|
|
281
|
-
}
|
|
282
|
-
get staticFrictionCoeff() {
|
|
283
|
-
return this._circle.staticFrictionCoeff;
|
|
284
|
-
}
|
|
285
|
-
set staticFrictionCoeff(coeff) {
|
|
286
|
-
this._circle.staticFrictionCoeff = coeff;
|
|
287
|
-
}
|
|
288
|
-
getAdjacentFaces(collisionNormal) {
|
|
289
|
-
return this._circle.getAdjacentFaces(collisionNormal);
|
|
290
|
-
}
|
|
291
|
-
get momentOfInertia() {
|
|
292
|
-
return this._circle.momentOfInertia;
|
|
293
|
-
}
|
|
294
|
-
setSleeping(sleeping) {
|
|
295
|
-
if (sleeping && !this.isSleeping) {
|
|
296
|
-
this.isSleeping = true;
|
|
297
|
-
this._circle.linearVelocity = { x: 0, y: 0 };
|
|
298
|
-
this._circle.angularVelocity = 0;
|
|
299
|
-
} else if (!sleeping && this.isSleeping) {
|
|
300
|
-
this.isSleeping = false;
|
|
301
|
-
this.timeAtRest = 0;
|
|
302
|
-
}
|
|
303
|
-
}
|
|
304
|
-
updateSleeping(deltaTime) {
|
|
305
|
-
if (this._circle.isStatic() || this._circle.isMovingStatic())
|
|
306
|
-
return;
|
|
307
|
-
const speed = PointCal.magnitude(this._circle.linearVelocity);
|
|
308
|
-
const angularSpeed = Math.abs(this._circle.angularVelocity);
|
|
309
|
-
if (speed < this.sleepThreshold && angularSpeed < this.sleepThreshold) {
|
|
310
|
-
this.timeAtRest += deltaTime;
|
|
311
|
-
if (this.timeAtRest >= this.sleepTime) {
|
|
312
|
-
this.setSleeping(true);
|
|
313
|
-
}
|
|
314
|
-
} else {
|
|
315
|
-
this.timeAtRest = 0;
|
|
316
|
-
if (this.isSleeping) {
|
|
317
|
-
this.setSleeping(false);
|
|
318
|
-
}
|
|
319
|
-
}
|
|
320
|
-
}
|
|
321
|
-
}
|
|
322
|
-
|
|
323
|
-
class VisualPolygonBody {
|
|
324
|
-
_polygon;
|
|
325
|
-
_context;
|
|
326
|
-
collisionFilter = { ...DEFAULT_COLLISION_FILTER };
|
|
327
|
-
isSleeping = false;
|
|
328
|
-
sleepThreshold = 0.01;
|
|
329
|
-
sleepTime = 0.5;
|
|
330
|
-
timeAtRest = 0;
|
|
331
|
-
constructor(center = { x: 0, y: 0 }, vertices, drawingContext, _orientationAngle = 0, mass = 50, isStatic = false, frictionEnabled = true) {
|
|
332
|
-
this._polygon = new Polygon(center, vertices, _orientationAngle, mass, isStatic, frictionEnabled);
|
|
333
|
-
this._context = drawingContext;
|
|
334
|
-
}
|
|
335
|
-
draw(ctx) {
|
|
336
|
-
ctx.beginPath();
|
|
337
|
-
let vertices = this._polygon.getVerticesAbsCoord();
|
|
338
|
-
ctx.moveTo(vertices[0].x, vertices[0].y);
|
|
339
|
-
vertices.forEach((vertex) => {
|
|
340
|
-
ctx.lineTo(vertex.x, vertex.y);
|
|
341
|
-
});
|
|
342
|
-
ctx.lineTo(vertices[0].x, vertices[0].y);
|
|
343
|
-
ctx.stroke();
|
|
344
|
-
}
|
|
345
|
-
step(deltaTime) {
|
|
346
|
-
this._polygon.step(deltaTime);
|
|
347
|
-
this.draw(this._context);
|
|
348
|
-
}
|
|
349
|
-
isStatic() {
|
|
350
|
-
return this._polygon.isStatic();
|
|
351
|
-
}
|
|
352
|
-
isMovingStatic() {
|
|
353
|
-
return this._polygon.isMovingStatic();
|
|
354
|
-
}
|
|
355
|
-
getMinMaxProjection(unitvector) {
|
|
356
|
-
return this._polygon.getMinMaxProjection(unitvector);
|
|
357
|
-
}
|
|
358
|
-
getCollisionAxes(relativeBody) {
|
|
359
|
-
return this._polygon.getCollisionAxes(relativeBody);
|
|
360
|
-
}
|
|
361
|
-
applyForce(force) {
|
|
362
|
-
this._polygon.applyForce(force);
|
|
363
|
-
}
|
|
364
|
-
applyForceInOrientation(force) {
|
|
365
|
-
this._polygon.applyForceInOrientation(force);
|
|
366
|
-
}
|
|
367
|
-
setLinearVelocity(linearVelocity) {
|
|
368
|
-
this._polygon.setLinearVelocity(linearVelocity);
|
|
369
|
-
}
|
|
370
|
-
move(delta) {
|
|
371
|
-
this._polygon.move(delta);
|
|
372
|
-
}
|
|
373
|
-
get center() {
|
|
374
|
-
return this._polygon.center;
|
|
375
|
-
}
|
|
376
|
-
set center(dest) {
|
|
377
|
-
this._polygon.center = dest;
|
|
378
|
-
}
|
|
379
|
-
get linearVelocity() {
|
|
380
|
-
return this._polygon.linearVelocity;
|
|
381
|
-
}
|
|
382
|
-
set linearVelocity(dest) {
|
|
383
|
-
this._polygon.linearVelocity = dest;
|
|
384
|
-
}
|
|
385
|
-
get angularVelocity() {
|
|
386
|
-
return this._polygon.angularVelocity;
|
|
387
|
-
}
|
|
388
|
-
set angularVelocity(angularVelocity) {
|
|
389
|
-
this._polygon.angularVelocity = angularVelocity;
|
|
390
|
-
}
|
|
391
|
-
get orientationAngle() {
|
|
392
|
-
return this._polygon.orientationAngle;
|
|
393
|
-
}
|
|
394
|
-
significantVertex(collisionNormal) {
|
|
395
|
-
return this._polygon.significantVertex(collisionNormal);
|
|
396
|
-
}
|
|
397
|
-
getSignificantVertices(collisionNormal) {
|
|
398
|
-
return this._polygon.getSignificantVertices(collisionNormal);
|
|
399
|
-
}
|
|
400
|
-
get AABB() {
|
|
401
|
-
return this._polygon.AABB;
|
|
402
|
-
}
|
|
403
|
-
get mass() {
|
|
404
|
-
return this._polygon.mass;
|
|
405
|
-
}
|
|
406
|
-
get staticFrictionCoeff() {
|
|
407
|
-
return this._polygon.staticFrictionCoeff;
|
|
408
|
-
}
|
|
409
|
-
set staticFrictionCoeff(coeff) {
|
|
410
|
-
this._polygon.staticFrictionCoeff = coeff;
|
|
411
|
-
}
|
|
412
|
-
get momentOfInertia() {
|
|
413
|
-
return this._polygon.momentOfInertia;
|
|
414
|
-
}
|
|
415
|
-
getNormalOfSignificantFace(collisionNormal) {
|
|
416
|
-
return this._polygon.getNormalOfSignificantFace(collisionNormal);
|
|
417
|
-
}
|
|
418
|
-
getAdjacentFaces(collisionNormal) {
|
|
419
|
-
return this._polygon.getAdjacentFaces(collisionNormal);
|
|
420
|
-
}
|
|
421
|
-
setSleeping(sleeping) {
|
|
422
|
-
if (sleeping && !this.isSleeping) {
|
|
423
|
-
this.isSleeping = true;
|
|
424
|
-
this._polygon.linearVelocity = { x: 0, y: 0 };
|
|
425
|
-
this._polygon.angularVelocity = 0;
|
|
426
|
-
} else if (!sleeping && this.isSleeping) {
|
|
427
|
-
this.isSleeping = false;
|
|
428
|
-
this.timeAtRest = 0;
|
|
429
|
-
}
|
|
430
|
-
}
|
|
431
|
-
updateSleeping(deltaTime) {
|
|
432
|
-
if (this._polygon.isStatic() || this._polygon.isMovingStatic())
|
|
433
|
-
return;
|
|
434
|
-
const speed = PointCal.magnitude(this._polygon.linearVelocity);
|
|
435
|
-
const angularSpeed = Math.abs(this._polygon.angularVelocity);
|
|
436
|
-
if (speed < this.sleepThreshold && angularSpeed < this.sleepThreshold) {
|
|
437
|
-
this.timeAtRest += deltaTime;
|
|
438
|
-
if (this.timeAtRest >= this.sleepTime) {
|
|
439
|
-
this.setSleeping(true);
|
|
440
|
-
}
|
|
441
|
-
} else {
|
|
442
|
-
this.timeAtRest = 0;
|
|
443
|
-
if (this.isSleeping) {
|
|
444
|
-
this.setSleeping(false);
|
|
445
|
-
}
|
|
446
|
-
}
|
|
447
|
-
}
|
|
448
|
-
}
|
|
449
|
-
|
|
450
|
-
class Polygon extends BaseRigidBody {
|
|
451
|
-
vertices;
|
|
452
|
-
_momentOfInertia;
|
|
453
|
-
constructor(center = { x: 0, y: 0 }, vertices, _orientationAngle = 0, mass = 50, isStatic = false, frictionEnabled = true) {
|
|
454
|
-
super(center, _orientationAngle, mass, isStatic, frictionEnabled);
|
|
455
|
-
this.vertices = vertices;
|
|
456
|
-
this.step = this.step.bind(this);
|
|
457
|
-
let numerator = this.vertices.reduce((acc, vertex, index) => {
|
|
458
|
-
let nextPointIndex = index < this.vertices.length - 1 ? index + 1 : 0;
|
|
459
|
-
let nextPoint = this.vertices[nextPointIndex];
|
|
460
|
-
let crossProduct = PointCal.crossProduct(nextPoint, vertex);
|
|
461
|
-
return acc + PointCal.magnitude(crossProduct) * (PointCal.dotProduct(vertex, vertex) + PointCal.dotProduct(vertex, nextPoint) + PointCal.dotProduct(nextPoint, nextPoint));
|
|
462
|
-
}, 0);
|
|
463
|
-
let denomiator = this.vertices.reduce((acc, vertex, index) => {
|
|
464
|
-
return acc + PointCal.magnitude(PointCal.crossProduct(this.vertices[index < this.vertices.length - 1 ? index + 1 : 0], vertex));
|
|
465
|
-
}, 0);
|
|
466
|
-
this._momentOfInertia = this._mass * numerator / (6 * denomiator);
|
|
467
|
-
}
|
|
468
|
-
getVerticesAbsCoord() {
|
|
469
|
-
return this.vertices.map((vertex) => {
|
|
470
|
-
return PointCal.addVector(this._center, PointCal.rotatePoint(vertex, this._orientationAngle));
|
|
471
|
-
});
|
|
472
|
-
}
|
|
473
|
-
getCollisionAxes(relativeBody) {
|
|
474
|
-
return this.getVerticesAbsCoord().map((vertex, index, absVertices) => {
|
|
475
|
-
let vector = PointCal.subVector(vertex, absVertices[absVertices.length - 1]);
|
|
476
|
-
if (index > 0) {
|
|
477
|
-
vector = PointCal.subVector(vertex, absVertices[index - 1]);
|
|
478
|
-
}
|
|
479
|
-
return PointCal.unitVector(PointCal.rotatePoint(vector, Math.PI / 2));
|
|
480
|
-
});
|
|
481
|
-
}
|
|
482
|
-
getMinMaxProjection(unitvector) {
|
|
483
|
-
let vertices = this.getVerticesAbsCoord();
|
|
484
|
-
let projections = vertices.map((vertex) => {
|
|
485
|
-
return PointCal.dotProduct(vertex, unitvector);
|
|
486
|
-
});
|
|
487
|
-
return { min: Math.min(...projections), max: Math.max(...projections) };
|
|
488
|
-
}
|
|
489
|
-
get AABB() {
|
|
490
|
-
let points = this.getVerticesAbsCoord();
|
|
491
|
-
let xCoords = points.map((vertex) => vertex.x);
|
|
492
|
-
let yCoords = points.map((vertex) => vertex.y);
|
|
493
|
-
return { min: { x: Math.min(...xCoords), y: Math.min(...yCoords) }, max: { x: Math.max(...xCoords), y: Math.max(...yCoords) } };
|
|
494
|
-
}
|
|
495
|
-
significantVertex(collisionNormal) {
|
|
496
|
-
let vertices = this.getVerticesAbsCoord();
|
|
497
|
-
let verticesProjected = vertices.map((vertex) => PointCal.dotProduct(vertex, collisionNormal));
|
|
498
|
-
let maxIndex = verticesProjected.indexOf(Math.max(...verticesProjected));
|
|
499
|
-
return vertices[maxIndex];
|
|
500
|
-
}
|
|
501
|
-
getSignificantVertices(collisionNormal) {
|
|
502
|
-
let vertices = this.getVerticesAbsCoord();
|
|
503
|
-
let verticesProjected = vertices.map((vertex) => PointCal.dotProduct(vertex, collisionNormal));
|
|
504
|
-
let maxIndex = verticesProjected.indexOf(Math.max(...verticesProjected));
|
|
505
|
-
const tipVertex = vertices[maxIndex];
|
|
506
|
-
let prevPointIndex = maxIndex > 0 ? maxIndex - 1 : vertices.length - 1;
|
|
507
|
-
let nextPointIndex = maxIndex < vertices.length - 1 ? maxIndex + 1 : 0;
|
|
508
|
-
const prevPoint = vertices[prevPointIndex];
|
|
509
|
-
const nextPoint = vertices[nextPointIndex];
|
|
510
|
-
const prevPointProjected = PointCal.dotProduct(prevPoint, collisionNormal);
|
|
511
|
-
const nextPointProjected = PointCal.dotProduct(nextPoint, collisionNormal);
|
|
512
|
-
if (prevPointProjected > nextPointProjected) {
|
|
513
|
-
return [prevPoint, tipVertex];
|
|
514
|
-
} else {
|
|
515
|
-
return [tipVertex, nextPoint];
|
|
516
|
-
}
|
|
517
|
-
}
|
|
518
|
-
getNormalOfSignificantFace(collisionNormal) {
|
|
519
|
-
const vertices = this.getSignificantVertices(collisionNormal);
|
|
520
|
-
const direction = PointCal.unitVectorFromA2B(vertices[0], vertices[1]);
|
|
521
|
-
return PointCal.rotatePoint(direction, -Math.PI / 2);
|
|
522
|
-
}
|
|
523
|
-
getAdjacentFaces(collisionNormal) {
|
|
524
|
-
let vertices = this.getVerticesAbsCoord();
|
|
525
|
-
let verticesProjected = vertices.map((vertex) => PointCal.dotProduct(vertex, collisionNormal));
|
|
526
|
-
let maxIndex = verticesProjected.indexOf(Math.max(...verticesProjected));
|
|
527
|
-
const tipVertex = vertices[maxIndex];
|
|
528
|
-
let prevPointIndex = maxIndex > 0 ? maxIndex - 1 : vertices.length - 1;
|
|
529
|
-
let nextPointIndex = maxIndex < vertices.length - 1 ? maxIndex + 1 : 0;
|
|
530
|
-
const prevPoint = vertices[prevPointIndex];
|
|
531
|
-
const nextPoint = vertices[nextPointIndex];
|
|
532
|
-
const prevPointProjected = PointCal.dotProduct(prevPoint, collisionNormal);
|
|
533
|
-
const nextPointProjected = PointCal.dotProduct(nextPoint, collisionNormal);
|
|
534
|
-
const adjacentFaces = [];
|
|
535
|
-
const adjacentFacesWithIndex = [];
|
|
536
|
-
if (prevPointProjected > nextPointProjected) {
|
|
537
|
-
adjacentFaces.push(prevPoint, tipVertex);
|
|
538
|
-
adjacentFacesWithIndex.push({ startPoint: { coord: prevPoint, index: prevPointIndex }, endPoint: { coord: tipVertex, index: maxIndex } });
|
|
539
|
-
adjacentFacesWithIndex.unshift({ startPoint: { coord: tipVertex, index: maxIndex }, endPoint: { coord: nextPoint, index: nextPointIndex } });
|
|
540
|
-
let prevPrevPointIndex = prevPointIndex > 0 ? prevPointIndex - 1 : vertices.length - 1;
|
|
541
|
-
adjacentFacesWithIndex.unshift({ startPoint: { coord: vertices[prevPrevPointIndex], index: prevPrevPointIndex }, endPoint: { coord: prevPoint, index: prevPointIndex } });
|
|
542
|
-
} else {
|
|
543
|
-
adjacentFaces.push(tipVertex, nextPoint);
|
|
544
|
-
adjacentFacesWithIndex.push({ startPoint: { coord: tipVertex, index: maxIndex }, endPoint: { coord: nextPoint, index: nextPointIndex } });
|
|
545
|
-
let nextNextPointIndex = nextPointIndex < vertices.length - 1 ? nextPointIndex + 1 : 0;
|
|
546
|
-
adjacentFacesWithIndex.unshift({ startPoint: { coord: nextPoint, index: nextPointIndex }, endPoint: { coord: vertices[nextNextPointIndex], index: nextNextPointIndex } });
|
|
547
|
-
adjacentFacesWithIndex.unshift({ startPoint: { coord: prevPoint, index: prevPointIndex }, endPoint: { coord: tipVertex, index: maxIndex } });
|
|
548
|
-
}
|
|
549
|
-
return adjacentFacesWithIndex;
|
|
550
|
-
}
|
|
551
|
-
get momentOfInertia() {
|
|
552
|
-
return this._momentOfInertia;
|
|
553
|
-
}
|
|
554
|
-
}
|
|
555
|
-
|
|
556
|
-
class Circle extends BaseRigidBody {
|
|
557
|
-
_radius;
|
|
558
|
-
_momentOfInertia;
|
|
559
|
-
constructor(center = { x: 0, y: 0 }, radius, _orientationAngle = 0, mass = 50, isStatic = false, frictionEnabled = true) {
|
|
560
|
-
super(center, _orientationAngle, mass, isStatic, frictionEnabled);
|
|
561
|
-
this._radius = radius;
|
|
562
|
-
this.step = this.step.bind(this);
|
|
563
|
-
this._momentOfInertia = this._mass * this._radius * this._radius / 2;
|
|
564
|
-
}
|
|
565
|
-
getMinMaxProjection(unitvector) {
|
|
566
|
-
let PositiveFurthest = PointCal.addVector(this._center, PointCal.multiplyVectorByScalar(unitvector, this._radius));
|
|
567
|
-
let NegativeFurthest = PointCal.addVector(this._center, PointCal.multiplyVectorByScalar(unitvector, -this._radius));
|
|
568
|
-
return { min: PointCal.dotProduct(NegativeFurthest, unitvector), max: PointCal.dotProduct(PositiveFurthest, unitvector) };
|
|
569
|
-
}
|
|
570
|
-
getCollisionAxes(relativeBody) {
|
|
571
|
-
return [PointCal.unitVector(PointCal.subVector(relativeBody.center, this._center))];
|
|
572
|
-
}
|
|
573
|
-
get AABB() {
|
|
574
|
-
return { min: PointCal.subVector(this._center, { x: this._radius, y: this._radius }), max: PointCal.addVector(this._center, { x: this._radius, y: this._radius }) };
|
|
575
|
-
}
|
|
576
|
-
significantVertex(collisionNormal) {
|
|
577
|
-
return PointCal.addVector(this._center, PointCal.multiplyVectorByScalar(collisionNormal, this._radius));
|
|
578
|
-
}
|
|
579
|
-
get radius() {
|
|
580
|
-
return this._radius;
|
|
581
|
-
}
|
|
582
|
-
getSignificantVertices(collisionNormal) {
|
|
583
|
-
return [PointCal.addVector(this._center, PointCal.multiplyVectorByScalar(collisionNormal, this._radius))];
|
|
584
|
-
}
|
|
585
|
-
getNormalOfSignificantFace(collisionNormal) {
|
|
586
|
-
return PointCal.unitVector(collisionNormal);
|
|
587
|
-
}
|
|
588
|
-
getAdjacentFaces(collisionNormal) {
|
|
589
|
-
return [];
|
|
590
|
-
}
|
|
591
|
-
get momentOfInertia() {
|
|
592
|
-
return this._momentOfInertia;
|
|
593
|
-
}
|
|
594
|
-
}
|
|
595
|
-
// src/quadtree.ts
|
|
596
|
-
class RectangleBound {
|
|
597
|
-
bottomLeft;
|
|
598
|
-
width;
|
|
599
|
-
height;
|
|
600
|
-
constructor(bottomLeft, width, height) {
|
|
601
|
-
this.bottomLeft = bottomLeft;
|
|
602
|
-
this.width = width;
|
|
603
|
-
this.height = height;
|
|
604
|
-
}
|
|
605
|
-
getWidth() {
|
|
606
|
-
return this.width;
|
|
607
|
-
}
|
|
608
|
-
getHeight() {
|
|
609
|
-
return this.height;
|
|
610
|
-
}
|
|
611
|
-
getbottomLeft() {
|
|
612
|
-
return this.bottomLeft;
|
|
613
|
-
}
|
|
614
|
-
}
|
|
615
|
-
|
|
616
|
-
class QuadTree {
|
|
617
|
-
MAX_OBJECTS = 10;
|
|
618
|
-
MAX_LEVELS = 5;
|
|
619
|
-
level;
|
|
620
|
-
objects = [];
|
|
621
|
-
nodes = [];
|
|
622
|
-
bounds;
|
|
623
|
-
constructor(level, bounds) {
|
|
624
|
-
this.level = level;
|
|
625
|
-
this.objects = [];
|
|
626
|
-
this.bounds = bounds;
|
|
627
|
-
this.nodes = [undefined, undefined, undefined, undefined];
|
|
628
|
-
}
|
|
629
|
-
draw(context) {
|
|
630
|
-
context.beginPath();
|
|
631
|
-
context.rect(this.bounds.getbottomLeft().x, this.bounds.getbottomLeft().y, this.bounds.getWidth(), this.bounds.getHeight());
|
|
632
|
-
context.stroke();
|
|
633
|
-
for (let index = 0;index < this.nodes.length; index++) {
|
|
634
|
-
let node = this.nodes[index];
|
|
635
|
-
if (node != null) {
|
|
636
|
-
node.draw(context);
|
|
637
|
-
}
|
|
638
|
-
}
|
|
639
|
-
}
|
|
640
|
-
clear() {
|
|
641
|
-
this.objects = [];
|
|
642
|
-
for (let index = 0;index < this.nodes.length; index++) {
|
|
643
|
-
let node = this.nodes[index];
|
|
644
|
-
if (node != null) {
|
|
645
|
-
node.clear();
|
|
646
|
-
node = undefined;
|
|
647
|
-
}
|
|
648
|
-
}
|
|
649
|
-
}
|
|
650
|
-
split() {
|
|
651
|
-
let subWidth = this.bounds.getWidth() / 2;
|
|
652
|
-
let subHeight = this.bounds.getHeight() / 2;
|
|
653
|
-
let bottomLeft = this.bounds.getbottomLeft();
|
|
654
|
-
this.nodes[0] = new QuadTree(this.level + 1, new RectangleBound({ x: bottomLeft.x, y: bottomLeft.y }, subWidth, subHeight));
|
|
655
|
-
this.nodes[1] = new QuadTree(this.level + 1, new RectangleBound({ x: bottomLeft.x, y: bottomLeft.y + subHeight }, subWidth, subHeight));
|
|
656
|
-
this.nodes[2] = new QuadTree(this.level + 1, new RectangleBound({ x: bottomLeft.x + subWidth, y: bottomLeft.y + subHeight }, subWidth, subHeight));
|
|
657
|
-
this.nodes[3] = new QuadTree(this.level + 1, new RectangleBound({ x: bottomLeft.x + subWidth, y: bottomLeft.y }, subWidth, subHeight));
|
|
658
|
-
}
|
|
659
|
-
getIndex(vBody) {
|
|
660
|
-
let midPoint = { x: this.bounds.getbottomLeft().x + this.bounds.getWidth() / 2, y: this.bounds.getbottomLeft().y + this.bounds.getHeight() / 2 };
|
|
661
|
-
let points = vBody.AABB;
|
|
662
|
-
let bottom = points.max.y < midPoint.y && points.min.y > this.bounds.getbottomLeft().y;
|
|
663
|
-
let left = points.max.x < midPoint.x && points.min.x > this.bounds.getbottomLeft().x;
|
|
664
|
-
let right = points.max.x > midPoint.x && points.min.x > midPoint.x;
|
|
665
|
-
let top = points.max.y > midPoint.y && points.min.y > midPoint.y;
|
|
666
|
-
if (bottom && left) {
|
|
667
|
-
return 0;
|
|
668
|
-
} else if (bottom && right) {
|
|
669
|
-
return 3;
|
|
670
|
-
} else if (top && left) {
|
|
671
|
-
return 1;
|
|
672
|
-
} else if (top && right) {
|
|
673
|
-
return 2;
|
|
674
|
-
}
|
|
675
|
-
return -1;
|
|
676
|
-
}
|
|
677
|
-
insert(vBody) {
|
|
678
|
-
let node = this.nodes[0];
|
|
679
|
-
if (node != null) {
|
|
680
|
-
let index = this.getIndex(vBody);
|
|
681
|
-
if (index !== -1) {
|
|
682
|
-
node = this.nodes[index];
|
|
683
|
-
node?.insert(vBody);
|
|
684
|
-
return;
|
|
685
|
-
}
|
|
686
|
-
}
|
|
687
|
-
this.objects.push(vBody);
|
|
688
|
-
if (this.objects.length > this.MAX_OBJECTS && this.level < this.MAX_LEVELS) {
|
|
689
|
-
if (this.nodes[0] == null || this.nodes[0] == undefined) {
|
|
690
|
-
this.split();
|
|
691
|
-
}
|
|
692
|
-
let i = 0;
|
|
693
|
-
while (i < this.objects.length) {
|
|
694
|
-
let index = this.getIndex(this.objects[i]);
|
|
695
|
-
let node2 = this.nodes[index];
|
|
696
|
-
if (index != -1 && node2 !== undefined) {
|
|
697
|
-
let vBody2 = this.objects[i];
|
|
698
|
-
this.objects.splice(i, 1);
|
|
699
|
-
node2.insert(vBody2);
|
|
700
|
-
} else {
|
|
701
|
-
i++;
|
|
702
|
-
}
|
|
703
|
-
}
|
|
704
|
-
}
|
|
705
|
-
}
|
|
706
|
-
retrieve(vBody) {
|
|
707
|
-
let index = this.getIndex(vBody);
|
|
708
|
-
let res = [];
|
|
709
|
-
let node = this.nodes[index];
|
|
710
|
-
if (index !== -1 && node !== undefined) {
|
|
711
|
-
res.push(...node.retrieve(vBody));
|
|
712
|
-
}
|
|
713
|
-
res.push(...this.objects);
|
|
714
|
-
return res;
|
|
715
|
-
}
|
|
716
|
-
}
|
|
717
|
-
// src/dynamic-tree.ts
|
|
718
|
-
class TreeNode {
|
|
719
|
-
parent = null;
|
|
720
|
-
children = [null, null];
|
|
721
|
-
aabb = { min: { x: 0, y: 0 }, max: { x: 0, y: 0 } };
|
|
722
|
-
object = null;
|
|
723
|
-
height = 0;
|
|
724
|
-
constructor(object) {
|
|
725
|
-
if (object) {
|
|
726
|
-
this.setLeaf(object);
|
|
727
|
-
}
|
|
728
|
-
}
|
|
729
|
-
isLeaf() {
|
|
730
|
-
return this.children[0] === null;
|
|
731
|
-
}
|
|
732
|
-
setLeaf(object) {
|
|
733
|
-
this.object = object;
|
|
734
|
-
this.updateAABB();
|
|
735
|
-
this.children = [null, null];
|
|
736
|
-
this.height = 0;
|
|
737
|
-
}
|
|
738
|
-
setBranch(child1, child2) {
|
|
739
|
-
child1.parent = this;
|
|
740
|
-
child2.parent = this;
|
|
741
|
-
this.children = [child1, child2];
|
|
742
|
-
this.object = null;
|
|
743
|
-
this.updateAABB();
|
|
744
|
-
this.height = 1 + Math.max(child1.height, child2.height);
|
|
745
|
-
}
|
|
746
|
-
updateAABB(margin = 0.1) {
|
|
747
|
-
if (this.isLeaf() && this.object) {
|
|
748
|
-
const obj = this.object.AABB;
|
|
749
|
-
this.aabb = {
|
|
750
|
-
min: { x: obj.min.x - margin, y: obj.min.y - margin },
|
|
751
|
-
max: { x: obj.max.x + margin, y: obj.max.y + margin }
|
|
752
|
-
};
|
|
753
|
-
} else if (this.children[0] && this.children[1]) {
|
|
754
|
-
const aabb1 = this.children[0].aabb;
|
|
755
|
-
const aabb2 = this.children[1].aabb;
|
|
756
|
-
this.aabb = {
|
|
757
|
-
min: {
|
|
758
|
-
x: Math.min(aabb1.min.x, aabb2.min.x),
|
|
759
|
-
y: Math.min(aabb1.min.y, aabb2.min.y)
|
|
760
|
-
},
|
|
761
|
-
max: {
|
|
762
|
-
x: Math.max(aabb1.max.x, aabb2.max.x),
|
|
763
|
-
y: Math.max(aabb1.max.y, aabb2.max.y)
|
|
764
|
-
}
|
|
765
|
-
};
|
|
766
|
-
}
|
|
767
|
-
}
|
|
768
|
-
getSibling() {
|
|
769
|
-
if (!this.parent)
|
|
770
|
-
return null;
|
|
771
|
-
return this.parent.children[0] === this ? this.parent.children[1] : this.parent.children[0];
|
|
772
|
-
}
|
|
773
|
-
getBalance() {
|
|
774
|
-
if (this.isLeaf())
|
|
775
|
-
return 0;
|
|
776
|
-
const leftHeight = this.children[0] ? this.children[0].height : 0;
|
|
777
|
-
const rightHeight = this.children[1] ? this.children[1].height : 0;
|
|
778
|
-
return leftHeight - rightHeight;
|
|
779
|
-
}
|
|
780
|
-
}
|
|
781
|
-
|
|
782
|
-
class SweepAndPrune {
|
|
783
|
-
xEndpoints = [];
|
|
784
|
-
objects = new Map;
|
|
785
|
-
nextId = 0;
|
|
786
|
-
clear() {
|
|
787
|
-
this.xEndpoints = [];
|
|
788
|
-
this.objects.clear();
|
|
789
|
-
this.nextId = 0;
|
|
790
|
-
}
|
|
791
|
-
insert(object) {
|
|
792
|
-
const id = this.nextId++;
|
|
793
|
-
const minEndpoint = { value: object.AABB.min.x, isMin: true, object, id };
|
|
794
|
-
const maxEndpoint = { value: object.AABB.max.x, isMin: false, object, id };
|
|
795
|
-
this.objects.set(object, { minEndpoint, maxEndpoint });
|
|
796
|
-
this.insertEndpointSorted(minEndpoint);
|
|
797
|
-
this.insertEndpointSorted(maxEndpoint);
|
|
798
|
-
}
|
|
799
|
-
update(object) {
|
|
800
|
-
const endpoints = this.objects.get(object);
|
|
801
|
-
if (!endpoints)
|
|
802
|
-
return;
|
|
803
|
-
const newMinX = object.AABB.min.x;
|
|
804
|
-
const newMaxX = object.AABB.max.x;
|
|
805
|
-
if (endpoints.minEndpoint.value !== newMinX) {
|
|
806
|
-
this.removeEndpoint(endpoints.minEndpoint);
|
|
807
|
-
endpoints.minEndpoint.value = newMinX;
|
|
808
|
-
this.insertEndpointSorted(endpoints.minEndpoint);
|
|
809
|
-
}
|
|
810
|
-
if (endpoints.maxEndpoint.value !== newMaxX) {
|
|
811
|
-
this.removeEndpoint(endpoints.maxEndpoint);
|
|
812
|
-
endpoints.maxEndpoint.value = newMaxX;
|
|
813
|
-
this.insertEndpointSorted(endpoints.maxEndpoint);
|
|
814
|
-
}
|
|
815
|
-
}
|
|
816
|
-
remove(object) {
|
|
817
|
-
const endpoints = this.objects.get(object);
|
|
818
|
-
if (!endpoints)
|
|
819
|
-
return;
|
|
820
|
-
this.removeEndpoint(endpoints.minEndpoint);
|
|
821
|
-
this.removeEndpoint(endpoints.maxEndpoint);
|
|
822
|
-
this.objects.delete(object);
|
|
823
|
-
}
|
|
824
|
-
retrieve(queryObject) {
|
|
825
|
-
const result = [];
|
|
826
|
-
const queryMin = queryObject.AABB.min.x;
|
|
827
|
-
const queryMax = queryObject.AABB.max.x;
|
|
828
|
-
for (const endpoint of this.xEndpoints) {
|
|
829
|
-
if (endpoint.value > queryMax)
|
|
830
|
-
break;
|
|
831
|
-
if (endpoint.isMin && endpoint.object !== queryObject) {
|
|
832
|
-
const objMax = this.objects.get(endpoint.object)?.maxEndpoint.value;
|
|
833
|
-
if (objMax !== undefined && objMax >= queryMin) {
|
|
834
|
-
if (this.aabbIntersects(endpoint.object.AABB, queryObject.AABB)) {
|
|
835
|
-
result.push(endpoint.object);
|
|
836
|
-
}
|
|
837
|
-
}
|
|
838
|
-
}
|
|
839
|
-
}
|
|
840
|
-
return result;
|
|
841
|
-
}
|
|
842
|
-
findAllOverlaps() {
|
|
843
|
-
const pairs = [];
|
|
844
|
-
const activeObjects = new Set;
|
|
845
|
-
for (const endpoint of this.xEndpoints) {
|
|
846
|
-
if (endpoint.isMin) {
|
|
847
|
-
for (const activeObj of activeObjects) {
|
|
848
|
-
if (this.aabbIntersects(endpoint.object.AABB, activeObj.AABB)) {
|
|
849
|
-
pairs.push({ a: endpoint.object, b: activeObj });
|
|
850
|
-
}
|
|
851
|
-
}
|
|
852
|
-
activeObjects.add(endpoint.object);
|
|
853
|
-
} else {
|
|
854
|
-
activeObjects.delete(endpoint.object);
|
|
855
|
-
}
|
|
856
|
-
}
|
|
857
|
-
return pairs;
|
|
858
|
-
}
|
|
859
|
-
insertEndpointSorted(endpoint) {
|
|
860
|
-
let left = 0;
|
|
861
|
-
let right = this.xEndpoints.length;
|
|
862
|
-
while (left < right) {
|
|
863
|
-
const mid = Math.floor((left + right) / 2);
|
|
864
|
-
const midEndpoint = this.xEndpoints[mid];
|
|
865
|
-
if (midEndpoint.value < endpoint.value || midEndpoint.value === endpoint.value && midEndpoint.isMin && !endpoint.isMin) {
|
|
866
|
-
left = mid + 1;
|
|
867
|
-
} else {
|
|
868
|
-
right = mid;
|
|
869
|
-
}
|
|
870
|
-
}
|
|
871
|
-
this.xEndpoints.splice(left, 0, endpoint);
|
|
872
|
-
}
|
|
873
|
-
removeEndpoint(endpoint) {
|
|
874
|
-
const index = this.xEndpoints.indexOf(endpoint);
|
|
875
|
-
if (index !== -1) {
|
|
876
|
-
this.xEndpoints.splice(index, 1);
|
|
877
|
-
}
|
|
878
|
-
}
|
|
879
|
-
aabbIntersects(aabb1, aabb2) {
|
|
880
|
-
return !(aabb1.max.x < aabb2.min.x || aabb1.min.x > aabb2.max.x || aabb1.max.y < aabb2.min.y || aabb1.min.y > aabb2.max.y);
|
|
881
|
-
}
|
|
882
|
-
draw(context) {
|
|
883
|
-
context.strokeStyle = "orange";
|
|
884
|
-
context.lineWidth = 1;
|
|
885
|
-
context.globalAlpha = 0.3;
|
|
886
|
-
for (const endpoint of this.xEndpoints) {
|
|
887
|
-
context.beginPath();
|
|
888
|
-
context.moveTo(endpoint.value, -1000);
|
|
889
|
-
context.lineTo(endpoint.value, 1000);
|
|
890
|
-
context.stroke();
|
|
891
|
-
context.fillStyle = endpoint.isMin ? "green" : "red";
|
|
892
|
-
context.fillText(endpoint.isMin ? "min" : "max", endpoint.value, -950);
|
|
893
|
-
}
|
|
894
|
-
context.globalAlpha = 1;
|
|
895
|
-
}
|
|
896
|
-
getStats() {
|
|
897
|
-
return {
|
|
898
|
-
endpointCount: this.xEndpoints.length,
|
|
899
|
-
objectCount: this.objects.size
|
|
900
|
-
};
|
|
901
|
-
}
|
|
902
|
-
}
|
|
903
|
-
|
|
904
|
-
class DynamicTree {
|
|
905
|
-
root = null;
|
|
906
|
-
nodeCount = 0;
|
|
907
|
-
margin = 0.1;
|
|
908
|
-
clear() {
|
|
909
|
-
this.root = null;
|
|
910
|
-
this.nodeCount = 0;
|
|
911
|
-
}
|
|
912
|
-
insert(object) {
|
|
913
|
-
const node = new TreeNode(object);
|
|
914
|
-
this.insertNode(node);
|
|
915
|
-
}
|
|
916
|
-
insertNode(node) {
|
|
917
|
-
this.nodeCount++;
|
|
918
|
-
if (!this.root) {
|
|
919
|
-
this.root = node;
|
|
920
|
-
return;
|
|
921
|
-
}
|
|
922
|
-
let sibling = this.findBestSibling(node);
|
|
923
|
-
const oldParent = sibling.parent;
|
|
924
|
-
const newParent = new TreeNode;
|
|
925
|
-
newParent.parent = oldParent;
|
|
926
|
-
newParent.setBranch(sibling, node);
|
|
927
|
-
if (oldParent) {
|
|
928
|
-
if (oldParent.children[0] === sibling) {
|
|
929
|
-
oldParent.children[0] = newParent;
|
|
930
|
-
} else {
|
|
931
|
-
oldParent.children[1] = newParent;
|
|
932
|
-
}
|
|
933
|
-
} else {
|
|
934
|
-
this.root = newParent;
|
|
935
|
-
}
|
|
936
|
-
let ancestor = newParent.parent;
|
|
937
|
-
while (ancestor) {
|
|
938
|
-
ancestor.updateAABB(this.margin);
|
|
939
|
-
ancestor.height = 1 + Math.max(ancestor.children[0] ? ancestor.children[0].height : 0, ancestor.children[1] ? ancestor.children[1].height : 0);
|
|
940
|
-
ancestor = this.balance(ancestor);
|
|
941
|
-
ancestor = ancestor.parent;
|
|
942
|
-
}
|
|
943
|
-
}
|
|
944
|
-
findBestSibling(node) {
|
|
945
|
-
let current = this.root;
|
|
946
|
-
while (!current.isLeaf()) {
|
|
947
|
-
const child1 = current.children[0];
|
|
948
|
-
const child2 = current.children[1];
|
|
949
|
-
const area = this.getArea(current.aabb);
|
|
950
|
-
const combinedAABB = this.combineAABB(current.aabb, node.aabb);
|
|
951
|
-
const combinedArea = this.getArea(combinedAABB);
|
|
952
|
-
const cost = 2 * combinedArea;
|
|
953
|
-
const inheritanceCost = 2 * (combinedArea - area);
|
|
954
|
-
let cost1;
|
|
955
|
-
const aabb1 = this.combineAABB(node.aabb, child1.aabb);
|
|
956
|
-
if (child1.isLeaf()) {
|
|
957
|
-
cost1 = this.getArea(aabb1) + inheritanceCost;
|
|
958
|
-
} else {
|
|
959
|
-
const oldArea = this.getArea(child1.aabb);
|
|
960
|
-
const newArea = this.getArea(aabb1);
|
|
961
|
-
cost1 = newArea - oldArea + inheritanceCost;
|
|
962
|
-
}
|
|
963
|
-
let cost2;
|
|
964
|
-
const aabb2 = this.combineAABB(node.aabb, child2.aabb);
|
|
965
|
-
if (child2.isLeaf()) {
|
|
966
|
-
cost2 = this.getArea(aabb2) + inheritanceCost;
|
|
967
|
-
} else {
|
|
968
|
-
const oldArea = this.getArea(child2.aabb);
|
|
969
|
-
const newArea = this.getArea(aabb2);
|
|
970
|
-
cost2 = newArea - oldArea + inheritanceCost;
|
|
971
|
-
}
|
|
972
|
-
if (cost < cost1 && cost < cost2) {
|
|
973
|
-
break;
|
|
974
|
-
}
|
|
975
|
-
current = cost1 < cost2 ? child1 : child2;
|
|
976
|
-
}
|
|
977
|
-
return current;
|
|
978
|
-
}
|
|
979
|
-
balance(node) {
|
|
980
|
-
if (node.isLeaf()) {
|
|
981
|
-
return node;
|
|
982
|
-
}
|
|
983
|
-
const balance = node.getBalance();
|
|
984
|
-
if (balance < -1) {
|
|
985
|
-
return this.rotateLeft(node);
|
|
986
|
-
} else if (balance > 1) {
|
|
987
|
-
return this.rotateRight(node);
|
|
988
|
-
}
|
|
989
|
-
return node;
|
|
990
|
-
}
|
|
991
|
-
rotateLeft(node) {
|
|
992
|
-
const child2 = node.children[1];
|
|
993
|
-
const child2Child1 = child2.children[0];
|
|
994
|
-
const child2Child2 = child2.children[1];
|
|
995
|
-
child2.children[0] = node;
|
|
996
|
-
child2.parent = node.parent;
|
|
997
|
-
node.parent = child2;
|
|
998
|
-
if (child2.parent) {
|
|
999
|
-
if (child2.parent.children[0] === node) {
|
|
1000
|
-
child2.parent.children[0] = child2;
|
|
1001
|
-
} else {
|
|
1002
|
-
child2.parent.children[1] = child2;
|
|
1003
|
-
}
|
|
1004
|
-
} else {
|
|
1005
|
-
this.root = child2;
|
|
1006
|
-
}
|
|
1007
|
-
if (child2Child1.height > child2Child2.height) {
|
|
1008
|
-
child2.children[1] = child2Child1;
|
|
1009
|
-
node.children[1] = child2Child2;
|
|
1010
|
-
child2Child2.parent = node;
|
|
1011
|
-
node.updateAABB(this.margin);
|
|
1012
|
-
child2.updateAABB(this.margin);
|
|
1013
|
-
node.height = 1 + Math.max(node.children[0].height, node.children[1].height);
|
|
1014
|
-
child2.height = 1 + Math.max(node.height, child2Child1.height);
|
|
1015
|
-
} else {
|
|
1016
|
-
child2.children[1] = child2Child2;
|
|
1017
|
-
node.children[1] = child2Child1;
|
|
1018
|
-
child2Child1.parent = node;
|
|
1019
|
-
node.updateAABB(this.margin);
|
|
1020
|
-
child2.updateAABB(this.margin);
|
|
1021
|
-
node.height = 1 + Math.max(node.children[0].height, node.children[1].height);
|
|
1022
|
-
child2.height = 1 + Math.max(node.height, child2Child2.height);
|
|
1023
|
-
}
|
|
1024
|
-
return child2;
|
|
1025
|
-
}
|
|
1026
|
-
rotateRight(node) {
|
|
1027
|
-
const child1 = node.children[0];
|
|
1028
|
-
const child1Child1 = child1.children[0];
|
|
1029
|
-
const child1Child2 = child1.children[1];
|
|
1030
|
-
child1.children[1] = node;
|
|
1031
|
-
child1.parent = node.parent;
|
|
1032
|
-
node.parent = child1;
|
|
1033
|
-
if (child1.parent) {
|
|
1034
|
-
if (child1.parent.children[0] === node) {
|
|
1035
|
-
child1.parent.children[0] = child1;
|
|
1036
|
-
} else {
|
|
1037
|
-
child1.parent.children[1] = child1;
|
|
1038
|
-
}
|
|
1039
|
-
} else {
|
|
1040
|
-
this.root = child1;
|
|
1041
|
-
}
|
|
1042
|
-
if (child1Child1.height > child1Child2.height) {
|
|
1043
|
-
child1.children[0] = child1Child1;
|
|
1044
|
-
node.children[0] = child1Child2;
|
|
1045
|
-
child1Child2.parent = node;
|
|
1046
|
-
node.updateAABB(this.margin);
|
|
1047
|
-
child1.updateAABB(this.margin);
|
|
1048
|
-
node.height = 1 + Math.max(node.children[0].height, node.children[1].height);
|
|
1049
|
-
child1.height = 1 + Math.max(child1Child1.height, node.height);
|
|
1050
|
-
} else {
|
|
1051
|
-
child1.children[0] = child1Child2;
|
|
1052
|
-
node.children[0] = child1Child1;
|
|
1053
|
-
child1Child1.parent = node;
|
|
1054
|
-
node.updateAABB(this.margin);
|
|
1055
|
-
child1.updateAABB(this.margin);
|
|
1056
|
-
node.height = 1 + Math.max(node.children[0].height, node.children[1].height);
|
|
1057
|
-
child1.height = 1 + Math.max(child1Child2.height, node.height);
|
|
1058
|
-
}
|
|
1059
|
-
return child1;
|
|
1060
|
-
}
|
|
1061
|
-
retrieve(object) {
|
|
1062
|
-
const result = [];
|
|
1063
|
-
if (!this.root)
|
|
1064
|
-
return result;
|
|
1065
|
-
const stack = [this.root];
|
|
1066
|
-
while (stack.length > 0) {
|
|
1067
|
-
const node = stack.pop();
|
|
1068
|
-
if (this.aabbIntersects(node.aabb, object.AABB)) {
|
|
1069
|
-
if (node.isLeaf()) {
|
|
1070
|
-
if (node.object && node.object !== object) {
|
|
1071
|
-
result.push(node.object);
|
|
1072
|
-
}
|
|
1073
|
-
} else {
|
|
1074
|
-
if (node.children[0])
|
|
1075
|
-
stack.push(node.children[0]);
|
|
1076
|
-
if (node.children[1])
|
|
1077
|
-
stack.push(node.children[1]);
|
|
1078
|
-
}
|
|
1079
|
-
}
|
|
1080
|
-
}
|
|
1081
|
-
return result;
|
|
1082
|
-
}
|
|
1083
|
-
draw(context) {
|
|
1084
|
-
if (!this.root)
|
|
1085
|
-
return;
|
|
1086
|
-
this.drawNode(context, this.root, 0);
|
|
1087
|
-
}
|
|
1088
|
-
drawNode(context, node, depth) {
|
|
1089
|
-
const { min, max } = node.aabb;
|
|
1090
|
-
const colors = ["red", "blue", "green", "orange", "purple", "brown"];
|
|
1091
|
-
context.strokeStyle = colors[depth % colors.length];
|
|
1092
|
-
context.lineWidth = Math.max(1, 3 - depth);
|
|
1093
|
-
context.beginPath();
|
|
1094
|
-
context.rect(min.x, min.y, max.x - min.x, max.y - min.y);
|
|
1095
|
-
context.stroke();
|
|
1096
|
-
if (!node.isLeaf()) {
|
|
1097
|
-
if (node.children[0])
|
|
1098
|
-
this.drawNode(context, node.children[0], depth + 1);
|
|
1099
|
-
if (node.children[1])
|
|
1100
|
-
this.drawNode(context, node.children[1], depth + 1);
|
|
1101
|
-
}
|
|
1102
|
-
}
|
|
1103
|
-
getArea(aabb) {
|
|
1104
|
-
return (aabb.max.x - aabb.min.x) * (aabb.max.y - aabb.min.y);
|
|
1105
|
-
}
|
|
1106
|
-
combineAABB(aabb1, aabb2) {
|
|
1107
|
-
return {
|
|
1108
|
-
min: {
|
|
1109
|
-
x: Math.min(aabb1.min.x, aabb2.min.x),
|
|
1110
|
-
y: Math.min(aabb1.min.y, aabb2.min.y)
|
|
1111
|
-
},
|
|
1112
|
-
max: {
|
|
1113
|
-
x: Math.max(aabb1.max.x, aabb2.max.x),
|
|
1114
|
-
y: Math.max(aabb1.max.y, aabb2.max.y)
|
|
1115
|
-
}
|
|
1116
|
-
};
|
|
1117
|
-
}
|
|
1118
|
-
aabbIntersects(aabb1, aabb2) {
|
|
1119
|
-
return !(aabb1.max.x < aabb2.min.x || aabb1.min.x > aabb2.max.x || aabb1.max.y < aabb2.min.y || aabb1.min.y > aabb2.max.y);
|
|
1120
|
-
}
|
|
1121
|
-
getStats() {
|
|
1122
|
-
return {
|
|
1123
|
-
nodeCount: this.nodeCount,
|
|
1124
|
-
height: this.root ? this.root.height : 0
|
|
1125
|
-
};
|
|
1126
|
-
}
|
|
1127
|
-
}
|
|
1128
|
-
// src/collision.ts
|
|
1129
|
-
import { PointCal as PointCal2 } from "@ue-too/math";
|
|
1130
|
-
function resolveCollision(bodyA, bodyB, normal) {
|
|
1131
|
-
if (bodyA.isStatic() && bodyB.isStatic()) {
|
|
1132
|
-
return;
|
|
1133
|
-
}
|
|
1134
|
-
let restitution = 0.4;
|
|
1135
|
-
let inverseMassA = bodyA.isStatic() || bodyA.isMovingStatic() ? 0 : 1 / bodyA.mass;
|
|
1136
|
-
let inverseMassB = bodyB.isStatic() || bodyB.isMovingStatic() ? 0 : 1 / bodyB.mass;
|
|
1137
|
-
let relativeVelocity = PointCal2.subVector(bodyA.linearVelocity, bodyB.linearVelocity);
|
|
1138
|
-
let J = -(1 + restitution) * PointCal2.dotProduct(relativeVelocity, normal);
|
|
1139
|
-
J /= inverseMassA + inverseMassB;
|
|
1140
|
-
let deltaVelocityA = PointCal2.multiplyVectorByScalar(normal, J * inverseMassA);
|
|
1141
|
-
let deltaVelocityB = PointCal2.multiplyVectorByScalar(normal, J * inverseMassB);
|
|
1142
|
-
bodyA.linearVelocity = PointCal2.addVector(bodyA.linearVelocity, deltaVelocityA);
|
|
1143
|
-
bodyB.linearVelocity = PointCal2.subVector(bodyB.linearVelocity, deltaVelocityB);
|
|
1144
|
-
}
|
|
1145
|
-
function resolveCollisionWithRotation(bodyA, bodyB, contactManifold) {
|
|
1146
|
-
if (bodyA.isStatic() && bodyB.isStatic()) {
|
|
1147
|
-
return;
|
|
1148
|
-
}
|
|
1149
|
-
let restitution = 0.4;
|
|
1150
|
-
let inverseMassA = bodyA.isStatic() || bodyA.isMovingStatic() ? 0 : 1 / bodyA.mass;
|
|
1151
|
-
let inverseMassB = bodyB.isStatic() || bodyB.isMovingStatic() ? 0 : 1 / bodyB.mass;
|
|
1152
|
-
let inverseMMOIA = bodyA.isStatic() || bodyA.isMovingStatic() ? 0 : 1 / bodyA.momentOfInertia;
|
|
1153
|
-
let inverseMMOIB = bodyB.isStatic() || bodyB.isMovingStatic() ? 0 : 1 / bodyB.momentOfInertia;
|
|
1154
|
-
const Js = [];
|
|
1155
|
-
for (let index = 0;index < contactManifold.contactPoints.length; index++) {
|
|
1156
|
-
const contactPoint = contactManifold.contactPoints[index];
|
|
1157
|
-
const rA = PointCal2.subVector(contactPoint, bodyA.center);
|
|
1158
|
-
const rB = PointCal2.subVector(contactPoint, bodyB.center);
|
|
1159
|
-
const rAPerpendicular = { x: -rA.y, y: rA.x };
|
|
1160
|
-
const rBPerpendicular = { x: -rB.y, y: rB.x };
|
|
1161
|
-
const angularVelocityA = PointCal2.multiplyVectorByScalar(rAPerpendicular, bodyA.angularVelocity);
|
|
1162
|
-
const angularVelocityB = PointCal2.multiplyVectorByScalar(rBPerpendicular, bodyB.angularVelocity);
|
|
1163
|
-
let relativeVelocity = PointCal2.subVector(PointCal2.addVector(bodyA.linearVelocity, angularVelocityA), PointCal2.addVector(bodyB.linearVelocity, angularVelocityB));
|
|
1164
|
-
let relativeVelocityNormal = PointCal2.dotProduct(relativeVelocity, contactManifold.normal);
|
|
1165
|
-
const rAPerpendicularNormal = PointCal2.dotProduct(rAPerpendicular, contactManifold.normal);
|
|
1166
|
-
const rBPerpendicularNormal = PointCal2.dotProduct(rBPerpendicular, contactManifold.normal);
|
|
1167
|
-
const denominator = inverseMassA + inverseMassB + rAPerpendicularNormal * rAPerpendicularNormal * inverseMMOIA + rBPerpendicularNormal * rBPerpendicularNormal * inverseMMOIB;
|
|
1168
|
-
let J = -(1 + restitution) * relativeVelocityNormal;
|
|
1169
|
-
J /= denominator;
|
|
1170
|
-
J /= contactManifold.contactPoints.length;
|
|
1171
|
-
Js.push(PointCal2.multiplyVectorByScalar(contactManifold.normal, J));
|
|
1172
|
-
}
|
|
1173
|
-
Js.forEach((impulse, index) => {
|
|
1174
|
-
let deltaVelocityA = PointCal2.multiplyVectorByScalar(impulse, inverseMassA);
|
|
1175
|
-
let deltaVelocityB = PointCal2.multiplyVectorByScalar(impulse, inverseMassB);
|
|
1176
|
-
bodyA.linearVelocity = PointCal2.addVector(bodyA.linearVelocity, deltaVelocityA);
|
|
1177
|
-
let resA = PointCal2.crossProduct(PointCal2.subVector(contactManifold.contactPoints[index], bodyA.center), impulse).z;
|
|
1178
|
-
resA = resA == undefined ? 0 : resA;
|
|
1179
|
-
let resB = PointCal2.crossProduct(PointCal2.subVector(contactManifold.contactPoints[index], bodyB.center), impulse).z;
|
|
1180
|
-
resB = resB == undefined ? 0 : resB;
|
|
1181
|
-
bodyA.angularVelocity += resA * inverseMMOIA;
|
|
1182
|
-
bodyB.angularVelocity -= resB * inverseMMOIB;
|
|
1183
|
-
bodyB.linearVelocity = PointCal2.subVector(bodyB.linearVelocity, deltaVelocityB);
|
|
1184
|
-
});
|
|
1185
|
-
}
|
|
1186
|
-
function aabbIntersects(aabbA, aabbB) {
|
|
1187
|
-
if (aabbA.min.x <= aabbB.max.x && aabbB.min.x <= aabbA.max.x && (aabbA.min.y <= aabbB.max.y && aabbB.min.y <= aabbA.max.y)) {
|
|
1188
|
-
return true;
|
|
1189
|
-
}
|
|
1190
|
-
return false;
|
|
1191
|
-
}
|
|
1192
|
-
function intersects(bodyA, bodyB) {
|
|
1193
|
-
let axis = [];
|
|
1194
|
-
let bodyAAxes = bodyA.getCollisionAxes(bodyB);
|
|
1195
|
-
let bodyBAxes = bodyB.getCollisionAxes(bodyA);
|
|
1196
|
-
axis.push(...bodyAAxes);
|
|
1197
|
-
axis.push(...bodyBAxes);
|
|
1198
|
-
let collision = true;
|
|
1199
|
-
let minDepth = Number.MAX_VALUE;
|
|
1200
|
-
let minAxis = axis[0];
|
|
1201
|
-
axis.forEach((projAxis) => {
|
|
1202
|
-
let bodyAInterval = bodyA.getMinMaxProjection(projAxis);
|
|
1203
|
-
let bodyBInterval = bodyB.getMinMaxProjection(projAxis);
|
|
1204
|
-
if (bodyAInterval.min >= bodyBInterval.max || bodyBInterval.min >= bodyAInterval.max) {
|
|
1205
|
-
collision = false;
|
|
1206
|
-
} else {
|
|
1207
|
-
let depth = Math.abs(Math.min(bodyAInterval.max, bodyBInterval.max) - Math.max(bodyBInterval.min, bodyAInterval.min));
|
|
1208
|
-
if (depth < minDepth) {
|
|
1209
|
-
minDepth = depth;
|
|
1210
|
-
minAxis = projAxis;
|
|
1211
|
-
if (bodyAInterval.max < bodyBInterval.max) {
|
|
1212
|
-
minAxis = PointCal2.multiplyVectorByScalar(minAxis, -1);
|
|
1213
|
-
}
|
|
1214
|
-
}
|
|
1215
|
-
}
|
|
1216
|
-
});
|
|
1217
|
-
if (collision) {
|
|
1218
|
-
return { collision, depth: minDepth, normal: minAxis };
|
|
1219
|
-
} else {
|
|
1220
|
-
return { collision: false, depth: undefined, normal: undefined };
|
|
1221
|
-
}
|
|
1222
|
-
}
|
|
1223
|
-
function narrowPhaseWithRigidBody(bodies, combinationsToCheck, resolveCollisionFlag) {
|
|
1224
|
-
if (!resolveCollisionFlag) {
|
|
1225
|
-
return [];
|
|
1226
|
-
}
|
|
1227
|
-
const contactPoints = [];
|
|
1228
|
-
combinationsToCheck.forEach((combination) => {
|
|
1229
|
-
let bodyA = combination.bodyA;
|
|
1230
|
-
let bodyB = combination.bodyB;
|
|
1231
|
-
if (bodyA == bodyB) {
|
|
1232
|
-
return;
|
|
1233
|
-
}
|
|
1234
|
-
let bodyAZ = bodyA.center.z == undefined ? 0 : bodyA.center.z;
|
|
1235
|
-
let bodyBZ = bodyB.center.z == undefined ? 0 : bodyB.center.z;
|
|
1236
|
-
if (Math.abs(bodyAZ - bodyBZ) > 0.5) {
|
|
1237
|
-
return;
|
|
1238
|
-
}
|
|
1239
|
-
let { collision, depth, normal: normalAxis } = intersects(bodyA, bodyB);
|
|
1240
|
-
if (collision && normalAxis !== undefined && depth !== undefined) {
|
|
1241
|
-
let moveDisplacement = PointCal2.multiplyVectorByScalar(normalAxis, depth / 2);
|
|
1242
|
-
let revMoveDisplacement = PointCal2.multiplyVectorByScalar(normalAxis, -depth / 2);
|
|
1243
|
-
if (!bodyA.isStatic()) {
|
|
1244
|
-
bodyA.move(moveDisplacement);
|
|
1245
|
-
}
|
|
1246
|
-
if (!bodyB.isStatic()) {
|
|
1247
|
-
bodyB.move(revMoveDisplacement);
|
|
1248
|
-
}
|
|
1249
|
-
if (bodyA.isStatic()) {
|
|
1250
|
-
bodyB.move(revMoveDisplacement);
|
|
1251
|
-
}
|
|
1252
|
-
if (bodyB.isStatic()) {
|
|
1253
|
-
bodyA.move(moveDisplacement);
|
|
1254
|
-
}
|
|
1255
|
-
const bodyASigNormal = bodyA.getNormalOfSignificantFace(PointCal2.multiplyVectorByScalar(normalAxis, -1));
|
|
1256
|
-
const bodyBSigNormal = bodyB.getNormalOfSignificantFace(normalAxis);
|
|
1257
|
-
const bodyASigVertices = bodyA.getSignificantVertices(PointCal2.multiplyVectorByScalar(normalAxis, -1));
|
|
1258
|
-
const bodyBSigVertices = bodyB.getSignificantVertices(normalAxis);
|
|
1259
|
-
const bodyAParallelIndicator = Math.abs(PointCal2.dotProduct(bodyASigNormal, PointCal2.multiplyVectorByScalar(normalAxis, -1)));
|
|
1260
|
-
const bodyBParallelIndicator = Math.abs(PointCal2.dotProduct(bodyBSigNormal, normalAxis));
|
|
1261
|
-
if (bodyBSigVertices.length == 1 || bodyASigVertices.length == 1) {
|
|
1262
|
-
if (bodyBSigVertices.length == 1) {
|
|
1263
|
-
contactPoints.push(bodyBSigVertices[0]);
|
|
1264
|
-
} else {
|
|
1265
|
-
contactPoints.push(bodyASigVertices[0]);
|
|
1266
|
-
}
|
|
1267
|
-
} else if (bodyAParallelIndicator > bodyBParallelIndicator) {
|
|
1268
|
-
const adjacentFaces = bodyA.getAdjacentFaces(PointCal2.multiplyVectorByScalar(normalAxis, -1));
|
|
1269
|
-
let faceToClip = [...bodyBSigVertices];
|
|
1270
|
-
for (let index = 0;index < adjacentFaces.length - 1; index++) {
|
|
1271
|
-
let startPoint2 = adjacentFaces[index].startPoint.coord;
|
|
1272
|
-
let endPoint2 = adjacentFaces[index].endPoint.coord;
|
|
1273
|
-
let direction2 = PointCal2.subVector(endPoint2, startPoint2);
|
|
1274
|
-
let sigStart2 = PointCal2.subVector(faceToClip[0], startPoint2);
|
|
1275
|
-
let sigEnd2 = PointCal2.subVector(faceToClip[1], startPoint2);
|
|
1276
|
-
let startInside2 = PointCal2.angleFromA2B(direction2, sigStart2) >= 0;
|
|
1277
|
-
let endInside2 = PointCal2.angleFromA2B(direction2, sigEnd2) >= 0;
|
|
1278
|
-
if ((startInside2 ? 1 : 0) ^ (endInside2 ? 1 : 0)) {
|
|
1279
|
-
let intersectionPoint = PointCal2.getLineIntersection(startPoint2, endPoint2, faceToClip[0], faceToClip[1]);
|
|
1280
|
-
if (intersectionPoint.intersects && intersectionPoint.intersection !== undefined) {
|
|
1281
|
-
if (startInside2) {
|
|
1282
|
-
faceToClip[1] = intersectionPoint.intersection;
|
|
1283
|
-
} else {
|
|
1284
|
-
faceToClip[0] = intersectionPoint.intersection;
|
|
1285
|
-
}
|
|
1286
|
-
}
|
|
1287
|
-
}
|
|
1288
|
-
}
|
|
1289
|
-
const referenceFace = adjacentFaces[adjacentFaces.length - 1];
|
|
1290
|
-
let startPoint = referenceFace.startPoint.coord;
|
|
1291
|
-
let endPoint = referenceFace.endPoint.coord;
|
|
1292
|
-
let direction = PointCal2.subVector(endPoint, startPoint);
|
|
1293
|
-
let sigStart = PointCal2.subVector(faceToClip[0], startPoint);
|
|
1294
|
-
let sigEnd = PointCal2.subVector(faceToClip[1], startPoint);
|
|
1295
|
-
let startInside = PointCal2.angleFromA2B(direction, sigStart) >= 0;
|
|
1296
|
-
let endInside = PointCal2.angleFromA2B(direction, sigEnd) >= 0;
|
|
1297
|
-
if (startInside) {
|
|
1298
|
-
contactPoints.push(faceToClip[0]);
|
|
1299
|
-
}
|
|
1300
|
-
if (endInside) {
|
|
1301
|
-
contactPoints.push(faceToClip[1]);
|
|
1302
|
-
}
|
|
1303
|
-
} else {
|
|
1304
|
-
const adjacentFaces = bodyB.getAdjacentFaces(normalAxis);
|
|
1305
|
-
let faceToClip = [...bodyASigVertices];
|
|
1306
|
-
if (faceToClip.length == 0) {
|
|
1307
|
-
console.log("warning");
|
|
1308
|
-
}
|
|
1309
|
-
let count = 0;
|
|
1310
|
-
for (let index = 0;index < adjacentFaces.length - 1; index++) {
|
|
1311
|
-
let startPoint2 = adjacentFaces[index].startPoint.coord;
|
|
1312
|
-
let endPoint2 = adjacentFaces[index].endPoint.coord;
|
|
1313
|
-
let direction2 = PointCal2.subVector(endPoint2, startPoint2);
|
|
1314
|
-
let sigStart2 = PointCal2.subVector(faceToClip[0], startPoint2);
|
|
1315
|
-
let sigEnd2 = PointCal2.subVector(faceToClip[1], startPoint2);
|
|
1316
|
-
let startInside2 = PointCal2.angleFromA2B(direction2, sigStart2) >= 0;
|
|
1317
|
-
let endInside2 = PointCal2.angleFromA2B(direction2, sigEnd2) >= 0;
|
|
1318
|
-
if ((startInside2 ? 1 : 0) ^ (endInside2 ? 1 : 0)) {
|
|
1319
|
-
count += 1;
|
|
1320
|
-
let intersectionPoint = PointCal2.getLineIntersection(startPoint2, endPoint2, faceToClip[0], faceToClip[1]);
|
|
1321
|
-
if (intersectionPoint.intersects && intersectionPoint.intersection !== undefined) {
|
|
1322
|
-
if (startInside2) {
|
|
1323
|
-
faceToClip[1] = intersectionPoint.intersection;
|
|
1324
|
-
} else {
|
|
1325
|
-
faceToClip[0] = intersectionPoint.intersection;
|
|
1326
|
-
}
|
|
1327
|
-
}
|
|
1328
|
-
}
|
|
1329
|
-
}
|
|
1330
|
-
const referenceFace = adjacentFaces[adjacentFaces.length - 1];
|
|
1331
|
-
let startPoint = referenceFace.startPoint.coord;
|
|
1332
|
-
let endPoint = referenceFace.endPoint.coord;
|
|
1333
|
-
let direction = PointCal2.subVector(endPoint, startPoint);
|
|
1334
|
-
let sigStart = PointCal2.subVector(faceToClip[0], startPoint);
|
|
1335
|
-
let sigEnd = PointCal2.subVector(faceToClip[1], startPoint);
|
|
1336
|
-
let startInside = PointCal2.angleFromA2B(direction, sigStart) >= 0;
|
|
1337
|
-
let endInside = PointCal2.angleFromA2B(direction, sigEnd) >= 0;
|
|
1338
|
-
if (startInside) {
|
|
1339
|
-
contactPoints.push(faceToClip[0]);
|
|
1340
|
-
}
|
|
1341
|
-
if (endInside) {
|
|
1342
|
-
contactPoints.push(faceToClip[1]);
|
|
1343
|
-
}
|
|
1344
|
-
}
|
|
1345
|
-
if (resolveCollisionFlag) {
|
|
1346
|
-
resolveCollisionWithRotation(bodyA, bodyB, { normal: normalAxis, contactPoints });
|
|
1347
|
-
}
|
|
1348
|
-
}
|
|
1349
|
-
});
|
|
1350
|
-
return contactPoints;
|
|
1351
|
-
}
|
|
1352
|
-
function narrowPhase(bodies, combinationsToCheck, resolveCollisionFlag) {
|
|
1353
|
-
if (!resolveCollisionFlag) {
|
|
1354
|
-
return;
|
|
1355
|
-
}
|
|
1356
|
-
combinationsToCheck.forEach((combination) => {
|
|
1357
|
-
let bodyA = bodies[combination.bodyAIndex];
|
|
1358
|
-
let bodyB = bodies[combination.bodyBIndex];
|
|
1359
|
-
let { collision, depth, normal: normalAxis } = intersects(bodyA, bodyB);
|
|
1360
|
-
if (collision && normalAxis !== undefined && depth !== undefined) {
|
|
1361
|
-
let moveDisplacement = PointCal2.multiplyVectorByScalar(normalAxis, depth / 2);
|
|
1362
|
-
let revMoveDisplacement = PointCal2.multiplyVectorByScalar(normalAxis, -depth / 2);
|
|
1363
|
-
if (!bodyA.isStatic()) {
|
|
1364
|
-
bodyA.move(moveDisplacement);
|
|
1365
|
-
}
|
|
1366
|
-
if (!bodyB.isStatic()) {
|
|
1367
|
-
bodyB.move(revMoveDisplacement);
|
|
1368
|
-
}
|
|
1369
|
-
if (bodyA.isStatic()) {
|
|
1370
|
-
bodyB.move(revMoveDisplacement);
|
|
1371
|
-
}
|
|
1372
|
-
if (bodyB.isStatic()) {
|
|
1373
|
-
bodyA.move(moveDisplacement);
|
|
1374
|
-
}
|
|
1375
|
-
if (resolveCollisionFlag) {
|
|
1376
|
-
resolveCollision(bodyA, bodyB, normalAxis);
|
|
1377
|
-
}
|
|
1378
|
-
}
|
|
1379
|
-
});
|
|
1380
|
-
}
|
|
1381
|
-
function broadPhaseWithRigidBodyReturned(quadTree, bodies) {
|
|
1382
|
-
let possibleCombi = [];
|
|
1383
|
-
for (let index = 0;index <= bodies.length - 1; index++) {
|
|
1384
|
-
let objsToCheck = quadTree.retrieve(bodies[index]);
|
|
1385
|
-
for (let jindex = 0;jindex <= objsToCheck.length - 1; jindex++) {
|
|
1386
|
-
let bodyA = bodies[index];
|
|
1387
|
-
let bodyB = objsToCheck[jindex];
|
|
1388
|
-
if (bodyA.isStatic() && bodyB.isStatic()) {
|
|
1389
|
-
continue;
|
|
1390
|
-
}
|
|
1391
|
-
if (!aabbIntersects(bodyA.AABB, bodyB.AABB)) {
|
|
1392
|
-
continue;
|
|
1393
|
-
}
|
|
1394
|
-
possibleCombi.push({ bodyA, bodyB });
|
|
1395
|
-
}
|
|
1396
|
-
}
|
|
1397
|
-
return possibleCombi;
|
|
1398
|
-
}
|
|
1399
|
-
function broadPhaseWithSpatialIndex(spatialIndex, bodies) {
|
|
1400
|
-
let possibleCombi = [];
|
|
1401
|
-
for (let index = 0;index <= bodies.length - 1; index++) {
|
|
1402
|
-
let objsToCheck = spatialIndex.retrieve(bodies[index]);
|
|
1403
|
-
for (let jindex = 0;jindex <= objsToCheck.length - 1; jindex++) {
|
|
1404
|
-
let bodyA = bodies[index];
|
|
1405
|
-
let bodyB = objsToCheck[jindex];
|
|
1406
|
-
if (bodyA.isStatic() && bodyB.isStatic()) {
|
|
1407
|
-
continue;
|
|
1408
|
-
}
|
|
1409
|
-
if (!aabbIntersects(bodyA.AABB, bodyB.AABB)) {
|
|
1410
|
-
continue;
|
|
1411
|
-
}
|
|
1412
|
-
possibleCombi.push({ bodyA, bodyB });
|
|
1413
|
-
}
|
|
1414
|
-
}
|
|
1415
|
-
return possibleCombi;
|
|
1416
|
-
}
|
|
1417
|
-
function broadPhase(quadTree, bodies) {
|
|
1418
|
-
let possibleCombi = [];
|
|
1419
|
-
for (let index = 0;index <= bodies.length - 1; index++) {
|
|
1420
|
-
let objsToCheck = quadTree.retrieve(bodies[index]);
|
|
1421
|
-
for (let jindex = 0;jindex <= objsToCheck.length - 1; jindex++) {
|
|
1422
|
-
let bodyA = bodies[index];
|
|
1423
|
-
let bodyB = objsToCheck[jindex];
|
|
1424
|
-
if (bodyA.isStatic() && bodyB.isStatic()) {
|
|
1425
|
-
continue;
|
|
1426
|
-
}
|
|
1427
|
-
if (!aabbIntersects(bodyA.AABB, bodyB.AABB)) {
|
|
1428
|
-
continue;
|
|
1429
|
-
}
|
|
1430
|
-
possibleCombi.push({ bodyAIndex: index, bodyBIndex: jindex });
|
|
1431
|
-
}
|
|
1432
|
-
}
|
|
1433
|
-
return possibleCombi;
|
|
1434
|
-
}
|
|
1435
|
-
function broadPhaseWithSpatialIndexFiltered(spatialIndex, bodies) {
|
|
1436
|
-
let possibleCombi = [];
|
|
1437
|
-
for (let index = 0;index <= bodies.length - 1; index++) {
|
|
1438
|
-
let objsToCheck = spatialIndex.retrieve(bodies[index]);
|
|
1439
|
-
for (let jindex = 0;jindex <= objsToCheck.length - 1; jindex++) {
|
|
1440
|
-
let bodyA = bodies[index];
|
|
1441
|
-
let bodyB = objsToCheck[jindex];
|
|
1442
|
-
if (bodyA.isStatic() && bodyB.isStatic()) {
|
|
1443
|
-
continue;
|
|
1444
|
-
}
|
|
1445
|
-
if (!canCollide(bodyA.collisionFilter, bodyB.collisionFilter)) {
|
|
1446
|
-
continue;
|
|
1447
|
-
}
|
|
1448
|
-
if (!aabbIntersects(bodyA.AABB, bodyB.AABB)) {
|
|
1449
|
-
continue;
|
|
1450
|
-
}
|
|
1451
|
-
possibleCombi.push({ bodyA, bodyB });
|
|
1452
|
-
}
|
|
1453
|
-
}
|
|
1454
|
-
return possibleCombi;
|
|
1455
|
-
}
|
|
1456
|
-
function narrowPhaseWithRigidBodyAndPairs(bodies, combinationsToCheck, resolveCollisionFlag) {
|
|
1457
|
-
const contactPoints = [];
|
|
1458
|
-
const collisions = [];
|
|
1459
|
-
combinationsToCheck.forEach((combination) => {
|
|
1460
|
-
let bodyA = combination.bodyA;
|
|
1461
|
-
let bodyB = combination.bodyB;
|
|
1462
|
-
let { collision, depth, normal: normalAxis } = intersects(bodyA, bodyB);
|
|
1463
|
-
if (collision && normalAxis !== undefined && depth !== undefined) {
|
|
1464
|
-
const collisionContactPoints = [];
|
|
1465
|
-
const contactCenter = {
|
|
1466
|
-
x: (bodyA.center.x + bodyB.center.x) / 2,
|
|
1467
|
-
y: (bodyA.center.y + bodyB.center.y) / 2
|
|
1468
|
-
};
|
|
1469
|
-
collisionContactPoints.push(contactCenter);
|
|
1470
|
-
const collisionData = {
|
|
1471
|
-
bodyA,
|
|
1472
|
-
bodyB,
|
|
1473
|
-
contactPoints: collisionContactPoints,
|
|
1474
|
-
normal: normalAxis,
|
|
1475
|
-
depth
|
|
1476
|
-
};
|
|
1477
|
-
collisions.push(collisionData);
|
|
1478
|
-
contactPoints.push(...collisionContactPoints);
|
|
1479
|
-
if (resolveCollisionFlag) {
|
|
1480
|
-
let moveDisplacement = PointCal2.multiplyVectorByScalar(normalAxis, depth / 2);
|
|
1481
|
-
let revMoveDisplacement = PointCal2.multiplyVectorByScalar(normalAxis, -depth / 2);
|
|
1482
|
-
if (!bodyA.isStatic()) {
|
|
1483
|
-
bodyA.move(moveDisplacement);
|
|
1484
|
-
}
|
|
1485
|
-
if (!bodyB.isStatic()) {
|
|
1486
|
-
bodyB.move(revMoveDisplacement);
|
|
1487
|
-
}
|
|
1488
|
-
if (bodyA.isStatic()) {
|
|
1489
|
-
bodyB.move(revMoveDisplacement);
|
|
1490
|
-
}
|
|
1491
|
-
if (bodyB.isStatic()) {
|
|
1492
|
-
bodyA.move(moveDisplacement);
|
|
1493
|
-
}
|
|
1494
|
-
resolveCollisionWithRotation(bodyA, bodyB, { normal: normalAxis, contactPoints: collisionContactPoints });
|
|
1495
|
-
}
|
|
1496
|
-
}
|
|
1497
|
-
});
|
|
1498
|
-
return { contactPoints, collisions };
|
|
1499
|
-
}
|
|
1500
|
-
// src/pair-manager.ts
|
|
1501
|
-
class PairManager {
|
|
1502
|
-
pairs = new Map;
|
|
1503
|
-
frameNumber = 0;
|
|
1504
|
-
maxPairAge = 10;
|
|
1505
|
-
constructor() {}
|
|
1506
|
-
getPairId(bodyA, bodyB) {
|
|
1507
|
-
const idA = this.getBodyId(bodyA);
|
|
1508
|
-
const idB = this.getBodyId(bodyB);
|
|
1509
|
-
return idA < idB ? `${idA}:${idB}` : `${idB}:${idA}`;
|
|
1510
|
-
}
|
|
1511
|
-
getBodyId(body) {
|
|
1512
|
-
return body.toString();
|
|
1513
|
-
}
|
|
1514
|
-
updatePairs(newCollisions) {
|
|
1515
|
-
this.frameNumber++;
|
|
1516
|
-
const events = {
|
|
1517
|
-
created: [],
|
|
1518
|
-
updated: [],
|
|
1519
|
-
removed: []
|
|
1520
|
-
};
|
|
1521
|
-
this.pairs.forEach((pair) => {
|
|
1522
|
-
pair.isActive = false;
|
|
1523
|
-
});
|
|
1524
|
-
for (const collision of newCollisions) {
|
|
1525
|
-
const id = this.getPairId(collision.bodyA, collision.bodyB);
|
|
1526
|
-
const existingPair = this.pairs.get(id);
|
|
1527
|
-
if (existingPair) {
|
|
1528
|
-
existingPair.isActive = true;
|
|
1529
|
-
existingPair.frameUpdated = this.frameNumber;
|
|
1530
|
-
existingPair.contactPoints = collision.contactPoints || [];
|
|
1531
|
-
existingPair.normal = collision.normal;
|
|
1532
|
-
existingPair.depth = collision.depth;
|
|
1533
|
-
events.updated.push(existingPair);
|
|
1534
|
-
} else {
|
|
1535
|
-
const newPair = {
|
|
1536
|
-
bodyA: collision.bodyA,
|
|
1537
|
-
bodyB: collision.bodyB,
|
|
1538
|
-
id,
|
|
1539
|
-
isActive: true,
|
|
1540
|
-
contactPoints: collision.contactPoints || [],
|
|
1541
|
-
normal: collision.normal,
|
|
1542
|
-
depth: collision.depth,
|
|
1543
|
-
frameCreated: this.frameNumber,
|
|
1544
|
-
frameUpdated: this.frameNumber
|
|
1545
|
-
};
|
|
1546
|
-
this.pairs.set(id, newPair);
|
|
1547
|
-
events.created.push(newPair);
|
|
1548
|
-
}
|
|
1549
|
-
}
|
|
1550
|
-
const pairsToRemove = [];
|
|
1551
|
-
this.pairs.forEach((pair, id) => {
|
|
1552
|
-
if (!pair.isActive && this.frameNumber - pair.frameUpdated > this.maxPairAge) {
|
|
1553
|
-
pairsToRemove.push(id);
|
|
1554
|
-
events.removed.push(pair);
|
|
1555
|
-
}
|
|
1556
|
-
});
|
|
1557
|
-
pairsToRemove.forEach((id) => {
|
|
1558
|
-
this.pairs.delete(id);
|
|
1559
|
-
});
|
|
1560
|
-
return events;
|
|
1561
|
-
}
|
|
1562
|
-
getActivePairs() {
|
|
1563
|
-
return Array.from(this.pairs.values()).filter((pair) => pair.isActive);
|
|
1564
|
-
}
|
|
1565
|
-
getPair(bodyA, bodyB) {
|
|
1566
|
-
const id = this.getPairId(bodyA, bodyB);
|
|
1567
|
-
return this.pairs.get(id);
|
|
1568
|
-
}
|
|
1569
|
-
clear() {
|
|
1570
|
-
this.pairs.clear();
|
|
1571
|
-
this.frameNumber = 0;
|
|
1572
|
-
}
|
|
1573
|
-
getStats() {
|
|
1574
|
-
return {
|
|
1575
|
-
totalPairs: this.pairs.size,
|
|
1576
|
-
activePairs: this.getActivePairs().length,
|
|
1577
|
-
frameNumber: this.frameNumber
|
|
1578
|
-
};
|
|
1579
|
-
}
|
|
1580
|
-
}
|
|
1581
|
-
|
|
1582
|
-
// src/world.ts
|
|
1583
|
-
class World {
|
|
1584
|
-
rigidBodyList;
|
|
1585
|
-
rigidBodyMap;
|
|
1586
|
-
_resolveCollision;
|
|
1587
|
-
maxTransWidth;
|
|
1588
|
-
maxTransHeight;
|
|
1589
|
-
bound;
|
|
1590
|
-
spatialIndex;
|
|
1591
|
-
spatialIndexType;
|
|
1592
|
-
constraints;
|
|
1593
|
-
pinJoints = [];
|
|
1594
|
-
pairManager;
|
|
1595
|
-
enableSleeping = true;
|
|
1596
|
-
_context = null;
|
|
1597
|
-
constructor(maxTransWidth, maxTransHeight, spatialIndexType = "dynamictree") {
|
|
1598
|
-
this.maxTransHeight = maxTransHeight;
|
|
1599
|
-
this.maxTransWidth = maxTransWidth;
|
|
1600
|
-
this.spatialIndexType = spatialIndexType;
|
|
1601
|
-
this.bound = new RectangleBound({ x: -this.maxTransWidth, y: -this.maxTransHeight }, 2 * this.maxTransWidth, 2 * this.maxTransHeight);
|
|
1602
|
-
if (spatialIndexType === "dynamictree") {
|
|
1603
|
-
this.spatialIndex = new DynamicTree;
|
|
1604
|
-
} else if (spatialIndexType === "sap") {
|
|
1605
|
-
this.spatialIndex = new SweepAndPrune;
|
|
1606
|
-
} else {
|
|
1607
|
-
this.spatialIndex = new QuadTree(0, this.bound);
|
|
1608
|
-
}
|
|
1609
|
-
this.rigidBodyList = [];
|
|
1610
|
-
this.rigidBodyMap = new Map;
|
|
1611
|
-
this._resolveCollision = true;
|
|
1612
|
-
this.constraints = [];
|
|
1613
|
-
this.pairManager = new PairManager;
|
|
1614
|
-
}
|
|
1615
|
-
addRigidBody(ident, body) {
|
|
1616
|
-
this.rigidBodyList.push(body);
|
|
1617
|
-
this.rigidBodyMap.set(ident, body);
|
|
1618
|
-
if (this.spatialIndexType === "sap") {
|
|
1619
|
-
this.spatialIndex.insert(body);
|
|
1620
|
-
}
|
|
1621
|
-
}
|
|
1622
|
-
removeRigidBody(ident) {
|
|
1623
|
-
if (this.rigidBodyMap.has(ident)) {
|
|
1624
|
-
const body = this.rigidBodyMap.get(ident);
|
|
1625
|
-
this.rigidBodyMap.delete(ident);
|
|
1626
|
-
if (body && this.spatialIndexType === "sap" && this.spatialIndex instanceof SweepAndPrune) {
|
|
1627
|
-
this.spatialIndex.remove(body);
|
|
1628
|
-
}
|
|
1629
|
-
const index = this.rigidBodyList.findIndex((b) => b === body);
|
|
1630
|
-
if (index !== -1) {
|
|
1631
|
-
this.rigidBodyList.splice(index, 1);
|
|
1632
|
-
}
|
|
1633
|
-
}
|
|
1634
|
-
}
|
|
1635
|
-
step(deltaTime) {
|
|
1636
|
-
if (this.enableSleeping) {
|
|
1637
|
-
this.getRigidBodyList().forEach((rigidBody) => {
|
|
1638
|
-
rigidBody.updateSleeping(deltaTime);
|
|
1639
|
-
});
|
|
1640
|
-
}
|
|
1641
|
-
if (this._resolveCollision) {
|
|
1642
|
-
const contactPoints = this.resolveCollisionPhase();
|
|
1643
|
-
}
|
|
1644
|
-
this.constraints.forEach((constraint) => constraint.enforce(deltaTime));
|
|
1645
|
-
this.getRigidBodyList().forEach((rigidBody) => {
|
|
1646
|
-
if (!rigidBody.isSleeping) {
|
|
1647
|
-
rigidBody.step(deltaTime);
|
|
1648
|
-
}
|
|
1649
|
-
});
|
|
1650
|
-
}
|
|
1651
|
-
resolveCollisionPhase() {
|
|
1652
|
-
let rigidBodyList = [];
|
|
1653
|
-
if (this.spatialIndexType === "sap") {
|
|
1654
|
-
this.rigidBodyMap.forEach((body) => {
|
|
1655
|
-
if (!this.enableSleeping || !body.isSleeping) {
|
|
1656
|
-
rigidBodyList.push(body);
|
|
1657
|
-
if (this.spatialIndex instanceof SweepAndPrune) {
|
|
1658
|
-
this.spatialIndex.update(body);
|
|
1659
|
-
}
|
|
1660
|
-
}
|
|
1661
|
-
});
|
|
1662
|
-
} else {
|
|
1663
|
-
this.spatialIndex.clear();
|
|
1664
|
-
this.rigidBodyMap.forEach((body) => {
|
|
1665
|
-
if (!this.enableSleeping || !body.isSleeping) {
|
|
1666
|
-
rigidBodyList.push(body);
|
|
1667
|
-
this.spatialIndex.insert(body);
|
|
1668
|
-
}
|
|
1669
|
-
});
|
|
1670
|
-
}
|
|
1671
|
-
let possibleCombinations = broadPhaseWithSpatialIndexFiltered(this.spatialIndex, rigidBodyList);
|
|
1672
|
-
let collisionResults = narrowPhaseWithRigidBodyAndPairs(rigidBodyList, possibleCombinations, this._resolveCollision);
|
|
1673
|
-
const pairEvents = this.pairManager.updatePairs(collisionResults.collisions);
|
|
1674
|
-
if (this.enableSleeping) {
|
|
1675
|
-
pairEvents.created.forEach((pair) => {
|
|
1676
|
-
if (pair.bodyA.isSleeping)
|
|
1677
|
-
pair.bodyA.setSleeping(false);
|
|
1678
|
-
if (pair.bodyB.isSleeping)
|
|
1679
|
-
pair.bodyB.setSleeping(false);
|
|
1680
|
-
});
|
|
1681
|
-
}
|
|
1682
|
-
return collisionResults.contactPoints;
|
|
1683
|
-
}
|
|
1684
|
-
get resolveCollision() {
|
|
1685
|
-
return this._resolveCollision;
|
|
1686
|
-
}
|
|
1687
|
-
set resolveCollision(resolveCollision2) {
|
|
1688
|
-
this._resolveCollision = resolveCollision2;
|
|
1689
|
-
}
|
|
1690
|
-
getRigidBodyList() {
|
|
1691
|
-
let rigidBodyList = [];
|
|
1692
|
-
this.rigidBodyMap.forEach((body) => {
|
|
1693
|
-
rigidBodyList.push(body);
|
|
1694
|
-
});
|
|
1695
|
-
return rigidBodyList;
|
|
1696
|
-
}
|
|
1697
|
-
getRigidBodyMap() {
|
|
1698
|
-
return this.rigidBodyMap;
|
|
1699
|
-
}
|
|
1700
|
-
setMaxTransHeight(height) {
|
|
1701
|
-
this.maxTransHeight = height;
|
|
1702
|
-
}
|
|
1703
|
-
setMaxTransWidth(width) {
|
|
1704
|
-
this.maxTransWidth = width;
|
|
1705
|
-
}
|
|
1706
|
-
addConstraint(constraint) {
|
|
1707
|
-
this.constraints.push(constraint);
|
|
1708
|
-
}
|
|
1709
|
-
getConstraints() {
|
|
1710
|
-
return this.constraints;
|
|
1711
|
-
}
|
|
1712
|
-
addPinJoint(bodyA, bodyB, anchorA, anchorB) {
|
|
1713
|
-
this.pinJoints.push({ bodyA, bodyB, anchorA, anchorB });
|
|
1714
|
-
}
|
|
1715
|
-
get currentSpatialIndexType() {
|
|
1716
|
-
return this.spatialIndexType;
|
|
1717
|
-
}
|
|
1718
|
-
setSpatialIndexType(type) {
|
|
1719
|
-
if (type === this.spatialIndexType)
|
|
1720
|
-
return;
|
|
1721
|
-
this.spatialIndexType = type;
|
|
1722
|
-
if (type === "dynamictree") {
|
|
1723
|
-
this.spatialIndex = new DynamicTree;
|
|
1724
|
-
} else if (type === "sap") {
|
|
1725
|
-
this.spatialIndex = new SweepAndPrune;
|
|
1726
|
-
} else {
|
|
1727
|
-
this.spatialIndex = new QuadTree(0, this.bound);
|
|
1728
|
-
}
|
|
1729
|
-
}
|
|
1730
|
-
getSpatialIndexStats() {
|
|
1731
|
-
if (this.spatialIndex instanceof DynamicTree) {
|
|
1732
|
-
return this.spatialIndex.getStats();
|
|
1733
|
-
} else if (this.spatialIndex instanceof SweepAndPrune) {
|
|
1734
|
-
return {
|
|
1735
|
-
type: this.spatialIndexType,
|
|
1736
|
-
...this.spatialIndex.getStats()
|
|
1737
|
-
};
|
|
1738
|
-
}
|
|
1739
|
-
return { type: this.spatialIndexType, objects: this.rigidBodyMap.size };
|
|
1740
|
-
}
|
|
1741
|
-
get sleepingEnabled() {
|
|
1742
|
-
return this.enableSleeping;
|
|
1743
|
-
}
|
|
1744
|
-
set sleepingEnabled(enabled) {
|
|
1745
|
-
this.enableSleeping = enabled;
|
|
1746
|
-
if (!enabled) {
|
|
1747
|
-
this.rigidBodyMap.forEach((body) => {
|
|
1748
|
-
if (body.isSleeping) {
|
|
1749
|
-
body.setSleeping(false);
|
|
1750
|
-
}
|
|
1751
|
-
});
|
|
1752
|
-
}
|
|
1753
|
-
}
|
|
1754
|
-
getPairManager() {
|
|
1755
|
-
return this.pairManager;
|
|
1756
|
-
}
|
|
1757
|
-
getCollisionStats() {
|
|
1758
|
-
return {
|
|
1759
|
-
...this.pairManager.getStats(),
|
|
1760
|
-
sleepingBodies: Array.from(this.rigidBodyMap.values()).filter((body) => body.isSleeping).length,
|
|
1761
|
-
totalBodies: this.rigidBodyMap.size
|
|
1762
|
-
};
|
|
1763
|
-
}
|
|
1764
|
-
}
|
|
1765
|
-
// src/constraint.ts
|
|
1766
|
-
import { PointCal as PointCal3 } from "@ue-too/math";
|
|
1767
|
-
|
|
1768
|
-
class FixedPinJoint {
|
|
1769
|
-
anchorA;
|
|
1770
|
-
worldAnchorA;
|
|
1771
|
-
bodyA;
|
|
1772
|
-
constructor(bodyA, anchorA, worldAnchorA) {
|
|
1773
|
-
this.bodyA = bodyA;
|
|
1774
|
-
this.anchorA = anchorA;
|
|
1775
|
-
this.worldAnchorA = worldAnchorA;
|
|
1776
|
-
}
|
|
1777
|
-
enforce(dt) {
|
|
1778
|
-
this.solveWorldPinJointConstraint(dt);
|
|
1779
|
-
}
|
|
1780
|
-
solveWorldPinJointConstraint(dt) {
|
|
1781
|
-
const body = this.bodyA;
|
|
1782
|
-
const localAnchor = this.anchorA;
|
|
1783
|
-
const worldAnchor = this.worldAnchorA;
|
|
1784
|
-
const worldAnchorOnBody = PointCal3.addVector(body.center, PointCal3.rotatePoint(localAnchor, body.orientationAngle));
|
|
1785
|
-
const diff = PointCal3.subVector(worldAnchorOnBody, worldAnchor);
|
|
1786
|
-
const r = PointCal3.subVector(worldAnchorOnBody, body.center);
|
|
1787
|
-
const velocity = PointCal3.addVector(body.linearVelocity, PointCal3.crossProduct({ x: 0, y: 0, z: body.angularVelocity }, r));
|
|
1788
|
-
const invMass = body.isStatic() ? 0 : 1 / body.mass;
|
|
1789
|
-
const invI = body.isStatic() ? 0 : 1 / body.momentOfInertia;
|
|
1790
|
-
const K = {
|
|
1791
|
-
x: invMass + invI * r.y * r.y,
|
|
1792
|
-
y: invMass + invI * r.x * r.x,
|
|
1793
|
-
xy: -invI * r.x * r.y
|
|
1794
|
-
};
|
|
1795
|
-
const baumgarte = 1;
|
|
1796
|
-
const impulse = {
|
|
1797
|
-
x: -K.x * diff.x - K.xy * diff.y - baumgarte * diff.x / dt - velocity.x,
|
|
1798
|
-
y: -K.xy * diff.x - K.y * diff.y - baumgarte * diff.y / dt - velocity.y
|
|
1799
|
-
};
|
|
1800
|
-
if (!body.isStatic()) {
|
|
1801
|
-
body.linearVelocity.x += invMass * impulse.x;
|
|
1802
|
-
body.linearVelocity.y += invMass * impulse.y;
|
|
1803
|
-
body.angularVelocity += invI * (r.x * impulse.y - r.y * impulse.x);
|
|
1804
|
-
}
|
|
1805
|
-
}
|
|
1806
|
-
}
|
|
1807
|
-
|
|
1808
|
-
class PinJoint {
|
|
1809
|
-
anchorA;
|
|
1810
|
-
anchorB;
|
|
1811
|
-
bodyA;
|
|
1812
|
-
bodyB;
|
|
1813
|
-
constructor(bodyA, bodyB, anchorA, anchorB) {
|
|
1814
|
-
this.bodyA = bodyA;
|
|
1815
|
-
this.bodyB = bodyB;
|
|
1816
|
-
this.anchorA = anchorA;
|
|
1817
|
-
this.anchorB = anchorB;
|
|
1818
|
-
}
|
|
1819
|
-
enforce(dt) {
|
|
1820
|
-
this.solvePinJointConstraint(dt);
|
|
1821
|
-
}
|
|
1822
|
-
solvePinJointConstraint(dt) {
|
|
1823
|
-
const bodyA = this.bodyA;
|
|
1824
|
-
const bodyB = this.bodyB;
|
|
1825
|
-
const anchorA = this.anchorA;
|
|
1826
|
-
const anchorB = this.anchorB;
|
|
1827
|
-
const worldAnchorA = PointCal3.addVector(bodyA.center, PointCal3.rotatePoint(anchorA, bodyA.orientationAngle));
|
|
1828
|
-
const worldAnchorB = PointCal3.addVector(bodyB.center, PointCal3.rotatePoint(anchorB, bodyB.orientationAngle));
|
|
1829
|
-
const diff = PointCal3.subVector(worldAnchorB, worldAnchorA);
|
|
1830
|
-
const rA = PointCal3.subVector(worldAnchorA, bodyA.center);
|
|
1831
|
-
const rB = PointCal3.subVector(worldAnchorB, bodyB.center);
|
|
1832
|
-
const relativeVelocity = PointCal3.subVector(PointCal3.addVector(bodyB.linearVelocity, PointCal3.crossProduct({ x: 0, y: 0, z: bodyB.angularVelocity }, rB)), PointCal3.addVector(bodyA.linearVelocity, PointCal3.crossProduct({ x: 0, y: 0, z: bodyA.angularVelocity }, rA)));
|
|
1833
|
-
const invMassA = bodyA.isStatic() ? 0 : 1 / bodyA.mass;
|
|
1834
|
-
const invMassB = bodyB.isStatic() ? 0 : 1 / bodyB.mass;
|
|
1835
|
-
const invIA = bodyA.isStatic() ? 0 : 1 / bodyA.momentOfInertia;
|
|
1836
|
-
const invIB = bodyB.isStatic() ? 0 : 1 / bodyB.momentOfInertia;
|
|
1837
|
-
const K = {
|
|
1838
|
-
x: invMassA + invMassB + invIA * rA.y * rA.y + invIB * rB.y * rB.y,
|
|
1839
|
-
y: invMassA + invMassB + invIA * rA.x * rA.x + invIB * rB.x * rB.x,
|
|
1840
|
-
xy: -invIA * rA.x * rA.y - invIB * rB.x * rB.y
|
|
1841
|
-
};
|
|
1842
|
-
const baumgarte = 1;
|
|
1843
|
-
const impulse = {
|
|
1844
|
-
x: -K.x * diff.x - K.xy * diff.y - baumgarte * diff.x / dt - relativeVelocity.x,
|
|
1845
|
-
y: -K.xy * diff.x - K.y * diff.y - baumgarte * diff.y / dt - relativeVelocity.y
|
|
1846
|
-
};
|
|
1847
|
-
if (!bodyA.isStatic()) {
|
|
1848
|
-
bodyA.linearVelocity.x -= invMassA * impulse.x;
|
|
1849
|
-
bodyA.linearVelocity.y -= invMassA * impulse.y;
|
|
1850
|
-
bodyA.angularVelocity -= invIA * (rA.x * impulse.y - rA.y * impulse.x);
|
|
1851
|
-
}
|
|
1852
|
-
if (!bodyB.isStatic()) {
|
|
1853
|
-
bodyB.linearVelocity.x += invMassB * impulse.x;
|
|
1854
|
-
bodyB.linearVelocity.y += invMassB * impulse.y;
|
|
1855
|
-
bodyB.angularVelocity += invIB * (rB.x * impulse.y - rB.y * impulse.x);
|
|
1856
|
-
}
|
|
1857
|
-
}
|
|
1858
|
-
}
|
|
1859
|
-
function solvePinJointConstraint(constraint, dt) {
|
|
1860
|
-
const { bodyA, bodyB, anchorA, anchorB } = constraint;
|
|
1861
|
-
const worldAnchorA = PointCal3.addVector(bodyA.center, PointCal3.rotatePoint(anchorA, bodyA.orientationAngle));
|
|
1862
|
-
const worldAnchorB = PointCal3.addVector(bodyB.center, PointCal3.rotatePoint(anchorB, bodyB.orientationAngle));
|
|
1863
|
-
const diff = PointCal3.subVector(worldAnchorB, worldAnchorA);
|
|
1864
|
-
const rA = PointCal3.subVector(worldAnchorA, bodyA.center);
|
|
1865
|
-
const rB = PointCal3.subVector(worldAnchorB, bodyB.center);
|
|
1866
|
-
const relativeVelocity = PointCal3.subVector(PointCal3.addVector(bodyB.linearVelocity, PointCal3.crossProduct({ x: 0, y: 0, z: bodyB.angularVelocity }, rB)), PointCal3.addVector(bodyA.linearVelocity, PointCal3.crossProduct({ x: 0, y: 0, z: bodyA.angularVelocity }, rA)));
|
|
1867
|
-
const invMassA = bodyA.isStatic() ? 0 : 1 / bodyA.mass;
|
|
1868
|
-
const invMassB = bodyB.isStatic() ? 0 : 1 / bodyB.mass;
|
|
1869
|
-
const invIA = bodyA.isStatic() ? 0 : 1 / bodyA.momentOfInertia;
|
|
1870
|
-
const invIB = bodyB.isStatic() ? 0 : 1 / bodyB.momentOfInertia;
|
|
1871
|
-
const K = {
|
|
1872
|
-
x: invMassA + invMassB + invIA * rA.y * rA.y + invIB * rB.y * rB.y,
|
|
1873
|
-
y: invMassA + invMassB + invIA * rA.x * rA.x + invIB * rB.x * rB.x,
|
|
1874
|
-
xy: -invIA * rA.x * rA.y - invIB * rB.x * rB.y
|
|
1875
|
-
};
|
|
1876
|
-
const baumgarte = 0.5;
|
|
1877
|
-
const impulse = {
|
|
1878
|
-
x: -K.x * diff.x - K.xy * diff.y - baumgarte * diff.x / dt - relativeVelocity.x,
|
|
1879
|
-
y: -K.xy * diff.x - K.y * diff.y - baumgarte * diff.y / dt - relativeVelocity.y
|
|
1880
|
-
};
|
|
1881
|
-
if (!bodyA.isStatic()) {
|
|
1882
|
-
bodyA.linearVelocity.x -= invMassA * impulse.x;
|
|
1883
|
-
bodyA.linearVelocity.y -= invMassA * impulse.y;
|
|
1884
|
-
bodyA.angularVelocity -= invIA * (rA.x * impulse.y - rA.y * impulse.x);
|
|
1885
|
-
}
|
|
1886
|
-
if (!bodyB.isStatic()) {
|
|
1887
|
-
bodyB.linearVelocity.x += invMassB * impulse.x;
|
|
1888
|
-
bodyB.linearVelocity.y += invMassB * impulse.y;
|
|
1889
|
-
bodyB.angularVelocity += invIB * (rB.x * impulse.y - rB.y * impulse.x);
|
|
1890
|
-
}
|
|
1891
|
-
}
|
|
1892
|
-
function solveWorldPinJointConstraint(constraint, dt) {
|
|
1893
|
-
const { body, localAnchor, worldAnchor } = constraint;
|
|
1894
|
-
const worldAnchorOnBody = PointCal3.addVector(body.center, PointCal3.rotatePoint(localAnchor, body.orientationAngle));
|
|
1895
|
-
const diff = PointCal3.subVector(worldAnchorOnBody, worldAnchor);
|
|
1896
|
-
const r = PointCal3.subVector(worldAnchorOnBody, body.center);
|
|
1897
|
-
const velocity = PointCal3.addVector(body.linearVelocity, PointCal3.crossProduct({ x: 0, y: 0, z: body.angularVelocity }, r));
|
|
1898
|
-
const invMass = body.isStatic() ? 0 : 1 / body.mass;
|
|
1899
|
-
const invI = body.isStatic() ? 0 : 1 / body.momentOfInertia;
|
|
1900
|
-
const K = {
|
|
1901
|
-
x: invMass + invI * r.y * r.y,
|
|
1902
|
-
y: invMass + invI * r.x * r.x,
|
|
1903
|
-
xy: -invI * r.x * r.y
|
|
1904
|
-
};
|
|
1905
|
-
const baumgarte = 0.2;
|
|
1906
|
-
const impulse = {
|
|
1907
|
-
x: -K.x * diff.x - K.xy * diff.y - baumgarte * diff.x / dt - velocity.x,
|
|
1908
|
-
y: -K.xy * diff.x - K.y * diff.y - baumgarte * diff.y / dt - velocity.y
|
|
1909
|
-
};
|
|
1910
|
-
if (!body.isStatic()) {
|
|
1911
|
-
body.linearVelocity.x += invMass * impulse.x;
|
|
1912
|
-
body.linearVelocity.y += invMass * impulse.y;
|
|
1913
|
-
body.angularVelocity += invI * (r.x * impulse.y - r.y * impulse.x);
|
|
1914
|
-
}
|
|
1915
|
-
}
|
|
1916
|
-
export {
|
|
1917
|
-
solveWorldPinJointConstraint,
|
|
1918
|
-
solvePinJointConstraint,
|
|
1919
|
-
resolveCollisionWithRotation,
|
|
1920
|
-
resolveCollision,
|
|
1921
|
-
narrowPhaseWithRigidBodyAndPairs,
|
|
1922
|
-
narrowPhaseWithRigidBody,
|
|
1923
|
-
narrowPhase,
|
|
1924
|
-
intersects,
|
|
1925
|
-
canCollide,
|
|
1926
|
-
broadPhaseWithSpatialIndexFiltered,
|
|
1927
|
-
broadPhaseWithSpatialIndex,
|
|
1928
|
-
broadPhaseWithRigidBodyReturned,
|
|
1929
|
-
broadPhase,
|
|
1930
|
-
aabbIntersects,
|
|
1931
|
-
World,
|
|
1932
|
-
VisualPolygonBody,
|
|
1933
|
-
VisaulCircleBody,
|
|
1934
|
-
SweepAndPrune,
|
|
1935
|
-
RectangleBound,
|
|
1936
|
-
QuadTree,
|
|
1937
|
-
Polygon,
|
|
1938
|
-
PinJoint,
|
|
1939
|
-
PairManager,
|
|
1940
|
-
FixedPinJoint,
|
|
1941
|
-
DynamicTree,
|
|
1942
|
-
DEFAULT_COLLISION_FILTER,
|
|
1943
|
-
CollisionCategory,
|
|
1944
|
-
Circle,
|
|
1945
|
-
BaseRigidBody
|
|
1946
|
-
};
|
|
1947
|
-
|
|
1948
|
-
//# debugId=E95FEA238308AF2664756E2164756E21
|
|
3
|
+
//# debugId=ABD8E4A8414E2D7E64756E2164756E21
|