C++ btVector3::setZ方法代码示例

void cSpace::movePointOutsideCSpace(btVector3& pt)
{
	float angle=0;
	float zHeight = pt.z();
	pt.setZ(0);
	btVector3 nudge(0.01,0,0);
	btVector3 npt = pt;
	
	while(isPointInsideCSpace(npt))							// check if the point is inside of the object
	{
		npt = pt + nudge.rotate(btVector3(0,0,1),angle);	// calculate a new point a radius of vector nudge away at angle
		angle -= 0.2;										// in radians, CCW search direction
		
		if(angle < -PI) {									// if the search has gone a full rotation
			nudge.setX(nudge.x() + 0.01);					// add another centemeter to the search vector radius
			if(nudge.length() > 1) {
				qDebug("Nudge length is greater than 1");
				break;
			}
			angle = 0;
		}
	}
	
	pt = npt;
	pt.setZ(zHeight);
}

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

void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
{
	// the swing axis is computed as the "twist-free" cone rotation,
	// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
	// so, if we're outside the limits, the closest way back inside the cone isn't 
	// along the vector back to the center. better (and more stable) to use the ellipse normal.

	// convert swing axis to direction from center to surface of ellipse
	// (ie. rotate 2D vector by PI/2)
	btScalar y = -vSwingAxis.z();
	btScalar z =  vSwingAxis.y();

	// do the math...
	if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
	{
		// compute gradient/normal of ellipse surface at current "point"
		btScalar grad = y/z;
		grad *= m_swingSpan2 / m_swingSpan1;

		// adjust y/z to represent normal at point (instead of vector to point)
		if (y > 0)
			y =  fabs(grad * z);
		else
			y = -fabs(grad * z);

		// convert ellipse direction back to swing axis
		vSwingAxis.setZ(-y);
		vSwingAxis.setY( z);
		vSwingAxis.normalize();
	}
}

// dir = 1 for CCW search or clockwise around obstacle
// dir = -1 for CW search or counter clockwise around obstacle
bool cSpace::movePointAroundCSpace(btVector3& pt, btVector3 startVect, float stpMag, int dir)
{
	float angle=0;
	float zHeight = pt.z();
	pt.setZ(0);
	btVector3 nudge = stpMag * startVect.normalize();
	btVector3 npt = pt + nudge;
	
	while(isPointInsideCSpace(npt))							// check if the point is inside of the object
	{
		npt = pt + nudge.rotate(btVector3(0,0,1),angle);	// calculate a new point a radius of vector nudge away at angle
		angle += (dir * 0.1);								// in radians
		
		if(fabs(angle) > TWOPI)								// if the search has gone a full rotation
			return false;									// and there is no path outside a C-Space return false, stuck
	}
	
	pt = npt;
	pt.setZ(zHeight);
	return true;
}

ItemController::ItemController(btVector3 position, std::weak_ptr<PhysicsWorld> world, TroenGame* troenGame, LevelView* levelView)
{
	AbstractController();

	m_type = (ItemController::Type) (int) floor(randf(0, COUNT));
	m_position = position;
	m_troenGame = troenGame;

	osg::Vec3 viewDimensions = getDimensions();
	btVector3 modelDimensions(viewDimensions.x(), viewDimensions.y(), 10);
	position.setZ(position.z() + viewDimensions.z() / 2.f);

	m_model = m_itemModel = std::make_shared<ItemModel>(modelDimensions, position, world, this);
	m_view = m_itemView = std::make_shared<ItemView>(getDimensions(), btToOSGVec3(position), levelView, m_type);
}

// Converts a quaternion to an euler angle
void _Physics::QuaternionToEuler(const btQuaternion &Quat, btVector3 &Euler) {
	btScalar W = Quat.getW();
	btScalar X = Quat.getX();
	btScalar Y = Quat.getY();
	btScalar Z = Quat.getZ();
	float WSquared = W * W;
	float XSquared = X * X;
	float YSquared = Y * Y;
	float ZSquared = Z * Z;

	Euler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
	Euler.setY(asinf(-2.0f * (X * Z - Y * W)));
	Euler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
	Euler *= irr::core::RADTODEG;
}

// Converts a quaternion to an euler angle
void QuaternionToEuler(const btQuaternion &tQuat, btVector3 &tEuler) {
  btScalar w = tQuat.getW();
  btScalar x = tQuat.getX();
  btScalar y = tQuat.getY();
  btScalar z = tQuat.getZ();
  float wSquared = w * w;
  float xSquared = x * x;
  float ySquared = y * y;
  float zSquared = z * z;

  tEuler.setX(atan2f(2.0f * (y * z + x * w), -xSquared - ySquared + zSquared + wSquared));
  tEuler.setY(asinf(-2.0f * (x * z - y * w)));
  tEuler.setZ(atan2f(2.0f * (x * y + z * w), xSquared - ySquared - zSquared + wSquared));
  tEuler *= SIMD_DEGS_PER_RAD;
}

void Physics::QuaternionToEuler( const CIwFQuat& TQuat, btVector3 &TEuler )
{
	float a[3];

	const float w = TQuat.s;
	const float x = TQuat.x;
	const float y = TQuat.y;
	const float z = TQuat.z;

	QuaternionToEuler( w, x, y, z, a );

	TEuler.setX( a[0] );
	TEuler.setY( a[1] );
	TEuler.setZ( a[2] );
}

// Converts a quaternion to an euler angle
void CTBulletHelper::QuaternionToEuler(const btQuaternion &TQuat, btVector3 &TEuler) {
	btScalar W = TQuat.getW();
	btScalar X = TQuat.getX();
	btScalar Y = TQuat.getY();
	btScalar Z = TQuat.getZ();
	float WSquared = W * W;
	float XSquared = X * X;
	float YSquared = Y * Y;
	float ZSquared = Z * Z;
	
	TEuler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
	TEuler.setY(asinf(-2.0f * (X * Z - Y * W)));
	TEuler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
	TEuler *= core::RADTODEG;
}

void ConvertPosToBull(const Vector& pos, btVector3& bull) {
	bull.setX(HL2BULL(pos.x));
	bull.setY(HL2BULL(pos.z));
	bull.setZ(-HL2BULL(pos.y));
}

//.........这里部分代码省略.........
				// triangles
				for(size_t f = 0; f < nTriangles; f++)
				{
					triangles[3*f] = trianglesCH[f].X();
					triangles[3*f+1] = trianglesCH[f].Y();
					triangles[3*f+2] = trianglesCH[f].Z();
				}

				delete [] pointsCH;
				delete [] trianglesCH;

				ConvexResult r(nPoints, vertices, nTriangles, triangles);
				convexDecomposition.ConvexDecompResult(r);
			}

			for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
			{
				btVector3 centroid = convexDecomposition.m_convexCentroids[i];
				trans.setOrigin(centroid);
				btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
				compound->addChildShape(trans,convexShape);

				btRigidBody* body;
				body = localCreateRigidBody( 1.0, trans,convexShape);
			}
/*			for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
			{

				btVector3 centroid = convexDecomposition.m_convexCentroids[i];
				trans.setOrigin(centroid);
				btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
				compound->addChildShape(trans,convexShape);

				btRigidBody* body;
				body = localCreateRigidBody( 1.0, trans,convexShape);
			}*/

#if 1
			btScalar mass=10.f;
			trans.setOrigin(-convexDecompositionObjectOffset);
			btRigidBody* body = localCreateRigidBody( mass, trans,compound);
			body->setCollisionFlags(body->getCollisionFlags() |   btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);

			convexDecompositionObjectOffset.setZ(6);
			trans.setOrigin(-convexDecompositionObjectOffset);
			body = localCreateRigidBody( mass, trans,compound);
			body->setCollisionFlags(body->getCollisionFlags() |   btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);

			convexDecompositionObjectOffset.setZ(-6);
			trans.setOrigin(-convexDecompositionObjectOffset);
			body = localCreateRigidBody( mass, trans,compound);
			body->setCollisionFlags(body->getCollisionFlags() |   btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
#endif
		}


		if (outputFile)
			fclose(outputFile);


	}



#ifdef TEST_SERIALIZATION
	//test serializing this

	int maxSerializeBufferSize = 1024*1024*5;

	btDefaultSerializer*	serializer = new btDefaultSerializer(maxSerializeBufferSize);
	m_dynamicsWorld->serialize(serializer);

	FILE* f2 = fopen("testFile.bullet","wb");
	fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
	fclose(f2);

	exitPhysics();

	//now try again from the loaded file
	setupEmptyDynamicsWorld();
#endif //TEST_SERIALIZATION

#endif //NO_OBJ_TO_BULLET

#ifdef TEST_SERIALIZATION

	btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
	//fileLoader->setVerboseMode(true);

	fileLoader->loadFile("testFile.bullet");
	//fileLoader->loadFile("testFile64Double.bullet");
	//fileLoader->loadFile("testFile64Single.bullet");
	//fileLoader->loadFile("testFile32Single.bullet");




#endif //TEST_SERIALIZATION

}

void ConvertAngularImpulseToBull(const AngularImpulse& angularimp, btVector3& bull) {
	bull.setX(DEG2RAD(angularimp.x));
	bull.setY(DEG2RAD(angularimp.z));
	bull.setZ(-DEG2RAD(angularimp.y));
}

//.........这里部分代码省略.........
		delete hull;



		m_collisionShapes.push_back(convexShape);

		float mass = 1.f;
		
		btTransform startTransform;
		startTransform.setIdentity();
		startTransform.setOrigin(btVector3(0,2,0));

		localCreateRigidBody(mass, startTransform,convexShape);
		
		bool useQuantization = true;
		btCollisionShape* concaveShape = new btBvhTriangleMeshShape(trimesh,useQuantization);
		startTransform.setOrigin(convexDecompositionObjectOffset);
		localCreateRigidBody(0.f,startTransform,concaveShape);

		m_collisionShapes.push_back (concaveShape);

	}
			

	if (tcount)
	{

		char outputFileName[512];
  		strcpy(outputFileName,filename);
  		char *dot = strstr(outputFileName,".");
  		if ( dot ) 
			*dot = 0;
		strcat(outputFileName,"_convex.obj");
  		FILE* outputFile = fopen(outputFileName,"wb");
				
		unsigned int depth = 5;
		float cpercent     = 5;
		float ppercent     = 15;
		unsigned int maxv  = 16;
		float skinWidth    = 0.0;

		printf("WavefrontObj num triangles read %i\n",tcount);
		ConvexDecomposition::DecompDesc desc;
		desc.mVcount       =	wo.mVertexCount;
		desc.mVertices     = wo.mVertices;
		desc.mTcount       = wo.mTriCount;
		desc.mIndices      = (unsigned int *)wo.mIndices;
		desc.mDepth        = depth;
		desc.mCpercent     = cpercent;
		desc.mPpercent     = ppercent;
		desc.mMaxVertices  = maxv;
		desc.mSkinWidth    = skinWidth;

		MyConvexDecomposition	convexDecomposition(outputFile,this);
		desc.mCallback = &convexDecomposition;
		
		

		//convexDecomposition.performConvexDecomposition(desc);

		ConvexBuilder cb(desc.mCallback);
		cb.process(desc);
		//now create some bodies
		
		{
			btCompoundShape* compound = new btCompoundShape();
			m_collisionShapes.push_back (compound);

			btTransform trans;
			trans.setIdentity();
			for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
			{
				
				btVector3 centroid = convexDecomposition.m_convexCentroids[i];
				trans.setOrigin(centroid);
				btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
				compound->addChildShape(trans,convexShape);
			}
			btScalar mass=10.f;
			trans.setOrigin(-convexDecompositionObjectOffset);
			localCreateRigidBody( mass, trans,compound);
			convexDecompositionObjectOffset.setZ(6);
			trans.setOrigin(-convexDecompositionObjectOffset);
			localCreateRigidBody( mass, trans,compound);
			convexDecompositionObjectOffset.setZ(-6);
			trans.setOrigin(-convexDecompositionObjectOffset);
			localCreateRigidBody( mass, trans,compound);
			
		}

		
		if (outputFile)
			fclose(outputFile);


	}


	
}

//.........这里部分代码省略.........
		desc.mIndices      = (unsigned int *)wo.mIndices;
		desc.mDepth        = depth;
		desc.mCpercent     = cpercent;
		desc.mPpercent     = ppercent;
		desc.mMaxVertices  = maxv;
		desc.mSkinWidth    = skinWidth;

		MyConvexDecomposition	convexDecomposition(outputFile,this);
		desc.mCallback = &convexDecomposition;
		
		

		//convexDecomposition.performConvexDecomposition(desc);

		ConvexBuilder cb(desc.mCallback);
		cb.process(desc);
		//now create some bodies
		
		if (1)
		{
			btCompoundShape* compound = new btCompoundShape();
			m_collisionShapes.push_back (compound);

			btTransform trans;
			trans.setIdentity();
			for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
			{
				
				btVector3 centroid = convexDecomposition.m_convexCentroids[i];
				trans.setOrigin(centroid);
				btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
				compound->addChildShape(trans,convexShape);

				btRigidBody* body;
				body = localCreateRigidBody( 1.0, trans,convexShape);

			}
#if 1
			btScalar mass=10.f;
			trans.setOrigin(-convexDecompositionObjectOffset);
			btRigidBody* body = localCreateRigidBody( mass, trans,compound);
			body->setCollisionFlags(body->getCollisionFlags() |   btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);

			convexDecompositionObjectOffset.setZ(6);
			trans.setOrigin(-convexDecompositionObjectOffset);
			body = localCreateRigidBody( mass, trans,compound);
			body->setCollisionFlags(body->getCollisionFlags() |   btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);

			convexDecompositionObjectOffset.setZ(-6);
			trans.setOrigin(-convexDecompositionObjectOffset);
			body = localCreateRigidBody( mass, trans,compound);
			body->setCollisionFlags(body->getCollisionFlags() |   btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
#endif
		}

		
		if (outputFile)
			fclose(outputFile);


	}



#ifdef TEST_SERIALIZATION
	//test serializing this 

	int maxSerializeBufferSize = 1024*1024*5;

	btDefaultSerializer*	serializer = new btDefaultSerializer(maxSerializeBufferSize);
	m_dynamicsWorld->serialize(serializer);
	
	FILE* f2 = fopen("testFile.bullet","wb");
	fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
	fclose(f2);

	exitPhysics();

	//now try again from the loaded file
	setupEmptyDynamicsWorld();
#endif //TEST_SERIALIZATION

#endif //NO_OBJ_TO_BULLET

#ifdef TEST_SERIALIZATION

	btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
	//fileLoader->setVerboseMode(true);

	fileLoader->loadFile("testFile.bullet");
	//fileLoader->loadFile("testFile64Double.bullet");
	//fileLoader->loadFile("testFile64Single.bullet");
	//fileLoader->loadFile("testFile32Single.bullet");
	



#endif //TEST_SERIALIZATION
	
}

void ConvertDirectionToBull(const Vector& dir, btVector3& bull) {
	bull.setX(dir.x);
	bull.setY(dir.z);
	bull.setZ(-dir.y);
}