[Box2d + SFML 2.0] problem z prismatic joints (wyginają się)

#include <Box2D/Box2D.h>
#include <SFML/Graphics.hpp>
#include <iostream>


#define RADTODEG(v)     (v*(180.0f / M_PI))
#define DEGTORAD(v)     (v*(M_PI / 180.0f))

class Box2dDebug
    : public b2Draw
{
public:
   
    sf::RenderWindow * window;
   
    Box2dDebug( sf::RenderWindow & win )
    {
        window = & win;
    }
   
    void DrawPolygon( const b2Vec2 * vertices, int32 vertexCount, const b2Color & color )
    {
        this->DrawSolidPolygon( vertices, vertexCount, color );
    }
    void DrawSolidPolygon( const b2Vec2 * vertices, int32 vertexCount, const b2Color & color )
    {
        sf::ConvexShape polygon;
        polygon.setPointCount( vertexCount );
        for( int q = 0; q < vertexCount; ++q ) {
            b2Vec2 v = vertices[ q ];
            polygon.setPoint( q, sf::Vector2f(( v.x ),( v.y ) ) );
        }
        polygon.setFillColor( sf::Color::Transparent );
        polygon.setOutlineColor( sf::Color( color.r, color.b, color.g, 255 ) );
        polygon.setOutlineThickness( 1 );
        window->draw( polygon );
    }
    void DrawCircle( const b2Vec2 & center, float32 radius, const b2Color & color )
    {
        this->DrawSolidCircle( center, radius, b2Vec2( 0, 0 ), color );
    }
    void DrawSolidCircle( const b2Vec2 & center, float32 radius, const b2Vec2 & axis, const b2Color & color )
    {
        sf::CircleShape circle(( radius ), 15 );
        circle.setPosition( sf::Vector2f(( center.x ) -( radius ),( center.y ) -( radius ) ) );
        circle.setFillColor( sf::Color::Transparent );
        circle.setOutlineColor( sf::Color( color.r, color.b, color.g, 255 ) );
        circle.setOutlineThickness( 1 );
        window->draw( circle );
    }
    void DrawSegment( const b2Vec2 & p1, const b2Vec2 & p2, const b2Color & color )
    {
        DrawSolidCircle( p1, 3, b2Vec2( 0, 0 ), b2Color( 255, 0, 0 ) );
    }
    void DrawTransform( const b2Transform & xf )
    {
    }
   
};

b2Body * addBox( b2World & world, float w, float h, float x, float y, float a, b2BodyType type )
{
    b2BodyDef bodydef;
    bodydef.type = type;
    bodydef.position.Set(( x ),( y ) );
    bodydef.angle = a;
    //bodydef.fixedRotation = true;
    b2Body * body = world.CreateBody( & bodydef );
   
    b2PolygonShape polygonShape;
    polygonShape.SetAsBox(( w ) * 0.5,( h ) * 0.5 );
   
    b2FixtureDef fixture;
    fixture.density = 1.605; //gęstość ciezar
    fixture.friction = 0.05; //Współczynnik tarcia
    fixture.restitution = 0.3; //bump
    fixture.shape = & polygonShape;
    body->CreateFixture( & fixture );
   
    return body;
}
b2RevoluteJointDef RevoluteJoint( b2Body *& b1, b2Body *& b2, b2Vec2 anchor )
{
    b2RevoluteJointDef revoluteJointDef;
   
    revoluteJointDef.Initialize( b1, b2, anchor );
    revoluteJointDef.motorSpeed = 100;
    revoluteJointDef.maxMotorTorque = 50000;
    revoluteJointDef.enableMotor = true;
    revoluteJointDef.enableLimit = true;
    return revoluteJointDef;
   
}
b2PrismaticJointDef PrismaticJoint( b2Body *& b1, b2Body *& b2, b2Vec2 anchor )
{
    b2PrismaticJointDef prismaticJointDef;
    b2Vec2 axis( 0, 1 );
    axis.Normalize();
   
    prismaticJointDef.Initialize( b1, b2, anchor, axis );
    prismaticJointDef.upperTranslation = 40; // ???
    prismaticJointDef.lowerTranslation =- 40; // ???
    prismaticJointDef.motorSpeed = 2510.1;
    prismaticJointDef.maxMotorForce = 10000.0;
    prismaticJointDef.enableMotor = true;
    prismaticJointDef.enableLimit = true;
    return prismaticJointDef;
}


int main()
{
    std::cout << "SFML v" << SFML_VERSION_MAJOR << "." << SFML_VERSION_MINOR << std::endl;
    std::cout << "Box2d v" << b2_version.major << "." << b2_version.minor << "." << b2_version.revision << std::endl;
   
    sf::RenderWindow Window( sf::VideoMode( 800, 600, 32 ), "Test" );
    b2World world( b2Vec2( 0.0, 10 ) );
    Box2dDebug Box2dDebug( Window );
    Box2dDebug.SetFlags( b2Draw::e_shapeBit | b2Draw::e_jointBit ); // | b2Draw::e_centerOfMassBit | b2Draw::e_jointBit | b2Draw::e_aabbBit | b2Draw::e_pairBit
    world.SetDebugDraw( & Box2dDebug );
   
    b2Body * staticbody2 = addBox( world, 1000, 10, Window.getSize().x, 300, DEGTORAD( 90.0 ), b2_staticBody );
    b2Body * staticbody3 = addBox( world, 1000, 10, 0, 300, DEGTORAD( 90.0 ), b2_staticBody );
    b2Body * staticbody4 = addBox( world, 1000, 10, Window.getSize().x * 0.5, Window.getSize().y, DEGTORAD( 0 ), b2_staticBody );
    b2Body * staticbody5 = addBox( world, 1000, 10, Window.getSize().x * 0.5, 0, DEGTORAD( 0 ), b2_staticBody );
   
    // BODY
    b2Body * A = addBox( world, 90, 20, Window.getSize().x * 0.5, 30, DEGTORAD( 0 ), b2_dynamicBody );
    b2MassData md;
    A->GetMassData( & md );
    md.mass = A->GetMass() / 2.0;
    A->SetMassData( & md );
   
    //LEGS
    b2Body * B = addBox( world, 20, 20, Window.getSize().x * 0.5, 190, DEGTORAD( 0 ), b2_dynamicBody );
    b2Body * C = addBox( world, 20, 20, Window.getSize().x * 0.5, 190, DEGTORAD( 0 ), b2_dynamicBody );
   
    b2PrismaticJointDef pj1 = PrismaticJoint( A, B, B->GetWorldCenter() );
    pj1.localAnchorA.x = 80;
    world.CreateJoint( & pj1 );
   
    b2PrismaticJointDef pj2 = PrismaticJoint( A, C, C->GetWorldCenter() );
    pj2.localAnchorA.x = - 80;
    world.CreateJoint( & pj2 );
   
    while( Window.isOpen() )
    {
        sf::Event event;
        while( Window.pollEvent( event ) )
        {
            if( event.type == sf::Event::Closed )
                 Window.close();
           
        }
        Window.clear( sf::Color( 127, 127, 127, 0 ) );
        world.Step( 1.0 / 60.0, 16, 8 );
        world.DrawDebugData();
        Window.display();
    }
    return 0;
}