C++ Vec3r类代码示例

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

bool Line::intersect(const SphereVolume &sphere,
                           Real         &enter, 
                           Real         &exit  ) const
{
    Vec3r v;
    Pnt3r center;

    sphere.getCenter(center);

    Real radius;
    Real h;
    Real b;
    Real d;
    Real t1;
    Real t2;

    radius = sphere.getRadius();

    v = center - _pos;

    h = (v.dot(v))-(radius * radius);
    b = (v.dot(_dir));

    if(h >= 0.f && b <= 0.f)
        return false;

    d = b * b - h;

    if(d < 0.f)
        return false;

    d  = osgSqrt(d);
    t1 = b - d;

//    if (t1 > 1)
//        return false;

    t2 = b + d;

    if( t1 < TypeTraits<Real>::getDefaultEps() )
    {
        if( t2 < TypeTraits<Real>::getDefaultEps() /*|| t2 > 1*/)
        {
            return false;
        }
    }

    enter = t1;
    exit  = t2;

    return true;
}

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

inline static real angle(WOEdge *h)
{
	const Vec3r& n1 = h->GetbFace()->GetNormal();
	const Vec3r& n2 = h->GetaFace()->GetNormal();
	const Vec3r v = h->GetVec();
	real sine = (n1 ^ n2) * v / v.norm();
	if (sine >= 1.0) {
		return M_PI / 2.0;
	}
	if (sine <= -1.0) {
		return -M_PI / 2.0;
	}
	return ::asin(sine);
}

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

static real angle_from_cotan(WVertex *vo, WVertex *v1, WVertex *v2)
{
	/* cf. Appendix B and the caption of Table 1 from [Meyer et al 2002] */
	real udotv, denom;

	Vec3r u (v1->GetVertex() - vo->GetVertex());
	Vec3r v(v2->GetVertex() - vo->GetVertex());

	udotv = u * v;
	denom = sqrt(u.squareNorm() * v.squareNorm() - udotv * udotv);

	/* Note: I assume this is what they mean by using atan2(). -Ray Jones */

	/* tan = denom/udotv = y/x (see man page for atan2) */
	return (fabs(atan2(denom, udotv)));
}

void MonaghanKernel::monaghanGradient( const Vec3r& r, Vec3r& gradient )
{
    HReal dist = r.length();
    HReal q = dist*m_invH;
    gradient.fill(0.0);
    if( q >= 0 && q < 1 )
    {
        HReal scalar = -3.0f*(2-q)*(2-q);
        scalar += 12.0f*(1-q)*(1-q);
        gradient = (m_g*m_invH*scalar/dist)*r;
    }
    else if ( q >=1 && q < 2 )
    {
        HReal scalar = -3.0f*(2-q)*(2-q);
        gradient = (m_g*scalar*m_invH/dist)*r;
    }
}

void Grid::castInfiniteRay(const Vec3r& orig,
			   const Vec3r& dir,
			   OccludersSet& occluders,
			   unsigned timestamp) {
  Vec3r end = Vec3r(orig + FLT_MAX * dir / dir.norm());
  bool inter = initInfiniteRay(orig, dir, timestamp);
  if(!inter)
      return;
  allOccludersGridVisitor visitor(occluders);
  castRayInternal(visitor);
}

// precondition1: P is inside the sphere
// precondition2: P,V points to the outside of the sphere (i.e. OP.V > 0)
static bool sphere_clip_vector(const Vec3r& O, real r, const Vec3r& P, Vec3r& V)
{
	Vec3r W = P - O;
	real a = V.squareNorm();
	real b = 2.0 * V * W;
	real c = W.squareNorm() - r * r;
	real delta = b * b - 4 * a * c;
	if (delta < 0) {
		// Should not happen, but happens sometimes (numerical precision)
		return true;
	}
	real t = - b + ::sqrt(delta) / (2.0 * a);
	if (t < 0.0) {
		// Should not happen, but happens sometimes (numerical precision)
		return true;
	}
	if (t >= 1.0) {
		// Inside the sphere
		return false;
	}

	V[0] = (t * V.x());
	V[1] = (t * V.y());
	V[2] = (t * V.z());

	return true;
}

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

void ThirdPersonCamera::LookAt(const Vec3r &eye, const Vec3r &target, const Vec3r &up)
{
    m_eye = eye;
    m_target = target;
    m_targetYAxis = up;

    m_zAxis = eye - target;
    m_zAxis.Normalise();

    m_viewDir = m_zAxis * -1;

    m_xAxis = Vec3r::CrossProduct(up, m_zAxis);
    m_xAxis.Normalise();

    m_yAxis = Vec3r::CrossProduct(m_zAxis, m_xAxis);
    m_yAxis.Normalise();
   // m_xAxis.Normalise();

    m_viewMatrix.Access(0,0) = m_xAxis.X;
    m_viewMatrix.Access(1,0) = m_xAxis.Y;
    m_viewMatrix.Access(2,0) = m_xAxis.Z;
	m_viewMatrix.Access(3,0) = -Vec3r::DotProduct(m_xAxis, eye);

    m_viewMatrix.Access(0,1) = m_yAxis.X;
    m_viewMatrix.Access(1,1) = m_yAxis.Y;
    m_viewMatrix.Access(2,1) = m_yAxis.Z;
	m_viewMatrix.Access(3,1) = -Vec3r::DotProduct(m_yAxis, eye);
				
    m_viewMatrix.Access(0,2) = m_zAxis.X;
    m_viewMatrix.Access(1,2) = m_zAxis.Y;
    m_viewMatrix.Access(2,2) = m_zAxis.Z;    
	m_viewMatrix.Access(3,2) = -Vec3r::DotProduct(m_zAxis, eye);

	m_orientation.FromMatrix(m_viewMatrix.m_matrix);

    Vec3r offset = m_target - m_eye;

	m_offsetDistance = offset.Length();
}

HReal MonaghanKernel::monaghanValue( const Vec3r & r )
{
    HReal value = 0.0;
    HReal q = r.length()*m_invH;
    if( q >= 0 && q < 1 )
    {
        value = m_v*( (2-q)*(2-q)*(2-q) - 4.0f*(1-q)*(1-q)*(1-q));
    }
    else if ( q >=1 && q < 2 )
    {
        value = m_v*( (2-q)*(2-q)*(2-q) );
    }
    else
    {
        value = 0.0f;
    }
    return value;
}

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

real CurvePoint::curvature2d_as_angle() const
{
#if 0
	Vec3r edgeA = (_FEdges[0])->orientation2d();
	Vec3r edgeB = (_FEdges[1])->orientation2d();
	Vec2d N1(-edgeA.y(), edgeA.x());
	N1.normalize();
	Vec2d N2(-edgeB.y(), edgeB.x());
	N2.normalize();
	return acos((N1 * N2));
#endif
	if (__A == 0)
		return __B->curvature2d_as_angle();
	if (__B == 0)
		return __A->curvature2d_as_angle();
	return ((1 - _t2d) * __A->curvature2d_as_angle() + _t2d * __B->curvature2d_as_angle());
}