private void DrawShape(Fixture fixture, Transform xf, Color color)
{
Color coreColor = new Color(0.9f, 0.6f, 0.6f);
switch (fixture.GetType())
{
case ShapeType.CircleShape:
{
CircleShape circle = (CircleShape)fixture.GetShape();
Vec2 center = Math.Mul(xf, circle._p);
float radius = circle._radius;
Vec2 axis = xf.R.Col1;
_debugDraw.DrawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.PolygonShape:
{
PolygonShape poly = (PolygonShape)fixture.GetShape();
int vertexCount = poly.VertexCount;
Box2DXDebug.Assert(vertexCount <= Settings.MaxPolygonVertices);
Vec2[] vertices = new Vec2[Settings.MaxPolygonVertices];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Math.Mul(xf, poly.Vertices[i]);
}
_debugDraw.DrawSolidPolygon(vertices, vertexCount, color);
}
break;
}
}
public static Mat22 Mul(Mat22 A, Mat22 B)
{
Mat22 result = default(Mat22);
result.Set(Math.Mul(A, B.Col1), Math.Mul(A, B.Col2));
return(result);
}
public override bool TestPoint(Transform transform, Vec2 p)
{
Vec2 center = transform.Position + Math.Mul(transform.R, _p);
Vec2 d = p - center;
return(Vec2.Dot(d, d) <= _radius * _radius);
}
/// <summary>
/// A * B
/// </summary>
public static Mat22 Mul(Mat22 A, Mat22 B)
{
Mat22 C = new Mat22();
C.Set(Math.Mul(A, B.Col1), Math.Mul(A, B.Col2));
return(C);
}
public void SetAsBox(float hx, float hy, Vec2 center, float angle)
{
VertexCount = 4;
Vertices[0].Set(-hx, -hy);
Vertices[1].Set(hx, -hy);
Vertices[2].Set(hx, hy);
Vertices[3].Set(-hx, hy);
Normals[0].Set(0.0f, -1.0f);
Normals[1].Set(1.0f, 0.0f);
Normals[2].Set(0.0f, 1.0f);
Normals[3].Set(-1.0f, 0.0f);
Centroid = center;
Transform xf = new Transform();
xf.Position = center;
xf.R.Set(angle);
// Transform vertices and normals.
for (int i = 0; i < VertexCount; ++i)
{
Vertices[i] = Math.Mul(xf, Vertices[i]);
Normals[i] = Math.Mul(xf.R, Normals[i]);
}
}
internal Body(BodyDef bd, World world)
{
_flags = 0;
if (bd.IsBullet)
{
_flags |= BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
_flags |= BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
_flags |= BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
_flags |= BodyFlags.Sleep;
}
_world = world;
_xf.Position = bd.Position;
_xf.R.Set(bd.Angle);
_sweep.LocalCenter.SetZero();
_sweep.T0 = 1.0f;
_sweep.A0 = _sweep.A = bd.Angle;
_sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter);
_jointList = null;
_contactList = null;
_prev = null;
_next = null;
_linearVelocity = bd.LinearVelocity;
_angularVelocity = bd.AngularVelocity;
_linearDamping = bd.LinearDamping;
_angularDamping = bd.AngularDamping;
_force.Set(0.0f, 0.0f);
_torque = 0.0f;
_sleepTime = 0.0f;
_mass = 0;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
_type = BodyType.Static;
_userData = bd.UserData;
_fixtureList = null;
_fixtureCount = 0;
}
public Vector2 TransformPoint(Vector2 vector)
{
#if USE_MATRIX_FOR_ROTATION
return(position + Math.Mul(rotation, vector));
#else
return(position + (rotation.Xyz * vector.ToVector3()).ToVector2());
#endif
}
// <summary>
// Transforms direction from local space to world space.
// </summary>
public Vector2 TransformDirection(Vector2 vector)
{
#if USE_MATRIX_FOR_ROTATION
return(Math.Mul(rotation, vector));
#else
return((rotation.Xyz * vector.ToVector3()).ToVector2());
#endif
}
public static Vector2 Mul(Transform T, Vector2 v)
{
#if USE_MATRIX_FOR_ROTATION
return(T.position + Math.Mul(T.R, v));
#else
return(T.position + T.TransformDirection(v));
#endif
}
public override void ComputeAABB(out AABB aabb, ref Transform transform)
{
aabb = new AABB();
Vec2 p = transform.Position + Math.Mul(transform.R, _p);
aabb.LowerBound.Set(p.X - _radius, p.Y - _radius);
aabb.UpperBound.Set(p.X + _radius, p.Y + _radius);
}
/// Set the mass properties to override the mass properties of the fixtures.
/// Note that this changes the center of mass position. You can make the body
/// static by using zero mass.
/// Note that creating or destroying fixtures can also alter the mass.
/// @warning The supplied rotational inertia is assumed to be relative to the center of mass.
/// @param massData the mass properties.
// TODO ERIN adjust linear velocity and torque to account for movement of center.
public void SetMassData(MassData massData)
{
Box2DXDebug.Assert(_world.IsLocked() == false);
if (_world.IsLocked() == true)
{
return;
}
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
_mass = massData.Mass;
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
}
if (massData.I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
_I = massData.I - _mass * Vec2.Dot(massData.Center, massData.Center);
_invI = 1.0f / _I;
}
// Move center of mass.
Vec2 oldCenter = _sweep.C;
_sweep.LocalCenter = massData.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;
}
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();
}
}
}
internal void SynchronizeFixtures()
{
Transform xf1 = new Transform();
xf1.R.Set(_sweep.A0);
xf1.Position = _sweep.C0 - Math.Mul(xf1.R, _sweep.LocalCenter);
BroadPhase broadPhase = _world._contactManager._broadPhase;
for (Fixture f = _fixtureList; f != null; f = f._next)
{
f.Synchronize(broadPhase, xf1, _xf);
}
}
public override void Step(Settings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceOutput output;
Collision.Distance(out output, ref cache, ref input, _polygonA, _polygonB);
StringBuilder strBld = new StringBuilder();
strBld.AppendFormat("distance = {0}", new object[] { output.Distance });
OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
_textLine += 15;
strBld = new StringBuilder();
strBld.AppendFormat("iterations = {0}", new object[] { output.Iterations });
OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
int i;
for (i = 0; i < _polygonA.VertexCount; ++i)
{
_dv[i] = Math.Mul(_transformA, _polygonA.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color);
for (i = 0; i < _polygonB.VertexCount; ++i)
{
_dv[i] = Math.Mul(_transformB, _polygonB.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonB.VertexCount, color);
}
Vec2 x1 = output.PointA;
Vec2 x2 = output.PointB;
OpenGLDebugDraw.DrawPoint(x1, 4.0f, new Color(1, 0, 0));
OpenGLDebugDraw.DrawSegment(x1, x2, new Color(1, 1, 0));
OpenGLDebugDraw.DrawPoint(x2, 4.0f, new Color(1, 0, 0));
}
public override void ComputeAABB(out AABB aabb, ref Transform xf)
{
Vec2 lower = Math.Mul(xf, Vertices[0]);
Vec2 upper = lower;
for (int i = 1; i < VertexCount; ++i)
{
Vec2 v = Math.Mul(xf, Vertices[i]);
lower = Math.Min(lower, v);
upper = Math.Max(upper, v);
}
Vec2 r = new Vec2(_radius, _radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
}
public void GetXForm(out XForm xf, float t)
{
xf = default(XForm);
if (1f - this.T0 > Math.FLOAT32_EPSILON)
{
float num = (t - this.T0) / (1f - this.T0);
xf.Position = (1f - num) * this.C0 + num * this.C;
float angle = (1f - num) * this.A0 + num * this.A;
xf.R.Set(angle);
}
else
{
xf.Position = this.C;
xf.R.Set(this.A);
}
xf.Position -= Math.Mul(xf.R, this.LocalCenter);
}
public float T0; //time interval = [T0,1], where T0 is in [0,1]
/// <summary>
/// Get the interpolated transform at a specific time.
/// </summary>
/// <param name="t">The normalized time in [0,1].</param>
public void GetXForm(out XForm xf, float t)
{
xf = new XForm();
// center = p + R * LocalCenter
if (1.0f - T0 > Math.FLOAT32_EPSILON)
{
float alpha = (t - T0) / (1.0f - T0);
xf.Position = (1.0f - alpha) * C0 + alpha * C;
float angle = (1.0f - alpha) * A0 + alpha * A;
xf.R.Set(angle);
}
else
{
xf.Position = C;
xf.R.Set(A);
}
// Shift to origin
xf.Position -= Math.Mul(xf.R, LocalCenter);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override void RayCast(out RayCastOutput output, ref RayCastInput input, Transform transform)
{
output = new RayCastOutput();
Vec2 position = transform.Position + Math.Mul(transform.R, _p);
Vec2 s = input.P1 - position;
float b = Vec2.Dot(s, s) - _radius * _radius;
// Solve quadratic equation.
Vec2 r = input.P2 - input.P1;
float c = Vec2.Dot(s, r);
float rr = Vec2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.FLT_EPSILON)
{
output.Hit = false;
return;
}
// Find the point of intersection of the line with the circle.
float a = -(c + Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Hit = true;
output.Fraction = a;
output.Normal = s + a * r;
output.Normal.Normalize();
return;
}
output.Hit = false;
return;
}
public void SetTransform(Vec2 position, float angle)
{
Box2DXDebug.Assert(_world.IsLocked() == false);
if (_world.IsLocked() == true)
{
return;
}
_xf.R.Set(angle);
_xf.Position = position;
_sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter);
_sweep.A0 = _sweep.A = angle;
BroadPhase broadPhase = _world._contactManager._broadPhase;
for (Fixture f = _fixtureList; f != null; f = f._next)
{
f.Synchronize(broadPhase, _xf, _xf);
}
_world._contactManager.FindNewContacts();
}
/// This resets the mass properties to the sum of the mass properties of the fixtures.
/// This normally does not need to be called unless you called SetMassData to override
/// the mass and you later want to reset the mass.
public void ResetMass()
{
// Compute mass data from shapes. Each shape has its own density.
_mass = 0.0f;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
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);
// Determine the new body type.
BodyType oldType = _type;
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
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();
}
}
}
public override void RayCast(out RayCastOutput output, ref RayCastInput input, Transform xf)
{
output = new RayCastOutput();
float lower = 0.0f, upper = input.MaxFraction;
// Put the ray into the polygon's frame of reference.
Vec2 p1 = Math.MulT(xf.R, input.P1 - xf.Position);
Vec2 p2 = Math.MulT(xf.R, input.P2 - xf.Position);
Vec2 d = p2 - p1;
int index = -1;
output.Hit = false;
for (int i = 0; i < VertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vec2.Dot(Normals[i], Vertices[i] - p1);
float denominator = Vec2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return;
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Hit = true;
output.Fraction = lower;
output.Normal = Math.Mul(xf.R, Normals[index]);
return;
}
}
internal void SynchronizeTransform()
{
_xf.R.Set(_sweep.A);
_xf.Position = _sweep.C - Math.Mul(_xf.R, _sweep.LocalCenter);
}
public Vector2 TransformDirection(Vector2 vector)
{
return(Math.Mul(R, vector));
}
public static Vec2 Mul(XForm T, Vec2 v)
{
return(T.Position + Math.Mul(T.R, v));
}
public Vector2 TransformPoint(Vector2 vector)
{
return(position + Math.Mul(R, vector));
}