public override void OnRender()
{
var transformA = new Transform {
Position = new Vector2(0.0f, 0.25f)
};
transformA.Rotation.SetIdentity();
var transformB = new Transform();
transformB.SetIdentity();
var input = new ShapeCastInput();
input.ProxyA.Set(_vAs, _countA, _radiusA);
input.ProxyB.Set(_vBs, _countB, _radiusB);
input.TransformA = transformA;
input.TransformB = transformB;
input.TranslationB.Set(8.0f, 0.0f);
var hit = DistanceAlgorithm.ShapeCast(out var output, input);
var transformB2 = new Transform
{
Rotation = transformB.Rotation, Position = transformB.Position + output.Lambda * input.TranslationB
};
var distanceInput = new DistanceInput
{
TransformA = transformA,
TransformB = transformB2,
UseRadii = false
};
distanceInput.ProxyA.Set(_vAs, _countA, _radiusA);
distanceInput.ProxyB.Set(_vBs, _countB, _radiusB);
var simplexCache = new SimplexCache();
DistanceAlgorithm.Distance(out var distanceOutput, ref simplexCache, distanceInput);
DrawString(
$"hit = {hit}, iters = {output.Iterations}, lambda = {output.Lambda}, distance = {distanceOutput.Distance}");
var vertices = new Vector2[Settings.MaxPolygonVertices];
for (var i = 0; i < _countA; ++i)
{
vertices[i] = MathUtils.Mul(transformA, _vAs[i]);
}
//g_debugDraw.DrawCircle(vertices[0], _radiusA, b2Color(0.9f, 0.9f, 0.9f));
Drawer.DrawPolygon(vertices, _countA, Color.FromArgb(230, 230, 230));
for (var i = 0; i < _countB; ++i)
{
vertices[i] = MathUtils.Mul(transformB, _vBs[i]);
}
//g_debugDraw.DrawCircle(vertices[0], _radiusB, b2Color(0.5f, 0.9f, 0.5f));
Drawer.DrawPolygon(vertices, _countB, Color.FromArgb(127, 230, 127));
for (var i = 0; i < _countB; ++i)
{
vertices[i] = MathUtils.Mul(transformB2, _vBs[i]);
}
//g_debugDraw.DrawCircle(vertices[0], _radiusB, b2Color(0.5f, 0.7f, 0.9f));
Drawer.DrawPolygon(vertices, _countB, Color.FromArgb(127, 129, 230));
if (hit)
{
var p1 = output.Point;
Drawer.DrawPoint(p1, 10.0f, Color.FromArgb(230, 77, 77));
var p2 = p1 + output.Normal;
Drawer.DrawSegment(p1, p2, Color.FromArgb(230, 77, 77));
}
}
public override void Update(GameSettings settings, GameTime gameTime)
{
base.Update(settings, gameTime);
ShapeCastInput input = new ShapeCastInput();
input.ProxyA = new DistanceProxy(_vAs, _radiusA);
input.ProxyB = new DistanceProxy(_vBs, _radiusB);
input.TransformA = _transformA;
input.TransformB = _transformB;
input.TranslationB = _translationB;
ShapeCastOutput output;
bool hit = DistanceGJK.ShapeCast(ref input, out output);
Transform transformB2;
transformB2.q = _transformB.q;
transformB2.p = _transformB.p + output.Lambda * input.TranslationB;
DistanceInput distanceInput = new DistanceInput();
distanceInput.ProxyA = new DistanceProxy(_vAs, _radiusA);
distanceInput.ProxyB = new DistanceProxy(_vBs, _radiusB);
distanceInput.TransformA = _transformA;
distanceInput.TransformB = transformB2;
distanceInput.UseRadii = false;
SimplexCache simplexCache;
DistanceOutput distanceOutput;
DistanceGJK.ComputeDistance(ref distanceInput, out distanceOutput, out simplexCache);
DrawString($"hit = {(hit ? "true" : "false")}, iters = {output.Iterations}, lambda = {output.Lambda}, distance = {distanceOutput.Distance}");
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
for (int i = 0; i < _countA; ++i)
{
vertices[i] = MathUtils.Mul(ref _transformA, _vAs[i]);
}
DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
if (_countA == 1)
{
DebugView.DrawCircle(vertices[0], _radiusA, new Color(0.9f, 0.9f, 0.9f));
}
else
{
DebugView.DrawPolygon(vertices, _countA, new Color(0.9f, 0.9f, 0.9f));
}
for (int i = 0; i < _countB; ++i)
{
vertices[i] = MathUtils.Mul(ref _transformB, _vBs[i]);
}
if (_countB == 1)
{
DebugView.DrawCircle(vertices[0], _radiusB, new Color(0.5f, 0.9f, 0.5f));
}
else
{
DebugView.DrawPolygon(vertices, _countB, new Color(0.5f, 0.9f, 0.5f));
}
for (int i = 0; i < _countB; ++i)
{
vertices[i] = MathUtils.Mul(ref transformB2, _vBs[i]);
}
if (_countB == 1)
{
DebugView.DrawCircle(vertices[0], _radiusB, new Color(0.5f, 0.7f, 0.9f));
}
else
{
DebugView.DrawPolygon(vertices, _countB, new Color(0.5f, 0.7f, 0.9f));
}
if (hit)
{
Vector2 p1 = output.Point;
DebugView.DrawPoint(p1, 10.0f, new Color(0.9f, 0.3f, 0.3f));
Vector2 p2 = p1 + output.Normal;
DebugView.DrawSegment(p1, p2, new Color(0.9f, 0.3f, 0.3f));
}
DebugView.EndCustomDraw();
}
