static void DrawShape(cpShape *shape, DrawNode *renderer)
{
    cpBody *body = shape->body;
    Color4F color = ColorForBody(body);

    switch (shape->CP_PRIVATE(klass)->type)
    {
    case CP_CIRCLE_SHAPE:
    {
        cpCircleShape *circle = (cpCircleShape *)shape;
        cpVect center = circle->tc;
        cpFloat radius = circle->r;
        renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color);
        renderer->drawSegment(cpVert2Point(center), cpVert2Point(cpvadd(center, cpvmult(body->rot, radius))), 1.0, color);
    }
    break;
    case CP_SEGMENT_SHAPE:
    {
        cpSegmentShape *seg = (cpSegmentShape *)shape;
        renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 2.0), color);
    }
    break;
    case CP_POLY_SHAPE:
    {
        cpPolyShape *poly = (cpPolyShape *)shape;
        Color4F line = color;
        line.a = cpflerp(color.a, 1.0, 0.5);
        Vec2* pPoints = cpVertArray2ccpArrayN(poly->tVerts, poly->numVerts);
        renderer->drawPolygon(pPoints, poly->numVerts, color, 1.0, line);
        CC_SAFE_DELETE_ARRAY(pPoints);
    }
    break;
    default:
        cpAssertHard(false, "Bad assertion in DrawShape()");
    }
}
static void DrawShape(cpShape *shape, DrawNode *renderer)
{
    cpBody *body = cpShapeGetBody(shape);
    Color4F color = ColorForBody(body);
    
    switch (shape->CP_PRIVATE(klass)->type)
    {
        case CP_CIRCLE_SHAPE:
        {
            cpCircleShape *circle = (cpCircleShape *)shape;
            cpVect center = circle->tc;
            cpFloat radius = circle->r;
            renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color);
            renderer->drawSegment(cpVert2Point(center), cpVert2Point(cpvadd(center, cpvmult(cpBodyGetRotation(body), radius))), 1.0, color);
        }
             break;
        case CP_SEGMENT_SHAPE:
        {
            cpSegmentShape *seg = (cpSegmentShape *)shape;
            renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 2.0), color);
        }
            break;
        case CP_POLY_SHAPE:
        {
            cpPolyShape* poly = (cpPolyShape*)shape;
            Color4F line = color;
            line.a = cpflerp(color.a, 1.0, 0.5);
            int num = poly->count;
            Vec2* pPoints = new (std::nothrow) Vec2[num];
            for(int i=0;i<num;++i)
                pPoints[i] = cpVert2Point(poly->planes[i].v0);
            renderer->drawPolygon(pPoints, num, color, 1.0, line);
            CC_SAFE_DELETE_ARRAY(pPoints);
        }
            break;
        default:
            cpAssertHard(false, "Bad assertion in DrawShape()");
    }
}
static void DrawConstraint(cpConstraint *constraint, DrawNode *renderer)
{
    cpBody *body_a = cpConstraintGetBodyA(constraint);
    cpBody *body_b = cpConstraintGetBodyB(constraint);
    
    if(cpConstraintIsPinJoint(constraint))
    {
        cpVect a = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpPinJointGetAnchorA(constraint), cpBodyGetRotation(body_a)));
        cpVect b = cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpPinJointGetAnchorB(constraint), cpBodyGetRotation(body_b)));
        
        renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
        renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
        renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
    }
    else if(cpConstraintIsSlideJoint(constraint))
    {
        cpVect a = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpSlideJointGetAnchorA(constraint), cpBodyGetRotation(body_a)));
        cpVect b = cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpSlideJointGetAnchorB(constraint), cpBodyGetRotation(body_b)));
        
        renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
        renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
        renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
    }
    else if(cpConstraintIsPivotJoint(constraint))
    {
        cpVect a = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpPivotJointGetAnchorA(constraint), cpBodyGetRotation(body_a)));
        cpVect b = cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpPivotJointGetAnchorB(constraint), cpBodyGetRotation(body_b)));
        
        renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
        renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
    }
    else if(cpConstraintIsGrooveJoint(constraint))
    {
        cpVect a = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpGrooveJointGetGrooveA(constraint), cpBodyGetRotation(body_a)));
        cpVect b = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpGrooveJointGetGrooveB(constraint), cpBodyGetRotation(body_a)));
        cpVect c = cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpGrooveJointGetAnchorB(constraint), cpBodyGetRotation(body_b)));
        
        renderer->drawDot(cpVert2Point(c), 3.0, CONSTRAINT_COLOR);
        renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
    }
    else if(cpConstraintIsDampedSpring(constraint))
    {
        // TODO: uninplemented
    }
    else
    {
        //        printf("Cannot draw constraint\n");
    }
}
static void DrawConstraint(cpConstraint *constraint, DrawNode *renderer)
{
    cpBody *body_a = constraint->a;
    cpBody *body_b = constraint->b;

    const cpConstraintClass *klass = constraint->CP_PRIVATE(klass);
    if (klass == cpPinJointGetClass())
    {
        cpPinJoint *joint = (cpPinJoint *)constraint;

        cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
        cpVect b = cpBodyLocal2World(body_b, joint->anchr2);

        renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
        renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
        renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
    }
    else if (klass == cpSlideJointGetClass())
    {
        cpSlideJoint *joint = (cpSlideJoint *)constraint;

        cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
        cpVect b = cpBodyLocal2World(body_b, joint->anchr2);

        renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
        renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
        renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
    }
    else if (klass == cpPivotJointGetClass())
    {
        cpPivotJoint *joint = (cpPivotJoint *)constraint;

        cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
        cpVect b = cpBodyLocal2World(body_b, joint->anchr2);

        renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
        renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
    }
    else if (klass == cpGrooveJointGetClass())
    {
        cpGrooveJoint *joint = (cpGrooveJoint *)constraint;

        cpVect a = cpBodyLocal2World(body_a, joint->grv_a);
        cpVect b = cpBodyLocal2World(body_a, joint->grv_b);
        cpVect c = cpBodyLocal2World(body_b, joint->anchr2);

        renderer->drawDot(cpVert2Point(c), 3.0, CONSTRAINT_COLOR);
        renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
    }
    else if (klass == cpDampedSpringGetClass())
    {
        // TODO
    }
    else
    {
        //		printf("Cannot draw constraint\n");
    }
}