新增verlet物理引擎（实验性）

source/src/Math/Vector2.ts

@@ -227,15 +227,11 @@ module es {
          * @param value
          */
         public divide(value: Vector2): Vector2 {
-            this.x /= value.x;
-            this.y /= value.y;
-            return this;
+            return new Vector2(this.x / value.x, this.y / value.y);
         }
 
         public divideScaler(value: number): Vector2 {
-            this.x /= value;
-            this.y /= value;
-            return this;
+            return new Vector2(this.x / value, this.y / value);
         }
 
         /**

source/src/Physics/Shapes/Polygon.ts

@@ -34,6 +34,10 @@ module es {
             this.isBox = isBox;
         }
 
+        public create(vertCount: number, radius: number) {
+            Polygon.buildSymmetricalPolygon(vertCount, radius);
+          }
+
         public _edgeNormals: Vector2[];
 
         /**
@@ -99,10 +103,10 @@ module es {
          * @param radius
          */
         public static buildSymmetricalPolygon(vertCount: number, radius: number) {
-            let verts = new Array(vertCount);
+            const verts = new Array(vertCount);
 
             for (let i = 0; i < vertCount; i++) {
-                let a = 2 * Math.PI * (i / vertCount);
+                const a = 2 * Math.PI * (i / vertCount);
                 verts[i] = new Vector2(Math.cos(a) * radius, Math.sin(a) * radius);
             }
 

source/src/Physics/Verlet/Composites/Ball.ts

@@ -0,0 +1,9 @@
+///<reference path="./Composite.ts" />
+module es {
+    export class Ball extends Composite {
+        constructor(position: Vector2, radius: number = 10) {
+            super();
+            this.addParticle(new Particle(position)).radius = radius;
+        }
+    }
+}

source/src/Physics/Verlet/Composites/Box.ts

@@ -0,0 +1,23 @@
+///<reference path="./Composite.ts" />
+module es {
+    export class VerletBox extends es.Composite {
+        constructor(center: es.Vector2, width: number, height: number, borderStiffness: number = 0.2, diagonalStiffness: number = 0.5) {
+            super();
+
+            const tl = this.addParticle(new Particle(center.add(new Vector2(-width / 2, -height / 2))));
+            const tr = this.addParticle(new Particle(center.add(new Vector2(width / 2, -height / 2))));
+            const br = this.addParticle(new Particle(center.add(new Vector2(width / 2, height / 2))));
+            const bl = this.addParticle(new Particle(center.add(new Vector2(-width / 2, height / 2))));
+
+            this.addConstraint(new DistanceConstraint(tl, tr, borderStiffness));
+            this.addConstraint(new DistanceConstraint(tr, br, borderStiffness));
+            this.addConstraint(new DistanceConstraint(br, bl, borderStiffness));
+            this.addConstraint(new DistanceConstraint(bl, tl, borderStiffness));
+
+            this.addConstraint(new DistanceConstraint(tl, br, diagonalStiffness))
+                .setCollidesWithColliders(false);
+            this.addConstraint(new DistanceConstraint(bl, tr, diagonalStiffness))
+                .setCollidesWithColliders(false);
+        }
+    }
+}

source/src/Physics/Verlet/Composites/Composite.ts

@@ -0,0 +1,88 @@
+module es {
+    export class Composite {
+        public friction: Vector2 = new Vector2(0.98, 1);
+        public drawParticles: boolean = true;
+        public drawConstraints: boolean = true;
+        public collidesWithLayers: number = Physics.allLayers;
+        public particles: Particle[] = [];
+        _constraints: Constraint[] = [];
+
+        public addParticle(particle: Particle): Particle {
+            this.particles.push(particle);
+            return particle;
+        }
+
+        public removeParticle(particle: Particle) {
+            const index = this.particles.indexOf(particle);
+            this.particles.splice(index, 1);
+        }
+
+        public removeAll() {
+            this.particles.length = 0;
+            this._constraints.length = 0;
+        }
+
+        public addConstraint<T extends Constraint>(constraint: T): T {
+            this._constraints.push(constraint);
+            constraint.composite = this;
+            return constraint;
+        }
+
+        public removeConstraint(constraint: Constraint) {
+            const index = this._constraints.indexOf(constraint);
+            this._constraints.splice(index, 1);
+        }
+
+        public applyForce(force: Vector2) {
+            for (let j = 0; j < this.particles.length; j ++)
+                this.particles[j].applyForce(force);
+        }
+
+        public solveConstraints() {
+            for (let i = this._constraints.length - 1; i >= 0; i --)
+                this._constraints[i].solve();
+        }
+
+        public updateParticles(deltaTimeSquared: number, gravity: Vector2) {
+            for (let j = 0; j < this.particles.length; j ++) {
+                const p = this.particles[j];
+                if (p.isPinned) {
+                    p.position = p.pinnedPosition;
+                    continue;
+                }
+
+                p.applyForce(gravity.scale(p.mass));
+
+                const vel = p.position.sub(p.lastPosition).multiply(this.friction);
+                const nextPos = p.position.add(vel).add(p.acceleration.scale(0.5 * deltaTimeSquared));
+
+                p.lastPosition = p.position;
+                p.position = nextPos;
+                p.acceleration.x = p.acceleration.y = 0;
+            }
+        }
+
+        public handleConstraintCollisions() {
+            for (let i = this._constraints.length - 1; i >= 0; i --) {
+                if (this._constraints[i].collidesWithColliders)
+                    this._constraints[i].handleCollisions(this.collidesWithLayers);
+            }
+        }
+
+        public debugRender(batcher: IBatcher) {
+            if (this.drawConstraints) {
+                for (let i = 0; i < this._constraints.length; i ++)
+                    this._constraints[i].debugRender(batcher);
+            }
+
+            if (this.drawParticles) {
+                for (let i = 0; i < this.particles.length; i ++) {
+                    if (this.particles[i].radius == 0) 
+                        batcher.drawPixel(this.particles[i].position, new Color(220, 52, 94), 4);
+                    else
+                        batcher.drawCircleLow(this.particles[i].position, this.particles[i].radius, new Color(220, 52, 94), 1, 4);
+                }
+            }
+        }
+    }
+}

source/src/Physics/Verlet/Composites/LineSegments.ts

@@ -0,0 +1,20 @@
+module es {
+    export class LineSegments extends Composite {
+        constructor(vertices: Vector2[], stiffness: number) {
+            super();
+
+            for (let i = 0; i < vertices.length; i ++) {
+                const p = new Particle(vertices[i]);
+                this.addParticle(p);
+
+                if (i > 0)
+                    this.addConstraint(new DistanceConstraint(this.particles[i], this.particles[i - 1], stiffness));
+            }
+        }
+
+        public pinParticleAtIndex(index: number): LineSegments {
+            this.particles[index].pin();
+            return this;
+        }
+    }
+}

source/src/Physics/Verlet/Constraints/AngleConstraint.ts

@@ -0,0 +1,47 @@
+///<reference path="./Constraint.ts" />
+module es {
+    export class AngleConstraint extends Constraint {
+        public stiffness: number;
+        public angleInRadius: number;
+        
+        _particleA: Particle;
+        _centerParticle: Particle;
+        _particleC: Particle;
+
+        constructor(a: Particle, center: Particle, c: Particle, stiffness: number) {
+            super();
+            this._particleA = a;
+            this._centerParticle = center;
+            this._particleC = c;
+            this.stiffness = stiffness;
+
+            this.collidesWithColliders = false;
+
+            this.angleInRadius = this.angleBetweenParticles();
+        }
+
+        angleBetweenParticles(): number {
+            const first = this._particleA.position.sub(this._centerParticle.position);
+            const second = this._particleC.position.sub(this._centerParticle.position);
+
+            return Math.atan2(first.x * second.y - first.y * second.x, first.x * second.x + first.y * second.y);
+        }
+
+        public solve() {
+            const angleBetween = this.angleBetweenParticles();
+            let diff = angleBetween - this.angleInRadius;
+
+            if (diff <= -Math.PI)
+                diff += 2 * Math.PI;
+            else if(diff >= Math.PI)
+                diff -= 2 * Math.PI;
+
+            diff *= this.stiffness;
+
+            this._particleA.position = MathHelper.rotateAround2(this._particleA.position, this._centerParticle.position, diff);
+            this._particleC.position = MathHelper.rotateAround2(this._particleC.position, this._centerParticle.position, -diff);
+            this._centerParticle.position = MathHelper.rotateAround2(this._centerParticle.position, this._particleA.position, diff);
+            this._centerParticle.position = MathHelper.rotateAround2(this._centerParticle.position, this._particleC.position, -diff);
+        }
+    }
+}

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

@@ -0,0 +1,16 @@
+module es {
+    export abstract class Constraint {
+        public composite: Composite;
+        public collidesWithColliders: boolean = true;
+
+        public abstract solve(): void;
+
+        public handleCollisions(collidesWithLayers: number) {
+
+        }
+
+        public debugRender(batcher: IBatcher) {
+            
+        }
+    }
+}

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

@@ -0,0 +1,123 @@
+module es {
+    export class DistanceConstraint extends Constraint {
+        public stiffness: number;
+        public restingDistance: number;
+        public tearSensitivity = Number.POSITIVE_INFINITY;
+        public shouldApproximateCollisionsWithPoints: boolean;
+        public totalPointsToApproximateCollisionsWith = 5;
+        _particleOne: Particle;
+        _particleTwo: Particle;
+        static _polygon: Polygon = new Polygon([]);
+
+        constructor(first: Particle, second: Particle, stiffness: number, distance: number = -1) {
+            super();
+
+            DistanceConstraint._polygon.create(2, 1);
+            this._particleOne = first;
+            this._particleTwo = second;
+            this.stiffness = stiffness;
+
+            if (distance > -1)
+                this.restingDistance = distance;
+            else
+                this.restingDistance = Vector2.distance(first.position, second.position);
+        }
+
+        public static create(a: Particle, center: Particle, c: Particle, stiffness: number, angleInDegrees: number) {
+            const aToCenter = Vector2.distance(a.position, center.position);
+            const cToCenter = Vector2.distance(c.position, center.position);
+            const distance = Math.sqrt(aToCenter * aToCenter + cToCenter * cToCenter - (2 * aToCenter * cToCenter * Math.cos(angleInDegrees * MathHelper.Deg2Rad)));
+
+            return new DistanceConstraint(a, c, stiffness, distance);
+        }
+
+        public setTearSensitivity(tearSensitivity: number) {
+            this.tearSensitivity = tearSensitivity;
+            return this;
+        }
+
+        public setCollidesWithColliders(collidesWithColliders: boolean) {
+            this.collidesWithColliders = collidesWithColliders;
+            return this;
+        }
+
+        public setShouldApproximateCollisionsWithPoints(shouldApproximateCollisionsWithPoints: boolean) {
+            this.shouldApproximateCollisionsWithPoints = shouldApproximateCollisionsWithPoints;
+            return this;
+        }
+        
+        public solve(): void {
+            const diff = this._particleOne.position.sub(this._particleTwo.position);
+            const d = diff.magnitude();
+
+            const difference = (this.restingDistance - d) / d;
+            if (d / this.restingDistance > this.tearSensitivity) {
+                this.composite.removeConstraint(this);
+                return;
+            }
+
+            const im1 = 1 / this._particleOne.mass;
+            const im2 = 1 / this._particleTwo.mass;
+            const scalarP1 = (im1 / (im1 + im2)) * this.stiffness;
+            const scalarP2 = this.stiffness - scalarP1;
+
+            this._particleOne.position = this._particleOne.position.add(diff.scale(scalarP1 * difference));
+            this._particleTwo.position = this._particleTwo.position.sub(diff.scale(scalarP2 * difference));
+        }
+
+        public handleCollisions(collidesWithLayers: number) {
+            if (this.shouldApproximateCollisionsWithPoints) {
+                this.approximateCollisionsWithPoints(collidesWithLayers);
+                return;
+            }
+
+            const minX = Math.min(this._particleOne.position.x, this._particleTwo.position.x);
+            const maxX = Math.max(this._particleOne.position.x, this._particleTwo.position.x);
+            const minY = Math.min(this._particleOne.position.y, this._particleTwo.position.y);
+            const maxY = Math.max(this._particleOne.position.y, this._particleTwo.position.y);
+            DistanceConstraint._polygon.bounds = Rectangle.fromMinMax(minX, minY, maxX, maxY);
+
+            let midPoint: Vector2;
+            this.preparePolygonForCollisionChecks(midPoint);
+
+            const colliders = Physics.boxcastBroadphase(DistanceConstraint._polygon.bounds, collidesWithLayers);
+            for (let i = 0; i < colliders.length; i ++) {
+                const collider = colliders[i];
+                const result = new CollisionResult();
+                if (DistanceConstraint._polygon.collidesWithShape(collider.shape, result)) {
+                    this._particleOne.position = this._particleOne.position.sub(result.minimumTranslationVector);
+                    this._particleTwo.position = this._particleTwo.position.sub(result.minimumTranslationVector);
+                }
+            }
+        }
+
+        approximateCollisionsWithPoints(collidesWithLayers: number) {
+            let pt: Vector2;
+            for (let j = 0; j < this.totalPointsToApproximateCollisionsWith - 1; j ++) {
+                pt = Vector2.lerp(this._particleOne.position, this._particleTwo.position, (j + 1) / this.totalPointsToApproximateCollisionsWith);
+                const collidedCount = Physics.overlapCircleAll(pt, 3, VerletWorld._colliders, collidesWithLayers);
+                for (let i = 0; i < collidedCount; i ++) {
+                    const collider = VerletWorld._colliders[i];
+                    const collisionResult = new CollisionResult();
+                    if (collider.shape.pointCollidesWithShape(pt, collisionResult)) {
+                        this._particleOne.position = this._particleOne.position.sub(collisionResult.minimumTranslationVector);
+                        this._particleTwo.position = this._particleTwo.position.sub(collisionResult.minimumTranslationVector);
+                    }
+                }
+            }
+        }
+
+        preparePolygonForCollisionChecks(midPoint: Vector2) {
+            const tempMidPoint = Vector2.lerp(this._particleOne.position, this._particleTwo.position, 0.5);
+            midPoint.setTo(tempMidPoint.x, tempMidPoint.y);
+            DistanceConstraint._polygon.position = midPoint;
+            DistanceConstraint._polygon.points[0] = this._particleOne.position.sub(DistanceConstraint._polygon.position);
+            DistanceConstraint._polygon.points[1] = this._particleTwo.position.sub(DistanceConstraint._polygon.position);
+            DistanceConstraint._polygon.recalculateCenterAndEdgeNormals();
+        }
+
+        public debugRender(batcher: IBatcher) {
+            batcher.drawLine(this._particleOne.position, this._particleTwo.position, new Color(67, 62, 54), 1);
+        }
+    }
+}

source/src/Physics/Verlet/Particle.ts

@@ -0,0 +1,39 @@
+module es {
+    export class Particle {
+        public position: Vector2;
+        public lastPosition: Vector2;
+        public mass = 1;
+        public radius: number;
+        public collidesWithColliders: boolean = true;
+        public isPinned: boolean;
+        public acceleration: Vector2;
+        public pinnedPosition: Vector2;
+
+        constructor(position: {x: number, y: number}) {
+            this.position = new Vector2(position.x, position.y);
+            this.lastPosition = new Vector2(position.x, position.y);
+        }
+
+        public applyForce(force: Vector2) {
+            this.acceleration = this.acceleration.add(force.divideScaler(this.mass));
+        }
+
+        public pin(): Particle {
+            this.isPinned = true;
+            this.pinnedPosition = this.position;
+            return this;
+        }
+
+        public pinTo(position: Vector2): Particle {
+            this.isPinned = true;
+            this.pinnedPosition = position;
+            this.position = this.pinnedPosition;
+            return this;
+        }
+
+        public unpin(): Particle {
+            this.isPinned = false;
+            return this;
+        }
+    }
+}

source/src/Physics/Verlet/VerletWorld.ts

@@ -0,0 +1,160 @@
+module es {
+    export class VerletWorld {
+        public gravity: Vector2 = new Vector2(0, 980);
+        public constraintIterations = 3;
+        public maximumStepIterations = 5;
+        public simulationBounds: Rectangle;
+        public allowDragging: boolean = true;
+        public selectionRadiusSquared = 20 * 20;
+        _draggedParticle: Particle;
+        _composites: Composite[] = [];
+        public static _colliders: Collider[] = [];
+        _tempCircle: Circle = new Circle(1);
+
+        _leftOverTime: number;
+        _fixedDeltaTime: number = 1 / 60;
+        _iterationSteps: number;
+        _fixedDeltaTimeSq: number;
+
+        constructor(simulationBounds: Rectangle = null) {
+            this.simulationBounds = simulationBounds;
+            this._fixedDeltaTime = Math.pow(this._fixedDeltaTime, 2);
+        }
+
+        public update() {
+            this.updateTiming();
+
+            if (this.allowDragging)
+                this.handleDragging();
+
+            for (let iteration = 1; iteration <= this._iterationSteps; iteration ++) {
+                for (let i = this._composites.length - 1; i >= 0; i --) {
+                    const composite = this._composites[i];
+                    for (let s = 0; s < this.constraintIterations; s ++)
+                        composite.solveConstraints();
+
+                    composite.updateParticles(this._fixedDeltaTimeSq, this.gravity);
+
+                    composite.handleConstraintCollisions();
+
+                    for (let j = 0; j < composite.particles.length; j ++) {
+                        const p = composite.particles[j];
+
+                        if (this.simulationBounds) {
+                            this.constrainParticleToBounds(p);
+                        }
+
+                        if (p.collidesWithColliders)
+                            this.handleCollisions(p, composite.collidesWithLayers);
+                    }
+                }
+            }
+        }
+
+        constrainParticleToBounds(p: Particle) {
+            const tempPos = p.position;
+            const bounds = this.simulationBounds;
+
+            if (p.radius == 0) {
+                if (tempPos.y > bounds.height)
+                    tempPos.y = bounds.height;
+                else if (tempPos.y < bounds.y)
+                    tempPos.y = bounds.y;
+
+                if (tempPos.x < bounds.x)
+                    tempPos.x = bounds.x;
+                else if (tempPos.x > bounds.width)
+                    tempPos.x = bounds.width;
+            } else {
+                if (tempPos.y < bounds.y + p.radius)
+                    tempPos.y = 2 * (bounds.y + p.radius) - tempPos.y;
+                if (tempPos.y > bounds.height - p.radius)
+                    tempPos.y = 2 * (bounds.height - p.radius) - tempPos.y;
+                if (tempPos.x > bounds.width - p.radius)
+                    tempPos.x = 2 * (bounds.width - p.radius) - tempPos.x;
+                if (tempPos.x < bounds.x + p.radius)
+                    tempPos.x = 2 * (bounds.x + p.radius) - tempPos.x;
+            }
+
+            p.position = tempPos;
+        }
+
+        handleCollisions(p: Particle, collidesWithLayers: number) {
+            const collidedCount = Physics.overlapCircleAll(p.position, p.radius, VerletWorld._colliders, collidesWithLayers);
+            for (let i = 0; i < collidedCount; i++) {
+                const collider = VerletWorld._colliders[i];
+                if (collider.isTrigger)
+                    continue;
+
+                const collisionResult = new CollisionResult();
+
+                if (p.radius < 2) {
+                    if (collider.shape.pointCollidesWithShape(p.position, collisionResult)) {
+                        p.position = p.position.sub(collisionResult.minimumTranslationVector);
+                    }
+                } else {
+                    this._tempCircle.radius = p.radius;
+                    this._tempCircle.position = p.position;
+
+                    if (this._tempCircle.collidesWithShape(collider.shape, collisionResult)) {
+                        p.position = p.position.sub(collisionResult.minimumTranslationVector);
+                    }
+                }
+            }
+        }
+
+        updateTiming() {
+            this._leftOverTime += Time.deltaTime;
+            this._iterationSteps = Math.trunc(this._leftOverTime / this._fixedDeltaTime);
+            this._leftOverTime -= this._iterationSteps * this._fixedDeltaTime;
+
+            this._iterationSteps = Math.min(this._iterationSteps, this.maximumStepIterations);
+        }
+
+        public addComposite<T extends Composite>(composite: T): T {
+            this._composites.push(composite);
+            return composite;
+        }
+
+        public removeComposite(composite: Composite) {
+            const index = this._composites.indexOf(composite);
+            this._composites.splice(index, 1);
+        }
+
+        handleDragging() {
+
+        }
+
+        public getNearestParticle(position: Vector2) {
+            let nearestSquaredDistance = this.selectionRadiusSquared;
+            let particle: Particle = null;
+
+            for (let j = 0; j < this._composites.length; j++) {
+                const particles = this._composites[j].particles;
+                for (let i = 0; i < particles.length; i++) {
+                    const p = particles[i];
+                    const squaredDistanceToParticle = Vector2.sqrDistance(p.position, position);
+                    if (squaredDistanceToParticle <= this.selectionRadiusSquared &&
+                        (particle == null || squaredDistanceToParticle < nearestSquaredDistance)) {
+                        particle = p;
+                        nearestSquaredDistance = squaredDistanceToParticle;
+                    }
+                }
+            }
+
+            return particle;
+        }
+
+        public debugRender(batcher: IBatcher) {
+            for (let i = 0; i < this._composites.length; i ++) {
+                this._composites[i].debugRender(batcher);
+            }
+
+            if (this.allowDragging) {
+                if (this._draggedParticle != null) {
+                    batcher.drawCircle(this._draggedParticle.position, 8, Color.White);
+                }
+            }
+        }
+    }
+}

source/src/Utils/Extensions/RectangleExt.ts

@@ -345,12 +345,13 @@ module es {
                 let tempMat: Matrix2D;
                 
                 // 考虑到原点，将参考点设置为世界参考
-                let transformMatrix = Matrix2D.createTranslation(-worldPosX - origin.x, -worldPosY - origin.y);
-                tempMat = Matrix2D.createScale(scale.x, scale.y);
+                let transformMatrix = new Matrix2D();
+                Matrix2D.createTranslation(-worldPosX - origin.x, -worldPosY - origin.y, transformMatrix);
+                Matrix2D.createScale(scale.x, scale.y, tempMat);
                 transformMatrix = transformMatrix.multiply(tempMat);
-                tempMat = Matrix2D.createRotation(rotation);
+                Matrix2D.createRotation(rotation, tempMat);
                 transformMatrix =transformMatrix.multiply(tempMat);
-                tempMat = Matrix2D.createTranslation(worldPosX, worldPosY);
+                Matrix2D.createTranslation(worldPosX, worldPosY, tempMat);
                 transformMatrix = transformMatrix.multiply(tempMat);
 
                 // TODO: 我们可以把世界变换留在矩阵中，避免在世界空间中得到所有的四个角