public static Vec2 Cross(float s, Vec2 a)
{
Vec2 result = default(Vec2);
result.Set(-s * a.Y, s * a.X);
return(result);
}
/// <summary>
/// Multiply a matrix times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another.
/// </summary>
public static Vec2 Mul(Mat22 A, Vec2 v)
{
Vec2 u = new Vec2();
u.Set(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y);
return(u);
}
public static Vec2 Abs(Vec2 a)
{
Vec2 b = new Vec2();
b.Set(Math.Abs(a.X), Math.Abs(a.Y));
return(b);
}
public static Vec2 Mul(Mat22 A, Vec2 v)
{
Vec2 result = default(Vec2);
result.Set(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y);
return(result);
}
/// <summary>
/// Perform the cross product on a vector and a scalar.
/// In 2D this produces a vector.
/// </summary>
public static Vec2 Cross(Vec2 a, float s)
{
Vec2 v = new Vec2();
v.Set(s * a.Y, -s * a.X);
return(v);
}
public static Vec2 MulT(Mat22 A, Vec2 v)
{
Vec2 result = default(Vec2);
result.Set(Vec2.Dot(v, A.Col1), Vec2.Dot(v, A.Col2));
return(result);
}
public static Vec2 operator *(Vec2 v1, float a)
{
Vec2 result = default(Vec2);
result.Set(v1.X * a, v1.Y * a);
return(result);
}
/// <summary>
/// Perform the cross product on a scalar and a vector.
/// In 2D this produces a vector.
/// </summary>
public static Vec2 Cross(float s, Vec2 a)
{
Vec2 v = new Vec2();
v.Set(-s * a.Y, s * a.X);
return(v);
}
public static Vec2 operator -(Vec2 v1)
{
Vec2 result = default(Vec2);
result.Set(-v1.X, -v1.Y);
return(result);
}
public static Vec2 operator -(Vec2 v1, Vec2 v2)
{
Vec2 result = default(Vec2);
result.Set(v1.X - v2.X, v1.Y - v2.Y);
return(result);
}
public static Vec2 Cross(Vec2 a, float s)
{
Vec2 result = default(Vec2);
result.Set(s * a.Y, -s * a.X);
return(result);
}
/// <summary>
/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another (inverse transform).
/// </summary>
public static Vec2 MulT(Mat22 A, Vec2 v)
{
Vec2 u = new Vec2();
u.Set(Vec2.Dot(v, A.Col1), Vec2.Dot(v, A.Col2));
return(u);
}
/// <summary>
/// Negate this vector.
/// </summary>
public static Vec2 operator -(Vec2 v1)
{
Vec2 v = new Vec2();
v.Set(-v1.X, -v1.Y);
return(v);
}
public static Vec2 operator -(Vec2 v1, Vec2 v2)
{
Vec2 v = new Vec2();
v.Set(v1.X - v2.X, v1.Y - v2.Y);
return(v);
}
public static Vec2 operator *(float a, Vec2 v1)
{
Vec2 v = new Vec2();
v.Set(v1.X * a, v1.Y * a);
return(v);
}
public static Vec2 Abs(Vec2 a)
{
Vec2 result = default(Vec2);
result.Set(Math.Abs(a.X), Math.Abs(a.Y));
return(result);
}
public static Mat22 MulT(Mat22 A, Mat22 B)
{
Vec2 c = default(Vec2);
c.Set(Vec2.Dot(A.Col1, B.Col1), Vec2.Dot(A.Col2, B.Col1));
Vec2 c2 = default(Vec2);
c2.Set(Vec2.Dot(A.Col1, B.Col2), Vec2.Dot(A.Col2, B.Col2));
Mat22 result = default(Mat22);
result.Set(c, c2);
return(result);
}
/// <summary>
/// A^T * B
/// </summary>
public static Mat22 MulT(Mat22 A, Mat22 B)
{
Vec2 c1 = new Vec2();
c1.Set(Vec2.Dot(A.Col1, B.Col1), Vec2.Dot(A.Col2, B.Col1));
Vec2 c2 = new Vec2();
c2.Set(Vec2.Dot(A.Col1, B.Col2), Vec2.Dot(A.Col2, B.Col2));
Mat22 C = new Mat22();
C.Set(c1, c2);
return(C);
}
/// <summary>
/// This constructor sets the body definition default values.
/// </summary>
public BodyDef()
{
MassData = new MassData();
MassData.Center.SetZero();
MassData.Mass = 0.0f;
MassData.I = 0.0f;
UserData = null;
Position = new Vec2();
Position.Set(0.0f, 0.0f);
Angle = 0.0f;
LinearDamping = 0.0f;
AngularDamping = 0.0f;
AllowSleep = true;
IsSleeping = false;
FixedRotation = false;
IsBullet = false;
}
public static Vec2 ComputeCentroid(Vec2[] vs, int count)
{
Box2DXDebug.Assert(count >= 2);
Vec2 c = new Vec2(); c.Set(0.0f, 0.0f);
float area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 pRef = new Vec2(0.0f, 0.0f);
#if O
// This code would put the reference point inside the polygon.
for (int i = 0; i < count; ++i)
{
pRef += vs[i];
}
pRef *= 1.0f / count;
#endif
const float inv3 = 1.0f / 3.0f;
for (int i = 0; i < count; ++i)
{
// Triangle vertices.
Vec2 p1 = pRef;
Vec2 p2 = vs[i];
Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0];
Vec2 e1 = p2 - p1;
Vec2 e2 = p3 - p1;
float D = Vec2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
Box2DXDebug.Assert(area > Settings.FLT_EPSILON);
c *= 1.0f / area;
return c;
}
public static Vec2 operator -(Vec2 v1, Vec2 v2)
{
Vec2 v = new Vec2();
v.Set(v1.X - v2.X, v1.Y - v2.Y);
return v;
}
public override void ComputeMass(out MassData massData)
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
Box2DXDebug.Assert(_vertexCount >= 3);
Vec2 center = new Vec2();
center.Set(0.0f, 0.0f);
float area = 0.0f;
float I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 pRef = new Vec2(0.0f, 0.0f);
#if O
// This code would put the reference point inside the polygon.
for (int i = 0; i < _vertexCount; ++i)
{
pRef += _vertices[i];
}
pRef *= 1.0f / count;
#endif
float k_inv3 = 1.0f / 3.0f;
for (int i = 0; i < _vertexCount; ++i)
{
// Triangle vertices.
Vec2 p1 = pRef;
Vec2 p2 = _vertices[i];
Vec2 p3 = i + 1 < _vertexCount ? _vertices[i + 1] : _vertices[0];
Vec2 e1 = p2 - p1;
Vec2 e2 = p3 - p1;
float D = Vec2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (p1 + p2 + p3);
float px = p1.X, py = p1.Y;
float ex1 = e1.X, ey1 = e1.Y;
float ex2 = e2.X, ey2 = e2.Y;
float intx2 = k_inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) + 0.5f * px * px;
float inty2 = k_inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) + 0.5f * py * py;
I += D * (intx2 + inty2);
}
// Total mass
massData.Mass = _density * area;
// Center of mass
Box2DXDebug.Assert(area > Common.Settings.FLT_EPSILON);
center *= 1.0f / area;
massData.Center = center;
// Inertia tensor relative to the local origin.
massData.I = _density * I;
}
/// <summary>
/// This constructor sets the body definition default values.
/// </summary>
public BodyDef()
{
UserData = null;
Position = new Vec2();
Position.Set(0.0f, 0.0f);
Angle = 0.0f;
LinearVelocity = new Vec2(0f, 0f);
AngularVelocity = 0.0f;
LinearDamping = 0.0f;
AngularDamping = 0.0f;
AllowSleep = true;
IsSleeping = false;
FixedRotation = false;
IsBullet = false;
}
private void MoveAABB(ref AABB aabb)
{
Vec2 d = new Vec2();
d.X = Box2DXMath.Random(-0.5f, 0.5f);
d.Y = Box2DXMath.Random(-0.5f, 0.5f);
//d.x = 2.0f;
//d.y = 0.0f;
aabb.LowerBound += d;
aabb.UpperBound += d;
Vec2 c0 = 0.5f * (aabb.LowerBound + aabb.UpperBound);
Vec2 min = new Vec2(); min.Set(-k_extent, 0.0f);
Vec2 max = new Vec2(); max.Set(k_extent, 2.0f * k_extent);
Vec2 c = Box2DXMath.Clamp(c0, min, max);
aabb.LowerBound += c - c0;
aabb.UpperBound += c - c0;
}
private void GetRandomAABB(ref AABB aabb)
{
Vec2 w = new Vec2(); w.Set(k_width, k_width);
aabb.LowerBound.X = Box2DXMath.Random(-k_extent, k_extent);
aabb.LowerBound.Y = Box2DXMath.Random(0.0f, 2.0f * k_extent);
aabb.UpperBound = aabb.LowerBound + w;
}
public Test()
{
_worldAABB = new AABB();
_worldAABB.LowerBound.Set(-200.0f, -100.0f);
_worldAABB.UpperBound.Set(200.0f, 200.0f);
Vec2 gravity = new Vec2();
gravity.Set(0.0f, -10.0f);
bool doSleep = true;
_world = new World(_worldAABB, gravity, doSleep);
_bomb = null;
_textLine = 30;
_mouseJoint = null;
_pointCount = 0;
_destructionListener.test = this;
_boundaryListener.test = this;
//_contactListener.test = this;
_world.SetDestructionListener(_destructionListener);
_world.SetBoundaryListener(_boundaryListener);
//_world.SetContactListener(_contactListener);
_world.SetDebugDraw(_debugDraw);
}
/// <summary>
/// Perform the cross product on a scalar and a vector.
/// In 2D this produces a vector.
/// </summary>
public static Vec2 Cross(float s, Vec2 a)
{
Vec2 v = new Vec2();
v.Set(-s * a.Y, s * a.X);
return v;
}
public void MouseDown(Vec2 p)
{
_mouseWorld = p;
if (_mouseJoint != null)
{
return;
}
// Make a small box.
AABB aabb = new AABB();
Vec2 d = new Vec2();
d.Set(0.001f, 0.001f);
aabb.LowerBound = p - d;
aabb.UpperBound = p + d;
// Query the world for overlapping shapes.
MyQueryCallback callback = new MyQueryCallback(p);
_world.QueryAABB(callback, aabb);
if (callback._fixture != null)
{
Body body = callback._fixture.GetBody();
MouseJointDef md = new MouseJointDef();
md.Body1 = _groundBody;
md.Body2 = body;
md.Target = p;
#if TARGET_FLOAT32_IS_FIXED
md.maxForce = (body->GetMass() < 16.0)? (1000.0f * body->GetMass()) : float32(16000.0);
#else
md.MaxForce = 1000.0f * body.GetMass();
#endif
_mouseJoint = (MouseJoint)_world.CreateJoint(md);
body.WakeUp();
}
}
/// <summary>
/// Mouse interaction event.
/// </summary>
public void MouseDown(Vec2 p)
{
if (_mouseJoint != null)
{
return;
}
// Make a small box.
AABB aabb = new AABB();
Vec2 d = new Vec2();
d.Set(0.001f, 0.001f);
aabb.LowerBound = p - d;
aabb.UpperBound = p + d;
// Query the world for overlapping shapes.
int k_maxCount = 10;
Shape[] shapes = new Shape[k_maxCount];
int count = _world.Query(aabb, shapes, k_maxCount);
Body body = null;
for (int i = 0; i < count; ++i)
{
Body shapeBody = shapes[i].GetBody();
if (shapeBody.IsStatic() == false && shapeBody.GetMass() > 0.0f)
{
bool inside = shapes[i].TestPoint(shapeBody.GetXForm(), p);
if (inside)
{
body = shapes[i].GetBody();
break;
}
}
}
if (body != null)
{
MouseJointDef md = new MouseJointDef();
md.Body1 = _world.GetGroundBody();
md.Body2 = body;
md.Target = p;
md.MaxForce = 1000.0f * body.GetMass();
md.FrequencyHz = 30f;
_mouseJoint = (MouseJoint)_world.CreateJoint(md);
body.WakeUp();
}
}
/// <summary>
/// Create an empty Box2D world.
/// </summary>
private World CreateBox2DWorld()
{
AABB worldAABB = new AABB();
worldAABB.LowerBound.Set(_simParams._lowerBoundPhysics.X, _simParams._lowerBoundPhysics.Y);
worldAABB.UpperBound.Set(_simParams._upperBoundPhysics.X, _simParams._upperBoundPhysics.Y);
Vec2 gravity = new Vec2();
gravity.Set(0.0f, _simParams._gravity);
bool doSleep = false;
World world = new World(worldAABB, gravity, doSleep);
return world;
}
public Dominos()
{
Body b1;
{
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
BodyDef bd = new BodyDef();
b1 = _world.CreateBody(bd);
b1.CreateFixture(shape,0);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(6.0f, 0.25f);
BodyDef bd = new BodyDef();
bd.Position.Set(-1.5f, 10.0f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(shape,0);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.1f, 1.0f);
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 20.0f;
fd.Friction = 0.1f;
for (int i = 0; i < 10; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(-6.0f + 1.0f * i, 11.25f);
Body body = _world.CreateBody(bd);
body.CreateFixture(fd);
}
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(7.0f, 0.25f, Vec2.Zero, 0.3f);
BodyDef bd = new BodyDef();
bd.Position.Set(1.0f, 6.0f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(shape,0);
}
Body b2;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.25f, 1.5f);
BodyDef bd = new BodyDef();
bd.Position.Set(-7.0f, 4.0f);
b2 = _world.CreateBody(bd);
b2.CreateFixture(shape,0);
}
Body b3;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(6.0f, 0.125f);
BodyDef bd = new BodyDef();
bd.Position.Set(-0.9f, 1.0f);
bd.Angle = -0.15f;
b3 = _world.CreateBody(bd);
b3.CreateFixture(shape, 10.0f);
}
RevoluteJointDef jd = new RevoluteJointDef();
Vec2 anchor = new Vec2();
anchor.Set(-2.0f, 1.0f);
jd.Initialize(b1, b3, anchor);
jd.CollideConnected = true;
_world.CreateJoint(jd);
Body b4;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.25f, 0.25f);
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f, 15.0f);
b4 = _world.CreateBody(bd);
b4.CreateFixture(shape, 10.0f);
}
anchor.Set(-7.0f, 15.0f);
jd.Initialize(b2, b4, anchor);
_world.CreateJoint(jd);
Body b5;
{
BodyDef bd = new BodyDef();
bd.Position.Set(6.5f, 3.0f);
b5 = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 10.0f;
fd.Friction = 0.1f;
shape.SetAsBox(1.0f, 0.1f,new Vec2(0.0f, -0.9f), 0.0f);
b5.CreateFixture(fd);
shape.SetAsBox(0.1f, 1.0f, new Vec2(-0.9f, 0.0f), 0.0f);
b5.CreateFixture(fd);
shape.SetAsBox(0.1f, 1.0f,new Vec2(0.9f, 0.0f), 0.0f);
b5.CreateFixture(fd);
}
anchor.Set(6.0f, 2.0f);
jd.Initialize(b1, b5, anchor);
_world.CreateJoint(jd);
Body b6;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(1.0f, 0.1f);
BodyDef bd = new BodyDef();
bd.Position.Set(6.5f, 4.1f);
b6 = _world.CreateBody(bd);
b6.CreateFixture(shape, 30.0f);
}
anchor.Set(7.5f, 4.0f);
jd.Initialize(b5, b6, anchor);
_world.CreateJoint(jd);
Body b7;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.1f, 1.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(7.4f, 1.0f);
b7 = _world.CreateBody(bd);
b7.CreateFixture(shape, 10.0f);
}
DistanceJointDef djd =new DistanceJointDef();
djd.Body1 = b3;
djd.Body2 = b7;
djd.LocalAnchor1.Set(6.0f, 0.0f);
djd.LocalAnchor2.Set(0.0f, -1.0f);
Vec2 d = djd.Body2.GetWorldPoint(djd.LocalAnchor2) - djd.Body1.GetWorldPoint(djd.LocalAnchor1);
djd.Length = d.Length();
_world.CreateJoint(djd);
{
float radius = 0.2f;
CircleShape shape = new CircleShape();
shape._radius = radius;
for (int i = 0; i < 4; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(5.9f + 2.0f * radius * i, 2.4f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 10.0f);
}
}
}
public void MouseDown(Vec2 p)
{
if (_mouseJoint != null)
{
return;
}
// Make a small box.
AABB aabb = new AABB();
Vec2 d = new Vec2();
d.Set(0.001f, 0.001f);
aabb.LowerBound = p - d;
aabb.UpperBound = p + d;
// Query the world for overlapping shapes.
int k_maxCount = 10;
Fixture[] shapes = new Fixture[k_maxCount];
int count = _world.Query(aabb, shapes, k_maxCount);
Body body = null;
for (int i = 0; i < count; ++i)
{
Body shapeBody = shapes[i].Body;
if (shapeBody.IsStatic() == false && shapeBody.GetMass() > 0.0f)
{
bool inside = shapes[i].TestPoint(p);
if (inside)
{
body = shapes[i].Body;
break;
}
}
}
if (body != null)
{
MouseJointDef md = new MouseJointDef();
md.Body1 = _world.GetGroundBody();
md.Body2 = body;
md.Target = p;
#if TARGET_FLOAT32_IS_FIXED
md.MaxForce = (body.GetMass() < 16.0f)?
(1000.0f * body.GetMass()) : 16000.0f;
#else
md.MaxForce = 1000.0f * body.GetMass();
#endif
_mouseJoint = (MouseJoint)_world.CreateJoint(md);
body.WakeUp();
}
}
public static Vec2 operator *(float a, Vec2 v1)
{
Vec2 v = new Vec2();
v.Set(v1.X * a, v1.Y * a);
return v;
}
/// <summary>
/// Perform the cross product on a vector and a scalar.
/// In 2D this produces a vector.
/// </summary>
public static Vec2 Cross(Vec2 a, float s)
{
Vec2 v = new Vec2();
v.Set(s * a.Y, -s * a.X);
return v;
}
public Test()
{
Vec2 gravity = new Vec2();
gravity.Set(0.0f, -10.0f);
bool doSleep = true;
_world = new World(gravity, doSleep);
_bomb = null;
_textLine = 30;
_mouseJoint = null;
_pointCount = 0;
_destructionListener.test = this;
_world.SetDestructionListener(_destructionListener);
_world.SetContactListener(this);
_world.SetDebugDraw(_debugDraw);
_bombSpawning = false;
_stepCount = 0;
BodyDef bodyDef = new BodyDef();
_groundBody = _world.CreateBody(bodyDef);
}
/// <summary>
/// Negate this vector.
/// </summary>
public static Vec2 operator -(Vec2 v1)
{
Vec2 v = new Vec2();
v.Set(-v1.X, -v1.Y);
return v;
}
private void DrawDebugData()
{
if (_debugDraw == null)
{
return;
}
DebugDraw.DrawFlags flags = _debugDraw.Flags;
if ((flags & DebugDraw.DrawFlags.Shape) != 0)
{
bool core = (flags & DebugDraw.DrawFlags.CoreShape) == DebugDraw.DrawFlags.CoreShape;
for (Body b = _bodyList; b != null; b = b.GetNext())
{
XForm xf = b.GetXForm();
for (Shape s = b.GetShapeList(); s != null; s = s.GetNext())
{
if (b.IsStatic())
{
DrawShape(s, xf, new Color(0.5f, 0.9f, 0.5f), core);
}
else if (b.IsSleeping())
{
DrawShape(s, xf, new Color(0.5f, 0.5f, 0.9f), core);
}
else
{
DrawShape(s, xf, new Color(0.9f, 0.9f, 0.9f), core);
}
}
}
}
if ((flags & DebugDraw.DrawFlags.Joint) != 0)
{
for (Joint j = _jointList; j != null; j = j.GetNext())
{
if (j.GetType() != JointType.MouseJoint)
{
DrawJoint(j);
}
}
}
if ((flags & DebugDraw.DrawFlags.Pair) != 0)
{
BroadPhase bp = _broadPhase;
Vec2 invQ = new Vec2();
invQ.Set(1.0f / bp._quantizationFactor.X, 1.0f / bp._quantizationFactor.Y);
Color color = new Color(0.9f, 0.9f, 0.3f);
for (int i = 0; i < PairManager.TableCapacity; ++i)
{
ushort index = bp._pairManager._hashTable[i];
while (index != PairManager.NullPair)
{
Pair pair = bp._pairManager._pairs[index];
Proxy p1 = bp._proxyPool[pair.ProxyId1];
Proxy p2 = bp._proxyPool[pair.ProxyId2];
AABB b1 = new AABB(), b2 = new AABB();
b1.LowerBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p1.LowerBounds[0]].Value;
b1.LowerBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p1.LowerBounds[1]].Value;
b1.UpperBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p1.UpperBounds[0]].Value;
b1.UpperBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p1.UpperBounds[1]].Value;
b2.LowerBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p2.LowerBounds[0]].Value;
b2.LowerBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p2.LowerBounds[1]].Value;
b2.UpperBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p2.UpperBounds[0]].Value;
b2.UpperBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p2.UpperBounds[1]].Value;
Vec2 x1 = 0.5f * (b1.LowerBound + b1.UpperBound);
Vec2 x2 = 0.5f * (b2.LowerBound + b2.UpperBound);
_debugDraw.DrawSegment(x1, x2, color);
index = pair.Next;
}
}
}
if ((flags & DebugDraw.DrawFlags.Aabb) != 0)
{
BroadPhase bp = _broadPhase;
Vec2 worldLower = bp._worldAABB.LowerBound;
Vec2 worldUpper = bp._worldAABB.UpperBound;
Vec2 invQ = new Vec2();
invQ.Set(1.0f / bp._quantizationFactor.X, 1.0f / bp._quantizationFactor.Y);
Color color = new Color(0.9f, 0.3f, 0.9f);
for (int i = 0; i < Settings.MaxProxies; ++i)
{
Proxy p = bp._proxyPool[i];
if (p.IsValid == false)
{
continue;
}
AABB b = new AABB();
b.LowerBound.X = worldLower.X + invQ.X * bp._bounds[0][p.LowerBounds[0]].Value;
b.LowerBound.Y = worldLower.Y + invQ.Y * bp._bounds[1][p.LowerBounds[1]].Value;
b.UpperBound.X = worldLower.X + invQ.X * bp._bounds[0][p.UpperBounds[0]].Value;
b.UpperBound.Y = worldLower.Y + invQ.Y * bp._bounds[1][p.UpperBounds[1]].Value;
Vec2[] vs1 = new Vec2[4];
vs1[0].Set(b.LowerBound.X, b.LowerBound.Y);
vs1[1].Set(b.UpperBound.X, b.LowerBound.Y);
vs1[2].Set(b.UpperBound.X, b.UpperBound.Y);
vs1[3].Set(b.LowerBound.X, b.UpperBound.Y);
_debugDraw.DrawPolygon(vs1, 4, color);
}
Vec2[] vs = new Vec2[4];
vs[0].Set(worldLower.X, worldLower.Y);
vs[1].Set(worldUpper.X, worldLower.Y);
vs[2].Set(worldUpper.X, worldUpper.Y);
vs[3].Set(worldLower.X, worldUpper.Y);
_debugDraw.DrawPolygon(vs, 4, new Color(0.3f, 0.9f, 0.9f));
}
if ((flags & DebugDraw.DrawFlags.Obb) != 0)
{
Color color = new Color(0.5f, 0.3f, 0.5f);
for (Body b = _bodyList; b != null; b = b.GetNext())
{
XForm xf = b.GetXForm();
for (Shape s = b.GetShapeList(); s != null; s = s.GetNext())
{
if (s.GetType() != ShapeType.PolygonShape)
{
continue;
}
PolygonShape poly = (PolygonShape)s;
OBB obb = poly.GetOBB();
Vec2 h = obb.Extents;
Vec2[] vs = new Vec2[4];
vs[0].Set(-h.X, -h.Y);
vs[1].Set(h.X, -h.Y);
vs[2].Set(h.X, h.Y);
vs[3].Set(-h.X, h.Y);
for (int i = 0; i < 4; ++i)
{
vs[i] = obb.Center + Common.Math.Mul(obb.R, vs[i]);
vs[i] = Common.Math.Mul(xf, vs[i]);
}
_debugDraw.DrawPolygon(vs, 4, color);
}
}
}
if ((flags & DebugDraw.DrawFlags.CenterOfMass) != 0)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
XForm xf = b.GetXForm();
xf.Position = b.GetWorldCenter();
_debugDraw.DrawXForm(xf);
}
}
}
