public void TestRaycastPolygon()
{
// lines
Vector2[][] lines =
{
new[] { new Vector2(-20.02394f, -5.17816f), new Vector2(-15.39518f, -10.87007f) },
new[] { new Vector2(-15.39518f, -10.87007f), new Vector2(-20.02394f, -5.17816f) },
new[] { new Vector2(-5.008571f, -5.03002f), new Vector2(-15.0059f, -25.00544f) }
};
Vector2[] intrDot =
{
new Vector2(-17.85f, -7.86f),
new Vector2(-17.85f, -7.86f),
new Vector2(-6.6701f, -8.35f)
};
Vector2[] polPoints =
{
new Vector2(-20.001f, -10.0002f), new Vector2(-15.001f, -5.0234f),
new Vector2(-10.0231f, -5.002f), new Vector2(-5.003f, -10.02234f),
new Vector2(-5.10021f, -15.0091f), new Vector2(-10.000001f, -20.00002f),
new Vector2(-15.10221f, -20.0056f), new Vector2(-20.0034f, -15.1234f)
};
// True fraction data
float[] trueFraction = { 0.4705673f, 0.5294338f, 0.1662021f };
//Create circle obj
PolygonShape polShape = new PolygonShape(polPoints);
Collision.Transform polTransform =
new Collision.Transform(new Vector2(0, 0), 0 * Mathf.Deg2Rad);
for (var obj = 0; obj < lines.Length; obj++)
{
float fraction;
Vector2 normal;
bool rayCast = polShape.RayCast(polTransform, lines[obj][0], lines[obj][1], out fraction,
out normal);
Assert.That(rayCast, "RayCast to Polygon test failed. Result {0} is incorrect", obj);
Vector2 point = lines[obj][0] + (lines[obj][1] - lines[obj][0]) * fraction;
string err =
String.Format(
"RayCast to Polygon test({0}) failed; Intersect: trueFraction {1} (testFraction: {2}), intrPoint({3}), testPoint {4}, normal {5}",
obj, trueFraction[obj], fraction, intrDot[obj], point, normal);
Assert.That(Mathf.FloatEquals(fraction, trueFraction[obj], 0.005f), err);
}
}
public override void Step(TestSettings settings)
{
bool advanceRay = settings.pause == false || settings.singleStep;
base.Step(settings);
m_debugDraw.DrawString("Press 1-5 to drop stuff, m to change the mode");
switch (m_mode)
{
case Mode.e_closest:
m_debugDraw.DrawString("Ray-cast mode: closest - find closest fixture along the ray");
break;
case Mode.e_any:
m_debugDraw.DrawString("Ray-cast mode: any - check for obstruction");
break;
case Mode.e_multiple:
m_debugDraw.DrawString("Ray-cast mode: multiple - gather multiple fixtures");
break;
}
float L = 11.0f;
Vec2 point1 = new Vec2(0.0f, 10.0f);
Vec2 d = new Vec2(L * (float)Math.Cos(m_angle), L * (float)Math.Sin(m_angle));
Vec2 point2 = point1 + d;
if (m_mode == Mode.e_closest)
{
RayCastClosestCallback callback = new RayCastClosestCallback();
m_world.RayCast(callback, point1, point2);
if (callback.m_hit)
{
m_debugDraw.DrawPoint(callback.m_point, 5.0f, Color.FromArgb(100, 225, 100));
m_debugDraw.DrawSegment(point1, callback.m_point, Color.FromArgb(200, 200, 200));
Vec2 head = callback.m_point + 0.5f * callback.m_normal;
m_debugDraw.DrawSegment(callback.m_point, head, Color.FromArgb(225, 225, 100));
}
else
{
m_debugDraw.DrawSegment(point1, point2, Color.FromArgb(200, 200, 200));
}
}
else if (m_mode == Mode.e_any)
{
RayCastAnyCallback callback = new RayCastAnyCallback();
m_world.RayCast(callback, point1, point2);
if (callback.m_hit)
{
m_debugDraw.DrawPoint(callback.m_point, 5.0f, Color.FromArgb(100, 225, 100));
m_debugDraw.DrawSegment(point1, callback.m_point, Color.FromArgb(200, 200, 200));
Vec2 head = callback.m_point + 0.5f * callback.m_normal;
m_debugDraw.DrawSegment(callback.m_point, head, Color.FromArgb(225, 225, 100));
}
else
{
m_debugDraw.DrawSegment(point1, point2, Color.FromArgb(200, 200, 200));
}
}
else if (m_mode == Mode.e_multiple)
{
RayCastMultipleCallback callback = new RayCastMultipleCallback();
m_world.RayCast(callback, point1, point2);
m_debugDraw.DrawSegment(point1, point2, Color.FromArgb(200, 200, 200));
for (int i = 0; i < callback.m_count; ++i)
{
Vec2 p = callback.m_points[i];
Vec2 n = callback.m_normals[i];
m_debugDraw.DrawPoint(p, 5.0f, Color.FromArgb(100, 225, 100));
m_debugDraw.DrawSegment(point1, p, Color.FromArgb(200, 200, 200));
Vec2 head = p + 0.5f * n;
m_debugDraw.DrawSegment(p, head, Color.FromArgb(225, 225, 100));
}
}
if (advanceRay)
{
m_angle += 0.25f * (float)Math.PI / 180.0f;
}
#if ZERO
// This case was failing.
{
Vec2[] vertices = new Vec2[4];
//vertices[0].Set(-22.875f, -3.0f);
//vertices[1].Set(22.875f, -3.0f);
//vertices[2].Set(22.875f, 3.0f);
//vertices[3].Set(-22.875f, 3.0f);
PolygonShape shape = new PolygonShape();
//shape.Set(vertices, 4);
shape.SetAsBox(22.875f, 3.0f);
RayCastInput input;
input.p1.Set(10.2725f, 1.71372f);
input.p2.Set(10.2353f, 2.21807f);
//input.maxFraction = 0.567623f;
input.maxFraction = 0.56762173f;
Transform xf;
xf.SetIdentity();
xf.position.Set(23.0f, 5.0f);
RayCastOutput output;
bool hit;
hit = shape.RayCast(out output, input, xf);
hit = false;
Color color(1.0f, 1.0f, 1.0f);
Vec2[] vs = new Vec2[4];
for (int i = 0; i < 4; ++i)
{
vs[i] = Utilities.Mul(xf, shape.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vs, 4, color);
m_debugDraw.DrawSegment(input.p1, input.p2, color);
}
#endif
}
