C++ btAssert函数代码示例

void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
		btMultiBodyJacobianData& data,
		const btContactSolverInfo& infoGlobal)
{
    // only positions need to be updated -- data.m_jacobians and force
    // directions were set in the ctor and never change.
	
	if (m_numDofsFinalized != m_jacSizeBoth)
	{
        finalizeMultiDof();
	}

	//don't crash
	if (m_numDofsFinalized != m_jacSizeBoth)
		return;

	
	if (m_maxAppliedImpulse==0.f)
		return;
	
	// note: we rely on the fact that data.m_jacobians are
	// always initialized to zero by the Constraint ctor
	int linkDoF = 0;
	unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
	unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);

	// row 0: the lower bound
	jacobianA(0)[offsetA] = 1;
	jacobianB(0)[offsetB] = m_gearRatio;

	const btScalar posError = 0;
	const btVector3 dummy(0, 0, 0);
	btScalar erp = infoGlobal.m_erp;
	btScalar kp = 1;
	btScalar kd = 1;
	int numRows = getNumRows();

	for (int row=0;row<numRows;row++)
	{
		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();


        int dof = 0;
        btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
        btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
		btScalar auxVel = 0;
		
		if (m_gearAuxLink>=0)
		{
			auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
		}
		currentVelocity += auxVel;
			
		//btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
        //btScalar velocityError = (m_desiredVelocity - currentVelocity);

        btScalar desiredRelativeVelocity =   auxVel;
    
		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);

		constraintRow.m_orgConstraint = this;
		constraintRow.m_orgDofIndex = row;
		{
			//expect either prismatic or revolute joint type for now
			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
			switch (m_bodyA->getLink(m_linkA).m_jointType)
			{
				case btMultibodyLink::eRevolute:
				{
					constraintRow.m_contactNormal1.setZero();
					constraintRow.m_contactNormal2.setZero();
					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
					
					break;
				}
				case btMultibodyLink::ePrismatic:
				{
					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
					constraintRow.m_contactNormal1=prismaticAxisInWorld;
					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
					constraintRow.m_relpos1CrossNormal.setZero();
					constraintRow.m_relpos2CrossNormal.setZero();					
					break;
				}
				default:
				{
					btAssert(0);
				}
			};
			
		}

	}

}

	MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
	{
		btAssert(0);
		(void)other;
		return *this;
	}

void	btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
{
    (void)index;

    btAssert(m_useQuantization);

    int curNodeSubPart=-1;

    //get access info to trianglemesh data
    const unsigned char *vertexbase = 0;
    int numverts = 0;
    PHY_ScalarType type = PHY_INTEGER;
    int stride = 0;
    const unsigned char *indexbase = 0;
    int indexstride = 0;
    int numfaces = 0;
    PHY_ScalarType indicestype = PHY_INTEGER;

    btVector3	triangleVerts[3];
    btVector3	aabbMin,aabbMax;
    const btVector3& meshScaling = meshInterface->getScaling();

    int i;
    for (i=endNode-1; i>=firstNode; i--)
    {


        btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
        if (curNode.isLeafNode())
        {
            //recalc aabb from triangle data
            int nodeSubPart = curNode.getPartId();
            int nodeTriangleIndex = curNode.getTriangleIndex();
            if (nodeSubPart != curNodeSubPart)
            {
                if (curNodeSubPart >= 0)
                    meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
                meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);

                curNodeSubPart = nodeSubPart;
                btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
            }
            //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,

            unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);


            for (int j=2; j>=0; j--)
            {

                int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
                if (type == PHY_FLOAT)
                {
                    float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
                    triangleVerts[j] = btVector3(
                                           graphicsbase[0]*meshScaling.getX(),
                                           graphicsbase[1]*meshScaling.getY(),
                                           graphicsbase[2]*meshScaling.getZ());
                }
                else
                {
                    double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
                    triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
                }
            }



            aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
            aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
            aabbMin.setMin(triangleVerts[0]);
            aabbMax.setMax(triangleVerts[0]);
            aabbMin.setMin(triangleVerts[1]);
            aabbMax.setMax(triangleVerts[1]);
            aabbMin.setMin(triangleVerts[2]);
            aabbMax.setMax(triangleVerts[2]);

            quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
            quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);

        } else
        {
            //combine aabb from both children

            btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];

            btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
                                                 &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];


            {
                for (int i=0; i<3; i++)
                {
                    curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
                    if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
                        curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];

                    curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
                    if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
                        curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
//.........这里部分代码省略.........

/**
  basic algorithm:
    - convert input aabb to local coordinates (scale down and shift for local origin)
    - convert input aabb to a range of heightfield grid points (quantize)
    - iterate over all triangles in that subset of the grid
 */
void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
{
	// scale down the input aabb's so they are in local (non-scaled) coordinates
	btVector3	localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
	btVector3	localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);

	// account for local origin
	localAabbMin += m_localOrigin;
	localAabbMax += m_localOrigin;

	//quantize the aabbMin and aabbMax, and adjust the start/end ranges
	int	quantizedAabbMin[3];
	int	quantizedAabbMax[3];
	quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
	quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
	
	// expand the min/max quantized values
	// this is to catch the case where the input aabb falls between grid points!
	for (int i = 0; i < 3; ++i) {
		quantizedAabbMin[i]--;
		quantizedAabbMax[i]++;
	}	

	int startX=0;
	int endX=m_heightStickWidth-1;
	int startJ=0;
	int endJ=m_heightStickLength-1;

	switch (m_upAxis)
	{
	case 0:
		{
			if (quantizedAabbMin[1]>startX)
				startX = quantizedAabbMin[1];
			if (quantizedAabbMax[1]<endX)
				endX = quantizedAabbMax[1];
			if (quantizedAabbMin[2]>startJ)
				startJ = quantizedAabbMin[2];
			if (quantizedAabbMax[2]<endJ)
				endJ = quantizedAabbMax[2];
			break;
		}
	case 1:
		{
			if (quantizedAabbMin[0]>startX)
				startX = quantizedAabbMin[0];
			if (quantizedAabbMax[0]<endX)
				endX = quantizedAabbMax[0];
			if (quantizedAabbMin[2]>startJ)
				startJ = quantizedAabbMin[2];
			if (quantizedAabbMax[2]<endJ)
				endJ = quantizedAabbMax[2];
			break;
		};
	case 2:
		{
			if (quantizedAabbMin[0]>startX)
				startX = quantizedAabbMin[0];
			if (quantizedAabbMax[0]<endX)
				endX = quantizedAabbMax[0];
			if (quantizedAabbMin[1]>startJ)
				startJ = quantizedAabbMin[1];
			if (quantizedAabbMax[1]<endJ)
				endJ = quantizedAabbMax[1];
			break;
		}
	default:
		{
			//need to get valid m_upAxis
			btAssert(0);
		}
	}

	
  

	for(int j=startJ; j<endJ; j++)
	{
		for(int x=startX; x<endX; x++)
		{
			btVector3 vertices[3];
			if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision  && !(j & 1)))
			{
        //first triangle
        getVertex(x,j,vertices[0]);
        getVertex(x+1,j,vertices[1]);
        getVertex(x+1,j+1,vertices[2]);
        callback->processTriangle(vertices,x,j);
        //second triangle
      //  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
        getVertex(x+1,j+1,vertices[1]);
        getVertex(x,j+1,vertices[2]);
        callback->processTriangle(vertices,x,j);				
			} else
//.........这里部分代码省略.........

void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const
{

	btAssert(0);
}

void SpuContactManifoldCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
	btAssert(0);
}

void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
#endif
{
	m_cachedSeparatingDistance = 0.f;

	btScalar distance=btScalar(0.);
	btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
	btVector3 pointOnA,pointOnB;
	btTransform	localTransA = input.m_transformA;
	btTransform localTransB = input.m_transformB;
	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
	localTransA.getOrigin() -= positionOffset;
	localTransB.getOrigin() -= positionOffset;

	bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();

	btScalar marginA = m_marginA;
	btScalar marginB = m_marginB;

	gNumGjkChecks++;

#ifdef DEBUG_SPU_COLLISION_DETECTION
	spu_printf("inside gjk\n");
#endif
	//for CCD we don't use margins
	if (m_ignoreMargin)
	{
		marginA = btScalar(0.);
		marginB = btScalar(0.);
#ifdef DEBUG_SPU_COLLISION_DETECTION
		spu_printf("ignoring margin\n");
#endif
	}

	m_curIter = 0;
	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
	m_cachedSeparatingAxis.setValue(0,1,0);

	bool isValid = false;
	bool checkSimplex = false;
	bool checkPenetration = true;
	m_degenerateSimplex = 0;

	m_lastUsedMethod = -1;

	{
		btScalar squaredDistance = BT_LARGE_FLOAT;
		btScalar delta = btScalar(0.);
		
		btScalar margin = marginA + marginB;
		
		

		m_simplexSolver->reset();
		
		for ( ; ; )
		//while (true)
		{

			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();

#if 1

			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);

//			btVector3 pInA  = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
//			btVector3 qInB  = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);

#else
#ifdef __SPU__
			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
#else
			btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
			btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
#ifdef TEST_NON_VIRTUAL
			btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
			btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
			btAssert((pInAv-pInA).length() < 0.0001);
			btAssert((qInBv-qInB).length() < 0.0001);
#endif //
#endif //__SPU__
#endif


			btVector3  pWorld = localTransA(pInA);	
			btVector3  qWorld = localTransB(qInB);

#ifdef DEBUG_SPU_COLLISION_DETECTION
		spu_printf("got local supporting vertices\n");
#endif

			if (check2d)
			{
				pWorld[2] = 0.f;
				qWorld[2] = 0.f;
			}

//.........这里部分代码省略.........

void OpenGL2Renderer::renderPhysicsWorld(int numObjects, btCollisionObject** objectArray)
{
	init();

	updateCamera();

	if (1)
	{			
		if(m_enableshadows)
		{
			glClear(GL_STENCIL_BUFFER_BIT);
			glEnable(GL_CULL_FACE);
			GLint err = glGetError();
			btAssert(err==GL_NO_ERROR);
			
			renderscene(0,numObjects,objectArray);

			err = glGetError();
			btAssert(err==GL_NO_ERROR);
			
			glDisable(GL_LIGHTING);
			glDepthMask(GL_FALSE);
			glDepthFunc(GL_LEQUAL);
			glEnable(GL_STENCIL_TEST);
			glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE);
			glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL);
			glFrontFace(GL_CCW);
			glStencilOp(GL_KEEP,GL_KEEP,GL_INCR);
			renderscene(1,numObjects,objectArray);
			glFrontFace(GL_CW);
			glStencilOp(GL_KEEP,GL_KEEP,GL_DECR);
			renderscene(1,numObjects,objectArray);
			glFrontFace(GL_CCW);

			err = glGetError();
			btAssert(err==GL_NO_ERROR);
			
			glPolygonMode(GL_FRONT,GL_FILL);
			glPolygonMode(GL_BACK,GL_FILL);
			glShadeModel(GL_SMOOTH);
			glEnable(GL_DEPTH_TEST);
			glDepthFunc(GL_LESS);
			glEnable(GL_LIGHTING);
			glDepthMask(GL_TRUE);
			glCullFace(GL_BACK);
			glFrontFace(GL_CCW);
			glEnable(GL_CULL_FACE);
			glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);

			glDepthFunc(GL_LEQUAL);
			glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL );
			glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP );
			glDisable(GL_LIGHTING);
			renderscene(2,numObjects,objectArray);
			glEnable(GL_LIGHTING);
			glDepthFunc(GL_LESS);
			glDisable(GL_STENCIL_TEST);
			glDisable(GL_CULL_FACE);
			
			err = glGetError();
			btAssert(err==GL_NO_ERROR);
			
		}
		else
		{
			glDisable(GL_CULL_FACE);
			renderscene(0,numObjects,objectArray);
		}

		int	xOffset = 10;
		int yStart = 20;
		int yIncr = 20;


		glDisable(GL_LIGHTING);
		glColor3f(0, 0, 0);

		if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0)
		{
			setOrthographicProjection();

			//showProfileInfo(xOffset,yStart,yIncr);

#if 0//#ifdef USE_QUICKPROF

		
			if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
			{
				static int counter = 0;
				counter++;
				std::map<std::string, hidden::ProfileBlock*>::iterator iter;
				for (iter = btProfiler::mProfileBlocks.begin(); iter != btProfiler::mProfileBlocks.end(); ++iter)
				{
					char blockTime[128];
					sprintf(blockTime, "%s: %lf",&((*iter).first[0]),btProfiler::getBlockTime((*iter).first, btProfiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
					glRasterPos3f(xOffset,yStart,0);
					GLDebugDrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
					yStart += yIncr;

				}
//.........这里部分代码省略.........

void OpenGL2Renderer::updateCamera()
{
	GLint err = glGetError();
	btAssert(err==GL_NO_ERROR);
	
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	btScalar rele = m_ele * btScalar(0.01745329251994329547);// rads per deg
	btScalar razi = m_azi * btScalar(0.01745329251994329547);// rads per deg


	btQuaternion rot(m_cameraUp,razi);


	btVector3 eyePos(0,0,0);
	eyePos[m_forwardAxis] = -m_cameraDistance;

	btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
	if (forward.length2() < SIMD_EPSILON)
	{
		forward.setValue(1.f,0.f,0.f);
	}
	btVector3 right = m_cameraUp.cross(forward);
	btQuaternion roll(right,-rele);

	err = glGetError();
	btAssert(err==GL_NO_ERROR);
	
	eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;

	m_cameraPosition[0] = eyePos.getX();
	m_cameraPosition[1] = eyePos.getY();
	m_cameraPosition[2] = eyePos.getZ();
	m_cameraPosition += m_cameraTargetPosition;

	if (m_openglViewportWidth == 0 && m_openglViewportHeight == 0)
		return;

	btScalar aspect;
	btVector3 extents;

	aspect = m_openglViewportWidth / (btScalar)m_openglViewportHeight;
	extents.setValue(aspect * 1.0f, 1.0f,0);
	
	
	if (m_ortho)
	{
		// reset matrix
		glLoadIdentity();
		extents *= m_cameraDistance;
		btVector3 lower = m_cameraTargetPosition - extents;
		btVector3 upper = m_cameraTargetPosition + extents;
		glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
		
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
	} else
	{
		glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2], 
			m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2], 
			m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
	}
	err = glGetError();
	btAssert(err==GL_NO_ERROR);
	
}

void	btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
{
    (void)aabbMin;
    (void)aabbMax;
    int numtotalphysicsverts = 0;
    int part, graphicssubparts = getNumSubParts();
    const unsigned char * vertexbase;
    const unsigned char * indexbase;
    int indexstride;
    PHY_ScalarType type;
    PHY_ScalarType gfxindextype;
    int stride, numverts, numtriangles;
    int gfxindex;
    btVector3 triangle[3];

    btVector3 meshScaling = getScaling();

    ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype
    for (part=0; part<graphicssubparts ; part++)
    {
        getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part);
        numtotalphysicsverts+=numtriangles*3; //upper bound

        ///unlike that developers want to pass in double-precision meshes in single-precision Bullet build
        ///so disable this feature by default
        ///see patch http://code.google.com/p/bullet/issues/detail?id=213

        switch (type)
        {
        case PHY_FLOAT:
        {

            float* graphicsbase;

            switch (gfxindextype)
            {
            case PHY_INTEGER:
            {
                for (gfxindex=0; gfxindex<numtriangles; gfxindex++)
                {
                    unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
                    graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
                    triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
                    graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
                    triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
                    graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
                    triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
                    callback->internalProcessTriangleIndex(triangle, part, gfxindex);
                }
                break;
            }
            case PHY_SHORT:
            {
                for (gfxindex=0; gfxindex<numtriangles; gfxindex++)
                {
                    unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
                    graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
                    triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
                    graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
                    triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
                    graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
                    triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
                    callback->internalProcessTriangleIndex(triangle, part, gfxindex);
                }
                break;
            }
            case PHY_UCHAR:
            {
                for (gfxindex=0; gfxindex<numtriangles; gfxindex++)
                {
                    unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride);
                    graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
                    triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
                    graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
                    triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
                    graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
                    triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
                    callback->internalProcessTriangleIndex(triangle, part, gfxindex);
                }
                break;
            }
            default:
                btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
            }
            break;
        }

        case PHY_DOUBLE:
        {
            double* graphicsbase;

            switch (gfxindextype)
            {
            case PHY_INTEGER:
            {
                for (gfxindex=0; gfxindex<numtriangles; gfxindex++)
                {
                    unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
                    graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
                    triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ());
//.........这里部分代码省略.........

void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
{
    btAssert(!m_useSolveConstraintObsolete);
    int i, s = info->rowskip;
    // transforms in world space
    btTransform trA = transA*m_rbAFrame;
    btTransform trB = transB*m_rbBFrame;
    // pivot point
    btVector3 pivotAInW = trA.getOrigin();
    btVector3 pivotBInW = trB.getOrigin();
#if 1
    // difference between frames in WCS
    btVector3 ofs = trB.getOrigin() - trA.getOrigin();
    // now get weight factors depending on masses
    btScalar miA = getRigidBodyA().getInvMass();
    btScalar miB = getRigidBodyB().getInvMass();
    bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
    btScalar miS = miA + miB;
    btScalar factA, factB;
    if(miS > btScalar(0.f))
    {
        factA = miB / miS;
    }
    else
    {
        factA = btScalar(0.5f);
    }
    factB = btScalar(1.0f) - factA;
    // get the desired direction of hinge axis
    // as weighted sum of Z-orthos of frameA and frameB in WCS
    btVector3 ax1A = trA.getBasis().getColumn(2);
    btVector3 ax1B = trB.getBasis().getColumn(2);
    btVector3 ax1 = ax1A * factA + ax1B * factB;
    ax1.normalize();
    // fill first 3 rows
    // we want: velA + wA x relA == velB + wB x relB
    btTransform bodyA_trans = transA;
    btTransform bodyB_trans = transB;
    int s0 = 0;
    int s1 = s;
    int s2 = s * 2;
    int nrow = 2; // last filled row
    btVector3 tmpA, tmpB, relA, relB, p, q;
    // get vector from bodyB to frameB in WCS
    relB = trB.getOrigin() - bodyB_trans.getOrigin();
    // get its projection to hinge axis
    btVector3 projB = ax1 * relB.dot(ax1);
    // get vector directed from bodyB to hinge axis (and orthogonal to it)
    btVector3 orthoB = relB - projB;
    // same for bodyA
    relA = trA.getOrigin() - bodyA_trans.getOrigin();
    btVector3 projA = ax1 * relA.dot(ax1);
    btVector3 orthoA = relA - projA;
    btVector3 totalDist = projA - projB;
    // get offset vectors relA and relB
    relA = orthoA + totalDist * factA;
    relB = orthoB - totalDist * factB;
    // now choose average ortho to hinge axis
    p = orthoB * factA + orthoA * factB;
    btScalar len2 = p.length2();
    if(len2 > SIMD_EPSILON)
    {
        p /= btSqrt(len2);
    }
    else
    {
        p = trA.getBasis().getColumn(1);
    }
    // make one more ortho
    q = ax1.cross(p);
    // fill three rows
    tmpA = relA.cross(p);
    tmpB = relB.cross(p);
    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
    tmpA = relA.cross(q);
    tmpB = relB.cross(q);
    if(hasStaticBody && getSolveLimit())
    {   // to make constraint between static and dynamic objects more rigid
        // remove wA (or wB) from equation if angular limit is hit
        tmpB *= factB;
        tmpA *= factA;
    }
    for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
    tmpA = relA.cross(ax1);
    tmpB = relB.cross(ax1);
    if(hasStaticBody)
    {   // to make constraint between static and dynamic objects more rigid
        // remove wA (or wB) from equation
        tmpB *= factB;
        tmpA *= factA;
    }
    for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
    for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];

    btScalar k = info->fps * info->erp;

    if (!m_angularOnly)
    {
//.........这里部分代码省略.........

void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
		btMultiBodyJacobianData& data,
		const btContactSolverInfo& infoGlobal)
{

//	int i=1;
int numDim = BTMBP2PCONSTRAINT_DIM;
	for (int i=0;i<numDim;i++)
	{

		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
        //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
        constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
        constraintRow.m_contactNormal1.setValue(0,0,0);
        constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
        constraintRow.m_contactNormal2.setValue(0,0,0);
        constraintRow.m_angularComponentA.setValue(0,0,0);
        constraintRow.m_angularComponentB.setValue(0,0,0);

		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;

		btVector3 contactNormalOnB(0,0,0);
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
		contactNormalOnB[i] = -1;
#else
		contactNormalOnB[i%3] = -1;
#endif

		btScalar penetration = 0;

		 // Convert local points back to world
		btVector3 pivotAworld = m_pivotInA;
		if (m_rigidBodyA)
		{

			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
			pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
		} else
		{
			if (m_bodyA)
				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
		}
		btVector3 pivotBworld = m_pivotInB;
		if (m_rigidBodyB)
		{
			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
			pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
		} else
		{
			if (m_bodyB)
				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);

		}

		btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;

#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST


		fillMultiBodyConstraint(constraintRow, data, 0, 0,
															contactNormalOnB, pivotAworld, pivotBworld,						//sucks but let it be this way "for the time being"
															posError,
															infoGlobal,
															-m_maxAppliedImpulse, m_maxAppliedImpulse
															);
    //@todo: support the case of btMultiBody versus btRigidBody,
    //see btPoint2PointConstraint::getInfo2NonVirtual
#else
		const btVector3 dummy(0, 0, 0);

		btAssert(m_bodyA->isMultiDof());

		btScalar* jac1 = jacobianA(i);
		const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
		const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;

		m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);

		fillMultiBodyConstraint(constraintRow, data, jac1, 0,
													dummy, dummy, dummy,						//sucks but let it be this way "for the time being"
													posError,
													infoGlobal,
													-m_maxAppliedImpulse, m_maxAppliedImpulse
													);
#endif
	}
}

void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
{
	btAssert((index >= 0) && (index < 6));
	m_equilibriumPoint[index] = val;
}

//.........这里部分代码省略.........

		do
		{
			
			int src = edge->getSourceVertex();
			tmpFaces[i].m_indices.push_back(src);
			int targ = edge->getTargetVertex();
			btVector3 wa = convexUtil->vertices[src];

			btVector3 wb = convexUtil->vertices[targ];
			btVector3 newEdge = wb-wa;
			newEdge.normalize();
			if (numEdges<2)
				edges[numEdges++] = newEdge;

			edge = edge->getNextEdgeOfFace();
		} while (edge!=firstEdge);

		btScalar planeEq = 1e30f;

		
		if (numEdges==2)
		{
			faceNormals[i] = edges[0].cross(edges[1]);
			faceNormals[i].normalize();
			tmpFaces[i].m_plane[0] = faceNormals[i].getX();
			tmpFaces[i].m_plane[1] = faceNormals[i].getY();
			tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
			tmpFaces[i].m_plane[3] = planeEq;

		}
		else
		{
			btAssert(0);//degenerate?
			faceNormals[i].setZero();
		}

		for (int v=0;v<tmpFaces[i].m_indices.size();v++)
		{
			btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
			if (planeEq>eq)
			{
				planeEq=eq;
			}
		}
		tmpFaces[i].m_plane[3] = -planeEq;
	}

	//merge coplanar faces and copy them to m_polyhedron

	btScalar faceWeldThreshold= 0.999f;
	btAlignedObjectArray<int> todoFaces;
	for (int i=0;i<tmpFaces.size();i++)
		todoFaces.push_back(i);

	while (todoFaces.size())
	{
		btAlignedObjectArray<int> coplanarFaceGroup;
		int refFace = todoFaces[todoFaces.size()-1];

		coplanarFaceGroup.push_back(refFace);
		btFace& faceA = tmpFaces[refFace];
		todoFaces.pop_back();

		btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
		for (int j=todoFaces.size()-1;j>=0;j--)

void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
{
  // compute the centroid of the polygon in cx,cy
  int i,j;
  btScalar a,cx,cy,q;
  if (n==1) {
    cx = p[0];
    cy = p[1];
  }
  else if (n==2) {
    cx = btScalar(0.5)*(p[0] + p[2]);
    cy = btScalar(0.5)*(p[1] + p[3]);
  }
  else {
    a = 0;
    cx = 0;
    cy = 0;
    for (i=0; i<(n-1); i++) {
      q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
      a += q;
      cx += q*(p[i*2]+p[i*2+2]);
      cy += q*(p[i*2+1]+p[i*2+3]);
    }
    q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
	if (btFabs(a+q) > SIMD_EPSILON)
	{
		a = 1.f/(btScalar(3.0)*(a+q));
	} else
	{
		a=BT_LARGE_FLOAT;
	}
    cx = a*(cx + q*(p[n*2-2]+p[0]));
    cy = a*(cy + q*(p[n*2-1]+p[1]));
  }

  // compute the angle of each point w.r.t. the centroid
  btScalar A[8];
  for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);

  // search for points that have angles closest to A[i0] + i*(2*pi/m).
  int avail[8];
  for (i=0; i<n; i++) avail[i] = 1;
  avail[i0] = 0;
  iret[0] = i0;
  iret++;
  for (j=1; j<m; j++) {
    a = btScalar(j)*(2*M__PI/m) + A[i0];
    if (a > M__PI) a -= 2*M__PI;
    btScalar maxdiff=1e9,diff;

    *iret = i0;			// iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0

    for (i=0; i<n; i++) {
      if (avail[i]) {
	diff = btFabs (A[i]-a);
	if (diff > M__PI) diff = 2*M__PI - diff;
	if (diff < maxdiff) {
	  maxdiff = diff;
	  *iret = i;
	}
      }
    }
#if defined(DEBUG) || defined (_DEBUG)
    btAssert (*iret != i0);	// ensure iret got set
#endif
    avail[*iret] = 0;
    iret++;
  }
}