public Microsoft.Xna.Framework.Vector2 ChangeRotation(VelcroPhysics.Dynamics.Body body, Microsoft.Xna.Framework.Vector2 src)
{
VelcroPhysics.Shared.Transform xf;
body.GetTransform(out xf);
src = VelcroPhysics.Utilities.MathUtils.Mul(ref xf, src);
return(src);
}
protected void DrawBox2DBodyGizmos(VelcroPhysics.Dynamics.World b2World)
{
for (int ib = 0, count = b2World.BodyList.Count; ib < count; ++ib)
{
VelcroPhysics.Dynamics.Body body = b2World.BodyList[ib];
if (null == body)
{
continue;
}
Color fixtureColor;
if (body.IsStatic)
{
fixtureColor = new Color(1, 0, 1);
}
else
{
if (body.BodyType == VelcroPhysics.Dynamics.BodyType.Kinematic)
{
fixtureColor = Color.yellow;
}
else
{
fixtureColor = Color.green;
}
if ((body._flags & VelcroPhysics.Dynamics.BodyFlags.AwakeFlag) == 0)
{
fixtureColor = Color.gray * 0.5f + fixtureColor * 0.5f;
}
}
Microsoft.Xna.Framework.Vector2 center = body.WorldCenter;
for (int ifix = 0, countFix = body.FixtureList.Count; ifix < countFix; ++ifix)
{
VelcroPhysics.Dynamics.Fixture fixture = body.FixtureList[ifix];
if (fixture.Friction > 5f)
{
fixtureColor.b = 1.0f;
}
if (fixture.Shape.ShapeType == VelcroPhysics.Collision.Shapes.ShapeType.Circle)
{
float radius = fixture.Shape.Radius * GlobalVarFun.DisplayToPhysicUnitRatio;
Vector3 v3Center = GlobalVarFun.GetDisplayPosition(center.X, center.Y);
Gizmos.color = fixtureColor;
Gizmos.DrawWireSphere(v3Center, radius);
}
else if (fixture.Shape.ShapeType == VelcroPhysics.Collision.Shapes.ShapeType.Polygon)
{
var shape = fixture.Shape as VelcroPhysics.Collision.Shapes.PolygonShape;
int vertexCount = shape.Vertices.Count;
for (int iv = 0; iv < vertexCount; ++iv)
{
Microsoft.Xna.Framework.Vector2 src = (shape.Vertices[(iv) % vertexCount]);
Microsoft.Xna.Framework.Vector2 des = (shape.Vertices[(iv + 1) % vertexCount]);
src = ChangeRotation(body, src);
des = ChangeRotation(body, des);
Vector3 v3Src = GlobalVarFun.GetDisplayPosition(src.X, src.Y);
Vector3 v3Des = GlobalVarFun.GetDisplayPosition(des.X, des.Y);
v3Src.z = 0;
v3Des.z = 0;
Gizmos.color = fixtureColor;
Gizmos.DrawLine(v3Src, v3Des);
}
}
else if (fixture.Shape.ShapeType == VelcroPhysics.Collision.Shapes.ShapeType.Edge)
{
var shape = fixture.Shape as VelcroPhysics.Collision.Shapes.EdgeShape;
Microsoft.Xna.Framework.Vector2 startPos = shape._vertex1;
Microsoft.Xna.Framework.Vector2 endPos = shape._vertex2;
Microsoft.Xna.Framework.Vector2 src = startPos;
Microsoft.Xna.Framework.Vector2 des = endPos;
src = ChangeRotation(body, src);
des = ChangeRotation(body, des);
Vector3 v3Src = GlobalVarFun.GetDisplayPosition(src.X, src.Y);
Vector3 v3Des = GlobalVarFun.GetDisplayPosition(des.X, des.Y);
v3Src.z = 0;
v3Des.z = 0;
Gizmos.color = fixtureColor;
Gizmos.DrawLine(v3Src, v3Des);
}
}
}
}
