public scDebugData GetOrCreateDebugDataForBody(scBody body)
 {
     if(bodyMap.ContainsKey(body))
         return bodyMap[body];
     var debugData = new scDebugData();
     bodyMap.Add(body, debugData);
     return debugData;
 }
        public void DrawShape(scDebugData debugData, SpriteBatch spriteBatch, scShape shape, scTransform transform)
        {
            switch (shape.ShapeType)
            {
                case scShapeType.Circle:
                {
                    var circle = (scCircleShape)shape;
                    spriteBatch.DrawCircle(transform.position + circle.localPosition, circle.radius, debugData.CircleShapeSides, debugData.CircleShapeColor);
                    break;
                }
                case scShapeType.Rectangle:
                {
                    var rectangle = (scRectangleShape)shape;

                    var A = transform.position + rectangle.vertices[0].Rotate(transform.rotation.radians);
                    var B = transform.position + rectangle.vertices[1].Rotate(transform.rotation.radians);
                    var C = transform.position + rectangle.vertices[2].Rotate(transform.rotation.radians);
                    var D = transform.position + rectangle.vertices[3].Rotate(transform.rotation.radians);

                    spriteBatch.DrawLine(A, B, debugData.RectangleShapeColor);
                    spriteBatch.DrawLine(B, C, debugData.RectangleShapeColor);
                    spriteBatch.DrawLine(C, D, debugData.RectangleShapeColor);
                    spriteBatch.DrawLine(D, A, debugData.RectangleShapeColor);

                    break;
                }
                case scShapeType.Edge:
                {
                    var edge = (scEdgeShape)shape;
                    var start = scTransformUtils.applyTransform(transform, edge.start);
                    var end = scTransformUtils.applyTransform(transform, edge.end);
                    spriteBatch.DrawLine(start, end, debugData.EdgeShapeColor);
                    break;
                }
            }
        }