| public Multiply ( Matrix2x3, secondMatrix ) : void | ||
| secondMatrix | Matrix2x3, | |
| return | void | |
/// <summary>
/// Updates all the values caluclated from the State.Position.
/// Re-calculates the Matrices property the re-calculates the Rectangle property
/// from that.
/// </summary>
public void ApplyPosition()
{
MathHelper.ClampAngle(ref state.Position.Angular);
Matrix2x3 matrix;
ALVector2D.ToMatrix2x3(ref state.Position, out matrix);
Matrix2x3.Multiply(ref matrix, ref transformation, out matrix);
matrices.SetToWorld(ref matrix);
shape.CalcBoundingRectangle(ref matrix, out rectangle);
if (engine == null || !engine.inUpdate)
{
OnPositionChanged();
}
}
private bool CanCollideInternal(Body thisBody, Body otherBody)
{
Matrix2x3 m1, m2;
Matrix2x3.Multiply(ref directionMatrix, ref thisBody.Matrices.ToWorld, out m1);
Matrix2x3.Multiply(ref directionMatrix, ref otherBody.Matrices.ToWorld, out m2);
BoundingRectangle r1, r2;
thisBody.Shape.CalcBoundingRectangle(ref m1, out r1);
otherBody.Shape.CalcBoundingRectangle(ref m2, out r2);
return((r1.Min.X + depthAllowed > r2.Max.X) ||
!(r1.Max.Y > r2.Min.Y &&
r1.Min.Y < r2.Max.Y));
}
protected override bool CanCollide(Body thisBody, Body otherBody, Ignorer other)
{
if (otherBody.IgnoresPhysicsLogics ||
otherBody.IsBroadPhaseOnly)
{
return(true);
}
Matrix2x3 m1, m2;
Matrix2x3.Multiply(ref directionMatrix, ref thisBody.Matrices.ToWorld, out m1);
Matrix2x3.Multiply(ref directionMatrix, ref otherBody.Matrices.ToWorld, out m2);
BoundingRectangle r1, r2;
thisBody.Shape.CalcBoundingRectangle(ref m1, out r1);
otherBody.Shape.CalcBoundingRectangle(ref m2, out r2);
return(r1.Min.X + depthAllowed > r2.Max.X);
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
entityADynamic = entityA != null && entityA.isDynamic;
entityBDynamic = entityB != null && entityB.isDynamic;
contactCount = ContactManifoldConstraint.penetrationConstraints.Count;
switch (contactCount)
{
case 1:
manifoldCenter = ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position;
break;
case 2:
Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref ContactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
manifoldCenter.X *= .5f;
manifoldCenter.Y *= .5f;
manifoldCenter.Z *= .5f;
break;
case 3:
Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref ContactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
ref manifoldCenter,
out manifoldCenter);
manifoldCenter.X *= .333333333f;
manifoldCenter.Y *= .333333333f;
manifoldCenter.Z *= .333333333f;
break;
case 4:
//This isn't actually the center of the manifold. Is it good enough? Sure seems like it.
Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref ContactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
ref manifoldCenter,
out manifoldCenter);
Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[3].contact.Position,
ref manifoldCenter,
out manifoldCenter);
manifoldCenter.X *= .25f;
manifoldCenter.Y *= .25f;
manifoldCenter.Z *= .25f;
break;
default:
manifoldCenter = Toolbox.NoVector;
break;
}
//Compute the three dimensional relative velocity at the point.
Vector3 velocityA, velocityB;
if (entityA != null)
{
Vector3.Subtract(ref manifoldCenter, ref entityA.position, out ra);
Vector3.Cross(ref entityA.angularVelocity, ref ra, out velocityA);
Vector3.Add(ref velocityA, ref entityA.linearVelocity, out velocityA);
}
else
{
velocityA = new Vector3();
}
if (entityB != null)
{
Vector3.Subtract(ref manifoldCenter, ref entityB.position, out rb);
Vector3.Cross(ref entityB.angularVelocity, ref rb, out velocityB);
Vector3.Add(ref velocityB, ref entityB.linearVelocity, out velocityB);
}
else
{
velocityB = new Vector3();
}
Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity);
//Get rid of the normal velocity.
Vector3 normal = ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Normal;
float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y +
normal.Z * relativeVelocity.Z;
relativeVelocity.X -= normalVelocityScalar * normal.X;
relativeVelocity.Y -= normalVelocityScalar * normal.Y;
relativeVelocity.Z -= normalVelocityScalar * normal.Z;
//Create the jacobian entry and decide the friction coefficient.
float length = relativeVelocity.LengthSquared();
if (length > Toolbox.Epsilon)
{
length = (float)Math.Sqrt(length);
float inverseLength = 1 / length;
linearA.M11 = relativeVelocity.X * inverseLength;
linearA.M12 = relativeVelocity.Y * inverseLength;
linearA.M13 = relativeVelocity.Z * inverseLength;
friction = length > CollisionResponseSettings.StaticFrictionVelocityThreshold
? ContactManifoldConstraint.materialInteraction.KineticFriction
: ContactManifoldConstraint.materialInteraction.StaticFriction;
}
else
{
friction = ContactManifoldConstraint.materialInteraction.StaticFriction;
//If there was no velocity, try using the previous frame's jacobian... if it exists.
//Reusing an old one is okay since jacobians are cleared when a contact is initialized.
if (!(linearA.M11 != 0 || linearA.M12 != 0 || linearA.M13 != 0))
{
//Otherwise, just redo it all.
//Create arbitrary axes.
Vector3 axis1;
Vector3.Cross(ref normal, ref Toolbox.RightVector, out axis1);
length = axis1.LengthSquared();
if (length > Toolbox.Epsilon)
{
length = (float)Math.Sqrt(length);
float inverseLength = 1 / length;
linearA.M11 = axis1.X * inverseLength;
linearA.M12 = axis1.Y * inverseLength;
linearA.M13 = axis1.Z * inverseLength;
}
else
{
Vector3.Cross(ref normal, ref Toolbox.UpVector, out axis1);
axis1.Normalize();
linearA.M11 = axis1.X;
linearA.M12 = axis1.Y;
linearA.M13 = axis1.Z;
}
}
}
//Second axis is first axis x normal
linearA.M21 = linearA.M12 * normal.Z - linearA.M13 * normal.Y;
linearA.M22 = linearA.M13 * normal.X - linearA.M11 * normal.Z;
linearA.M23 = linearA.M11 * normal.Y - linearA.M12 * normal.X;
//Compute angular jacobians
if (entityA != null)
{
//angularA 1 = ra x linear axis 1
angularA.M11 = ra.Y * linearA.M13 - ra.Z * linearA.M12;
angularA.M12 = ra.Z * linearA.M11 - ra.X * linearA.M13;
angularA.M13 = ra.X * linearA.M12 - ra.Y * linearA.M11;
//angularA 2 = ra x linear axis 2
angularA.M21 = ra.Y * linearA.M23 - ra.Z * linearA.M22;
angularA.M22 = ra.Z * linearA.M21 - ra.X * linearA.M23;
angularA.M23 = ra.X * linearA.M22 - ra.Y * linearA.M21;
}
//angularB 1 = linear axis 1 x rb
if (entityB != null)
{
angularB.M11 = linearA.M12 * rb.Z - linearA.M13 * rb.Y;
angularB.M12 = linearA.M13 * rb.X - linearA.M11 * rb.Z;
angularB.M13 = linearA.M11 * rb.Y - linearA.M12 * rb.X;
//angularB 2 = linear axis 2 x rb
angularB.M21 = linearA.M22 * rb.Z - linearA.M23 * rb.Y;
angularB.M22 = linearA.M23 * rb.X - linearA.M21 * rb.Z;
angularB.M23 = linearA.M21 * rb.Y - linearA.M22 * rb.X;
}
//Compute inverse effective mass matrix
Matrix2x2 entryA, entryB;
//these are the transformed coordinates
Matrix2x3 transform;
Matrix3x2 transpose;
if (entityADynamic)
{
Matrix2x3.Multiply(ref angularA, ref entityA.inertiaTensorInverse, out transform);
Matrix2x3.Transpose(ref angularA, out transpose);
Matrix2x2.Multiply(ref transform, ref transpose, out entryA);
entryA.M11 += entityA.inverseMass;
entryA.M22 += entityA.inverseMass;
}
else
{
entryA = new Matrix2x2();
}
if (entityBDynamic)
{
Matrix2x3.Multiply(ref angularB, ref entityB.inertiaTensorInverse, out transform);
Matrix2x3.Transpose(ref angularB, out transpose);
Matrix2x2.Multiply(ref transform, ref transpose, out entryB);
entryB.M11 += entityB.inverseMass;
entryB.M22 += entityB.inverseMass;
}
else
{
entryB = new Matrix2x2();
}
velocityToImpulse.M11 = -entryA.M11 - entryB.M11;
velocityToImpulse.M12 = -entryA.M12 - entryB.M12;
velocityToImpulse.M21 = -entryA.M21 - entryB.M21;
velocityToImpulse.M22 = -entryA.M22 - entryB.M22;
Matrix2x2.Invert(ref velocityToImpulse, out velocityToImpulse);
}
protected internal override void RunLogic(TimeStep step)
{
Scalar area = MathHelper.Pi * radius * radius;
Scalar density = explosionBody.Mass.Mass / area;
BoundingCircle circle = new BoundingCircle(explosionBody.State.Position.Linear, radius);
Matrix2x3 temp;
ALVector2D.ToMatrix2x3(ref explosionBody.State.Position, out temp);
Matrices matrices = new Matrices();
matrices.SetToWorld(ref temp);
Vector2D relativeVelocity = Vector2D.Zero;
Vector2D velocityDirection = Vector2D.Zero;
Vector2D dragDirection = Vector2D.Zero;
for (int index = 0; index < items.Count; ++index)
{
Wrapper wrapper = items[index];
Body body = wrapper.body;
Matrix2x3 matrix;
Matrix2x3.Multiply(ref matrices.ToBody, ref body.Matrices.ToWorld, out matrix);
ContainmentType containmentType;
BoundingRectangle rect = body.Rectangle;
circle.Contains(ref rect, out containmentType);
if (containmentType == ContainmentType.Intersects)
{
return;
GetTangentCallback callback = delegate(Vector2D centroid)
{
centroid = body.Matrices.ToWorld * centroid;
Vector2D p1 = centroid - explosionBody.State.Position.Linear;
Vector2D p2 = centroid - body.State.Position.Linear;
PhysicsHelper.GetRelativeVelocity(
ref explosionBody.State.Velocity,
ref body.State.Velocity,
ref p1,
ref p2,
out relativeVelocity);
relativeVelocity = p1.Normalized * this.pressurePulseSpeed;
relativeVelocity = -relativeVelocity;
velocityDirection = relativeVelocity.Normalized;
dragDirection = matrices.ToBodyNormal * velocityDirection.LeftHandNormal;
return(dragDirection);
};
DragInfo dragInfo = wrapper.affectable.GetExplosionInfo(matrix, radius, callback);
if (dragInfo == null)
{
continue;
}
if (velocityDirection == Vector2D.Zero)
{
continue;
}
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.ToBody * matrices.ToWorld) * dragInfo.DragCenter);
}
else if (containmentType == ContainmentType.Contains)
{
Vector2D centroid = body.Matrices.ToWorld * wrapper.affectable.Centroid;
Vector2D p1 = centroid - explosionBody.State.Position.Linear;
Vector2D p2 = centroid - body.State.Position.Linear;
PhysicsHelper.GetRelativeVelocity(
ref explosionBody.State.Velocity,
ref body.State.Velocity,
ref p1,
ref p2,
out relativeVelocity);
relativeVelocity = p1.Normalized * this.pressurePulseSpeed;
relativeVelocity = -relativeVelocity;
velocityDirection = relativeVelocity.Normalized;
dragDirection = 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);
}
}
}
