C++ btVector3类代码示例

void collisionWithBound(btVector3 &vel, btVector3 &pos, const btVector3 &normal, const btScalar &penetrationDist)
{
    if( penetrationDist < btScalar(0.0) )
	{
        
        pos = pos - normal * penetrationDist;
        
        vel = vel - (1 + Constant.m_boundaryRestitution * (-penetrationDist) / (Constant.m_timeStep * vel.length())) * vel.dot(normal) * normal;
	}
}

///combined discrete/continuous sphere-triangle
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact)
{

	const btVector3* vertices = &m_triangle->getVertexPtr(0);
	const btVector3& c = sphereCenter;
	btScalar r = m_sphere->getRadius();

	btVector3 delta (0,0,0);

	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
	normal.normalize();
	btVector3 p1ToCentre = c - vertices[0];
	btScalar distanceFromPlane = p1ToCentre.dot(normal);

	if (distanceFromPlane < btScalar(0.))
	{
		//triangle facing the other way
	
		distanceFromPlane *= btScalar(-1.);
		normal *= btScalar(-1.);
	}

	///todo: move this gContactBreakingThreshold into a proper structure
	extern btScalar gContactBreakingThreshold;

	btScalar contactMargin = gContactBreakingThreshold;
	bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
	bool isInsideShellPlane = distanceFromPlane < r;
	
	btScalar deltaDotNormal = delta.dot(normal);
	if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
		return false;

	// Check for contact / intersection
	bool hasContact = false;
	btVector3 contactPoint;
	if (isInsideContactPlane) {
		if (facecontains(c,vertices,normal)) {
			// Inside the contact wedge - touches a point on the shell plane
			hasContact = true;
			contactPoint = c - normal*distanceFromPlane;
		} else {
			// Could be inside one of the contact capsules
			btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
			btVector3 nearestOnEdge;
			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
				
				btPoint3 pa;
				btPoint3 pb;
				
				m_triangle->getEdge(i,pa,pb);

				btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
				if (distanceSqr < contactCapsuleRadiusSqr) {
					// Yep, we're inside a capsule
					hasContact = true;
					contactPoint = nearestOnEdge;
				}
				
			}
		}
	}

	if (hasContact) {
		btVector3 contactToCentre = c - contactPoint;
		btScalar distanceSqr = contactToCentre.length2();
		if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
			btScalar distance = btSqrt(distanceSqr);
			resultNormal = contactToCentre;
			resultNormal.normalize();
			point = contactPoint;
			depth = -(r-distance);
			return true;
		}

		if (delta.dot(contactToCentre) >= btScalar(0.0)) 
			return false;
		
		// Moving towards the contact point -> collision
		point = contactPoint;
		timeOfImpact = btScalar(0.0);
		return true;
	}
	
	return false;
}

 vector3df toIrrlicht(btVector3 vec) {
   return vector3df(vec.getX(), vec.getY(), vec.getZ());
 }

osg::Vec3
osgbCollision::asOsgVec3( const btVector3& v )
{
    return osg::Vec3( v.x(), v.y(), v.z() );
}

void ConvexDecompositionDemo::initPhysics(const char* filename)
{

	gContactAddedCallback = &MyContactCallback;

	setupEmptyDynamicsWorld();

	getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);

	setTexturing(true);
	setShadows(true);

	setCameraDistance(26.f);


#ifndef NO_OBJ_TO_BULLET

	ConvexDecomposition::WavefrontObj wo;

	tcount = 0;
    const char* prefix[]={"./","../","../../","../../../","../../../../", "ConvexDecompositionDemo/", "Demos/ConvexDecompositionDemo/",
    "../Demos/ConvexDecompositionDemo/","../../Demos/ConvexDecompositionDemo/"};
    int numPrefixes = sizeof(prefix)/sizeof(const char*);
    char relativeFileName[1024];

    for (int i=0;i<numPrefixes;i++)
    {
        sprintf(relativeFileName,"%s%s",prefix[i],filename);
        tcount = wo.loadObj(relativeFileName);
        if (tcount)
            break;
    }



	btTransform startTransform;
	startTransform.setIdentity();
	startTransform.setOrigin(btVector3(0,-4.5,0));

	btCollisionShape* boxShape = new btBoxShape(btVector3(30,2,30));
	m_collisionShapes.push_back(boxShape);
	localCreateRigidBody(0.f,startTransform,boxShape);

	class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
	{
		ConvexDecompositionDemo*	m_convexDemo;

		public:

		btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
		btAlignedObjectArray<btVector3> m_convexCentroids;

		MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
			:m_convexDemo(demo),
				mBaseCount(0),
			mHullCount(0),
			mOutputFile(outputFile)

		{
		}

			virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
			{

				btTriangleMesh* trimesh = new btTriangleMesh();
				m_convexDemo->m_trimeshes.push_back(trimesh);

				btVector3 localScaling(6.f,6.f,6.f);

				//export data to .obj
				printf("ConvexResult. ");
				if (mOutputFile)
				{
					fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );

					fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
					fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);

					for (unsigned int i=0; i<result.mHullVcount; i++)
					{
						const float *p = &result.mHullVertices[i*3];
						fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
					}

					//calc centroid, to shift vertices around center of mass
					centroid.setValue(0,0,0);

					btAlignedObjectArray<btVector3> vertices;
					if ( 1 )
					{
						//const unsigned int *src = result.mHullIndices;
						for (unsigned int i=0; i<result.mHullVcount; i++)
						{
							btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
							vertex *= localScaling;
							centroid += vertex;

						}
					}

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

void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int	debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax)
{

	GLuint err = glGetError();
	btAssert(err==GL_NO_ERROR);

	if (shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE)
	{
		btVector3 org(m[12], m[13], m[14]);
		btVector3 dx(m[0], m[1], m[2]);
		btVector3 dy(m[4], m[5], m[6]);
//		btVector3 dz(m[8], m[9], m[10]);
		const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
		btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
		dx *= halfExtent[0];
		dy *= halfExtent[1];
//		dz *= halfExtent[2];
		glColor3f(1,1,1);
		glDisable(GL_LIGHTING);
		glLineWidth(2);

		glBegin(GL_LINE_LOOP);
		glDrawVector(org - dx - dy);
		glDrawVector(org - dx + dy);
		glDrawVector(org + dx + dy);
		glDrawVector(org + dx - dy);
		glEnd();
		return;
	} 
	else if((shape->getShapeType() == BOX_SHAPE_PROXYTYPE) && (debugMode & btIDebugDraw::DBG_FastWireframe))
	{
		btVector3 org(m[12], m[13], m[14]);
		btVector3 dx(m[0], m[1], m[2]);
		btVector3 dy(m[4], m[5], m[6]);
		btVector3 dz(m[8], m[9], m[10]);
		const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
		btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
		dx *= halfExtent[0];
		dy *= halfExtent[1];
		dz *= halfExtent[2];
		glBegin(GL_LINE_LOOP);
		glDrawVector(org - dx - dy - dz);
		glDrawVector(org + dx - dy - dz);
		glDrawVector(org + dx + dy - dz);
		glDrawVector(org - dx + dy - dz);
		glDrawVector(org - dx + dy + dz);
		glDrawVector(org + dx + dy + dz);
		glDrawVector(org + dx - dy + dz);
		glDrawVector(org - dx - dy + dz);
		glEnd();
		glBegin(GL_LINES);
		glDrawVector(org + dx - dy - dz);
		glDrawVector(org + dx - dy + dz);
		glDrawVector(org + dx + dy - dz);
		glDrawVector(org + dx + dy + dz);
		glDrawVector(org - dx - dy - dz);
		glDrawVector(org - dx + dy - dz);
		glDrawVector(org - dx - dy + dz);
		glDrawVector(org - dx + dy + dz);
		glEnd();
		return;
	}

	glPushMatrix(); 

#if defined(BT_USE_DOUBLE_PRECISION)
	glMultMatrixd(m);
#else
	glMultMatrixf(m);
#endif


	if (shape->getShapeType() == UNIFORM_SCALING_SHAPE_PROXYTYPE)
	{
		const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape);
		const btConvexShape* convexShape = scalingShape->getChildShape();
		float	scalingFactor = (float)scalingShape->getUniformScalingFactor();
		{
			btScalar tmpScaling[4][4]={{scalingFactor,0,0,0},
			{0,scalingFactor,0,0},
			{0,0,scalingFactor,0},
			{0,0,0,1}};

			drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode,worldBoundsMin,worldBoundsMax);
		}
		glPopMatrix();
		return;
	}

	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
	{
		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
		for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
		{
			btTransform childTrans = compoundShape->getChildTransform(i);
			const btCollisionShape* colShape = compoundShape->getChildShape(i);
			btScalar childMat[16];
			childTrans.getOpenGLMatrix(childMat);
			drawOpenGL(childMat,colShape,color,debugMode,worldBoundsMin,worldBoundsMax);
		}
//.........这里部分代码省略.........

void	btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{

//Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility
//#define USE_BOX_INERTIA_APPROXIMATION 1
#ifndef USE_BOX_INERTIA_APPROXIMATION

	/*
	cylinder is defined as following:
	*
	* - principle axis aligned along y by default, radius in x, z-value not used
	* - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used
	* - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used
	*
	*/

	btScalar radius2;	// square of cylinder radius
	btScalar height2;	// square of cylinder height
	btVector3 halfExtents = getHalfExtentsWithMargin();	// get cylinder dimension
	btScalar div12 = mass / 12.f;
	btScalar div4 = mass / 4.f;
	btScalar div2 = mass / 2.f;
	int idxRadius, idxHeight;

	switch (m_upAxis)	// get indices of radius and height of cylinder
	{
		case 0:		// cylinder is aligned along x
			idxRadius = 1;
			idxHeight = 0;
			break;
		case 2:		// cylinder is aligned along z
			idxRadius = 0;
			idxHeight = 2;
			break;
		default:	// cylinder is aligned along y
			idxRadius = 0;
			idxHeight = 1;
	}

	// calculate squares
	radius2 = halfExtents[idxRadius] * halfExtents[idxRadius];
	height2 = btScalar(4.) * halfExtents[idxHeight] * halfExtents[idxHeight];

	// calculate tensor terms
	btScalar t1 = div12 * height2 + div4 * radius2;
	btScalar t2 = div2 * radius2;

	switch (m_upAxis)	// set diagonal elements of inertia tensor
	{
		case 0:		// cylinder is aligned along x
			inertia.setValue(t2,t1,t1);
			break;
		case 2:		// cylinder is aligned along z
			inertia.setValue(t1,t1,t2);
			break;
		default:	// cylinder is aligned along y
			inertia.setValue(t1,t2,t1);
	}
#else //USE_BOX_INERTIA_APPROXIMATION
	//approximation of box shape
	btVector3 halfExtents = getHalfExtentsWithMargin();

	btScalar lx=btScalar(2.)*(halfExtents.x());
	btScalar ly=btScalar(2.)*(halfExtents.y());
	btScalar lz=btScalar(2.)*(halfExtents.z());

	inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
					mass/(btScalar(12.0)) * (lx*lx + lz*lz),
					mass/(btScalar(12.0)) * (lx*lx + ly*ly));
#endif //USE_BOX_INERTIA_APPROXIMATION
}

Ogre::Vector3 PhysicsManager::convert(const btVector3& vec)
{
	return Ogre::Vector3(vec.x(),vec.y(),vec.z());
}

/**
 * Returns the distance between the two anchors.
 * This is similar to the getActualLength function in tgBulletSpringCable.
 */
const double tgBulletCompressionSpring::getCurrentAnchorDistance() const
{
    const btVector3 dist =
      this->anchor2->getWorldPosition() - this->anchor1->getWorldPosition();
    return dist.length();
}

    void PhysicsDebugDraw::drawSphere(const btVector3& p, btScalar radius, const btVector3& color)
    {

		debugRenderer->drawSphere(Vec3(p.getX(), p.getY(), p.getZ()), radius, Vec4(color.getX(), color.getY(), color.getZ(), 1.f));
    }

    void PhysicsDebugDraw::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha)
    {
		debugRenderer->drawTriangle(
			Vec3(a.getX(), a.getY(), a.getZ()),
			Vec3(b.getX(), b.getY(), b.getZ()),
			Vec3(c.getX(), c.getY(), c.getZ()),
			Vec4(color.getX(), color.getY(), color.getZ(), alpha)
		);
    }

    void PhysicsDebugDraw::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor)
    {
		debugRenderer->drawLine(
			Vec3(from.getX(), from.getY(), from.getZ()),
			Vec3(to.getX(), to.getY(), to.getZ()),
			Vec4(fromColor.getX(), fromColor.getY(), fromColor.getZ(), 1.f),
			Vec4(toColor.getX(), toColor.getY(), toColor.getZ(), 1.f)
		);
    }

Vec3f::Vec3f(const btVector3& v)
: x(v.getX())
, y(v.getY())
, z(v.getZ()) {
}

static void				drawBox(	btIDebugDraw* idraw,
								const btVector3& mins,
								const btVector3& maxs,
								const btVector3& color)
{
	const btVector3	c[]={	btVector3(mins.x(),mins.y(),mins.z()),
		btVector3(maxs.x(),mins.y(),mins.z()),
		btVector3(maxs.x(),maxs.y(),mins.z()),
		btVector3(mins.x(),maxs.y(),mins.z()),
		btVector3(mins.x(),mins.y(),maxs.z()),
		btVector3(maxs.x(),mins.y(),maxs.z()),
		btVector3(maxs.x(),maxs.y(),maxs.z()),
		btVector3(mins.x(),maxs.y(),maxs.z())};
	idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
	idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
	idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
	idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
	idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
	idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
}

btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) 
{
	//project the center of the sphere on the closest face of the box
	btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
	btScalar minDist = faceDist;
	closestPoint.setX( boxHalfExtent.getX() );
	normal.setValue(btScalar(1.0f),  btScalar(0.0f),  btScalar(0.0f));

	faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
	if (faceDist < minDist)
	{
		minDist = faceDist;
		closestPoint = sphereRelPos;
		closestPoint.setX( -boxHalfExtent.getX() );
		normal.setValue(btScalar(-1.0f),  btScalar(0.0f),  btScalar(0.0f));
	}

	faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
	if (faceDist < minDist)
	{
		minDist = faceDist;
		closestPoint = sphereRelPos;
		closestPoint.setY( boxHalfExtent.getY() );
		normal.setValue(btScalar(0.0f),  btScalar(1.0f),  btScalar(0.0f));
	}

	faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
	if (faceDist < minDist)
	{
		minDist = faceDist;
		closestPoint = sphereRelPos;
		closestPoint.setY( -boxHalfExtent.getY() );
		normal.setValue(btScalar(0.0f),  btScalar(-1.0f),  btScalar(0.0f));
	}

	faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
	if (faceDist < minDist)
	{
		minDist = faceDist;
		closestPoint = sphereRelPos;
		closestPoint.setZ( boxHalfExtent.getZ() );
		normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(1.0f));
	}

	faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
	if (faceDist < minDist)
	{
		minDist = faceDist;
		closestPoint = sphereRelPos;
		closestPoint.setZ( -boxHalfExtent.getZ() );
		normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(-1.0f));
	}

	return minDist;
}