C++ btScalar函数代码示例

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 normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
	btScalar k = info->fps * normalErp;

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

int btGeneric6DofConstraint::get_limit_motor_info2(
    btRotationalLimitMotor * limot,
    btRigidBody * body0, btRigidBody * body1,
    btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
{
    int srow = row * info->rowskip;
    int powered = limot->m_enableMotor;
    int limit = limot->m_currentLimit;
    if (powered || limit)
    {   // if the joint is powered, or has joint limits, add in the extra row
        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
        btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
        J1[srow+0] = ax1[0];
        J1[srow+1] = ax1[1];
        J1[srow+2] = ax1[2];
        if(rotational)
        {
            J2[srow+0] = -ax1[0];
            J2[srow+1] = -ax1[1];
            J2[srow+2] = -ax1[2];
        }
        if((!rotational))
        {
            btVector3 ltd;    // Linear Torque Decoupling vector
            btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
            ltd = c.cross(ax1);
            info->m_J1angularAxis[srow+0] = ltd[0];
            info->m_J1angularAxis[srow+1] = ltd[1];
            info->m_J1angularAxis[srow+2] = ltd[2];

            c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
            ltd = -c.cross(ax1);
            info->m_J2angularAxis[srow+0] = ltd[0];
            info->m_J2angularAxis[srow+1] = ltd[1];
            info->m_J2angularAxis[srow+2] = ltd[2];
        }
        // if we're limited low and high simultaneously, the joint motor is
        // ineffective
        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
        info->m_constraintError[srow] = btScalar(0.f);
        if (powered)
        {
            info->cfm[srow] = 0.0f;
            if(!limit)
            {
                btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;

                btScalar mot_fact = getMotorFactor(    limot->m_currentPosition,
                                                    limot->m_loLimit,
                                                    limot->m_hiLimit,
                                                    tag_vel,
                                                    info->fps * info->erp);
                info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
                info->m_upperLimit[srow] = limot->m_maxMotorForce;
            }
        }
        if(limit)
        {
            btScalar k = info->fps * limot->m_ERP;
            if(!rotational)
            {
                info->m_constraintError[srow] += k * limot->m_currentLimitError;
            }
            else
            {
                info->m_constraintError[srow] += -k * limot->m_currentLimitError;
            }
            info->cfm[srow] = 0.0f;
            if (limot->m_loLimit == limot->m_hiLimit)
            {   // limited low and high simultaneously
                info->m_lowerLimit[srow] = -SIMD_INFINITY;
                info->m_upperLimit[srow] = SIMD_INFINITY;
            }
            else
            {
                if (limit == 1)
                {
                    info->m_lowerLimit[srow] = 0;
                    info->m_upperLimit[srow] = SIMD_INFINITY;
                }
                else
                {
                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
                    info->m_upperLimit[srow] = 0;
                }
                // deal with bounce
                if (limot->m_bounce > 0)
                {
                    // calculate joint velocity
                    btScalar vel;
                    if (rotational)
                    {
                        vel = body0->getAngularVelocity().dot(ax1);
                        if (body1)
                            vel -= body1->getAngularVelocity().dot(ax1);
                    }
                    else
                    {
                        vel = body0->getLinearVelocity().dot(ax1);
//.........这里部分代码省略.........

void	btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const
{
	//moving concave objects not supported
	
	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}

btRigidBody* Physics::addRigidSphere(Ogre::Entity* entity, Ogre::SceneNode* node,
                                     btScalar mass, btScalar rest, btVector3 localInertia, btVector3 origin, btQuaternion *rotation) {
    Ogre::Vector3 s = entity->getBoundingBox().getHalfSize();
    btCollisionShape *sphereShape = new btSphereShape( btScalar(s[0]) );
    addRigidBody(entity, node, sphereShape, mass, rest, localInertia, origin, rotation);
};

	SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans)
		: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-1e30))
		
	{
		m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis();
	}

bool Skeleton_Builder::BuildTail(float tail_width, float tail_height, float torso_height)
{
	Bone* UTail = new Bone();
	Bone* MTail = new Bone();
	Bone* LTail = new Bone();
	Bone* LLTail = new Bone();
	tail_height /= 4;
	
	Ogre::Quaternion neckrotation;
	neckrotation.FromAngleAxis(Ogre::Radian(Ogre::Degree(mTailIncline)), Ogre::Vector3::UNIT_X);
	btTransform t;
	btVector3 min, max;

	mHipNode->GetCollider()->getAabb(t, min, max);
	mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2);
	mBuilder->SetRelativePosition(Ogre::Vector3(0, -(torso_height /2), mDepth / 2), neckrotation, *mHipNode->GetNode());
	if (!mBuilder->BuildBone(*LLTail, Bone::BoneType::Tail))
		return false;
	mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2);
	mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height , 0), neckrotation, *LLTail->GetNode());
	if (!mBuilder->BuildBone(*LTail, Bone::BoneType::Tail))
		return false;
	mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2);
	mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height, 0), neckrotation, *LTail->GetNode());
	if (!mBuilder->BuildBone(*MTail, Bone::BoneType::Tail))
		return false;
	mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2);
	mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height, 0), neckrotation, *MTail->GetNode());
	if (!mBuilder->BuildBone(*UTail, Bone::BoneType::Tail))
		return false;
	if (mHasMuscle)
	{
		mMuscleBuilder->CreateMuscle(mHipNode, LLTail, mMuscles);
		mMuscleBuilder->CreateMuscle(LLTail, LTail, mMuscles);
		mMuscleBuilder->CreateMuscle(LTail, MTail, mMuscles);
		mMuscleBuilder->CreateMuscle(MTail, UTail, mMuscles);
	}

	//add joints
	btTransform localA, localB;
	btVector3 JointPos;

	JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (torso_height / 2), 0));
	SetJointTransform(localA, localB, JointPos, mHipNode, LLTail, btVector3(0, btScalar(-M_PI_2), 0));
	btConeTwistConstraint* shoulderNeckConst = new btConeTwistConstraint(*mHipNode->GetRigidBody(), *LLTail->GetRigidBody(), localA, localB);
	shoulderNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4),btScalar(M_PI_4 / 4));

	JointPos = Utils::OgreBTVector(LLTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0));
	SetJointTransform(localA, localB, JointPos, LLTail, LTail, btVector3(0, btScalar(-M_PI_2), 0));
	btConeTwistConstraint* LNeckConst = new btConeTwistConstraint(*LLTail->GetRigidBody(), *LTail->GetRigidBody(), localA, localB);
	LNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));

	JointPos = Utils::OgreBTVector(LTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0));
	SetJointTransform(localA, localB, JointPos, LTail, MTail, btVector3(0, btScalar(-M_PI_2), 0));
	btConeTwistConstraint* MNeckConst = new btConeTwistConstraint(*LTail->GetRigidBody(), *MTail->GetRigidBody(), localA, localB);
	MNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));

	JointPos = Utils::OgreBTVector(MTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0));
	SetJointTransform(localA, localB, JointPos, MTail, UTail, btVector3(0, btScalar(-M_PI_2), 0));
	btConeTwistConstraint* HNeckConst = new btConeTwistConstraint(*MTail->GetRigidBody(), *UTail->GetRigidBody(), localA, localB);
	HNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));

	mConstraints.push_back(shoulderNeckConst);
	mConstraints.push_back(LNeckConst);
	mConstraints.push_back(MNeckConst);
	mConstraints.push_back(HNeckConst);

	return true;
}

bool	btGjkConvexCast::calcTimeOfImpact(
					const btTransform& fromA,
					const btTransform& toA,
					const btTransform& fromB,
					const btTransform& toB,
					CastResult& result)
{


	m_simplexSolver->reset();

	/// compute linear velocity for this interval, to interpolate
	//assume no rotation/angular velocity, assert here?
	btVector3 linVelA, linVelB;
	linVelA = toA.getOrigin()-fromA.getOrigin();
	linVelB = toB.getOrigin()-fromB.getOrigin();

	btScalar radius = btScalar(0.001);
	btScalar lambda = btScalar(0.);
	btVector3 v(1,0,0);

	int maxIter = MAX_ITERATIONS;

	btVector3 n;
	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
	bool hasResult = false;
	btVector3 c;
	btVector3 r = (linVelA-linVelB);

	btScalar lastLambda = lambda;
	//btScalar epsilon = btScalar(0.001);

	int numIter = 0;
	//first solution, using GJK


	btTransform identityTrans;
	identityTrans.setIdentity();


//	result.drawCoordSystem(sphereTr);

	btPointCollector	pointCollector;

		
	btGjkPairDetector gjk(m_convexA, m_convexB, m_simplexSolver,0);//m_penetrationDepthSolver);		
	btGjkPairDetector::ClosestPointInput input;

	//we don't use margins during CCD
	//	gjk.setIgnoreMargin(true);

	input.m_transformA = fromA;
	input.m_transformB = fromB;
	gjk.getClosestPoints(input, pointCollector,0);

	hasResult = pointCollector.m_hasResult;
	c = pointCollector.m_pointInWorld;

	if (hasResult)
	{
		btScalar dist;
		dist = pointCollector.m_distance;
		n = pointCollector.m_normalOnBInWorld;

	

		//not close enough
		while (dist > radius)
		{
			numIter++;
			if (numIter > maxIter)
			{
				return false; //todo: report a failure
			}
			btScalar dLambda = btScalar(0.);

			btScalar projectedLinearVelocity = r.dot(n);
			
			dLambda = dist / (projectedLinearVelocity);

			lambda = lambda - dLambda;

			if (lambda > btScalar(1.))
				return false;

			if (lambda < btScalar(0.))
				return false;

			//todo: next check with relative epsilon
			if (lambda <= lastLambda)
			{
				return false;
				//n.setValue(0,0,0);
				break;
			}
			lastLambda = lambda;

			//interpolate to next lambda
			result.DebugDraw( lambda );
			input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda);
//.........这里部分代码省略.........

void	SerializeDemo::initPhysics()
{
	setTexturing(true);
	setShadows(true);

	setCameraDistance(btScalar(SCALING*50.));

	setupEmptyDynamicsWorld();
	
#ifdef DESERIALIZE_SOFT_BODIES
	m_fileLoader = new MySoftBulletWorldImporter((btSoftRigidDynamicsWorld*)m_dynamicsWorld);
#else
	m_fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
#endif //DESERIALIZE_SOFT_BODIES
//	fileLoader->setVerboseMode(true);

	if (!m_fileLoader->loadFile("testFile.bullet"))
//	if (!m_fileLoader->loadFile("../SoftDemo/testFile.bullet"))
	{
		///create a few basic rigid bodies and save them to testFile.bullet
		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
	//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
		btCollisionObject* groundObject = 0;

		
		m_collisionShapes.push_back(groundShape);

		btTransform groundTransform;
		groundTransform.setIdentity();
		groundTransform.setOrigin(btVector3(0,-50,0));

		//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);

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


		{
			//create a few dynamic rigidbodies
			// Re-using the same collision is better for memory usage and performance

			int numSpheres = 2;
			btVector3 positions[2] = {btVector3(0.1f,0.2f,0.3f),btVector3(0.4f,0.5f,0.6f)};
			btScalar	radii[2] = {0.3f,0.4f};

			btMultiSphereShape* colShape = new btMultiSphereShape(positions,radii,numSpheres);

			//btCollisionShape* colShape = new btCapsuleShapeZ(SCALING*1,SCALING*1);
			//btCollisionShape* colShape = new btCylinderShapeZ(btVector3(SCALING*1,SCALING*1,SCALING*1));
			//btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
			//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
			m_collisionShapes.push_back(colShape);

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

			btScalar	mass(1.f);

			//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)
				colShape->calculateLocalInertia(mass,localInertia);

			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;

			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(20+2.0*k + start_y),
											btScalar(2.0*j + start_z)));

				
						//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
						btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
//.........这里部分代码省略.........

void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
{
    //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
    if (trimeshShape->getTriangleInfoMap())
        return;

    trimeshShape->setTriangleInfoMap(triangleInfoMap);

    btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
    const btVector3& meshScaling = meshInterface->getScaling();

    for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
    {
        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;
        //PHY_ScalarType indexType=0;

        btVector3 triangleVerts[3];
        meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
        btVector3 aabbMin,aabbMax;

        for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
        {
            unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*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(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
            aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
            aabbMin.setMin(triangleVerts[0]);
            aabbMax.setMax(triangleVerts[0]);
            aabbMin.setMin(triangleVerts[1]);
            aabbMax.setMax(triangleVerts[1]);
            aabbMin.setMin(triangleVerts[2]);
            aabbMax.setMax(triangleVerts[2]);

            btConnectivityProcessor connectivityProcessor;
            connectivityProcessor.m_partIdA = partId;
            connectivityProcessor.m_triangleIndexA = triangleIndex;
            connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
            connectivityProcessor.m_triangleInfoMap  = triangleInfoMap;

            trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
        }

    }

}

/// Changes a btManifoldPoint collision normal to the normal from the mesh.
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
{
    //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
    if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
        return;

    btBvhTriangleMeshShape* trimesh = 0;

    if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
       trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
   else
       trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();

    btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
    if (!triangleInfoMapPtr)
        return;

    int hash = btGetHash(partId0,index0);


    btTriangleInfo* info = triangleInfoMapPtr->find(hash);
    if (!info)
        return;

    btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;

    const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
    btVector3 v0,v1,v2;
    tri_shape->getVertex(0,v0);
    tri_shape->getVertex(1,v1);
    tri_shape->getVertex(2,v2);

    //btVector3 center = (v0+v1+v2)*btScalar(1./3.);

    btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
    btVector3 tri_normal;
    tri_shape->calcNormal(tri_normal);

    //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
    btVector3 nearest;
    btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);

    btVector3 contact = cp.m_localPointB;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
    const btTransform& tr = colObj0->getWorldTransform();
    btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



    bool isNearEdge = false;

    int numConcaveEdgeHits = 0;
    int numConvexEdgeHits = 0;

    btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
    localContactNormalOnB.normalize();//is this necessary?

    // Get closest edge
    int      bestedge=-1;
    btScalar    disttobestedge=BT_LARGE_FLOAT;
    //
    // Edge 0 -> 1
    if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
    {
       btVector3 nearest;
       btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
       btScalar     len=(contact-nearest).length();
       //
       if( len < disttobestedge )
       {
          bestedge=0;
          disttobestedge=len;
      }
   }
    // Edge 1 -> 2
    if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
    {
       btVector3 nearest;
       btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
       btScalar     len=(contact-nearest).length();
       //
       if( len < disttobestedge )
       {
          bestedge=1;
          disttobestedge=len;
      }
   }
    // Edge 2 -> 0
    if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
    {
       btVector3 nearest;
       btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
       btScalar     len=(contact-nearest).length();
       //
       if( len < disttobestedge )
       {
          bestedge=2;
          disttobestedge=len;
//.........这里部分代码省略.........

void	BasicDemo::initPhysics()
{
    setTexturing(true);
    setShadows(true);

    setCameraDistance(btScalar(SCALING*50.));

    ///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->setGravity(btVector3(0,-10,0));

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

    m_collisionShapes.push_back(groundShape);

    btTransform groundTransform;
    groundTransform.setIdentity();
    groundTransform.setOrigin(btVector3(0,-50,0));

    //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);

        //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

        btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
        //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
        m_collisionShapes.push_back(colShape);

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

        btScalar	mass(1.f);

        //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)
            colShape->calculateLocalInertia(mass,localInertia);

        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;

        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(
                                                 2.0*i + start_x,
                                                 20+2.0*k + start_y,
                                                 2.0*j + start_z));


                    //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->setContactProcessingThreshold(colShape->getContactBreakingThreshold());
                    body->setActivationState(ISLAND_SLEEPING);

                    m_dynamicsWorld->addRigidBody(body);
//.........这里部分代码省略.........

bool	btSubsimplexConvexCast::calcTimeOfImpact(
		const btTransform& fromA,
		const btTransform& toA,
		const btTransform& fromB,
		const btTransform& toB,
		CastResult& result)
{

	m_simplexSolver->reset();

	btVector3 linVelA,linVelB;
	linVelA = toA.getOrigin()-fromA.getOrigin();
	linVelB = toB.getOrigin()-fromB.getOrigin();

	btScalar lambda = btScalar(0.);

	btTransform interpolatedTransA = fromA;
	btTransform interpolatedTransB = fromB;

	///take relative motion
	btVector3 r = (linVelA-linVelB);
	btVector3 v;
	
	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
	v = supVertexA-supVertexB;
	int maxIter = sscc_MAX_ITERATIONS;

	btVector3 n;
	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
	bool hasResult = false;
	btVector3 c;

	btScalar lastLambda = lambda;


	btScalar dist2 = v.length2();
#ifdef BT_USE_DOUBLE_PRECISION
	btScalar epsilon = btScalar(0.0001);
#else
	btScalar epsilon = btScalar(0.0001);
#endif //BT_USE_DOUBLE_PRECISION
	btVector3	w,p;
	btScalar VdotR;
	
	while ( (dist2 > epsilon) && maxIter--)
	{
		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
		w = supVertexA-supVertexB;

		btScalar VdotW = v.dot(w);

		if (lambda > btScalar(1.0))
		{
			return false;
		}

		if ( VdotW > btScalar(0.))
		{
			VdotR = v.dot(r);

			if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
				return false;
			else
			{
				lambda = lambda - VdotW / VdotR;
				//interpolate to next lambda
				//	x = s + lambda * r;
				interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
				//m_simplexSolver->reset();
				//check next line
				 w = supVertexA-supVertexB;
				lastLambda = lambda;
				n = v;
				hasResult = true;
			}
		} 
		///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
		if (!m_simplexSolver->inSimplex(w))
			m_simplexSolver->addVertex( w, supVertexA , supVertexB);

		if (m_simplexSolver->closest(v))
		{
			dist2 = v.length2();
			hasResult = true;
			//todo: check this normal for validity
			//n=v;
			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
			//printf("DIST2=%f\n",dist2);
			//printf("numverts = %i\n",m_simplexSolver->numVertices());
		} else
		{
			dist2 = btScalar(0.);
		} 
	}

	//int numiter = sscc_MAX_ITERATIONS - maxIter;
//	printf("number of iterations: %d", numiter);
//.........这里部分代码省略.........

	bool	m_useWalkDirection;
	btScalar	m_velocityTimeInterval;
	int m_upAxis;

	static btVector3* getUpAxisDirections();
	bool  m_interpolateUp;
	bool  full_drop;
	bool  bounce_fix;

	btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal);
	btVector3 parallelComponent(const btVector3& direction, const btVector3& normal);
	btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal);

	bool recoverFromPenetration(btCollisionWorld* collisionWorld);
	void stepUp(btCollisionWorld* collisionWorld);
	void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
	void stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove);
	void stepDown(btCollisionWorld* collisionWorld, btScalar dt);
public:

	BT_DECLARE_ALIGNED_ALLOCATOR();

	KinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, int upAxis = 1);
	~KinematicCharacterController();


	///btActionInterface interface
	virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime)
	{
		preStep(collisionWorld);
		playerStep(collisionWorld, deltaTime);

bool	btBulletWorldImporter::loadFileFromMemory(  bParse::btBulletFile* bulletFile2)
{
	

	bool ok = (bulletFile2->getFlags()& bParse::FD_OK)!=0;
	
	if (ok)
		bulletFile2->parse(m_verboseDumpAllTypes);
	else 
		return false;
	
	if (m_verboseDumpAllTypes)
	{
		bulletFile2->dumpChunks(bulletFile2->getFileDNA());
	}

	int i;
	btHashMap<btHashPtr,btCollisionShape*>	shapeMap;

	for (i=0;i<bulletFile2->m_collisionShapes.size();i++)
	{
		btCollisionShapeData* shapeData = (btCollisionShapeData*)bulletFile2->m_collisionShapes[i];
		btCollisionShape* shape = convertCollisionShape(shapeData);
		if (shape)
			shapeMap.insert(shapeData,shape);
	}

	btHashMap<btHashPtr,btCollisionObject*>	bodyMap;


	for (i=0;i<bulletFile2->m_rigidBodies.size();i++)
	{
		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
		{
			btRigidBodyDoubleData* colObjData = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i];
			btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f);
			btVector3 localInertia;
			localInertia.setZero();
			btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape);
			if (shapePtr && *shapePtr)
			{
				btTransform startTransform;
				startTransform.deSerializeDouble(colObjData->m_collisionObjectData.m_worldTransform);
			//	startTransform.setBasis(btMatrix3x3::getIdentity());
				btCollisionShape* shape = (btCollisionShape*)*shapePtr;
				if (mass)
				{
					shape->calculateLocalInertia(mass,localInertia);
				}
				bool isDynamic = mass!=0.f;
				btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape);
				bodyMap.insert(colObjData,body);
			} else
			{
				printf("error: no shape found\n");
			}
	
		} else
		{
			btRigidBodyFloatData* colObjData = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i];
			btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f);
			btVector3 localInertia;
			localInertia.setZero();
			btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape);
			if (shapePtr && *shapePtr)
			{
				btTransform startTransform;
				startTransform.deSerializeFloat(colObjData->m_collisionObjectData.m_worldTransform);
			//	startTransform.setBasis(btMatrix3x3::getIdentity());
				btCollisionShape* shape = (btCollisionShape*)*shapePtr;
				if (mass)
				{
					shape->calculateLocalInertia(mass,localInertia);
				}
				bool isDynamic = mass!=0.f;
				btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape);
				bodyMap.insert(colObjData,body);
			} else
			{
				printf("error: no shape found\n");
			}
		}
	}

	for (i=0;i<bulletFile2->m_collisionObjects.size();i++)
	{
		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
		{
			btCollisionObjectDoubleData* colObjData = (btCollisionObjectDoubleData*)bulletFile2->m_collisionObjects[i];
			btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionShape);
			if (shapePtr && *shapePtr)
			{
				btTransform startTransform;
				startTransform.deSerializeDouble(colObjData->m_worldTransform);
				btCollisionShape* shape = (btCollisionShape*)*shapePtr;
				btCollisionObject* body = createCollisionObject(startTransform,shape);
				bodyMap.insert(colObjData,body);
			} else
			{
				printf("error: no shape found\n");
//.........这里部分代码省略.........

btRigidBody& btBulletWorldImporter::getFixedBody()
{
	static btRigidBody s_fixed(0, 0,0);
	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
	return s_fixed;
}