C++ SVector3::normalize方法代码示例

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 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;
}

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;
}

// 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]));
}

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 buildMetricTangentToCurve(SVector3 &t, double l_t, double l_n)
{
  if (l_t == 0.0) return SMetric3(1.e-22);
  SVector3 a;
  if (fabs(t(0)) <= fabs(t(1)) && fabs(t(0)) <= fabs(t(2))){
    a = SVector3(1,0,0);
  }
  else if (fabs(t(1)) <= fabs(t(0)) && fabs(t(1)) <= fabs(t(2))){
    a = SVector3(0,1,0);
  }
  else{
    a = SVector3(0,0,1);
  }
  SVector3 b = crossprod (t,a);
  SVector3 c = crossprod (b,t);
  b.normalize();
  c.normalize();
  t.normalize();
  SMetric3 Metric (1./(l_t*l_t),1./(l_n*l_n),1./(l_n*l_n),t,b,c);
  //  printf("bmttc %g %g %g %g %g\n",l_t,l_n,Metric(0,0),Metric(0,1),Metric(1,1));
  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;

}

void CActorEntity::update( float timeAlpha )
{
	bool alive = mGameEntity->isAlive();
	if( alive ) {
		// fade out the outline
		float dt = CSystemTimer::getInstance().getDeltaTimeS();
		mOutlineTTL -= dt;
		if( mOutlineTTL < 0.0f )
			mOutlineTTL = 0.0f;

		SMatrix4x4& m = mWorldMat;
		SVector3 pos = samplePos( timeAlpha );
		SVector3 dir = samplePos( timeAlpha + 0.1f ) - pos;
		if( dir.lengthSq() < 1.0e-3f )
			dir = m.getAxisZ();
		else
			dir.normalize();

		if( mGameEntity->getType() == ENTITY_BLOCKER ) {
			double tt = CSystemTimer::getInstance().getTimeS();
			D3DXMatrixRotationY( &m, tt * 0.2f );
			m.getOrigin() = pos;
			m.getOrigin().y += sinf( tt * 0.6f ) * 0.2f;
		} else {
			m.getOrigin() = pos;
			m.getAxisZ() = dir;
			m.getAxisZ().y *= 0.2f;
			m.getAxisZ().normalize();
			m.getAxisY().set( 0, 1, 0 );
			m.getAxisX() = m.getAxisY().cross( m.getAxisZ() );
			m.getAxisX().normalize();
			m.getAxisY() = m.getAxisZ().cross( m.getAxisX() );
		}
	} else {
		mOutlineTTL = 0.0f;
	}
}

void CCollisionRay::setDirection( const SVector3& direction ) 
{
	SVector3 d = direction;
	d.normalize();
	mRay.mDir = convert( d ); 
}

//.........这里部分代码省略.........
	gUIDlg->enableNonUserEvents( false );
	gUISliderTime->setValue( gameTime );
	gUIDlg->enableNonUserEvents( true );
	gi.setTime( gameTime );
	*/

	//
	// camera

	int selEntityID = gi.getEntities().getSelectedEntityID();
	const CActorEntity* selEntity = gi.getEntities().getActorEntityByID( selEntityID );
	bool hasSelected = (selEntity != NULL);
	if( hasSelected ) {
		SVector3 selPos = selEntity->mWorldMat.getOrigin();
		if( !gAppSettings.followMode ) {
			selPos.y = VIEWER_Y;
			SVector3 newPos = smoothCD( gViewer.getOrigin(), selPos, gViewerVel, 0.25f, dt );
			gTryPositionViewer( newPos, selPos );
		} else {
			//SVector3 selPrevPos = selEntity.samplePos( gi.getTime() - 10.0f );
			// TBD
			SVector3 selPrevPos = selPos;
			selPrevPos.y = (selPrevPos.y + VIEWER_Y)*0.5f;
			selPos.y = selPrevPos.y;
			SVector3 dirZ = selPos - selPrevPos;
			dirZ.y = 0.0f;
			const float MIN_DIST = 2.0f;
			if( dirZ.lengthSq() < MIN_DIST*MIN_DIST ) {
				dirZ = gViewer.getAxisZ();
				selPrevPos = selPrevPos - dirZ * MIN_DIST;
			}
			const float PREF_DIST = 5.0f;
			if( dirZ.lengthSq() < PREF_DIST*PREF_DIST ) {
				dirZ.normalize();
				selPrevPos = selPos - dirZ * PREF_DIST;
			} else {
				dirZ.normalize();
			}
			const float SMOOTHT = 0.5f;
			SVector3 newPos = smoothCD( gViewer.getOrigin(), selPrevPos, gViewerVel, SMOOTHT, dt );
			gTryPositionViewer( newPos, selPrevPos );
			dirZ = smoothCD( gViewer.getAxisZ(), dirZ, gViewerZVel, SMOOTHT, dt );
			if( dirZ.lengthSq() < 0.5f ) {
				dirZ = gViewer.getAxisZ();
			} else {
				gViewer.getAxisZ() = dirZ.getNormalized();
			}
			gViewer.getAxisY().set(0,1,0);
			gViewer.getAxisX() = gViewer.getAxisY().cross( gViewer.getAxisZ() );
		}
	} else {
		gViewerVel.set(0,0,0);
		gViewerZVel.set(0,0,0);
	}

	//
	// check if current viewer's position is valid

	{
		SVector3 testViewerPos = gViewer.getOrigin();
		int cx = testViewerPos.x;
		int cy = -testViewerPos.z;
		if( cx >= 0 && cy >= 0 && cx < gmap.getCellsX() && cy < gmap.getCellsY() && gmap.isBlood( gmap.getCell(cx,cy).type ) ) {
			gi.getLevelMesh().fitSphere( testViewerPos, VIEWER_R*2.0f );
			const float SMALL_FIT = 0.2f;
			if( SVector3(testViewerPos - gViewer.getOrigin()).lengthSq() < SMALL_FIT * SMALL_FIT ) {

bool frameFieldBackgroundMesh2D::compute_RK_infos(double u,double v, double x, double y, double z, RK_form &infos)
{
    // check if point is in domain
    if (!inDomain(u,v)) return false;

    // get stored angle

    double angle_current = angle(u,v);

    // compute t1,t2: cross field directions

    // 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 false;
    }

    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();

    // get metric

    double L = size(u,v);
    infos.metricField = SMetric3(1./(L*L));
    FieldManager *fields = gf->model()->getFields();
    if(fields->getBackgroundField() > 0) {
        Field *f = fields->get(fields->getBackgroundField());
        if (!f->isotropic()) {
            (*f)(x,y,z, infos.metricField,gf);
        }
        else {
            L = (*f)(x,y,z,gf);
            infos.metricField = SMetric3(1./(L*L));
        }
    }
    double M = dot(s1,s1);
    double N = dot(s2,s2);
    double E = dot(s1,s2);
    // compute the first fundamental form i.e. the metric tensor at the point
    // M_{ij} = s_i \cdot s_j
    double metric[2][2] = {{M,E},{E,N}};

    // get sizes

    double size_1 = sqrt(1. / dot(t1,infos.metricField,t1));
    double size_2 = sqrt(1. / dot(t2,infos.metricField,t2));

    // compute covariant coordinates of t1 and t2 - cross field directions in parametric domain
    double covar1[2],covar2[2];
    // t1 = a s1 + b s2 -->
    // t1 . s1 = a M + b E
    // t1 . s2 = a E + b N --> solve the 2 x 2 system
    // and get covariant coordinates a and b
    double rhs1[2] = {dot(t1,s1),dot(t1,s2)};
    bool singular = false;
    if (!sys2x2(metric,rhs1,covar1)) {
        Msg::Info("Argh surface %d %g %g %g -- %g %g %g -- %g %g",gf->tag(),s1.x(),s1.y(),s1.z(),s2.x(),s2.y(),s2.z(),size_1,size_2);
        covar1[1] = 1.0;
        covar1[0] = 0.0;
        singular = true;
    }
    double rhs2[2] = {dot(t2,s1),dot(t2,s2)};
    if (!sys2x2(metric,rhs2,covar2)) {
        Msg::Info("Argh surface %d %g %g %g -- %g %g %g",gf->tag(),s1.x(),s1.y(),s1.z(),s2.x(),s2.y(),s2.z());
        covar2[0] = 1.0;
        covar2[1] = 0.0;
        singular = true;
    }

    // transform the sizes with respect to the metric
    // consider a vector v of size 1 in the parameter plane
    // its length is sqrt (v^T M v) --> if I want a real size
    // of size1 in direction v, it should be sqrt(v^T M v) * size1
    double l1 = sqrt(covar1[0]*covar1[0]+covar1[1]*covar1[1]);
    double l2 = sqrt(covar2[0]*covar2[0]+covar2[1]*covar2[1]);

    covar1[0] /= l1;
    covar1[1] /= l1;
    covar2[0] /= l2;
    covar2[1] /= l2;

    double size_param_1  = size_1 / sqrt (  M*covar1[0]*covar1[0]+
                                            2*E*covar1[1]*covar1[0]+
                                            N*covar1[1]*covar1[1]);
    double size_param_2  = size_2 / sqrt (  M*covar2[0]*covar2[0]+
                                            2*E*covar2[1]*covar2[0]+
                                            N*covar2[1]*covar2[1]);
//.........这里部分代码省略.........

void frameFieldBackgroundMesh2D::computeCrossField(simpleFunction<double> &eval_diffusivity)
{
    angles.clear();

    DoubleStorageType _cosines4,_sines4;

    list<GEdge*> e;
    GFace *face = dynamic_cast<GFace*>(gf);
    if(!face) {
        Msg::Error("Entity is not a face in background mesh");
        return;
    }

    replaceMeshCompound(face, e);

    list<GEdge*>::const_iterator it = e.begin();

    for( ; it != e.end(); ++it ) {
        if (!(*it)->isSeam(face)) {
            for(unsigned int i = 0; i < (*it)->lines.size(); i++ ) {
                MVertex *v[2];
                v[0] = (*it)->lines[i]->getVertex(0);
                v[1] = (*it)->lines[i]->getVertex(1);
                SPoint2 p1,p2;
                reparamMeshEdgeOnFace(v[0],v[1],face,p1,p2);
                Pair<SVector3, SVector3> der = face->firstDer((p1+p2)*.5);
                SVector3 t1 = der.first();
                SVector3 t2 = der.second();
                SVector3 n = crossprod(t1,t2);
                n.normalize();
                SVector3 d1(v[1]->x()-v[0]->x(),v[1]->y()-v[0]->y(),v[1]->z()-v[0]->z());
                t1.normalize();
                d1.normalize();
                double _angle = myAngle (t1,d1,n);
                normalizeAngle (_angle);
                for (int i=0; i<2; i++) {
                    DoubleStorageType::iterator itc = _cosines4.find(v[i]);
                    DoubleStorageType::iterator its = _sines4.find(v[i]);
                    if (itc != _cosines4.end()) {
                        itc->second  = 0.5*(itc->second + cos(4*_angle));
                        its->second  = 0.5*(its->second + sin(4*_angle));
                    }
                    else {
                        _cosines4[v[i]] = cos(4*_angle);
                        _sines4[v[i]] = sin(4*_angle);
                    }
                }
            }
        }
    }

    propagateValues(_cosines4,eval_diffusivity,false);
    propagateValues(_sines4,eval_diffusivity,false);

    std::map<MVertex*,MVertex*>::iterator itv2 = _2Dto3D.begin();
    for ( ; itv2 != _2Dto3D.end(); ++itv2) {
        MVertex *v_2D = itv2->first;
        MVertex *v_3D = itv2->second;
        double angle = atan2(_sines4[v_3D],_cosines4[v_3D]) / 4.0;
        normalizeAngle (angle);
        angles[v_2D] = angle;
    }
}