Port the tumbler from Box2D (#4486)

Content.Server/Physics/TestbedCommand.cs

@@ -21,6 +21,7 @@
 // SOFTWARE.
 
 
+using System;
 using Content.Server.Administration;
 using Content.Shared.Administration;
 using Content.Shared.Physics;
@@ -33,6 +34,7 @@ using Robust.Shared.Maths;
 using Robust.Shared.Physics;
 using Robust.Shared.Physics.Collision.Shapes;
 using Robust.Shared.Physics.Dynamics;
+using Robust.Shared.Physics.Dynamics.Joints;
 using Robust.Shared.Timing;
 
 
@@ -97,6 +99,10 @@ namespace Content.Server.Physics
                     SetupPlayer(mapId, shell, player, mapManager);
                     CreatePyramid(mapId);
                     break;
+                case "tumbler":
+                    SetupPlayer(mapId, shell, player, mapManager);
+                    CreateTumbler(mapId);
+                    break;
                 default:
                     shell.WriteLine($"testbed {args[0]} not found!");
                     return;
@@ -291,5 +297,83 @@ namespace Content.Server.Physics
                 x += deltaX;
             }
         }
+
+        private void CreateTumbler(MapId mapId)
+        {
+            var broadphaseSystem = EntitySystem.Get<SharedBroadphaseSystem>();
+            var compManager = IoCManager.Resolve<IComponentManager>();
+            var entityManager = IoCManager.Resolve<IEntityManager>();
+
+            var groundEnt = entityManager.SpawnEntity(null, new MapCoordinates(0f, 0f, mapId));
+            var ground = compManager.AddComponent<PhysicsComponent>(groundEnt);
+
+            var bodyEnt = entityManager.SpawnEntity(null, new MapCoordinates(0f, 10f, mapId));
+            var body = compManager.AddComponent<PhysicsComponent>(bodyEnt);
+
+            body.BodyType = BodyType.Dynamic;
+            body.SleepingAllowed = false;
+            body.FixedRotation = false;
+
+            // TODO: Box2D just derefs, bleh shape structs someday
+            var shape1 = new PolygonShape();
+            shape1.SetAsBox(0.5f, 10.0f, new Vector2(10.0f, 0.0f), 0.0f);
+            broadphaseSystem.CreateFixture(body, shape1, 20.0f);
+
+            var shape2 = new PolygonShape();
+            shape2.SetAsBox(0.5f, 10.0f, new Vector2(-10.0f, 0.0f), 0f);
+            broadphaseSystem.CreateFixture(body, shape2, 20.0f);
+
+            var shape3 = new PolygonShape();
+            shape3.SetAsBox(10.0f, 0.5f, new Vector2(0.0f, 10.0f), 0f);
+            broadphaseSystem.CreateFixture(body, shape3, 20.0f);
+
+            var shape4 = new PolygonShape();
+            shape4.SetAsBox(10.0f, 0.5f, new Vector2(0.0f, -10.0f), 0f);
+            broadphaseSystem.CreateFixture(body, shape4, 20.0f);
+
+            foreach (var fixture in body.Fixtures)
+            {
+                fixture.CollisionLayer = (int) CollisionGroup.Impassable;
+            }
+
+            // TODO: Should Joints be their own entities? Box2D just adds them directly to the world.
+            // HMMM
+            // At the least it should be its own damn component
+            var revolute = new RevoluteJoint(ground, body)
+            {
+                LocalAnchorA = new Vector2(0f, 10f),
+                LocalAnchorB = new Vector2(0f, 0f),
+                ReferenceAngle = 0f,
+                MotorSpeed = 0.05f * MathF.PI,
+                MaxMotorTorque = 100000000f,
+                EnableMotor = true
+            };
+            body.AddJoint(revolute);
+
+            // Box2D has this as 800 which is jesus christo.
+            // Wouldn't recommend higher than 100 in debug and higher than 300 on release unless
+            // you really want a profile.
+            var count = 50;
+
+            EntitySystem.Get<SharedPhysicsSystem>().Maps[mapId].Gravity = new Vector2(0f, -9.8f);
+            var mapManager = IoCManager.Resolve<IMapManager>();
+
+            for (var i = 0; i < count; i++)
+            {
+                Timer.Spawn(i * 20, () =>
+                {
+                    if (!mapManager.MapExists(mapId)) return;
+                    var ent = entityManager.SpawnEntity(null, new MapCoordinates(0f, 10f, mapId));
+                    var body = compManager.AddComponent<PhysicsComponent>(ent);
+                    body.BodyType = BodyType.Dynamic;
+                    body.FixedRotation = false;
+                    var shape = new PolygonShape();
+                    shape.SetAsBox(0.125f, 0.125f);
+                    broadphaseSystem.CreateFixture(body, shape, 0.0625f);
+                    body.Fixtures[0].CollisionMask = (int) CollisionGroup.Impassable;
+                    body.Fixtures[0].CollisionLayer = (int) CollisionGroup.Impassable;
+                });
+            }
+        }
     }
 }