[Box2d] Wykrywanie "czy można skoczyć"

#ifndef FOOTEST_H
#define FOOTEST_H
#define DEGTORAD 0.0174532925199432957f
#define RADTODEG 57.295779513082320876f
#include <string>

int numFootContact;

class MyContactListener
    : public b2ContactListener
{
    virtual void BeginContact( b2Contact * contact )
    {
        void * fixtureUserData = contact->GetFixtureA()->GetUserData();
        if(( int ) fixtureUserData == 3 )
             numFootContact++;
       
        fixtureUserData = contact->GetFixtureB()->GetUserData();
        if(( int ) fixtureUserData == 3 )
             numFootContact++;
       
    }
    virtual void EndContact( b2Contact * contact )
    {
        void * fixtureUserData = contact->GetFixtureA()->GetUserData();
        if(( int ) fixtureUserData == 3 )
             numFootContact--;
       
        fixtureUserData = contact->GetFixtureB()->GetUserData();
        if(( int ) fixtureUserData == 3 )
             numFootContact--;
       
    }
};

MyContactListener * MyContactListenerIstance;

class FooTest
    : public Test
{
public:
    enum _moveState { MS_STOP, MS_LEFT, MS_RIGHT };
    _moveState moveState;
    b2Body * m_body;
    float remainingJumpSteps;
    int m_jumpTimeout;
   
   
    FooTest()
    {
        m_jumpTimeout = 0;
        //body definition
        b2BodyDef myBodyDef;
        myBodyDef.type = b2_dynamicBody;
        myBodyDef.position.Set( - 5, 5 );
       
        b2PolygonShape polygonShape;
        polygonShape.SetAsBox( 1, 1 );
       
        b2FixtureDef myFixtureDef;
        myFixtureDef.shape = & polygonShape;
        myFixtureDef.density = 1;
       
        m_body = m_world->CreateBody( & myBodyDef );
        m_body->CreateFixture( & myFixtureDef );
       
        for( int i = 0; i < 5; i++ )
        {
            b2Fixture * fixture = m_world->CreateBody( & myBodyDef )->CreateFixture( & myFixtureDef );
            fixture->SetUserData(( void * ) 1 );
        }
       
        polygonShape.SetAsBox( 0.5f, 1 );
       
        for( int i = 0; i < 5; i++ )
        {
            b2Fixture * fixture = m_world->CreateBody( & myBodyDef )->CreateFixture( & myFixtureDef );
            fixture->SetUserData(( void * ) 2 );
        }
       
        myBodyDef.type = b2_staticBody;
        myBodyDef.position.Set( 0, 0 );
        b2Body * staticBody = m_world->CreateBody( & myBodyDef );
       
        //add four walls to the static body
        polygonShape.SetAsBox( 20, 1, b2Vec2( 0, 0 ), 0 ); //ground
        staticBody->CreateFixture( & myFixtureDef );
        polygonShape.SetAsBox( 20, 1, b2Vec2( 0, 40 ), 0 ); //ceiling
        staticBody->CreateFixture( & myFixtureDef );
        polygonShape.SetAsBox( 1, 20, b2Vec2( - 20, 20 ), 0 ); //left wall
        staticBody->CreateFixture( & myFixtureDef );
        polygonShape.SetAsBox( 1, 20, b2Vec2( 20, 20 ), 0 ); //right wall
        staticBody->CreateFixture( & myFixtureDef );
       
        //body player
        {
            b2BodyDef myBodyDef;
            myBodyDef.position.Set( 0, 10 );
            myBodyDef.type = b2_dynamicBody;
            myBodyDef.fixedRotation = true;
           
            b2PolygonShape polygonShape;
            polygonShape.SetAsBox( 1, 2 ); // 2x4 rectangle
           
            b2FixtureDef myFixtureDef;
            myFixtureDef.shape = & polygonShape;
            myFixtureDef.density = 1;
           
            m_body = m_world->CreateBody( & myBodyDef );
           
            m_body->CreateFixture( & myFixtureDef );
           
            //add foot sensor
           
            polygonShape.SetAsBox( 0.3f, 0.3f, b2Vec2( 0, - 2 ), 0 );
            myFixtureDef.isSensor = true;
            b2Fixture * footSensorFixture = m_body->CreateFixture( & myFixtureDef );
            footSensorFixture->SetUserData(( void * ) 3 );
        }
       
        moveState = MS_STOP;
        remainingJumpSteps = 0;
       
        m_world->SetContactListener( MyContactListenerIstance );
    };
    void Step( Settings * settings )
    {
        //run the default physics and rendering
        Test::Step( settings );
       
        m_jumpTimeout--;
       
        //show some text in the main screen
        m_debugDraw.DrawString( 5, m_textLine, "Now we have a foo test" );
        m_textLine += 15;
        m_debugDraw.DrawString( 5, m_textLine, "Can I jump here? %s", numFootContact > 0 ? "yes": "no" );
        m_textLine += 15;
       
        b2Vec2 vel = m_body->GetLinearVelocity();
        float desiredVel = 0;
        switch( moveState )
        {
        case MS_LEFT: desiredVel = - 5; break;
        case MS_STOP: desiredVel = 0; break;
        case MS_RIGHT: desiredVel = 5; break;
        }
        float velChange = desiredVel - vel.x;
        float force = m_body->GetMass() * velChange /( 1 / 60.0 ); //f = mv/t
        m_body->ApplyForce( b2Vec2( force, 0 ), m_body->GetWorldCenter() );
       
       
    }
   
    static Test * Create()
    {
        return new FooTest;
    }
   
    void Keyboard( unsigned char key )
    {
        switch( key )
        {
        case 'q': //move left
            moveState = MS_LEFT;
            break;
        case 'w': //stop
            moveState = MS_STOP;
            break;
        case 'e': //move right
            moveState = MS_RIGHT;
            break;
        case 'j':
            {
                if( numFootContact < 1 ) break;
               
                if( m_jumpTimeout > 0 ) break;
               
                m_body->ApplyLinearImpulse( b2Vec2( 0, m_body->GetMass() * 10 ), m_body->GetWorldCenter() );
                m_jumpTimeout = 15;
                break;
            }
            default:
            //run default behaviour
            Test::Keyboard( key );
        }
    }
};

#endif