优化collision

source/src/Physics/Physics.ts

@@ -6,4 +6,8 @@ class Physics {
     public static overlapCircleAll(center: Vector2, randius: number, results: any[], layerMask = -1){
         return this._spatialHash.overlapCircle(center, randius, results, layerMask);
     }
+
+    public static boxcastBroadphase(rect: Rectangle, layerMask: number = this.allLayers){
+        return this._spatialHash.aabbBroadphase(rect, null, layerMask);
+    }
 }

source/src/Physics/Shapes/Polygon.ts

@@ -4,13 +4,40 @@ class Polygon extends Shape {
     public isUnrotated: boolean = true;
     private _polygonCenter: Vector2;
     private _areEdgeNormalsDirty = true;
-    private _originalPoint: Vector2[]
+    private _originalPoint: Vector2[];
+
+    public _edgeNormals: Vector2[];
+    public get edgeNormals(){
+        if (this._areEdgeNormalsDirty)
+            this.buildEdgeNormals();
+        return this._edgeNormals;
+    }
+    public isBox: boolean;
 
     constructor(vertCount: number, radius: number) {
         super();
         this.setPoints(Polygon.buildSymmertricalPolygon(vertCount, radius));
     }
 
+    private buildEdgeNormals(){
+        let totalEdges = this.isBox ? 2 : this.points.length;
+        if (this._edgeNormals == null || this._edgeNormals.length != totalEdges)
+            this._edgeNormals = new Vector2[totalEdges];
+
+        let p2: Vector2;
+        for (let i = 0; i < totalEdges; i ++){
+            let p1 = this.points[i];
+            if (i + 1 >= this.points.length)
+                p2 = this.points[0];
+            else
+                p2 = this.points[i + 1];
+
+            let perp = Vector2Ext.perpendicular(p1, p2);
+            perp = Vector2.normalize(perp);
+            this._edgeNormals[i] = perp;
+        }
+    }
+
     public setPoints(points: Vector2[]) {
         this.points = points;
         this.recalculateCenterAndEdgeNormals();
@@ -19,6 +46,11 @@ class Polygon extends Shape {
         this._originalPoint = points;
     }
 
+    public collidesWithShape(other: Shape){
+        if (other instanceof Polygon)
+            return ShapeCollisions.polygonToPolygon(this, other);
+    }
+
     public recalculateCenterAndEdgeNormals() {
         this._polygonCenter = Polygon.findPolygonCenter(this.points);
         this._areEdgeNormalsDirty = true;

source/src/Physics/Shapes/ShapeCollisions/ShapeCollisions.ts

@@ -1,4 +1,37 @@
 class ShapeCollisions {
+    public static polygonToPolygon(first: Polygon, second: Polygon){
+        let result = new CollisionResult();
+        let isIntersecting = true;
+        // let firstEdges = first.ed
+    }
+
+    public static circleToPolygon(circle: Circle, polygon: Polygon){
+        let result = new CollisionResult();
+
+        let poly2Circle = Vector2.subtract(circle.position, polygon.position);
+
+        let gpp = Polygon.getClosestPointOnPolygonToPoint(polygon.points, poly2Circle);
+        let closestPoint = gpp.closestPoint;
+        let distanceSquared: number = gpp.distanceSquared;
+        result.normal = gpp.edgeNormal;
+
+        let circleCenterInsidePoly = polygon.containsPoint(circle.position);
+        if (distanceSquared > circle.radius * circle.radius && !circleCenterInsidePoly)
+            return result;
+
+        let mtv: Vector2;
+        if (circleCenterInsidePoly){
+            mtv = Vector2.multiply(result.normal, new Vector2(Math.sqrt(distanceSquared) - circle.radius, Math.sqrt(distanceSquared) - circle.radius));
+        }else{
+            if (distanceSquared == 0){
+                mtv = Vector2.multiply(result.normal, new Vector2(circle.radius, circle.radius));
+            }else{
+                let distance = Math.sqrt(distanceSquared);
+                // mtv = Vector2.multiply( -Vector2.subtract(poly2Circle, closestPoint), new Vector2((circle.radius - distanceSquared) / distance))
+            }
+        }
+    }
+
     public static circleToRect(circle: Circle, box: Rect): CollisionResult{
         let result = new CollisionResult();
         let closestPointOnBounds = box.bounds.getClosestPointOnRectangleBorderToPoint(circle.position).res;

source/src/Physics/Verlet/Constraint/DistanceConstraint.ts

@@ -3,6 +3,8 @@ class DistanceConstraint extends Constraint {
     public stiffness: number = 0;
     public restingDistance: number = 0;
     public tearSensitivity = Number.POSITIVE_INFINITY;
+    public shouldApproximateCollisionWithPoints: boolean;
+    public totalPointsToApproximateCollisionsWith = 5;
 
     private _particleOne: Particle;
     private _particleTwo: Particle;
@@ -28,11 +30,48 @@ class DistanceConstraint extends Constraint {
     }
 
     public handleCollisions(collidersWithLayers){
+        if (this.shouldApproximateCollisionWithPoints){
+            this.approximateCollisionWithPoints(collidersWithLayers)
+            return;
+        }
+
         let minX = Math.min(this._particleOne.position.x, this._particleTwo.position.x);
         let maxX = Math.max(this._particleOne.position.x, this._particleTwo.position.x);
         let minY = Math.min(this._particleOne.position.y, this._particleTwo.position.y);
         let maxY = Math.max(this._particleOne.position.y, this._particleTwo.position.y);
-        // DistanceConstraint._polygon.bounds = Rectangle.
+        DistanceConstraint._polygon.bounds = Rectangle.fromMinMax(minX, minY, maxX, maxY);
+
+        let midPoint: Vector2 = this.preparePolygonForCollisionChecks();
+        let colliders = Physics.boxcastBroadphase(DistanceConstraint._polygon.bounds, collidersWithLayers);
+        colliders.forEach(collider => {
+            
+        });
+    }
+
+    private preparePolygonForCollisionChecks(){
+        let midPoint = Vector2.lerp(this._particleOne.position, this._particleTwo.position, 0.5);
+        DistanceConstraint._polygon.position = midPoint;
+        DistanceConstraint._polygon.points[0] = Vector2.subtract(this._particleOne.position, DistanceConstraint._polygon.position);
+        DistanceConstraint._polygon.points[1] = Vector2.subtract(this._particleTwo.position, DistanceConstraint._polygon.position);
+        DistanceConstraint._polygon.recalculateCenterAndEdgeNormals();
+
+        return midPoint;
+    }
+
+    private approximateCollisionWithPoints(collidersWithLayers: number){
+        let pt;
+        for (let j = 0; j < this.totalPointsToApproximateCollisionsWith - 1; j ++){
+            pt = Vector2.lerp(this._particleOne.position, this._particleTwo.position, (j + 1) / this.totalPointsToApproximateCollisionsWith);
+            let collidedCount = Physics.overlapCircleAll(pt, 3, VerletWorld.colliders, collidersWithLayers);
+            for (let i = 0; i < collidedCount; i ++){
+                let collider = VerletWorld.colliders[i];
+                let collisionResult: CollisionResult = collider.shape.pointCollidesWithShape(pt);
+                if (collisionResult){
+                    this._particleOne.position = Vector2.subtract(this._particleOne.position, collisionResult.minimumTranslationVector);
+                    this._particleTwo.position = Vector2.subtract(this._particleTwo.position, collisionResult.minimumTranslationVector);
+                }
+            }
+        }
     }
 
     public solve() {

source/src/Physics/Verlet/VerletWorld.ts

@@ -14,7 +14,7 @@ class VerletWorld {
     private _composites: Composite[] = [];
     private _fixedDeltaTimeSq: number;
 
-    private static _colliders: Collider[] = new Array(4);
+    public static colliders: Collider[] = new Array(4);
     private _tempCircle: Circle = new Circle(1);
 
     constructor(simulationBounds?: Rectangle){
@@ -52,9 +52,9 @@ class VerletWorld {
     }
 
     private handleCollisions(p: Particle, collidesWithLayers: number){
-        let collidedCount = Physics.overlapCircleAll(p.position, p.radius, VerletWorld._colliders, collidesWithLayers);
+        let collidedCount = Physics.overlapCircleAll(p.position, p.radius, VerletWorld.colliders, collidesWithLayers);
         for (let i = 0; i < collidedCount; i ++){
-            let collider = VerletWorld._colliders[i];
+            let collider = VerletWorld.colliders[i];
             if (collider.isTrigger)
                 continue;