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

// Returns angle between 2 vectors in degrees
double ViroManipulator::VecAngle(osg::Vec3d a, osg::Vec3d b){
	double ang;
	a.normalize();
	b.normalize();
	ang = RadiansToDegrees( acos(a * b) );
	return fabs( ang );
}

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

/**
 * Simulate a track-ball.  Project the points onto the virtual
 * trackball, then figure out the axis of rotation, which is the cross
 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 * Note:  This is a deformed trackball-- is a trackball in the center,
 * but is deformed into a hyperbolic sheet of rotation away from the
 * center.  This particular function was chosen after trying out
 * several variations.
 *
 * It is assumed that the arguments to this routine are in the range
 * (-1.0 ... 1.0)
 */
void OrbitManipulator::trackball( osg::Vec3d& axis, float& angle, float p1x, float p1y, float p2x, float p2y )
{
    /*
        * First, figure out z-coordinates for projection of P1 and P2 to
        * deformed sphere
        */

    osg::Matrixd rotation_matrix(_rotation);

    osg::Vec3d uv = Vec3d(0.0f,1.0f,0.0f)*rotation_matrix;
    osg::Vec3d sv = Vec3d(1.0f,0.0f,0.0f)*rotation_matrix;
    osg::Vec3d lv = Vec3d(0.0f,0.0f,-1.0f)*rotation_matrix;

    osg::Vec3d p1 = sv * p1x + uv * p1y - lv * tb_project_to_sphere(_trackballSize, p1x, p1y);
    osg::Vec3d p2 = sv * p2x + uv * p2y - lv * tb_project_to_sphere(_trackballSize, p2x, p2y);

    /*
        *  Now, we want the cross product of P1 and P2
        */
    axis = p2^p1;
    axis.normalize();

    /*
        *  Figure out how much to rotate around that axis.
        */
    float t = (p2 - p1).length() / ( /*2.0*/ 1.3 * _trackballSize);

    /*
        * Avoid problems with out-of-control values...
        */
    if (t > 1.0) t = 1.0;
    if (t < -1.0) t = -1.0;
    angle = inRadians(asin(t));
}

void
MorphologyViewerWidget::_get_transformation( unsigned int index
                                           , osg::Vec3d  & eye
                                           , osg::Vec3d  & center
                                           , double      & distance
                                           , osg::Vec3d  & up
                                           , osg::Vec3d  & look
                                           , osg::Vec3d  & side
                                           )
{
    osgViewer::View * view = _viewer -> getView(index);
    osgGA::TrackballManipulator * manipulator = dynamic_cast<osgGA::TrackballManipulator *>(view -> getCameraManipulator());
    manipulator -> getTransformation(eye, center, up);
    up.normalize();
    look = center - eye;
    distance = look.normalize();
    side = look ^ up;
    side.normalize();
}

void ossimPlanetSceneView::getLookDirection(osg::Vec3d& direction)const
{
   osg::Vec3 eyeTemp(0,0,0);
   osg::Vec3 center(0,0,0);
   osg::Vec3 up(0,0,0);
   
   (const_cast<ossimPlanetSceneView*>(this))->getViewMatrixAsLookAt(eyeTemp, center, up);

   direction = (center-eyeTemp);

   direction.normalize();
}

osg::Quat FaceTransform::computeQuat(osg::Vec3d direction)
{
    direction.normalize();

    double zAngle = atan2(direction.y(), direction.x());
    osg::Quat zRot(zAngle, osg::Vec3d(0,0,1));

    osg::Vec3d yAxis = zRot * osg::Vec3d(0,1,0);
    double zLength = (direction - osg::Vec3d(0,0,direction.z())).length();
    double yAngle = - atan2(direction.z(), zLength) + osg::PI/2;
    osg::Quat yRot(yAngle, yAxis);

    return zRot * yRot;
}

// Computes all intersections in line segment (p1,p2) and returns the nearest impact point in 
// vResult and its associated surface normal.
bool ViroManipulator::Intersect(osg::Vec3d p1,osg::Vec3d p2, osg::Vec3d& vResult,osg::Vec3d& vNorm){
	osg::ref_ptr<osg::LineSegment> seg = new osg::LineSegment;
	seg->set(p1,p2);
	if ( !seg->valid() ) return false;

	osgUtil::IntersectVisitor iv;
	iv.setTraversalMode(osgUtil::IntersectVisitor::TRAVERSE_ACTIVE_CHILDREN); //Only visible children
	iv.setLODSelectionMode(osgUtil::IntersectVisitor::USE_SEGMENT_START_POINT_AS_EYE_POINT_FOR_LOD_LEVEL_SELECTION);
	iv.addLineSegment(seg.get());
	iv.setEyePoint( _vEye );

	_NODE->accept( iv );

	bool hitfound = false;
	if (iv.hits()){
		osgUtil::IntersectVisitor::HitList& hitList = iv.getHitList(seg.get());
		if (!hitList.empty()){
			for(osgUtil::IntersectVisitor::HitList::iterator hitr=hitList.begin(); hitr!=hitList.end(); ++hitr){
				if( hitr->_geode.valid() && (hitr->_geode.get()->getName()!="Contraint_OutBox_Geode") && (hitr->_geode.get()->getName()!="environment")){
						osg::Vec3d ip = hitr->getWorldIntersectPoint();
						osg::Vec3d in = hitr->getWorldIntersectNormal();

						if(!hitfound){
							vResult = ip;
							vNorm   = in;
							}
						else{
							osg::Vec3d vLastIP    = vResult;
							osg::Vec3d vCurrentIP = ip;
							double dPrev    = Vec3d(_vEye - vLastIP).length();
							double dCurrent = Vec3d(_vEye - vCurrentIP).length();

							if( dCurrent < dPrev ){
								vResult = ip;
								vNorm   = in;
								}
							}
						hitfound = true;
						}
					}
				}
			}
	if (hitfound){
		vNorm.normalize();
		vNorm = RightNormal(vResult,vNorm);
		}
	
	return hitfound;
}

		void Manipulator::calcRotationArgs(osg::Vec3d& axis, float& angle, 
			float p1x, float p1y,float p2x, float p2y) {
			osg::Matrixd rotation_matrix(_rotation);

			osg::Vec3d uv = osg::Vec3d(0.0f,1.0f,0.0f)*rotation_matrix;
			osg::Vec3d sv = osg::Vec3d(1.0f,0.0f,0.0f)*rotation_matrix;
			osg::Vec3d lv = osg::Vec3d(0.0f,0.0f,-1.0f)*rotation_matrix;

			osg::Vec3d p1 = sv * p1x + uv * p1y - lv * tb_project_to_sphere(_trackball_size, p1x, p1y);
			osg::Vec3d p2 = sv * p2x + uv * p2y - lv * tb_project_to_sphere(_trackball_size, p2x, p2y);

			axis = p2^p1;
			axis.normalize();

			float t = (p2 - p1).length() / (2.0 * _trackball_size);

			if (t > 1.0) t = 1.0;
			if (t < -1.0) t = -1.0;
			angle = osg::inRadians(asin(t));
			//2014/4/28
			//2014/10/26,0.1
			angle = 0.02 * angle;
		}

//.........这里部分代码省略.........
    else if(type == 'a') {
	SceneManager::instance()->setObjectMatrix(_origMatrix);
    }

    else if(type == 'm') {

 	if(flagRot) {
	    flagRot = false;
	}

	else {
	tstart = time(0);

	zIn = 1e+10; 
	zOut = 1e+10;

	osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();

	
	osg::Vec3d tolatLon(0.573827, -2.04617,0);
	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);