public static void Simulate() { Vector2d vector2d; FInt fInt; Vector2d vector2d1; FInt fInt1; FInt halfF; Vector2d vector2d2; Vector2d vector2d3; FInt fInt2; Partition.StartPartitioning(); for (ushort i = 0; i < DPhysicsManager.PeakCount; i = (ushort)(i + 1)) { Body simObjects = DPhysicsManager.SimObjects[i]; if (!(simObjects == null) && simObjects.Active) { Dictionary <ushort, CollisionPair> myPairs = simObjects.MyPairs; for (ushort j = (ushort)(i + 1); j < DPhysicsManager.PeakCount; j = (ushort)(j + 1)) { Body body = DPhysicsManager.SimObjects[j]; if (body != null) { CollisionPair item = myPairs[j]; item.GenerateCollision(); CollisionResult myCollisionResult = item.MyCollisionResult; if (myCollisionResult.Intersect && item.SimulatePhysics) { bool actedCount = simObjects.ActedCount > 0; bool flag = body.ActedCount > 0; if (actedCount) { Body actedCount1 = simObjects; actedCount1.ActedCount = actedCount1.ActedCount - 1; } if (flag) { Body body1 = body; body1.ActedCount = body1.ActedCount - 1; } bool mass = simObjects.Mass != 0; bool mass1 = body.Mass != 0; if (!simObjects.IsTrigger && !body.IsTrigger && (mass || mass1)) { bool flag1 = true; body.Velocity.Subtract(ref simObjects.Velocity, out vector2d); if (vector2d.x.AbsoluteValueMoreThan(DPhysicsManager.SleepVelocity.RawValue) || vector2d.y.AbsoluteValueMoreThan(DPhysicsManager.SleepVelocity.RawValue)) { Vector2d.Dot(ref myCollisionResult.PenetrationVector, ref vector2d, out fInt); if (fInt.RawValue < (long)0) { flag1 = false; } } if (flag1) { myCollisionResult.PenetrationVector.Multiply(DPhysicsManager.CollisionDamp.RawValue, out vector2d1); if (mass && mass1) { simObjects.Speed.Add(body.Speed.RawValue, out fInt1); if (!fInt1.AbsoluteValueMoreThan(DPhysicsManager.SleepVelocity.RawValue)) { halfF = FInt.HalfF; } else { body.Speed.Divide(fInt1.RawValue, out halfF); } Vector2d vector2d4 = Vector2d.zero; if (!halfF.AbsoluteValueMoreThan(DPhysicsManager.MinimumCollisionOffset.RawValue)) { vector2d1.Multiply(DPhysicsManager.MinimumCollisionOffset.RawValue, out vector2d4); } else { vector2d1.Multiply(halfF.RawValue, out vector2d4); } simObjects.Offset(ref vector2d4); halfF = halfF - FInt.OneF; if (!halfF.AbsoluteValueMoreThan(DPhysicsManager.MinimumCollisionOffset.RawValue)) { vector2d1.Multiply(DPhysicsManager.MinimumCollisionOffset.RawValue, out vector2d4); } else { vector2d1.Multiply(halfF.RawValue, out vector2d4); } body.Offset(ref vector2d4); } else if (mass) { simObjects.Offset(ref vector2d1); } else { myCollisionResult.PenetrationVector.Invert(); body.Offset(ref vector2d1); } } if (actedCount && flag) { if (mass) { Vector2d.Dot(ref simObjects.Velocity, ref myCollisionResult.PenetrationDirection, out fInt2); myCollisionResult.PenetrationDirection.Multiply(fInt2.RawValue, out vector2d2); vector2d2.Multiply((DPhysicsManager.Restitution * simObjects.Mass).RawValue, out vector2d2); } else { Vector2d.Dot(ref body.Velocity, ref myCollisionResult.PenetrationDirection, out fInt2); myCollisionResult.PenetrationDirection.Multiply(fInt2.RawValue, out vector2d2); vector2d2.Multiply(DPhysicsManager.Restitution.RawValue * (long)body.Mass, out vector2d2); vector2d2.Invert(); } if (mass1) { Vector2d.Dot(ref body.Velocity, ref myCollisionResult.PenetrationDirection, out fInt2); myCollisionResult.PenetrationDirection.Multiply(fInt2.RawValue, out vector2d3); vector2d3.Multiply((DPhysicsManager.Restitution * body.Mass).RawValue, out vector2d3); } else { Vector2d.Dot(ref simObjects.Velocity, ref myCollisionResult.PenetrationDirection, out fInt2); myCollisionResult.PenetrationDirection.Multiply(fInt2.RawValue, out vector2d3); vector2d3.Multiply(DPhysicsManager.Restitution.RawValue * (long)simObjects.Mass, out vector2d3); vector2d3.Invert(); } simObjects.ApplyForce(ref vector2d3); body.ApplyForce(ref vector2d2); vector2d2.Invert(); vector2d3.Invert(); simObjects.ApplyForce(ref vector2d2); body.ApplyForce(ref vector2d3); } } } } } } } Body[] bodyArray = DPhysicsManager.SimObjects; for (int k = 0; k < (int)bodyArray.Length; k++) { Body body2 = bodyArray[k]; if (body2 != null) { DPhysicsManager.SimulateBody(body2); } } DPhysicsManager.LastSimulateTime = Time.time; }
public void Calculate() { FInt fInt; FInt fInt1; Vector2d vector2d; Vector2d vector2d1; Vector2d vector2d2; Vector2d vector2d3; Vector2d vector2d4; FInt fInt2; int num; this.Intersect = false; this.PenetrationVector = Vector2d.zero; DCollider bodyA = this.pair.BodyA.dCollider; DCollider bodyB = this.pair.BodyB.dCollider; if (bodyA.MyBounds.IsCircle && bodyB.MyBounds.IsCircle) { if (Bounder.CanIntersect(bodyA, bodyB, ref this.pair.CombinedSqrRadius, out fInt)) { if (!bodyA.IsCircle || !bodyB.IsCircle) { goto Label0; } this.Intersect = true; Mathd.Sqrt(fInt.RawValue, out fInt1); if (fInt1.RawValue <= (long)0) { bodyA.center.Subtract(ref this.pair.BodyA.Velocity, out vector2d1); bodyB.center.Subtract(ref this.pair.BodyB.Velocity, out vector2d2); vector2d1.Subtract(ref vector2d2, out vector2d); vector2d.Magnitude(out fInt1); vector2d.Normalize(); } else { bodyA.center.Subtract(ref bodyB.center, out vector2d); vector2d.Normalize(); } FInt fInt3 = (bodyA.radius + bodyB.radius) - fInt1; vector2d.Multiply(fInt3.RawValue, out this.PenetrationVector); this.PenetrationDirection = vector2d; } return; } else if (!Bounder.CanIntersect(bodyA, bodyB)) { return; } Label0: num = (bodyA.Edges != null ? (int)bodyA.Edges.Length : 0); int num1 = num; int num2 = (bodyB.Edges != null ? (int)bodyB.Edges.Length : 0); FInt maxValue = FInt.MaxValue; Vector2d vector2d5 = new Vector2d(); this.Intersect = true; for (int i = 0; i < num1 + num2; i++) { vector2d3 = (i >= num1 ? bodyB.Edges[i - num1] : bodyA.Edges[i]); Vector2d vector2d6 = vector2d3.localright; vector2d6.Normalize(); FInt zeroF = FInt.ZeroF; FInt zeroF1 = FInt.ZeroF; FInt zeroF2 = FInt.ZeroF; FInt zeroF3 = FInt.ZeroF; CollisionResult.ProjectPolygon(vector2d6, bodyA, out zeroF, out zeroF2); CollisionResult.ProjectPolygon(vector2d6, bodyB, out zeroF1, out zeroF3); FInt fInt4 = CollisionResult.IntervalDistance(zeroF, zeroF2, zeroF1, zeroF3); if (fInt4.RawValue >= (long)0) { this.Intersect = false; return; } fInt4.Inverse(out fInt4); if (fInt4.RawValue < maxValue.RawValue) { maxValue = fInt4; vector2d5 = vector2d6; bodyA.center.Subtract(ref bodyB.center, out vector2d4); Vector2d.Dot(ref vector2d4, ref vector2d5, out fInt2); if (fInt2.RawValue < (long)0) { vector2d5.Invert(); } } } this.PenetrationDirection = vector2d5; vector2d5.Multiply(maxValue.RawValue, out this.PenetrationVector); }