C++ Vec3d::length方法代码示例

static void StateVectorToElements(const Point3d& position,
                                  const Vec3d& v,
                                  double GM,
                                  OrbitalElements* elements)
{
    Vec3d R = position - Point3d(0.0, 0.0, 0.0);
    Vec3d L = R ^ v;
    double magR = R.length();
    double magL = L.length();
    double magV = v.length();
    L *= (1.0 / magL);

    Vec3d W = L ^ (R / magR);

    // Compute the semimajor axis
    double a = 1.0 / (2.0 / magR - square(magV) / GM);

    // Compute the eccentricity
    double p = square(magL) / GM;
    double q = R * v;
    double ex = 1.0 - magR / a;
    double ey = q / sqrt(a * GM);
    double e = sqrt(ex * ex + ey * ey);

    // Compute the mean anomaly
    double E = atan2(ey, ex);
    double M = E - e * sin(E);

    // Compute the inclination
    double cosi = L * Vec3d(0, 1.0, 0);
    double i = 0.0;
    if (cosi < 1.0)
        i = acos(cosi);

    // Compute the longitude of ascending node
    double Om = atan2(L.x, L.z);

    // Compute the argument of pericenter
    Vec3d U = R / magR;
    double s_nu = (v * U) * sqrt(p / GM);
    double c_nu = (v * W) * sqrt(p / GM) - 1;
    s_nu /= e;
    c_nu /= e;
    Vec3d P = U * c_nu - W * s_nu;
    Vec3d Q = U * s_nu + W * c_nu;
    double om = atan2(P.y, Q.y);

    // Compute the period
    double T = 2 * PI * sqrt(cube(a) / GM);

    elements->semimajorAxis     = a;
    elements->eccentricity      = e;
    elements->inclination       = i;
    elements->longAscendingNode = Om;
    elements->argPericenter     = om;
    elements->meanAnomaly       = M;
    elements->period            = T;
}

static EllipticalOrbit* StateVectorToOrbit(const Point3d& position,
                                           const Vec3d& v,
                                           double mass,
                                           double t)
{
    Vec3d R = position - Point3d(0.0, 0.0, 0.0);
    Vec3d L = R ^ v;
    double magR = R.length();
    double magL = L.length();
    double magV = v.length();
    L *= (1.0 / magL);

    Vec3d W = L ^ (R / magR);

    double G = astro::G * 1e-9; // convert from meters to kilometers
    double GM = G * mass;

    // Compute the semimajor axis
    double a = 1.0 / (2.0 / magR - square(magV) / GM);

    // Compute the eccentricity
    double p = square(magL) / GM;
    double q = R * v;
    double ex = 1.0 - magR / a;
    double ey = q / sqrt(a * GM);
    double e = sqrt(ex * ex + ey * ey);

    // Compute the mean anomaly
    double E = atan2(ey, ex);
    double M = E - e * sin(E);

    // Compute the inclination
    double cosi = L * Vec3d(0, 1.0, 0);
    double i = 0.0;
    if (cosi < 1.0)
        i = acos(cosi);

    // Compute the longitude of ascending node
    double Om = atan2(L.x, L.z);

    // Compute the argument of pericenter
    Vec3d U = R / magR;
    double s_nu = (v * U) * sqrt(p / GM);
    double c_nu = (v * W) * sqrt(p / GM) - 1;
    s_nu /= e;
    c_nu /= e;
    Vec3d P = U * c_nu - W * s_nu;
    Vec3d Q = U * s_nu + W * c_nu;
    double om = atan2(P.y, Q.y);

    // Compute the period
    double T = 2 * PI * sqrt(cube(a) / GM);
    T = T / 86400.0; // Convert from seconds to days

    return new EllipticalOrbit(a * (1 - e), e, i, Om, om, M, T, t);
}

// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to )
{
    const value_type epsilon = 0.0000001;

    value_type length1  = from.length();
    value_type length2  = to.length();

    // dot product vec1*vec2
    value_type cosangle = from*to/(length1*length2);

    if ( fabs(cosangle - 1) < epsilon )
    {
        OSG_INFO<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;

        // cosangle is close to 1, so the vectors are close to being coincident
        // Need to generate an angle of zero with any vector we like
        // We'll choose (1,0,0)
        makeRotate( 0.0, 0.0, 0.0, 1.0 );
    }
    else
    if ( fabs(cosangle + 1.0) < epsilon )
    {
        // vectors are close to being opposite, so will need to find a
        // vector orthongonal to from to rotate about.
        Vec3d tmp;
        if (fabs(from.x())<fabs(from.y()))
            if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
            else tmp.set(0.0,0.0,1.0);
        else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
        else tmp.set(0.0,0.0,1.0);

        Vec3d fromd(from.x(),from.y(),from.z());

        // find orthogonal axis.
        Vec3d axis(fromd^tmp);
        axis.normalize();

        _v[0] = axis[0]; // sin of half angle of PI is 1.0.
        _v[1] = axis[1]; // sin of half angle of PI is 1.0.
        _v[2] = axis[2]; // sin of half angle of PI is 1.0.
        _v[3] = 0; // cos of half angle of PI is zero.

    }
    else
    {
        // This is the usual situation - take a cross-product of vec1 and vec2
        // and that is the axis around which to rotate.
        Vec3d axis(from^to);
        value_type angle = acos( cosangle );
        makeRotate( angle, axis );
    }
}

// Intersect ray r with the triangle abc.  If it hits returns true,
// and puts the t parameter, barycentric coordinates, normal, object id,
// and object material in the isect object
bool TrimeshFace::intersectLocal( const ray& r, isect& i ) const
{
  const Vec3d& a = parent->vertices[ids[0]];
  const Vec3d& b = parent->vertices[ids[1]];
  const Vec3d& c = parent->vertices[ids[2]];

  // tangent vectors
  Vec3d t1 = b - a;
  Vec3d t2 = c - a;
  
  Vec3d n = crossProd(t1,t2);

  double D = -n*a;

  // if the surface is parallel to the ray there is no intersection
  if(r.getDirection()*n == 0)
  {
    return false;
  }  

  double t = -(n*r.getPosition() + D)/(n*r.getDirection() );
  if (t <= RAY_EPSILON)
    return false;

  // point of intersection with the same plane (doesn't mean intersection with triangle) p(t)=p+t*d
  Vec3d p = r.at(t);

  // triangle area
  double A = n.length()/2.0;

  // barycentric coords
  double wa = crossProd(c-b, p-b).length() / (2.0*A);
  double wb = crossProd(a-c, p-c).length() / (2.0*A);
  double wc = crossProd(b-a, p-a).length() / (2.0*A);

  if((wa >= 0.0) && (wb >= 0.0) && (wc >= 0.0) && (wa+wb+wc-1.0 <= 0.00001)) {
    i.setT(t);
    i.setBary(wa, wb, wc);
    if (parent->normals.size() == 0) {
      i.setN(n);
    } else {
      Vec3d inter_n = wa*parent->normals[ids[0]] + wb*parent->normals[ids[1]]
                    + wc*parent->normals[ids[2]];
      inter_n.normalize();
      i.setN(inter_n);
    }
    i.setObject(this);
    if (parent->materials.size() == 0) {
      i.setMaterial(this->getMaterial() );
    } else {
      Material inter_m = wa*(*parent->materials[ids[0]]);
      inter_m += wb*(*parent->materials[ids[1]]);
      inter_m += wc*(*parent->materials[ids[2]]);
      i.setMaterial(inter_m);
    }
    return true;
  }

  return false;
}

bool EclipseFinder::testEclipse(const Body& receiver, const Body& caster,
                                double now) const
{
    // Ignore situations where the shadow casting body is much smaller than
    // the receiver, as these shadows aren't likely to be relevant.  Also,
    // ignore eclipses where the caster is not an ellipsoid, since we can't
    // generate correct shadows in this case.
    if (caster.getRadius() >= receiver.getRadius() * MinRelativeOccluderRadius &&
        caster.isEllipsoid())
    {
        // All of the eclipse related code assumes that both the caster
        // and receiver are spherical.  Irregular receivers will work more
        // or less correctly, but casters that are sufficiently non-spherical
        // will produce obviously incorrect shadows.  Another assumption we
        // make is that the distance between the caster and receiver is much
        // less than the distance between the sun and the receiver.  This
        // approximation works everywhere in the solar system, and likely
        // works for any orbitally stable pair of objects orbiting a star.
        Point3d posReceiver = receiver.getAstrocentricPosition(now);
        Point3d posCaster = caster.getAstrocentricPosition(now);

        const Star* sun = receiver.getSystem()->getStar();
        assert(sun != NULL);
        double distToSun = posReceiver.distanceFromOrigin();
        float appSunRadius = (float) (sun->getRadius() / distToSun);

        Vec3d dir = posCaster - posReceiver;
        double distToCaster = dir.length() - receiver.getRadius();
        float appOccluderRadius = (float) (caster.getRadius() / distToCaster);

        // The shadow radius is the radius of the occluder plus some additional
        // amount that depends upon the apparent radius of the sun.  For
        // a sun that's distant/small and effectively a point, the shadow
        // radius will be the same as the radius of the occluder.
        float shadowRadius = (1 + appSunRadius / appOccluderRadius) *
            caster.getRadius();

        // Test whether a shadow is cast on the receiver.  We want to know
        // if the receiver lies within the shadow volume of the caster.  Since
        // we're assuming that everything is a sphere and the sun is far
        // away relative to the caster, the shadow volume is a
        // cylinder capped at one end.  Testing for the intersection of a
        // singly capped cylinder is as simple as checking the distance
        // from the center of the receiver to the axis of the shadow cylinder.
        // If the distance is less than the sum of the caster's and receiver's
        // radii, then we have an eclipse.
        float R = receiver.getRadius() + shadowRadius;
        double dist = distance(posReceiver,
                               Ray3d(posCaster, posCaster - Point3d(0, 0, 0)));
        if (dist < R)
        {
            // Ignore "eclipses" where the caster and receiver have
            // intersecting bounding spheres.
            if (distToCaster > caster.getRadius())
                return true;
        }
    }

    return false;
}

double PointLight::distanceAttenuation( const Vec3d& P ) const
{

  // YOUR CODE HERE
  // These three values are the a, b, and c in the distance
  // attenuation function (from the slide labelled 
  // "Intensity drop-off with distance"):
  //    f(d) = min( 1, 1/( a + b d + c d^2 ) )
 //     float constantTerm;		// a
 //     float linearTerm;		// b
 //     float quadraticTerm;	// c

  // You'll need to modify this method to attenuate the intensity 
  // of the light based on the distance between the source and the 
  // point P.  For now, we assume no attenuation and just return 1.0

  Vec3d distance = position - P;
  double d = distance.length();
  double intensity = min(1.0, 1.0/(constantTerm + 
                                     linearTerm*d + 
                                       quadraticTerm*pow(d,2)));

  return intensity;

}

void VRConstraint::setRConstraint(Vec3d params, TCMode mode, bool local) {
    if (params.length() > 1e-4 && mode != POINT) params.normalize();
    this->local = local;
    active = true;

    if (mode == POINT) {
        setMinMax(3, params[0], params[0]);
        setMinMax(4, params[1], params[1]);
        setMinMax(5, params[2], params[2]);
    }

    if (mode == LINE) {
        auto p = Vec3d(refMatrixA[3]);
        lock({3,4});
        free({5});
        auto po = Pose::create(p, params);
        po->makeUpOrthogonal();
        setReferenceA( po );
    }

    if (mode == PLANE) {
        auto p = Vec3d(refMatrixA[3]);
        lock({4});
        free({3,5});
        auto po = Pose::create(p, Vec3d(refMatrixA[2]), params);
        po->makeDirOrthogonal();
        setReferenceA( po );
    }
}

// Apply the Blinn-Phong model to this point on the surface of the object, 
//  returning the color of that point.
Vec3d Material::shade( Scene *scene, const ray& r, const isect& i ) const
{
	// YOUR CODE HERE

		// For now, this method just returns the diffuse color of the object.
		// This gives a single matte color for every distinct surface in the
		// scene, and that's it.  Simple, but enough to get you started.
		// (It's also inconsistent with the Phong model...)

		// Your mission is to fill in this method with the rest of the phong
		// shading model, including the contributions of all the light sources.
		// You will need to call both distanceAttenuation() and shadowAttenuation()
		// somewhere in your code in order to compute shadows and light falloff.
		if (debugMode)
			std::cout << "Debugging the Phong code (or lack thereof...)" << std::endl;

		// When you're iterating through the lights,
		// you'll want to use code that looks something
		// like this:
		Vec3d light = ke(i);
		Vec3d normal = i.N;
		Vec3d iDot = r.at(i.t);
		if (r.getDirection() * normal > 0) {
			normal = -normal;
			light += prod(prod(scene->ambient(), ka(i)), kt(i));
		}
		else {
			light += prod(scene->ambient(), ka(i));
		}

		for (vector<Light*>::const_iterator litr = scene->beginLights();
			litr != scene->endLights();
			++litr)
		{
			Light* pLight = *litr;

			double distAttenuation = pLight->distanceAttenuation(iDot);
			Vec3d shadowAttenuation = pLight->shadowAttenuation(iDot);
			Vec3d atten = distAttenuation * shadowAttenuation;
			Vec3d L = pLight->getDirection(iDot);


			if (L * normal > 0) {
				Vec3d H = (L + -1 * r.getDirection());
				if (H.length() != 0)
					H.normalize();

				double sDot = max(0.0, normal * H);
				Vec3d dTerm = kd(i) * (normal * L);
				Vec3d sTerm = ks(i) * (pow(sDot, shininess(i)));
				Vec3d newLight = dTerm + sTerm;
				newLight = prod(newLight, pLight->getColor());

				light += prod(atten, newLight);
			}
		}

	return light;
}

Vec3d rectToSpherical(const Vec3d& v)
{
    double r = v.length();
    double theta = atan2(v.y, v.x);
    if (theta < 0)
        theta = theta + 2 * PI;
    double phi = asin(v.z / r);

    return Vec3d(theta, phi, r);
}

static int vector_length(lua_State* l)
{
    CelxLua celx(l);

    celx.checkArgs(1, 1, "No arguments expected for vector:length");
    Vec3d* v = this_vector(l);
    double length = v->length();
    lua_pushnumber(l, (lua_Number)length);
    return 1;
}

Vec3d PointLight::shadowAttenuation(const Vec3d& P) const
{

  // YOUR CODE HERE:
  // You should implement shadow-handling code here.
  Vec3d direction = getDirection(P);
  ray r(P, direction, ray::SHADOW );
  isect i;
  if(scene->intersect( r, i )){
    Vec3d iposition = r.at(i.t);
    Vec3d iray = iposition - P;
    Vec3d lightray = position - P;
    double dlight = lightray.length();
    double diray = iray.length();
    if(diray > dlight)
      return Vec3d(1,1,1);
    else
      return i.getMaterial().kt(i);
  } 
  else {
    return Vec3d(1,1,1);
  }
}

void VRAdjacencyGraph::compCurvatures(int range) {
    vertex_curvatures.clear();
    auto sgeo = geo.lock();
    if (!sgeo) return;

    auto pos = sgeo->getMesh()->geo->getPositions();
    auto norms = sgeo->getMesh()->geo->getNormals();
    int N = pos->size();

    /*auto curvMax = [&](int i, int range) {
		Vec3d n = norms->getValue<Vec3f>(i);
		Vec3d vi = pos->getValue<Pnt3f>(i);
		float K = 0;
		float Kmax = 0;
		auto Ne = getNeighbors(i,range);
		if (Ne.size() == 0) return K;

		for (int j : Ne) {
			if (j >= N) continue;
			Vec3d d = pos->getValue<Pnt3f>(j) - vi;
			float k = 2*n.dot(d)/d.squareLength();
			if (abs(k) > Kmax) {
                K = k;
                Kmax = abs(k);
			}
		}

		return K;
    };*/

    auto curvAvg = [&](int i, int range) {
		Vec3d n = Vec3d(norms->getValue<Vec3f>(i));
		Pnt3f vi = pos->getValue<Pnt3f>(i);
		float K = 0;
		auto Ne = getNeighbors(i,range);
		if (Ne.size() == 0) return K;

		for (int j : Ne) {
			if (j >= N) continue;
			Vec3d d = Vec3d(pos->getValue<Pnt3f>(j) - vi);
			K += 2*n.dot(d)/d.length();
		}

		K /= Ne.size();
		return K;
    };

    vertex_curvatures.resize(N);
    for (int i = 0; i < N; i++) vertex_curvatures[i] = curvAvg(i,range);
}

void TerrainManipulator::clampOrientation()
{
    if (!getVerticalAxisFixed())
    {
        Matrixd rotation_matrix;
        rotation_matrix.makeRotate(_rotation);

        Vec3d lookVector = -getUpVector(rotation_matrix);
        Vec3d upVector = getFrontVector(rotation_matrix);

        CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
        Vec3d localUp = getUpVector(coordinateFrame);
        //Vec3d localUp = _previousUp;

        Vec3d sideVector = lookVector ^ localUp;

        if (sideVector.length()<0.1)
        {
            OSG_INFO<<"Side vector short "<<sideVector.length()<<std::endl;

            sideVector = upVector^localUp;
            sideVector.normalize();
        }

        Vec3d newUpVector = sideVector^lookVector;
        newUpVector.normalize();

        Quat rotate_roll;
        rotate_roll.makeRotate(upVector,newUpVector);

        if (!rotate_roll.zeroRotation())
        {
            _rotation = _rotation * rotate_roll;
        }
    }
}

void VRCamera::focusObject(VRObjectPtr t) {
    auto bb = t->getBoundingbox();
    Vec3d c = bb->center();

    Vec3d d = getDir();
    focusPoint(c);

    Vec3d dp = getDir();
    if (dp.length() > 1e-4) d = dp; // only use new dir if it is valid
    d.normalize();
    //float r = max(bb->radius()*2, 0.1f);
    float r = bb->radius() / tan(fov*0.5);
    setFrom(c - d*r); // go back or forth to see whole node

    //cout << "VRCamera::focus " << t->getName() << " pos " << c << " size " << r << endl;
}

//--------------------------------------------------------------------------------------------------
/// Repositions and orients the camera to view the rotation point along the 
/// direction "alongDirection". The distance to the rotation point is maintained.
///
//--------------------------------------------------------------------------------------------------
void ManipulatorTrackball::setView( const Vec3d& alongDirection, const Vec3d& upDirection )
{
    if (m_camera.isNull()) return;

    Vec3d dir = alongDirection;
    if (!dir.normalize()) return;
    Vec3d up = upDirection;
    if(!up.normalize()) up = Vec3d::Z_AXIS;
    
    if((up * dir) < 1e-2) up = dir.perpendicularVector();

    Vec3d cToE = m_camera->position() - m_rotationPoint;
    Vec3d newEye = m_rotationPoint - cToE.length() * dir;

    m_camera->setFromLookAt(newEye, m_rotationPoint, upDirection);
}