/// Get the mass data of the body. The rotational inertia is relative
/// to the center of mass.
/// @return a struct containing the mass, inertia and center of the body.
public void GetMassData(out MassData data)
{
data = new MassData();
data.Mass = _mass;
data.I = _I;
Vec2 center = Vec2.Zero;
for (Fixture f = _fixtureList; f != null; f = f._next)
{
MassData massData = f.GetMassData();
_mass += massData.Mass;
center += massData.Mass * massData.Center;
_I += massData.I;
}
// Compute center of mass.
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
center *= _invMass;
}
if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
// Center the inertia about the center of mass.
_I -= _mass * Vec2.Dot(center, center);
Box2DXDebug.Assert(_I > 0.0f);
_invI = 1.0f / _I;
}
else
{
_I = 0.0f;
_invI = 0.0f;
}
// Move center of mass.
Vec2 oldCenter = _sweep.C;
_sweep.LocalCenter = center;
_sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter);
// Update center of mass velocity.
_linearVelocity += Vec2.Cross(_angularVelocity, _sweep.C - oldCenter);
BodyType oldType = _type;
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
_angularVelocity = 0.0f;
_linearVelocity.SetZero();
}
else
{
_type = BodyType.Dynamic;
}
// If the body type changed, we need to flag contacts for filtering.
if (oldType != _type)
{
for (ContactEdge ce = _contactList; ce != null; ce = ce.Next)
{
ce.Contact.FlagForFiltering();
}
}
}
/// <summary>
// From Real-time Collision Detection, p179.
/// </summary>
public void RayCast(out RayCastOutput output, RayCastInput input)
{
float tmin = -Common.Settings.FLT_MAX;
float tmax = Common.Settings.FLT_MAX;
output = new RayCastOutput();
output.Hit = false;
Vec2 p = input.P1;
Vec2 d = input.P2 - input.P1;
Vec2 absD = Common.Math.Abs(d);
Vec2 normal = new Vec2(0);
for (int i = 0; i < 2; ++i)
{
if (absD[i] < Common.Settings.FLT_EPSILON)
{
// Parallel.
if (p[i] < LowerBound[i] || UpperBound[i] < p[i])
{
return;
}
}
else
{
float inv_d = 1.0f / d[i];
float t1 = (LowerBound[i] - p[i]) * inv_d;
float t2 = (UpperBound[i] - p[i]) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
Common.Math.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal[i] = s;
tmin = t1;
}
// Pull the max down
tmax = Common.Math.Min(tmax, t2);
if (tmin > tmax)
{
return;
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.MaxFraction < tmin)
{
return;
}
// Intersection.
output.Fraction = tmin;
output.Normal = normal;
output.Hit = true;
}
public void RayCast(out RayCastOutput output, RayCastInput input)
{
output = new RayCastOutput();
float tmin = -Settings.FLT_MAX;
float tmax = Settings.FLT_MAX;
output.Hit = false;
Vec2 p = input.P1;
Vec2 d = input.P2 - input.P1;
Vec2 absD = Math.Abs(d);
Vec2 normal = new Vec2();
if (absD.X < Settings.FLT_EPSILON)
{
// Parallel.
if (p.X < LowerBound.X || UpperBound.X < p.X)
{
return;
}
}
else
{
float inv_d = 1.0f / d.X;
float t1 = (LowerBound.X - p.X) * inv_d;
float t2 = (UpperBound.X - p.X) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
Common.Math.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.X = s;
tmin = t1;
}
// Pull the max down
tmax = Common.Math.Min(tmax, t2);
if (tmin > tmax)
{
return;
}
}
if (absD.Y < Settings.FLT_EPSILON)
{
// Parallel.
if (p.Y < LowerBound.Y || UpperBound.Y < p.Y)
{
return;
}
}
else
{
float inv_d = 1.0f / d.Y;
float t1 = (LowerBound.Y - p.Y) * inv_d;
float t2 = (UpperBound.Y - p.Y) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
Common.Math.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.Y = s;
tmin = t1;
}
// Pull the max down
tmax = Common.Math.Min(tmax, t2);
if (tmin > tmax)
{
return;
}
}
//// Does the ray start inside the box?
//// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.MaxFraction < tmin)
{
return;
}
//// Intersection.
output.Fraction = tmin;
output.Normal = normal;
output.Hit = true;
}
/// <summary>
/// Set this to the identity transform.
/// </summary>
public void SetIdentity()
{
Position.SetZero();
R.SetIdentity();
}
