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

double MultitouchNavigation::angleBetween3DVectors(osg::Vec3 v1, osg::Vec3 v2)
{
    // http://codered.sat.qc.ca/redmine/projects/spinframework/repository/revisions/b6245189c19a7c6ba4fdb126940321c41c44e228/raw/src/spin/osgUtil.cpp

    // normalize vectors (note: this must be done alone, not within any vector arithmetic. why?!)
    v1.normalize();
    v2.normalize();

    // Get the dot product of the vectors
    double dotProduct = v1 * v2;

    // for acos, the value has to be between -1.0 and 1.0, but due to numerical imprecisions it sometimes comes outside this range
    if (dotProduct > 1.0)
        dotProduct = 1.0;
    if (dotProduct < -1.0)
        dotProduct = -1.0;

    // Get the angle in radians between the 2 vectors (should this be -acos ? ie, negative?)
    double angle = acos(dotProduct);

    // Here we make sure that the angle is not a -1.#IND0000000 number, which means indefinite
    if (std::isnan(angle)) //__isnand(x)
        return 0;

    // Return the angle in radians
    return (angle);
}

bool
HeightFieldUtils::getNormalAtNormalizedLocation(const osg::HeightField* input,
                                                double nx, double ny,
                                                osg::Vec3& output,
                                                ElevationInterpolation interp)
{
    double xcells = (double)(input->getNumColumns()-1);
    double ycells = (double)(input->getNumRows()-1);

    double w = input->getXInterval() * xcells * 111000.0;
    double h = input->getYInterval() * ycells * 111000.0;

    double ndx = 1.0/xcells;
    double ndy = 1.0/ycells;

    double xmin = osg::clampAbove( nx-ndx, 0.0 );
    double xmax = osg::clampBelow( nx+ndx, 1.0 );
    double ymin = osg::clampAbove( ny-ndy, 0.0 );
    double ymax = osg::clampBelow( ny+ndy, 1.0 );

    osg::Vec3 west (xmin*w, ny*h, getHeightAtNormalizedLocation(input, xmin, ny, interp));
    osg::Vec3 east (xmax*w, ny*h, getHeightAtNormalizedLocation(input, xmax, ny, interp));
    osg::Vec3 south(nx*w, ymin*h, getHeightAtNormalizedLocation(input, nx, ymin, interp));
    osg::Vec3 north(nx*w, ymax*h, getHeightAtNormalizedLocation(input, nx, ymax, interp));

    output = (west-east) ^ (north-south);
    output.normalize();
    return true;
}

// -------------------------------------------------------------------------
void Hermite::getTangentOnSegment(osg::Vec3& tangent, double distance, unsigned int segment)
{
	float fH1 = ( 6.0f * distance - 6.0f) * distance;
	float fH2 = ( 3.0f * distance - 4.0f) * distance + 1.0f;
	float fH3 = ( 3.0f * distance - 2.0f) * distance;
	float fH4 = (-6.0f * distance + 6.0f) * distance;

	const ControlPoint& begin = (*_segments)[segment].getBegin();
	const ControlPoint& end   = (*_segments)[segment].getEnd();

	tangent = begin.position * fH1 + begin.tangent * fH2 + end.tangent * fH3 + end.position * fH4;
	tangent.normalize();
	
	/*
	tangent[0] = begin->position.x * fH1 +
		begin->tangent.x  * fH2 +
		end->tangent.x    * fH3 +
		end->position.x   * fH4;
	tangent[1] = begin->position.y * fH1 +
		begin->tangent.y  * fH2 +
		end->tangent.y    * fH3 +
		end->position.y   * fH4;
	tangent[2] = begin->position.z * fH1 +
		begin->tangent.z  * fH2 +
		end->tangent.z    * fH3 +
		end->position.z   * fH4;

	Math::normalize(tangent);
	*/
	
}

    void normal(osg::Vec3 v1, osg::Vec3 v2, osg::Vec3 v3)
    {
        osg::Vec3 u,v;

        // right hand system, CCW triangle
        u = v2 - v1;
        v = v3 - v1;
        triNormal = u^v;
        triNormal.normalize();
    }

/**
 * Retrieves the normal of the terrain at the specified location.
 *
 * @param x the x coordinate to check
 * @param y the y coordinate to check
 * @param normal the location at which to store the normal
 * instead of triangle mesh height
 */
void SurfaceObject::GetNormal(float x, float y, osg::Vec3& normal)
{
      float z = GetHeight(x, y);

      osg::Vec3 v1(0.1f, 0.0f, GetHeight(x + 0.1f, y) - z);
      osg::Vec3 v2(0.0f, 0.1f, GetHeight(x, y + 0.1f) - z );

      normal = v1 ^ v2;

      normal.normalize();

      /*   if (normal.x() != 0 ||  normal.y() != 0 || normal.z() != 1 )
   	std::cout<<"NORM "<<"("<<normal.x()<<",\t"<<normal.y()<<",\t"<<normal.z()<<")"<<std::endl;
*/
}

bool
TileModel::ElevationData::getNormal(const osg::Vec3d&      ndc,
                                    const GeoLocator*      ndcLocator,
                                    osg::Vec3&             output,
                                    ElevationInterpolation interp ) const
{
    if ( !_locator.valid() || !ndcLocator )
    {
        output.set(0,0,1);
        return false;
    }

    double xcells = (double)(_hf->getNumColumns()-1);
    double ycells = (double)(_hf->getNumRows()-1);
    double xres = 1.0/xcells;
    double yres = 1.0/ycells;

    osg::Vec3d hf_ndc;
    GeoLocator::convertLocalCoordBetween( *ndcLocator, ndc, *_locator.get(), hf_ndc );

    float centerHeight = HeightFieldUtils::getHeightAtNormalizedLocation(_hf.get(), hf_ndc.x(), hf_ndc.y(), interp);

    osg::Vec3d west ( hf_ndc.x()-xres, hf_ndc.y(), 0.0 );
    osg::Vec3d east ( hf_ndc.x()+xres, hf_ndc.y(), 0.0 );
    osg::Vec3d south( hf_ndc.x(), hf_ndc.y()-yres, 0.0 );
    osg::Vec3d north( hf_ndc.x(), hf_ndc.y()+yres, 0.0 );

    if (!HeightFieldUtils::getHeightAtNormalizedLocation(_neighbors, west.x(),  west.y(),  west.z(), interp))
        west.z() = centerHeight;
    if (!HeightFieldUtils::getHeightAtNormalizedLocation(_neighbors, east.x(),  east.y(),  east.z(), interp))
        east.z() = centerHeight;
    if (!HeightFieldUtils::getHeightAtNormalizedLocation(_neighbors, south.x(), south.y(), south.z(), interp))
        south.z() = centerHeight;
    if (!HeightFieldUtils::getHeightAtNormalizedLocation(_neighbors, north.x(), north.y(), north.z(), interp))
        north.z() = centerHeight;

    osg::Vec3d westWorld, eastWorld, southWorld, northWorld;
    _locator->unitToModel(west,  westWorld);
    _locator->unitToModel(east,  eastWorld);
    _locator->unitToModel(south, southWorld);
    _locator->unitToModel(north, northWorld);

    output = (eastWorld-westWorld) ^ (northWorld-southWorld);
    output.normalize();

    return true;
}

/*
 * Ok, 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 Vwr::CameraManipulator::trackball(osg::Vec3& 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::Matrix rotation_matrix(_rotation);

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

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

	/*
	 *  Now, we want the cross product of P1 and P2
	 */

	// Robert,
	//
	// This was the quick 'n' dirty  fix to get the trackball doing the right
	// thing after fixing the Quat rotations to be right-handed.  You may want
	// to do something more elegant.
	//   axis = p1^p2;
	axis = p2 ^ p1;
	axis.normalize();

	/*
	 *  Figure out how much to rotate around that axis.
	 */
	float t = (p2 - p1).length() / (2.0 * _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 dtAnim::GetCelestialCoordinates(osg::Vec3 target,
                                     const osg::Vec3 &lookForward,                                    
                                     float &azimuth, 
                                     float &elevation)
{  
   target.normalize();

   // Derive the reference frame for the "look" pose
   osg::Vec3 frameRight = lookForward ^ osg::Z_AXIS;
   osg::Vec3 frameUp    = frameRight ^ lookForward;

   osg::Matrix frameMatrix(frameRight.x(),  frameRight.y(),  frameRight.z(),  0.0f,                            
                           lookForward.x(), lookForward.y(), lookForward.z(), 0.0f,
                           frameUp.x(),     frameUp.y(),     frameUp.z(),     0.0f,
                           0.0f,            0.0f,            0.0f,            1.0f);  

   // Transform the target into the proper coordinate frame
   target = frameMatrix * target;

   // Project our target vector onto the xy plane 
   // in order to calculate azimuth
   osg::Vec3f targetRight = target ^ osg::Z_AXIS;
   targetRight.normalize();

   osg::Vec3f targetForward = osg::Z_AXIS ^ targetRight;
   targetForward.normalize();    

   // Use the projected vector to calculate the azimuth
   // between the projection and the character forward
   float lookDotTargetForward   = targetForward * osg::Y_AXIS;
   float targetDotTargetForward = targetForward * target;

   // acos needs it's parameter between -1 and 1
   dtUtil::Clamp(lookDotTargetForward, -1.0f, 1.0f);
   dtUtil::Clamp(targetDotTargetForward, -1.0f, 1.0f);

   // We need to manually determine the sign
   float azimuthSign   = ((target * osg::X_AXIS) > 0.0f) ? -1.0f: 1.0f;    
   float elevationSign = ((target * osg::Z_AXIS) > 0.0f) ? 1.0f: -1.0f;

   // We can use the angle between the projection
   // and the original target to determine elevation
   elevation  = acos(targetDotTargetForward) * elevationSign;
   azimuth    = acos(lookDotTargetForward) * azimuthSign;
}

bool
HeightFieldUtils::getNormalAtNormalizedLocation(const osg::HeightField* input,
                                                double nx, double ny,
                                                osg::Vec3& output,
                                                ElevationInterpolation interp)
{
    double dx = 1.0/(double)(input->getNumColumns()-1);
    double dy = 1.0/(double)(input->getNumRows()-1);

    double xmin = osg::clampAbove( nx-dx, 0.0 );
    double xmax = osg::clampBelow( nx+dx, 1.0 );
    double ymin = osg::clampAbove( ny-dx, 0.0 );
    double ymax = osg::clampBelow( ny+dy, 1.0 );

    osg::Vec3 west (xmin, ny, getHeightAtNormalizedLocation(input, xmin, ny, interp));
    osg::Vec3 east (xmax, ny, getHeightAtNormalizedLocation(input, xmax, ny, interp));
    osg::Vec3 south(nx, ymin, getHeightAtNormalizedLocation(input, nx, ymin, interp));
    osg::Vec3 north(nx, ymax, getHeightAtNormalizedLocation(input, nx, ymax, interp));

    output = (west-east) ^ (north-south);
    output.normalize();
    return true;
}

void Wire::cutWire(osg::Vec3 normal, float dist)
{

    normal.normalize();

    DrawArrayLengths *newPrimitives = new DrawArrayLengths(PrimitiveSet::POLYGON);
    UShortArray *newIndices = new UShortArray();

    int startIndex = 0;

    int index = vert->getNumElements();
    for (unsigned int i = 0; i < primitives->getNumPrimitives(); i++)
    {
        float oldd = -1;
        int numInd = 0;
        for (int v = 0; v < (*primitives)[i]; v++)
        {
            osg::Vec3 p = (*vert)[indices->index(startIndex + v)];
            float d = (p - normal * dist) * normal;
            /* Grundidee:
			* wir laufen allen Punkte von jedem Polygon einmal
			* durch; liegt der Punkte im "Verbleib" Bereich, wird
			* wird er einfach kopiert, liegt er ausserhalb, dann
			* wird er weggelassen
			* !! Immer wenn ein Wechsel zwischen "verbleibt" und\
			* und "weglassen" erfolgt, dann muss mindestens ein
			* zusaetzlicher Punkt eingefuegt werden.
			*/
            if (v == 0 && d >= 0)
            {
                newIndices->push_back((*indices)[startIndex + v]);
                numInd++;
            }
            else
            {
                if (d < 0)
                {
                    if (oldd > 0)
                    {
                        osg::Vec3 b = (*vert)[(*indices)[startIndex + v - 1]];
                        osg::Vec3 r = p - b;
                        float t = (dist - normal * p) / (normal * r);
                        osg::Vec3 newP = p + r * t;
                        osg::Vec3 n1 = (*normals)[(*indices)[startIndex + v]];
                        osg::Vec3 n2 = (*normals)[(*indices)[startIndex + v - 1]];
                        osg::Vec3 newN = n1 + (n1 - n2) * t;
                        vert->push_back(newP);
                        normals->push_back(newN);
                        newIndices->push_back(index);
                        index++;
                        numInd++;
                    }
                    else
                    {
                    }
                }
                else
                {
                    if (oldd < 0)
                    {
                        osg::Vec3 b = (*vert)[(*indices)[startIndex + v - 1]];
                        osg::Vec3 r = p - b;
                        float t = (dist - normal * p) / (normal * r);
                        osg::Vec3 newP = p + r * t;
                        osg::Vec3 n1 = (*normals)[(*indices)[startIndex + v]];
                        osg::Vec3 n2 = (*normals)[(*indices)[startIndex + v - 1]];
                        osg::Vec3 newN = n1 + (n1 - n2) * t;
                        vert->push_back(newP);
                        normals->push_back(newN);
                        newIndices->push_back(index);
                        index++;
                        numInd++;
                    }

                    newIndices->push_back((*indices)[startIndex + v]);
                    numInd++;
                }
            }
            oldd = d;
        }

        osg::Vec3 p = (*vert)[indices->index(startIndex)];
        float d = (p - normal * dist) * normal;

        if (d < 0)
        {
            if (oldd > 0)
            {
                osg::Vec3 b = (*vert)[(*indices)[startIndex + (*primitives)[i] - 1]];
                osg::Vec3 r = p - b;
                float t = (dist - normal * p) / (normal * r);
                osg::Vec3 newP = p + r * t;
                osg::Vec3 n1 = (*normals)[(*indices)[startIndex]];
                osg::Vec3 n2 = (*normals)[(*indices)[startIndex + (*primitives)[i] - 1]];
                osg::Vec3 newN = n1 + (n1 - n2) * t;
                vert->push_back(newP);
                normals->push_back(newN);
                newIndices->push_back(index);
                index++;
                numInd++;
//.........这里部分代码省略.........