C++ SimdTransform::setOrigin方法代码示例

void	CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
{

	
	SimdTransform	rayFromTrans,rayToTrans;
	rayFromTrans.setIdentity();
	rayFromTrans.setOrigin(rayFromWorld);
	rayToTrans.setIdentity();
	
	rayToTrans.setOrigin(rayToWorld);

	//do culling based on aabb (rayFrom/rayTo)
	SimdVector3 rayAabbMin = rayFromWorld;
	SimdVector3 rayAabbMax = rayFromWorld;
	rayAabbMin.setMin(rayToWorld);
	rayAabbMax.setMax(rayToWorld);


	/// brute force go over all objects. Once there is a broadphase, use that, or
	/// add a raycast against aabb first.
	
	std::vector<CollisionObject*>::iterator iter;
	
	for (iter=m_collisionObjects.begin();
	!(iter==m_collisionObjects.end()); iter++)
	{
		
		CollisionObject*	collisionObject= (*iter);

		//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
		SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
		collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);

		//check aabb overlap

		if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
		{
			RayTestSingle(rayFromTrans,rayToTrans,
				collisionObject,
					 collisionObject->m_collisionShape,
					  collisionObject->m_worldTransform,
					  resultCallback);
			
		}
	}

}

void renderme()
{
	float m[16];
	int i;

	for (i=0;i<numObjects;i++)
	{
		SimdTransform transA;
		transA.setIdentity();
		
		float pos[3];
		float rot[4];

		ms[i].getWorldPosition(pos[0],pos[1],pos[2]);
		ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]);

		SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]);
		transA.setRotation(q);

		SimdPoint3 dpos;
		dpos.setValue(pos[0],pos[1],pos[2]);

		transA.setOrigin( dpos );
		transA.getOpenGLMatrix( m );
		
		SimdVector3 wireColor(0.f,0.f,1.f); //wants deactivation

		///color differently for active, sleeping, wantsdeactivation states
		if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active
		{
			wireColor = SimdVector3 (1.f,0.f,0.f);
		}
		if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
		{
			wireColor = SimdVector3 (0.f,1.f,0.f);
		}

		char	extraDebug[125];
		//sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
		shapePtr[shapeIndex[i]]->SetExtraDebugInfo(extraDebug);
		GL_ShapeDrawer::DrawOpenGL(m,shapePtr[shapeIndex[i]],wireColor,getDebugMode());
	}

}

    virtual void	AddConvexVerticesCollider(std::vector<SimdVector3>& vertices, bool isEntity, const SimdVector3& entityTargetLocation)
    {
        ///perhaps we can do something special with entities (isEntity)
        ///like adding a collision Triggering (as example)

        if (vertices.size() > 0)
        {
            bool isDynamic = false;
            float mass = 0.f;
            SimdTransform startTransform;
            //can use a shift
            startTransform.setIdentity();
            startTransform.setOrigin(SimdVector3(0,0,-10.f));
            //this create an internal copy of the vertices
            CollisionShape* shape = new ConvexHullShape(&vertices[0],vertices.size());

            m_demoApp->LocalCreatePhysicsObject(isDynamic, mass, startTransform,shape);
        }
    }

int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
													float pivotX,float pivotY,float pivotZ,
													float axisX,float axisY,float axisZ)
{


	CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
	CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;

	RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
	RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;

	ASSERT(rb0);

	SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
	SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
	SimdVector3 axisInA(axisX,axisY,axisZ);
	SimdVector3 axisInB = rb1 ? 
		(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
	rb0->getCenterOfMassTransform().getBasis() * axisInA;

	bool angularOnly = false;

	switch (type)
	{
	case PHY_POINT2POINT_CONSTRAINT:
		{

			Point2PointConstraint* p2p = 0;

			if (rb1)
			{
				p2p = new Point2PointConstraint(*rb0,
					*rb1,pivotInA,pivotInB);
			} else
			{
				p2p = new Point2PointConstraint(*rb0,
					pivotInA);
			}

			m_constraints.push_back(p2p);
			p2p->SetUserConstraintId(gConstraintUid++);
			p2p->SetUserConstraintType(type);
			//64 bit systems can't cast pointer to int. could use size_t instead.
			return p2p->GetUserConstraintId();

			break;
		}

	case PHY_GENERIC_6DOF_CONSTRAINT:
		{
			Generic6DofConstraint* genericConstraint = 0;

			if (rb1)
			{
				SimdTransform frameInA;
				SimdTransform frameInB;
				
				SimdVector3 axis1, axis2;
				SimdPlaneSpace1( axisInA, axis1, axis2 );

				frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
					                          axisInA.y(), axis1.y(), axis2.y(),
											  axisInA.z(), axis1.z(), axis2.z() );

	
				SimdPlaneSpace1( axisInB, axis1, axis2 );
				frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
					                          axisInB.y(), axis1.y(), axis2.y(),
											  axisInB.z(), axis1.z(), axis2.z() );

				frameInA.setOrigin( pivotInA );
				frameInB.setOrigin( pivotInB );

				genericConstraint = new Generic6DofConstraint(
					*rb0,*rb1,
					frameInA,frameInB);


			} else
			{
				// TODO: Implement single body case...

			}
			

			m_constraints.push_back(genericConstraint);
			genericConstraint->SetUserConstraintId(gConstraintUid++);
			genericConstraint->SetUserConstraintType(type);
			//64 bit systems can't cast pointer to int. could use size_t instead.
			return genericConstraint->GetUserConstraintId();

			break;
		}
	case PHY_ANGULAR_CONSTRAINT:
		angularOnly = true;


	case PHY_LINEHINGE_CONSTRAINT:
		{
//.........这里部分代码省略.........

void Raytracer::displayCallback() 
{

	updateCamera();

	for (int i=0;i<numObjects;i++)
	{
		transforms[i].setIdentity();
		SimdVector3	pos(-3.5f+i*2.5f,0.f,0.f);
		transforms[i].setOrigin( pos );
		SimdQuaternion orn;
		if (i < 2)
		{
			orn.setEuler(yaw,pitch,roll);
			transforms[i].setRotation(orn);
		}
	}
	myMink.SetTransformA(SimdTransform(transforms[0].getRotation()));

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	glDisable(GL_LIGHTING);
	if (once)
	{
		glGenTextures(1, &glTextureId);
		glBindTexture(GL_TEXTURE_2D,glTextureId );
		once = 0;
		glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	}



	glDisable(GL_TEXTURE_2D);
	glDisable(GL_BLEND);

#define RAYTRACER
#ifdef RAYTRACER






	SimdVector4 rgba(1.f,0.f,0.f,0.5f);

	float top = 1.f;
	float bottom = -1.f;
	float nearPlane = 1.f;

	float tanFov = (top-bottom)*0.5f / nearPlane;

	float fov = 2.0 * atanf (tanFov);


	SimdVector3	rayFrom = getCameraPosition();
	SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
	rayForward.normalize();
	float farPlane = 600.f;
	rayForward*= farPlane;

	SimdVector3 rightOffset;
	SimdVector3 vertical(0.f,1.f,0.f);
	SimdVector3 hor;
	hor = rayForward.cross(vertical);
	hor.normalize();
	vertical = hor.cross(rayForward);
	vertical.normalize();

	float tanfov = tanf(0.5f*fov);

	hor *= 2.f * farPlane * tanfov;
	vertical *= 2.f * farPlane * tanfov;

	SimdVector3 rayToCenter = rayFrom + rayForward;

	SimdVector3 dHor = hor * 1.f/float(screenWidth);
	SimdVector3 dVert = vertical * 1.f/float(screenHeight);

	SimdTransform rayFromTrans;
	rayFromTrans.setIdentity();
	rayFromTrans.setOrigin(rayFrom);

	SimdTransform rayFromLocal;
	SimdTransform	rayToLocal;


	SphereShape pointShape(0.0f);


	///clear texture
	for (int x=0;x<screenWidth;x++)
	{
		for (int y=0;y<screenHeight;y++)
		{
			SimdVector4 rgba(0.f,0.f,0.f,0.f);
			raytracePicture->SetPixel(x,y,rgba);
		}
//.........这里部分代码省略.........

bool BU_CollisionPair::calcTimeOfImpact(
					const SimdTransform& fromA,
					const SimdTransform& toA,
					const SimdTransform& fromB,
					const SimdTransform& toB,
					CastResult& result)
{



	
	SimdVector3 linvelA,angvelA;
	SimdVector3 linvelB,angvelB;

	SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
	SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);


	SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
	SimdQuaternion angularMotionA(0,0,0,1.f);
	SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
	SimdQuaternion angularMotionB(0,0,0,1);
	


	result.m_fraction = 1.f;

	SimdTransform impactTransA;
	SimdTransform impactTransB;

	int index=0;

	SimdScalar toiUnscaled=result.m_fraction;
	const SimdScalar toiUnscaledLimit = result.m_fraction;

	SimdTransform a2w;
	a2w = fromA;
	SimdTransform b2w = fromB;

/* debugging code
	{
		const int numvertsB = m_convexB->GetNumVertices();
		for (int v=0;v<numvertsB;v++)
		{
			SimdPoint3 pt;
			m_convexB->GetVertex(v,pt);
			pt = b2w * pt;
			char buf[1000];

			if (pt.y() < 0.)
			{
				sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			} else
			{
				sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);

			}
		}
	}
*/


	SimdTransform b2wp = b2w;
	
	b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
	b2wp.setRotation( b2w.getRotation() + angularMotionB);

	impactTransB = b2wp;
	
	SimdTransform a2wp;
	a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
	a2wp.setRotation(a2w.getRotation()+angularMotionA);

	impactTransA = a2wp;

	SimdTransform a2winv;
	a2winv = a2w.inverse();

	SimdTransform b2wpinv;
	b2wpinv = b2wp.inverse();

	SimdTransform b2winv;
	b2winv = b2w.inverse();

	SimdTransform a2wpinv;
	a2wpinv = a2wp.inverse();

		//Redon's version with concatenated transforms

	SimdTransform relative;

	relative = b2w * b2wpinv * a2wp * a2winv;

	//relative = a2winv * a2wp  * b2wpinv * b2w;

	SimdQuaternion qrel;
//.........这里部分代码省略.........

void ConvexDecompositionDemo::initPhysics(const char* filename)
{
	ConvexDecomposition::WavefrontObj wo;

	tcount = wo.loadObj(filename);

	CollisionDispatcher* dispatcher = new	CollisionDispatcher();


	SimdVector3 worldAabbMin(-10000,-10000,-10000);
	SimdVector3 worldAabbMax(10000,10000,10000);

	OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
	//OverlappingPairCache* broadphase = new SimpleBroadphase();

	m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
	m_physicsEnvironmentPtr->setDeactivationTime(2.f);

	m_physicsEnvironmentPtr->setGravity(0,-10,0);

	SimdTransform startTransform;
	startTransform.setIdentity();
	startTransform.setOrigin(SimdVector3(0,-4,0));

	LocalCreatePhysicsObject(false,0,startTransform,new BoxShape(SimdVector3(30,2,30)));

	class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
	{

		ConvexDecompositionDemo*	m_convexDemo;
		public:

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

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

				TriangleMesh* trimesh = new TriangleMesh();

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

				//export data to .obj
				printf("ConvexResult\n");
				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);
					if ( 1 )
					{
						const unsigned int *src = result.mHullIndices;
						for (unsigned int i=0; i<result.mHullTcount; i++)
						{
							unsigned int index0 = *src++;
							unsigned int index1 = *src++;
							unsigned int index2 = *src++;
							SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
							SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
							SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
							vertex0 *= localScaling;
							vertex1 *= localScaling;
							vertex2 *= localScaling;
							centroid += vertex0;
							centroid += vertex1;
							centroid += vertex2;
							
						}
					}

					centroid *= 1.f/(float(result.mHullTcount) * 3);

					if ( 1 )
					{
						const unsigned int *src = result.mHullIndices;
						for (unsigned int i=0; i<result.mHullTcount; i++)
						{
							unsigned int index0 = *src++;
							unsigned int index1 = *src++;
							unsigned int index2 = *src++;


							SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
							SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
//.........这里部分代码省略.........

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

				TriangleMesh* trimesh = new TriangleMesh();

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

				//export data to .obj
				printf("ConvexResult\n");
				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);
					if ( 1 )
					{
						const unsigned int *src = result.mHullIndices;
						for (unsigned int i=0; i<result.mHullTcount; i++)
						{
							unsigned int index0 = *src++;
							unsigned int index1 = *src++;
							unsigned int index2 = *src++;
							SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
							SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
							SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
							vertex0 *= localScaling;
							vertex1 *= localScaling;
							vertex2 *= localScaling;
							centroid += vertex0;
							centroid += vertex1;
							centroid += vertex2;
							
						}
					}

					centroid *= 1.f/(float(result.mHullTcount) * 3);

					if ( 1 )
					{
						const unsigned int *src = result.mHullIndices;
						for (unsigned int i=0; i<result.mHullTcount; i++)
						{
							unsigned int index0 = *src++;
							unsigned int index1 = *src++;
							unsigned int index2 = *src++;


							SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
							SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
							SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
							vertex0 *= localScaling;
							vertex1 *= localScaling;
							vertex2 *= localScaling;
							
							vertex0 -= centroid;
							vertex1 -= centroid;
							vertex2 -= centroid;

							trimesh->AddTriangle(vertex0,vertex1,vertex2);

							index0+=mBaseCount;
							index1+=mBaseCount;
							index2+=mBaseCount;
							
							fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
						}
					}

					bool isDynamic = true;
					float mass = 1.f;
					CollisionShape* convexShape = new ConvexTriangleMeshShape(trimesh);
					SimdTransform trans;
					trans.setIdentity();
					trans.setOrigin(centroid);
					m_convexDemo->LocalCreatePhysicsObject(isDynamic, mass, trans,convexShape);

					mBaseCount+=result.mHullVcount; // advance the 'base index' counter.


				}
			}