coginverse.cxx

有限元学习研究用源代码(老外的),供科研人员参考

#include "cog.hxx"

// This file contains the inverse variants of the basic functions:
// That means, to find the intersection on the boundary starting from
// an intersection inside:
/*
cogIndex Cogeometry::Line(cogFlag1& f, const cogFlag1& o, 
			  const cogLine& s) const;
cogIndex Cogeometry::Triangle(cogFlag2& f, const cogFlag2& o,
			      const cogTriangle& s) const;
cogIndex Cogeometry::Tetrahedron(cogFlag3& f, const cogFlag3& o, 
				 const cogTetrahedron& s) const;
				 */

cogIndex Cogeometry::Line(cogFlag1& f, const cogFlag1& o, 
			  const cogLine& s) const
{
  if((
      (o.po[0]-o.p0[0])*(s.c1[0]-s.c0[0])+
      (o.po[1]-o.p0[1])*(s.c1[1]-s.c0[1])+
      (o.po[2]-o.p0[2])*(s.c1[2]-s.c0[2]))
     >=0)
    return Line(f,cogLine(o.po,s.c1));
  else
    //    wzAssert(0);
    return Line(f,cogLine(o.p0,s.c1));
}

cogIndex Cogeometry::Triangle(cogFlag2& f, const cogFlag2& o,
         const cogTriangle& s) const
{
  cogFloat x[3],y[3],z[3],u[3],k,s0=0,s1=0,s2=0;
  wzPoint g2,g1,g0,go;  cogFlag2 t2(g2,g1,g0,go);
  wzPoint *c0,*c1,*c2;
  cogIndex i,m01,m02,m12,rc;
  // at first, we have to order the three points:
  for(i=0;i<3;i++){
    k = o.p1[i]-o.p2[i];
    s0 += k*(s.c0[i]-o.p2[i]);
    s1 += k*(s.c1[i]-o.p2[i]);
    s2 += k*(s.c2[i]-o.p2[i]);
  }
  if(s0<s1){
    if(s1<s2)		{m01=01;m12=12;m02=02; c0=&s.c0;c1=&s.c1;c2=&s.c2;}
    else if(s0<s2)	{m01=02;m12=12;m02=01; c0=&s.c0;c1=&s.c2;c2=&s.c1;}
    else 		{m01=02;m12=01;m02=12; c0=&s.c2;c1=&s.c0;c2=&s.c1;}
  }else{
    if(s0<s2)		{m01=01;m12=02;m02=12; c0=&s.c1;c1=&s.c0;c2=&s.c2;}
    else if(s1<s2)	{m01=12;m12=02;m02=01; c0=&s.c1;c1=&s.c2;c2=&s.c0;}
    else 		{m01=12;m12=01;m02=02; c0=&s.c2;c1=&s.c1;c2=&s.c0;}
  }
  // now we find the intersection of (o,c1) with (c0,c2)
  for(i=0;i<3;i++){
    x[i] = (*c1)[i]-o.p1[i];
    y[i] = (*c2)[i]-(*c0)[i];
  }
  z[0] = x[1]*y[2]-x[2]*y[1];
  z[1] = x[2]*y[0]-x[0]*y[2];
  z[2] = x[0]*y[1]-x[1]*y[0];
  u[0] = z[1]*x[2]-z[2]*x[1];
  u[1] = z[2]*x[0]-z[0]*x[2];
  u[2] = z[0]*x[1]-z[1]*x[0];
  // now u should be orthogonal to x=(o,c1), in the plane y=(c0,c2)
  s0=s1=0;	
  for(i=0;i<3;i++){
    s1 += u[i]*y[i];
    s0 += u[i]*((*c1)[i]-(*c0)[i]);
  }
  wzAssert(s1>0);	// not (o == c1 or c0 == c2)
  s0 /= s1;
  // in the following cases the intersection is de-facto on the upper line:
  if(s0>0.99){
    rc=cogRCFaceFoundOn12; 
    t2 = o;
    goto found;
  } 
  // in the following case, the intersection is de-facto on the lower line:
  if(s0<0.01){
    wzPoint h1,h0,ho;  cogFlag1 t(h1,h0,ho);
    if((
	(o.po[0]-o.p0[0])*((*c1)[0]-(*c0)[0])+
	(o.po[1]-o.p0[1])*((*c1)[1]-(*c0)[1])+
	(o.po[2]-o.p0[2])*((*c1)[2]-(*c0)[2]))
       >0){
      otherSide(t,o);
    }else{
      t = o;
    }
    rc = Triangle(t2,t,cogTriangle(*c0,*c1,*c2));
    goto found;
  }
  {
    wzPoint p3((*c0)[0]+s0*y[0],(*c0)[1]+s0*y[1],(*c0)[2]+s0*y[2]); Point(p3);
    wzPoint h1,h0,ho;  cogFlag1 t(h1,h0,ho);
    if((
	(o.po[0]-o.p0[0])*(p3[0]-(*c1)[0])+
	(o.po[1]-o.p0[1])*(p3[1]-(*c1)[1])+
	(o.po[2]-o.p0[2])*(p3[2]-(*c1)[2]))
       <0){
      t = o;
    }else{
      otherSide(t,o);
    }
    for(i=0;i<5;i++){
      try{
	rc = Triangle(t2,t,cogTriangle(p3,*c1,*c2));
      }catch(cogTriangleRoundTripFailed){
	wzAssert(t.p1.near(p3,Delta));
	t2 = t; rc = cogRCFaceFoundOn02;
      }
      if(rc != cogRCFaceFoundOn01) break;
      t = t2;
      rc = Triangle(t2,t,cogTriangle(*c1,p3,*c0));
      switch(rc){
      case cogRCFaceFoundOn01:
	t = t2; continue;
      case cogRCFaceFoundOn12:
	rc = cogRCFaceFoundOn02; goto found;
      case cogRCFaceFoundOn02:
	rc = cogRCFaceFoundOn01; goto found;
      }
    }
  }
found:
  int m;
  switch(rc){
  case cogRCEdgeFound:	f = t2; return rc;
  case cogRCFaceFoundOn01: m = m01; break;
  case cogRCFaceFoundOn12: m = m12; break;
  case cogRCFaceFoundOn02: m = m02; break;
  default: wzAssert(0);
  }
  switch(m){
  case cogRCFaceFoundOn01: c1 = &s.c0; c2 = &s.c1; break;
  case cogRCFaceFoundOn02: c1 = &s.c2; c2 = &s.c0; break;
  case cogRCFaceFoundOn12: c1 = &s.c1; c2 = &s.c2; break;
  default: wzAssert(0);
  }
  if((t2.po[0]-t2.p0[0])*((*c2)[0]-(*c1)[0])+
     (t2.po[1]-t2.p0[1])*((*c2)[1]-(*c1)[1])+
     (t2.po[2]-t2.p0[2])*((*c2)[2]-(*c1)[2])
     <0){
    otherSide(f,t2);
  }else{
    f=t2;
  }
  return m;
}

cogIndex Cogeometry::Tetrahedron(cogFlag3& f, const cogFlag3& o, 
				 const cogTetrahedron& s) const
{
  wzAssert(0);
  return cogRCError;
}