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

void FEdgeXDetector::preProcessFace(WXFace *iFace)
{
	Vec3r firstPoint = iFace->GetVertex(0)->GetVertex();
	Vec3r N = iFace->GetNormal();

	// Compute the dot product between V (=_Viewpoint - firstPoint) and N:
	Vec3r V;
	if (_orthographicProjection) {
		V = Vec3r(0.0, 0.0, _Viewpoint.z() - firstPoint.z());
	}
	else {
		V = Vec3r(_Viewpoint - firstPoint);
	}
	N.normalize();
	V.normalize();
	iFace->setDotP(N * V);

	// compute the distance between the face center and the viewpoint:
	if (_orthographicProjection) {
		iFace->setZ(iFace->center().z() - _Viewpoint.z());
	}
	else {
		Vec3r dist_vec(iFace->center() - _Viewpoint);
		iFace->setZ(dist_vec.norm());
	}
}

void FEdgeXDetector::ProcessSilhouetteFace(WXFace *iFace, bool meshSilhouettes)
{

    real NdotVepsilonHack = 0;// 0.05; //0.1; //0.01; //0.1;

    // SILHOUETTE LAYER
    // Compute the dot products between View direction and N at each vertex
    // of the face:
    Vec3r point;
    int closestPointId = 0;
    real dist, minDist = FLT_MAX;
    int numVertices = iFace->numberOfVertices();
    WXFaceLayer * faceLayer = new WXFaceLayer(iFace, Nature::SILHOUETTE, true);

    Vec3r normal;
    if(meshSilhouettes){
        // Use per face normal
        normal = (iFace->GetVertex(2)->GetVertex() - iFace->GetVertex(0)->GetVertex()) ^ (iFace->GetVertex(1)->GetVertex() - iFace->GetVertex(0)->GetVertex());
        normal.normalize();
    }

    for(int i=0; i<numVertices; i++){
        point = iFace->GetVertex(i)->GetVertex();
        if(!meshSilhouettes){
            // Use per vertex normal
            normal = iFace->GetVertexNormal(i);
            normal.normalize();
        }
        Vec3r V(_Viewpoint - point);
        V.normalize();
        real d = normal * V + NdotVepsilonHack;
        faceLayer->PushDotP(d);
        // Find the point the closest to the viewpoint
        Vec3r dist_vec(point - _Viewpoint);
        dist = dist_vec.norm();
        if(dist < minDist) {
            minDist = dist;
            closestPointId = i;
        }

        // store ndotv at the vertex for use in the region-based visibility
        //    assert(dynamic_cast<WXVertex*>(iFace->GetVertex(i))!=NULL);
        ((WXVertex*)iFace->GetVertex(i))->setNdotV(d);
    }
    // Set the closest point id:
    faceLayer->SetClosestPointIndex(closestPointId);
    // Add this layer to the face:
    iFace->AddSmoothLayer(faceLayer);
}

  Vec3r rotateVector(const Matrix44r& mat, const Vec3r& v) {
    Vec3r res;
    for (unsigned i = 0; i < 3; i++) {
      res[i] = 0;
      for (unsigned j = 0; j < 3; j++)
	res[i] += mat(i, j) * v[j];
    }
    res.normalize();
    return res;
  }

void FEdgeXDetector::ProcessSilhouetteFace(WXFace *iFace)
{
	// SILHOUETTE LAYER
	Vec3r normal;
	// Compute the dot products between View direction and N at each vertex of the face:
	Vec3r point;
	int closestPointId = 0;
	real dist, minDist = FLT_MAX;
	int numVertices = iFace->numberOfVertices();
	WXFaceLayer *faceLayer = new WXFaceLayer(iFace, Nature::SILHOUETTE, true);
	for (int i = 0; i < numVertices; i++) {
		point = iFace->GetVertex(i)->GetVertex();
		normal = iFace->GetVertexNormal(i);
		normal.normalize();
		Vec3r V;
		if (_orthographicProjection) {
			V = Vec3r(0.0, 0.0, _Viewpoint.z() - point.z());
		}
		else {
			V = Vec3r(_Viewpoint - point);
		}
		V.normalize();
		real d = normal * V;
		faceLayer->PushDotP(d);
		// Find the point the closest to the viewpoint
		if (_orthographicProjection) {
			dist = point.z() - _Viewpoint.z();
		}
		else {
			Vec3r dist_vec(point - _Viewpoint);
			dist = dist_vec.norm();
		}
		if (dist < minDist) {
			minDist = dist;
			closestPointId = i;
		}
	}
	// Set the closest point id:
	faceLayer->setClosestPointIndex(closestPointId);
	// Add this layer to the face:
	iFace->AddSmoothLayer(faceLayer);
}

void FEdgeXDetector::preProcessFace(WXFace *iFace){
    Vec3r firstPoint = iFace->GetVertex(0)->GetVertex();
    Vec3r N = iFace->GetNormal();

    // Compute the dot product between V (=_Viewpoint - firstPoint) and N:
    Vec3r V(_Viewpoint - firstPoint);
    N.normalize();
    V.normalize();
    iFace->SetDotP(N * V);

    // compute the distance between the face center and the viewpoint:
    Vec3r dist_vec(iFace->center() - _Viewpoint);
    iFace->SetZ(dist_vec.norm());
}

    void VelocityMotor::internal_update()
    {
        // check if we have a solid
        if (mData.solid == NULL || isEnabled() == false) return ;

        Vec3r targetVelocity = mData.velocity;
        Solid * solid = mData.solid;

        Vec3r currentAchievedVelocity = solid->getGlobalLinearVel();

        if (doesGravityAffectSolid())
        {
            Vec3r gravity = mSimulator->getGravity();
            if (gravity.length() > 0)
            {
                Vec3r gravity_velocity = project(gravity, currentAchievedVelocity);
                currentAchievedVelocity -= gravity_velocity;
            }
        }

        Vec3r deltaVelocity = targetVelocity - currentAchievedVelocity;

        Vec3r forceVector = deltaVelocity / mSimulator->getStepSize() * solid->getMass();
        if (!doesGravityAffectSolid())
            forceVector -= mSimulator->getGravity() * solid->getMass();

        if (forceVector.length() > getMaximumForce())
        {
            forceVector.normalize();
            forceVector *= getMaximumForce();
        }

        Force controllingForce;
        controllingForce.duration = 0;
        controllingForce.singleStep = true;
        controllingForce.type = GLOBAL_FORCE;
        controllingForce.vec = forceVector;

        solid->addForce(controllingForce);
    }

/*! gts_vertex_principal_directions:
 *  @v: a #WVertex.
 *  @s: a #GtsSurface.
 *  @Kh: mean curvature normal (a #Vec3r).
 *  @Kg: Gaussian curvature (a real).
 *  @e1: first principal curvature direction (direction of largest curvature).
 *  @e2: second principal curvature direction.
 *
 *  Computes the principal curvature directions at a point given @Kh and @Kg, the mean curvature normal and
 *  Gaussian curvatures at that point, computed with gts_vertex_mean_curvature_normal() and
 *  gts_vertex_gaussian_curvature(), respectively.
 *
 *  Note that this computation is very approximate and tends to be unstable. Smoothing of the surface or the principal
 *  directions may be necessary to achieve reasonable results.
 */
void gts_vertex_principal_directions(WVertex *v, Vec3r Kh, real Kg, Vec3r &e1, Vec3r &e2)
{
	Vec3r N;
	real normKh;

	Vec3r basis1, basis2, d, eig;
	real ve2, vdotN;
	real aterm_da, bterm_da, cterm_da, const_da;
	real aterm_db, bterm_db, cterm_db, const_db;
	real a, b, c;
	real K1, K2;
	real *weights, *kappas, *d1s, *d2s;
	int edge_count;
	real err_e1, err_e2;
	int e;
	WVertex::incoming_edge_iterator itE;

	/* compute unit normal */
	normKh = Kh.norm();

	if (normKh > 0.0) {
		Kh.normalize();
	}
	else {
		/* This vertex is a point of zero mean curvature (flat or saddle point). Compute a normal by averaging
		 * the adjacent triangles
		 */
		N[0] = N[1] = N[2] = 0.0;

		for (itE = v->incoming_edges_begin(); itE != v->incoming_edges_end(); itE++)
			N = Vec3r(N + (*itE)->GetaFace()->GetNormal());
		real normN = N.norm();
		if (normN <= 0.0)
			return;
		N.normalize();
	}

	/* construct a basis from N: */
	/* set basis1 to any component not the largest of N */
	basis1[0] =  basis1[1] =  basis1[2] = 0.0;
	if (fabs (N[0]) > fabs (N[1]))
		basis1[1] = 1.0;
	else
		basis1[0] = 1.0;

	/* make basis2 orthogonal to N */
	basis2 = (N ^ basis1);
	basis2.normalize();

	/* make basis1 orthogonal to N and basis2 */
	basis1 = (N ^ basis2);
	basis1.normalize();

	aterm_da = bterm_da = cterm_da = const_da = 0.0;
	aterm_db = bterm_db = cterm_db = const_db = 0.0;
	int nb_edges = v->GetEdges().size();

	weights = (real *)malloc(sizeof(real) * nb_edges);
	kappas = (real *)malloc(sizeof(real) * nb_edges);
	d1s = (real *)malloc(sizeof(real) * nb_edges);
	d2s = (real *)malloc(sizeof(real) * nb_edges);
	edge_count = 0;

	for (itE = v->incoming_edges_begin(); itE != v->incoming_edges_end(); itE++) {
		WOEdge *e;
		WFace *f1, *f2;
		real weight, kappa, d1, d2;
		Vec3r vec_edge;
		if (!*itE)
			continue;
		e = *itE;

		/* since this vertex passed the tests in gts_vertex_mean_curvature_normal(), this should be true. */
		//g_assert(gts_edge_face_number (e, s) == 2);

		/* identify the two triangles bordering e in s */
		f1 = e->GetaFace();
		f2 = e->GetbFace();

		/* We are solving for the values of the curvature tensor
		 *     B = [ a b ; b c ].
		 *  The computations here are from section 5 of [Meyer et al 2002].
		 *
		 *  The first step is to calculate the linear equations governing the values of (a,b,c). These can be computed
		 *  by setting the derivatives of the error E to zero (section 5.3).
//.........这里部分代码省略.........

FEdge *ViewEdgeXBuilder::BuildSmoothFEdge(FEdge *feprevious, const OWXFaceLayer& ifl)
{
	WOEdge *woea, *woeb;
	real ta, tb;
	SVertex *va, *vb;
	FEdgeSmooth *fe;
	// retrieve exact silhouette data
	WXSmoothEdge *se = ifl.fl->getSmoothEdge();

	if (ifl.order) {
		woea = se->woea();
		woeb = se->woeb();
		ta = se->ta();
		tb = se->tb();
	}
	else {
		woea = se->woeb();
		woeb = se->woea();
		ta = se->tb();
		tb = se->ta();
	}

	Vec3r normal;
	// Make the 2 Svertices
	if (feprevious == 0) { // that means that we don't have any vertex already built for that face
		Vec3r A1(woea->GetaVertex()->GetVertex());
		Vec3r A2(woea->GetbVertex()->GetVertex());
		Vec3r A(A1 + ta * (A2 - A1));

		va = MakeSVertex(A, false);
		// Set normal:
		Vec3r NA1(ifl.fl->getFace()->GetVertexNormal(woea->GetaVertex()));
		Vec3r NA2(ifl.fl->getFace()->GetVertexNormal(woea->GetbVertex()));
		Vec3r na((1 - ta) * NA1 + ta * NA2);
		na.normalize();
		va->AddNormal(na);
		normal = na;

		// Set CurvatureInfo
		CurvatureInfo *curvature_info_a =
		        new CurvatureInfo(*(dynamic_cast<WXVertex*>(woea->GetaVertex())->curvatures()),
		                          *(dynamic_cast<WXVertex*>(woea->GetbVertex())->curvatures()), ta);
		va->setCurvatureInfo(curvature_info_a);
	}
	else {
		va = feprevious->vertexB();
	}

	Vec3r B1(woeb->GetaVertex()->GetVertex());
	Vec3r B2(woeb->GetbVertex()->GetVertex());
	Vec3r B(B1 + tb * (B2 - B1));

	if (feprevious && (B - va->point3D()).norm() < 1.0e-6)
		return feprevious;

	vb = MakeSVertex(B, false);
	// Set normal:
	Vec3r NB1(ifl.fl->getFace()->GetVertexNormal(woeb->GetaVertex()));
	Vec3r NB2(ifl.fl->getFace()->GetVertexNormal(woeb->GetbVertex()));
	Vec3r nb((1 - tb) * NB1 + tb * NB2);
	nb.normalize();
	normal += nb;
	vb->AddNormal(nb);

	// Set CurvatureInfo
	CurvatureInfo *curvature_info_b =
	        new CurvatureInfo(*(dynamic_cast<WXVertex*>(woeb->GetaVertex())->curvatures()),
	                          *(dynamic_cast<WXVertex*>(woeb->GetbVertex())->curvatures()), tb);
	vb->setCurvatureInfo(curvature_info_b);

	// Creates the corresponding feature edge
	fe = new FEdgeSmooth(va, vb);
	fe->setNature(ifl.fl->nature());
	fe->setId(_currentFId);
	fe->setFrsMaterialIndex(ifl.fl->getFace()->frs_materialIndex());
	fe->setFace(ifl.fl->getFace());
	fe->setFaceMark(ifl.fl->getFace()->GetMark());
	if (feprevious == 0)
		normal.normalize();
	fe->setNormal(normal);
	fe->setPreviousEdge(feprevious);
	if (feprevious)
		feprevious->setNextEdge(fe);
	_pCurrentSShape->AddEdge(fe);
	va->AddFEdge(fe);
	vb->AddFEdge(fe);

	++_currentFId;
	ifl.fl->userdata = fe;
	return fe;
}

bool Line::intersect(const CylinderVolume &cyl, 
                           Real           &enter,  
                           Real           &exit ) const
{
    Real  radius = cyl.getRadius();

    Vec3r adir;
    Vec3r o_adir;
    Pnt3r apos;

    cyl.getAxis(apos, adir);

    o_adir = adir;
    adir.normalize();

    bool isect;

    Real  ln;
    Real  dl;
    Vec3r RC;
    Vec3r n;
    Vec3r D;

    RC = _pos - apos;

    n  = _dir.cross (adir);
    ln =  n  .length(    );

    if(ln == 0.f)    // IntersectionLine is parallel to CylinderAxis
    {
        D  = RC - (RC.dot(adir)) * adir;
        dl = D.length();

        if(dl <= radius)   // line lies in cylinder
        {
            enter = 0.f;
            exit  = Inf;
        }
        else
        {
            return false;
        }
    }
    else
    {
        n.normalize();

        dl    = osgAbs(RC.dot(n));        //shortest distance
        isect = (dl <= radius);

        if(isect)
        {                 // if ray hits cylinder
            Real  t;
            Real  s;
            Vec3r O;

            O = RC.cross(adir);
            t = - (O.dot(n)) / ln;
            O = n.cross(adir);

            O.normalize();

            s = osgAbs (
                (osgSqrt ((radius * radius) - (dl * dl))) / (_dir.dot(O)));

            exit = t + s;

            if(exit < 0.f)
                return false;

            enter = t - s;

            if(enter < 0.f)
                enter = 0.f;
        }
        else
        {
            return false;
        }
    }

    Real t;

    Plane bottom(-adir, apos);

    if(bottom.intersect(*this, t))
    {
        if(bottom.isInHalfSpace(_pos))
        {
            if(t > enter) 
                enter = t;
        }
        else
        {
            if(t < exit) 
                exit = t;
        }
    }
    else
    {
//.........这里部分代码省略.........