void OnCollided(object sender, CollisionEventArgs e) { if (e.Other.IgnoresPhysicsLogics || Scalar.IsPositiveInfinity(e.Other.Mass.Mass)) { return; } IExplosionAffectable affectable = e.Other.Shape as IExplosionAffectable; if (affectable != null) { Wrapper wrapper = new Wrapper(); wrapper.body = e.Other; wrapper.affectable = affectable; items.Add(wrapper); } }
protected internal override void RunLogic(TimeStep step) { for (int index = 0; index < items.Count; ++index) { Wrapper wrapper = items[index]; Body body = wrapper.body; if (wrapper.affectable == null || body.IgnoresPhysicsLogics || Scalar.IsPositiveInfinity(body.Mass.Mass)) { continue; } Vector2D centroid = wrapper.body.Matrices.ToWorldNormal * wrapper.affectable.Centroid; Vector2D buoyancyForce = body.State.Acceleration.Linear * wrapper.affectable.Area * -Density; wrapper.body.ApplyForce(buoyancyForce, centroid); Vector2D relativeVelocity = body.State.Velocity.Linear - FluidVelocity; Vector2D velocityDirection = relativeVelocity.Normalized; if (velocityDirection == Vector2D.Zero) { continue; } Vector2D dragDirection = body.Matrices.ToBodyNormal * velocityDirection.LeftHandNormal; DragInfo dragInfo = wrapper.affectable.GetFluidInfo(dragDirection); if (dragInfo.DragArea < .01f) { continue; } Scalar speedSq = relativeVelocity.MagnitudeSq; Scalar dragForceMag = -.5f * Density * speedSq * dragInfo.DragArea * DragCoefficient; Scalar maxDrag = -MathHelper.Sqrt(speedSq) * body.Mass.Mass * step.DtInv; if (dragForceMag < maxDrag) { dragForceMag = maxDrag; } Vector2D dragForce = dragForceMag * velocityDirection; wrapper.body.ApplyForce(dragForce, body.Matrices.ToWorldNormal * dragInfo.DragCenter); wrapper.body.ApplyTorque( -body.Mass.MomentOfInertia * (body.Coefficients.DynamicFriction + Density + DragCoefficient) * body.State.Velocity.Angular); } }
/*=====================================Log====================================== ** ** ==============================================================================*/ public static double Log(double a, double newBase) { if (Double.IsNaN(a)) { return(a); // IEEE 754-2008: NaN payload must be preserved } if (Double.IsNaN(newBase)) { return(newBase); // IEEE 754-2008: NaN payload must be preserved } if (newBase == 1) { return(Double.NaN); } if (a != 1 && (newBase == 0 || Double.IsPositiveInfinity(newBase))) { return(Double.NaN); } return(Log(a) / Log(newBase)); }
public static double Log(double d) { // d == 0 ==> -Infty Contract.Ensures(d != 0.0 || Double.IsNegativeInfinity(Contract.Result <double>())); // 0 < d < 1 ==> < 0 Contract.Ensures(!(0.0 < d && d < 1.0) || Contract.Result <double>() < 0); // d == 1 ==> 1 Contract.Ensures(d != 1.0 || Contract.Result <double>() == 0.0); // d > 1 ==> > 1 Contract.Ensures(d <1.0 || Contract.Result <double>()> 0.0); // d == NaN || d is -Infty ==> Nan Contract.Ensures(!Double.IsNaN(d) || !Double.IsNegativeInfinity(d) || Double.IsNaN(Contract.Result <Double>())); // d is +Infty ==> +Infty Contract.Ensures(!Double.IsPositiveInfinity(d) || Double.IsPositiveInfinity(Contract.Result <Double>())); return(default(double)); }
private static TimeSpan From(double value, long tickMultiplicator) { if (Double.IsNaN(value)) { throw new ArgumentException(Locale.GetText("Value cannot be NaN."), "value"); } if (Double.IsNegativeInfinity(value) || Double.IsPositiveInfinity(value) || (value < MinValue.Ticks) || (value > MaxValue.Ticks)) { throw new OverflowException(Locale.GetText("Outside range [MinValue,MaxValue]")); } try { value = (value * (tickMultiplicator / TicksPerMillisecond)); checked { long val = (long)Math.Round(value); return(new TimeSpan(val * TicksPerMillisecond)); } } catch (OverflowException) { throw new OverflowException(Locale.GetText("Resulting timespan is too big.")); } }
protected internal override void RunLogic(TimeStep step) { for (int index = 0; index < items.Count; ++index) { Wrapper wrapper = items[index]; Body body = wrapper.body; if (wrapper.affectable == null || body.IgnoresPhysicsLogics || Scalar.IsPositiveInfinity(body.Mass.Mass)) { continue; } IShape shape = body.Shape; int isInsideCount = 0; foreach (Vector2D corner in body.Rectangle.Corners()) { Scalar distance = line.GetDistance(corner); if (distance <= 0) { isInsideCount++; } } if (isInsideCount == 0) { continue; } if (isInsideCount != 4) { Vector2D relativeVelocity = Vector2D.Zero; Vector2D velocityDirection = Vector2D.Zero; Vector2D dragDirection = Vector2D.Zero; Vector2D centroid = Vector2D.Zero; Line bodyLine; Line.Transform(ref body.Matrices.ToBody, ref line, out bodyLine); GetTangentCallback callback = delegate(Vector2D centTemp) { centroid = body.Matrices.ToWorldNormal * centTemp; PhysicsHelper.GetRelativeVelocity(ref body.State.Velocity, ref centroid, out relativeVelocity); relativeVelocity = FluidVelocity - relativeVelocity; velocityDirection = relativeVelocity.Normalized; dragDirection = body.Matrices.ToBodyNormal * velocityDirection.LeftHandNormal; return(dragDirection); }; FluidInfo lineInfo = wrapper.affectable.GetFluidInfo(callback, bodyLine); if (lineInfo == null) { continue; } // Vector2D centTemp = lineInfo.Centroid; Scalar areaTemp = lineInfo.Area; // Vector2D centroid = body.Matrices.ToWorldNormal * centTemp; Vector2D buoyancyForce = body.State.Acceleration.Linear * areaTemp * -Density; body.ApplyForce(buoyancyForce, centroid); /* * PhysicsHelper.GetRelativeVelocity(ref body.State.Velocity, ref centroid, out relativeVelocity); * relativeVelocity = FluidVelocity-relativeVelocity; * velocityDirection = relativeVelocity.Normalized; * dragDirection = body.Matrices.ToBodyNormal * velocityDirection.LeftHandNormal; * * lineInfo = wrapper.affectable.GetFluidInfo(dragDirection, bodyLine); * if (lineInfo == null) { continue; }*/ Scalar speedSq = relativeVelocity.MagnitudeSq; Scalar dragForceMag = -.5f * Density * speedSq * lineInfo.DragArea * DragCoefficient; Scalar maxDrag = -MathHelper.Sqrt(speedSq) * body.Mass.Mass * step.DtInv; if (dragForceMag < maxDrag) { dragForceMag = maxDrag; } Vector2D dragForce = dragForceMag * velocityDirection; body.ApplyForce(dragForce, body.Matrices.ToWorldNormal * lineInfo.DragCenter); body.ApplyTorque( -body.Mass.MomentOfInertia * (body.Coefficients.DynamicFriction + Density + DragCoefficient) * body.State.Velocity.Angular); } else { Vector2D relativeVelocity = body.State.Velocity.Linear - FluidVelocity; Vector2D velocityDirection = relativeVelocity.Normalized; Vector2D dragDirection = body.Matrices.ToBodyNormal * velocityDirection.LeftHandNormal; Vector2D centroid = wrapper.body.Matrices.ToWorldNormal * wrapper.affectable.Centroid; Vector2D buoyancyForce = body.State.Acceleration.Linear * wrapper.affectable.Area * -Density; wrapper.body.ApplyForce(buoyancyForce, centroid); if (velocityDirection == Vector2D.Zero) { continue; } DragInfo dragInfo = wrapper.affectable.GetFluidInfo(dragDirection); if (dragInfo.DragArea < .01f) { continue; } Scalar speedSq = relativeVelocity.MagnitudeSq; Scalar dragForceMag = -.5f * Density * speedSq * dragInfo.DragArea * DragCoefficient; Scalar maxDrag = -MathHelper.Sqrt(speedSq) * body.Mass.Mass * step.DtInv; if (dragForceMag < maxDrag) { dragForceMag = maxDrag; } Vector2D dragForce = dragForceMag * velocityDirection; wrapper.body.ApplyForce(dragForce, body.Matrices.ToWorldNormal * dragInfo.DragCenter); wrapper.body.ApplyTorque( -body.Mass.MomentOfInertia * (body.Coefficients.DynamicFriction + Density + DragCoefficient) * body.State.Velocity.Angular); } } }
void Solvers.ISequentialImpulsesJoint.PreStep(TimeStep step) { Scalar mass1Inv = body.Mass.MassInv; Scalar inertia1Inv = body.Mass.MomentOfInertiaInv; // Pre-compute anchors, mass matrix, and bias. Vector2D.TransformNormal(ref body.Matrices.ToWorld, ref localAnchor1, out r1); // deltaV = deltaV0 + K * impulse // invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] Matrix2x2 K; K.m00 = mass1Inv; K.m11 = mass1Inv; K.m00 += inertia1Inv * r1.Y * r1.Y; K.m01 = -inertia1Inv * r1.X * r1.Y; K.m10 = -inertia1Inv * r1.X * r1.Y; K.m11 += inertia1Inv * r1.X * r1.X; K.m00 += softness; K.m11 += softness; Matrix2x2.Invert(ref K, out M); Vector2D dp; Vector2D.Add(ref body.State.Position.Linear, ref r1, out dp); Vector2D.Subtract(ref anchor, ref dp, out dp); if (!Scalar.IsPositiveInfinity(distanceTolerance) && dp.MagnitudeSq > distanceTolerance * distanceTolerance) { this.Lifetime.IsExpired = true; } if (solver.PositionCorrection) { //bias = -0.1f * dtInv * dp; Scalar flt = -biasFactor * step.DtInv; Vector2D.Multiply(ref dp, ref flt, out bias); } else { bias = Vector2D.Zero; } if (solver.WarmStarting) { PhysicsHelper.SubtractImpulse( ref body.State.Velocity, ref accumulatedImpulse, ref r1, ref mass1Inv, ref inertia1Inv); } else { accumulatedImpulse = Vector2D.Zero; } body.ApplyProxy(); }