source: GTP/trunk/Lib/Vis/Preprocessing/src/Mesh.cpp @ 2609

#include "Ray.h"
#include "Mesh.h"
#include "MeshKdTree.h"
#include "Triangle3.h"
#include "ResourceManager.h"

using namespace std;

namespace GtpVisibilityPreprocessor {

bool MeshDebug = false;

  //int Intersectable::sMailId = 1;//2147483647;
  //int Intersectable::sReservedMailboxes = 1;

struct SortableVertex {

  Vector3 vertex;

  int originalId;
  int newId;
  int finalPos;

  SortableVertex() {}

  SortableVertex(const Vector3 &v,
                                 const int id):
        vertex(v),
        originalId(id),
        newId(id)
  {}
  
  friend bool operator<(const SortableVertex &a,
                                                const SortableVertex &b)
  {
        if (a.vertex.x < b.vertex.x)
          return true;
        else
          if (a.vertex.x > b.vertex.x)
                return false;
        
        if (a.vertex.y < b.vertex.y)
          return true;
        else
          if (a.vertex.y > b.vertex.y)
                return false;
        
        if (a.vertex.z < b.vertex.z)
          return true;
        else
          //      if (a.z > b.z)
          return false;
        
        //      return false;
  }

};


Mesh::Mesh(const int vertices, const int faces):
mFaces(),
mMaterial(NULL),
mKdTree(NULL),
mVertices(),
mIsConvex(false),
mIsWatertight(false),
mId(0)
{
        mVertices.reserve(vertices);
        mFaces.reserve(faces);
}


void
Mesh::ComputeBoundingBox()
{

  mBox.Initialize();
  VertexContainer::const_iterator vi = mVertices.begin();
  for (; vi != mVertices.end(); vi++) {
    mBox.Include(*vi);
  }
//mBox.Enlarge(1e-4f);
}

void
Mesh::Preprocess(bool cleanup)
{
  if (cleanup)
        Cleanup();
 
        ComputeBoundingBox();
 
        /** true if it is a watertight convex mesh 
        */
        mIsConvex = false;

        if (mFaces.size() > MeshKdTree::mTermMinCost) 
        {
                mKdTree = new MeshKdTree(this);
                MeshKdLeaf *root = (MeshKdLeaf *)mKdTree->GetRoot();
                
                for (int i = 0; i < mFaces.size(); i++)
                        root->mFaces.push_back(i);
                
                cout<<"KD";
                
                mKdTree->Construct();

                if (mKdTree->GetRoot()->IsLeaf()) 
                {
                        cout<<"d";
                        delete mKdTree;
                        mKdTree = NULL;
                }
        }
}


void
Mesh::IndexVertices()
{
  int i;
  // check whether the vertices can be simplfied and reindexed
  vector<SortableVertex> svertices(mVertices.size());

  for (i=0; i < mVertices.size(); i++)
        svertices[i] = SortableVertex(mVertices[i], i);

  sort(svertices.begin(), svertices.end());

  for (i=0; i < svertices.size() - 1; i++)
        if (svertices[i].vertex == svertices[i+1].vertex)
          svertices[i+1].newId = svertices[i].newId;

  // remove the same vertices
  int k = 0;
  mVertices[0] = svertices[0].vertex;
  svertices[0].finalPos = 0;
  
  for (i=1; i < svertices.size(); i++) {
        if (svertices[i].newId != svertices[i-1].newId)
          k++;
        
        mVertices[k] = svertices[i].vertex;
        svertices[i].finalPos = k;
  }

  mVertices.resize(k + 1);
  
  vector<int> remapBuffer(svertices.size());
  for (i = 0; i < svertices.size(); i++)
        remapBuffer[svertices[i].originalId] = svertices[i].finalPos;
  
  // remap all faces
  
  for (int faceIndex = 0; faceIndex < mFaces.size(); faceIndex++) {
        Face *face = mFaces[faceIndex];
        for (int i = 0; i < face->mVertexIndices.size(); i++) {
          face->mVertexIndices[i] = remapBuffer[face->mVertexIndices[i]];
        }
  }
}

AxisAlignedBox3
Mesh::GetFaceBox(const int faceIndex)
{
  Face *face = mFaces[faceIndex];
  AxisAlignedBox3 box;
  box.SetMin( mVertices[face->mVertexIndices[0]] );
  box.SetMax(box.Min());
  for (int i = 1; i < face->mVertexIndices.size(); i++) {
    box.Include(mVertices[face->mVertexIndices[i]]);
  }
  return box;
}

int
Mesh::CastRayToFace(
                                        const int faceIndex,
                                        Ray &ray,
                                        float &nearestT,
                                        Vector3 &nearestNormal,
                                        int &nearestFace,
                                        Intersectable *instance
                                        )
{
  float t;
  int hit = 0;
  Vector3 normal;
  if (RayFaceIntersection(faceIndex, ray, t, normal, nearestT) == Ray::INTERSECTION) {
    switch (ray.GetType()) {
    case Ray::GLOBAL_RAY:
      ray.intersections.push_back(Ray::Intersection(t, normal, instance, faceIndex));
      hit++;
      break;
    case Ray::LOCAL_RAY:
      nearestT = t;
          nearestNormal = normal;
      nearestFace = faceIndex;
      hit++;
      break;
    case Ray::LINE_SEGMENT:
      if (t <= 1.0f) {
                ray.intersections.push_back(Ray::Intersection(t, normal, instance, faceIndex));
                hit++;
      }
      break;
    }
  }
  return hit;
}

int
Mesh::CastRay(
                          Ray &ray,
                          MeshInstance *instance
                          )
{
  if (mKdTree) {
    return mKdTree->CastRay(ray, instance);
  }
  
  int faceIndex = 0;
  int hits = 0;
  float nearestT = MAX_FLOAT;
  Vector3 nearestNormal;
  int nearestFace = -1;
  
  if (ray.GetType() == Ray::LOCAL_RAY && !ray.intersections.empty())
    nearestT = ray.intersections[0].mT;

        
  for ( ;
                faceIndex < mFaces.size();
                faceIndex++) {
    hits += CastRayToFace(faceIndex,
                                                  ray,
                                                  nearestT,
                                                  nearestNormal,
                                                  nearestFace,
                                                  instance);
    if (mIsConvex && nearestFace != -1)
      break;
  }
  
  if ( hits && ray.GetType() == Ray::LOCAL_RAY ) {
    if (!ray.intersections.empty())
      ray.intersections[0] = Ray::Intersection(nearestT,
                                                                                           nearestNormal,
                                                                                           instance,
                                                                                           nearestFace);
    else
      ray.intersections.push_back(Ray::Intersection(nearestT,
                                                                                                        nearestNormal,
                                                                                                        instance,
                                                                                                        nearestFace));
  }
  
  return hits;
}

int
Mesh::CastRayToSelectedFaces(
                                                         Ray &ray,
                                                         const vector<int> &faces,
                                                         Intersectable *instance
                                                         )
{
  vector<int>::const_iterator fi;
  int faceIndex = 0;
  int hits = 0;
  float nearestT = MAX_FLOAT;
  Vector3 nearestNormal;
  int nearestFace = -1;
  
  if (ray.GetType() == Ray::LOCAL_RAY && ray.intersections.size())
    nearestT = ray.intersections[0].mT;

  for ( fi = faces.begin();
                fi != faces.end();
                fi++) {
    hits += CastRayToFace(*fi, ray, nearestT, nearestNormal, nearestFace, instance);
    if (mIsConvex && nearestFace != -1)
      break;
  }
  
  if ( hits && ray.GetType() == Ray::LOCAL_RAY ) {
    if (ray.intersections.size())
      ray.intersections[0] = Ray::Intersection(nearestT, nearestNormal,instance, nearestFace);
    else
      ray.intersections.push_back(Ray::Intersection(nearestT, nearestNormal,instance, nearestFace));
  }
  
  return hits;
}


// int_lineseg returns 1 if the given line segment intersects a 2D
// ray travelling in the positive X direction.  This is used in the
// Jordan curve computation for polygon intersection.
inline int
int_lineseg(float px,
                        float py,
                        float u1,
                        float v1,
                        float u2,
                        float v2)
{
  float ydiff;

  u1 -= px; u2 -= px;     // translate line
  v1 -= py; v2 -= py;

  if ((v1 > 0 && v2 > 0) ||
      (v1 < 0 && v2 < 0) ||
      (u1 < 0 && u2 < 0))
    return 0;

  if (u1 > 0 && u2 > 0)
    return 1;

  ydiff = v2 - v1;
  if (fabs(ydiff) < Limits::Small) {      // denominator near 0
    if (((fabs(v1) > Limits::Small) ||
                 (u1 > 0) || (u2 > 0)))
      return 0;
    return 1;
  }
  
  double t = -v1 / ydiff;                 // Compute parameter

  double thresh;

  if (ydiff < 0.0f)
        thresh = -1e-20;
  else
        thresh = 1e-20;

  
  return (u1 + t * (u2 - u1)) > thresh; //-Limits::Small;
}



// intersection with the polygonal face of the mesh
int
Mesh::RayFaceIntersection(const int faceIndex,
                                                  const Ray &ray,
                                                  float &t,
                                                  Vector3 &normal,
                                                  const float nearestT
                                                  )
{
  Face *face  = mFaces[faceIndex];

  Plane3 plane = GetFacePlane(faceIndex);
  float dot = DotProd(plane.mNormal, ray.GetDir());

  if (MeshDebug) {
        cout<<endl<<endl;
        cout<<"normal="<<plane.mNormal<<endl;
        cout<<"dot="<<dot<<endl;
  }
  
        
  // Watch for near-zero denominator
  // ONLY single sided polygons!!!!!
  if (ray.mFlags & Ray::CULL_BACKFACES) {
        if (dot > -Limits::Small)
          //  if (fabs(dot) < Limits::Small)
          return Ray::NO_INTERSECTION;
  } else {
        if (fabs(dot) < Limits::Small)
          return Ray::NO_INTERSECTION;
  }
  
  normal = plane.mNormal;

  t = (-plane.mD - DotProd(plane.mNormal, ray.GetLoc())) / dot;

  if (MeshDebug)
        cout<<"t="<<t<<endl;

  if (t <= Limits::Small)
    return Ray::INTERSECTION_OUT_OF_LIMITS;
  
  if ((ray.GetType() == Ray::LOCAL_RAY) && (t >= nearestT)) {
    return Ray::INTERSECTION_OUT_OF_LIMITS; // no intersection was found
  }
  
  int count = 0;
  float u, v, u1, v1, u2, v2;
  int i;

  int paxis = plane.mNormal.DrivingAxis();

  
  // Project the intersection point onto the coordinate plane
  // specified by which.
  ray.Extrap(t).ExtractVerts(&u, &v, paxis);


  int size = (int)face->mVertexIndices.size();

  if (MeshDebug)
        cout<<"size="<<size<<endl;
  
  mVertices[face->mVertexIndices[size - 1]].
    ExtractVerts(&u1, &v1, paxis );
  
  //$$JB changed 12.4.2006 from 0 ^^
  if (0 && size <= 4) {
    // assume a convex face
    for (i = 0; i < size; i++) {
      mVertices[face->mVertexIndices[i]].ExtractVerts(&u2, &v2, paxis);
      // line u1, v1, u2, v2
      if ((v1 - v2)*(u - u1) + (u2 - u1)*(v - v1) > 0) {
                if (MeshDebug)
                  cout<<"exit on "<<i<<endl;
                return Ray::NO_INTERSECTION;
          }
      u1 = u2;
      v1 = v2;
    }
    
    return Ray::INTERSECTION;
  }
  
  // We're stuck with the Jordan curve computation.  Count number
  // of intersections between the line segments the polygon comprises
  // with a ray originating at the point of intersection and
  // travelling in the positive X direction.
  for (i = 0; i < size; i++) {
    mVertices[face->mVertexIndices[i]].ExtractVerts(&u2, &v2, paxis);
    count += (int_lineseg(u, v, u1, v1, u2, v2) != 0);
    u1 = u2;
    v1 = v2;
  }

  if (MeshDebug)
        cout<<"count="<<count<<endl;

  // We hit polygon if number of intersections is odd.
  return (count & 1) ? Ray::INTERSECTION : Ray::NO_INTERSECTION;
}

int
Mesh::GetRandomSurfacePoint(Vector3 &point, Vector3 &normal)
{
        //const int faceIndex = (int)RandomValue(0, (Real)((int)mFaces.size()-1));
        const int faceIndex = (int)RandomValue(0, (Real)mFaces.size() - 0.5f);

        // assume the face is convex and generate a convex combination
        Face *face = mFaces[faceIndex];

        point = Vector3(0,0,0);
        float sum = 0.0f;

        for (int i = 0; i < face->mVertexIndices.size(); i++) {
                float r = RandomValue(0,1);
                sum += r;
                point += mVertices[face->mVertexIndices[i]]*r;
        }
        point *= 1.0f/sum;

        normal = GetFacePlane(faceIndex).mNormal;

        return faceIndex;
}

int
Mesh::GetRandomVisibleSurfacePoint(Vector3 &point,
                                                                   Vector3 &normal,
                                                                   const Vector3 &viewpoint,
                                                                   const int maxTries)
{
  Plane3 plane;
  const int faceIndex = (int)RandomValue(0, (Real)mFaces.size() - 0.5f);
        int tries;
  for (tries = 0; tries < maxTries; tries++) {
    Face *face = mFaces[faceIndex];
    plane = GetFacePlane(faceIndex);
    
    if (plane.Side(viewpoint) > 0) {
      point = Vector3(0,0,0);
      float sum = 0.0f;
      // pickup a point inside this triangle
      for (int i = 0; i < face->mVertexIndices.size(); i++) {
                                float r = RandomValue(0,1);
                                sum += r;
                                point += mVertices[face->mVertexIndices[i]]*r;
      }
      point *= 1.0f/sum;
      break;
    }
  }
  
  normal = plane.mNormal;
  return (tries < maxTries) ? faceIndex + 1 : 0;
}


Plane3
Mesh::GetFacePlane(const int faceIndex) const
{
  Face *face = mFaces[faceIndex];
        return Plane3(mVertices[face->mVertexIndices[0]],
                                                                mVertices[face->mVertexIndices[1]],
                                                                mVertices[face->mVertexIndices[2]]);
}

bool
Mesh::ValidateFace(const int i)
{
        Face *face = mFaces[i];

        Plane3 plane = Plane3(mVertices[face->mVertexIndices[0]],
                                                  mVertices[face->mVertexIndices[1]],
                                                  mVertices[face->mVertexIndices[2]]);
        
        if (!eq(Magnitude(plane.mNormal), 1.0f))
                return false;

        return true;
}


bool Mesh::CheckMesh() const
{
        VertexContainer::const_iterator vit, vit_end = mVertices.end();

        for (vit = mVertices.begin(); vit != vit_end; ++ vit)
        {
                for (int i = 0; i < 3; ++ i)
                {
                        const float x = (*vit)[i];
                        if (x != x) // nan
                        //if (isnan((*vit)[i]))
                        {
                                cout << "warning: vertex is ill defined" << endl;
                                return false;
                        }
                }
        }
        return true;
}


static void PrintFace(const Mesh *mesh, const int idx, ostream &app)
{
        Face *face = mesh->mFaces[idx];

        VertexIndexContainer::iterator it, it_end = face->mVertexIndices.end();

        cout << "face " << idx << endl;
        for (it = face->mVertexIndices.begin(); it != it_end; ++ it)
        {
                cout << (*it) << ": " << mesh->mVertices[*it] << " ";
        }
        cout << endl;
}


void Mesh::Print(ostream &app) const
{
        VertexContainer::const_iterator vit, vit_end = mVertices.end();

        for (vit = mVertices.begin(); vit != vit_end; ++ vit)
        {
                app << (*vit) << " ";
        }
        app << endl;

        for (int i = 0; i < (int)mFaces.size(); ++ i)
        {
                PrintFace(this, i, app);
        }
}


void
Mesh::Cleanup()
{
        int toRemove = 0;
        FaceContainer newFaces;
        for (int i=0; i < mFaces.size(); i++)
                if (ValidateFace(i)) {
                        newFaces.push_back(mFaces[i]);
                } else {
                        cout<<"d";
                        delete mFaces[i];
                        toRemove++;
                }
        
        if (toRemove) {
                mFaces = newFaces;
        }
        
        // cleanup vertices??
}


Vector3 Mesh::GetNormal(const int idx) const
{
        return GetFacePlane(idx).mNormal;
}


void
Mesh::AddTriangle(const Triangle3 &triangle)
{
  int index = (int)mVertices.size();

  for (int i=0; i < 3; i++) {
    mVertices.push_back(triangle.mVertices[i]);
  }
  
  AddFace(new Face(index + 0, index + 1, index + 2) );
}

void
Mesh::AddRectangle(const Rectangle3 &rect)
{
  int index = (int)mVertices.size();

  for (int i=0; i < 4; i++) {
    mVertices.push_back(rect.mVertices[i]);
  }
  
  AddFace(new Face(index + 0, index + 1, index + 2, index + 3) );
}

void
Mesh::AssignRandomMaterial()
{  
        mMaterial = MaterialManager::GetSingleton()->CreateResource();
  
        Material randMat = RandomMaterial();

        mMaterial->mDiffuseColor = randMat.mDiffuseColor;
        mMaterial->mSpecularColor = randMat.mSpecularColor;
        mMaterial->mAmbientColor = randMat.mAmbientColor;
}


Mesh *CreateMeshFromBox(const AxisAlignedBox3 &box)
{
        Mesh *mesh = MeshManager::GetSingleton()->CreateResource();

        // add 8 vertices of the box
        const int index = (int)mesh->mVertices.size();
        
        for (int i=0; i < 8; ++ i) 
        {
        Vector3 v;
                box.GetVertex(i, v);
                mesh->mVertices.push_back(v);
        }

        mesh->AddFace(new Face(index + 0, index + 1, index + 3, index + 2) );
        mesh->AddFace(new Face(index + 0, index + 2, index + 6, index + 4) );
        mesh->AddFace(new Face(index + 4, index + 6, index + 7, index + 5) );
  
        mesh->AddFace(new Face(index + 3, index + 1, index + 5, index + 7) );
        mesh->AddFace(new Face(index + 0, index + 4, index + 5, index + 1) );
        mesh->AddFace(new Face(index + 2, index + 3, index + 7, index + 6) );
  
        return mesh;
}


Mesh::Mesh(const int id, const int vertices, const int faces):
mFaces(),
mMaterial(NULL),
mKdTree(NULL),
mVertices(),
mIsConvex(false),
mIsWatertight(false),
mId(id)
{
    mVertices.reserve(vertices);
    mFaces.reserve(faces);
}


Mesh::Mesh(const int id): 
mId(id), mVertices(), mFaces(), mMaterial(NULL), mKdTree(NULL) 
{}


// apply transformation to each vertex
void Mesh::ApplyTransformation(const Matrix4x4 &m)
{
        VertexContainer::iterator it, it_end = mVertices.end();

        for (it = mVertices.begin(); it != it_end; ++ it)
        {
                (*it) = m * (*it);        
        }
}


Mesh::Mesh(const Mesh &rhs):
mKdTree(NULL)
{
        mVertices = rhs.mVertices;
        mFaces.reserve(rhs.mFaces.size());
        mId = rhs.mId;
        mMaterial = rhs.mMaterial;
        
        FaceContainer::const_iterator it, it_end = rhs.mFaces.end();

        for (it = rhs.mFaces.begin(); it != it_end; ++ it)
        {
                Face *face = *it;
                mFaces.push_back(new Face(*face));
        }
}


Mesh& Mesh::operator=(const Mesh& m) 
{
    if (this == &m) 
                return *this;
 
        CLEAR_CONTAINER(mFaces);

        mVertices = m.mVertices;
        mFaces.reserve(m.mFaces.size());
        mMaterial = m.mMaterial;
        // note: we don't copy id on purpose
        //mId = m.mId;
        
        FaceContainer::const_iterator it, it_end = m.mFaces.end();

        for (it = m.mFaces.begin(); it != it_end; ++ it)
        {
                Face *face = *it;
                mFaces.push_back(new Face(*face));
        }

        return *this;
}


Mesh::~Mesh()
{
        for (int i=0; i < mFaces.size(); ++ i)
                delete mFaces[i];

        DEL_PTR(mKdTree);
}
  

void Mesh::Clear() 
{
        mVertices.clear();
        mFaces.clear();

        DEL_PTR(mKdTree);
}



/********************************************************/
/*                      MeshInstance implementation             */
/********************************************************/

int
MeshInstance::CastRay(
                                          Ray &ray
                                          )
{
  int res = mMesh->CastRay(ray, this);

  return res;
}

int
MeshInstance::CastRay(
                                          Ray &ray,
                                          const vector<int> &faces
                                          )
{
  return mMesh->CastRayToSelectedFaces(ray, faces, this);
}



int
MeshInstance::GetRandomSurfacePoint(Vector3 &point, Vector3 &normal)
{
  return mMesh->GetRandomSurfacePoint(point, normal);
}

int
MeshInstance::GetRandomVisibleSurfacePoint(Vector3 &point,
                                                                                   Vector3 &normal,
                                                                                   const Vector3 &viewpoint,
                                                                                   const int maxTries)
{
        return mMesh->GetRandomVisibleSurfacePoint(point, normal, viewpoint, maxTries);
}


void MeshInstance::SetMaterial(Material *mat)
{
        mMaterial = mat;
}

Material *MeshInstance::GetMaterial() const
{
        return mMaterial;
}


Vector3 MeshInstance::GetNormal(const int idx) const
{
        return mMesh->GetNormal(idx);
}


int MeshInstance::GetRandomEdgePoint(Vector3 &point, Vector3 &normal)
{
        // get random face
        const int faceIdx = (int)RandomValue(0.0f, (float)mMesh->mFaces.size() - 0.5f);
        Face *face = mMesh->mFaces[faceIdx];

        // get random edge of face (hack: this is not uniform in the edges!
        const int edgeIdx = (int)RandomValue(0.0f, (float)face->mVertexIndices.size() - 0.5f);

        //cout << "idx = " << edgeIdx << " s: " << face->mVertexIndices.size() << endl;
        const int vertexIdxA = face->mVertexIndices[edgeIdx];
        const int vertexIdxB = face->mVertexIndices[(edgeIdx + 1) % (int)face->mVertexIndices.size()];

        const Vector3 a = mMesh->mVertices[vertexIdxA];
        const Vector3 b = mMesh->mVertices[vertexIdxB];

        const float w = RandomValue(0.0f, 1.0f);

        // get random point on edge
        point = a * w + b * (1.0f - w);

        //cout << "va " << a << " vb " << b << "p " << point << endl;
        // hack: set normal of face as normal
        normal = mMesh->GetFacePlane(faceIdx).mNormal;

    return 1;
}


/*************************************************************/
/*           TransformedMeshInstance implementation          */
/*************************************************************/


TransformedMeshInstance::TransformedMeshInstance(Mesh *mesh): 
MeshInstance(mesh)
{
    mWorldTransform = IdentityMatrix();
}


int TransformedMeshInstance::GetRandomSurfacePoint(Vector3 &point, Vector3 &normal)
{
  const int index = mMesh->GetRandomSurfacePoint(point, normal);
  point = mWorldTransform * point;
  normal = TransformNormal(mWorldTransform, normal);
  return index;
}


int TransformedMeshInstance::CastRay(Ray &ray)
{
        ray.ApplyTransform(Invert(mWorldTransform));
        
        const int res = mMesh->CastRay(ray, this);
        ray.ApplyTransform(mWorldTransform);

    return res;
}


int TransformedMeshInstance::CastRay(Ray &ray, const vector<int> &faces)
{
        ray.ApplyTransform(Invert(mWorldTransform));

        const int res = mMesh->CastRayToSelectedFaces(ray, faces, this);
        ray.ApplyTransform(mWorldTransform);

         return res;
}


void TransformedMeshInstance::ApplyWorldTransform(const Matrix4x4 &m)
{
        mWorldTransform = m * mWorldTransform;
}


void TransformedMeshInstance::LoadWorldTransform(const Matrix4x4 &m)
{
        mWorldTransform = m;
}


void TransformedMeshInstance::GetWorldTransform(Matrix4x4 &m) const
{
        m = mWorldTransform;
}


AxisAlignedBox3 TransformedMeshInstance::GetBox() const 
{
    return Transform(mMesh->mBox, mWorldTransform);
}


void TransformedMeshInstance::GetTransformedMesh(Mesh &transformedMesh) const
{
        // copy mesh
        transformedMesh = *mMesh;
        transformedMesh.ApplyTransformation(mWorldTransform);
}


Vector3 TransformedMeshInstance::GetNormal(const int idx) const
{
        Mesh mesh;
        GetTransformedMesh(mesh);
        return mesh.GetFacePlane(idx).mNormal;
}

}