示例#1
0
void BasicRobot::SetRotation (float qx, float qy, float qz, float qa)
{
    btTransform t = body->getCenterOfMassTransform();
    t.setRotation(btQuaternion(btVector3 (qx, qy, qz), qa));
    body->setCenterOfMassTransform(t);
}
示例#2
0
文件: menu.cpp 项目: xxicemanx/Plinko
void processStateMenuEvents( int option ){

#if 0
   vec3 motion;

   switch( option ){

      case 0:
         options.view_mode = 4;
         break;
      case 1:
         //reset score
         options.p1_score = 0;
         options.p2_score = 0;
         options.hud.p1_score(options.p1_score);
         options.hud.p2_score(options.p2_score);

         //reset puck and paddle position
         options.physics.puckRigidBody->setWorldTransform(btTransform(btQuaternion(1,0,0,0),btVector3(0, 0, 0)));
         options.physics.paddle1RigidBody->setWorldTransform(btTransform(btQuaternion(1,0,0,0),btVector3(-3, 0, 0)));
         options.physics.paddle2RigidBody->setWorldTransform(btTransform(btQuaternion(1,0,0,0),btVector3( 3, 0, 0)));

         options.set_paddle1_dest(vec2(-3.0, 0.0));
         options.set_paddle2_dest(vec2( 3.0, 0.0));

         /*     Start Puck     */
         //get an update of the motion state
         options.physics.puckRigidBody->getMotionState()->getWorldTransform( options.physics.puck_trans );

         //load new position into structure for paddle1 model
         motion = vec3( options.physics.paddle1_trans.getOrigin().getX(), options.physics.paddle1_trans.getOrigin().getY(), options.physics.paddle1_trans.getOrigin().getZ());

         //push new position into paddle1
         options.puck->set_translation( motion );
         /*   End puck    */

         /*    Start paddle 1    */
         //get an update of the motion state
         options.physics.paddle1RigidBody->getMotionState()->getWorldTransform( options.physics.paddle1_trans );

         //load new position into structure for paddle1 model
         motion = vec3( options.physics.paddle1_trans.getOrigin().getX(), options.physics.paddle1_trans.getOrigin().getY(), options.physics.paddle1_trans.getOrigin().getZ());

         //push new position into paddle1
         options.paddle1->set_translation( motion );
         /*    End paddle 1   */
         /*    Start paddle 2    */
         //get an update of the motion state
         options.physics.paddle2RigidBody->getMotionState()->getWorldTransform( options.physics.paddle2_trans );

         //load new position into structure for paddle2 model
         motion = vec3( options.physics.paddle2_trans.getOrigin().getX(), options.physics.paddle2_trans.getOrigin().getY(), options.physics.paddle2_trans.getOrigin().getZ());

         //push new position into paddle2
         options.paddle2->set_translation( motion );
         /*    End paddle 2   */

         // reset velocities
         options.physics.paddle1RigidBody->setLinearVelocity(  btVector3(0.0,0.0,0.0));
         options.physics.paddle1RigidBody->setAngularVelocity( btVector3(0.0,0.0,0.0));

         options.physics.paddle2RigidBody->setLinearVelocity(  btVector3(0.0,0.0,0.0));
         options.physics.paddle2RigidBody->setAngularVelocity( btVector3(0.0,0.0,0.0));

         options.physics.puckRigidBody->setLinearVelocity(  btVector3(0.0,0.0,0.0));
         options.physics.puckRigidBody->setAngularVelocity( btVector3(0.0,0.0,0.0));


         break;
      case 2:
         options.view_mode = 0; 
         break;

   }
#endif
}
示例#3
0
   void BulletPhysicsEngine::update()
   {
      for (U32 i = 0; i < 1024; ++i)
      {
         BulletPhysicsObject* obj = &mPhysicsObjects[i];
         if ( obj->deleted )
            continue;

         if ( obj->shouldBeDeleted )
         {
            if ( obj->initialized )
            {
               mDynamicsWorld->removeRigidBody(obj->_rigidBody);
               obj->destroy();
            }

            obj->deleted = true;
            continue;
         }
      }

      for (U32 i = 0; i < 1024; ++i)
      {
         BulletPhysicsObject* obj = &mPhysicsObjects[i];
         if ( obj->deleted )
            continue;

         if ( !obj->initialized )
         {
            obj->initialize();
            mDynamicsWorld->addRigidBody(obj->_rigidBody);
         } else {
            // Pull updates from Physics thread.
            btMotionState* objMotion = obj->_rigidBody->getMotionState();
            if ( objMotion )
            {
               btTransform trans;
               objMotion->getWorldTransform(trans);

               F32 mat[16];
               trans.getOpenGLMatrix(mat);

               obj->mPosition.set(mat[12], mat[13], mat[14]);
               btQuaternion rot = trans.getRotation();
               obj->mRotation.set(QuatToEuler(rot.x(), rot.y(), rot.z(), rot.w()));
            }

            // Apply actions from Game thread.
            while ( obj->mPhysicsActions.size() > 0 )
            {
               Physics::PhysicsAction action = obj->mPhysicsActions.front();

               switch(action.actionType)
               {
                  case Physics::PhysicsAction::setPosition:
                     obj->mPosition = action.vector3Value;
                     obj->_rigidBody->setWorldTransform(btTransform(btQuaternion(0, 0, 0, 1),btVector3(action.vector3Value.x, action.vector3Value.y, action.vector3Value.z)));
                     obj->_rigidBody->activate();
                     break;

                  case Physics::PhysicsAction::setLinearVelocity:
                     obj->_rigidBody->setLinearVelocity(btVector3(action.vector3Value.x * 25.0f, action.vector3Value.y * 25.0f, action.vector3Value.z * 25.0f));
                     obj->_rigidBody->activate();
                     break;
               }

               obj->mPhysicsActions.pop_front();
            }
         }
      }
   }
示例#4
0
/** Loads a history from history.dat in the current directory.
 */
void History::Load()
{
    char s[1024], s1[1024];
    int  n;
    FILE *fd = fopen("history.dat","r");
    if (fd == NULL)
    {
        fprintf(stderr, "ERROR: could not open history.dat\n");
        exit(-2);
    }
    
    if (fgets(s, 1023, fd) == NULL)
    {
        fprintf(stderr, "ERROR: could not read history.dat\n");
        exit(-2);
    }
    
    if (sscanf(s,"Version: %s",s1)!=1)
    {
        fprintf(stderr, "ERROR: no Version information found in history file (bogus history file)\n");
        exit(-2);
    }
    else
    {
#ifdef VERSION
        if (strcmp(s1,VERSION))
        {
            fprintf(stderr, "WARNING: history is version '%s'\n",s1);
            fprintf(stderr, "         STK version is '%s'\n",VERSION);
        }
#endif
    }
    
    if (fgets(s, 1023, fd) == NULL)
    {
        fprintf(stderr, "ERROR: could not read history.dat\n");
        exit(-2);
    }
    
    unsigned int num_karts;
    if(sscanf(s, "numkarts: %d",&num_karts)!=1)
    {
        fprintf(stderr,"WARNING: No number of karts found in history file.\n");
        exit(-2);
    }
    race_manager->setNumKarts(num_karts);

    fgets(s, 1023, fd);
    if(sscanf(s, "numplayers: %d",&n)!=1)
    {
        fprintf(stderr,"WARNING: No number of players found in history file.\n");
        exit(-2);
    }
    race_manager->setNumPlayers(n);

    fgets(s, 1023, fd);
    if(sscanf(s, "difficulty: %d",&n)!=1)
    {
        fprintf(stderr,"WARNING: No difficulty found in history file.\n");
        exit(-2);
    }
    race_manager->setDifficulty((RaceManager::Difficulty)n);

    fgets(s, 1023, fd);
    if(sscanf(s, "track: %s",s1)!=1)
    {
        fprintf(stderr,"WARNING: Track not found in history file.\n");
    }
    race_manager->setTrack(s1);
    // This value doesn't really matter, but should be defined, otherwise
    // the racing phase can switch to 'ending'
    race_manager->setNumLaps(10);

    for(unsigned int i=0; i<num_karts; i++)
    {
        fgets(s, 1023, fd);
        if(sscanf(s, "model %d: %s",&n, s1)!=2)
        {
            fprintf(stderr,"WARNING: No model information for kart %d found.\n",
                    i);
            exit(-2);
        }
        m_kart_ident.push_back(s1);
        if(i<race_manager->getNumPlayers())
        {
            race_manager->setLocalKartInfo(i, s1);
        }
    }   // for i<nKarts
    // FIXME: The model information is currently ignored
    fgets(s, 1023, fd);
    if(sscanf(s,"size: %d",&m_size)!=1)
    {
        fprintf(stderr,"WARNING: Number of records not found in history file.\n");
        exit(-2);
    }
    allocateMemory(m_size);
    m_current = -1;

    for(int i=0; i<m_size; i++)
    {
        fgets(s, 1023, fd);
        sscanf(s, "delta: %f\n",&m_all_deltas[i]);
    }

    for(int i=0; i<m_size; i++)
    {
        int j=0;
        for(unsigned int k=0; k<num_karts; k++)
        {
            fgets(s, 1023, fd);
            int buttonsCompressed;
            float x,y,z,rx,ry,rz,rw;
            sscanf(s, "%d %f %f %d  %f %f %f  %f %f %f %f\n",
                    &j, 
                    &m_all_controls[i].m_steer,
                    &m_all_controls[i].m_accel,
                    &buttonsCompressed,
                    &x, &y, &z,
                    &rx, &ry, &rz, &rw
                    );
            unsigned int index     = num_karts * i+k;
            m_all_xyz[index]       = Vec3(x,y,z);
            m_all_rotations[index] = btQuaternion(rx,ry,rz,rw);
            m_all_controls[index].setButtonsCompressed(char(buttonsCompressed));
        }   // for i
    }   // for k
    fprintf(fd, "History file end.\n");
    fclose(fd);
}   // Load
void RollingFrictionDemo::initPhysics()
{
	m_guiHelper->setUpAxis(2);

	///collision configuration contains default setup for memory, collision setup
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	//m_collisionConfiguration->setConvexConvexMultipointIterations();

	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);

	m_broadphase = new btDbvtBroadphase();

	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;

	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
	//	m_dynamicsWorld->getSolverInfo().m_singleAxisRollingFrictionThreshold = 0.f;//faster but lower quality
	m_dynamicsWorld->setGravity(btVector3(0, 0, -10));

	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);

	{
		///create a few basic rigid bodies
		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(25.)));

		m_collisionShapes.push_back(groundShape);

		btTransform groundTransform;
		groundTransform.setIdentity();
		groundTransform.setOrigin(btVector3(0, 0, -28));
		groundTransform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI * 0.03));
		//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
		btScalar mass(0.);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0, 0, 0);
		if (isDynamic)
			groundShape->calculateLocalInertia(mass, localInertia);

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);
		body->setFriction(.5);

		//add the body to the dynamics world
		m_dynamicsWorld->addRigidBody(body);
	}

	{
		///create a few basic rigid bodies
		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.), btScalar(100.), btScalar(50.)));

		m_collisionShapes.push_back(groundShape);

		btTransform groundTransform;
		groundTransform.setIdentity();
		groundTransform.setOrigin(btVector3(0, 0, -54));
		//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
		btScalar mass(0.);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0, 0, 0);
		if (isDynamic)
			groundShape->calculateLocalInertia(mass, localInertia);

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);
		body->setFriction(.1);
		//add the body to the dynamics world
		m_dynamicsWorld->addRigidBody(body);
	}

	{
		//create a few dynamic rigidbodies
		// Re-using the same collision is better for memory usage and performance
#define NUM_SHAPES 10
		btCollisionShape* colShapes[NUM_SHAPES] = {
			new btSphereShape(btScalar(0.5)),
			new btCapsuleShape(0.25, 0.5),
			new btCapsuleShapeX(0.25, 0.5),
			new btCapsuleShapeZ(0.25, 0.5),
			new btConeShape(0.25, 0.5),
			new btConeShapeX(0.25, 0.5),
			new btConeShapeZ(0.25, 0.5),
			new btCylinderShape(btVector3(0.25, 0.5, 0.25)),
			new btCylinderShapeX(btVector3(0.5, 0.25, 0.25)),
			new btCylinderShapeZ(btVector3(0.25, 0.25, 0.5)),
		};
		for (int i = 0; i < NUM_SHAPES; i++)
			m_collisionShapes.push_back(colShapes[i]);

		/// Create Dynamic Objects
		btTransform startTransform;
		startTransform.setIdentity();

		btScalar mass(1.f);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static

		float start_x = START_POS_X - ARRAY_SIZE_X / 2;
		float start_y = START_POS_Y;
		float start_z = START_POS_Z - ARRAY_SIZE_Z / 2;

		{
			int shapeIndex = 0;
			for (int k = 0; k < ARRAY_SIZE_Y; k++)
			{
				for (int i = 0; i < ARRAY_SIZE_X; i++)
				{
					for (int j = 0; j < ARRAY_SIZE_Z; j++)
					{
						startTransform.setOrigin(SCALING * btVector3(
															   btScalar(2.0 * i + start_x),
															   btScalar(2.0 * j + start_z),
															   btScalar(20 + 2.0 * k + start_y)));

						shapeIndex++;
						btCollisionShape* colShape = colShapes[shapeIndex % NUM_SHAPES];
						bool isDynamic = (mass != 0.f);
						btVector3 localInertia(0, 0, 0);

						if (isDynamic)
							colShape->calculateLocalInertia(mass, localInertia);

						//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
						btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
						btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
						btRigidBody* body = new btRigidBody(rbInfo);
						body->setFriction(1.f);
						body->setRollingFriction(.1);
						body->setSpinningFriction(0.1);
						body->setAnisotropicFriction(colShape->getAnisotropicRollingFrictionDirection(), btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);

						m_dynamicsWorld->addRigidBody(body);
					}
				}
			}
		}
	}

	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

	if (0)
	{
		btSerializer* s = new btDefaultSerializer;
		m_dynamicsWorld->serialize(s);
		char resourcePath[1024];
		if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024))
		{
			FILE* f = fopen(resourcePath, "wb");
			fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
			fclose(f);
		}
	}
}
    virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
    {
        //skip self-collisions
        if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
            return;

        //skip duplicates (disabled for now)
        //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
        //	return;

        //search for shared vertices and edges
        int numshared = 0;
        int sharedVertsA[3]={-1,-1,-1};
        int sharedVertsB[3]={-1,-1,-1};

        ///skip degenerate triangles
        btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
        if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
            return;


        btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
        ///skip degenerate triangles
        if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
            return;

#if 0
        printf("triangle A[0]	=	(%f,%f,%f)\ntriangle A[1]	=	(%f,%f,%f)\ntriangle A[2]	=	(%f,%f,%f)\n",
            m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
            m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
            m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());

        printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
        printf("triangle B[0]	=	(%f,%f,%f)\ntriangle B[1]	=	(%f,%f,%f)\ntriangle B[2]	=	(%f,%f,%f)\n",
            triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
            triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
            triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
#endif

        for (int i=0;i<3;i++)
        {
            for (int j=0;j<3;j++)
            {
                if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
                {
                    sharedVertsA[numshared] = i;
                    sharedVertsB[numshared] = j;
                    numshared++;
                    ///degenerate case
                    if(numshared >= 3)
                        return;
                }
            }
            ///degenerate case
            if(numshared >= 3)
                return;
        }

        switch (numshared)
        {
        case 0:
            {
                break;
            }
        case 1:
            {
                //shared vertex
                break;
            }
        case 2:
            {
                //shared edge
                //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
                if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
                {
                    sharedVertsA[0] = 2;
                    sharedVertsA[1] = 0;
                    int tmp = sharedVertsB[1];
                    sharedVertsB[1] = sharedVertsB[0];
                    sharedVertsB[0] = tmp;
                }

                int hash = btGetHash(m_partIdA,m_triangleIndexA);

                btTriangleInfo* info = m_triangleInfoMap->find(hash);
                if (!info)
                {
                    btTriangleInfo tmp;
                    m_triangleInfoMap->insert(hash,tmp);
                    info = m_triangleInfoMap->find(hash);
                }

                int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
                int otherIndexA = 3-sumvertsA;


                btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);

                btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
                int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);

                btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
                //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);

                btVector3 normalA;
                btVector3 normalB;
                tA.calcNormal(normalA);
                tB.calcNormal(normalB);
                edge.normalize();
                btVector3 edgeCrossA = edge.cross(normalA).normalize();

                {
                    btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
                    if (edgeCrossA.dot(tmp) < 0)
                    {
                        edgeCrossA*=-1;
                    }
                }

                btVector3 edgeCrossB = edge.cross(normalB).normalize();

                {
                    btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
                    if (edgeCrossB.dot(tmp) < 0)
                    {
                        edgeCrossB*=-1;
                    }
                }

                btScalar	angle2 = 0;
                btScalar	ang4 = 0.f;


                btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
                btScalar len2 = calculatedEdge.length2();

                btScalar correctedAngle(0);
                btVector3 calculatedNormalB = normalA;
                bool isConvex = false;

                if (len2<m_triangleInfoMap->m_planarEpsilon)
                {
                    angle2 = 0.f;
                    ang4 = 0.f;
                } else
                {
                    calculatedEdge.normalize();
                    btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
                    calculatedNormalA.normalize();
                    angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
                    ang4 = SIMD_PI-angle2;
                    btScalar dotA = normalA.dot(edgeCrossB);
                    ///@todo: check if we need some epsilon, due to floating point imprecision
                    isConvex = (dotA<0.);

                    correctedAngle = isConvex ? ang4 : -ang4;
                    btQuaternion orn2 = btQuaternion(btVector3(calculatedEdge.x(), calculatedEdge.y(), calculatedEdge.z()),-correctedAngle);
                    calculatedNormalB = btMatrix3x3(orn2)*normalA;


                }





                //alternatively use
                //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);


                switch (sumvertsA)
                {
                case 1:
                    {
                        btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
                        btQuaternion orn(edge,-correctedAngle);
                        btVector3 computedNormalB = quatRotate(orn,normalA);
                        btScalar bla = computedNormalB.dot(normalB);
                        if (bla<0)
                        {
                            computedNormalB*=-1;
                            info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
                        }
#ifdef DEBUG_INTERNAL_EDGE
                        if ((computedNormalB-normalB).length()>0.0001)
                        {
                            printf("warning: normals not identical\n");
                        }
#endif//DEBUG_INTERNAL_EDGE

                        info->m_edgeV0V1Angle = -correctedAngle;

                        if (isConvex)
                            info->m_flags |= TRI_INFO_V0V1_CONVEX;
                        break;
                    }
                case 2:
                    {
                        btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
                        btQuaternion orn(edge,-correctedAngle);
                        btVector3 computedNormalB = quatRotate(orn,normalA);
                        if (computedNormalB.dot(normalB)<0)
                        {
                            computedNormalB*=-1;
                            info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
                        }

#ifdef DEBUG_INTERNAL_EDGE
                        if ((computedNormalB-normalB).length()>0.0001)
                        {
                            printf("warning: normals not identical\n");
                        }
#endif //DEBUG_INTERNAL_EDGE
                        info->m_edgeV2V0Angle = -correctedAngle;
                        if (isConvex)
                            info->m_flags |= TRI_INFO_V2V0_CONVEX;
                        break;
                    }
                case 3:
                    {
                        btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
                        btQuaternion orn(edge,-correctedAngle);
                        btVector3 computedNormalB = quatRotate(orn,normalA);
                        if (computedNormalB.dot(normalB)<0)
                        {
                            info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
                            computedNormalB*=-1;
                        }
#ifdef DEBUG_INTERNAL_EDGE
                        if ((computedNormalB-normalB).length()>0.0001)
                        {
                            printf("warning: normals not identical\n");
                        }
#endif //DEBUG_INTERNAL_EDGE
                        info->m_edgeV1V2Angle = -correctedAngle;

                        if (isConvex)
                            info->m_flags |= TRI_INFO_V1V2_CONVEX;
                        break;
                    }
                }

                break;
            }
        }
    }
void TestJointTorqueSetup::initPhysics()
{
    int upAxis = 1;
	gJointFeedbackInWorldSpace = true;
	gJointFeedbackInJointFrame = true;

	m_guiHelper->setUpAxis(upAxis);

    btVector4 colors[4] =
    {
        btVector4(1,0,0,1),
        btVector4(0,1,0,1),
        btVector4(0,1,1,1),
        btVector4(1,1,0,1),
    };
    int curColor = 0;



    

	this->createEmptyDynamicsWorld();
    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
        //btIDebugDraw::DBG_DrawConstraints
        +btIDebugDraw::DBG_DrawWireframe
        +btIDebugDraw::DBG_DrawContactPoints
        +btIDebugDraw::DBG_DrawAabb
        );//+btIDebugDraw::DBG_DrawConstraintLimits);

	

    //create a static ground object
    if (1)
        {
            btVector3 groundHalfExtents(1,1,0.2);
            groundHalfExtents[upAxis]=1.f;
            btBoxShape* box = new btBoxShape(groundHalfExtents);
            box->initializePolyhedralFeatures();

            m_guiHelper->createCollisionShapeGraphicsObject(box);
            btTransform start; start.setIdentity();
            btVector3 groundOrigin(-0.4f, 3.f, 0.f);
            groundOrigin[upAxis] -=.5;
			groundOrigin[2]-=0.6;
            start.setOrigin(groundOrigin);
			btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
		
		//	start.setRotation(groundOrn);
            btRigidBody* body =  createRigidBody(0,start,box);
			body->setFriction(0);
            btVector4 color = colors[curColor];
			curColor++;
			curColor&=3;
            m_guiHelper->createRigidBodyGraphicsObject(body,color);
        }

    {
        bool floating = false;
        bool damping = false;
        bool gyro = false;
        int numLinks = 2;
        bool spherical = false;					//set it ot false -to use 1DoF hinges instead of 3DoF sphericals
        bool canSleep = false;
        bool selfCollide = false;
          btVector3 linkHalfExtents(0.05, 0.37, 0.1);
        btVector3 baseHalfExtents(0.05, 0.37, 0.1);

        btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
        //mbC->forceMultiDof();							//if !spherical, you can comment this line to check the 1DoF algorithm
        //init the base
        btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
        float baseMass = 1.f;

        if(baseMass)
        {
            //btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
			btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
            shape->calculateLocalInertia(baseMass, baseInertiaDiag);
            delete shape;
        }


        btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
		
        m_multiBody = pMultiBody;
        btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
	//	baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
        pMultiBody->setBasePos(basePosition);
        pMultiBody->setWorldToBaseRot(baseOriQuat);
        btVector3 vel(0, 0, 0);
    //	pMultiBody->setBaseVel(vel);

        //init the links
        btVector3 hingeJointAxis(1, 0, 0);
        
        //y-axis assumed up
        btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0);						//par body's COM to cur body's COM offset
        btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);							//cur body's COM to cur body's PIV offset
        btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;	//par body's COM to cur body's PIV offset

        //////
        btScalar q0 = 0.f * SIMD_PI/ 180.f;
        btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
        quat0.normalize();
        /////

        for(int i = 0; i < numLinks; ++i)
        {
			float linkMass = 1.f;
			//if (i==3 || i==2)
			//	linkMass= 1000;
			btVector3 linkInertiaDiag(0.f, 0.f, 0.f);

			btCollisionShape* shape = 0;
			if (i==0)
			{
				shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
			} else
			{
				shape = new btSphereShape(radius);
			}
			shape->calculateLocalInertia(linkMass, linkInertiaDiag);
			delete shape;


            if(!spherical)
			{
                //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
		
				if (i==0)
				{
				pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, 
					btQuaternion(0.f, 0.f, 0.f, 1.f), 
					hingeJointAxis, 
					parentComToCurrentPivot, 
					currentPivotToCurrentCom, false);
				} else
				{
					btVector3 parentComToCurrentCom(0, -radius * 2.f, 0);						//par body's COM to cur body's COM offset
					btVector3 currentPivotToCurrentCom(0, -radius, 0);							//cur body's COM to cur body's PIV offset
					btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;	//par body's COM to cur body's PIV offset


					pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, 
					btQuaternion(0.f, 0.f, 0.f, 1.f), 
					parentComToCurrentPivot, 
					currentPivotToCurrentCom);
				}
					
				//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
		
			}
            else
			{
                //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
                pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
			}
        }

        pMultiBody->finalizeMultiDof();

		//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
			for (int i=0;i<pMultiBody->getNumLinks();i++)
		{
			btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
			pMultiBody->getLink(i).m_jointFeedback = fb;
			m_jointFeedbacks.push_back(fb);
			//break;
		}
        btMultiBodyDynamicsWorld* world = m_dynamicsWorld;

        ///
        world->addMultiBody(pMultiBody);
        btMultiBody* mbC = pMultiBody;
        mbC->setCanSleep(canSleep);
        mbC->setHasSelfCollision(selfCollide);
        mbC->setUseGyroTerm(gyro);
        //
        if(!damping)
        {
            mbC->setLinearDamping(0.f);
            mbC->setAngularDamping(0.f);
        }else
        {	mbC->setLinearDamping(0.1f);
            mbC->setAngularDamping(0.9f);
        }
        //
    	m_dynamicsWorld->setGravity(btVector3(0,0,-10));

        //////////////////////////////////////////////
        if(0)//numLinks > 0)
        {
            btScalar q0 = 45.f * SIMD_PI/ 180.f;
            if(!spherical)
			{
				mbC->setJointPosMultiDof(0, &q0);
			}
            else
            {
                btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
                quat0.normalize();
                mbC->setJointPosMultiDof(0, quat0);
            }
        }
        ///

        btAlignedObjectArray<btQuaternion> world_to_local;
        world_to_local.resize(pMultiBody->getNumLinks() + 1);

        btAlignedObjectArray<btVector3> local_origin;
        local_origin.resize(pMultiBody->getNumLinks() + 1);
        world_to_local[0] = pMultiBody->getWorldToBaseRot();
        local_origin[0] = pMultiBody->getBasePos();
      //  double friction = 1;
        {

        //	float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
//            btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};


            if (1)
            {
                btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
                m_guiHelper->createCollisionShapeGraphicsObject(shape);

                btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
                col->setCollisionShape(shape);

                btTransform tr;
                tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this 
				//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
               
                tr.setOrigin(local_origin[0]);
				btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
				
                tr.setRotation(orn);
                col->setWorldTransform(tr);

				bool isDynamic = (baseMass > 0 && floating);
				int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
				int collisionFilterMask = isDynamic? 	int(btBroadphaseProxy::AllFilter) : 	int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);


                world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);

                btVector3 color(0.0,0.0,0.5);
                m_guiHelper->createCollisionObjectGraphicsObject(col,color);

//                col->setFriction(friction);
                pMultiBody->setBaseCollider(col);

            }
        }


        for (int i=0; i < pMultiBody->getNumLinks(); ++i)
        {
            const int parent = pMultiBody->getParent(i);
            world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
            local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
        }


        for (int i=0; i < pMultiBody->getNumLinks(); ++i)
        {

            btVector3 posr = local_origin[i+1];
        //	float pos[4]={posr.x(),posr.y(),posr.z(),1};

            btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
			btCollisionShape* shape =0;

			if (i==0)
			{
				shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
			} else
			{
				
				shape = new btSphereShape(radius);
			}

            m_guiHelper->createCollisionShapeGraphicsObject(shape);
            btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);

            col->setCollisionShape(shape);
            btTransform tr;
            tr.setIdentity();
            tr.setOrigin(posr);
            tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
            col->setWorldTransform(tr);
     //       col->setFriction(friction);
			bool isDynamic = 1;//(linkMass > 0);
			int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
			int collisionFilterMask = isDynamic? 	int(btBroadphaseProxy::AllFilter) : 	int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);

			//if (i==0||i>numLinks-2)
			{
				world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
				   btVector4 color = colors[curColor];
			curColor++;
			curColor&=3;
            m_guiHelper->createCollisionObjectGraphicsObject(col,color);


            pMultiBody->getLink(i).m_collider=col;
			}
         
        }
    }

	btSerializer* s = new btDefaultSerializer;
	m_dynamicsWorld->serialize(s);
	char resourcePath[1024];
	if (b3ResourcePath::findResourcePath("multibody.bullet",resourcePath,1024))
	{
		FILE* f = fopen(resourcePath,"wb");
		fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
		fclose(f);
	}
}
btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, const btMultiBodySettings& settings)
{
	int curColor=0;
					
	
	int cubeShapeId = m_glApp->registerCubeShape();
		
	int n_links = settings.m_numLinks;
	float mass = 13.5*scaling;
	btVector3 inertia = btVector3 (91,344,253)*scaling*scaling;
	
	
	bool isMultiDof = false;
	btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep, isMultiDof);
//		bod->setHasSelfCollision(false);

	//btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1);
	btQuaternion orn(0,0,0,1);
	bod->setBasePos(settings.m_basePosition);
	bod->setWorldToBaseRot(orn);
	btVector3 vel(0,0,0);
	bod->setBaseVel(vel);

	{
			
		btVector3 joint_axis_hinge(1,0,0);
		btVector3 joint_axis_prismatic(0,0,1);
		btQuaternion parent_to_child = orn.inverse();
		btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic);
		btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge);
        
		int this_link_num = -1;
		int link_num_counter = 0;

		

		btVector3 pos = btVector3 (0,0,9.0500002)*scaling;

		btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling;

		for (int i=0;i<n_links;i++)
		{
			float initial_joint_angle=0.3;
			if (i>0)
				initial_joint_angle = -0.06f;

			const int child_link_num = link_num_counter++;

			

			if (settings.m_usePrismatic)// && i==(n_links-1))
			{
					bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
						parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision);

			} else
			{
				bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge,
										joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)),settings.m_disableParentCollision);
			}
			bod->setJointPos(child_link_num, initial_joint_angle);
			this_link_num = i;
		
			if (0)//!useGroundShape && i==4)
			{
				btVector3 pivotInAworld(0,20,46);
				btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld);
				btVector3 pivotInBworld = pivotInAworld;
				btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld);
				world->addMultiBodyConstraint(p2p);
			}
			//add some constraint limit
			if (settings.m_usePrismatic)
			{
	//			btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
			
				if (settings.m_createConstraints)
				{	
					btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
					world->addMultiBodyConstraint(con);
				}
			
			} else
			{
				if (settings.m_createConstraints)
				{	
					if (1)
					{
						btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,0,500000); 
						world->addMultiBodyConstraint(con);
					}

					btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
					world->addMultiBodyConstraint(con);
				}

			}
		}
	}

	//add a collider for the base
	{
			
		btAlignedObjectArray<btQuaternion> world_to_local;
		world_to_local.resize(n_links+1);

		btAlignedObjectArray<btVector3> local_origin;
		local_origin.resize(n_links+1);
		world_to_local[0] = bod->getWorldToBaseRot();
		local_origin[0] = bod->getBasePos();
		//float halfExtents[3]={7.5,0.05,4.5};
		float halfExtents[3]={7.5,0.45,4.5};
		{
			
			float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
			float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};

			
			if (1)
			{
				btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling);
				btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1);

				col->setCollisionShape(box);
								
				btTransform tr;
				tr.setIdentity();
				tr.setOrigin(local_origin[0]);
				tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
							col->setWorldTransform(tr);
				
				b3Vector4 color = colors[curColor++];
				curColor&=3;

				int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents);
				col->setUserIndex(index);




				world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter));
				col->setFriction(friction);
				bod->setBaseCollider(col);
				
			}
		}


		for (int i=0;i<bod->getNumLinks();i++)
		{
			const int parent = bod->getParent(i);
			world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1];
			local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i)));
		}

		
		for (int i=0;i<bod->getNumLinks();i++)
		{
		
			btVector3 posr = local_origin[i+1];
			float pos[4]={posr.x(),posr.y(),posr.z(),1};
			
			float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};

			btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling);
			btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i);

			col->setCollisionShape(box);
			btTransform tr;
			tr.setIdentity();
			tr.setOrigin(posr);
			tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
			col->setWorldTransform(tr);
			col->setFriction(friction);

								
			b3Vector4 color = colors[curColor++];
			curColor&=3;

			int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents);
			col->setUserIndex(index);



			world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter));
			
			bod->getLink(i).m_collider=col;
			//app->drawBox(halfExtents, pos,quat);
		}

	}
	world->addMultiBody(bod);

	return bod;
}
示例#9
0
void	ConstraintDemo::initPhysics()
{
    setTexturing(true);
    setShadows(true);

    setCameraDistance(26.f);
    m_Time = 0;

    setupEmptyDynamicsWorld();

    m_dynamicsWorld->setDebugDrawer(&gDebugDrawer);


    //btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(40.),btScalar(50.)));
    btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),40);

    m_collisionShapes.push_back(groundShape);
    btTransform groundTransform;
    groundTransform.setIdentity();
    groundTransform.setOrigin(btVector3(0,-56,0));
    btRigidBody* groundBody;
    groundBody= localCreateRigidBody(0, groundTransform, groundShape);



    btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
    m_collisionShapes.push_back(shape);
    btTransform trans;
    trans.setIdentity();
    trans.setOrigin(btVector3(0,20,0));

    float mass = 1.f;

#if ENABLE_ALL_DEMOS
///gear constraint demo

#define THETA SIMD_PI/4.f
#define L_1 (2 - tan(THETA))
#define L_2 (1 / cos(THETA))
#define RATIO L_2 / L_1

    btRigidBody* bodyA=0;
    btRigidBody* bodyB=0;

    {
        btCollisionShape* cylA = new btCylinderShape(btVector3(0.2,0.25,0.2));
        btCollisionShape* cylB = new btCylinderShape(btVector3(L_1,0.025,L_1));
        btCompoundShape* cyl0 = new btCompoundShape();
        cyl0->addChildShape(btTransform::getIdentity(),cylA);
        cyl0->addChildShape(btTransform::getIdentity(),cylB);

        btScalar mass = 6.28;
        btVector3 localInertia;
        cyl0->calculateLocalInertia(mass,localInertia);
        btRigidBody::btRigidBodyConstructionInfo ci(mass,0,cyl0,localInertia);
        ci.m_startWorldTransform.setOrigin(btVector3(-8,1,-8));

        btRigidBody* body = new btRigidBody(ci);//1,0,cyl0,localInertia);
        m_dynamicsWorld->addRigidBody(body);
        body->setLinearFactor(btVector3(0,0,0));
        body->setAngularFactor(btVector3(0,1,0));
        bodyA = body;
    }

    {
        btCollisionShape* cylA = new btCylinderShape(btVector3(0.2,0.26,0.2));
        btCollisionShape* cylB = new btCylinderShape(btVector3(L_2,0.025,L_2));
        btCompoundShape* cyl0 = new btCompoundShape();
        cyl0->addChildShape(btTransform::getIdentity(),cylA);
        cyl0->addChildShape(btTransform::getIdentity(),cylB);

        btScalar mass = 6.28;
        btVector3 localInertia;
        cyl0->calculateLocalInertia(mass,localInertia);
        btRigidBody::btRigidBodyConstructionInfo ci(mass,0,cyl0,localInertia);
        ci.m_startWorldTransform.setOrigin(btVector3(-10,2,-8));


        btQuaternion orn(btVector3(0,0,1),-THETA);
        ci.m_startWorldTransform.setRotation(orn);

        btRigidBody* body = new btRigidBody(ci);//1,0,cyl0,localInertia);
        body->setLinearFactor(btVector3(0,0,0));
        btHingeConstraint* hinge = new btHingeConstraint(*body,btVector3(0,0,0),btVector3(0,1,0),true);
        m_dynamicsWorld->addConstraint(hinge);
        bodyB= body;
        body->setAngularVelocity(btVector3(0,3,0));

        m_dynamicsWorld->addRigidBody(body);
    }

    btVector3	axisA(0,1,0);
    btVector3	axisB(0,1,0);
    btQuaternion orn(btVector3(0,0,1),-THETA);
    btMatrix3x3 mat(orn);
    axisB = mat.getRow(1);

    btGearConstraint* gear = new btGearConstraint(*bodyA,*bodyB, axisA,axisB,RATIO);
    m_dynamicsWorld->addConstraint(gear,true);


#endif


#if ENABLE_ALL_DEMOS
    //point to point constraint with a breaking threshold
    {
        trans.setIdentity();
        trans.setOrigin(btVector3(1,30,-5));
        localCreateRigidBody( mass,trans,shape);
        trans.setOrigin(btVector3(0,0,-5));

        btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
        trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));
        mass = 1.f;
        //	btRigidBody* body1 = 0;//localCreateRigidBody( mass,trans,shape);
        btVector3 pivotInA(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,0);
        btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,pivotInA);
        m_dynamicsWorld->addConstraint(p2p);
        p2p ->setBreakingImpulseThreshold(10.2);
        p2p->setDbgDrawSize(btScalar(5.f));
    }
#endif



#if ENABLE_ALL_DEMOS
    //point to point constraint (ball socket)
    {
        btRigidBody* body0 = localCreateRigidBody( mass,trans,shape);
        trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));

        mass = 1.f;
//		btRigidBody* body1 = 0;//localCreateRigidBody( mass,trans,shape);
//		btRigidBody* body1 = localCreateRigidBody( 0.0,trans,0);
        //body1->setActivationState(DISABLE_DEACTIVATION);
        //body1->setDamping(0.3,0.3);

        btVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS);
        btVector3 axisInA(0,0,1);

        //	btVector3 pivotInB = body1 ? body1->getCenterOfMassTransform().inverse()(body0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
//		btVector3 axisInB = body1?
//			(body1->getCenterOfMassTransform().getBasis().inverse()*(body1->getCenterOfMassTransform().getBasis() * axisInA)) :
        body0->getCenterOfMassTransform().getBasis() * axisInA;

#define P2P
#ifdef P2P
        btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,pivotInA);
        //btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB);
        //btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB);
        m_dynamicsWorld->addConstraint(p2p);
        p2p->setDbgDrawSize(btScalar(5.f));
#else
        btHingeConstraint* hinge = new btHingeConstraint(*body0,pivotInA,axisInA);

        //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
        //float	targetVelocity = 0.f;
        //float	maxMotorImpulse = 0.01;
        float	targetVelocity = 1.f;
        float	maxMotorImpulse = 1.0f;
        hinge->enableAngularMotor(true,targetVelocity,maxMotorImpulse);
        m_dynamicsWorld->addConstraint(hinge);
        hinge->setDbgDrawSize(btScalar(5.f));
#endif //P2P




    }
#endif

#if ENABLE_ALL_DEMOS
    //create a slider, using the generic D6 constraint
    {
        mass = 1.f;
        btVector3 sliderWorldPos(0,10,0);
        btVector3 sliderAxis(1,0,0);
        btScalar angle=0.f;//SIMD_RADS_PER_DEG * 10.f;
        btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis ,angle));
        trans.setIdentity();
        trans.setOrigin(sliderWorldPos);
        //trans.setBasis(sliderOrientation);
        sliderTransform = trans;

        d6body0 = localCreateRigidBody( mass,trans,shape);
        d6body0->setActivationState(DISABLE_DEACTIVATION);
        btRigidBody* fixedBody1 = localCreateRigidBody(0,trans,0);
        m_dynamicsWorld->addRigidBody(fixedBody1);

        btTransform frameInA, frameInB;
        frameInA = btTransform::getIdentity();
        frameInB = btTransform::getIdentity();
        frameInA.setOrigin(btVector3(0., 5., 0.));
        frameInB.setOrigin(btVector3(0., 5., 0.));

//		bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
        bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits
        spSlider6Dof = new btGeneric6DofConstraint(*fixedBody1, *d6body0,frameInA,frameInB,useLinearReferenceFrameA);
        spSlider6Dof->setLinearLowerLimit(lowerSliderLimit);
        spSlider6Dof->setLinearUpperLimit(hiSliderLimit);

        //range should be small, otherwise singularities will 'explode' the constraint
//		spSlider6Dof->setAngularLowerLimit(btVector3(-1.5,0,0));
//		spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));
//		spSlider6Dof->setAngularLowerLimit(btVector3(0,0,0));
//		spSlider6Dof->setAngularUpperLimit(btVector3(0,0,0));
        spSlider6Dof->setAngularLowerLimit(btVector3(-SIMD_PI,0,0));
        spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0));

        spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
        spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
        spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;


        m_dynamicsWorld->addConstraint(spSlider6Dof);
        spSlider6Dof->setDbgDrawSize(btScalar(5.f));

    }
#endif
#if ENABLE_ALL_DEMOS
    {   // create a door using hinge constraint attached to the world
        btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f));
        m_collisionShapes.push_back(pDoorShape);
        btTransform doorTrans;
        doorTrans.setIdentity();
        doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f));
        btRigidBody* pDoorBody = localCreateRigidBody( 1.0, doorTrans, pDoorShape);
        pDoorBody->setActivationState(DISABLE_DEACTIVATION);
        const btVector3 btPivotA(10.f +  2.1f, -2.0f, 0.0f ); // right next to the door slightly outside
        btVector3 btAxisA( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis

        spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA );

//		spDoorHinge->setLimit( 0.0f, SIMD_PI_2 );
        // test problem values
//		spDoorHinge->setLimit( -SIMD_PI, SIMD_PI*0.8f);

//		spDoorHinge->setLimit( 1.f, -1.f);
//		spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI);
//		spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.3f, 0.0f);
//		spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
        spDoorHinge->setLimit( -SIMD_PI * 0.25f, SIMD_PI * 0.25f );
//		spDoorHinge->setLimit( 0.0f, 0.0f );
        m_dynamicsWorld->addConstraint(spDoorHinge);
        spDoorHinge->setDbgDrawSize(btScalar(5.f));

        //doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f));
        //btRigidBody* pDropBody = localCreateRigidBody( 10.0, doorTrans, shape);
    }
#endif
#if ENABLE_ALL_DEMOS
    {   // create a generic 6DOF constraint

        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(10.), btScalar(6.), btScalar(0.)));
        tr.getBasis().setEulerZYX(0,0,0);
//		btRigidBody* pBodyA = localCreateRigidBody( mass, tr, shape);
        btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
//		btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, 0);
        pBodyA->setActivationState(DISABLE_DEACTIVATION);

        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(0.), btScalar(6.), btScalar(0.)));
        tr.getBasis().setEulerZYX(0,0,0);
        btRigidBody* pBodyB = localCreateRigidBody(mass, tr, shape);
//		btRigidBody* pBodyB = localCreateRigidBody(0.f, tr, shape);
        pBodyB->setActivationState(DISABLE_DEACTIVATION);

        btTransform frameInA, frameInB;
        frameInA = btTransform::getIdentity();
        frameInA.setOrigin(btVector3(btScalar(-5.), btScalar(0.), btScalar(0.)));
        frameInB = btTransform::getIdentity();
        frameInB.setOrigin(btVector3(btScalar(5.), btScalar(0.), btScalar(0.)));

        btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
//		btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false);
        pGen6DOF->setLinearLowerLimit(btVector3(-10., -2., -1.));
        pGen6DOF->setLinearUpperLimit(btVector3(10., 2., 1.));
//		pGen6DOF->setLinearLowerLimit(btVector3(-10., 0., 0.));
//		pGen6DOF->setLinearUpperLimit(btVector3(10., 0., 0.));
//		pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.));
//		pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.));

//		pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true;
//		pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
//		pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;


//		pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.));
//		pGen6DOF->setAngularUpperLimit(btVector3(0., -SIMD_HALF_PI*0.9, 0.));
//		pGen6DOF->setAngularLowerLimit(btVector3(0., 0., -SIMD_HALF_PI));
//		pGen6DOF->setAngularUpperLimit(btVector3(0., 0., SIMD_HALF_PI));

        pGen6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI * 0.5f, -0.75, -SIMD_HALF_PI * 0.8f));
        pGen6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI * 0.5f, 0.75, SIMD_HALF_PI * 0.8f));
//		pGen6DOF->setAngularLowerLimit(btVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f));
//		pGen6DOF->setAngularUpperLimit(btVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f));
//		pGen6DOF->setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f));
//		pGen6DOF->setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI * 0.8f,  -SIMD_HALF_PI * 1.98f));



//		pGen6DOF->setAngularLowerLimit(btVector3(-0.75,-0.5, -0.5));
//		pGen6DOF->setAngularUpperLimit(btVector3(0.75,0.5, 0.5));
//		pGen6DOF->setAngularLowerLimit(btVector3(-0.75,0., 0.));
//		pGen6DOF->setAngularUpperLimit(btVector3(0.75,0., 0.));
//		pGen6DOF->setAngularLowerLimit(btVector3(0., -0.7,0.));
//		pGen6DOF->setAngularUpperLimit(btVector3(0., 0.7, 0.));
//		pGen6DOF->setAngularLowerLimit(btVector3(-1., 0.,0.));
//		pGen6DOF->setAngularUpperLimit(btVector3(1., 0., 0.));

        m_dynamicsWorld->addConstraint(pGen6DOF, true);
        pGen6DOF->setDbgDrawSize(btScalar(5.f));
    }
#endif
#if ENABLE_ALL_DEMOS
    {   // create a ConeTwist constraint

        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(-10.), btScalar(5.), btScalar(0.)));
        tr.getBasis().setEulerZYX(0,0,0);
        btRigidBody* pBodyA = localCreateRigidBody( 1.0, tr, shape);
//		btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
        pBodyA->setActivationState(DISABLE_DEACTIVATION);

        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(-10.), btScalar(-5.), btScalar(0.)));
        tr.getBasis().setEulerZYX(0,0,0);
        btRigidBody* pBodyB = localCreateRigidBody(0.0, tr, shape);
//		btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape);

        btTransform frameInA, frameInB;
        frameInA = btTransform::getIdentity();
        frameInA.getBasis().setEulerZYX(0, 0, SIMD_PI_2);
        frameInA.setOrigin(btVector3(btScalar(0.), btScalar(-5.), btScalar(0.)));
        frameInB = btTransform::getIdentity();
        frameInB.getBasis().setEulerZYX(0,0,  SIMD_PI_2);
        frameInB.setOrigin(btVector3(btScalar(0.), btScalar(5.), btScalar(0.)));

        m_ctc = new btConeTwistConstraint(*pBodyA, *pBodyB, frameInA, frameInB);
//		m_ctc->setLimit(btScalar(SIMD_PI_4), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f);
//		m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 1.0f); // soft limit == hard limit
        m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 0.5f);
        m_dynamicsWorld->addConstraint(m_ctc, true);
        m_ctc->setDbgDrawSize(btScalar(5.f));
        // s_bTestConeTwistMotor = true; // use only with old solver for now
        s_bTestConeTwistMotor = false;
    }
#endif
#if ENABLE_ALL_DEMOS
    {   // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
        btRigidBody* pBody = localCreateRigidBody( 1.0, tr, shape);
        pBody->setActivationState(DISABLE_DEACTIVATION);
        const btVector3 btPivotA( 10.0f, 0.0f, 0.0f );
        btVector3 btAxisA( 0.0f, 0.0f, 1.0f );

        btHingeConstraint* pHinge = new btHingeConstraint( *pBody, btPivotA, btAxisA );
//		pHinge->enableAngularMotor(true, -1.0, 0.165); // use for the old solver
        pHinge->enableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
        m_dynamicsWorld->addConstraint(pHinge);
        pHinge->setDbgDrawSize(btScalar(5.f));
    }
#endif

#if ENABLE_ALL_DEMOS
    {
        // create a universal joint using generic 6DOF constraint
        // create two rigid bodies
        // static bodyA (parent) on top:
        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(20.), btScalar(4.), btScalar(0.)));
        btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
        pBodyA->setActivationState(DISABLE_DEACTIVATION);
        // dynamic bodyB (child) below it :
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(20.), btScalar(0.), btScalar(0.)));
        btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape);
        pBodyB->setActivationState(DISABLE_DEACTIVATION);
        // add some (arbitrary) data to build constraint frames
        btVector3 parentAxis(1.f, 0.f, 0.f);
        btVector3 childAxis(0.f, 0.f, 1.f);
        btVector3 anchor(20.f, 2.f, 0.f);

        btUniversalConstraint* pUniv = new btUniversalConstraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis);
        pUniv->setLowerLimit(-SIMD_HALF_PI * 0.5f, -SIMD_HALF_PI * 0.5f);
        pUniv->setUpperLimit(SIMD_HALF_PI * 0.5f,  SIMD_HALF_PI * 0.5f);
        // add constraint to world
        m_dynamicsWorld->addConstraint(pUniv, true);
        // draw constraint frames and limits for debugging
        pUniv->setDbgDrawSize(btScalar(5.f));
    }
#endif

#if ENABLE_ALL_DEMOS
    {   // create a generic 6DOF constraint with springs

        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.)));
        tr.getBasis().setEulerZYX(0,0,0);
        btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
        pBodyA->setActivationState(DISABLE_DEACTIVATION);

        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.)));
        tr.getBasis().setEulerZYX(0,0,0);
        btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape);
        pBodyB->setActivationState(DISABLE_DEACTIVATION);

        btTransform frameInA, frameInB;
        frameInA = btTransform::getIdentity();
        frameInA.setOrigin(btVector3(btScalar(10.), btScalar(0.), btScalar(0.)));
        frameInB = btTransform::getIdentity();
        frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));

        btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
        pGen6DOFSpring->setLinearUpperLimit(btVector3(5., 0., 0.));
        pGen6DOFSpring->setLinearLowerLimit(btVector3(-5., 0., 0.));

        pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f));
        pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f));

        m_dynamicsWorld->addConstraint(pGen6DOFSpring, true);
        pGen6DOFSpring->setDbgDrawSize(btScalar(5.f));

        pGen6DOFSpring->enableSpring(0, true);
        pGen6DOFSpring->setStiffness(0, 39.478f);
        pGen6DOFSpring->setDamping(0, 0.5f);
        pGen6DOFSpring->enableSpring(5, true);
        pGen6DOFSpring->setStiffness(5, 39.478f);
        pGen6DOFSpring->setDamping(0, 0.3f);
        pGen6DOFSpring->setEquilibriumPoint();
    }
#endif
#if ENABLE_ALL_DEMOS
    {
        // create a Hinge2 joint
        // create two rigid bodies
        // static bodyA (parent) on top:
        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(-20.), btScalar(4.), btScalar(0.)));
        btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
        pBodyA->setActivationState(DISABLE_DEACTIVATION);
        // dynamic bodyB (child) below it :
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(-20.), btScalar(0.), btScalar(0.)));
        btRigidBody* pBodyB = localCreateRigidBody(1.0, tr, shape);
        pBodyB->setActivationState(DISABLE_DEACTIVATION);
        // add some data to build constraint frames
        btVector3 parentAxis(0.f, 1.f, 0.f);
        btVector3 childAxis(1.f, 0.f, 0.f);
        btVector3 anchor(-20.f, 0.f, 0.f);
        btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis);
        pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f);
        pHinge2->setUpperLimit( SIMD_HALF_PI * 0.5f);
        // add constraint to world
        m_dynamicsWorld->addConstraint(pHinge2, true);
        // draw constraint frames and limits for debugging
        pHinge2->setDbgDrawSize(btScalar(5.f));
    }
#endif
#if  ENABLE_ALL_DEMOS
    {
        // create a Hinge joint between two dynamic bodies
        // create two rigid bodies
        // static bodyA (parent) on top:
        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(-20.), btScalar(-2.), btScalar(0.)));
        btRigidBody* pBodyA = localCreateRigidBody( 1.0f, tr, shape);
        pBodyA->setActivationState(DISABLE_DEACTIVATION);
        // dynamic bodyB:
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(-30.), btScalar(-2.), btScalar(0.)));
        btRigidBody* pBodyB = localCreateRigidBody(10.0, tr, shape);
        pBodyB->setActivationState(DISABLE_DEACTIVATION);
        // add some data to build constraint frames
        btVector3 axisA(0.f, 1.f, 0.f);
        btVector3 axisB(0.f, 1.f, 0.f);
        btVector3 pivotA(-5.f, 0.f, 0.f);
        btVector3 pivotB( 5.f, 0.f, 0.f);
        spHingeDynAB = new btHingeConstraint(*pBodyA, *pBodyB, pivotA, pivotB, axisA, axisB);
        spHingeDynAB->setLimit(-SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f);
        // add constraint to world
        m_dynamicsWorld->addConstraint(spHingeDynAB, true);
        // draw constraint frames and limits for debugging
        spHingeDynAB->setDbgDrawSize(btScalar(5.f));
    }
#endif

#if ENABLE_ALL_DEMOS
    {   // 6DOF connected to the world, with motor
        btTransform tr;
        tr.setIdentity();
        tr.setOrigin(btVector3(btScalar(10.), btScalar(-15.), btScalar(0.)));
        btRigidBody* pBody = localCreateRigidBody( 1.0, tr, shape);
        pBody->setActivationState(DISABLE_DEACTIVATION);
        btTransform frameB;
        frameB.setIdentity();
        btGeneric6DofConstraint* pGen6Dof = new btGeneric6DofConstraint( *pBody, frameB, false );
        m_dynamicsWorld->addConstraint(pGen6Dof);
        pGen6Dof->setDbgDrawSize(btScalar(5.f));

        pGen6Dof->setAngularLowerLimit(btVector3(0,0,0));
        pGen6Dof->setAngularUpperLimit(btVector3(0,0,0));
        pGen6Dof->setLinearLowerLimit(btVector3(-10., 0, 0));
        pGen6Dof->setLinearUpperLimit(btVector3(10., 0, 0));

        pGen6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
        pGen6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
        pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
    }
#endif



}
示例#10
0
/** Sets all start positions depending on the quad graph. The number of
 *  entries needed is defined by the size of the start_transform (though all
 *  entries will be overwritten).
 *  E.g. the karts will be placed as:
 *   1           \
 *     2          +--  row with three karts, each kart is 'sidewards_distance'
 *       3       /     to the right of the previous kart, and
 *   4                 'forwards_distance' behind the previous kart.
 *     5               The next row starts again with the kart being
 *       6             'forwards_distance' behind the end of the previous row.
 *  etc.
 *  \param start_transforms A vector sized to the needed number of start
 *               positions. The values will all be overwritten with the
 *               default start positions.
 *  \param karts_per_row How many karts to place in each row.
 *  \param forwards_distance Distance in forward (Z) direction between
 *               each kart.
 *  \param sidewards_distance Distance in sidewards (X) direction between
 *               karts.
 */
void QuadGraph::setDefaultStartPositions(AlignedArray<btTransform>
                                                       *start_transforms,
                                         unsigned int karts_per_row,
                                         float forwards_distance,
                                         float sidewards_distance,
                                         float upwards_distance) const
{
    // We start just before the start node (which will trigger lap
    // counting when reached). The first predecessor is the one on
    // the main driveline.
    int current_node = m_all_nodes[getStartNode()]->getPredecessor(0);

    float distance_from_start = 0.1f+forwards_distance;

    // Maximum distance to left (or right) of centre line
    const float max_x_dist    = 0.5f*(karts_per_row-0.5f)*sidewards_distance;
    // X position relative to the centre line
    float x_pos               = -max_x_dist + sidewards_distance*0.5f;
    unsigned int row_number   = 0;

    for(unsigned int i=0; i<(unsigned int)start_transforms->size(); i++)
    {
        if (current_node == -1)
        {
            (*start_transforms)[i].setOrigin(Vec3(0,0,0));
            (*start_transforms)[i].setRotation(btQuaternion(btVector3(0, 1, 0),
                                                            0));
        }
        else
        {
            // First find on which segment we have to start
            while(distance_from_start > getNode(current_node).getNodeLength())
            {
                distance_from_start -= getNode(current_node).getNodeLength();
                // Only follow the main driveline, i.e. first predecessor
                current_node = getNode(current_node).getPredecessor(0);
            }
            const GraphNode &gn   = getNode(current_node);
            Vec3 center_line = gn.getLowerCenter() - gn.getUpperCenter();
            center_line.normalize();

            Vec3 horizontal_line = gn[2] - gn[3];
            horizontal_line.normalize();

            Vec3 start = gn.getUpperCenter()
                       + center_line     * distance_from_start
                       + horizontal_line * x_pos;
            // Add a certain epsilon to the height in case that the
            // drivelines are beneath the track.
            (*start_transforms)[i].setOrigin(start+Vec3(0,upwards_distance,0));
            (*start_transforms)[i].setRotation(
                btQuaternion(btVector3(0, 1, 0),
                             gn.getAngleToSuccessor(0)));
            if(x_pos >= max_x_dist-sidewards_distance*0.5f)
            {
                x_pos  = -max_x_dist;
                // Every 2nd row will be pushed sideways by half the distance
                // between karts, so that a kart can drive between the karts in
                // the row ahead of it.
                row_number ++;
                if(row_number % 2 == 0)
                    x_pos += sidewards_distance*0.5f;
            }
            else
                x_pos += sidewards_distance;
            distance_from_start += forwards_distance;
        }
    }   // for i<stk_config->m_max_karts
}   // setStartPositions
示例#11
0
btQuaternion tanks::tanks_to_bullet(Quaternion4<float> q)
{
	return btQuaternion(q.x, q.y, q.z, q.w);
};
示例#12
0
class btMultiBody* MultiBodyVehicleSetup::createMultiBodyVehicle()
{
    class btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
    int numWheels = 4;
    
    int totalLinks = numWheels;//number of body parts (links) (in)directly attached to the base, NOT including the base/root itself
    
    btCollisionShape* chassis = new btBoxShape(gVehicleBaseHalfExtents);//CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
    m_collisionShapes.push_back(chassis);
    btCollisionShape* wheel = new btCylinderShapeX(gVehicleWheelHalfExtents);//CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
    m_collisionShapes.push_back(wheel);
    
    
    btVector3 baseLocalInertia(0, 0, 0);
    chassis->calculateLocalInertia(gVehicleBaseMass, baseLocalInertia);
    
    bool multiDof = false;
    bool isFixedBase = false;
    bool canSleep = false;
    
    btMultiBody * bod = new btMultiBody(totalLinks, gVehicleBaseMass, baseLocalInertia, isFixedBase, canSleep);// , multiDof);
    bod->setHasSelfCollision(false);
    
    btQuaternion baseOrn(0, 0, 0, 1);
    btVector3 basePos(0, 0, 0);
    bod->setBasePos(basePos);
    
    bod->setWorldToBaseRot(baseOrn);
    btVector3 vel(0, 0, 0);
    bod->setBaseVel(vel);
    
    {
        
       
        
        
        int linkNum = 0;
    
        
        btVector3 wheelJointAxisWorld(1, 0, 0);
        btQuaternion parent_to_child = baseOrn.inverse();//??
        for (int j = 0; j < numWheels; j++, linkNum++)
        {
            int parent_link_num = -1;
            
            float initial_joint_angle = 0.0;
            
            btVector3 localWheelInertia(0, 0, 0);
            wheel->calculateLocalInertia(gVehicleWheelMass, localWheelInertia);
            bool disableParentCollision = true;
            btVector3 pivotToChildCOM(0, 0, 0.25);
            btVector3 pivotToWheelCOM(0, 0, 0);
            {
                bod->setupRevolute(linkNum, gVehicleWheelMass, localWheelInertia, parent_link_num, parent_to_child, wheelJointAxisWorld,
                                   wheelAttachmentPosInWorld[j], pivotToWheelCOM, disableParentCollision);
            }
            bod->setJointPos(linkNum, initial_joint_angle);
           
	    if (j<2)
{ 
            btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod, linkNum, 1., 50);
            world->addMultiBodyConstraint(con);
}	            
        }
       
        
    }
    
    //add a collider for the base
    {
        
        btAlignedObjectArray<btQuaternion> world_to_local;
        world_to_local.resize(totalLinks + 1);
        
        btAlignedObjectArray<btVector3> local_origin;
        local_origin.resize(totalLinks + 1);
        world_to_local[0] = bod->getWorldToBaseRot();
        local_origin[0] = bod->getBasePos();
        {
            
            float pos[4] = { local_origin[0].x(), local_origin[0].y(), local_origin[0].z(), 1 };
            float quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
            
            
            if (1)
            {
                
                btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod, -1);
                col->setCollisionShape(chassis);
                btTransform tr;
                tr.setIdentity();
                tr.setOrigin(local_origin[0]);
                tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
                col->setWorldTransform(tr);
                world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);// 2, 1 + 2);
                col->setFriction(friction);
                bod->setBaseCollider(col);
            }
        }
        
        //initialize local coordinate frames, relative to parent
        for (int i = 0; i<bod->getNumLinks(); i++)
        {
            const int parent = bod->getParent(i);
            world_to_local[i + 1] = bod->getParentToLocalRot(i) * world_to_local[parent + 1];
            local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), bod->getRVector(i)));
        }
        
        int linkIndex = 0;
        
     
        
        for (int j = 0; j<numWheels; j++, linkIndex++)
        {
            
            btVector3 posr = local_origin[linkIndex + 1];
            float pos[4] = { posr.x(), posr.y(), posr.z(), 1 };
            float quat[4] = { -world_to_local[linkIndex + 1].x(), -world_to_local[linkIndex + 1].y(), -world_to_local[linkIndex + 1].z(), world_to_local[linkIndex + 1].w() };
            
            btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod, linkIndex);
            
            col->setCollisionShape(wheel);
            btTransform tr;
            tr.setIdentity();
            tr.setOrigin(posr);
            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
            col->setWorldTransform(tr);
            col->setFriction(friction);
            world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);// 2, 1 + 2);
            bod->getLink(linkIndex).m_collider = col;
        }
    }
    
    
    world->addMultiBody(bod);
//    world->setGravity(btVector3(0,0,0));
    
    return bod;
}
示例#13
0
int main(int argc, char *argv[])
{
  // Build the broadphase
  btBroadphaseInterface* broadphase = new btDbvtBroadphase();

  btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
  btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);

  // The actual physics solver
  btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

  btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
  dynamicsWorld->setGravity(btVector3(0, -10, 0));


  btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),1);
  btCollisionShape* fallShape = new btSphereShape(1);

  btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), btVector3(0,-1,0)));

  btRigidBody::btRigidBodyConstructionInfo
    groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
  btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
  dynamicsWorld->addRigidBody(groundRigidBody);

  btDefaultMotionState* fallMotionState =
    new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), btVector3(0,50,0)));
  btScalar mass = 1;
  btVector3 fallInertia(0,0,0);
  fallShape->calculateLocalInertia(mass, fallInertia);
  btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass, fallMotionState, fallShape, fallInertia);
  btRigidBody* fallRigidBody = new btRigidBody(fallRigidBodyCI);
  dynamicsWorld->addRigidBody(fallRigidBody);

  for (int i=0; i<300; i++) {
    dynamicsWorld->stepSimulation( 1.f/60.f, 10);

    btTransform trans;
    fallRigidBody->getMotionState()->getWorldTransform(trans);

    std::cout << "sphere height: " << trans.getOrigin().getY() << std::endl;
  }

  dynamicsWorld->removeRigidBody(fallRigidBody);
  delete fallRigidBody->getMotionState();
  delete fallRigidBody;

  dynamicsWorld->removeRigidBody(groundRigidBody);
  delete groundRigidBody->getMotionState();
  delete groundRigidBody;

  delete fallShape;
  delete groundShape;

  // Clean up
  delete dynamicsWorld;
  delete solver;
  delete dispatcher;
  delete collisionConfiguration;
  delete broadphase;

  return 0;
}
示例#14
0
void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
	btDiscreteDynamicsWorld::integrateTransforms(timeStep);

	{
		BT_PROFILE("btMultiBody stepPositions");
		//integrate and update the Featherstone hierarchies
		btAlignedObjectArray<btQuaternion> world_to_local;
		btAlignedObjectArray<btVector3> local_origin;

		for (int b=0;b<m_multiBodies.size();b++)
		{
			btMultiBody* bod = m_multiBodies[b];
			bool isSleeping = false;
			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
			{
				isSleeping = true;
			} 
			for (int b=0;b<bod->getNumLinks();b++)
			{
				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
					isSleeping = true;
			}


			if (!isSleeping)
			{
				int nLinks = bod->getNumLinks();

				///base + num m_links
				world_to_local.resize(nLinks+1);
				local_origin.resize(nLinks+1);

				if(bod->isMultiDof())
				{
					if(!bod->__posUpdated)
						bod->stepPositionsMultiDof(timeStep);
					else
					{
						btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();

						bod->stepPositionsMultiDof(1, 0, pRealBuf);
						bod->__posUpdated = false;
					}
				}
				else
					bod->stepPositions(timeStep);			

				world_to_local[0] = bod->getWorldToBaseRot();
				local_origin[0] = bod->getBasePos();

				if (bod->getBaseCollider())
				{
					btVector3 posr = local_origin[0];
					float pos[4]={posr.x(),posr.y(),posr.z(),1};
					float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
					btTransform tr;
					tr.setIdentity();
					tr.setOrigin(posr);
					tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));

					bod->getBaseCollider()->setWorldTransform(tr);

				}
      
				for (int k=0;k<bod->getNumLinks();k++)
				{
					const int parent = bod->getParent(k);
					world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
					local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
				}


				for (int m=0;m<bod->getNumLinks();m++)
				{
					btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
					if (col)
					{
						int link = col->m_link;
						btAssert(link == m);

						int index = link+1;

						btVector3 posr = local_origin[index];
						float pos[4]={posr.x(),posr.y(),posr.z(),1};
						float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
						btTransform tr;
						tr.setIdentity();
						tr.setOrigin(posr);
						tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));

						col->setWorldTransform(tr);
					}
				}
			} else
			{
				bod->clearVelocities();
			}
		}
	}
}
示例#15
0
文件: cake.cpp 项目: Benau/stk-code
Cake::Cake (AbstractKart *kart) : Flyable(kart, PowerupManager::POWERUP_CAKE)
{
    m_target = NULL;

    setDoTerrainInfo(false);

    btVector3 gravity_vector;
    btQuaternion q = kart->getTrans().getRotation();
    gravity_vector = Vec3(0, -1, 0).rotate(q.getAxis(), q.getAngle());
    gravity_vector = gravity_vector.normalize() * m_gravity;
    // A bit of a hack: the mass of this kinematic object is still 1.0
    // (see flyable), which enables collisions. I tried setting
    // collisionFilterGroup/mask, but still couldn't get this object to
    // collide with the track. By setting the mass to 1, collisions happen.
    // (if bullet is compiled with _DEBUG, a warning will be printed the first
    // time a homing-track collision happens).
    float forward_offset=kart->getKartLength()/2.0f + m_extend.getZ()/2.0f;

    float up_velocity = m_speed/7.0f;

    // give a speed proportional to kart speed. m_speed is defined in flyable
    m_speed *= kart->getSpeed() / 23.0f;

    //when going backwards, decrease speed of cake by less
    if (kart->getSpeed() < 0) m_speed /= 3.6f;

    m_speed += 16.0f;

    if (m_speed < 1.0f) m_speed = 1.0f;

    btTransform trans = kart->getTrans();

    float heading=kart->getHeading();
    float pitch = kart->getTerrainPitch(heading);

    // Find closest kart in front of the current one
    const bool  backwards = kart->getControls().getLookBack();
    const AbstractKart *closest_kart=NULL;
    Vec3        direction;
    float       kart_dist_squared;
    getClosestKart(&closest_kart, &kart_dist_squared, &direction,
                   kart /* search in front of this kart */, backwards);

    // aim at this kart if 1) it's not too far, 2) if the aimed kart's speed
    // allows the projectile to catch up with it
    //
    // this code finds the correct angle and upwards velocity to hit an opponents'
    // vehicle if they were to continue travelling in the same direction and same speed
    // (barring any obstacles in the way of course)
    if(closest_kart != NULL && kart_dist_squared < m_st_max_distance_squared &&
        m_speed>closest_kart->getSpeed())
    {
        m_target = (AbstractKart*)closest_kart;

        float fire_angle     = 0.0f;
        getLinearKartItemIntersection (kart->getXYZ(), closest_kart,
                                       m_speed, m_gravity, forward_offset,
                                       &fire_angle, &up_velocity);

        // apply transformation to the bullet object (without pitch)
        btQuaternion q;
        q = trans.getRotation() * btQuaternion(btVector3(0, 1, 0), fire_angle);
        trans.setRotation(q);
        m_initial_velocity = Vec3(0.0f, up_velocity, m_speed);

        createPhysics(forward_offset, m_initial_velocity,
                      new btCylinderShape(0.5f*m_extend),
                      0.5f /* restitution */, gravity_vector,
                      true /* rotation */, false /* backwards */, &trans);
    }
    else
    {
        m_target = NULL;
        // kart is too far to be hit. so throw the projectile in a generic way,
        // straight ahead, without trying to hit anything in particular
        trans = kart->getAlignedTransform(pitch);

        m_initial_velocity = Vec3(0.0f, up_velocity, m_speed);

        createPhysics(forward_offset, m_initial_velocity,
                      new btCylinderShape(0.5f*m_extend),
                      0.5f /* restitution */, gravity_vector,
                      true /* rotation */, backwards, &trans);
    }


    //do not adjust height according to terrain
    setAdjustUpVelocity(false);

    m_body->setActivationState(DISABLE_DEACTIVATION);

    m_body->applyTorque( btVector3(5,-3,7) );

}   // Cake
示例#16
0
	TestRig (btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
		: m_ownerWorld (ownerWorld)
	{
		btVector3 vUp(0, 1, 0);

		//
		// Setup geometry
		//
		float fBodySize  = 0.25f;
		float fLegLength = 0.45f;
		float fForeLegLength = 0.75f;
		m_shapes[0] = new btCapsuleShape(btScalar(fBodySize), btScalar(0.10));
		int i;
		for ( i=0; i<NUM_LEGS; i++)
		{
			m_shapes[1 + 2*i] = new btCapsuleShape(btScalar(0.10), btScalar(fLegLength));
			m_shapes[2 + 2*i] = new btCapsuleShape(btScalar(0.08), btScalar(fForeLegLength));
		}

		//
		// Setup rigid bodies
		//
		float fHeight = 0.5;
		btTransform offset; offset.setIdentity();
		offset.setOrigin(positionOffset);		

		// root
		btVector3 vRoot = btVector3(btScalar(0.), btScalar(fHeight), btScalar(0.));
		btTransform transform;
		transform.setIdentity();
		transform.setOrigin(vRoot);
		if (bFixed)
		{
			m_bodies[0] = localCreateRigidBody(btScalar(0.), offset*transform, m_shapes[0]);
		} else
		{
			m_bodies[0] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[0]);
		}
		// legs
		for ( i=0; i<NUM_LEGS; i++)
		{
			float fAngle = 2 * M_PI * i / NUM_LEGS;
			float fSin = sin(fAngle);
			float fCos = cos(fAngle);

			transform.setIdentity();
			btVector3 vBoneOrigin = btVector3(btScalar(fCos*(fBodySize+0.5*fLegLength)), btScalar(fHeight), btScalar(fSin*(fBodySize+0.5*fLegLength)));
			transform.setOrigin(vBoneOrigin);

			// thigh
			btVector3 vToBone = (vBoneOrigin - vRoot).normalize();
			btVector3 vAxis = vToBone.cross(vUp);			
			transform.setRotation(btQuaternion(vAxis, M_PI_2));
			m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[1+2*i]);

			// shin
			transform.setIdentity();
			transform.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(fHeight-0.5*fForeLegLength), btScalar(fSin*(fBodySize+fLegLength))));
			m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[2+2*i]);
		}

		// Setup some damping on the m_bodies
		for (i = 0; i < BODYPART_COUNT; ++i)
		{
			m_bodies[i]->setDamping(0.05, 0.85);
			m_bodies[i]->setDeactivationTime(0.8);
			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
		}


		//
		// Setup the constraints
		//
		btHingeConstraint* hingeC;
		//btConeTwistConstraint* coneC;

		btTransform localA, localB, localC;

		for ( i=0; i<NUM_LEGS; i++)
		{
			float fAngle = 2 * M_PI * i / NUM_LEGS;
			float fSin = sin(fAngle);
			float fCos = cos(fAngle);

			// hip joints
			localA.setIdentity(); localB.setIdentity();
			localA.getBasis().setEulerZYX(0,-fAngle,0);	localA.setOrigin(btVector3(btScalar(fCos*fBodySize), btScalar(0.), btScalar(fSin*fBodySize)));
			localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
			hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
			hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
			//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
			m_joints[2*i] = hingeC;
			m_ownerWorld->addConstraint(m_joints[2*i], true);

			// knee joints
			localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
			localA.getBasis().setEulerZYX(0,-fAngle,0);	localA.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(0.), btScalar(fSin*(fBodySize+fLegLength))));
			localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
			localC = m_bodies[2+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
			hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
			//hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
			hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2));
			m_joints[1+2*i] = hingeC;
			m_ownerWorld->addConstraint(m_joints[1+2*i], true);
		}
	}
示例#17
0
void workshopScene::update(sceneInfo &info)
{
    gWorld->btWorld->stepSimulation(1.f/60.f);

    mousevelx = mousevelx * 0.95f + info.dmousex * 0.2f;
    mousevely = mousevely * 0.95f + info.dmousey * 0.2f;

    if (info.keys.held.MouseM)
    {
        camera.pitch -= info.dmousey * 0.02;
        camera.yaw -= info.dmousex * 0.02;
        camera.orientationFromAngles();
    }

    sceneInfo::keyState &keys = info.keys;
    if (!glfwGetKey('E'))
    {
        if (keys.held.W)
        {
            camera.position += camera.forward * 0.1;
        }
        else if (keys.held.S)
        {
            camera.position -= camera.forward * 0.1;
        }
        if (keys.held.D)
        {
            camera.position += camera.right * 0.1;
        }
        else if (keys.held.A)
        {
            camera.position -= camera.right * 0.1;
        }
    }
    if (info.captureMouse)
    {
        if (mouseWasCaptured)
        {
            cursorx += info.dmousex;
            cursory += info.dmousey;
            if (cursorx < 0)
                cursorx = 0;
            if (cursorx > info.width)
                cursorx = info.width;
            if (cursory < 0)
                cursory = 0;
            if (cursory > info.height)
                cursory = info.height;
        }
        else
        {
            cursorx = info.lastmousex;
            cursory = info.lastmousey;
        }
    }
    else if (mouseWasCaptured)
    {
        glfwSetMousePos(cursorx, cursory);
    }
    mouseWasCaptured = info.captureMouse;

    if (info.keys.held.space && selectedItem < partnames.size())
    {
        int initialcount = gWorld->objects.size();
        loadAssembly(partnames[selectedItem], btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 10, 0)), path, gWorld);
        for (unsigned int i = initialcount; i < gWorld->objects.size(); i++)
            gWorld->objects[i]->body->setDamping(0.95f, 0.96f);
    }

    if (cursorx < UI_SIZE)
    {
        if (keys.newPress.MouseL)
        {
            selectedItem = (cursory - 4) / (UI_SIZE + 8);
            selectedTool = -1;
        }
    }
    else if (cursory > info.height - UI_SMALL - 16)
    {
        if (keys.newPress.MouseL)
        {
            selectedTool = (cursorx - UI_SIZE - 16 - 8) / (UI_SMALL + 8);
            selectedItem = -1;
        }
    }
    else
    {
        mouseRayDir = camera.forward + camera.right * (cursorx - info.width / 2) / (float)info.height * 2 - camera.up * (cursory - info.height / 2) / (float)info.height * 2;
        btVector3 raystart = camera.position;
        btVector3 rayend = raystart + mouseRayDir * 100;
        mouseRayCallback = btCollisionWorld::ClosestRayResultCallback(raystart, rayend);
        gWorld->btWorld->rayTest(raystart, rayend, mouseRayCallback);

        if (keys.newPress.MouseL && mouseRayCallback.hasHit())
        {
            mouseHeldBody = (btRigidBody*)mouseRayCallback.m_collisionObject;
            if (selectedItem >= 0)
            {
                if (selectedItem < partnames.size())
                {
                    int initialcount = gWorld->objects.size();
                    btTransform trans(btQuaternion(0, 0, 0, 1), mouseRayCallback.m_hitPointWorld + mouseRayCallback.m_hitNormalWorld * 1.f);
                    loadAssembly(partnames[selectedItem], trans, path, gWorld);
                    for (unsigned int i = initialcount; i < gWorld->objects.size(); i++)
                        gWorld->objects[i]->body->setDamping(0.95f, 0.96f);
                }
            }
            else if (selectedTool == 0)
            {
                mousePerpDist = camera.forward.dot(mouseRayCallback.m_hitPointWorld - raystart);
                mouseHeldBody->setDamping(0.995, 0.98);
                mouseHeldBody->activate();
                btVector3 localPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * mouseRayCallback.m_hitPointWorld;
                mouseConstraint = new btGeneric6DofConstraint(*mouseHeldBody, btTransform(btQuaternion(0, 0, 0, 1), localPivot), false);
                if (glfwGetKey('E'))
                {
                     mouseConstraint->setAngularLowerLimit(btVector3(0, 0, 0));
                    mouseConstraint->setAngularUpperLimit(btVector3(0, 0, 0));
                }
                gWorld->btWorld->addConstraint(mouseConstraint);
            }
            else if (selectedTool == 1)
            {
                if (!mouseHeldBody->isStaticObject())
                {
                    if (!axisHasFirst)
                    {
                        axisHasFirst = true;
                        axisResult = mouseRayCallback;
                        axisFirstPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * axisResult.m_hitPointWorld;
                        axisFirstNormal = btTransform(mouseHeldBody->getCenterOfMassTransform().inverse().getRotation(), btVector3(0, 0, 0)) * axisResult.m_hitNormalWorld;
                        std::cout << "First point for axis.\n";
                    }
                    else
                    {
                        btVector3 axisSecondNormal = btTransform(mouseHeldBody->getCenterOfMassTransform().inverse().getRotation(), btVector3(0, 0, 0)) * mouseRayCallback.m_hitNormalWorld;
                        btVector3 axisSecondPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * mouseRayCallback.m_hitPointWorld + axisSecondNormal * 0.05;
                        gWorld->addConstraint(new btHingeConstraint(*(btRigidBody*)axisResult.m_collisionObject, *mouseHeldBody, axisFirstPivot, axisSecondPivot, -axisFirstNormal, axisSecondNormal));
                        mouseHeldBody->activate();
                        axisHasFirst = false;
                    }
                }
            }
            else if (selectedTool == 2)
            {
                btRigidBody *body = (btRigidBody*)mouseRayCallback.m_collisionObject;
                if (!body->isStaticObject())
                    gWorld->removeBody(body);
            }
        }
    }

    if (mouseConstraint)
    {
        if (keys.held.MouseL)
        {
            mousePerpDist *= pow(1.1, keys.dmouseWheel);
            mouseConstraint->getFrameOffsetA().setOrigin(camera.position + mouseRayDir * mousePerpDist);        // note that raydir is not unit length: it stretches from the camera to the near plane.
        }
        else
        {
            mouseHeldBody->setDamping(0.95, 0.96);
            gWorld->btWorld->removeConstraint(mouseConstraint);
            delete mouseConstraint;
            mouseConstraint = 0;
        }
        if (glfwGetKey('E'))
        {
            btTransform invrotate;
            mouseHeldBody->getMotionState()->getWorldTransform(invrotate);
            invrotate = invrotate.inverse();
            if (keys.held.W)
                mouseHeldBody->applyImpulse(invrotate * camera.forward, invrotate * camera.up);
            if (keys.held.S)
                mouseHeldBody->applyImpulse(-camera.forward, camera.up);
            if (keys.held.A)
                mouseHeldBody->applyImpulse(-camera.right, camera.forward);
            if (keys.held.D)
                mouseHeldBody->applyImpulse(camera.right, camera.forward);
        }
    }

    if (axisHasFirst && selectedTool != 1)
        axisHasFirst = false;
}