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

void ball::update(ofVec3f hand, ofVec3f prev_hand, ofVec3f handVel){
	
	
	//Trigger the average tracking
	//80 is the threshold!
	if (handVel.length() > 80 && hand.z < prev_hand.z && movementCount == 0) {
		
		currentFood = floor((ofRandom(7)));
		isTracking = true;
		cout << "threw!" << endl;
	}
	//else if (handVel.length() < 10) {
	else if (movementCount > 60) {
		//Throws!		
		//isTracking = false;
		//vel /= movementCount;
		movementCount = 0;
		pos = ofVec3f (2000, 2000, 0);
		vel.set (0, 0, 0);
	}	

	//Tracks
	if(isTracking){
		pos = hand;
		vel = (handVel/3);
		//movementCount ++;
		movementCount = 1;
		isTracking = false;
	}else if (movementCount > 0){
		pos += vel;
		//movementCount = 1;
		movementCount++;
	}
	
}

// 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 ofQuaternion::makeRotate_original( const ofVec3f& from, const ofVec3f& to ) {
	const float epsilon = 0.0000001f;

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

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

	if ( fabs(cosangle - 1) < epsilon ) {
		//osg::notify(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.
			ofVec3f 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);

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

			// find orthogonal axis.
			ofVec3f axis(fromd.getCrossed(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.
			ofVec3f axis(from.getCrossed(to));
			float angle = acos( cosangle );
			makeRotate( angle, axis );
		}
}

void AbstractPhysicsBehavior::drawForceArrow(ofVec3f position,
                                             ofVec3f force) {
  ofDrawArrow(position,
              position + force * 20.0f,
              ofClamp(force.length() * 2.0f,
                      0,
                      6.0f));
}

			//----------
			ofVec2f MovingHead::getPanTiltForTargetInObjectSpace(const ofVec3f & objectSpacePoint, float tiltOffset) {
				auto pan = atan2(objectSpacePoint.z, objectSpacePoint.x) * RAD_TO_DEG - 90.0f;
				if (pan < -180.0f) {
					pan += 360.0f;
				}

				auto tilt = acos(-objectSpacePoint.y / objectSpacePoint.length()) * RAD_TO_DEG; // will always produce positive tilt
				return ofVec2f(pan, tilt + tiltOffset);
			}

void CorrelateXYZtoXY::push(ofVec3f &xyz, ofVec2f &xy) {
	if (xyz.length() < 0.1)
		return;
	
	++nPoints;
	this->xyz.push_back(toCv(xyz));
	this->xy.push_back(toCv(xy));
	
	xyzPreview.addVertex(xyz);
}

//--------------------- AccumulateForce ----------------------------------
//
//  This function calculates how much of its max steering force the 
//  vehicle has left to apply and then applies that amount of the
//  force to add.
//------------------------------------------------------------------------
bool SteeringBehaviors::AccumulateForce(ofVec3f &RunningTot, ofVec3f ForceToAdd)
{
	float MagnitudeSoFar = RunningTot.length();
	double MagnitudeRemaining = m_Vehicle->MaxForce() - MagnitudeSoFar;

	if (MagnitudeRemaining <= 0.0) return false;

	float MagnitudeToAdd = ForceToAdd.length();
	
	//if the magnitude of the sum of ForceToAdd and the running total
	//does not exceed the maximum force available to this vehicle, just
	//add together. Otherwise add as much of the ForceToAdd vector is
	//possible without going over the max.
	if (MagnitudeToAdd < MagnitudeRemaining)
	{
		RunningTot += ForceToAdd;
	}
	else
	{
		RunningTot += (ForceToAdd.getNormalized() * MagnitudeRemaining);
	}
	return true;
}

// Renders a vector object 'v' as an arrow and a location 'x,y'
void FlowField2D::drawVector(ofVec3f v, float x, float y, float scale) {
	ofPushMatrix();
	float arrowsize = 4;
	// Translate to location to render vector
	ofTranslate(x, y);
	ofSetColor(100);
	// Call vector heading function to get direction (note that pointing up is a heading of 0) and rotate
	float rotate = atan2(v.y, v.x);
	ofRotate(ofRadToDeg(rotate), 0, 0, 1);
	// Calculate length of vector & scale it to be bigger or smaller if necessary

	float len = v.length() * scale;

	// Draw three lines to make an arrow (draw pointing up since we've rotate to the proper direction)
	ofLine(0, 0, len, 0);
	ofLine(len, 0, len - arrowsize, +arrowsize / 2);
	ofLine(len, 0, len - arrowsize, -arrowsize / 2);
	ofPopMatrix();
}

bool FitPlaneToPoints(ofxRay::Plane & plane, const vector<ofVec4f> &Points, ofVec3f &Basis1, ofVec3f &Basis2, float &NormalEigenvalue, float &ResidualError)
{
    ofVec3f Centroid, Normal;

    float ScatterMatrix[3][3];
    int  Order[3];
    float DiagonalMatrix[3];
    float OffDiagonalMatrix[3];

    // Find centroid
    Centroid = ofVec3f();
    float TotalWeight = 0.0f;
    for (UINT i = 0; i < Points.size(); i++)
    {
        TotalWeight += Points[i].w;
        Centroid += ofVec3f(Points[i].x, Points[i].y, Points[i].z) * Points[i].w;
    }
    Centroid /= TotalWeight;

    // Compute scatter matrix
    Find_ScatterMatrix(Points, Centroid, ScatterMatrix, Order);

    tred2(ScatterMatrix, DiagonalMatrix, OffDiagonalMatrix);
    tqli(DiagonalMatrix, OffDiagonalMatrix, ScatterMatrix);

    /*
    **    Find the smallest eigenvalue first.
    */
    float Min = DiagonalMatrix[0];
    float Max = DiagonalMatrix[0];
    UINT MinIndex = 0;
    UINT MiddleIndex = 0;
    UINT MaxIndex = 0;
    for (UINT i = 1; i < 3; i++)
    {
        if (DiagonalMatrix[i] < Min)
        {
            Min = DiagonalMatrix[i];
            MinIndex = i;
        }
        if (DiagonalMatrix[i] > Max)
        {
            Max = DiagonalMatrix[i];
            MaxIndex = i;
        }
    }
    for (UINT i = 0; i < 3; i++)
    {
        if (MinIndex != i && MaxIndex != i)
        {
            MiddleIndex = i;
        }
    }
    /*
    **    The normal of the plane is the smallest eigenvector.
    */
    for (UINT i = 0; i < 3; i++)
    {
        Normal[Order[i]] = ScatterMatrix[i][MinIndex];
        Basis1[Order[i]] = ScatterMatrix[i][MiddleIndex];
        Basis2[Order[i]] = ScatterMatrix[i][MaxIndex];
    }
    NormalEigenvalue = abs(DiagonalMatrix[MinIndex]);
    Basis1 *= (DiagonalMatrix[MiddleIndex]) / Basis1.length();
    Basis2 *= (DiagonalMatrix[MaxIndex]) / Basis2.length();

    if (!isValid(Basis1) || !isValid(Basis2) || !isValid(Normal))
    {
        plane.setCenter(Centroid);
        plane.setNormal(ofVec3f(1, 0, 0));
        Basis1 = ofVec3f(0, 1, 0);
        Basis2 = ofVec3f(0, 0, 1);
    }
    else
    {
        plane.setCenter(Centroid);
        plane.setNormal(Normal);
    }

    ResidualError = 0.0f;
    for (UINT i = 0; i < Points.size(); i++)
    {
        ResidualError += plane.getDistanceTo(ofVec3f(Points[i].x, Points[i].y, Points[i].z));
    }
    ResidualError /= Points.size();

    return true;
}