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

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

}

void clientMoveAndDisplay()
{
	if ( !g_pauseAnim )
	{
		SimdMatrix3x3 rot;
		rot.setEulerZYX( g_animAngle * 0.05, g_animAngle * 0.05, g_animAngle * 0.05 );

		SimdTransform t;
		t.setIdentity();
		t.setBasis( rot );

		//g_convexShapesTransform[ 0 ].mult( g_convexShapesTransform[ 0 ], t );
		g_convexShapesTransform[ 1 ].mult( g_convexShapesTransform[ 1 ], t );

		g_shapesPenetrate = calcPenDepth();
	}

	clientDisplay();
}

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

void LinearConvexCastDemo::initPhysics()
{
	setCameraDistance(30.f);
	tr[0].setOrigin(SimdVector3(0,0,0));
	tr[1].setOrigin(SimdVector3(0,10,0));

	SimdMatrix3x3 basisA;
	basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
		-0.00029313788f,0.99753088f,0.070228584f,
		-0.00089153741f,-0.070228823f,0.99753052f);

	SimdMatrix3x3 basisB;
	basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f,
		4.4865495e-018f,0.97979438f,0.20000751f,
		4.4410586e-017f,-0.20000751f,0.97979438f);

	tr[0].setBasis(basisA);
	tr[1].setBasis(basisB);



	SimdVector3 boxHalfExtentsA(0.2,4,4);
	SimdVector3 boxHalfExtentsB(6,6,6);

	BoxShape*	boxA = new BoxShape(boxHalfExtentsA);
/*	BU_Simplex1to4	boxB;
	boxB.AddVertex(SimdPoint3(-5,0,-5));
	boxB.AddVertex(SimdPoint3(5,0,-5));
	boxB.AddVertex(SimdPoint3(0,0,5));
	boxB.AddVertex(SimdPoint3(0,5,0));
*/

	BoxShape*	boxB = new BoxShape(boxHalfExtentsB);
	shapePtr[0] = boxA;
	shapePtr[1] = boxB;

	shapePtr[0]->SetMargin(0.01f);
	shapePtr[1]->SetMargin(0.01f);

	SimdTransform tr;
	tr.setIdentity();
}

int main(int argc,char** argv)
{
    setCameraDistance(20.f);

    tr[0].setOrigin(SimdVector3(0.0013328250f,8.1363249f,7.0390840f));
    tr[1].setOrigin(SimdVector3(0.00000000f,9.1262732f,2.0343180f));

    //tr[0].setOrigin(SimdVector3(0,0,0));
    //tr[1].setOrigin(SimdVector3(0,10,0));

    SimdMatrix3x3 basisA;
    basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
                    -0.00029313788f,0.99753088f,0.070228584f,
                    -0.00089153741f,-0.070228823f,0.99753052f);

    SimdMatrix3x3 basisB;
    basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f,
                    4.4865495e-018f,0.97979438f,0.20000751f,
                    4.4410586e-017f,-0.20000751f,0.97979438f);

    tr[0].setBasis(basisA);
    tr[1].setBasis(basisB);



    SimdVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f);
    SimdVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f);

    BoxShape	boxA(boxHalfExtentsA);
    BoxShape	boxB(boxHalfExtentsB);
    shapePtr[0] = &boxA;
    shapePtr[1] = &boxB;


    SimdTransform tr;
    tr.setIdentity();


    return glutmain(argc, argv,screenWidth,screenHeight,"Collision Demo");
}

void	PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
{
	//not yet, return box inertia

	float margin = GetMargin();

	SimdTransform ident;
	ident.setIdentity();
	SimdVector3 aabbMin,aabbMax;
	GetAabb(ident,aabbMin,aabbMax);
	SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;

	SimdScalar lx=2.f*(halfExtents.x()+margin);
	SimdScalar ly=2.f*(halfExtents.y()+margin);
	SimdScalar lz=2.f*(halfExtents.z()+margin);
	const SimdScalar x2 = lx*lx;
	const SimdScalar y2 = ly*ly;
	const SimdScalar z2 = lz*lz;
	const SimdScalar scaledmass = mass * 0.08333333f;

	inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));

}

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);
		}
//.........这里部分代码省略.........

void clientDisplay(void) {

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glDisable(GL_LIGHTING);

    //GL_ShapeDrawer::DrawCoordSystem();

    float m[16];
    int i;

    GjkPairDetector	convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);

    SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
    convexConvex.SetCachedSeperatingAxis(seperatingAxis);

    PointCollector gjkOutput;
    GjkPairDetector::ClosestPointInput input;
    input.m_transformA = tr[0];
    input.m_transformB = tr[1];

    convexConvex.GetClosestPoints(input ,gjkOutput,0);

    if (gjkOutput.m_hasResult)
    {
        SimdVector3 endPt = gjkOutput.m_pointInWorld +
                            gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;

        glBegin(GL_LINES);
        glColor3f(1, 0, 0);
        glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
        glVertex3d(endPt.x(),endPt.y(),endPt.z());
        //glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
        //glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
        glEnd();

    }

    for (i=0; i<numObjects; i++)
    {

        tr[i].getOpenGLMatrix( m );

        GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode());


    }

    simplex.SetSimplexSolver(&sGjkSimplexSolver);
    SimdPoint3 ybuf[4],pbuf[4],qbuf[4];
    int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
    simplex.Reset();

    for (i=0; i<numpoints; i++)
        simplex.AddVertex(ybuf[i]);

    SimdTransform ident;
    ident.setIdentity();
    ident.getOpenGLMatrix(m);
    GL_ShapeDrawer::DrawOpenGL(m,&simplex,SimdVector3(1,1,1),getDebugMode());


    SimdQuaternion orn;
    orn.setEuler(yaw,pitch,roll);
    tr[0].setRotation(orn);

//	pitch += 0.005f;
//	yaw += 0.01f;

    glFlush();
    glutSwapBuffers();
}

			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.


				}
			}