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

}
Ejemplo n.º 4
0
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);
}
Ejemplo n.º 5
0
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);
}
Ejemplo n.º 6
0
void Vehicle::SetThrottle(float throttle)
{
	throttle = btMin(btMax(-1.f, throttle), 1.f);//clamp
	engineForce = maxEngineForce * throttle;

	vehicle->applyEngineForce(engineForce, W_RL);
	vehicle->applyEngineForce(engineForce, W_RR);
}
Ejemplo n.º 7
0
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);

	


}
Ejemplo n.º 8
0
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());
}
Ejemplo n.º 9
0
void Vehicle::Update(float dt)
{
	const static float delta = 0.01f;
	//update steering
	float dif = targetSteering - steering;

	if(dif > delta)
	{
		steering += steeringIncrement*dt;
		steering = btMin(btMax(-1.f, steering), 1.f);//clamp
	}
	else if(dif < -delta)
	{
		steering -= steeringIncrement*dt;
		steering = btMin(btMax(-1.f, steering), 1.f);//clamp
	}

	vehicle->setSteeringValue(steering, W_FL);
	vehicle->setSteeringValue(steering, W_FR);
}
Ejemplo n.º 10
0
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;
}
Ejemplo n.º 11
0
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);

	


}
Ejemplo n.º 12
0
btHfFluidRigidCollisionConfiguration::btHfFluidRigidCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
:btDefaultCollisionConfiguration(constructionInfo)
{
	void* mem;

	mem = btAlignedAlloc(sizeof(btHfFluidRigidCollisionAlgorithm::CreateFunc),16);
	m_hfFluidRigidConvexCreateFunc = new(mem) btHfFluidRigidCollisionAlgorithm::CreateFunc;

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

	mem = btAlignedAlloc(sizeof(btHfFluidBuoyantShapeCollisionAlgorithm::CreateFunc),16);
	m_hfFluidBuoyantShapeCollisionCreateFunc = new(mem) btHfFluidBuoyantShapeCollisionAlgorithm::CreateFunc(m_simplexSolver, m_pdSolver);

	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(btHfFluidRigidCollisionAlgorithm);
		int maxSize1 = 0;
		int maxSize2 = 0;

		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);
		}
	}
}
Ejemplo n.º 13
0
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;
}
Ejemplo n.º 14
0
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;
}
Ejemplo n.º 15
0
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);
}
Ejemplo n.º 16
0
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);
}
Ejemplo n.º 17
0
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);
		}
	}
}
void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
{
	#ifndef __SPU__
	if (m_useSolveConstraintObsolete)
	{
		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;
			bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
			btVector3 vel2;
			bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
			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 ftorqueAxis1 = rel_pos1.cross(normal);
				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
				bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
				bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
		
			}
		}

		// apply motor
		if (m_bMotorEnabled)
		{
			// compute current and predicted transforms
			btTransform trACur = m_rbA.getCenterOfMassTransform();
			btTransform trBCur = m_rbB.getCenterOfMassTransform();
			btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
			btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
			btTransform trAPred; trAPred.setIdentity(); 
			btVector3 zerovec(0,0,0);
			btTransformUtil::integrateTransform(
				trACur, zerovec, omegaA, timeStep, trAPred);
			btTransform trBPred; trBPred.setIdentity(); 
			btTransformUtil::integrateTransform(
				trBCur, zerovec, omegaB, timeStep, trBPred);

			// compute desired transforms in world
			btTransform trPose(m_qTarget);
			btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
			btTransform trADes = trBPred * trABDes;
			btTransform trBDes = trAPred * trABDes.inverse();

			// compute desired omegas in world
			btVector3 omegaADes, omegaBDes;
			
			btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
			btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);

			// compute delta omegas
			btVector3 dOmegaA = omegaADes - omegaA;
			btVector3 dOmegaB = omegaBDes - omegaB;

			// compute weighted avg axis of dOmega (weighting based on inertias)
			btVector3 axisA, axisB;
			btScalar kAxisAInv = 0, kAxisBInv = 0;

			if (dOmegaA.length2() > SIMD_EPSILON)
			{
				axisA = dOmegaA.normalized();
				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
			}

			if (dOmegaB.length2() > SIMD_EPSILON)
			{
				axisB = dOmegaB.normalized();
				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
			}

			btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;

			static bool bDoTorque = true;
			if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
			{
				avgAxis.normalize();
				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
				btScalar kInvCombined = kAxisAInv + kAxisBInv;

				btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
									(kInvCombined * kInvCombined);

				if (m_maxMotorImpulse >= 0)
				{
					btScalar fMaxImpulse = m_maxMotorImpulse;
					if (m_bNormalizedMotorStrength)
						fMaxImpulse = fMaxImpulse/kAxisAInv;

					btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
					btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
					if (newUnclampedMag > fMaxImpulse)
					{
						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);
	}


}
Ejemplo n.º 20
0
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) 
		{
			//todo: add limits too
			btVector3 angularLimit(0,0,0);

			btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
			btScalar projRelVel = velrel.dot(axisA);

			btScalar desiredMotorVel = m_motorTargetVelocity;
			btScalar motor_relvel = desiredMotorVel - projRelVel;

			btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
			//todo: should clip against accumulated impulse
			btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
			btVector3 motorImp = clippedMotorImpulse * axisA;

			m_rbA.applyTorqueImpulse(motorImp+angularLimit);
			m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
			
		}
	}

}
Ejemplo n.º 21
0
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);

		}
	
	}

}
Ejemplo n.º 22
0
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);
}
Ejemplo n.º 23
0
void ObjectMotionState::setRestitution(float restitution) {
    _restitution = btMax(btMin(fabsf(restitution), 1.0f), 0.0f);
}
Ejemplo n.º 24
0
void    btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar    timeStep)
{

    ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    if (m_useSolveConstraintObsolete)
    {

        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,vel2;
            bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
            bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
            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;
                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
                bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
                bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
            }
        }


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

            btVector3 angVelA;
            bodyA.getAngularVelocity(angVelA);
            btVector3 angVelB;
            bodyB.getAngularVelocity(angVelB);

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

                    bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
                    bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));

                }

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

                    bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
                    bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));

                }





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



                    bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
                    bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));

                }
            }

            //apply motor
            if (m_enableAngularMotor)
            {
                //todo: add limits too
                btVector3 angularLimit(0,0,0);

                btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
                btScalar projRelVel = velrel.dot(axisA);

                btScalar desiredMotorVel = m_motorTargetVelocity;
                btScalar motor_relvel = desiredMotorVel - projRelVel;

                btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;

                // accumulated impulse clipping:
                btScalar fMaxImpulse = m_maxMotorImpulse;
                btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse;
                btScalar clippedMotorImpulse = unclippedMotorImpulse;
                if (newAccImpulse > fMaxImpulse)
                {
                    newAccImpulse = fMaxImpulse;
                    clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
                }
                else if (newAccImpulse < -fMaxImpulse)
                {
                    newAccImpulse = -fMaxImpulse;
                    clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
                }
                m_accMotorImpulse += clippedMotorImpulse;

                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);

            }
        }
    }

}
Ejemplo n.º 25
0
btVector3 Tire::getForce(
	btScalar normal_force,
	btScalar friction_coeff,
	btScalar inclination,
	btScalar ang_velocity,
	btScalar lon_velocity,
	btScalar lat_velocity)
{
	if (normal_force < 1E-3 || friction_coeff < 1E-3)
	{
		slide = slip = 0;
		ideal_slide = ideal_slip = 1;
		fx = fy = fz = mz = 0;
		vx = vy = 0;
		return btVector3(0, 0, 0);
	}

	// pacejka in kN
	normal_force = normal_force * 1E-3;

	// limit input
	btClamp(normal_force, btScalar(0), btScalar(max_load));
	btClamp(inclination, -max_camber, max_camber);

	// get ideal slip ratio
	btScalar sigma_hat(0);
	btScalar alpha_hat(0);
	getSigmaHatAlphaHat(normal_force, sigma_hat, alpha_hat);

	btScalar Fz = normal_force;
	btScalar gamma = inclination; // positive when tire top tilts to the right, viewed from rear
	btScalar denom = btMax(btFabs(lon_velocity), btScalar(1E-3));
	btScalar lon_slip_velocity = lon_velocity - ang_velocity;
	btScalar sigma = -lon_slip_velocity / denom; // longitudinal slip: negative in braking, positive in traction
	btScalar tan_alpha = lat_velocity / denom;
	btScalar alpha = -atan(tan_alpha) * 180.0 / M_PI; // sideslip angle: positive in a right turn(opposite to SAE tire coords)
	btScalar max_Fx(0), max_Fy(0), max_Mz(0);

	// combining method 1: beckman method for pre-combining longitudinal and lateral forces
	// seems to be a variant of Bakker 1987:
	// refined Kamm Circle for non-isotropic tire characteristics
	// and slip normalization to guarantee simultaneous sliding
	btScalar s = sigma / sigma_hat;
	btScalar a = alpha / alpha_hat;
	btScalar rho = btMax(btSqrt(s * s + a * a), btScalar(1E-4)); // normalized slip
	btScalar Fx = (s / rho) * PacejkaFx(rho * sigma_hat, Fz, friction_coeff, max_Fx);
	btScalar Fy = (a / rho) * PacejkaFy(rho * alpha_hat, Fz, gamma, friction_coeff, max_Fy);
	// Bakker (with unknown factor e(rho) to get the correct direction for large slip):
	// btScalar Fx = -s * PacejkaFx(rho * sigma_hat, Fz, friction_coeff, max_Fx);
	// btScalar Fy = -a * e(rho) * PacejkaFy(rho * alpha_hat, Fz, gamma, friction_coeff, max_Fy);

/*
	// combining method 2: orangutan
	btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
	btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);

	btScalar one = (sigma < 0.0) ? -1.0 : 1.0;
	btScalar pure_sigma = sigma / (one + sigma);
	btScalar pure_alpha = -tan_alpha / (one + sigma);
	btScalar pure_combined = sqrt(pure_sigma * pure_sigma + pure_alpha * pure_alpha);
	btScalar kappa_combined = pure_combined / (1.0 - pure_combined);
	btScalar alpha_combined = atan((one + sigma) * pure_combined);
	btScalar Flimitx = PacejkaFx(kappa_combined, Fz, friction_coeff, max_Fx);
	btScalar Flimity = PacejkaFy(alpha_combined * 180.0 / M_PI, Fz, gamma, friction_coeff, max_Fy);

	btScalar x = fabs(Fx) / (fabs(Fx) + fabs(Fy));
	btScalar y = 1.0 - x;
	btScalar Flimit = (fabs(x * Flimitx) + fabs(y * Flimity));
	btScalar Fmag = sqrt(Fx * Fx + Fy * Fy);
	if (Fmag > Flimit)
	{
		btScalar scale = Flimit / Fmag;
		Fx *= scale;
		Fy *= scale;
	}
*/
/*
	// combining method 3: traction circle
	btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
	btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);

	// determine to what extent the tires are long (x) gripping vs lat (y) gripping
	// 1.0 when Fy is zero, 0.0 when Fx is zero
	btScalar longfactor = 1.0;
	btScalar combforce = btFabs(Fx) + btFabs(Fy);
	if (combforce > 1) longfactor = btFabs(Fx) / combforce;

	// determine the maximum force for this amount of long vs lat grip by linear interpolation
	btScalar maxforce = btFabs(max_Fx) * longfactor + (1.0 - longfactor) * btFabs(max_Fy);

	// scale down forces to fit into the maximum
	if (combforce > maxforce)
	{
		Fx *= maxforce / combforce;
		Fy *= maxforce / combforce;
	}
*/
/*
	// combining method 4: traction ellipse (prioritize Fx)
	// issue: assumes that adhesion limits are fully reached for combined forces
	btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
	btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);
	if (Fx < max_Fx)
	{
		Fy = Fy * sqrt(1.0 - (Fx / max_Fx) * (Fx / max_Fx));
	}
	else
	{
		Fx = max_Fx;
		Fy = 0;
	}
*/
/*
	// combining method 5: traction ellipse (prioritize Fy)
	// issue: assumes that adhesion limits are fully reached for combined forces
	btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
	btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);
	if (Fy < max_Fy)
	{
		Fx = Fx * sqrt(1.0 - (Fy / max_Fy) * (Fy / max_Fy));
	}
	else
	{
		Fy = max_Fy;
		Fx = 0;
	}
*/
/*
	// no combining
	btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx);
	btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy);
*/
	btScalar Mz = PacejkaMz(sigma, alpha, Fz, gamma, friction_coeff, max_Mz);

	camber = inclination;
	slide = sigma;
	slip = alpha;
	ideal_slide = sigma_hat;
	ideal_slip = alpha_hat;
	fx = Fx;
	fy = Fy;
	fz = Fz;
	mz = Mz;
	vx = lon_slip_velocity;
	vy = lat_velocity;

	// Fx positive during traction, Fy positive in a right turn, Mz positive in a left turn
	return btVector3(Fx, Fy, Mz);
}
Ejemplo n.º 26
0
void Camera::Translate(const Vector3& vec)
{
	Vector3 forwards;
	float z = vec.z * -1.0f;

	if (m_type == CameraType::FIRST_PERSON) {
		forwards = WORLD_Y_AXIS.Cross(m_localXAxis);
		forwards.Normalize();
	}
	else {
		forwards = m_viewVector;
	}
	auto& current = m_transform.Position;
	//auto& current = btt.getOrigin();

	if (m_collider) {
		//btVector3 after = current;
		Vector3 after = current;
		after += m_localXAxis.AsBtVector3() * vec.x;
		after += WORLD_Y_AXIS.AsBtVector3() * vec.y;
		after += forwards.AsBtVector3() * z;		
		btTransform from, to;
		from.setIdentity();
		to.setIdentity();		
		
		from.setOrigin(current.AsBtVector3());
		to.setOrigin(after.AsBtVector3());

		btCollisionWorld::ClosestConvexResultCallback cb(current.AsBtVector3(), after.AsBtVector3());
		cb.m_collisionFilterMask = btBroadphaseProxy::DefaultFilter | btBroadphaseProxy::StaticFilter;

		btConvexShape* cs = static_cast<btConvexShape*>(m_collider->GetRigidBody()->getCollisionShape());
		m_physicsWorld->ConvexSweepTest(cs, from, to, cb);
		if (cb.hasHit() && cb.m_hitCollisionObject != m_collider->GetRigidBody()) {
			/*if () {
				printf("\nHit Self");
			}*/
			float epsilon = 0.0001;
			float min = btMax(epsilon, cb.m_closestHitFraction);			
			current += -forwards * 0.1;
			btVector3 newPos = current.AsBtVector3();
			//newPos.setInterpolate3(current.AsBtVector3(), after.AsBtVector3(), min);			
			m_transform.Position = Vector3(newPos);
		}
		else {			
			m_transform.Position = after;
		}
	}
	else
	{
		current += m_localXAxis.AsBtVector3() * vec.x;
		current += WORLD_Y_AXIS.AsBtVector3() * vec.y;
		current += forwards.AsBtVector3() * z;
		m_transform.Position = current;
	}
	if (m_collider) {
		btTransform btt;
		btt.setRotation(m_transform.Orientation.AsBtQuaternion());
		btt.setOrigin(m_transform.Position.AsBtVector3());		
		m_collider->SetWorldTransform(btt);
	}

}
Ejemplo n.º 27
0
void ObjectMotionState::setAngularDamping(float damping) {
    _angularDamping = btMax(btMin(fabsf(damping), 1.0f), 0.0f);
}
Ejemplo n.º 28
0
void ObjectMotionState::setLinearDamping(float damping) {
    _linearDamping = btMax(btMin(fabsf(damping), 1.0f), 0.0f);
}
Ejemplo n.º 29
0
void Vehicle::SetTargetSteering(float steering)
{
	steering = btMin(btMax(-1.f, steering), 1.f);//clamp
	targetSteering = steering * steeringClamp;
}
Ejemplo n.º 30
0
void ObjectMotionState::setFriction(float friction) {
    _friction = btMax(btMin(fabsf(friction), MAX_FRICTION), 0.0f);
}