bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) 
{
	const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
	btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
	btScalar boxMargin = boxShape->getMargin();
	penetrationDepth = 1.0f;

	// convert the sphere position to the box's local space
	btTransform const &m44T = boxObjWrap->getWorldTransform();
	btVector3 sphereRelPos = m44T.invXform(sphereCenter);

	// Determine the closest point to the sphere center in the box
	btVector3 closestPoint = sphereRelPos;
	closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
	closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
	closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
	closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
	closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
	closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
	
	btScalar intersectionDist = fRadius + boxMargin;
	btScalar contactDist = intersectionDist + maxContactDistance;
	normal = sphereRelPos - closestPoint;

	//if there is no penetration, we are done
	btScalar dist2 = normal.length2();
	if (dist2 > contactDist * contactDist)
	{
		return false;
	}

	btScalar distance;

	//special case if the sphere center is inside the box
	if (dist2 == 0.0f)
	{
		distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
	}
	else //compute the penetration details
	{
		distance = normal.length();
		normal /= distance;
	}

	pointOnBox = closestPoint + normal * boxMargin;
//	v3PointOnSphere = sphereRelPos - (normal * fRadius);	
	penetrationDepth = distance - intersectionDist;

	// transform back in world space
	btVector3 tmp = m44T(pointOnBox);
	pointOnBox = tmp;
//	tmp = m44T(v3PointOnSphere);
//	v3PointOnSphere = tmp;
	tmp = m44T.getBasis() * normal;
	normal = tmp;

	return true;
}

btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
{

	m_simplexSolver = new btVoronoiSimplexSolver();

	if (constructionInfo.m_useEpaPenetrationAlgorithm)
	{
		m_pdSolver = new btGjkEpaPenetrationDepthSolver;
	}else
	{
		m_pdSolver = new btMinkowskiPenetrationDepthSolver;
	}
	
	//default CreationFunctions, filling the m_doubleDispatch table
	m_convexConvexCreateFunc = new btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
	m_convexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::CreateFunc;
	m_swappedConvexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
	m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc;
	m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc;
	m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc;
	
	m_sphereSphereCF = new btSphereSphereCollisionAlgorithm::CreateFunc;
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
	m_sphereBoxCF = new btSphereBoxCollisionAlgorithm::CreateFunc;
	m_boxSphereCF = new btSphereBoxCollisionAlgorithm::CreateFunc;
	m_boxSphereCF->m_swapped = true;
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM

	m_sphereTriangleCF = new btSphereTriangleCollisionAlgorithm::CreateFunc;
	m_triangleSphereCF = new btSphereTriangleCollisionAlgorithm::CreateFunc;
	m_triangleSphereCF->m_swapped = true;
	
	m_boxBoxCF = new btBoxBoxCollisionAlgorithm::CreateFunc;

	//convex versus plane
	m_convexPlaneCF = new btConvexPlaneCollisionAlgorithm::CreateFunc;
	m_planeConvexCF = new btConvexPlaneCollisionAlgorithm::CreateFunc;
	m_planeConvexCF->m_swapped = true;
	
	///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
	int maxSize = sizeof(btConvexConvexAlgorithm);
	int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
	int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
	int sl = sizeof(btConvexSeparatingDistanceUtil);
	sl = sizeof(btGjkPairDetector);
	int	collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
}

btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
:btDefaultCollisionConfiguration(constructionInfo)
{
	void* mem;

	mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16);
	m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc;

	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
	m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;

	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
	m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
	m_swappedSoftRigidConvexCreateFunc->m_swapped=true;

#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
	mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
	m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;

	mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
	m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
	m_swappedSoftRigidConcaveCreateFunc->m_swapped=true;
#endif

	//replace pool by a new one, with potential larger size

	if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool)
	{
		int curElemSize = m_collisionAlgorithmPool->getElementSize();
		///calculate maximum element size, big enough to fit any collision algorithm in the memory pool


		int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm);
		int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm);
		int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm);

		int	collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1);
		collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
		
		if (collisionAlgorithmMaxElementSize > curElemSize)
		{
			m_collisionAlgorithmPool->~btPoolAllocator();
			btAlignedFree(m_collisionAlgorithmPool);
			void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
			m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
		}
	}

}

void Vehicle::SetBraking(float braking)
{
	braking = btMin(btMax(0.f, braking), 1.f);//clamp
	brakingForce = maxBrakingForce * braking;

	vehicle->setBrake(brakingForce, W_RL);
	vehicle->setBrake(brakingForce, W_RR);
}

static void						getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth)
{
	if(node->isinternal())
	{
		getmaxdepth(node->childs[0],depth+1,maxdepth);
		getmaxdepth(node->childs[1],depth+1,maxdepth);
	} else maxdepth=btMax(maxdepth,depth);
}

void	CLPhysicsDemo::init(int preferredDevice, int preferredPlatform, bool useInterop)
{
	
	InitCL(-1,-1,useInterop);

#define CUSTOM_CL_INITIALIZATION
#ifdef CUSTOM_CL_INITIALIZATION
	g_deviceCL = new adl::DeviceCL();
	g_deviceCL->m_deviceIdx = g_device;
	g_deviceCL->m_context = g_cxMainContext;
	g_deviceCL->m_commandQueue = g_cqCommandQue;
	g_deviceCL->m_kernelManager = new adl::KernelManager;

#else
	DeviceUtils::Config cfg;
	cfg.m_type = DeviceUtils::Config::DEVICE_CPU;
	g_deviceCL = DeviceUtils::allocate( TYPE_CL, cfg );
#endif

	//adl::Solver<adl::TYPE_CL>::allocate(g_deviceCL->allocate(
	m_data->m_linVelBuf = new adl::Buffer<btVector3>(g_deviceCL,MAX_CONVEX_BODIES_CL);
	m_data->m_angVelBuf = new adl::Buffer<btVector3>(g_deviceCL,MAX_CONVEX_BODIES_CL);
	m_data->m_bodyTimes = new adl::Buffer<float>(g_deviceCL,MAX_CONVEX_BODIES_CL);

	m_data->m_localShapeAABB = new adl::Buffer<btAABBHost>(g_deviceCL,MAX_CONVEX_SHAPES_CL);
	
	gLinVelMem = (cl_mem)m_data->m_linVelBuf->m_ptr;
	gAngVelMem = (cl_mem)m_data->m_angVelBuf->m_ptr;
	gBodyTimes = (cl_mem)m_data->m_bodyTimes->m_ptr;

	


	narrowphaseAndSolver = new btGpuNarrowphaseAndSolver(g_deviceCL);

	
	
	int maxObjects = btMax(256,MAX_CONVEX_BODIES_CL);
	int maxPairsSmallProxy = 32;
	btOverlappingPairCache* overlappingPairCache=0;

	m_data->m_Broadphase = new btGridBroadphaseCl(overlappingPairCache,btVector3(4.f, 4.f, 4.f), 128, 128, 128,maxObjects, maxObjects, maxPairsSmallProxy, 100.f, 128,
		g_cxMainContext ,g_device,g_cqCommandQue, g_deviceCL);

	

	cl_program prog = btOpenCLUtils::compileCLProgramFromString(g_cxMainContext,g_device,interopKernelString,0,"",INTEROPKERNEL_SRC_PATH);
	g_integrateTransformsKernel = btOpenCLUtils::compileCLKernelFromString(g_cxMainContext, g_device,interopKernelString, "integrateTransformsKernel" ,0,prog);
	

	initFindPairs(gFpIO, g_cxMainContext, g_device, g_cqCommandQue, MAX_CONVEX_BODIES_CL);

	


}

static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
{
	// It calculates possible 3 area constructed from random 4 points and returns the biggest one.

	btVector3 a[3],b[3];
	a[0] = p0 - p1;
	a[1] = p0 - p2;
	a[2] = p0 - p3;
	b[0] = p2 - p3;
	b[1] = p1 - p3;
	b[2] = p1 - p2;

	//todo: Following 3 cross production can be easily optimized by SIMD.
	btVector3 tmp0 = a[0].cross(b[0]);
	btVector3 tmp1 = a[1].cross(b[1]);
	btVector3 tmp2 = a[2].cross(b[2]);

	return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
}

btScalar CarEngine::Integrate(btScalar clutch_drag, btScalar clutch_angvel, btScalar dt)
{
	btScalar rpm = GetRPM();

	clutch_torque = clutch_drag;

	btScalar torque_limit = shaft.getMomentum(clutch_angvel) / dt;
	if ((clutch_torque > 0 && clutch_torque > torque_limit) ||
		(clutch_torque < 0 && clutch_torque < torque_limit))
	{
		clutch_torque = torque_limit;
	}

	stalled = (rpm < info.stall_rpm);

	//make sure the throttle is at least idling
	btScalar idle_position = info.idle_throttle + info.idle_throttle_slope * (rpm - info.start_rpm);
	if (throttle_position < idle_position)
		throttle_position = idle_position;

	//engine drive torque
	btScalar rev_limit = info.rpm_limit;
	if (rev_limit_exceeded)
		rev_limit -= 100.0; //tweakable
	rev_limit_exceeded = rpm > rev_limit;

	combustion_torque = info.GetTorque(throttle_position, rpm);

	//nitrous injection
	if (nos_mass > 0 && nos_boost_factor > 0)
	{
		btScalar boost = nos_boost_factor * info.nos_boost;
		combustion_torque += boost / shaft.ang_velocity;

		btScalar fuel_consumed = boost * info.fuel_rate * dt;
		btScalar nos_consumed = info.nos_fuel_ratio * fuel_consumed;
		nos_mass = btMax(btScalar(0), nos_mass - nos_consumed);
	}

	if (out_of_gas || rev_limit_exceeded || stalled)
		combustion_torque = 0.0;

	friction_torque = info.GetFrictionTorque(throttle_position, rpm);

	//try to model the static friction of the engine
	if (stalled)
		friction_torque *= 2;

	btScalar total_torque = combustion_torque + friction_torque + clutch_torque;
	shaft.applyMomentum(total_torque * dt);

	return clutch_torque;
}

void	CLPhysicsDemo::init(int preferredDevice, int preferredPlatform, bool useInterop)
{
	
	InitCL(preferredDevice,preferredPlatform,useInterop);


	m_narrowphaseAndSolver = new btGpuNarrowphaseAndSolver(g_cxMainContext,g_device,g_cqCommandQue);
	g_narrowphaseAndSolver = m_narrowphaseAndSolver;

	
	//adl::Solver<adl::TYPE_CL>::allocate(g_deviceCL->allocate(
	m_data->m_linVelBuf = new btOpenCLArray<btVector3>(g_cxMainContext,g_cqCommandQue,MAX_CONVEX_BODIES_CL,false);
	m_data->m_angVelBuf = new btOpenCLArray<btVector3>(g_cxMainContext,g_cqCommandQue,MAX_CONVEX_BODIES_CL,false);
	m_data->m_bodyTimes = new btOpenCLArray<float>(g_cxMainContext,g_cqCommandQue,MAX_CONVEX_BODIES_CL,false);
	
	m_data->m_localShapeAABBGPU = new btOpenCLArray<btAABBHost>(g_cxMainContext,g_cqCommandQue,MAX_CONVEX_SHAPES_CL,false);
	m_data->m_localShapeAABBCPU = new btAlignedObjectArray<btAABBHost>;
	

	

	
	
	
	int maxObjects = btMax(256,MAX_CONVEX_BODIES_CL);
	int maxPairsSmallProxy = 32;
	

	if (useSapGpuBroadphase)
	{
			m_data->m_BroadphaseSap = new btGpuSapBroadphase(g_cxMainContext ,g_device,g_cqCommandQue);//overlappingPairCache,btVector3(4.f, 4.f, 4.f), 128, 128, 128,maxObjects, maxObjects, maxPairsSmallProxy, 100.f, 128,
	} else
	{
		btOverlappingPairCache* overlappingPairCache=0;
		m_data->m_BroadphaseGrid = new btGridBroadphaseCl(overlappingPairCache,btVector3(4.f, 4.f, 4.f), 128, 128, 128,maxObjects, maxObjects, maxPairsSmallProxy, 100.f, 128,
			g_cxMainContext ,g_device,g_cqCommandQue);
	}		//g_cxMainContext ,g_device,g_cqCommandQue);
	

	m_data->m_pgsSolver = new btPgsJacobiSolver();
	

	cl_program prog = btOpenCLUtils::compileCLProgramFromString(g_cxMainContext,g_device,interopKernelString,0,"",INTEROPKERNEL_SRC_PATH);
	g_integrateTransformsKernel = btOpenCLUtils::compileCLKernelFromString(g_cxMainContext, g_device,interopKernelString, "integrateTransformsKernel" ,0,prog);
	g_integrateTransformsKernel2 = btOpenCLUtils::compileCLKernelFromString(g_cxMainContext, g_device,interopKernelString, "integrateTransformsKernel2" ,0,prog);

	initFindPairs(gFpIO, g_cxMainContext, g_device, g_cqCommandQue, MAX_CONVEX_BODIES_CL);

	


}

btScalar Tire::getSqueal() const
{
	btScalar squeal = 0;
	if (vx * vx > btScalar(1E-2) && slip * slip > btScalar(1E-6))
	{
		btScalar vx_body = vx / slip;
		btScalar vx_ideal = ideal_slip * vx_body;
		btScalar vy_ideal = ideal_slip_angle * vx_body; //btTan(ideal_slip_angle) * vx_body;
		btScalar vx_squeal = btFabs(vx / vx_ideal);
		btScalar vy_squeal = btFabs(vy / vy_ideal);
		// start squeal at 80% of the ideal slide/slip, max out at 160%
		squeal = btScalar(1.25) * btMax(vx_squeal, vy_squeal) - 1;
		squeal = Clamp(squeal, btScalar(0), btScalar(1));
	}
	return squeal;
}

btScalar Tire::getSqueal() const
{
	btScalar squeal = 0.0;
	if (vx * vx > 1E-2 && slide * slide > 1E-6)
	{
		btScalar vx_body = vx / slide;
		btScalar vx_ideal = ideal_slide * vx_body;
		btScalar vy_ideal = btTan(-ideal_slip / 180 * M_PI) * vx_body;
		btScalar vx_squeal = btFabs(vx / vx_ideal);
		btScalar vy_squeal = btFabs(vy / vy_ideal);
		// start squeal at 80% of the ideal slide/slip, max out at 160%
		squeal = 1.25 * btMax(vx_squeal, vy_squeal) - 1.0;
		btClamp(squeal, btScalar(0), btScalar(1));
	}
	return squeal;
}

btSoftBody*		btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*	vertices,
													 const int* triangles,
													 int ntriangles, bool randomizeConstraints)
{
	int		maxidx=0;
	int i,j,ni;

	for(i=0,ni=ntriangles*3;i<ni;++i)
	{
		maxidx=btMax(triangles[i],maxidx);
	}
	++maxidx;
	btAlignedObjectArray<bool>		chks;
	btAlignedObjectArray<btVector3>	vtx;
	chks.resize(maxidx*maxidx,false);
	vtx.resize(maxidx);
	for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
	{
		vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
	}
	btSoftBody*		psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
	for( i=0,ni=ntriangles*3;i<ni;i+=3)
	{
		const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
		for(int j=2,k=0;k<3;j=k++)
		{
			if(!chks[IDX(idx[j],idx[k])])
			{
				chks[IDX(idx[j],idx[k])]=true;
				chks[IDX(idx[k],idx[j])]=true;
				psb->appendLink(idx[j],idx[k]);
			}
		}
#undef IDX
		psb->appendFace(idx[0],idx[1],idx[2]);
	}

	if (randomizeConstraints)
	{
		psb->randomizeConstraints();
	}

	return(psb);
}

GL_ShapeDrawer::ShapeCache*		GL_ShapeDrawer::cache(btConvexShape* shape)
{
	ShapeCache*		sc=(ShapeCache*)shape->getUserPointer();
	if(!sc)
	{
		sc=new(btAlignedAlloc(sizeof(ShapeCache),16)) ShapeCache(shape);
		sc->m_shapehull.buildHull(shape->getMargin());
		m_shapecaches.push_back(sc);
		shape->setUserPointer(sc);
		/* Build edges	*/ 
		const int			ni=sc->m_shapehull.numIndices();
		const int			nv=sc->m_shapehull.numVertices();
		const unsigned int*	pi=sc->m_shapehull.getIndexPointer();
		const btVector3*	pv=sc->m_shapehull.getVertexPointer();
		btAlignedObjectArray<ShapeCache::Edge*>	edges;
		sc->m_edges.reserve(ni);
		edges.resize(nv*nv,0);
		for(int i=0;i<ni;i+=3)
		{
			const unsigned int* ti=pi+i;
			const btVector3		nrm=btCross(pv[ti[1]]-pv[ti[0]],pv[ti[2]]-pv[ti[0]]).normalized();
			for(int j=2,k=0;k<3;j=k++)
			{
				const unsigned int	a=ti[j];
				const unsigned int	b=ti[k];
				ShapeCache::Edge*&	e=edges[btMin(a,b)*nv+btMax(a,b)];
				if(!e)
				{
					sc->m_edges.push_back(ShapeCache::Edge());
					e=&sc->m_edges[sc->m_edges.size()-1];
					e->n[0]=nrm;e->n[1]=-nrm;
					e->v[0]=a;e->v[1]=b;
				}
				else
				{
					e->n[1]=nrm;
				}
			}
		}
	}
	return(sc);
}

static void debugDrawPhase(const btBatchedConstraints* bc,
						   btConstraintArray* constraints,
						   const btAlignedObjectArray<btSolverBody>& bodies,
						   int iPhase,
						   const btVector3& color0,
						   const btVector3& color1,
						   const btVector3& offset)
{
	BT_PROFILE("debugDrawPhase");
	if (bc && bc->m_debugDrawer && iPhase < bc->m_phases.size())
	{
		const btBatchedConstraints::Range& phase = bc->m_phases[iPhase];
		for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
		{
			float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1));
			btVector3 col = lerp(color0, color1, tt);
			debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset);
		}
	}
}

//.........这里部分代码省略.........
						newUnclampedAccImpulse.normalize();
						newUnclampedAccImpulse *= fMaxImpulse;
						impulse = newUnclampedAccImpulse - m_accMotorImpulse;
					}
					m_accMotorImpulse += impulse;
				}

				btScalar  impulseMag  = impulse.length();
				btVector3 impulseAxis =  impulse / impulseMag;

				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);

			}
		}
		else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
		{
			btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
			btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
			btVector3 relVel = angVelB - angVelA;
			if (relVel.length2() > SIMD_EPSILON)
			{
				btVector3 relVelAxis = relVel.normalized();
				btScalar m_kDamping =  btScalar(1.) /
					(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
					 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
				btVector3 impulse = m_damping * m_kDamping * relVel;

				btScalar  impulseMag  = impulse.length();
				btVector3 impulseAxis = impulse / impulseMag;
				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
			}
		}

		// joint limits
		{
			///solve angular part
			btVector3 angVelA;
			bodyA.internalGetAngularVelocity(angVelA);
			btVector3 angVelB;
			bodyB.internalGetAngularVelocity(angVelB);

			// solve swing limit
			if (m_solveSwingLimit)
			{
				btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
				btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
				if (relSwingVel > 0)
					amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
				btScalar impulseMag = amplitude * m_kSwing;

				// Clamp the accumulated impulse
				btScalar temp = m_accSwingLimitImpulse;
				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
				impulseMag = m_accSwingLimitImpulse - temp;

				btVector3 impulse = m_swingAxis * impulseMag;

				// don't let cone response affect twist
				// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
				{
					btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
					btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
					impulse = impulseNoTwistCouple;
				}

				impulseMag = impulse.length();
				btVector3 noTwistSwingAxis = impulse / impulseMag;

				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
			}


			// solve twist limit
			if (m_solveTwistLimit)
			{
				btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
				btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
				if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
					amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
				btScalar impulseMag = amplitude * m_kTwist;

				// Clamp the accumulated impulse
				btScalar temp = m_accTwistLimitImpulse;
				m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
				impulseMag = m_accTwistLimitImpulse - temp;

		//		btVector3 impulse = m_twistAxis * impulseMag;

				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
			}		
		}
	}
#else
btAssert(0);
#endif //__SPU__
}

btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
{

    void* mem = NULL;
	if (constructionInfo.m_useEpaPenetrationAlgorithm)
	{
		mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
		m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
	}else
	{
		mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
		m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
	}
	
	//default CreationFunctions, filling the m_doubleDispatch table
	mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
	m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver);
	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
	m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc;
	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
	m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16);
	m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc;

	mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16);
	m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc;

	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16);
	m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
	mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
	m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc;
	
	mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16);
	m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc;
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
	m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc;
	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
	m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc;
	m_boxSphereCF->m_swapped = true;
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM

	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
	m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
	m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
	m_triangleSphereCF->m_swapped = true;
	
	mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16);
	m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc;

	//convex versus plane
	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
	m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
	m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
	m_planeConvexCF->m_swapped = true;
	
	///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
	int maxSize = sizeof(btConvexConvexAlgorithm);
	int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
	int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
	int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm);

	int	collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
		
	if (constructionInfo.m_persistentManifoldPool)
	{
		m_ownsPersistentManifoldPool = false;
		m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool;
	} else
	{
		m_ownsPersistentManifoldPool = true;
		void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
		m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
	}
	
	collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0;
	if (constructionInfo.m_collisionAlgorithmPool)
	{
		m_ownsCollisionAlgorithmPool = false;
		m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool;
	} else
	{
		m_ownsCollisionAlgorithmPool = true;
		void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
		m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
	}


}

void	btHingeConstraint::solveConstraint(btScalar	timeStep)
{

	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();

	btScalar tau = btScalar(0.3);

	//linear part
	if (!m_angularOnly)
	{
		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();

		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
		btVector3 vel = vel1 - vel2;

		for (int i=0;i<3;i++)
		{		
			const btVector3& normal = m_jac[i].m_linearJointAxis;
			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();

			btScalar rel_vel;
			rel_vel = normal.dot(vel);
			//positional error (zeroth order error)
			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
			m_appliedImpulse += impulse;
			btVector3 impulse_vector = normal * impulse;
			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
		}
	}

	
	{
		///solve angular part

		// get axes in world space
		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
		btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);

		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();

		btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
		btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);

		btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
		btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
		btVector3 velrelOrthog = angAorthog-angBorthog;
		{
			//solve orthogonal angular velocity correction
			btScalar relaxation = btScalar(1.);
			btScalar len = velrelOrthog.length();
			if (len > btScalar(0.00001))
			{
				btVector3 normal = velrelOrthog.normalized();
				btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
					getRigidBodyB().computeAngularImpulseDenominator(normal);
				// scale for mass and relaxation
				velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
			}

			//solve angular positional correction
			btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
			btScalar len2 = angularError.length();
			if (len2>btScalar(0.00001))
			{
				btVector3 normal2 = angularError.normalized();
				btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
						getRigidBodyB().computeAngularImpulseDenominator(normal2);
				angularError *= (btScalar(1.)/denom2) * relaxation;
			}

			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);

			// solve limit
			if (m_solveLimit)
			{
				btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;

				btScalar impulseMag = amplitude * m_kHinge;

				// Clamp the accumulated impulse
				btScalar temp = m_accLimitImpulse;
				m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
				impulseMag = m_accLimitImpulse - temp;


				btVector3 impulse = axisA * impulseMag * m_limitSign;
				m_rbA.applyTorqueImpulse(impulse);
				m_rbB.applyTorqueImpulse(-impulse);
			}
		}

		//apply motor
		if (m_enableAngularMotor) 
//.........这里部分代码省略.........

void	btConeTwistConstraint::solveConstraint(btScalar	timeStep)
{

	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();

	btScalar tau = btScalar(0.3);
	btScalar damping = btScalar(1.);

	//linear part
	if (!m_angularOnly)
	{
		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();

		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
		btVector3 vel = vel1 - vel2;

		for (int i=0;i<3;i++)
		{		
			const btVector3& normal = m_jac[i].m_linearJointAxis;
			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();

			btScalar rel_vel;
			rel_vel = normal.dot(vel);
			//positional error (zeroth order error)
			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
			m_appliedImpulse += impulse;
			btVector3 impulse_vector = normal * impulse;
			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
		}
	}
	
	{
		///solve angular part
		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();

		// solve swing limit
		if (m_solveSwingLimit)
		{
			btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
			btScalar impulseMag = amplitude * m_kSwing;

			// Clamp the accumulated impulse
			btScalar temp = m_accSwingLimitImpulse;
			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
			impulseMag = m_accSwingLimitImpulse - temp;

			btVector3 impulse = m_swingAxis * impulseMag;

			m_rbA.applyTorqueImpulse(impulse);
			m_rbB.applyTorqueImpulse(-impulse);

		}

		// solve twist limit
		if (m_solveTwistLimit)
		{
			btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
			btScalar impulseMag = amplitude * m_kTwist;

			// Clamp the accumulated impulse
			btScalar temp = m_accTwistLimitImpulse;
			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
			impulseMag = m_accTwistLimitImpulse - temp;

			btVector3 impulse = m_twistAxis * impulseMag;

			m_rbA.applyTorqueImpulse(impulse);
			m_rbB.applyTorqueImpulse(-impulse);

		}
	
	}

}

void Vehicle::init(
	const VehicleInfo & info,
	const btVector3 & position,
	const btQuaternion & rotation,
	World & world)
{
	transform.setRotation(rotation);
	transform.setOrigin(position);

	body = new FractureBody(info.body);
	body->setCenterOfMassTransform(transform);
	body->setActivationState(DISABLE_DEACTIVATION);
	body->setContactProcessingThreshold(0.0);
	body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
	world.addRigidBody(body);
	world.addAction(this);
	this->world = &world;

	aero_device.resize(info.aerodevice.size());
	for (int i = 0; i < info.aerodevice.size(); ++i)
	{
		aero_device[i] = AeroDevice(info.aerodevice[i]);
	}

	antiroll.resize(info.antiroll.size());
	for (int i = 0; i < info.antiroll.size(); ++i)
	{
		antiroll[i] = info.antiroll[i];
	}

	differential.resize(info.differential.size());
	wheel.resize(info.wheel.size());
	wheel_contact.resize(wheel.size());
	diff_joint.resize(differential.size());
	clutch_joint.resize(differential.size() + 1);
	motor_joint.resize(wheel.size() * 2 + 1);

	for (int i = 0; i < wheel.size(); ++i)
	{
		wheel[i].init(info.wheel[i], world, *body);
		maxangle = btMax(maxangle, wheel[i].suspension.getMaxSteeringAngle());
	}

	for (int i = 0; i < differential.size(); ++i)
	{
		Shaft * shaft_a = LinkShaft(info.differential_link_a[i], wheel, differential);
		Shaft * shaft_b = LinkShaft(info.differential_link_b[i], wheel, differential);
		differential[i].init(info.differential[i], *shaft_a, *shaft_b);
	}

	Shaft * shaft_t = LinkShaft(info.transmission_link, wheel, differential);
	transmission.init(info.transmission, *shaft_t);
	clutch.init(info.clutch);
	engine.init(info.engine);

	// position is the center of a 2 x 4 x 1 meter box on track surface
	// move car to fit bounding box front lower edge of the position box
	btVector3 bmin, bmax;
	body->getCollisionShape()->getAabb(btTransform::getIdentity(), bmin, bmax);
	btVector3 fwd = body->getCenterOfMassTransform().getBasis().getColumn(1);
	btVector3 up = body->getCenterOfMassTransform().getBasis().getColumn(2);
	btVector3 fwd_offset = fwd * (2.0 - bmax.y());
	btVector3 up_offset = -up * (0.5 + bmin.z());
	setPosition(body->getCenterOfMassPosition() + up_offset + fwd_offset);
	//alignWithGround();

	// init constants
	calculateFrictionCoefficient(lon_friction_coeff, lat_friction_coeff);
	maxspeed = calculateMaxSpeed();
	width = (bmax - bmin).dot(direction::right);
}

void ObjectMotionState::setRestitution(float restitution) {
    _restitution = btMax(btMin(fabsf(restitution), 1.0f), 0.0f);
}