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

osg::Matrixd ViewingCore::computeProjection() const
{
    if( !( _scene.valid() ) ) {
        osg::notify( osg::WARN ) << "ViewingCore::computeProjection: _scene == NULL." << std::endl;
        return( osg::Matrixd::identity() );
    }

    // TBD do we really want eyeToCenter to be a vector
    // to the *bound* center, or to the *view* center?
    const osg::BoundingSphere& bs = _scene->getBound();
    const osg::Vec3d eyeToCenter( bs._center - getEyePosition() );
    if( _ortho ) {
        double zNear = eyeToCenter.length() - bs._radius;
        double zFar = eyeToCenter.length() + bs._radius;

        const double xRange = _aspect * ( _orthoTop - _orthoBottom );
        const double right = xRange * .5;

        return( osg::Matrixd::ortho( -right, right, _orthoBottom, _orthoTop, zNear, zFar ) );
    } else {
        double zNear = eyeToCenter.length() - bs._radius;
        double zFar = zNear + ( bs._radius * 2. );
        if( zNear < 0. ) {
            zNear = zFar / 2000.; // Default z ratio.
        }
        return( osg::Matrixd::perspective( _fovy, _aspect, zNear, zFar ) );
    }
}

void CameraFlight::navigate(osg::Matrix destMat, osg::Vec3 destVec)
{
    osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();

    switch(_flightMode)
    {
	case INSTANT:{
	    cout<<"USING INSTANT"<<endl;
	    SceneManager::instance()->setObjectMatrix(destMat);
	    break;
	}
	case SATELLITE:
	    cout<<"USING SATELLITE"<<endl;

	    t = 0.0;
	    total = 0.0;
	
    	    objMat.decompose(trans2, rot2, scale2, so2);
	    a = (maxHeight - trans2[1])/25.0;

	    map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
					destVec.x(),destVec.y(),destVec.z(),toVec.x(),toVec.y(),toVec.z());

	    fromVec = origPlanetPoint;

	    fromVec.normalize();
	    toVec.normalize();

	    origAngle = acos((fromVec * toVec)/((fromVec.length() * toVec.length())));	
	    origAngle = RadiansToDegrees(origAngle);

	    angle = origAngle;

    	    if(origAngle <= 10) {
		maxHeight = 6.5e+9;
	    }

	    else {
		maxHeight = 2.0e+10;
	    }

	    flagRot = true;
	    break;
	case AIRPLANE:
	    cout<<"USING AIRPLANE"<<endl;
	    break;
	default:
	    cout<<"PICK THE ALGORYTHM!!!!"<<endl;
	    break;
    }
}

bool
GeoMath::isPointVisible(const osg::Vec3d& eye,
                        const osg::Vec3d& target,
                        double            R)
{
    double r2 = R*R;

    // same quadrant:
    if ( eye * target >= 0.0 )
    {
        double d2 = eye.length2();
        double horiz2 = d2 - r2;
        double dist2 = (target-eye).length2();
        if ( dist2 < horiz2 )
        {
            return true;
        }
    }

    // different quadrants:
    else
    {
        // there's a horizon between them; now see if the thing is visible.
        // find the triangle formed by the viewpoint, the target point, and 
        // the center of the earth.
        double a = (target-eye).length();
        double b = target.length();
        double c = eye.length();

        // Heron's formula for triangle area:
        double s = 0.5*(a+b+c);
        double area = 0.25*sqrt( s*(s-a)*(s-b)*(s-c) );

        // Get the triangle's height:
        double h = (2*area)/a;

        if ( h >= R )
        {
            return true;
        }
    }

    return false;
}

void
Horizon::setEye(const osg::Vec3d& eyeECEF)
{
    _cv = osg::componentMultiply(eyeECEF, _scale);

    double cvMag2 = _cv*_cv;

    osg::Vec3d minCV = _cv;
    minCV.normalize();
    minCV = osg::componentMultiply(minCV, _scaleToMinHAE);
    double min_cvMag2 = minCV*minCV;

#if 0 // debugging
    osg::Vec3d msl = _cv;
    msl.normalize();
    msl = osg::componentMultiply(msl, _scale+osg::Vec3d(1,1,1));
    msl = osg::componentMultiply(msl, _scaleInv);
    double alt = eyeECEF.length() - msl.length();
#endif

    if ( cvMag2 >= min_cvMag2 )
    {
        _vhMag2 = cvMag2-1.0;
    }
    else
    {
        _cv = minCV;
        _vhMag2 = (_cv*_cv)-1.0;
    }
    
#if 0 // debugging
    osg::Vec3d vh = _cv;
    vh.normalize();
    vh = osg::componentMultiply(vh, (_scale*sqrt(_vhMag2))+osg::Vec3d(1,1,1));
    vh = _scaleInv * sqrt(_vhMag2);

    static int count=0;
    if (count++ %60 == 0) {
        OE_NOTICE << "cvmag2="<< cvMag2 << "; minMag2="<< min_cvMag2 << "; vhMag2=" << _vhMag2 << "; alt=" << alt << "; vh=" << vh.length() << "\n";
    }
#endif
}

//.........这里部分代码省略.........
	osg::Vec3d tolatLon1(0.622566, 2.43884, 0);
	osg::Vec3d toVec1, toVec2;

	map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
					tolatLon.x(),tolatLon.y(),tolatLon.z(),
					toVec1.x(),toVec1.y(),toVec1.z());

//	map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
//					tolatLon1.x(),tolatLon1.y(),tolatLon1.z(),
//					toVec2.x(),toVec2.y(),toVec2.z());

	fromVec = origPlanetPoint;

	toVec1.normalize();
//	toVec2.normalize();
	fromVec.normalize();
/*
	osg::Vec3 offset(0.0,1.0,0.0);

	offset = offset - fromVec;
	fromVec = fromVec + offset;
	toVec1 = toVec1 + offset;
	toVec2 = toVec2 + offset;


	printVec(fromVec);
        printVec(toVec1);
        printVec(toVec2);
*/
	cout<<endl;
	toVec = toVec1;

	double dot = fromVec * toVec;
	angle = acos(dot/((fromVec.length() * toVec.length())));	

	angle = RadiansToDegrees(angle);

	rotAngle = angle/100.0;
	cout<<angle<<endl; 	
	flagRot = true;
}
//	osg::Vec3 crsVec = toVec1^toVec2;
//	crsVec.normalize();

//	cout<<"Where you at"<<endl;
//	printVec(fromVec);

//	cout<<"UCSD"<<endl;
//	printVec(toVec1);

//	cout<<"Tokyo"<<endl;
//	printVec(toVec2);

	//osg::Vec3 rotVec = (fromVec ^ toVec);
	//rotVec.normalize();
	//origPlanetPoint.normalize();
	//latLonHeight.normalize();

//	printVec(crsVec);	
 //       osg::Matrix rotM;
//	rotM.makeRotate(DegreesToRadians(1.0),crsVec);



	//printVec(origPlanetPoint);
//	printVec(origPlanetPoint);