Box2D tutorial 4 - joints

//http://www.youtube.com/user/thecplusplusguy
//Box2D joints example (revolute joint and wheel joint)
#include <iostream>
#include <SDL/SDL.h>
#include <SDL/SDL_image.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <Box2D/Box2D.h>

const int WIDTH=640;
const int HEIGHT=480;
const float M2P=20;
const float P2M=1/M2P;
b2World* world;


int ground=0;

b2Body* addRect(int x,int y,int w,int h,bool dyn=true)
{
    b2BodyDef bodydef;
    bodydef.position.Set(x*P2M,y*P2M);
    if(dyn)
        bodydef.type=b2_dynamicBody;
    b2Body* body=world->CreateBody(&bodydef);

    b2PolygonShape shape;
    shape.SetAsBox(P2M*w/2,P2M*h/2);

    b2FixtureDef fixturedef;
    fixturedef.shape=&shape;
    fixturedef.density=1.0;
    body->CreateFixture(&fixturedef);
    body->SetUserData(&ground);
    return body;
}

b2Body* addCircle(int x,int y,int r,bool dyn=true)
{
    b2BodyDef bodydef;
    bodydef.position.Set(x*P2M,y*P2M);
    if(dyn)
        bodydef.type=b2_dynamicBody;
    b2Body* body=world->CreateBody(&bodydef);

    b2CircleShape shape;
    shape.m_radius=r*P2M;
    shape.m_p.Set(0,0);

    b2FixtureDef fixturedef;
    fixturedef.shape=&shape;
    fixturedef.density=1.0;
    fixturedef.friction=0.8;
    body->CreateFixture(&fixturedef);
    body->SetUserData(&ground);
    return body;
}

void drawCircle(b2Vec2 center,float r,float angle)
{
    const int PRECISION=30;
    glColor3f(1,1,1);
    glPushMatrix();
        glTranslatef(center.x*M2P,center.y*M2P,0);
        glRotatef(angle*180.0/M_PI,0,0,1);
        glBegin(GL_TRIANGLE_FAN);
            glVertex2f(0,0);
            for(float i=0.0;i<=360;i+=360.0/PRECISION)
            {
                if(i<50)
                    glColor3f(1,0,0);
                else
                    glColor3f(1,1,1);
                glVertex2f((cos(i*M_PI/180.0)*r)*M2P,(sin(i*M_PI/180.0)*r)*M2P);
        }
        glEnd();
    glPopMatrix();
}

void drawSquare(b2Vec2* points,b2Vec2 center,float angle)
{
    glColor3f(1,1,1);
    glPushMatrix();
        glTranslatef(center.x*M2P,center.y*M2P,0);
        glRotatef(angle*180.0/M_PI,0,0,1);
        glBegin(GL_QUADS);
            for(int i=0;i<4;i++)
                glVertex2f(points[i].x*M2P,points[i].y*M2P);
        glEnd();
    glPopMatrix();
}

b2Body* myRect;
int num=6;
void init()
{
    glMatrixMode(GL_PROJECTION);
        glOrtho(0,WIDTH,HEIGHT,0,-1,1);
    glMatrixMode(GL_MODELVIEW);
    glClearColor(0,0,0,1);
    world=new b2World(b2Vec2(0.0,9.81));
    addRect(WIDTH/2,HEIGHT-50,WIDTH,30,false);
}

void display()
{
    glClear(GL_COLOR_BUFFER_BIT);
    glLoadIdentity();
    int x,y;
    SDL_GetMouseState(&x,&y);
    b2Vec2 mouse(x,y);
    //mouse-=(M2P*myRect->GetWorldCenter());
    //myRect->ApplyForce(mouse,myRect->GetWorldCenter());
    b2Body* tmp=world->GetBodyList();
    b2Vec2 points[4];
    while(tmp)
    {
        if(tmp->GetFixtureList()->GetShape()->GetType()==0)
        {
            b2CircleShape* c=((b2CircleShape*)tmp->GetFixtureList()->GetShape());
            drawCircle(tmp->GetWorldCenter(),c->m_radius,tmp->GetAngle());
        }else{
            for(int i=0;i<4;i++)
                points[i]=((b2PolygonShape*)tmp->GetFixtureList()->GetShape())->GetVertex(i);
            drawSquare(points,tmp->GetWorldCenter(),tmp->GetAngle());
        }
        tmp=tmp->GetNext();
    }
}

void createBridge(b2Body* sta1,b2Body* sta2,int w,int h)
{
    b2Vec2 pos1=M2P*(sta1->GetWorldCenter()+(((b2PolygonShape*)sta1->GetFixtureList()->GetShape())->GetVertex(1)));
    b2Vec2 pos2=M2P*(sta2->GetWorldCenter()+(((b2PolygonShape*)sta2->GetFixtureList()->GetShape())->GetVertex(0)));
    int num=(pos2.x-pos1.x)/w;
    b2RevoluteJointDef jointDef;
    jointDef.bodyA=sta1;
    b2Body* prev,*cur;
    prev=addRect(pos1.x+(w/2),pos1.y,w,h,true);
    jointDef.bodyB=prev;
    jointDef.localAnchorA.Set(((b2PolygonShape*)sta1->GetFixtureList()->GetShape())->GetVertex(1).x,0);
    jointDef.localAnchorB.Set(-w/2*P2M,0);
    world->CreateJoint(&jointDef);
    for(int i=0;i<num-1;i++)
    {
        cur=addRect(pos1.x+i*w,pos1.y,w,h,true);
        jointDef.bodyA=prev;
        jointDef.bodyB=cur;
        jointDef.localAnchorA.Set(w/2*P2M,0);
        jointDef.localAnchorB.Set(-w/2*P2M,0);
        world->CreateJoint(&jointDef);
        prev=cur;
    }
    jointDef.bodyA=prev;
    jointDef.bodyB=sta2;
    jointDef.localAnchorA.Set(w/2*P2M,0);
    jointDef.localAnchorB.Set((((b2PolygonShape*)sta2->GetFixtureList()->GetShape())->GetVertex(0)).x,0);
    world->CreateJoint(&jointDef);
}

b2WheelJoint* wheel;

void makeCar()
{
    b2Body* body=addRect(200,200,100,40);
    b2Body* wheel1=addCircle(150,220,15);
    b2Body* wheel2=addCircle(250,220,15);

    b2WheelJointDef jointDef;
    jointDef.bodyA=body;
    jointDef.bodyB=wheel1;
    jointDef.localAnchorA.Set(-50*P2M,20*P2M);
    jointDef.localAnchorB.Set(0,0);
    jointDef.enableMotor=true;
    jointDef.maxMotorTorque=100.0;
    jointDef.dampingRatio=0.1;
    wheel=(b2WheelJoint*)world->CreateJoint(&jointDef);

    jointDef.bodyA=body;
    jointDef.bodyB=wheel2;
    jointDef.localAnchorA.Set(50*P2M,20*P2M);
    jointDef.localAnchorB.Set(0,0);
    jointDef.enableMotor=false;
    jointDef.maxMotorTorque=0.0;
    world->CreateJoint(&jointDef);
}

int main()
{
    SDL_Init(SDL_INIT_EVERYTHING);
    SDL_SetVideoMode(640,480,32,SDL_OPENGL);
    Uint32 start;
    SDL_Event event;
    bool running=true;
    init();
    b2Body* sta1=addRect(100,300,50,10,false);
    b2Body* sta2=addRect(600,300,50,10,false);
    createBridge(sta1,sta2,50,10);
    makeCar();
    while(running)
    {
        start=SDL_GetTicks();
        while(SDL_PollEvent(&event))
        {
            switch(event.type)
            {
                case SDL_QUIT:
                    running=false;
                    break;
                case SDL_KEYDOWN:
                    switch(event.key.keysym.sym)
                    {
                        case SDLK_ESCAPE:
                            running=false;
                            break;
                        case SDLK_SPACE:
                            //myRect->ApplyLinearImpulse(b2Vec2(0,-40),myRect->GetWorldCenter()+b2Vec2(-25*P2M,-25*P2M));
                            break;
                    }
                    break;
                case SDL_MOUSEBUTTONDOWN:
                    //addRect(event.button.x,event.button.y,20,20,true);
                    addCircle(event.button.x,event.button.y,15,true);
                    break;

            }
        }
        Uint8* keys=SDL_GetKeyState(0);
        if(keys[SDLK_d])
            wheel->SetMotorSpeed(50);
        if(keys[SDLK_a])
            wheel->SetMotorSpeed(-50);

        display();
        world->Step(1.0/30.0,8,3);  //update
        SDL_GL_SwapBuffers();
        if(1000.0/30>SDL_GetTicks()-start)
            SDL_Delay(1000.0/30-(SDL_GetTicks()-start));
    }
    SDL_Quit();
}