void disturbMidpoint(ofVec3f & midPoint, ofVec3f const & bottomLeft, ofVec3f const & topLeft, ofVec3f const & topRight, ofVec3f const & bottomRight) {
      //if (std::rand() % 2 == 0) {
      //  return;
      //}
        midPoint[2] += (topLeft.distance(bottomRight) + topRight.distance(bottomLeft)) * randomValue();
        
        if (midPoint[2] < minHeight) {
            minHeight = midPoint[2];
        }
        if (midPoint[2] > maxHeight) {
            maxHeight = midPoint[2];
        }

    }

// needs improvement. right now it just looks for the biggest cross product
void approximatePlane(const vector<ofVec3f>& points, int iterations, ofVec3f& center, ofVec3f& normal)
{
	int n = points.size();
	for(int i = 0; i < n; i++)
	{
		center += points[i];
	}
	center /= n;
	float maxLength = 0;
	for(int i = 0; i < n; i++)
	{
		ofVec3f side1 = points[i] - center;
		for(int j = i + 1; j < n; j++)
		{
			ofVec3f side2 = points[j] - center;
			ofVec3f curNormal = side1.getCrossed(side2);
			if(curNormal.z < 0) {
				curNormal *= -1;
			}
			float length = curNormal.length();
			if(length > maxLength)
			{
				normal = curNormal;
				maxLength = length;
			}
		}
	}
	normal.normalize();
}

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

ofVec3f findNormal(ofVec3f a, ofVec3f b, ofVec3f ref) {
    a = a - ref;
    b = b - ref;
    ofVec3f v = a.cross(b);
    v.normalize();
    return v;
}

ofVec3f GrainViewTransform(ofVec3f InPutPoint, ofVec3f Eye, ofVec3f Target, ofVec3f UpVec){

// Transform the cordinates of a point InPutPoint. For a camera placed at Eye and looking at target with Up vector UpVec
    
    ofMatrix4x4 ViewMat;
    ofVec3f zaxis(Target - Eye);
    zaxis.normalize();
    ofVec3f xaxis = UpVec.getCrossed(zaxis);
    xaxis.normalize();
    ofVec3f yaxis = zaxis.getCrossed(xaxis);
    // translate vect
    
    InPutPoint -= Eye;
    
    // project on each axis
    
    ofVec3f Outpos;
    
    Outpos.x = -xaxis.dot(InPutPoint);
    Outpos.y = -yaxis.dot(InPutPoint);
    Outpos.z = zaxis.dot(InPutPoint);
    
    return Outpos;
    
}

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

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

//--------------------------------------------------------------
ofVec3f ofApp::moveback(ofVec3f fltPos, ofVec3f aggPos,float aggRadius, float fltRadius){
    float disAct = fltPos.distance(aggPos);
    float disShould = aggRadius + fltRadius;
    ofVec3f tpmoveBack = fltPos - aggPos;
    tpmoveBack.normalize();
    tpmoveBack = tpmoveBack * (disShould - disAct);
    fltPos = fltPos + tpmoveBack;
    return fltPos;
}

ofVec3f ofCairoRenderer::transform(ofVec3f vec){
	if(!b3D) return vec;
	vec = modelView.preMult(vec);
	vec = projection.preMult(vec);

	//vec.set(vec.x/vec.z*viewportRect.width*0.5-ofGetWidth()*0.5-viewportRect.x,vec.y/vec.z*viewportRect.height*0.5-ofGetHeight()*0.5-viewportRect.y);
	vec.set(vec.x/vec.z*ofGetWidth()*0.5,vec.y/vec.z*ofGetHeight()*0.5);
	return vec;
}

			//----------
			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 testApp::update(){
    
    ofVec3f diff ;          //Difference between particle and mouse
    float dist ;            //distance from particle to mouse ( as the crow flies ) 
    float ratio ;           //Ratio of how strong the effect is = 1 + (-dist/maxDistance) ;
    const ofVec3f mousePosition = ofVec3f( mouseX , mouseY , 0 ) ; //Allocate and retrieve mouse values once.
    const ofVec3f origin = ofVec3f(0,0,0);
    //Create an iterator to cycle through the vector
    std::vector<Particle>::iterator p ; 
    for ( p = particles.begin() ; p != particles.end() ; p++ ) 
    {
        ratio = 1.0f ; 
        p->velocity *= friction ; 
        //reset acceleration every frame
        p->acceleration = ofVec3f() ; 
        diff = mousePosition - p->position ;  
        dist = mousePosition.distance( p->position ) ; 
        //If within the zone of interaction
        if ( dist * .5 < forceRadius )  
        {
            ratio = -1 + dist / forceRadius ; 
            //Repulsion
            if ( cursorMode == 0 ) 
                p->acceleration -= ( diff * ratio) ;
            //Attraction
            else
                p->acceleration += ( diff * ratio ) ; 
        }
        if ( springEnabled ) 
        {
            //Move back to the original position
            p->acceleration += springFactor * (p->spawnPoint - p->position );
        }
        
        p->velocity += p->acceleration * ratio ; 
        p->position += p->velocity ; 
    }
    
    if ( ofGetFrameNum() % 300 == 0 ) 
    {
        curImageIndex++ ;
        setupParticles() ;
    }
}

//----------------------------------------
void ofxViewportCam::frameBoundingBox(const ofVec3f &minCorner,
                                     const ofVec3f &maxCorner)
{
    ofVec3f center((minCorner + maxCorner) * 0.5f);
    setTargetPosition(center);
    this->distance = minCorner.distance(maxCorner);
    
    setPosition(target.getGlobalPosition());
    dolly(this->distance);
}

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

//--------------------------------------------------------------
void rotateToNormal(ofVec3f normal) {
	normal.normalize();

	float rotationAmount;
	ofVec3f rotationAngle;
	ofQuaternion rotation;

	ofVec3f axis(0, 0, 1);
	rotation.makeRotate(axis, normal);
	rotation.getRotate(rotationAmount, rotationAngle);
	ofRotateDeg(rotationAmount, rotationAngle.x, rotationAngle.y, rotationAngle.z);
}

//----------------------------------------
void ofNode::lookAt(const ofVec3f& lookAtPosition, ofVec3f upVector) {
	if(parent) upVector = upVector * ofMatrix4x4::getInverseOf(parent->getGlobalTransformMatrix());
	ofVec3f zaxis = (getGlobalPosition() - lookAtPosition).normalized();
	ofVec3f xaxis = upVector.getCrossed(zaxis).normalized();
	ofVec3f yaxis = zaxis.getCrossed(xaxis);
	
	ofMatrix4x4 m;
	m._mat[0].set(xaxis.x, xaxis.y, xaxis.z, 0);
	m._mat[1].set(yaxis.x, yaxis.y, yaxis.z, 0);
	m._mat[2].set(zaxis.x, zaxis.y, zaxis.z, 0);
	
	setGlobalOrientation(m.getRotate());
}

	void Graphics2552::rotateToNormal(ofVec3f normal) {
		normal.normalize();

		float rotationAmount;
		ofVec3f rotationAngle;
		ofQuaternion rotation;

		ofVec3f axis(0, 0, 1);
		rotation.makeRotate(axis, normal);
		rotation.getRotate(rotationAmount, rotationAngle);
		logVerbose("ofRotate " + ofToString(rotationAmount));
		ofRotate(rotationAmount, rotationAngle.x, rotationAngle.y, rotationAngle.z);
	}

ofVec3f RotateAux(ofVec3f input, float latitud, float azimuth, ofVec3f pivot){


    ofVec3f theOut = input.getRotated(azimuth, ofVec3f(1, 0, 0));
    
    theOut = theOut.getRotated(latitud, ofVec3f(0, 1, 0));
    
    theOut += pivot;

    return theOut;


}

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

//--------------------------------------------------
void ofPolyline::calcData(int index, ofVec3f &tangent, float &angle, ofVec3f &rotation, ofVec3f &normal) const {
    int i1 = getWrappedIndex(index - 1);
    int i2 = getWrappedIndex(index);
    int i3 = getWrappedIndex(index + 1);
    
    ofPoint p1(points[i1]);
    ofPoint p2(points[i2]);
    ofPoint p3(points[i3]);
    
    ofVec3f v1(p1 - p2); // vector to previous point
    ofVec3f v2(p3 - p2); // vector to next point
    v1.normalize();
    v2.normalize();
    
    tangent = (v2 - v1);
    tangent.normalize();
    
    rotation = v1.getCrossed(v2);
    angle = 180 - ofRadToDeg(acos(ofClamp(v1.x * v2.x + v1.y * v2.y + v1.z * v2.z, -1, 1)));

    normal = rightVector.getCrossed(tangent);
    normal.normalize();
}

void ofxARToolkitPlus::getTranslationAndOrientation(int markerIndex, ofVec3f &translation, ofMatrix4x4 &orientation) {
	
	ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);

	getTransMat( &marker, c, m34 );
	
	// Translation
	translation.set(m34[0][3], m34[1][3], m34[2][3]);
	
	// Orientation
	orientation.set(m34[0][0], m34[0][1], m34[0][2], 0,
					m34[1][0], m34[1][1], m34[1][2], 0,
					m34[2][0], m34[2][1], m34[2][2], 0,
					0, 0, 0, 1);
}