public void PaintDebug(DebugDrawer drawer)
{
if (Settings.Data.debugMode)
{
Movable locomotion = Locomotion;
drawer.DrawCircle(drawer.Position, Controller.searchRadius, new Color(1.0f, 0.0f, 0.0f, 0.2f));
//drawer.DrawLine(drawer.Position, debugHitLocation.RelativeTo(locomotion.GlobalPosition), new Color(0.0f, 1.0f, 0.0f), 2f, true);
if (target != null)
{
const float size = 24f;
drawer.DrawRect(new Rect2(target.Location.RelativeTo(locomotion.GlobalPosition) - new Vector2(size / 2f, size / 2f), size, size), new Color(0.0f, 1.0f, 0.0f), false);
}
if (state == ChaseAIState.Wandering)
{
drawer.DrawLine(drawer.Position, target.Location.RelativeTo(locomotion.GlobalPosition), new Color(0.0f, 0.0f, 1.0f), 2f, true);
drawer.DrawCircle(target.Location.RelativeTo(locomotion.GlobalPosition), 8f, new Color(0.0f, 0.0f, 1.0f));
}
else if (state == ChaseAIState.Chasing)
{
drawer.DrawLine(drawer.Position, target.Location.RelativeTo(locomotion.GlobalPosition), new Color(1.0f, 0.0f, 0.0f), 2f, true);
drawer.DrawCircle(target.Location.RelativeTo(locomotion.GlobalPosition), 8f, new Color(1.0f, 0.0f, 0.0f));
}
}
}
private void HandleHullCollisions()
{
for (int i = 0; i < Transforms.Count; ++i)
{
var tA = Transforms[i];
if (tA == null)
{
continue;
}
var hullA = Hulls[tA.GetInstanceID()].Hull;
var transformA = new RigidTransform(tA.rotation, tA.position);
HullDrawingUtility.DrawDebugHull(hullA, transformA, HullDrawingOptions);
if (LogClosestPoint)
{
var sw3 = System.Diagnostics.Stopwatch.StartNew();
var result3 = HullCollision.ClosestPoint(transformA, hullA, 0);
sw3.Stop();
var sw4 = System.Diagnostics.Stopwatch.StartNew();
var result4 = HullOperations.ClosestPoint.Invoke(transformA, hullA, 0);
sw4.Stop();
if (DrawClosestPoint)
{
DebugDrawer.DrawSphere(result4, 0.1f, Color.blue);
DebugDrawer.DrawLine(result4, Vector3.zero, Color.blue);
}
Debug.Log($"ClosestPoint between '{tA.name}' and world zero took: {sw3.Elapsed.TotalMilliseconds:N4}ms (Normal), {sw4.Elapsed.TotalMilliseconds:N4}ms (Burst)");
}
for (int j = i + 1; j < Transforms.Count; j++)
{
var tB = Transforms[j];
if (tB == null)
{
continue;
}
if (!tA.hasChanged && !tB.hasChanged)
{
continue;
}
var hullB = Hulls[tB.GetInstanceID()].Hull;
var transformB = new RigidTransform(tB.rotation, tB.position);
HullDrawingUtility.DrawDebugHull(hullB, transformB, HullDrawingOptions);
DrawHullCollision(tA.gameObject, tB.gameObject, transformA, hullA, transformB, hullB);
if (LogCollisions)
{
var sw1 = System.Diagnostics.Stopwatch.StartNew();
var result1 = HullCollision.IsColliding(transformA, hullA, transformB, hullB);
sw1.Stop();
var sw2 = System.Diagnostics.Stopwatch.StartNew();
var result2 = HullOperations.IsColliding.Invoke(transformA, hullA, transformB, hullB);
sw2.Stop();
Debug.Assert(result1 == result2);
Debug.Log($"Collisions between '{tA.name}'/'{tB.name}' took: {sw1.Elapsed.TotalMilliseconds:N4}ms (Normal), {sw2.Elapsed.TotalMilliseconds:N4}ms (Burst)");
}
}
}
if (LogCollisions)
{
TestBatchCollision();
}
}
protected override void Update(GameTime gameTime)
{
// Allows the game to exit
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
this.Exit();
}
KeyboardState keys = Keyboard.GetState();
JVector moveVector = JVector.Zero;
float amountOfMovement = 0.05f;
if (keys.IsKeyDown(Keys.Right))
{
moveVector.X += amountOfMovement;
}
if (keys.IsKeyDown(Keys.Left))
{
moveVector.X -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Down))
{
moveVector.Y -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Up))
{
moveVector.Y += amountOfMovement;
}
body1.Position += moveVector;
body1.Orientation += 0.001f;
body2.Orientation -= 0.001f;
float o1 = body1.Orientation;
float o2 = body2.Orientation;
JVector pos1 = body1.Position;
JVector pos2 = body2.Position;
sw.Start();
hit = Collision.Detect(DebugDrawer, body1.Shape, body2.Shape, ref o1, ref o2, ref pos1, ref pos2, out point, out normal, out penetration, out iterations);
sw.Stop();
ticks = sw.ElapsedTicks;
sw.Reset();
DebugDrawer.DrawPoint(point);
DebugDrawer.Color = Color.Red;
DebugDrawer.DrawLine(point, point + JVector.Normalize(normal) * penetration);
DebugDrawer.Color = Color.Black;
DebugDrawer.DrawLine(JVector.Up, JVector.Down);
DebugDrawer.DrawLine(JVector.Left, JVector.Right);
body1.DebugDraw(DebugDrawer);
body2.DebugDraw(DebugDrawer);
if (hit)
{
var oldPosition = body1.Position;
body1.Position -= normal * penetration;
body1.DebugDraw(DebugDrawer);
body1.Position = oldPosition;
}
base.Update(gameTime);
}
private static unsafe void DrawTests(int threadIndex, float3 start, float3 end, NativeString512 text, DrawingMethods methods, NativeArray <float3> polygon)
{
float3 offset = Vector3.up * 0.05f + Vector3.left * 0.05f;
float3 center = (start + (end - start) / 2);
if (methods.Sphere)
{
DebugDrawer.DrawSphere(start, 0.75f, UnityColors.GhostDodgerBlue);
}
if (methods.RectangleWithOutline)
{
var size = 0.25f;
var points = stackalloc[]
{
center + offset + new float3(0, 0, 0),
center + offset + new float3(0, size, 0),
center + offset + new float3(0, size, size),
center + offset + new float3(0, 0, size)
};
DebugDrawer.DrawSolidRectangleWithOutline(points, UnityColors.LightYellow, UnityColors.Yellow);
}
if (methods.Polygon)
{
DebugDrawer.DrawAAConvexPolygon(polygon, center + (float3)Vector3.down * 0.25f, UnityColors.GhostDodgerBlue);
}
if (methods.Line)
{
DebugDrawer.DrawLine(start + offset, end + offset);
}
if (methods.Ray)
{
DebugDrawer.DrawRay(center, Vector3.up, UnityColors.MediumBlue);
}
if (methods.Cone)
{
DebugDrawer.DrawCone(center + (float3)Vector3.up * 0.5f, Vector3.up, UnityColors.DarkKhaki, 22.5f);
}
if (methods.Circle)
{
DebugDrawer.DrawCircle(center, Vector3.up, 0.25f, UnityColors.AliceBlue);
}
if (methods.DottedLine)
{
DebugDrawer.DrawDottedLine(start, end, Color.yellow);
}
if (methods.WireCube)
{
DebugDrawer.DrawWireCube(end, Vector3.one / 2, Color.yellow);
}
if (methods.DottedWireCube)
{
DebugDrawer.DrawDottedWireCube(end, Vector3.one, Color.black);
}
if (methods.Label)
{
DebugDrawer.DrawLabel(center + (float3)Vector3.down * 0.25f, text);
}
if (methods.Arrow)
{
DebugDrawer.DrawArrow(start + (float3)Vector3.up * 0.5f, Vector3.up, Color.blue);
}
if (methods.Log)
{
DebugDrawer.Log(threadIndex, text);
}
if (methods.LogWarning)
{
DebugDrawer.LogWarning(text);
}
if (methods.LogError)
{
DebugDrawer.LogError(text);
}
if (methods.Point)
{
DebugDrawer.DrawPoint(center + (float3)Vector3.forward, UnityColors.Lavender, 0.25f);
}
}
}
public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color)
{
if (shape.ShapeType == BroadphaseNativeTypes.Compound)
{
CompoundShape compoundShape = shape as CompoundShape;
for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
DebugDrawObject(worldTransform * childTrans, colShape, color);
}
}
else
{
switch (shape.ShapeType)
{
case BroadphaseNativeTypes.Sphere:
{
SphereShape sphereShape = shape as SphereShape;
float radius = sphereShape.Margin; //radius doesn't include the margin, so draw with margin
Vector3 start = worldTransform.Translation;
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color);
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color);
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color);
//drawSphere
break;
}
case BroadphaseNativeTypes.MultiSphere:
case BroadphaseNativeTypes.Cone:
{
ConeShape coneShape = shape as ConeShape;
float radius = coneShape.Radius; //+coneShape->getMargin();
float height = coneShape.Height; //+coneShape->getMargin();
Vector3 start = worldTransform.Translation;
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color);
break;
}
case BroadphaseNativeTypes.Cylinder:
{
CylinderShape cylinder = shape as CylinderShape;
int upAxis = cylinder.UpAxis;
float radius = cylinder.Radius;
float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis);
Vector3 start = worldTransform.Translation;
Vector3 offsetHeight = new Vector3();
MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight);
Vector3 offsetRadius = new Vector3();
MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
break;
}
default:
{
if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh)
{
TriangleMeshShape concaveMesh = shape as TriangleMeshShape;
//btVector3 aabbMax(1e30f,1e30f,1e30f);
//btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
//todo pass camera, for some culling
Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax);
}
if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh)
{
ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape;
//todo: pass camera for some culling
Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
//DebugDrawcallback drawCallback;
DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax);
}
// for polyhedral shapes
if (shape.IsPolyhedral)
{
PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
for (int i = 0; i < polyshape.EdgeCount; i++)
{
Vector3 a, b;
polyshape.GetEdge(i, out a, out b);
a = Vector3.TransformNormal(a, worldTransform);
b = Vector3.TransformNormal(b, worldTransform);
DebugDrawer.DrawLine(a, b, color);
}
}
break;
}
}
}
}
