You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

499 lines
15 KiB

/*==LICENSE==*
CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011 Cyan Worlds, Inc.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
You can contact Cyan Worlds, Inc. by email legal@cyan.com
or by snail mail at:
Cyan Worlds, Inc.
14617 N Newport Hwy
Mead, WA 99021
*==LICENSE==*/
#include "plPhysXCooking.h"
#include "hsGeometry3.h"
#include "../plPhysX/plSimulationMgr.h"
#include "../plPhysX/plPXStream.h"
#include "../plPhysX/plPXConvert.h"
#include "hsSTLStream.h"
#include "Nx.h"
#include "NxStream.h"
#include "NxPhysics.h"
#include "NxCooking.h"
#include "NxPlane.h"
#include "NxUtilLib.h"
#include "NxMat33.h"
bool plPhysXCooking::fSkipErrors = false;
NxUtilLib* plPhysXCooking::fUtilLib =nil;
//assumes that the Vectors are normalized
hsBool ThreePlaneIntersect(const NxVec3& norm0, const NxVec3& point0,
const NxVec3& norm1, const NxVec3& point1,
const NxVec3& norm2, const NxVec3& point2, NxVec3& loc)
{
//need to make sure these planes aren't parallel
hsBool suc=0;
NxVec3 cross=norm1.cross( norm2);
hsScalar denom=norm0.dot(cross);
if(abs(denom)<0.0001) return 0;//basically paralell
// if we are here there must be a point in 3 space
try{
hsScalar d1,d2,d3;
d1=norm0.dot(point0);
d2=norm1.dot(point1);
d3=norm2.dot(point2);
NxVec3 n1Xn2=norm1.cross(norm2);
NxVec3 n2Xn0=norm2.cross(norm0);
NxVec3 n0Xn1=norm0.cross(norm1);
NxVec3 pos=(d1*n1Xn2+ d2*n2Xn0 + d3*n0Xn1)/(denom);
loc.x=pos.x;
loc.y=pos.y;
loc.z=pos.z;
suc= 1;
}
catch(...)
{
suc=0;
}
return suc;
}
void plPhysXCooking::Init()
{
NxInitCooking();
NxUtilLib* fUtilLib=NxGetUtilLib();
NxCookingParams parms=NxGetCookingParams();
parms.skinWidth=.05;
NxSetCookingParams(parms);
}
void plPhysXCooking::Shutdown()
{
NxCloseCooking();
fUtilLib=nil;
fSkipErrors = false;
}
hsVectorStream* plPhysXCooking::CookTrimesh(int nVerts, hsPoint3* verts, int nFaces, UInt16* faces)
{
NxTriangleMeshDesc triDesc;
triDesc.numVertices = nVerts;
triDesc.pointStrideBytes = sizeof(hsPoint3);
triDesc.points = verts;
triDesc.numTriangles = nFaces;
triDesc.triangleStrideBytes = sizeof(UInt16) * 3;
triDesc.triangles = faces;
triDesc.flags = NX_MF_16_BIT_INDICES;
hsVectorStream* ram = TRACKED_NEW hsVectorStream;
plPXStream buf(ram);
bool status = NxCookTriangleMesh(triDesc, buf);
hsAssert(status, "Trimesh failed to cook");
if (status)
{
ram->Rewind();
return ram;
}
else
{
delete ram;
return nil;
}
}
bool plPhysXCooking::IsPointInsideHull(hsPlane3* hull, int nPlanes, const hsPoint3& pos)
{
int i;
for( i = 0; i < nPlanes; i++ )
{
// add a fudge to the point so not to trip on the ever so slightly concave
// ... so pull the point out in the direction of the normal of the plane we are testing.
hsPoint3 fudgepos = pos + (hull[i].GetNormal()*0.005f);
if (!ITestPlane(fudgepos, hull[i]))
return false;
}
return true;
}
bool plPhysXCooking::TestIfConvex(NxConvexMesh* convexMesh, int nVerts, hsPoint3* verts)
{
bool retVal = true;
// build planes from the convex mesh
NxConvexMeshDesc desc;
convexMesh->saveToDesc(desc);
hsPlane3* planes = TRACKED_NEW hsPlane3[desc.numTriangles];
int i;
for ( i = 0; i < desc.numTriangles; i++)
{
UInt32* triangle = (UInt32*)(((char*)desc.triangles) + desc.triangleStrideBytes*i);
float* vertex1 = (float*)(((char*)desc.points) + desc.pointStrideBytes*triangle[0]);
float* vertex2 = (float*)(((char*)desc.points) + desc.pointStrideBytes*triangle[1]);
float* vertex3 = (float*)(((char*)desc.points) + desc.pointStrideBytes*triangle[2]);
hsPoint3 pt1(vertex1[0],vertex1[1],vertex1[2]);
hsPoint3 pt2(vertex2[0],vertex2[1],vertex2[2]);
hsPoint3 pt3(vertex3[0],vertex3[1],vertex3[2]);
planes[i] = hsPlane3(&pt1,&pt2,&pt3);
}
// now see if any of the points from the mesh are inside the hull
for (int j=0; j<nVerts && retVal; j++)
{
if ( IsPointInsideHull(planes,desc.numTriangles,verts[j]) )
retVal = false;
}
delete [] planes;
return retVal;
}
hsVectorStream* plPhysXCooking::CookHull(int nVerts, hsPoint3* verts, bool inflate)
{
NxConvexMeshDesc convexDesc;
convexDesc.numVertices = nVerts;
convexDesc.pointStrideBytes = sizeof(hsPoint3);
convexDesc.points = verts;
convexDesc.flags = NX_CF_COMPUTE_CONVEX;
if(inflate)
{
convexDesc.flags|= NX_CF_INFLATE_CONVEX ;
}
hsVectorStream* ram = TRACKED_NEW hsVectorStream;
plPXStream buf(ram);
bool status = NxCookConvexMesh(convexDesc, buf);
hsAssert(status, "Convex mesh failed to cook");
if (status)
{
ram->Rewind();
return ram;
}
else
{
delete ram;
return nil;
}
}
/*
NxTriangleMesh* ReadExplicit(hsStream* stream)
{
const int nVertices = stream->ReadSwap32();
hsPoint3* pVertices = TRACKED_NEW hsPoint3[nVertices];
stream->ReadSwapScalar(nVertices*3, (float*)pVertices);
const int nFaces = stream->ReadSwap32();
unsigned short* pTriangles = TRACKED_NEW unsigned short[nFaces * 3];
stream->ReadSwap16(nFaces * 3, pTriangles);
NxTriangleMeshDesc triDesc;
triDesc.numVertices = nVertices;
triDesc.pointStrideBytes = sizeof(hsPoint3);
triDesc.points = pVertices;
triDesc.numTriangles = nFaces;
triDesc.triangleStrideBytes = sizeof(UInt16) * 3;
triDesc.triangles = pTriangles;
triDesc.flags = NX_MF_16_BIT_INDICES;// | NX_MF_FLIPNORMALS;
hsRAMStream ram;
plNxStream buf(&ram);
NxInitCooking();
bool status = NxCookTriangleMesh(triDesc, buf);
hsAssert(status, "Trimesh failed to cook");
NxCloseCooking();
delete[] pVertices;
delete[] pTriangles;
if (status)
{
ram.Rewind();
return plSimulationMgr::GetInstance()->GetSDK()->createTriangleMesh(buf);
}
return nil;
}
NxConvexMesh* ReadConvexHull(hsStream* stream)
{
const int nVertices = stream->ReadSwap32();
hsPoint3* pVertices = TRACKED_NEW hsPoint3[nVertices];
stream->ReadSwapScalar(nVertices*3, (float*)pVertices);
NxConvexMeshDesc convexDesc;
convexDesc.numVertices = nVertices;
convexDesc.pointStrideBytes = sizeof(hsPoint3);
convexDesc.points = pVertices;
convexDesc.flags = NX_CF_COMPUTE_CONVEX;
hsRAMStream ram;
plNxStream buf(&ram);
NxInitCooking();
bool status = NxCookConvexMesh(convexDesc, buf);
hsAssert(status, "Convex mesh failed to cook");
NxCloseCooking();
delete[] pVertices;
if (status)
{
ram.Rewind();
return plSimulationMgr::GetInstance()->GetSDK()->createConvexMesh(buf);
}
return nil;
}
void ReadBoxFromHull(hsStream* stream, NxBoxShapeDesc& box)
{
const int nVertices = stream->ReadSwap32();
hsPoint3* pVertices = TRACKED_NEW hsPoint3[nVertices];
stream->ReadSwapScalar(nVertices*3, (float*)pVertices);
hsScalar minX, minY, minZ, maxX, maxY, maxZ;
minX = minY = minZ = FLT_MAX;
maxX = maxY = maxZ = -FLT_MAX;
for (int i = 0; i < nVertices; i++)
{
hsPoint3& vec = pVertices[i];
minX = hsMinimum(minX, vec.fX);
minY = hsMinimum(minY, vec.fY);
minZ = hsMinimum(minZ, vec.fZ);
maxX = hsMaximum(maxX, vec.fX);
maxY = hsMaximum(maxY, vec.fY);
maxZ = hsMaximum(maxZ, vec.fZ);
}
delete[] pVertices;
float xWidth = maxX - minX;
float yWidth = maxY - minY;
float zWidth = maxZ - minZ;
box.dimensions.x = xWidth / 2;
box.dimensions.y = yWidth / 2;
box.dimensions.z = zWidth / 2;
// hsMatrix44 mat;
// box.localPose.getRowMajor44(&mat.fMap[0][0]);
// hsPoint3 trans(minX + (xWidth / 2), minY + (yWidth / 2), minY + (yWidth / 2));
// mat.SetTranslate(&trans);
// box.localPose.setRowMajor44(&mat.fMap[0][0]);
}
*/
hsBool ProjectPointOnToPlane(const hsVector3& planeNormal,hsScalar& d0,
const hsVector3 pointToProject, hsPoint3& res)
{
NxVec3 vec=plPXConvert::Vector(planeNormal);
NxVec3 orig,projected;
orig=plPXConvert::Vector(pointToProject);
NxPlane* pl=new NxPlane(vec,d0);
projected=pl->project(orig);
res.fX=projected.x;
res.fY=projected.y;
res.fZ=projected.z;
return 1;
}
void plPhysXCooking::PCA(const NxVec3* points,int numPoints, NxMat33& out)
{
NxVec3 mean(0.f,0.f,0.f);
hsScalar Cov[3][3];
memset(Cov,0,9* sizeof hsScalar);
for(int i=0; i<numPoints;i++)
{
mean+=points[i];
}
mean=mean/(hsScalar)numPoints;
for(int i=0;i<numPoints;i++)
{
Cov[0][0]+=pow(points[i].x-mean.x ,2.0f)/(hsScalar)(numPoints);
Cov[1][1]+=pow(points[i].y-mean.y ,2.0f)/(hsScalar)(numPoints);
Cov[2][2]+=pow(points[i].z-mean.z ,2.0f)/(hsScalar)(numPoints);
Cov[0][1]+=(points[i].x-mean.x)*(points[i].y-mean.y)/(hsScalar)(numPoints);
Cov[0][2]+=(points[i].x-mean.x)*(points[i].z-mean.z)/(hsScalar)(numPoints);
Cov[1][2]+=(points[i].y-mean.y)*(points[i].z-mean.z)/(hsScalar)(numPoints);
}
Cov[2][0]=Cov[0][2];
Cov[1][0]=Cov[0][1];
Cov[2][1]=Cov[1][2];
NxF32 Covun[9];
for(int i=0;i<3;i++)
{
for(int j=0; j<3;j++)
{
Covun[3*i +j]=Cov[i][j];
}
}
NxVec3 eigenVals;
NxMat33 CovNx,Rot;
CovNx.setRowMajor(Covun);
if(fUtilLib==nil)Init();
NxDiagonalizeInertiaTensor(CovNx,eigenVals,out);
}
hsVectorStream* plPhysXCooking::IMakePolytope(const plMaxMeshExtractor::NeutralMesh& inMesh)
{
hsBool success=0;
std::vector<hsPoint3> outCloud;
hsPoint3 offset;
int numPlanes=26;
float planeMax[26];
int indexMax[26];
hsPoint3 AABBMin(FLT_MAX,FLT_MAX,FLT_MAX);
hsPoint3 AABBMax(-FLT_MAX,-FLT_MAX,-FLT_MAX);
//prep
NxVec3* vectors = TRACKED_NEW NxVec3[26];
int curvec=0;
for(int xcomp= -1;xcomp<2;xcomp++)
{
for(int ycomp= -1;ycomp<2;ycomp++)
{
for(int zcomp= -1;zcomp<2;zcomp++)
{
if(!((xcomp==0)&&(ycomp==0)&&(zcomp==0)))
{
vectors[curvec].set((hsScalar)(xcomp),(hsScalar)(ycomp),(hsScalar)(zcomp));
vectors[curvec].normalize();
planeMax[curvec]=(-FLT_MAX);
//indexMax[curvec]=0;
curvec++;
}
}
}
}
/*
for(int i=0;i<26;i++)
{//make your max and mins
planeMax[i]=(-FLT_MAX);
}
*/
hsPoint3 centroid(0.0f,0.0f,0.0f);
for(int i=0;i<inMesh.fNumVerts;i++) centroid+=inMesh.fVerts[i];
centroid=centroid/(float)inMesh.fNumVerts;
//temp
NxVec3* nxLocs=new NxVec3[inMesh.fNumVerts];
NxVec3* nxLocs2=new NxVec3[inMesh.fNumVerts];
for(int i=0;i<inMesh.fNumVerts;i++)
{
hsPoint3 temppt=inMesh.fVerts[i] - centroid;
nxLocs[i]=plPXConvert::Point(temppt);
}
NxMat33 rot;
NxVec3 eigen;
PCA(nxLocs,inMesh.fNumVerts,rot);
NxMat33 invrot;
rot.getInverse(invrot);
for(int i=0; i<inMesh.fNumVerts;i++)
{
nxLocs2[i]=invrot*nxLocs[i];
}
for(int i=0;i<inMesh.fNumVerts;i++)
{
for(int plane=0;plane<26;plane++)
{
float dist=nxLocs2[i].dot(vectors[plane]);
if(dist>=planeMax[plane])
{
planeMax[plane]=dist;
indexMax[plane]=i;
}
}
}
for(int i=0;i<inMesh.fNumVerts;i++)
{
AABBMin.fX = hsMinimum(nxLocs2[i].x, AABBMin.fX);
AABBMin.fY = hsMinimum(nxLocs2[i].y, AABBMin.fY);
AABBMin.fZ = hsMinimum(nxLocs2[i].z, AABBMin.fZ);
AABBMax.fX = hsMaximum(nxLocs2[i].x, AABBMax.fX);
AABBMax.fY = hsMaximum(nxLocs2[i].y, AABBMax.fY);
AABBMax.fZ = hsMaximum(nxLocs2[i].z, AABBMax.fZ);
}
int resultingPoints=0;
for(int i=0;i<26;i++)
{
for(int j=0;j<26;j++)
{
for(int k=0;k<26;k++)
{
NxVec3 res;
if(ThreePlaneIntersect(vectors[i],nxLocs2[indexMax[i]],vectors[j],nxLocs2[indexMax[j]], vectors[k],nxLocs2[indexMax[k]],res))
{
//check it is within all slabs
bool within=true;
int curplane=0;
do
{
float intersecdist=res.dot(vectors[curplane]);
if((intersecdist-planeMax[curplane])>0.0001)
{
within=false;
}
curplane++;
}
while((curplane<26)&&within);
if(within)
// if((res.x>=AABBMin.fX)&&(res.x<=AABBMax.fX)&&
// (res.y>=AABBMin.fY)&&(res.y<=AABBMax.fY)&&
// (res.z>=AABBMin.fZ)&&(res.z<=AABBMax.fZ))
{
NxVec3 reverted;
reverted=rot*res;
reverted.x=reverted.x +centroid.fX;
reverted.y=reverted.y +centroid.fY;
reverted.z=reverted.z +centroid.fZ;
hsPoint3 out;
out=plPXConvert::Point(reverted);
outCloud.push_back(out);
}
}
}
}
}
//planes discovered
//this is'nt right
//cleanup
offset=centroid;
delete[] vectors;
hsPoint3* pointages=TRACKED_NEW hsPoint3[outCloud.size()];
for(int x=0;x<outCloud.size();x++)pointages[x]=outCloud[x];
hsVectorStream* vectorstrm;
vectorstrm= CookHull(outCloud.size(),pointages,true);
delete[] pointages;
delete[] nxLocs;
delete[] nxLocs2;
return vectorstrm;
}