/**
@def Verify if a point lies on a face

@param Point3f p   - Coordinates of the point
@param FacePointer f - Pointer to the face

@return true if point p is on face f, false elsewhere.
*/
bool IsOnFace(Point3f p, CMeshO::FacePointer f){
    //Compute vectors
    Point3f a=f->V(0)->P();
    Point3f b=f->V(2)->P();
    Point3f c=f->V(1)->P();

    Point3f v0 = c-a;
    Point3f v1 = b-a;
    Point3f v2 = p-a;

    // Compute dot products
    float dot00 = v0.dot(v0);
    float dot01 = v0.dot(v1);
    float dot02 = v0.dot(v2);
    float dot11 = v1.dot(v1);
    float dot12 = v1.dot(v2);

    // Compute barycentric coordinates
    float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
    float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
    float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

    // Check if point is in triangle
    if(math::Abs(u)<0) u=0;
    if(math::Abs(v)<0) v=0;
    return (u >= 0) && (v >= 0) && (u + v <=1);
}

/**
@def This funcion calculate the cartesian coordinates of a point given from its barycentric coordinates

@param Point3f bc       - barycentric coordinates of the point
@param FacePointer f    - pointer to the face

@return cartesian coordinates of the point
*/
CMeshO::CoordType fromBarCoords(Point3f bc,CMeshO::FacePointer f){
    CMeshO::CoordType p;
    Point3f p0=f->P(0);
    Point3f p1=f->P(1);
    Point3f p2=f->P(2);
    p=f->P(0)*bc[0]+f->P(1)*bc[1]+f->P(2)*bc[2];
    return p;
}

//
//	Draws a triangle
//
void edit_topo::drawFace(CMeshO::FacePointer fp)
{
    glPointSize(3.0f);
	
	glBegin(GL_POINTS); //GL_LINE_LOOP);
		glVertex(fp->P(0)); 
		glVertex(fp->P(1));
		glVertex(fp->P(2));
	glEnd();
}

void LandmarkCapturePlugin::drawFace(CMeshO::FacePointer fp, MeshModel &m, GLArea *gla, QPainter *p)
{

  //glDepthMask(GL_FALSE);
  //glDisable(GL_DEPTH_TEST);
  //p->endNativePainting();
  //p->save();
  //p->setRenderHint(QPainter::TextAntialiasing);
  //p->setPen(Qt::white);
  //QFont qFont;
  //qFont.setStyleStrategy(QFont::NoAntialias);
  //qFont.setFamily("Helvetica");
  //qFont.setPixelSize(12);
  //p->setFont(qFont);
  QString buf = QString("f%1\n (%3 %4 %5)").arg(tri::Index(m.cm,fp)).arg(tri::Index(m.cm,fp->V(0))).arg(tri::Index(m.cm,fp->V(1))).arg(tri::Index(m.cm,fp->V(2)));
  Point3f c=Barycenter(*fp);
  vcg::glLabel::render(p,c,buf);
  for(int i=0;i<3;++i)
    {
       buf =QString("\nv%1:%2 (%3 %4 %5)").arg(i).arg(fp->V(i) - &m.cm.vert[0]).arg(fp->P(i)[0]).arg(fp->P(i)[1]).arg(fp->P(i)[2]);
      if( m.hasDataMask(MeshModel::MM_VERTQUALITY) )
        buf +=QString(" - Q(%1)").arg(fp->V(i)->Q());
      if( m.hasDataMask(MeshModel::MM_WEDGTEXCOORD) )
          buf +=QString("- uv(%1 %2) id:%3").arg(fp->WT(i).U()).arg(fp->WT(i).V()).arg(fp->WT(i).N());
      if( m.hasDataMask(MeshModel::MM_VERTTEXCOORD) )
          buf +=QString("- uv(%1 %2) id:%3").arg(fp->V(i)->T().U()).arg(fp->V(i)->T().V()).arg(fp->V(i)->T().N());
    vcg::glLabel::render(p,fp->V(i)->P(),buf);
    }

  //p->drawText(QRect(0,0,gla->width(),gla->height()), Qt::AlignLeft | Qt::TextWordWrap, buf);
  //p->restore();
  //p->beginNativePainting();
}

  int  DeleteCollinearBorder(CMeshO &m, float threshold)
  {
    int total=0;
    CMeshO::FaceIterator fi;
    for(fi=m.face.begin();fi!=m.face.end();++fi)
      if(!(*fi).IsD())
        {
          for(int i=0;i<3;++i)
          {
            if(face::IsBorder(*fi,i) && !face::IsBorder(*fi,(i+1)%3))
            {
              CMeshO::VertexPointer V0= (*fi).V0(i);
              CMeshO::VertexPointer V1= (*fi).V1(i);
              CMeshO::VertexPointer V2=0;
              CMeshO::FacePointer fadj = (*fi).FFp((i+1)%3);
              int adjBordInd =  (*fi).FFi((i+1)%3);
              if(fadj->V1(adjBordInd) == V1)
                V2 = fadj->V2(adjBordInd);
              else
                continue; // non coerent face ordering.
              if(face::IsBorder(*fadj,(adjBordInd+1)%3))
              {
                // the colinearity test;
                Point3f pp;
                float dist;
                SegmentPointDistance(Segment3f(V0->cP(),V2->cP()),V1->cP(),pp,dist);
                if(dist* threshold <  Distance(V0->cP(),V2->cP()) )
                {
                  (*fi).V1(i)=V2;
                  if(face::IsBorder(*fadj,(adjBordInd+2)%3))
                  {
                    (*fi).FFp((i+1)%3)=&*fi;
                    (*fi).FFi((i+1)%3)=(i+1)%3;
                  }
                  else
                  {
                    CMeshO::FacePointer fj = fadj->FFp((adjBordInd+2)%3);
                    int ij = fadj->FFi((adjBordInd+2)%3);
                    (*fi).FFp((i+1)%3)= fj;
                    (*fi).FFi((i+1)%3)= ij;
                    fj->FFp(ij)=&*fi;
                    fj->FFi(ij)=(i+1)%3;
                  }
                  tri::Allocator<CMeshO>::DeleteFace(m,*fadj);
                  total++;
                }
              }
            }
          }
        }
	  return total;

}

float GetVelocity(CMeshO::CoordType o_p,CMeshO::CoordType n_p,CMeshO::FacePointer f,CMeshO::CoordType g,float m,float v){
    Point3f n=f->N();
    float b=n[0]*g[0]+n[1]*g[1]+n[2]*g[2];
    float distance=Distance(o_p,n_p);
    Point3f force;
    force[0]=g[0]-b*n[0];
    force[1]=g[1]-b*n[1];
    force[2]=g[2]-b*n[2];
    if(force.Norm()==0) return 0;
    float acceleration=(force/m).Norm();
    float n_v=math::Sqrt(pow(v,2)+(2*acceleration*distance));
    return n_v;
}

CMeshO::CoordType GetNewVelocity(CMeshO::CoordType i_v,CMeshO::FacePointer face,CMeshO::FacePointer new_face,CMeshO::CoordType force,CMeshO::CoordType g,float m,float t){
    CMeshO::CoordType n_v;
    Point3f n= face->N();
    float b=n[0]*force[0]+n[1]*force[1]+n[2]*force[2];
    Point3f f;
    //Compute force component along the face
    f[0]=force[0]-b*n[0];
    f[1]=force[1]-b*n[1];
    f[2]=force[2]-b*n[2];
    CMeshO::CoordType a=f/m;
    n_v=i_v+a*t;
    return getVelocityComponent(n_v.Norm(),new_face,g);
}

/**
@def Simulate the movement of a point, affected by a force "dir" on a face and a gravity "g".

@param CoordType p   - coordinates of the point
@param CoordType v   - velocity of the particle
@param float     m   - mass of the particle
@param FaceType face - pointer to the face
@param CoordType dir - direction of the force
@param float 	   l   - length of the movement
@param float     t   - time step

@return new coordinates of the point
*/
CMeshO::CoordType StepForward(CMeshO::CoordType p,CMeshO::CoordType v,float m,CMeshO::FacePointer &face,CMeshO::CoordType force,float l,float t){
    Point3f new_pos;
    Point3f n= face->N();
    float a=n[0]*force[0]+n[1]*force[1]+n[2]*force[2];
    Point3f f;
    //Compute force component along the face
    f[0]=force[0]-a*n[0];
    f[1]=force[1]-a*n[1];
    f[2]=force[2]-a*n[2];

    new_pos=p+v*t*l+(f/m)*pow(t,2)*0.5*l;

    return new_pos;
}

CMeshO::CoordType getVelocityComponent(float v,CMeshO::FacePointer f,CMeshO::CoordType g){
    CMeshO::CoordType cV;
    Point3f n= f->N();
    float a=n[0]*g[0]+n[1]*g[1]+n[2]*g[2];
    Point3f d;
    d[0]=g[0]-a*n[0];
    d[1]=g[1]-a*n[1];
    d[2]=g[2]-a*n[2];
    cV=d/d.Norm();
    cV.Normalize();
    cV[0]=v*d[0];
    cV[1]=v*d[1];
    cV[2]=v*d[2];
    return cV;
}

/**
@def Compute the intersection of the segment from p1 to p2 and the face f

@param CoordType p1 - position of the first point
@param Coordtype p2 - position of the second point
@param Facepointer f - pointer to the face
@param CoordType int_point - intersection point this is a return parameter for the function.
@param FacePointer face - pointer to the new face

@return the intersection edge index if there is an intersection -1 elsewhere
Step
*/
int ComputeIntersection(CMeshO::CoordType p1,CMeshO::CoordType p2,CMeshO::FacePointer &f,CMeshO::FacePointer &new_f,CMeshO::CoordType &int_point){

    CMeshO::CoordType v0=f->V(0)->P();
    CMeshO::CoordType v1=f->V(1)->P();
    CMeshO::CoordType v2=f->V(2)->P();
    float dist[3];
    Point3f int_points[3];
    dist[0]=PSDist(p2,v0,v1,int_points[0]);
    dist[1]=PSDist(p2,v1,v2,int_points[1]);
    dist[2]=PSDist(p2,v2,v0,int_points[2]);
    int edge=-1;
    if(dist[0]<dist[1]){
        if(dist[0]<dist[2]) edge=0;
        else edge=2;
    }else{
        if(dist[1]<dist[2]) edge=1;
        else edge=2;
    }

    CMeshO::VertexType* v;
    if(Distance(int_points[edge],f->V(edge)->P())<Distance(int_points[edge],f->V((edge+1) % 3)->P())) v=f->V(edge);
    else v=f->V((edge+1) % 3);
    vcg::face::Pos<CMeshO::FaceType> p(f,edge,v);
    new_f=f->FFp(edge);
    if(new_f==f) return -1;

    if(Distance(int_points[edge],v->P())<EPSILON){
        p.FlipF();
        CMeshO::FacePointer tmp_f=p.F();

        int n_face=0;
        while(tmp_f!=f){
            p.FlipE();
            p.FlipF();
            tmp_f=p.F();
            n_face++;
        }
        if(n_face!=0){
            int r=(rand()%(n_face-1))+2;
            for(int i=0;i<r;i++){
                p.FlipE();
                p.FlipF();
            }
            new_f=p.F();
        }
    }
    int_point=GetSafePosition(int_points[edge],new_f);
    return edge;
}

/**
Verify if a point on that face fall out because of the inclination
@param FacePointer f -  Pointer to the face
@param Point3f     g -  Direction of the gravity
@param float       a -  Adhesion Factor

return true if a particle of that face fall out
*/
bool CheckFallPosition(CMeshO::FacePointer f,Point3f g,float a){
    Point3f n=f->N();
    if(a>1) return false;
    if(acos(n.dot(g)/(n.Norm()*g.Norm()))<((PI/2)*(1-a))) return true;
    return false;
}

int SnapVertexBorder(CMeshO &m, float threshold, vcg::CallBackPos * cb)
{
  tri::Allocator<CMeshO>::CompactVertexVector(m);
  tri::Allocator<CMeshO>::CompactFaceVector(m);

  tri::UpdateTopology<CMeshO>::FaceFace(m);
  tri::UpdateFlags<CMeshO>::FaceBorderFromFF(m);
  tri::UpdateFlags<CMeshO>::VertexBorderFromFace(m);
  tri::UpdateNormal<CMeshO>::PerVertexNormalizedPerFaceNormalized(m);
  typedef GridStaticPtr<CMeshO::FaceType, CMeshO::ScalarType > MetroMeshFaceGrid;
  MetroMeshFaceGrid   unifGridFace;
  typedef tri::FaceTmark<CMeshO> MarkerFace;
  MarkerFace markerFunctor;
  vcg::face::PointDistanceBaseFunctor<CMeshO::ScalarType> PDistFunct;
  tri::UpdateFlags<CMeshO>::FaceClearV(m);
  unifGridFace.Set(m.face.begin(),m.face.end());

  markerFunctor.SetMesh(&m);
  int faceFound;
  int K = 20;
  Point3f startPt;
  float maxDist = m.bbox.Diag()/20;
  vector<Point3f> splitVertVec;
  vector<CMeshO::FacePointer> splitFaceVec;
  vector<int> splitEdgeVec;
  for(CMeshO::VertexIterator vi=m.vert.begin();vi!=m.vert.end();++vi)
    if(!(*vi).IsD() && (*vi).IsB())
      {
        int percPos = (tri::Index(m,*vi) *100) / m.vn;
        cb(percPos,"Snapping vertices");
        vector<CMeshO::FacePointer> faceVec;
        vector<float> distVec;
        vector<Point3f> pointVec;
        Point3f u;
        startPt = (*vi).P();
        faceFound = unifGridFace.GetKClosest(PDistFunct,markerFunctor, K, startPt,maxDist, faceVec, distVec, pointVec);

        CMeshO::FacePointer bestFace = 0;
        float localThr, bestDist = std::numeric_limits<float>::max();
        Point3f bestPoint;
        int bestEdge;
//        qDebug("Found %i face for vertex %i",faceFound,vi-m.vert.begin());
        for(int i=0;i<faceFound;++i)
        {
          const float epsilonSmall = 1e-5;
          const float epsilonBig = 1e-2;
          CMeshO::FacePointer fp=faceVec[i];
          InterpolationParameters(*fp,fp->cN(),pointVec[i],u);
//          qDebug(" face %i face for vertex %5.3f %5.3f %5.3f  dist %5.3f  (%c %c %c)",fp-&*m.face.begin(),u[0],u[1],u[2],distVec[i],IsBorder(*fp,0)?'b':' ',IsBorder(*fp,1)?'b':' ',IsBorder(*fp,2)?'b':' ');
          for(int j=0;j<3;++j)
          {
            if(IsBorder(*fp,j) && !fp->IsV())
            {
              if( u[(j+0)%3] > epsilonBig &&
                  u[(j+1)%3] > epsilonBig &&
                  u[(j+2)%3] < epsilonSmall )
              {
                if(distVec[i] < bestDist)
                {
                  bestDist=distVec[i];
                  //bestPoint=pointVec[i];
                  bestPoint=(*vi).cP();
                  bestFace=fp;
                  bestEdge=j;
                }
              }
            }
          }
        } // end for each faceFound

        if(bestFace)
          localThr = threshold*Distance(bestFace->P0(bestEdge),bestFace->P1(bestEdge));
        if(bestDist < localThr && !bestFace->IsV())
        {
          bestFace->SetV();
          (*vi).C()= Color4b::Blue;
          //bestFace->C()=Color4b::LightBlue;
          (*vi).SetS();
          splitVertVec.push_back(bestPoint);
          splitEdgeVec.push_back(bestEdge);
          splitFaceVec.push_back(bestFace);
        }
      }
  tri::Allocator<CMeshO>::PointerUpdater<CMeshO::FacePointer> pu;
  CMeshO::VertexIterator firstVert = tri::Allocator<CMeshO>::AddVertices(m,splitVertVec.size());
  CMeshO::FaceIterator firstface = tri::Allocator<CMeshO>::AddFaces(m,splitVertVec.size(),pu);
//
//             ^                           ^
//           /   \                       / | \          .
//         /       \                   /   |   \        .
//       /           \               /     |     \      .
//     /       fp      \           /       |       \    .
//   /                   \       /    fp   |   ff    \  .
//  V0 ------------------V2     V0 -------fv---------V2
//       i

  for(size_t i=0;i<splitVertVec.size();++i)
    {
      firstVert->P() = splitVertVec[i];
      int eInd = splitEdgeVec[i];
//.........这里部分代码省略.........