/*
 * Copyright (c) 2009, Mason Green (zzzzrrr)
 * Based on Box2D by Erin Catto, http://www.box2d.org
 * 
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice,
 *   this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 * * Neither the name of the polygonal nor the names of its contributors may be
 *   used to endorse or promote products derived from this software without specific
 *   prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
module openmelee.render;

import xf.dog.Dog;
import xf.omg.core.LinearAlgebra;
import xf.hybrid.Event;
import xf.hybrid.Font;

import openmelee.melee;

/// Color for drawing. Each value has the range [0,1].
struct Color {
    static Color opCall(float r, float g, float b)
    {
        Color u;
        u.r = r;
        u.g = g;
        u.b = b;
        return u;
    }

    float r = 0;
    float g = 0;
    float b = 0;
}

class Render : Melee
{

this(Settings *settings) {

	super(settings);
}

void drawCircle(GL gl, vec2 center, float radius, bool water = false, float theta = float.nan)
{
    int segs = cast(int)(radius) + 20;
    if (segs > MAX_CIRCLE_RES) segs = MAX_CIRCLE_RES;
    double coef = 2.0 * PI / segs;

    auto realTheta = (theta <>= 0 ? theta : 0);
    if (water) {
        gl.immediate(GL_TRIANGLE_FAN,
        {
            gl.Vertex2fv(center.ptr);
            for (int n = 0; n <= segs; n++) {
                double rads = n * coef;
                gl.Vertex2f(radius * cos(rads + realTheta) + center.x, radius * sin(rads + realTheta) + center.y);
            }
        });
    }

    gl.immediate(GL_LINE_STRIP,
    {
        for (int n = 0; n <= segs; n++) {
            double rads = n * coef;
            gl.Vertex2f(radius * cos(rads + realTheta) + center.x, radius * sin(rads + realTheta) + center.y);
        }
        if (theta <>= 0)
            gl.Vertex2fv(center.ptr);
    });
}

void drawSolidCircle(GL gl, vec2 center, float radius, vec2 axis, Color color)
{
    const k_segments = 16.0f;
    const k_increment = 2.0f * PI / k_segments;
    float theta = 0.0f;
    gl.Enable(GL_BLEND);
    gl.BlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    gl.Color4f(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f);
    gl.Begin(GL_TRIANGLE_FAN);
    for (int i = 0; i < k_segments; ++i) {
        vec2 v = center + radius * vec2(cos(theta), sin(theta));
        gl.Vertex2f(v.x, v.y);
        theta += k_increment;
    }
    gl.End();
    gl.Disable(GL_BLEND);

    theta = 0.0f;
    gl.Color4f(color.r, color.g, color.b, 1.0f);
    gl.Begin(GL_LINE_LOOP);
    for (int i = 0; i < k_segments; ++i) {
        vec2 v = center + radius * vec2(cos(theta), sin(theta));
        gl.Vertex2f(v.x, v.y);
        theta += k_increment;
    }
    gl.End();

    vec2 p = center + radius * axis;
    gl.Begin(GL_LINES);
    gl.Vertex2f(center.x, center.y);
    gl.Vertex2f(p.x, p.y);
    gl.End();
}

void drawPolygon(GL gl, vec2[] glVerts, Color color)
{
    gl.Color3f(color.r, color.g, color.b);
    gl.immediate(GL_LINE_LOOP,
    {
        foreach (v; glVerts)
            gl.Vertex2fv(v.ptr);
    });
}

void drawSolidPolygon(GL gl, vec2[] vertices, Color color)
{
    gl.Enable(GL_BLEND);
    gl.BlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    gl.Color4f(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f);
    gl.Begin(GL_TRIANGLE_FAN);
    for (int i = 0; i < vertices.length; ++i) {
        gl.Vertex2f(vertices[i].x, vertices[i].y);
    }
    gl.End();
    gl.Disable(GL_BLEND);

    gl.Color4f(color.r, color.g, color.b, 1.0f);
    gl.Begin(GL_LINE_LOOP);
    for (int i = 0; i < vertices.length; ++i) {
        gl.Vertex2f(vertices[i].x, vertices[i].y);
    }
    gl.End();
}


void drawPoint(GL gl, vec2 p, float size, Color color)
{
    gl.Color3f(color.r, color.g, color.b);
    gl.PointSize(size);
    gl.Begin(GL_POINTS);
    gl.Vertex2f(p.x, p.y);
    gl.End();
    gl.PointSize(1.0f);
}

void drawSegment(GL gl, vec2 begin, vec2 end, Color color)
{
    gl.Color3f(color.r, color.g, color.b);
    gl.immediate(GL_LINES,
    {
        gl.Vertex2fv(begin.ptr);
        gl.Vertex2fv(end.ptr);
    });
}

// TODO: handle inequal radii correctly
void connectCircles(GL gl, vec2 center1, float radius1, vec2 center2, float radius2)
{
    auto d = center2 - center1;
    if (!d.length)
        return;
    d *= (d.length - radius1) / d.length;
    center1 += d;
    center2 -= d;
    gl.immediate(GL_LINES,
    {
        gl.Vertex2fv(center1.ptr);
        gl.Vertex2fv(center2.ptr);
    });
}

void drawXForm(GL gl, bzXForm xf)
{
    bzVec2 p1 = xf.position, p2;
    const k_axisScale = 0.4f;

    gl.Begin(GL_LINES);
    {
        gl.Color3f(1.0f, 0.0f, 0.0f);
        gl.Vertex2f(p1.x, p1.y);
        p2 = p1 + k_axisScale * xf.R.col1;
        gl.Vertex2f(p2.x, p2.y);

        gl.Color3f(0.0f, 1.0f, 0.0f);
        gl.Vertex2f(p1.x, p1.y);
        p2 = p1 + k_axisScale * xf.R.col2;
        gl.Vertex2f(p2.x, p2.y);
    }
    gl.End();
}

void drawSpring(GL gl, vec2 a, vec2 b, uint zigs)
{
    zigs++;

    // Portion of length dedicated to connectors
    const float connPart = 0.2;

    vec2 inc = (b - a) / (zigs);
    // One step from a to b
    vec2 zigLen = inc * (1 - connPart);
    // Length of a connector
    vec2 connLen = inc * (connPart / 2) * zigs;
    // Width of a zig
    vec2 zigWidth = (b - a).rotatedHalfPi.normalized;
    gl.immediate(GL_LINE_STRIP,
    {
        gl.Vertex2fv(a.ptr);

        a += connLen;
        gl.Vertex2fv(a.ptr);

        bool dir = true;
        a += zigWidth / 2 + zigLen / 2;
        for (int i = 0; i < zigs; i++) {
         gl.Vertex2fv(a.ptr);
         a += zigLen;
         if (dir) {
             a -= zigWidth;
         }else {
             a += zigWidth;
         }
         dir = !dir;
        }

        gl.Vertex2fv((b - connLen).ptr);
        gl.Vertex2fv(b.ptr);
    });
}

void drawShape(GL gl, bzShape shape, bzXForm xf, Color color, bool core)
{
    Color coreColor = Color(0.9f, 0.6f, 0.6f);

    switch (shape.type) {
    case bzShapeType.CIRCLE:
        auto circle = cast(bzCircle)shape;

        vec2 center = vec2.from(bzMul(xf, circle.localPosition));
        float radius = circle.radius;
        vec2 axis = vec2.from(xf.R.col1);

        gl.drawSolidCircle(center, radius, axis, color);

        if (core) {
            gl.Color3f(coreColor.r, coreColor.g, coreColor.b);
            gl.drawCircle(center, radius - k_toiSlop);
        }
        break;
    case bzShapeType.POLYGON:
    {
        bzPolygon poly = cast(bzPolygon)shape;
        bzVec2[] vertices = poly.worldVertices;
        vec2[]  verts;
        verts.length = vertices.length;
        foreach (int i, v; vertices) {
            verts[i] = vec2.from(v);
        }

        gl.drawSolidPolygon(verts, color);

        if (core) {
            bzVec2[] localCoreVertices = poly.coreVertices;
            verts.length = localCoreVertices.length;
            for (int i = 0; i < localCoreVertices.length; ++i) {
                verts[i] = vec2.from(bzMul(xf, localCoreVertices[i]));
            }
            gl.drawPolygon(verts, coreColor);
        }
    }
    break;

    case bzShapeType.EDGE:
    {
        bzEdge edge = cast(bzEdge)shape;

        vec2 p1 = vec2.from(bzMul(xf, edge.vertex1));
        vec2 p2 = vec2.from(bzMul(xf, edge.vertex2));
        gl.drawSegment(p1, p2, color);

        if (core) {
            p1 = vec2.from(bzMul(xf, edge.coreVertex1));
            p2 = vec2.from(bzMul(xf, edge.coreVertex2));
            gl.drawSegment(p1, p2, coreColor);
        }
    }
    break;
    }
}

void draw(vec2i screenSize, GL gl)
{
    this.screenSize = screenSize;

    gl.LoadIdentity();
    gl.MatrixMode(GL_PROJECTION);
    gl.LoadIdentity();
    
    float left = -screenSize.x / zoom;
    float right = screenSize.x / zoom;
    float bottom = -screenSize.y / zoom;
    float top = screenSize.y / zoom;

    gl.gluOrtho2D(left, right, bottom, top);
    gl.Translatef(-viewCenter.x, -viewCenter.y, 0);
    gl.MatrixMode(GL_MODELVIEW);
    gl.Disable(GL_DEPTH_TEST);
    gl.LoadIdentity();
    gl.Clear(GL_COLOR_BUFFER_BIT);

    // Draw dynamic bodies
    if (settings.drawShapes) {
        for (bzBody b = world.bodyList; b; b = b.next) {
            for (bzShape shape = b.shapeList; shape; shape = shape.next) {

                bzShape s = shape;
                bzXForm xf = b.xf;

                if (b.isStatic) {
                    gl.drawShape(s, xf, Color(0.5f, 0.9f, 0.5f), settings.drawCoreShapes);
                }else if (b.isSleeping) {
                    gl.drawShape(s, xf, Color(0.5f, 0.5f, 0.9f), settings.drawCoreShapes);
                }else if (b.userData) {
                    auto ss = cast(Select)b.userData;
                    if (ss) {
                        gl.drawShape(s, xf, Color(0, .5, 1), settings.drawCoreShapes);
                    }
                }else {
                    gl.drawShape(s, xf, Color(0.9f, 0.9f, 0.9f), settings.drawCoreShapes);
                }

                gl.LoadIdentity();
                gl.Flush();
            }
        }

        // Draw water
        bzFluidParticle[] particles = world.particles;
        gl.Color3f(0, 0, 1);
        foreach (p; particles) {
            gl.drawCircle(vec2.from(p.position), WATER_RADIUS, true);
        }
    }

    // Draw joints
    if (settings.drawJoints) {
        Color color = Color(0, 0, 1);
        gl.Color3f(0, 0, 1);

        gl.LineWidth(1);
        for (bzJoint joint = world.jointList; joint; joint = joint.next) {
            auto distance = cast(bzDistanceJoint)joint;
            auto pulley = cast(bzPulleyJoint)joint;
            auto revolute = cast(bzRevoluteJoint)joint;
            auto prismatic = cast(bzPrismaticJoint)joint;
            auto line = cast(bzLineJoint)joint;
            if (distance) {
                color = Color(.5, .5, 0);
                // Endpoints
                vec2 a = vec2.from(distance.anchor1);
                vec2 b = vec2.from(distance.anchor2);
                // Circles
                gl.drawCircle(a, HINGE_RADIUS);
                gl.drawCircle(b, HINGE_RADIUS);
                // Connecting line
                gl.connectCircles(a, HINGE_RADIUS, b, HINGE_RADIUS);
            }else if (pulley) {
                auto a = vec2.from(pulley.anchor1);
                auto b = vec2.from(pulley.groundAnchor1);
                auto c = vec2.from(pulley.groundAnchor2);
                auto d = vec2.from(pulley.anchor2);
                gl.drawCircle(a, HINGE_RADIUS);
                gl.drawCircle(b, HINGE_RADIUS);
                gl.connectCircles(a, HINGE_RADIUS, b, HINGE_RADIUS);
                gl.drawSegment(b, c, color);
                gl.drawCircle(c, HINGE_RADIUS);
                gl.drawCircle(d, HINGE_RADIUS);
                gl.connectCircles(c, HINGE_RADIUS, d, HINGE_RADIUS);
            }else if (revolute) {
                auto a = vec2.from(revolute.rBody1.position);
                auto b = vec2.from(revolute.anchor1);
                auto c = vec2.from(revolute.rBody2.position);
                gl.drawCircle(a, HINGE_RADIUS);
                gl.drawCircle(b, HINGE_RADIUS);
                gl.connectCircles(a, HINGE_RADIUS, b, HINGE_RADIUS);
                gl.drawCircle(c, HINGE_RADIUS);
                gl.connectCircles(b, HINGE_RADIUS, c, HINGE_RADIUS);
            }else if (prismatic) {
                auto a = vec2.from(prismatic.rBody1.position);
                auto b = vec2.from(prismatic.anchor1);
                auto c = vec2.from(prismatic.rBody2.position);
                gl.drawCircle(a, HINGE_RADIUS);
                gl.drawCircle(b, HINGE_RADIUS);
                gl.connectCircles(a, HINGE_RADIUS, b, HINGE_RADIUS);
                gl.drawCircle(c, HINGE_RADIUS);
                gl.connectCircles(b, HINGE_RADIUS, c, HINGE_RADIUS);
            }else if (line) {
                auto a = vec2.from(line.rBody1.position);
                auto b = vec2.from(line.anchor1);
                auto c = vec2.from(line.rBody2.position);
                gl.drawCircle(a, HINGE_RADIUS);
                gl.drawCircle(b, HINGE_RADIUS);
                gl.connectCircles(a, HINGE_RADIUS, b, HINGE_RADIUS);
                gl.drawCircle(c, HINGE_RADIUS);
                gl.connectCircles(b, HINGE_RADIUS, c, HINGE_RADIUS);
            }
        }

        if (settings.drawControllers) {
            bzForceGenerator[] forces = world.forces;
            foreach (f; forces) {
                auto spring1 = cast(bzSpring1) f;
                auto spring2 = cast(bzSpring2) f;
                auto buoyancy = cast(bzBuoyancy) f;

                if (spring1) {
                    auto bungee1 = cast(bzBungee1)spring1;
                    if (bungee1) {
                        gl.Color3f(.5, .5, 0);
                        // Endpoints
                        vec2 a = vec2.from(bungee1.rBody.position);
                        vec2 b = vec2.from(bungee1.anchor);
                        // Circles
                        gl.drawCircle(a, HINGE_RADIUS);
                        gl.drawCircle(b, HINGE_RADIUS);
                        // Connecting line
                        gl.connectCircles(a, HINGE_RADIUS, b, HINGE_RADIUS);
                    }else {
                        uint zigs = 10;
                        auto anchor1 = vec2.from(spring1.anchor);
                        auto anchor2 = vec2.from(spring1.rBody.position);
                        gl.drawSpring(anchor1, anchor2, zigs);
                    }
                }

                if (spring2) {
                    auto bungee2 = cast(bzBungee2)spring2;
                    if (bungee2) {
                        gl.Color3f(.5, .5, 0);
                        // Endpoints
                        vec2 a = vec2.from(bungee2.rBody.position);
                        vec2 b = vec2.from(bungee2.otherBody.position);
                        // Circles
                        gl.drawCircle(a, HINGE_RADIUS);
                        gl.drawCircle(b, HINGE_RADIUS);
                        // Connecting line
                        gl.connectCircles(a, HINGE_RADIUS, b, HINGE_RADIUS);
                    }else {
                        uint zigs = 10;
                        auto anchor1 = vec2.from(spring2.otherBody.position);
                        auto anchor2 = vec2.from(spring2.rBody.position);
                        gl.drawSpring(anchor1, anchor2, zigs);
                    }
                }

                if(buoyancy) {
                    float plane = buoyancy.planeOffset;
                    vec2 p1 = vec2(-50, plane);
                    vec2 p2 = vec2(50, plane);
                    gl.drawSegment(p1, p2, color);
                }
            }
        }
    }

    if(settings.drawPairs) {

        bzBroadPhase bp = world.broadPhase;
		bzVec2 invQ;
		invQ.set(1.0f / bp.m_quantizationFactor.x, 1.0f / bp.m_quantizationFactor.y);
		Color color = Color(0.9f, 0.9f, 0.3f);

        const k_tableCapacity = bzPairManager.TABLE_CAPACITY;

		for (int i = 0; i < k_tableCapacity; ++i)
		{
			ushort index = bp.m_pairManager.m_hashTable[i];
			while (index < bp.m_pairManager.m_pairs.length)
			{
                if(index == bzPairManager.NULL_PROXY) {
                    break;
                }
				bzPair pair = bp.m_pairManager.m_pairs[index];
				bzProxy p1 = bp.m_proxyPool[pair.proxyId1];
				bzProxy p2 = bp.m_proxyPool[pair.proxyId2];

				bzAABB b1, b2;
				b1.lowerBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p1.lowerBounds[0]].value;
				b1.lowerBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p1.lowerBounds[1]].value;
				b1.upperBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p1.upperBounds[0]].value;
				b1.upperBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p1.upperBounds[1]].value;
				b2.lowerBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p2.lowerBounds[0]].value;
				b2.lowerBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p2.lowerBounds[1]].value;
				b2.upperBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p2.upperBounds[0]].value;
				b2.upperBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p2.upperBounds[1]].value;

				bzVec2 x1 = 0.5f * (b1.lowerBound + b1.upperBound);
				bzVec2 x2 = 0.5f * (b2.lowerBound + b2.upperBound);

				gl.drawSegment(vec2.from(x1),vec2.from(x2), color);

				index = pair.next;
			}
		}
    }

    // Draw axis aligned bounding boxes (bzAABB)
    if (settings.drawAABBs) {
        bzBroadPhase bp = world.broadPhase;
        bzVec2 worldLower = bp.m_worldAABB.lowerBound;
        bzVec2 worldUpper = bp.m_worldAABB.upperBound;
        Color color;
        bzVec2 invQ;
        invQ.set(1.0f / bp.m_quantizationFactor.x, 1.0f / bp.m_quantizationFactor.y);
        color = Color(1.0f, 1.0f, 1.0f);
        /*
        for (int i = 0; i < k_maxProxies; ++i) {
            bzProxy p = bp.m_proxyPool[i];
            if (!p.isValid) {
                continue;
            }

            bzAABB b;
            b.lowerBound.x = worldLower.x + invQ.x * bp.m_bounds[0][p.lowerBounds[0]].value;
            b.lowerBound.y = worldLower.y + invQ.y * bp.m_bounds[1][p.lowerBounds[1]].value;
            b.upperBound.x = worldLower.x + invQ.x * bp.m_bounds[0][p.upperBounds[0]].value;
            b.upperBound.y = worldLower.y + invQ.y * bp.m_bounds[1][p.upperBounds[1]].value;

            vec2 vs[4];
            vs[0] = vec2(b.lowerBound.x, b.lowerBound.y);
            vs[1] = vec2(b.upperBound.x, b.lowerBound.y);
            vs[2] = vec2(b.upperBound.x, b.upperBound.y);
            vs[3] = vec2(b.lowerBound.x, b.upperBound.y);

            drawPolygon(gl, vs, color);
        }
        */
        vec2 vs[4];
        vs[0] = vec2(worldLower.x, worldLower.y);
        vs[1] = vec2(worldUpper.x, worldLower.y);
        vs[2] = vec2(worldUpper.x, worldUpper.y);
        vs[3] = vec2(worldLower.x, worldUpper.y);

        color = Color(0.3f, 0.9f, 0.9f);
        drawPolygon(gl, vs, color);
    }

    // Draw contact points
    if (settings.drawContactPoints) {
        const k_axisScale = 0.3f;
        const k_forceScale = 0.01f;


        for (int i = 0; i < pointCount; ++i) {
            ContactPoint point = points[i];
            Color color;

            if (point.state == ContactState.e_contactAdded) {
                // Add
                color = Color(0.3f, 0.95f, 0.3f);
                vec2 p = vec2.from(point.position);
                gl.drawPoint(p, 10.0f, color);
            }else if (point.state == ContactState.e_contactPersisted) {
                // Persist
                color = Color(0.3f, 0.3f, 0.95f);
                vec2 p = vec2.from(point.position);
                gl.drawPoint(p, 5.0f, color);
            }else {
                // Remove
                color = Color(0.95f, 0.3f, 0.3f);
                vec2 p = vec2.from(point.position);
                gl.drawPoint(p, 10.0f, color);
            }

            if (settings.drawContactNormals == 1) {
                vec2 p1 = vec2.from(point.position);
                vec2 p2 = p1 + k_axisScale * vec2.from(point.normal);
                color = Color(0.4f, 0.9f, 0.4f);
                gl.drawSegment(p1, p2, color);
            }else if (settings.drawContactForces) { /*
                       vec2 p1 = vec2.from(point.position);
                       vec2 p2 = p1 + k_forceScale * vec2.from(point.normalForce * point.normal);
                       color = Color(0.9f, 0.9f, 0.3f);
                       gl.drawSegment(p1, p2, color);*/
            }

            if (settings.drawFrictionForces) { /*
                      vec2 tangent = vec2.from(bzCross(point.normal, 1.0f));
                      vec2 p1 = point.position;
                      vec2 p2 = p1 + k_forceScale * vec2.from(point.tangentForce) * tangent;
                      color = Color(0.9f, 0.9f, 0.3f);
                      gl.drawSegment(p1, p2, color); */
            }
        }
    }

    if (settings.drawOBBs) {
        Color color = Color(0.5f, 0.3f, 0.5f);

        for (bzBody b = world.bodyList; b; b = b.next) {
            bzXForm xf = b.xf;
            for (bzShape s = b.shapeList; s; s = s.next) {
                if (s.type != bzShapeType.POLYGON) {
                    continue;
                }

                bzPolygon poly = cast(bzPolygon)s;
                bzOBB obb = poly.obb;
                bzVec2 h = obb.extents;
                bzVec2 vs[4];
                vs[0].set(-h.x, -h.y);
                vs[1].set(h.x, -h.y);
                vs[2].set(h.x, h.y);
                vs[3].set(-h.x, h.y);

                vec2[4] v;
                for (int i = 0; i < 4; ++i) {
                    vs[i] = obb.center + bzMul(obb.R, vs[i]);
                    v[i] = vec2.from(bzMul(xf, vs[i]));
                }

                drawPolygon(gl, v, color);
            }
        }
    }

    if (settings.drawCOMs) {
        for (bzBody b = world.bodyList; b; b = b.next) {
            bzXForm xf = b.xf;
            xf.position = b.worldCenter;
            drawXForm(gl, xf);
        }
    }

    // Nonphysical stuffs
    // Universal '.' cursor
    gl.Color3f(1, 1, 1);
    gl.immediate(GL_POINTS,
    {
        gl.Vertex2fv(mousePos.ptr);
    });

    pointCount = 0;
}

}