C++ SVector3类代码示例

  //---------------------------------------------------------------------------------------------------
  //  Public:   PerfectMatch
  //  Purpose:  Handles the degenerate case where v and this voxel
  //            have the same vertices.
  //---------------------------------------------------------------------------------------------------
  bool CXDMVoxel::PerfectMatch( const CXDMVoxel & v ) const
  {

    if ( v.nGeneration != this->nGeneration )
      return false;
    if ( v.bVoxelPointsUp != this->bVoxelPointsUp )
      return false;
  
    Float fError = fSideLength / Float( 10.0 ); 
  
    Bool pVertexMatch[3];

    for( Int i = 0; i < 3; i ++ )
    {
      pVertexMatch[i] = false;
      for( Int j = 0; j < 3; j ++ )
      {
        SVector3 oDisp = v.pVertex[i] - this->pVertex[j];
        if ( oDisp.GetLength() < fError )
          pVertexMatch[i] = true;
      }
    }
  
    return ( pVertexMatch[0] && pVertexMatch[1] && pVertexMatch[2] );
  }

  //---------------------------------------------------------------------------------------------------
  //
  //  Private: CheckAxis
  //
  //  Return true if the axis perpendicular to u = v2 - v1 is a separating axis  (i.e., no overlap)
  //  Therefore, we project to this perpendicular axis
  //
  //  Currently only works with 2D objects on the x-y plane
  //
  //
  //---------------------------------------------------------------------------------------------------
  bool CXDMVoxel::CheckSeparatingAxis( const CXDMVoxel & oVoxel, const CXDMVoxel & oTestVoxel,
                                       const SVector3 &oV1, const SVector3 &oV2) const
  {
    SVector3 oDir = oV2 - oV1;
    oDir.Normalize();

    // rotate 90 degress y <- x, x <- -y
    Float tmp = oDir.m_fY;
    oDir.m_fY = oDir.m_fX;
    oDir.m_fX = - tmp;

    Float fMin = MAX_FLOAT;
    Float fMax = MIN_FLOAT;

    for ( int i = 0; i < 3; i ++ )
    {
      Float fAxisProjection = Dot( oTestVoxel.pVertex[i], oDir );

      if ( fAxisProjection < fMin )
        fMin = fAxisProjection;
      if ( fAxisProjection > fMax )
        fMax = fAxisProjection;
    }
  
    for ( int i = 0; i < 3; i ++ )
    {
      Float fVertex = Dot( oVoxel.pVertex[i], oDir );  
      if( fVertex <= fMax  && fVertex >= fMin )   // if intersecting
      {
        return false;
      }
    }
    return true;
  }

// returns the cross field as a pair of othogonal vectors (NOT in parametric coordinates, but real 3D coordinates)
Pair<SVector3, SVector3> frameFieldBackgroundMesh2D::compute_crossfield_directions(double u, double v,
        double angle_current)
{
    // get the unit normal at that point
    GFace *face = dynamic_cast<GFace*>(gf);
    if(!face) {
        Msg::Error("Entity is not a face in background mesh");
        return Pair<SVector3,SVector3>(SVector3(), SVector3());
    }

    Pair<SVector3, SVector3> der = face->firstDer(SPoint2(u,v));
    SVector3 s1 = der.first();
    SVector3 s2 = der.second();
    SVector3 n = crossprod(s1,s2);
    n.normalize();

    SVector3 basis_u = s1;
    basis_u.normalize();
    SVector3 basis_v = crossprod(n,basis_u);

    // normalize vector t1 that is tangent to gf at uv
    SVector3 t1 = basis_u * cos(angle_current) + basis_v * sin(angle_current) ;
    t1.normalize();

    // compute the second direction t2 and normalize (t1,t2,n) is the tangent frame
    SVector3 t2 = crossprod(n,t1);
    t2.normalize();

    return Pair<SVector3,SVector3>(SVector3(t1[0],t1[1],t1[2]),
                                   SVector3(t2[0],t2[1],t2[2]));
}

double Filler::improvement(GEntity* ge,MElementOctree* octree,SPoint3 point,double h1,SVector3 direction){
  double x,y,z;
  double average;
  double h2;
  double coeffA,coeffB;

  x = point.x() + h1*direction.x();
  y = point.y() + h1*direction.y();
  z = point.z() + h1*direction.z();
  
  if(inside_domain(octree,x,y,z)){
    h2 = get_size(x,y,z);
  }
  else h2 = h1;

  coeffA = 1.0;
  coeffB = 0.16;
  
  if(h2>h1){
    average = coeffA*h1 + (1.0-coeffA)*h2;
  }
  else{
    average = coeffB*h1 + (1.0-coeffB)*h2;
  }
	
  return average;
}

double distMaxStraight(MElement *el)
{
  const polynomialBasis *lagrange = (polynomialBasis*)el->getFunctionSpace();
  const polynomialBasis *lagrange1 = (polynomialBasis*)el->getFunctionSpace(1);
  int nV = lagrange->points.size1();
  int nV1 = lagrange1->points.size1();
  int dim = lagrange1->dimension;
  SPoint3 sxyz[256];
  for (int i = 0; i < nV1; ++i) {
    sxyz[i] = el->getVertex(i)->point();
  }
  for (int i = nV1; i < nV; ++i) {
    double f[256];
    lagrange1->f(lagrange->points(i, 0), lagrange->points(i, 1),
                 dim < 3 ? 0 : lagrange->points(i, 2), f);
    for (int j = 0; j < nV1; ++j)
      sxyz[i] += sxyz[j] * f[j];
  }

  double maxdx = 0.0;
  for (int iV = nV1; iV < nV; iV++) {
    SVector3 d = el->getVertex(iV)->point()-sxyz[iV];
    double dx = d.norm();
    if (dx > maxdx) maxdx = dx;
  }

  return maxdx;
}

SMetric3 max_edge_curvature_metric(const GVertex *gv)
{
  SMetric3 val (1.e-12);
  std::list<GEdge*> l_edges = gv->edges();
  for (std::list<GEdge*>::const_iterator ite = l_edges.begin();
       ite != l_edges.end(); ++ite){
    GEdge *_myGEdge = *ite;
    Range<double> range = _myGEdge->parBounds(0);
    SMetric3 cc;
    if (gv == _myGEdge->getBeginVertex()) {
      SVector3 t = _myGEdge->firstDer(range.low());
      t.normalize();
      double l_t = ((2 * M_PI) /( fabs(_myGEdge->curvature(range.low()))
				*  CTX::instance()->mesh.minCircPoints ));
      double l_n = 1.e12;
      cc = buildMetricTangentToCurve(t,l_t,l_n);
    }
    else {
      SVector3 t = _myGEdge->firstDer(range.high());
      t.normalize();
      double l_t = ((2 * M_PI) /( fabs(_myGEdge->curvature(range.high()))
				  *  CTX::instance()->mesh.minCircPoints ));
      double l_n = 1.e12;
      cc = buildMetricTangentToCurve(t,l_t,l_n);
    }
    val = intersection(val,cc);
  }
  return val;
}

SMetric3 metric_based_on_surface_curvature(const GFace *gf, double u, double v,
					   bool surface_isotropic,
					   double d_normal ,
					   double d_tangent_max)
{
  if (gf->geomType() == GEntity::Plane)return SMetric3(1.e-12);
  double cmax, cmin;
  SVector3 dirMax,dirMin;
  cmax = gf->curvatures(SPoint2(u, v),&dirMax, &dirMin, &cmax,&cmin);
  if (cmin == 0)cmin =1.e-12;
  if (cmax == 0)cmax =1.e-12;
  double lambda1 =  ((2 * M_PI) /( fabs(cmin) *  CTX::instance()->mesh.minCircPoints ) );
  double lambda2 =  ((2 * M_PI) /( fabs(cmax) *  CTX::instance()->mesh.minCircPoints ) );
  SVector3 Z = crossprod(dirMax,dirMin);
  if (surface_isotropic)  lambda2 = lambda1 = std::min(lambda2,lambda1);
  dirMin.normalize();
  dirMax.normalize();
  Z.normalize();
  lambda1 = std::max(lambda1, CTX::instance()->mesh.lcMin);
  lambda2 = std::max(lambda2, CTX::instance()->mesh.lcMin);
  lambda1 = std::min(lambda1, CTX::instance()->mesh.lcMax);
  lambda2 = std::min(lambda2, CTX::instance()->mesh.lcMax);
  double lambda3 = std::min(d_normal, CTX::instance()->mesh.lcMax);
  lambda3 = std::max(lambda3, CTX::instance()->mesh.lcMin);
  lambda1 = std::min(lambda1, d_tangent_max);
  lambda2 = std::min(lambda2, d_tangent_max);

  SMetric3 curvMetric (1./(lambda1*lambda1),1./(lambda2*lambda2),
		       1./(lambda3*lambda3),
                       dirMin, dirMax, Z );
  return curvMetric;
}

double angle3Vertices(const MVertex *p1, const MVertex *p2, const MVertex *p3)
{
  SVector3 a(p1->x() - p2->x(), p1->y() - p2->y(), p1->z() - p2->z());
  SVector3 b(p3->x() - p2->x(), p3->y() - p2->y(), p3->z() - p2->z());
  SVector3 c = crossprod(a, b);
  double sinA = c.norm();
  double cosA = dot(a, b);
  return atan2 (sinA, cosA);
}

bool GEdge::XYZToU(const double X, const double Y, const double Z,
                   double &u, const double relax) const
{
  const int MaxIter = 25;
  const int NumInitGuess = 11;

  double err;//, err2;
  int iter;

  Range<double> uu = parBounds(0);
  double uMin = uu.low();
  double uMax = uu.high();

  SVector3 Q(X, Y, Z), P;

  double init[NumInitGuess];

  for (int i = 0; i < NumInitGuess; i++)
    init[i] = uMin + (uMax - uMin) / (NumInitGuess - 1) * i;

  for(int i = 0; i < NumInitGuess; i++){
    u = init[i];
    double uNew = u;
    //err2 = 1.0;
    iter = 1;

    SVector3 dPQ = P - Q;
    err = dPQ.norm();

    if (err < 1.e-8 * CTX::instance()->lc) return true;

    while(iter++ < MaxIter && err > 1e-8 * CTX::instance()->lc) {
      SVector3 der = firstDer(u);
      uNew = u - relax * dot(dPQ,der) / dot(der,der);
      uNew = std::min(uMax,std::max(uMin,uNew));
      P = position(uNew);

      dPQ = P - Q;
      err = dPQ.norm();
      //err2 = fabs(uNew - u);
      u = uNew;
    }

    if (err < 1e-8 * CTX::instance()->lc) return true;
  }

  if(relax > 1.e-2) {
    //    Msg::Info("point %g %g %g on edge %d : Relaxation factor = %g",
    //              Q.x(), Q.y(), Q.z(), 0.75 * relax);
    return XYZToU(Q.x(), Q.y(), Q.z(), u, 0.75 * relax);
  }

  //  Msg::Error("Could not converge reparametrisation of point (%e,%e,%e) on edge %d",
  //             Q.x(), Q.y(), Q.z(), tag());
  return false;
}

  //--------------------------------------------------------------------------------------------------------
  //
  //  GetAlignRotation
  //  -- Given *UNIT vector* oRef, oVec, return a matrix that rotates oVec -> oRef
  //
  //--------------------------------------------------------------------------------------------------------
  SMatrix3x3 GetAlignmentRotation( const SVector3 &oVecDir, const SVector3 &oRefDir )
  {
    
    SVector3 oAxis  = Cross( oVecDir, oRefDir );
    Float    fAngle = acos( Dot( oVecDir, oRefDir ) );

    oAxis.Normalize();
    SMatrix3x3 oRes;
    oRes.BuildRotationAboutAxis( oAxis, fAngle );
    return oRes;
  }

static void CalculateOrthoShadow( const CCameraEntity& cam, Light& light )
{
	SVector3 target = gCasterBounds.getCenter();
	SVector3 size = gCasterBounds.getMax() - gCasterBounds.getMin();
	float radius = size.length() * 0.5f;
	
	// figure out the light camera matrix that encloses whole scene
	light.mWorldMat.spaceFromAxisZ();
	light.mWorldMat.getOrigin() = target - light.mWorldMat.getAxisZ() * radius;
	light.setOrthoParams( radius*2, radius*2, radius*0.1f, radius*2 );
	light.setOntoRenderContext();
}

static void _relocateVertexOfPyramid(MVertex *ver,
                                     const std::vector<MElement *> &lt,
                                     double relax)
{
  if(ver->onWhat()->dim() != 3) return;
  double x = 0.0, y = 0.0, z = 0.0;
  int N = 0;
  MElement *pyramid = NULL;

  for(std::size_t i = 0; i < lt.size(); i++) {
    double XCG = 0.0, YCG = 0.0, ZCG = 0.0;
    if(lt[i]->getNumVertices() == 5)
      pyramid = lt[i];
    else {
      for(std::size_t j = 0; j < lt[i]->getNumVertices(); j++) {
        XCG += lt[i]->getVertex(j)->x();
        YCG += lt[i]->getVertex(j)->y();
        ZCG += lt[i]->getVertex(j)->z();
      }
      x += XCG;
      y += YCG;
      z += ZCG;
      N += lt[i]->getNumVertices();
    }
  }
  x /= N;
  y /= N;
  z /= N;

  if(pyramid) {
    MFace q = pyramid->getFace(4);
    double A = q.approximateArea();
    SVector3 n = q.normal();
    n.normalize();
    SPoint3 c = q.barycenter();
    SVector3 d(x - c.x(), y - c.y(), z - c.z());
    if(dot(n, d) < 0) n = n * (-1.0);
    double H = .5 * sqrt(fabs(A));
    double XOPT = c.x() + relax * H * n.x();
    double YOPT = c.y() + relax * H * n.y();
    double ZOPT = c.z() + relax * H * n.z();
    double FULL_MOVE_OBJ =
      objective_function(1.0, ver, XOPT, YOPT, ZOPT, lt, true);
    // printf("relax %g obj %g\n",relax,FULL_MOVE_OBJ);
    if(FULL_MOVE_OBJ > 0.1) {
      ver->x() = XOPT;
      ver->y() = YOPT;
      ver->z() = ZOPT;
      return;
    }
  }
}

SMetric3 buildMetricTangentToSurface(SVector3 &t1, SVector3 &t2,
                                     double l_t1, double l_t2, double l_n)
{
  t1.normalize();
  t2.normalize();
  SVector3 n = crossprod (t1,t2);
  n.normalize();

  l_t1 = std::max(l_t1, CTX::instance()->mesh.lcMin);
  l_t2 = std::max(l_t2, CTX::instance()->mesh.lcMin);
  l_t1 = std::min(l_t1, CTX::instance()->mesh.lcMax);
  l_t2 = std::min(l_t2, CTX::instance()->mesh.lcMax);
  SMetric3 Metric (1./(l_t1*l_t1),1./(l_t2*l_t2),1./(l_n*l_n),t1,t2,n);
  return Metric;
}

int edge_normal
(const MVertex *const vertex, const int zoneIndex, const GEdge *const gEdge,
 const CCon::FaceVector<MZoneBoundary<2>::GlobalVertexData<MEdge>::FaceDataB>
 &faces, SVector3 &boNormal, const int onlyFace = -1)
{

  double par=0.0;
  // Note: const_cast used to match MVertex.cpp interface
  if(!reparamMeshVertexOnEdge(const_cast<MVertex*>(vertex), gEdge, par)) return 1;

  const SVector3 tangent(gEdge->firstDer(par));
                                        // Tangent to the boundary face
  SPoint3 interior(0., 0., 0.);         // An interior point
  SVector3 meshPlaneNormal(0.);         // This normal is perpendicular to the
                                        // plane of the mesh

  // The interior point and mesh plane normal are computed from all elements in
  // the zone.
  int cFace = 0;
  int iFace = 0;
  int nFace = faces.size();
  if ( onlyFace >= 0 ) {
    iFace = onlyFace;
    nFace = onlyFace + 1;
  }
  for(; iFace != nFace; ++iFace) {
    if(faces[iFace].zoneIndex == zoneIndex) {
      ++cFace;
      interior += faces[iFace].parentElement->barycenter();
      // Make sure all the planes go in the same direction
      //**Required?
      SVector3 mpnt = faces[iFace].parentElement->getFace(0).normal();
      if(dot(mpnt, meshPlaneNormal) < 0.) mpnt.negate();
      meshPlaneNormal += mpnt;
    }
  }
  interior /= cFace;
  // Normal to the boundary edge (but unknown direction)
  boNormal = crossprod(tangent, meshPlaneNormal);
  boNormal.normalize();
  // Direction vector from vertex to interior (inwards).  The normal should
  // point in the same direction.
  if(dot(boNormal, SVector3(vertex->point(), interior)) < 0.)
    boNormal.negate();
  return 0;

}

 Bool Equivilent( const CSymmetryType & oSymOps, const SVector3 &v1, const SVector3 &v2, Float fError )
 {
   const vector<SMatrix3x3> &vOperatorList = oSymOps.GetOperatorList();
   
   for( Size_Type i = 0; i < vOperatorList.size(); i ++ )
   {
     SVector3 oTmp = vOperatorList[i] * v1;
     SVector3 oDiff = oTmp - v2;
     Float fDiff = oDiff.GetLength();
     
     if( fDiff < fError )
     {
       return true;
     }
   }
   return false;
 }