static void outputPlaneToStream(PxShape* planeShape, const PxRigidActor* actor, const PxTransform& globalPose, IntArray& geomStream, TriArray& worldTriangles, IntArray& triIndicesArray,
								const PxExtendedVec3& origin, const PxBounds3& tmpBounds, const CCTParams& params, Cm::RenderBuffer* renderBuffer)
{
	PX_ASSERT(planeShape->getGeometryType() == PxGeometryType::ePLANE);

	const PxF32 length = (tmpBounds.maximum - tmpBounds.minimum).magnitude();
	PxVec3 center = toVec3(origin);

	const PxPlane plane = PxPlaneEquationFromTransform(globalPose);

	PxVec3 right, up;
	Ps::computeBasis(plane.n, right, up);
	right *= length;
	up *= length;

	const PxVec3 p = plane.project(center);
	const PxVec3 p0 = p - right + up;
	const PxVec3 p1 = p - right - up;
	const PxVec3 p2 = p + right - up;
	const PxVec3 p3 = p + right + up;

	const PxU32 nbTouchedTris = 2;

	const PxVec3 offset(float(-origin.x), float(-origin.y), float(-origin.z));

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geomStream, sizeof(TouchedMesh)/sizeof(PxU32));
	touchedMesh->mType					= TouchedGeomType::eMESH;
	touchedMesh->mTGUserData			= planeShape;
	touchedMesh->mActor					= actor;
	touchedMesh->mOffset				= origin;
	touchedMesh->mNbTris				= nbTouchedTris;
	touchedMesh->mIndexWorldTriangles	= worldTriangles.size();

	// Reserve memory for incoming triangles
	PxTriangle* TouchedTriangles = reserve(worldTriangles, nbTouchedTris);

	triIndicesArray.pushBack(0);
	triIndicesArray.pushBack(1);

	TouchedTriangles[0].verts[0] = p0 + offset;
	TouchedTriangles[0].verts[1] = p1 + offset;
	TouchedTriangles[0].verts[2] = p2 + offset;

	TouchedTriangles[1].verts[0] = p0 + offset;
	TouchedTriangles[1].verts[1] = p2 + offset;
	TouchedTriangles[1].verts[2] = p3 + offset;

	if(gVisualizeTouchedTris)
		visualizeTouchedTriangles(touchedMesh->mNbTris, touchedMesh->mIndexWorldTriangles, &worldTriangles[0], renderBuffer, offset, params.mUpDirection);
}
static void outputConvexToStream(PxShape* convexShape, const PxTransform& absPose, IntArray& geomStream, TriArray& worldTriangles, IntArray& triIndicesArray,
								 const PxExtendedVec3& origin, const PxBounds3& tmpBounds)
{
	PX_ASSERT(convexShape->getGeometryType() == PxGeometryType::eCONVEXMESH);
	PxConvexMeshGeometry cg;
	convexShape->getConvexMeshGeometry(cg);
	PX_ASSERT(cg.convexMesh);

	// Do AABB-mesh query

	PxU32* TF;

	// Collide AABB against current mesh
	// The overlap function doesn't exist for convexes so let's just dump all tris
	PxConvexMesh& cm = *cg.convexMesh;

	// PT: convex triangles are not exposed anymore so we need to access convex polygons & triangulate them

	// PT: TODO: this is copied from "DrawObjects", move this to a shared place. Actually a helper directly in PxConvexMesh would be useful.
	PxU32 Nb = 0;
	{
		const PxU32 nbPolys = cm.getNbPolygons();
		const PxU8* polygons = cm.getIndexBuffer();

		for(PxU32 i=0;i<nbPolys;i++)
		{
			PxHullPolygon data;
			cm.getPolygonData(i, data);
			Nb += data.mNbVerts - 2;
		}

		// PT: revisit this code. We don't use the polygon offset?
		TF = (PxU32*)PxAlloca(sizeof(PxU32)*Nb*3);
		PxU32* t = TF;
		for(PxU32 i=0;i<nbPolys;i++)
		{
			PxHullPolygon data;
			cm.getPolygonData(i, data);

			const PxU32 nbV = data.mNbVerts;

			const PxU32 nbTris = nbV - 2;
			const PxU8 vref0 = *polygons;
			for(PxU32 j=0;j<nbTris;j++)
			{
				const PxU32 vref1 = polygons[(j+1)%nbV];
				const PxU32 vref2 = polygons[(j+2)%nbV];
				*t++ = vref0;
				*t++ = vref1;
				*t++ = vref2;
			}
			polygons += nbV;
		}
	}

	const PxVec3* verts = cm.getVertices();

	PxVec3 tmp = absPose.p;	// LOSS OF ACCURACY
	PxVec3 MeshOffset;
	MeshOffset.x = float(tmp.x - origin.x);
	MeshOffset.y = float(tmp.y - origin.y);
	MeshOffset.z = float(tmp.z - origin.z);

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geomStream, sizeof(TouchedMesh)/sizeof(PxU32));
	touchedMesh->mType					= TouchedGeomType::eMESH;
	touchedMesh->mUserData				= convexShape;
	touchedMesh->mOffset				= origin;
	touchedMesh->mNbTris				= Nb;
	touchedMesh->mIndexWorldTriangles	= worldTriangles.size();

	// Reserve memory for incoming triangles
	PxTriangle* TouchedTriangles = reserve(worldTriangles, Nb);

	// Loop through touched triangles
	while(Nb--)
	{
		// Compute triangle in world space, add to array
		PxTriangle& CurrentTriangle = *TouchedTriangles++;

		const PxU32 vref0 = *TF++;
		const PxU32 vref1 = *TF++;
		const PxU32 vref2 = *TF++;

		PxVec3 v0 = verts[vref0];
		PxVec3 v1 = verts[vref1];
		PxVec3 v2 = verts[vref2];
		v0 = absPose.q.rotate(v0);
		v1 = absPose.q.rotate(v1);
		v2 = absPose.q.rotate(v2);

		CurrentTriangle.verts[0] = v0;
		CurrentTriangle.verts[1] = v1;
		CurrentTriangle.verts[2] = v2;

		CurrentTriangle.verts[0] += MeshOffset;
		CurrentTriangle.verts[1] += MeshOffset;
		CurrentTriangle.verts[2] += MeshOffset;

		triIndicesArray.pushBack(PX_INVALID_U32);
	}
}
static void outputHeightFieldToStream(	PxShape* hfShape, const PxTransform& heightfieldPose, IntArray& geomStream, TriArray& worldTriangles, IntArray& triIndicesArray,
										const PxExtendedVec3& origin, const PxBounds3& tmpBounds, const CCTParams& params, Cm::RenderBuffer* renderBuffer)
{
	PX_ASSERT(hfShape->getGeometryType() == PxGeometryType::eHEIGHTFIELD);
	// Do AABB-mesh query

	PxHeightFieldGeometry hfGeom;
	hfShape->getHeightFieldGeometry(hfGeom);

	PxBoxGeometry boxGeom(tmpBounds.getExtents());
	PxTransform boxPose;
	boxPose.p = tmpBounds.getCenter();
	boxPose.q = PxQuat::createIdentity();

	// Collide AABB against current heightfield
	PxFindOverlapTriangleMeshUtil overlapUtil;
	const PxU32 nbTouchedTris = overlapUtil.findOverlap(boxGeom, boxPose, hfGeom, heightfieldPose);

	const PxVec3 offset(float(-origin.x), float(-origin.y), float(-origin.z));

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geomStream, sizeof(TouchedMesh)/sizeof(PxU32));
	touchedMesh->mType					= TouchedGeomType::eMESH; // ptchernev: seems to work
	touchedMesh->mUserData				= hfShape;
	touchedMesh->mOffset				= origin;
	touchedMesh->mNbTris				= nbTouchedTris;
	touchedMesh->mIndexWorldTriangles	= worldTriangles.size();

	const PxU32* PX_RESTRICT indices = overlapUtil.getResults();

	if(params.mSlopeLimit!=0.0f)
	{
		// Reserve memory for incoming triangles
//		PxTriangle* TouchedTriangles = reserve(worldTriangles, nbTouchedTris);

		// Loop through touched triangles
		PxU32 nbCreatedTris = 0;
		for(PxU32 i=0; i < nbTouchedTris; i++)
		{
			const PxU32 triangleIndex = indices[i];

			// Compute triangle in world space, add to array
			PxTriangle currentTriangle;
			PxMeshQuery::getTriangle(hfGeom, heightfieldPose, triangleIndex, currentTriangle);
			currentTriangle.verts[0] += offset;
			currentTriangle.verts[1] += offset;
			currentTriangle.verts[2] += offset;

			const PxU32 nbNewTris = createInvisibleWalls(params, currentTriangle, worldTriangles, triIndicesArray);
			nbCreatedTris += nbNewTris;
			if(!nbNewTris)
			{
				worldTriangles.pushBack(currentTriangle);

				triIndicesArray.pushBack(triangleIndex);
				nbCreatedTris++;
			}
		}
		touchedMesh->mNbTris = nbCreatedTris;
	}
	else
	{
		// Reserve memory for incoming triangles
		PxTriangle* TouchedTriangles = reserve(worldTriangles, nbTouchedTris);

		// Loop through touched triangles
		for(PxU32 i=0; i < nbTouchedTris; i++)
		{
			const PxU32 triangleIndex = indices[i];

			// Compute triangle in world space, add to array
			PxTriangle& currentTriangle = *TouchedTriangles++;
			PxMeshQuery::getTriangle(hfGeom, heightfieldPose, triangleIndex, currentTriangle);
			currentTriangle.verts[0] += offset;
			currentTriangle.verts[1] += offset;
			currentTriangle.verts[2] += offset;

			triIndicesArray.pushBack(triangleIndex);
		}
	}

	if(gVisualizeTouchedTris)
		visualizeTouchedTriangles(touchedMesh->mNbTris, touchedMesh->mIndexWorldTriangles, &worldTriangles[0], renderBuffer, offset, params.mUpDirection);
}
static PxU32 createInvisibleWalls(const CCTParams& params, const PxTriangle& currentTriangle, TriArray& worldTriangles, IntArray& triIndicesArray)
{
	const PxF32 wallHeight = params.mInvisibleWallHeight;
	if(wallHeight==0.0f)
		return 0;

	PxU32 nbNewTris = 0;	// Number of newly created tris

	const PxVec3& upDirection = params.mUpDirection;

	PxVec3 normal;
	currentTriangle.normal(normal);
	if(testSlope(normal, upDirection, params.mSlopeLimit))
	{
		const PxVec3 upWall = upDirection*wallHeight;
		PxVec3 v0p = currentTriangle.verts[0] +	upWall;
		PxVec3 v1p = currentTriangle.verts[1] +	upWall;
		PxVec3 v2p = currentTriangle.verts[2] +	upWall;

		// Extrude edge 0-1
		PxVec3 faceNormal01;
		{
			// 0-1-0p
			const PxTriangle tri0_1_0p(currentTriangle.verts[0], currentTriangle.verts[1], v0p);
			worldTriangles.pushBack(tri0_1_0p);

			// 0p-1-1p
			const PxTriangle tri0p_1_1p(v0p, currentTriangle.verts[1], v1p);
			worldTriangles.pushBack(tri0p_1_1p);

			tri0p_1_1p.normal(faceNormal01);
		}

		// Extrude edge 1-2
		PxVec3 faceNormal12;
		{
			// 1p-1-2p
			const PxTriangle tri1p_1_2p(v1p, currentTriangle.verts[1], v2p);
			worldTriangles.pushBack(tri1p_1_2p);

			// 2p-1-2
			const PxTriangle tri2p_1_2(v2p, currentTriangle.verts[1], currentTriangle.verts[2]);
			worldTriangles.pushBack(tri2p_1_2);

			tri2p_1_2.normal(faceNormal12);
		}

		// Extrude edge 2-0
		PxVec3 faceNormal20;
		{
			// 0p-2-0
			const PxTriangle tri0p_2_0(v0p, currentTriangle.verts[2], currentTriangle.verts[0]);
			worldTriangles.pushBack(tri0p_2_0);

			// 0p-2p-2
			const PxTriangle tri0p_2p_2(v0p, v2p, currentTriangle.verts[2]);
			worldTriangles.pushBack(tri0p_2p_2);

			tri0p_2p_2.normal(faceNormal20);
		}

		const PxU32 triIndex = PX_INVALID_U32;
		for(PxU32 i=0;i<6;i++)
			triIndicesArray.pushBack(triIndex);

		nbNewTris += 6;
	}
	return nbNewTris;
}
static void outputBoxToStream(PxShape* boxShape, const PxTransform& globalPose, IntArray& geomStream, TriArray& worldTriangles, IntArray& triIndicesArray,
								const PxExtendedVec3& origin, const PxBounds3& tmpBounds, const CCTParams& params, Cm::RenderBuffer* renderBuffer)
{
	PX_ASSERT(boxShape->getGeometryType() == PxGeometryType::eBOX);
	PxBoxGeometry bg;
	boxShape->getBoxGeometry(bg);

	//8 verts in local space.
	const PxF32 dx = bg.halfExtents.x;
	const PxF32 dy = bg.halfExtents.y;
	const PxF32 dz = bg.halfExtents.z;
	PxVec3 boxVerts[8]=
	{
		PxVec3(-dx,-dy,-dz),
		PxVec3(+dx,-dy,-dz),
		PxVec3(+dx,+dy,-dz),
		PxVec3(-dx,+dy,-dz),
		PxVec3(-dx,-dy,+dz),
		PxVec3(+dx,-dy,+dz),
		PxVec3(+dx,+dy,+dz),
		PxVec3(-dx,+dy,+dz)
	};
	//Transform verts into world space.
	PxVec3 pxOrigin = toVec3(origin);
	for(PxU32 i = 0; i < 8; i++)
	{
		boxVerts[i] = globalPose.transform(boxVerts[i]) - pxOrigin;
	}

	//Index of triangles.
	PxU32 boxTris[12][3]=
	{
		{0,2,1},
		{2,0,3},	//0,1,2,3

		{3,6,2},
		{6,3,7},	//3,2,6,7
		
		{7,5,6},
		{5,7,4},	//7,6,5,4

		{4,1,5},
		{1,4,0},	//4,5,1,0

		{0,7,3},
		{7,0,4},	//0,3,7,4

		{2,5,1},
		{5,2,6}		//2,1,5,6
	};

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geomStream, sizeof(TouchedMesh)/sizeof(PxU32));
	touchedMesh->mType					= TouchedGeomType::eMESH;
	touchedMesh->mUserData				= boxShape;
	touchedMesh->mOffset				= origin;
	touchedMesh->mNbTris				= 12;
	touchedMesh->mIndexWorldTriangles	= worldTriangles.size();

	// Reserve memory for incoming triangles
	PxTriangle* TouchedTriangles = reserve(worldTriangles, 12);

	for(PxU32 i = 0; i < 12; i++)
	{
		PxTriangle& CurrentTriangle = TouchedTriangles[i];

		CurrentTriangle.verts[0] = boxVerts[boxTris[i][0]];
		CurrentTriangle.verts[1] = boxVerts[boxTris[i][1]];
		CurrentTriangle.verts[2] = boxVerts[boxTris[i][2]];

		triIndicesArray.pushBack(PX_INVALID_U32);
	}
}
Пример #6
0
static void outputConvexToStream(
	NxConvexShape* convexShape,
	NxShape* shape,
	IntArray& geom_stream,
	TriArray& world_triangles,
	TriArray* world_edge_normals,
	IntArray& edge_flags,
	const NxExtendedVec3& origin,
	const NxBounds3& tmpBounds
	)
	{
	// Do AABB-mesh query

	NxU32 Nb;
	const NxU32* TF;

	// Collide AABB against current mesh
	// The overlap function doesn't exist for convexes so let's just dump all tris
	NxConvexMesh& cm = convexShape->getConvexMesh();
	Nb = cm.getCount(0, NX_ARRAY_TRIANGLES);
	NX_ASSERT(cm.getFormat(0, NX_ARRAY_TRIANGLES)==NX_FORMAT_INT);
	TF = (const NxU32*)cm.getBase(0, NX_ARRAY_TRIANGLES);
	NX_ASSERT(cm.getFormat(0, NX_ARRAY_VERTICES)==NX_FORMAT_FLOAT);
	const NxVec3* verts = (const NxVec3*)cm.getBase(0, NX_ARRAY_VERTICES);

	NxMat34 absPose = shape->getGlobalPose();

	NxVec3 tmp = shape->getGlobalPosition();	// LOSS OF ACCURACY
	NxVec3 MeshOffset;
	MeshOffset.x = float(tmp.x - origin.x);
	MeshOffset.y = float(tmp.y - origin.y);
	MeshOffset.z = float(tmp.z - origin.z);

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geom_stream, sizeof(TouchedMesh)/sizeof(NxU32));
	touchedMesh->mType					= TOUCHED_MESH;
	touchedMesh->mUserData				= shape;
	touchedMesh->mOffset				= origin;
	touchedMesh->mNbTris				= Nb;
	touchedMesh->mIndexWorldTriangles	= world_triangles.size();
	touchedMesh->mIndexWorldEdgeNormals	= world_edge_normals ? world_edge_normals->size() : 0;
	touchedMesh->mIndexEdgeFlags		= edge_flags.size();

	// Reserve memory for incoming triangles
	NxTriangle* TouchedTriangles = reserve(world_triangles, Nb);
	NxTriangle* EdgeTriangles = world_edge_normals ? reserve(*world_edge_normals, Nb) : NULL;

	// Loop through touched triangles
	while(Nb--)
		{
		// Compute triangle in world space, add to array
		NxTriangle& CurrentTriangle = *TouchedTriangles++;

		NxU32 vref0 = *TF++;
		NxU32 vref1 = *TF++;
		NxU32 vref2 = *TF++;

		NxVec3 v0 = verts[vref0];
		NxVec3 v1 = verts[vref1];
		NxVec3 v2 = verts[vref2];
		absPose.M.multiply(v0, v0);
		absPose.M.multiply(v1, v1);
		absPose.M.multiply(v2, v2);

		CurrentTriangle.verts[0] = v0;
		CurrentTriangle.verts[1] = v1;
		CurrentTriangle.verts[2] = v2;

		CurrentTriangle.verts[0] += MeshOffset;
		CurrentTriangle.verts[1] += MeshOffset;
		CurrentTriangle.verts[2] += MeshOffset;

		if(EdgeTriangles)
			{
			// #### hmmm
			NxTriangle edgeTri;
			edgeTri.verts[0].zero();
			edgeTri.verts[1].zero();
			edgeTri.verts[2].zero();

			*EdgeTriangles++ = edgeTri;
			}

		// #### hmmm
		NxU32 edgeFlags = 7;
		edge_flags.pushBack(edgeFlags);
		}
	}
Пример #7
0
static void outputHeightFieldToStream(
	NxHeightFieldShape* hfShape,
	NxShape* shape,
	IntArray& geom_stream,
	TriArray& world_triangles,
	TriArray* world_edge_normals,
	IntArray& edge_flags,
	const NxExtendedVec3& origin,
	const NxBounds3& tmpBounds
	)
	{
	// Do AABB-mesh query

	NxU32 Nb;
	const NxU32* TF;

	// Collide AABB against current mesh
	if(!hfShape->overlapAABBTriangles(tmpBounds, NX_QUERY_WORLD_SPACE, Nb, TF))
		return;

	NxVec3 tmp = shape->getGlobalPosition();	// LOSS OF ACCURACY
	NxVec3 MeshOffset;
	MeshOffset.x = float(tmp.x - origin.x);
	MeshOffset.y = float(tmp.y - origin.y);
	MeshOffset.z = float(tmp.z - origin.z);

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geom_stream, sizeof(TouchedMesh)/sizeof(NxU32));
	touchedMesh->mType					= TOUCHED_MESH; // ptchernev: seems to work
	touchedMesh->mUserData				= shape;
	touchedMesh->mOffset				= origin;
	touchedMesh->mNbTris				= Nb;
	touchedMesh->mIndexWorldTriangles	= world_triangles.size();
	touchedMesh->mIndexWorldEdgeNormals	= world_edge_normals ? world_edge_normals->size() : 0;
	touchedMesh->mIndexEdgeFlags		= edge_flags.size();

	// Reserve memory for incoming triangles
	NxTriangle* TouchedTriangles = reserve(world_triangles, Nb);
	NxTriangle* EdgeTriangles = world_edge_normals ? reserve(*world_edge_normals, Nb) : NULL;

	// Loop through touched triangles
	while(Nb--)
		{
		NxU32 Index = *TF++;
		// Compute triangle in world space, add to array
		NxTriangle& CurrentTriangle = *TouchedTriangles++;

		NxTriangle edgeTri;
		NxU32 edgeFlags;
		hfShape->getTriangle(CurrentTriangle, &edgeTri, &edgeFlags, Index, false);
		CurrentTriangle.verts[0] += MeshOffset;
		CurrentTriangle.verts[1] += MeshOffset;
		CurrentTriangle.verts[2] += MeshOffset;

		if(EdgeTriangles)
			{
			*EdgeTriangles++ = edgeTri;
			}

		edge_flags.pushBack(edgeFlags);

#ifdef VISUALIZE_CCT_TRIS
		// Visualize debug triangles
			{
			//		PhysicsSDK::getInstance().getDebugRenderable()->addTriangle((const NxVec3&)CurrentTriangle.mVerts[0], (const NxVec3&)CurrentTriangle.mVerts[1], (const NxVec3&)CurrentTriangle.mVerts[2], NX_ARGB_GREEN);
			NxGetPhysicsSDK()->getDebugRenderable()->addTriangle((const NxVec3&)CurrentTriangle.mVerts[0], (const NxVec3&)CurrentTriangle.mVerts[1], (const NxVec3&)CurrentTriangle.mVerts[2], NX_ARGB_GREEN);

			//		PhysicsSDK::getInstance().getDebugRenderable()->addLine((const NxVec3&)CurrentTriangle.mVerts[0], (const NxVec3&)CurrentTriangle.mVerts[1], NX_ARGB_GREEN);
			//		PhysicsSDK::getInstance().getDebugRenderable()->addLine((const NxVec3&)CurrentTriangle.mVerts[1], (const NxVec3&)CurrentTriangle.mVerts[2], NX_ARGB_GREEN);
			//		PhysicsSDK::getInstance().getDebugRenderable()->addLine((const NxVec3&)CurrentTriangle.mVerts[2], (const NxVec3&)CurrentTriangle.mVerts[0], NX_ARGB_GREEN);
			}
#endif
		}
	}
static void outputConvexToStream(PxShape* convexShape, const PxRigidActor* actor, const PxTransform& absPose_, IntArray& geomStream, TriArray& worldTriangles, IntArray& triIndicesArray,
								 const PxExtendedVec3& origin, const PxBounds3& tmpBounds, const CCTParams& params, Cm::RenderBuffer* renderBuffer, PxU16& nbTessellation)
{
	PX_ASSERT(convexShape->getGeometryType() == PxGeometryType::eCONVEXMESH);
	PxConvexMeshGeometry cg;
	convexShape->getConvexMeshGeometry(cg);
	PX_ASSERT(cg.convexMesh);

	// Do AABB-mesh query

	PxU32* TF;

	// Collide AABB against current mesh
	// The overlap function doesn't exist for convexes so let's just dump all tris
	PxConvexMesh& cm = *cg.convexMesh;

	// PT: convex triangles are not exposed anymore so we need to access convex polygons & triangulate them

	// PT: TODO: this is copied from "DrawObjects", move this to a shared place. Actually a helper directly in PxConvexMesh would be useful.
	PxU32 Nb = 0;
	{
		const PxU32 nbPolys = cm.getNbPolygons();
		const PxU8* polygons = cm.getIndexBuffer();

		for(PxU32 i=0;i<nbPolys;i++)
		{
			PxHullPolygon data;
			cm.getPolygonData(i, data);
			Nb += data.mNbVerts - 2;
		}

		// PT: revisit this code. We don't use the polygon offset?
		TF = (PxU32*)PxAlloca(sizeof(PxU32)*Nb*3);
		PxU32* t = TF;
		for(PxU32 i=0;i<nbPolys;i++)
		{
			PxHullPolygon data;
			cm.getPolygonData(i, data);

			const PxU32 nbV = data.mNbVerts;

			const PxU32 nbTris = nbV - 2;
			const PxU8 vref0 = *polygons;
			for(PxU32 j=0;j<nbTris;j++)
			{
				const PxU32 vref1 = polygons[(j+1)%nbV];
				const PxU32 vref2 = polygons[(j+2)%nbV];
				*t++ = vref0;
				*t++ = vref1;
				*t++ = vref2;
			}
			polygons += nbV;
		}
	}

	// PT: you can't use PxTransform with a non-uniform scaling
	const PxMat33 rot = PxMat33(absPose_.q) * cg.scale.toMat33();
	const PxMat44 absPose(rot, absPose_.p);

	const PxVec3 p = absPose.getPosition();
	const PxVec3 MeshOffset(float(p.x - origin.x), float(p.y - origin.y), float(p.z - origin.z));	// LOSS OF ACCURACY

	const PxVec3 offset(float(-origin.x), float(-origin.y), float(-origin.z));

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geomStream, sizeof(TouchedMesh)/sizeof(PxU32));
	touchedMesh->mType					= TouchedGeomType::eMESH;
	touchedMesh->mTGUserData			= convexShape;
	touchedMesh->mActor					= actor;
	touchedMesh->mOffset				= origin;
	touchedMesh->mIndexWorldTriangles	= worldTriangles.size();

	const PxVec3* verts = cm.getVertices();
	// Loop through touched triangles
	if(params.mTessellation)
	{
		const PxBoxGeometry boxGeom(tmpBounds.getExtents());
		const PxBounds3 cullingBox = PxBounds3::centerExtents(tmpBounds.getCenter() + offset, boxGeom.halfExtents);

		PxU32 nbCreatedTris = 0;
		while(Nb--)
		{
			// Compute triangle in world space, add to array
			PxTriangle currentTriangle;

			const PxU32 vref0 = *TF++;
			const PxU32 vref1 = *TF++;
			const PxU32 vref2 = *TF++;

			currentTriangle.verts[0] = MeshOffset + absPose.rotate(verts[vref0]);
			currentTriangle.verts[1] = MeshOffset + absPose.rotate(verts[vref1]);
			currentTriangle.verts[2] = MeshOffset + absPose.rotate(verts[vref2]);

			PxU32 nbNewTris = 0;
			tessellateTriangle(nbNewTris, currentTriangle, PX_INVALID_U32, worldTriangles, triIndicesArray, cullingBox, params, nbTessellation);
			nbCreatedTris += nbNewTris;
		}
		touchedMesh->mNbTris = nbCreatedTris;
	}
	else
	{
		// Reserve memory for incoming triangles
		PxTriangle* TouchedTriangles = reserve(worldTriangles, Nb);

		touchedMesh->mNbTris = Nb;
		while(Nb--)
		{
			// Compute triangle in world space, add to array
			PxTriangle& currentTriangle = *TouchedTriangles++;

			const PxU32 vref0 = *TF++;
			const PxU32 vref1 = *TF++;
			const PxU32 vref2 = *TF++;

			currentTriangle.verts[0] = MeshOffset + absPose.rotate(verts[vref0]);
			currentTriangle.verts[1] = MeshOffset + absPose.rotate(verts[vref1]);
			currentTriangle.verts[2] = MeshOffset + absPose.rotate(verts[vref2]);

			triIndicesArray.pushBack(PX_INVALID_U32);
		}
	}
	if(gVisualizeTouchedTris)
		visualizeTouchedTriangles(touchedMesh->mNbTris, touchedMesh->mIndexWorldTriangles, &worldTriangles[0], renderBuffer, offset, params.mUpDirection);
}
static void outputMeshToStream(	PxShape* meshShape, const PxRigidActor* actor, const PxTransform& meshPose, IntArray& geomStream, TriArray& worldTriangles, IntArray& triIndicesArray,
								const PxExtendedVec3& origin, const PxBounds3& tmpBounds, const CCTParams& params, Cm::RenderBuffer* renderBuffer, PxU16& nbTessellation)
{
	PX_ASSERT(meshShape->getGeometryType() == PxGeometryType::eTRIANGLEMESH);
	// Do AABB-mesh query

	PxTriangleMeshGeometry triGeom;
	meshShape->getTriangleMeshGeometry(triGeom);

	const PxBoxGeometry boxGeom(tmpBounds.getExtents());
	const PxTransform boxPose(tmpBounds.getCenter(), PxQuat(PxIdentity));

	// Collide AABB against current mesh
	PxMeshOverlapUtil overlapUtil;
	const PxU32 nbTouchedTris = overlapUtil.findOverlap(boxGeom, boxPose, triGeom, meshPose);

	const PxVec3 offset(float(-origin.x), float(-origin.y), float(-origin.z));

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geomStream, sizeof(TouchedMesh)/sizeof(PxU32));
	touchedMesh->mType					= TouchedGeomType::eMESH;
	touchedMesh->mTGUserData			= meshShape;
	touchedMesh->mActor					= actor;
	touchedMesh->mOffset				= origin;
	touchedMesh->mNbTris				= nbTouchedTris;
	touchedMesh->mIndexWorldTriangles	= worldTriangles.size();

	const PxU32* PX_RESTRICT indices = overlapUtil.getResults();

	if(params.mSlopeLimit!=0.0f)
	{
		if(!params.mTessellation)
		{
			// Loop through touched triangles
			PxU32 nbCreatedTris = 0;
			for(PxU32 i=0; i < nbTouchedTris; i++)
			{
				const PxU32 triangleIndex = indices[i];

				// Compute triangle in world space, add to array
				PxTriangle currentTriangle;
				PxMeshQuery::getTriangle(triGeom, meshPose, triangleIndex, currentTriangle);
				currentTriangle.verts[0] += offset;
				currentTriangle.verts[1] += offset;
				currentTriangle.verts[2] += offset;

				const PxU32 nbNewTris = createInvisibleWalls(params, currentTriangle, worldTriangles, triIndicesArray);
				nbCreatedTris += nbNewTris;
				if(!nbNewTris)
				{
					worldTriangles.pushBack(currentTriangle);

					triIndicesArray.pushBack(triangleIndex);
					nbCreatedTris++;
				}
			}
			touchedMesh->mNbTris = nbCreatedTris;
		}
		else
		{
			const PxBounds3 cullingBox = PxBounds3::centerExtents(boxPose.p + offset, boxGeom.halfExtents);

			// Loop through touched triangles
			PxU32 nbCreatedTris = 0;
			for(PxU32 i=0; i < nbTouchedTris; i++)
			{
				const PxU32 triangleIndex = indices[i];

				// Compute triangle in world space, add to array
				PxTriangle currentTriangle;
				PxMeshQuery::getTriangle(triGeom, meshPose, triangleIndex, currentTriangle);
				currentTriangle.verts[0] += offset;
				currentTriangle.verts[1] += offset;
				currentTriangle.verts[2] += offset;

				PxU32 nbNewTris = createInvisibleWalls(params, currentTriangle, worldTriangles, triIndicesArray);
				nbCreatedTris += nbNewTris;
				if(!nbNewTris)
				{
/*					worldTriangles.pushBack(currentTriangle);
					triIndicesArray.pushBack(triangleIndex);
					nbCreatedTris++;*/

					tessellateTriangle(nbNewTris, currentTriangle, triangleIndex, worldTriangles, triIndicesArray, cullingBox, params, nbTessellation);
					nbCreatedTris += nbNewTris;
//					printf("Tesselate: %d new tris\n", nbNewTris);
				}
			}
			touchedMesh->mNbTris = nbCreatedTris;
		}
	}
	else
	{
		if(!params.mTessellation)
		{
			// Reserve memory for incoming triangles
			PxTriangle* TouchedTriangles = reserve(worldTriangles, nbTouchedTris);

			// Loop through touched triangles
			for(PxU32 i=0; i < nbTouchedTris; i++)
			{
				const PxU32 triangleIndex = indices[i];

				// Compute triangle in world space, add to array
				PxTriangle& currentTriangle = *TouchedTriangles++;
				PxMeshQuery::getTriangle(triGeom, meshPose, triangleIndex, currentTriangle);
				currentTriangle.verts[0] += offset;
				currentTriangle.verts[1] += offset;
				currentTriangle.verts[2] += offset;

				triIndicesArray.pushBack(triangleIndex);
			}
		}
		else
		{
			const PxBounds3 cullingBox = PxBounds3::centerExtents(boxPose.p + offset, boxGeom.halfExtents);

			PxU32 nbCreatedTris = 0;
			for(PxU32 i=0; i < nbTouchedTris; i++)
			{
				const PxU32 triangleIndex = indices[i];

				// Compute triangle in world space, add to array
				PxTriangle currentTriangle;
				PxMeshQuery::getTriangle(triGeom, meshPose, triangleIndex, currentTriangle);

				currentTriangle.verts[0] += offset;
				currentTriangle.verts[1] += offset;
				currentTriangle.verts[2] += offset;

				PxU32 nbNewTris = 0;
				tessellateTriangle(nbNewTris, currentTriangle, triangleIndex, worldTriangles, triIndicesArray, cullingBox, params, nbTessellation);
//				printf("Tesselate: %d new tris\n", nbNewTris);
				nbCreatedTris += nbNewTris;
			}
			touchedMesh->mNbTris = nbCreatedTris;
		}
	}

	if(gVisualizeTouchedTris)
		visualizeTouchedTriangles(touchedMesh->mNbTris, touchedMesh->mIndexWorldTriangles, &worldTriangles[0], renderBuffer, offset, params.mUpDirection);
}
static void outputBoxToStream(	PxShape* boxShape, const PxRigidActor* actor, const PxTransform& globalPose, IntArray& geomStream, TriArray& worldTriangles, IntArray& triIndicesArray,
								const PxExtendedVec3& origin, const PxBounds3& tmpBounds, const CCTParams& params, PxU16& nbTessellation)
{
	PX_ASSERT(boxShape->getGeometryType() == PxGeometryType::eBOX);
	PxBoxGeometry bg;
	boxShape->getBoxGeometry(bg);

	//8 verts in local space.
	const PxF32 dx = bg.halfExtents.x;
	const PxF32 dy = bg.halfExtents.y;
	const PxF32 dz = bg.halfExtents.z;
	PxVec3 boxVerts[8]=
	{
		PxVec3(-dx,-dy,-dz),
		PxVec3(+dx,-dy,-dz),
		PxVec3(+dx,+dy,-dz),
		PxVec3(-dx,+dy,-dz),
		PxVec3(-dx,-dy,+dz),
		PxVec3(+dx,-dy,+dz),
		PxVec3(+dx,+dy,+dz),
		PxVec3(-dx,+dy,+dz)
	};
	//Transform verts into world space.
	const PxVec3 pxOrigin = toVec3(origin);
	for(PxU32 i = 0; i < 8; i++)
	{
		boxVerts[i] = globalPose.transform(boxVerts[i]) - pxOrigin;
	}

	//Index of triangles.
	const PxU32 boxTris[12][3]=
	{
		{0,2,1},
		{2,0,3},	//0,1,2,3

		{3,6,2},
		{6,3,7},	//3,2,6,7
		
		{7,5,6},
		{5,7,4},	//7,6,5,4

		{4,1,5},
		{1,4,0},	//4,5,1,0

		{0,7,3},
		{7,0,4},	//0,3,7,4

		{2,5,1},
		{5,2,6}		//2,1,5,6
	};

	TouchedMesh* touchedMesh			= (TouchedMesh*)reserve(geomStream, sizeof(TouchedMesh)/sizeof(PxU32));
	touchedMesh->mType					= TouchedGeomType::eMESH;
	touchedMesh->mTGUserData			= boxShape;
	touchedMesh->mActor					= actor;
	touchedMesh->mOffset				= origin;
	touchedMesh->mIndexWorldTriangles	= worldTriangles.size();

	if(params.mTessellation)
	{
		const PxBoxGeometry boxGeom(tmpBounds.getExtents());
		const PxVec3 offset(float(-origin.x), float(-origin.y), float(-origin.z));
		const PxBounds3 cullingBox = PxBounds3::centerExtents(tmpBounds.getCenter() + offset, boxGeom.halfExtents);

		PxU32 nbCreatedTris = 0;
		for(PxU32 i=0; i<12; i++)
		{
			// Compute triangle in world space, add to array
			const PxTriangle currentTriangle(boxVerts[boxTris[i][0]], boxVerts[boxTris[i][1]], boxVerts[boxTris[i][2]]);

			PxU32 nbNewTris = 0;
			tessellateTriangle(nbNewTris, currentTriangle, PX_INVALID_U32, worldTriangles, triIndicesArray, cullingBox, params, nbTessellation);
			nbCreatedTris += nbNewTris;
		}
		touchedMesh->mNbTris = nbCreatedTris;
	}
	else
	{
		touchedMesh->mNbTris = 12;

		// Reserve memory for incoming triangles
		PxTriangle* TouchedTriangles = reserve(worldTriangles, 12);

		for(PxU32 i=0; i<12; i++)
		{
			PxTriangle& currentTriangle = TouchedTriangles[i];
			currentTriangle.verts[0] = boxVerts[boxTris[i][0]];
			currentTriangle.verts[1] = boxVerts[boxTris[i][1]];
			currentTriangle.verts[2] = boxVerts[boxTris[i][2]];

			triIndicesArray.pushBack(PX_INVALID_U32);
		}
	}
}