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; } } } }