CWE-ou-minkata/Sources/Tools/MaxMain/plPhysXCooking.cpp

/*==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;
}