C++ btAlignedObjectArray::size方法代码示例

static void debugDrawAllBatches(const btBatchedConstraints* bc,
								btConstraintArray* constraints,
								const btAlignedObjectArray<btSolverBody>& bodies)
{
	BT_PROFILE("debugDrawAllBatches");
	if (bc && bc->m_debugDrawer && bc->m_phases.size() > 0)
	{
		btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
		btVector3 bboxMax = -bboxMin;
		for (int iBody = 0; iBody < bodies.size(); ++iBody)
		{
			const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
			bboxMin.setMin(pos);
			bboxMax.setMax(pos);
		}
		btVector3 bboxExtent = bboxMax - bboxMin;
		btVector3 offsetBase = btVector3(0, bboxExtent.y() * 1.1f, 0);
		btVector3 offsetStep = btVector3(0, 0, bboxExtent.z() * 1.1f);
		int numPhases = bc->m_phases.size();
		for (int iPhase = 0; iPhase < numPhases; ++iPhase)
		{
			float b = float(iPhase) / float(numPhases - 1);
			btVector3 color0 = btVector3(1, 0, b);
			btVector3 color1 = btVector3(0, 1, b);
			btVector3 offset = offsetBase + offsetStep * (float(iPhase) - float(numPhases - 1) * 0.5);
			debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
		}
	}
}

void ExampleEntries::initExampleEntries()
{
	m_data->m_allExamples.clear();

	for (int i=0;i<gAdditionalRegisteredExamples.size();i++)
	{
		m_data->m_allExamples.push_back(gAdditionalRegisteredExamples[i]);
	}
	
	

	int numDefaultEntries = sizeof(gDefaultExamples)/sizeof(ExampleEntry);
	for (int i=0;i<numDefaultEntries;i++)
	{
		m_data->m_allExamples.push_back(gDefaultExamples[i]);
	}

	if (m_data->m_allExamples.size()==0)
	{

		{
			ExampleEntry e(0,"Empty");
			m_data->m_allExamples.push_back(e);
		}

		{
			ExampleEntry e(1,"Empty","Empty Description", EmptyExample::CreateFunc);
			m_data->m_allExamples.push_back(e);
		}
	}

}

void openFileDemo(const char* filename)
{

	deleteDemo();
   
	s_guiHelper= new OpenGLGuiHelper(s_app, sUseOpenGL2);
	s_guiHelper->setVisualizerFlagCallback(OpenGLExampleBrowserVisualizerFlagCallback);

    s_parameterInterface->removeAllParameters();
   

	CommonExampleOptions options(s_guiHelper,1);
	options.m_fileName = filename;
	char fullPath[1024];
	sprintf(fullPath, "%s", filename);
	b3FileUtils::toLower(fullPath);
	
	for (int i=0;i<gFileImporterByExtension.size();i++)
    {
        if (strstr(fullPath, gFileImporterByExtension[i].m_extension.c_str()))
        {
            sCurrentDemo = gFileImporterByExtension[i].m_createFunc(options);
        }   
    }
    
	
    if (sCurrentDemo)
    {
        sCurrentDemo->initPhysics();
		sCurrentDemo->resetCamera();
    }


}

void updatePhysicsWorld()
{
	static int counter = 0;

	// Change wind velocity a bit based on a frame counter
	if( (counter % 400) == 0 )
	{
		_windAngle = (_windAngle + 0.05f);
		if( _windAngle > (2*3.141) )
			_windAngle = 0;

		for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
		{		
			btSoftBody *cloth = 0;

			cloth = m_flags[flagIndex];

			float localWind = _windAngle + 0.5*(((float(rand())/RAND_MAX))-0.1);
			float xCoordinate = cos(localWind)*_windStrength;
			float zCoordinate = sin(localWind)*_windStrength;

			cloth->setWindVelocity( btVector3(xCoordinate, 0, zCoordinate) );
		}
	}

	//btVector3 origin( capCollider->getWorldTransform().getOrigin() );
	//origin.setX( origin.getX() + 0.05 );
	//capCollider->getWorldTransform().setOrigin( origin );
	
	counter++;
}

void CcdPhysicsDemo::clientMoveAndDisplay()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

	//simple dynamics world doesn't handle fixed-time-stepping
	//float ms = getDeltaTimeMicroseconds();
	
	///step the simulation
	if (m_dynamicsWorld)
	{
		m_dynamicsWorld->stepSimulation(1./60.,0);//ms / 1000000.f);
		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	}
		
	renderme(); 

	displayText();
#if 0
	for (int i=0;i<debugContacts.size();i++)
	{
		getDynamicsWorld()->getDebugDrawer()->drawContactPoint(debugContacts[i],debugNormals[i],0,0,btVector3(1,0,0));
	}
#endif

	glFlush();

	swapBuffers();

}

void ExitBulletPhysics()
{
	//cleanup in the reverse order of creation/initialization

	for (int i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0; i--) //remove the rigidbodies from the dynamics world and delete them
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if(body && body->getMotionState())
			delete body->getMotionState();

		m_dynamicsWorld->removeCollisionObject(obj);
		delete obj;
	}


	for (int j=0; j<m_collisionShapes.size(); j++) //delete collision shapes
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}

	m_collisionShapes.clear();
	delete m_dynamicsWorld;
	delete m_solver;
	delete m_broadphase;
	delete m_dispatcher;
	delete m_collisionConfiguration;
}

static inline void			add(btAlignedObjectArray<T>& items,const Q& value)
{
	for(int i=0,ni=items.size();i<ni;++i)
	{
		items[i]+=value;
	}
}

void MotorDemo::setMotorTargets(btScalar deltaTime)
{

	float ms = deltaTime*1000000.;
	float minFPS = 1000000.f/60.f;
	if (ms > minFPS)
		ms = minFPS;

	m_Time += ms;

	//
	// set per-frame sinusoidal position targets using angular motor (hacky?)
	//	
	for (int r=0; r<m_rigs.size(); r++)
	{
		for (int i=0; i<2*NUM_LEGS; i++)
		{
			btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_rigs[r]->GetJoints()[i]);
			btScalar fCurAngle      = hingeC->getHingeAngle();
			
			btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
			btScalar fTargetAngle   = 0.5 * (1 + sin(2 * M_PI * fTargetPercent));
			btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
			btScalar fAngleError  = fTargetLimitAngle - fCurAngle;
			btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms;
			hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
		}
	}

	
}

/// removes all objects and shapes from the world
void TerrainDemo::clearWorld(void)
{
	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	m_collisionShapes.clear();

	// delete raw heightfield data
	delete m_rawHeightfieldData;
	m_rawHeightfieldData = NULL;
}

void doFlags()
{
	//float ms = getDeltaTimeMicroseconds();
	btScalar dt = (btScalar)m_clock.getTimeMicroseconds();
	m_clock.reset();

	///step the simulation
	if( m_dynamicsWorld )
	{
		m_dynamicsWorld->stepSimulation(dt/1000000.);

		static int frameCount = 0;
		frameCount++;
		if (frameCount==100)
		{
 			m_dynamicsWorld->stepSimulation(1./60.,0);
			CProfileManager::dumpAll();
		}
		updatePhysicsWorld();

		//m_dynamicsWorld->setDebugDrawer(&debugDraw);
		//debugDraw.setDebugMode(btIDebugDraw::DBG_DrawWireframe);
		//g_solver->copyBackToSoftBodies();

		//m_dynamicsWorld->debugDrawWorld();
		
	}
	

	for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
	{
		g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor );
		cloths[flagIndex].draw();
	}
}

	void	render()
	{
	

		
		btScalar childMat[16];
		m_bulletObject->getWorldTransform().getOpenGLMatrix(childMat);

		if (m_texture)
		{
			m_texture->initOpenGLTexture();

			glBindTexture(GL_TEXTURE_2D,m_texture->m_textureName);

			glEnable(GL_TEXTURE_2D);
			glDisable(GL_TEXTURE_GEN_S);
			glDisable(GL_TEXTURE_GEN_T);
			glDisable(GL_TEXTURE_GEN_R);
			
			glBlendFunc(GL_SRC_ALPHA,GL_ONE);
			glDepthFunc (GL_LEQUAL);
			glDisable(GL_BLEND);
			glEnable (GL_DEPTH_TEST);

			glMatrixMode(GL_TEXTURE);
			
			
			glMatrixMode(GL_MODELVIEW);


		} else
		{
			glDisable(GL_TEXTURE_2D);
		}

		glDisable(GL_LIGHTING);
		glPushMatrix();
		
		
		btglMultMatrix(childMat);
		
		//glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE);
		
		glBegin(GL_TRIANGLES);
		
		glColor4f(1, 1, 1,1);
		
		for (int i=0;i<m_indices.size();i++)
		{
			glNormal3f(1.f,0.f,0.f);
			glTexCoord2f(m_vertices[m_indices[i]].m_uv1[0],m_vertices[m_indices[i]].m_uv1[1]);
			glVertex3f(m_vertices[m_indices[i]].m_localxyz.getX(),m_vertices[m_indices[i]].m_localxyz.getY(),m_vertices[m_indices[i]].m_localxyz.getZ());
			
		}
		glEnd();

		glPopMatrix();
		
	}

/*
	for(int i = 0; i < box_body.size(); ++i) {
		glPushMatrix();
		btTransform trans;
		box_body[i]->getMotionState()->getWorldTransform(trans);
		float matrix[16];
		trans.getOpenGLMatrix(matrix);
		glMultMatrixf(matrix);
		glEnableClientState(GL_VERTEX_ARRAY);
		//		glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
		glVertexPointer(3, GL_FLOAT, 0, verticesBox);
		//		glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vbo[1]);
		glColor4f(255.0f, 0.0f, 0.0f, 255.0f);
		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[0]);
		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[4]);
		glColor4f(0.0f, 0.0f, 255.0f, 255.0f);
		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[8]);
		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[12]);
		glColor4f(0.0f, 255.0f, 0.0f, 255.0f);
		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[16]);
		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[20]);
		glDisableClientState(GL_VERTEX_ARRAY);
		glPopMatrix();

	}
//
}
*/
void renderShootBox(float x, float y, float z, GLuint program, ESMatrix* pMat, ESMatrix* vMat){
//*
	for(int i = 0; i < box_body.size(); ++i) {
		btTransform trans;


		//world->removeRigidBody(body);
		//motionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), btVector3(0,y,0)));
		//body->setMotionState(motionState);
		//body->setAngularVelocity(btVector3(x,0,z));
		box_body[i]->getMotionState()->getWorldTransform(trans);
		trans.setRotation(btQuaternion(btVector3(0,0,1),SIMD_PI*0.01*x));

//		btTransform groundTransform;
//		groundTransform.setIdentity();
//		groundTransform.setOrigin( btVector3(0,-10,0) );
//		groundTransform.setRotation(btQuaternion(btVector3(0,0,1),SIMD_PI*0.01 * x));
//		//delete motionState;
//		motionState = new btDefaultMotionState(groundTransform);
//		box_body[i]->setMotionState(motionState);
		float matrix[16] ;
		trans.getOpenGLMatrix(matrix);
		ESMatrix mvMat;
		ESMatrix mvpMat;
		esMatrixMultiply(&mvMat, (ESMatrix*)matrix, vMat);
		esMatrixMultiply(&mvpMat, &mvMat, pMat);

		glUseProgram(program);
		glUniformMatrix4fv( glGetUniformLocation(program, "mvpMat"), 1, false, (GLfloat*)&mvpMat );



		glFrontFace(GL_CW);

		glVertexAttribPointer(0, 3, GL_FLOAT, false, 0, verticesBox);

		glEnableVertexAttribArray(0);

		int index = (i)%5;
		switch(index){
			case 0:glUniform4f( glGetUniformLocation(program, "_Color"), 0.0f, 255.0f, 0.0f, 0.0f); break;
			case 1:glUniform4f( glGetUniformLocation(program, "_Color"), 0.0f, 255.0f, 255.0f, 0.0f); break;
			case 2:glUniform4f( glGetUniformLocation(program, "_Color"), 255.0f, 0.0f, 255.0f, 0.0f); break;
			case 3:glUniform4f( glGetUniformLocation(program, "_Color"), 0.0f, 0.0f, 0.0f, 0.0f); break;
			case 4:glUniform4f( glGetUniformLocation(program, "_Color"), 255.0f, 255.0f, 0.0f, 0.0f); break;
		}
//		glUniform4f( glGetUniformLocation(program, "_Color"), 255.0f, 0.0f, 0.0f, 255.0f);
//		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[0]);
//		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[4]);
//		glUniform4f( glGetUniformLocation(program, "_Color"), , 255.0f);
		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[8]);
//		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[12]);
//		glUniform4f( glGetUniformLocation(program, "_Color"), , 255.0f);
//		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[16]);
//		glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[20]);
		glUseProgram(0);
	}
//*/
}

void	btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
{
	const int numbrushes = planeEquations.size();
	// brute force:
	for (int i=0;i<numbrushes;i++)
	{
		const btVector3& N1 = planeEquations[i];
		

		for (int j=i+1;j<numbrushes;j++)
		{
			const btVector3& N2 = planeEquations[j];
				
			for (int k=j+1;k<numbrushes;k++)
			{

				const btVector3& N3 = planeEquations[k];

				btVector3 n2n3; n2n3 = N2.cross(N3);
				btVector3 n3n1; n3n1 = N3.cross(N1);
				btVector3 n1n2; n1n2 = N1.cross(N2);
				
				if ( ( n2n3.length2() > btScalar(0.0001) ) &&
					 ( n3n1.length2() > btScalar(0.0001) ) &&
					 ( n1n2.length2() > btScalar(0.0001) ) )
				{
					//point P out of 3 plane equations:

					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
					//P =  -------------------------------------------------------------------------  
					//   N1 . ( N2 * N3 )  


					btScalar quotient = (N1.dot(n2n3));
					if (btFabs(quotient) > btScalar(0.000001))
					{
						quotient = btScalar(-1.) / quotient;
						n2n3 *= N1[3];
						n3n1 *= N2[3];
						n1n2 *= N3[3];
						btVector3 potentialVertex = n2n3;
						potentialVertex += n3n1;
						potentialVertex += n1n2;
						potentialVertex *= quotient;

						//check if inside, and replace supportingVertexOut if needed
						if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
						{
							verticesOut.push_back(potentialVertex);
						}
					}
				}
			}
		}
	}
}

void MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime)
{
	//m_multiBody->addLinkForce(0,btVector3(100,100,100));
    if (0)//m_once)
    {
       m_once=false;
        m_multiBody->addJointTorque(0, 10.0);
    
        btScalar torque = m_multiBody->getJointTorque(0);
        b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
    }
   btScalar timeStep = 1./240.f;
 
    m_dynamicsWorld->stepSimulation(timeStep,0);

	static int count = 0;
	if ((count& 0x0f)==0)
	{
	if (m_motor)
	{
	float force = m_motor->getAppliedImpulse(0)/timeStep;
	b3Printf("motor applied force = %f\n", force);
	}	

	for (int i=0;i<m_jointFeedbacks.size();i++)
	{
			b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
			i,
			m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
			m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
			m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],

		m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
			m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
			m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]

		);

	}
	}
	count++;


	/*
    b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
             m_multiBody->getBaseOmega()[1],
             m_multiBody->getBaseOmega()[2]
             );
    */
    btScalar jointVel =m_multiBody->getJointVel(0);
    
//    b3Printf("child angvel = %f",jointVel);
    
    
    
}

// Function to remove an object from a vector maintaining correct ordering of the vector
template< typename T > static void removeFromVector( btAlignedObjectArray< T > &vectorToUpdate, int indexToRemove )
{
	int currentSize = vectorToUpdate.size();
	for( int i = indexToRemove; i < (currentSize-1); ++i )
	{
		vectorToUpdate[i] = vectorToUpdate[i+1];
	}
	if( currentSize > 0 )
		vectorToUpdate.resize( currentSize - 1 );
}