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();
}
