shared_ptr<handle> handle::press(ofVec2f t_vec, int ha) {

    //mouse coordinates already translated in testApp
    shared_ptr<handle> ptr;

    m_isActive = true;

    if(!m_parent && m_children.size() == 0 || ha == HA_VEC_SPAWN) {

        ptr = spawnHandle();
        m_inputMapper = shared_ptr<inputMapper>(new vecInput()); //need to think how parameters for this are set
        shared_ptr<vecInput> t_vi(static_pointer_cast <vecInput>(m_inputMapper));

        if(t_vec.length() > 10){
            t_vi->setDirGlobal(t_vec.getNormalized());
        }else{
            t_vi->setDirGlobal(ofVec2f(0,1));
        }

    }

    if(m_inputMapper) {
        reset();
        m_inputMapper->start();
    }


    return ptr;

}

 //-------------------------------------------------------------------
 static float getTriangleRadius(ofVec2f v1, ofVec2f v2, ofVec2f v3) {
     
     float a = v1.distance(v2);
     float b = v2.distance(v3);
     float c = v3.distance(v1);
     
     float k = 0.5 * (a+b+c);
     
     float r = sqrt( k * ((k-a)*(k-b)*(k-c)) ) / k;
     return r;
 }

 //-------------------------------------------------------------------
 static ofVec2f getTriangleCenter(ofVec2f v1, ofVec2f v2, ofVec2f v3) {
     
     float a = v2.distance(v3);
     float b = v1.distance(v3);
     float c = v1.distance(v2);
     float d = a+b+c;
     
     float ix = ((a * v1.x) + (b * v2.x) + (c * v3.x)) / d;
     float iy = ((a * v1.y) + (b * v2.y) + (c * v3.y)) / d;
     
     return ofVec2f(ix, iy);
 }

void View::DrawGravityAt(ofPoint position, const Model &model) const {
  const ofVec2f gravity = OpenFrameworksVector(model.GravityAt(Box2dVector(position)));
  ofPushStyle();
  ofPushMatrix();
  ofTranslate(position.x, position.y);
  ofScale(0.5, 0.5);
  ofSetColor(ofColor::slateGrey, 64.0);
  const ofVec2f arrowhead = gravity / 9.81;
  ofTriangle(arrowhead, kBallRadius * arrowhead.perpendiculared(),
             -kBallRadius * arrowhead.perpendiculared());
  ofPopMatrix();
  ofPopStyle();
}

void FlowField::setDirection(ofVec2f _dir) {
    _dir.normalize();
    for( int y=0; y<flowList.size(); y++){
        for( int x=0; x<flowList[y].size(); x++){
            flowList[y][x] += _dir;
        }
    }
}

int BSpline::GetControlPointAtPosition(ofVec2f position)
{
	for (int i = 0; i < controlPoints.size(); ++i) {
		if (position.distance(controlPoints[i]) < 10)
			return i;
	}
	return -1;
}

bool TriangleBrush::checkAngle(ofVec2f p1, ofVec2f p2, ofVec2f p3){
            float dist1 = p1.distance(p2);
            float dist2 = p1.distance(p3);
            if (dist2 < dist1) {
                return true;
            }else{
                return false;
            }
}

float getNearestVertex(const ofMesh& mesh, const ofVec2f& target, int& vertexIndex) {
	float bestDistance = 0;
	for(int i = 0; i < mesh.getNumVertices(); i++) {
		float distance = target.distance(mesh.getVertex(i));
		if(distance < bestDistance || i == 0) {
			bestDistance = distance;
			vertexIndex = i;
		}
	}
	return bestDistance;
}

ofVec2f closestPointOnRay(const ofVec2f& p1, const ofVec2f& p2, const ofVec2f& p3) {
	if(p1 == p2) {
		return p1;
	}
	
	float u = (p3.x - p1.x) * (p2.x - p1.x);
	u += (p3.y - p1.y) * (p2.y - p1.y);
	float len = (p2 - p1).length();
	u /= (len * len);
	
	return p1.getInterpolated(p2, u);
}

void menu::interaccion_point(ofVec2f ptF, bool isNeg) {
	float minDis = ofGetHeight()/3.0;// (isNeg)? 400 : 200;
	
	float minDis2 = minDis*minDis;
	
	float ff = 1.0;

	float fFuerza = 45.0;
	if(ptF.distance(centro)<distConf) {
//	if(ptF.squareDistance(centro)<distConf) {
		if(isNeg) {
			// Atrae circulos y repele boxes
			for(int i=0; i<circles.size(); i++) {
//				float dis = ptF.distance(circles[i].get()->getPosition());
				float dis2 = ptF.squareDistance(circles[i].get()->getPosition());
//				if(dis < minDis) 
//				if(dis2 < minDis2) 
//					circles[i].get()->addRepulsionForce(ptF, ff*1.4f*fFuerza/dis2);//3, 9);
//				else 
//					circles[i].get()->addAttractionPoint(ptF, ff*2.2*fFuerza/dis2);//4.0);
				
				circles[i].get()->addAttractionPoint(ptF, ff*2.2*fFuerza/dis2);//4.0);
				
			}
			for(int i=0; i<boxes.size(); i++) {
				float dis = ptF.distance(boxes[i].get()->getPosition());
				float dis2 = dis*dis;//ptF.squareDistance(boxes[i].get()->getPosition());
//				if(dis < minDis) 
//				if(dis2 < minDis2) 
//					boxes[i].get()->addAttractionPoint(ptF, ff*1.2*fFuerza/dis2);
//				else 
//					boxes[i].get()->addRepulsionForce(ptF, ff*0.8*fFuerza/dis2);//4.0);
				boxes[i].get()->addRepulsionForce(ptF, ff*0.9*fFuerza/dis2);//4.0);
			}
		}
		else {
			// Mouse Pressed
			// Atrae boxes y repele circulos
			for(int i=0; i<circles.size(); i++) {
//				float dis = ptF.distance(circles[i].get()->getPosition());
				float dis2 = ptF.squareDistance(circles[i].get()->getPosition());
//				if(dis < minDis) 
//				if(dis2 < minDis2) 
//					circles[i].get()->addAttractionPoint(ptF, ff*1.2*fFuerza/dis2);//3, 9);
//				else 
					circles[i].get()->addRepulsionForce(ptF, ff*0.8*fFuerza/dis2);//4.0);
			}
			for(int i=0; i<boxes.size(); i++) {
				float dis = ptF.distance(boxes[i].get()->getPosition());
				float dis2 = ptF.squareDistance(boxes[i].get()->getPosition());
//				if(dis < minDis) 
				if(dis2 < minDis2) 
//					boxes[i].get()->addRepulsionForce(ptF, ff*1.4*fFuerza/dis);
//				else 
					boxes[i].get()->addAttractionPoint(ptF, ff*2.2*fFuerza/dis2);//4.0);
			}
		}
	}

}

bool getTriangleFromSegmentSize(double _szA, double _szB, double _szC, double x0, double y0, double _angle, double _scale, ofVec2f & _ptA, ofVec2f & _ptB, ofVec2f & _ptC) {

    double r0 = _szB * _scale;
    double x1 = 0 + _szA * _scale;
    double y1 = 0;
    double r1 = _szC * _scale;

    vector<ofVec2f> _points;

    circlesIntersection(0, 0, r0, x1, y1, r1, _points);

    if(_points.size() == 0) return false; // dont intersect --- sides dont make a triangle

    _ptA.set(0, 0);
    _ptB.set(x1, y1);
    _ptC.set(_points[1].x, _points[1].y);

    _ptC.x = _ptB.x + (_ptA.x - _ptC.x);
    _ptC.y = _ptB.y + (_ptA.y - _ptC.y);

    double _sin = sin(ofDegToRad(_angle));
    double _cos = cos(ofDegToRad(_angle));

    ofVec2f _pivot = (_ptA + _ptB + _ptC) / 3.0;

    _ptA -= _pivot;
    _ptB -= _pivot;
    _ptC -= _pivot;

    _ptA = rotatePoint(_ptA.x, _ptA.y, 0, 0, 0, 0, _sin, _cos);
    _ptB = rotatePoint(_ptB.x, _ptB.y, 0, 0, 0, 0, _sin, _cos);
    _ptC = rotatePoint(_ptC.x, _ptC.y, 0, 0, 0, 0, _sin, _cos);

    _ptA += ofVec2f(x0,y0);
    _ptB += ofVec2f(x0,y0);
    _ptC += ofVec2f(x0,y0);

    return true;
}

vector<ofVec2f> PyramidBrush::sortClosest(ofVec2f p, vector<ofVec2f> coords){
    vector<ofVec2f> pos;
    float maxDist = ofGetWidth();
    int closestIndex = 0;
    for (int i = 0; i < coords.size(); i++) {
        float dist = p.distance(coords[i]);
        if (dist < maxDist){
            maxDist = dist;
            pos.push_back(coords[i]);
        }
    }
    return pos;
}

void KinectProjector::drawArrow(ofVec2f projectedPoint, ofVec2f v1)
{
    float angle = ofRadToDeg(atan2(v1.y,v1.x));
    float length = v1.length();
    ofFill();
    ofPushMatrix();
    ofTranslate(projectedPoint);
    ofRotate(angle);
    ofSetColor(255,0,0,255);
    ofDrawLine(0, 0, length, 0);
    ofDrawLine(length, 0, length-7, 5);
    ofDrawLine(length, 0, length-7, -5);
    ofPopMatrix();
}

void handle::pivot(float f, ofVec2f p, ofVec2f t, string jType, string actor){

    ofVec2f np(m_posC);

    if(jType == "weld"){

        np = m_posC.getRotated(f, p, ofVec3f(0,0,1));
        t.set(np - m_posC);
        m_posC.set(np);
        m_rotC += f;

    }else if(jType == "pivot"){
        if(m_posC != p){
            m_posC += t;
        }
    }

    //recursive function call on children
    vector <handleJoint>::iterator it = m_children.begin();

    while(it != m_children.end()){
        if(it->m_handle->getIndex() == actor){++it; continue;} //actor is to prevent double calls
        if(it->m_type == "free"){++it; continue;}
        if(jType == "weld" &&  it->m_type == "weld"){
            it->m_handle->pivot(f, p, t, "weld", m_index);
        }else if(jType == "pivot" || it->m_type == "pivot"){
            if(m_posC != p)it->m_handle->pivot(f, p, t, "pivot", m_index);
        }
        ++it;
    }

    //pivot the parent
    if(m_parent){
        if(actor != m_parent->getIndex()){

            handleJoint hj = m_parent->getChildJoint(m_index);

            if(jType == "weld" &&  hj.m_type == "weld"){
                m_parent->pivot(f, p, t, "weld", m_index);
            }else if(jType == "pivot" || hj.m_type == "pivot"){
                if(m_posC != p)m_parent->pivot(f, p, t, "pivot", m_index);
            }

        }
    }

}

ofVec2f Simple3DTracker::_predictNextPosition(ofVec2f currentPosition, float* minCost)
{
    int bestx = currentPosition.x, besty = currentPosition.y;
    float bestcost = 9999999, cost, distance;
    const float alpha = _weightedMatchingCoefficient;

    if(!_template || !_tmp || !_tmp2)
        return currentPosition;

    // template matching
    IplImage* haystack = _cameraImage();
    cvMatchTemplate(haystack, _template->getCvImage(), _tmp2, CV_TM_CCOEFF);
    cvNormalize(_tmp2, _tmp2, 1.0, 0.0, CV_MINMAX);

    // find the best match
    for(int y = 0; y < _tmp2->height; y++) {
        const float *src = (const float*)(_tmp2->imageData + y * _tmp2->widthStep);
        unsigned char *dst = (unsigned char*)(_tmp->getCvImage()->imageData + y * _tmp->getCvImage()->widthStep);
        for(int x = 0; x < _tmp2->width; x++) {
            dst[x] = (unsigned char)(src[x] * 255.0f);
            distance = currentPosition.distance(ofVec2f(x, y));
            if(distance <= _lookupRadius) {
                cost = (alpha * (1.0f - src[x])) + ((1.0f - alpha) * distance / _lookupRadius);
                if(cost <= bestcost) { // weighted matching
                    bestx = x;
                    besty = y;
                    bestcost = cost;
                }
            }
        }
    }
    _tmp->flagImageChanged();

    // get the resulting position...
    ofVec2f result(bestx + _template->width/2, besty + _template->height/2);

    // return the min cost?
    if(minCost)
        *minCost = bestcost;

    // update the template?
    if(result.distance(currentPosition) >= UPDATETPL_MINDIST)
        _setTemplate(result);

    // done!
    return result;
}

ofVec2f closestPointOnLine(const ofVec2f& p1, const ofVec2f& p2, const ofVec2f& p3) {
    if(p1 == p2) {
        return p1;
    }

    float u = (p3.x - p1.x) * (p2.x - p1.x);
    u += (p3.y - p1.y) * (p2.y - p1.y);
    float len = (p2 - p1).length();
    u /= (len * len);

    // clamp u
    if(u > 1) {
        u = 1;
    } else if(u < 0) {
        u = 0;
    }
    return p1.getInterpolated(p2, u);
}

	ofVec2f Camera::undistortCoordinate(const ofVec2f & xy) const {
		const int distortionLength = this->distortion.size();
		
		if (this->distortion.size() < 2) {
			return xy;
		}
		
		float r = xy.length();
		float rr = r*r;
		
		float rad_coeff = 1.0f + distortion[0] * rr + distortion[1] * rr * rr;
		if (distortionLength > 4) {
			rad_coeff += distortion[4] * rr * rr * rr;
		}
		
		float xn = xy.x * rad_coeff;
		float yn = xy.y * rad_coeff;
		
		xn += 2 * distortion[2] * xn * yn + distortion[3] * (rr + 2 * xn * xn);
		yn += distortion[2] * (rr + 2 * yn * yn) + 2 * distortion[3] * xn * yn;
		
		return ofVec2f(xn, yn);
	}

Confetti::Confetti(ofVec2f origin, ofVec2f velVector, float divergenceFactor, float tempStrengthVar, int tempId){
	
	particleId = tempId; 
	
	color = ofColor(ofRandom(200),ofRandom(200),ofRandom(200));
	//color = ofColor(255);
	gravity = ofVec3f(0,1,0);
	
	strengthVariation = tempStrengthVar;
	externalForce = ofVec2f(0,0);
	
	pos = ofVec3f(origin.x, origin.y, 0);
	
	accel = velVector;
	accel.z = ofRandom(-100,100);
	
	divergence = velVector.getRotated(ofRandom(-divergenceFactor,divergenceFactor));

	//vel = ofVec3f(velVector.x, velVector.y, velVector.x); // ASSIGNO X AL Z
	vel = accel;
	
	vel.x += divergence.x;
	vel.y += divergence.y;
	//vel.z += divergence.x;
	
	strength = (ofRandom(1 - strengthVariation,1));
	
	//strength = 1;
	vel *= strength;
	
	accel = 0;

	
	rotation = ofRandom(360);
	rotationStep = ofRandom(-10,10);

}

bool select_point(ofVec2f f, ofVec2f t, float r)
{
	if ( f.distance(t) < r ) return true;
	else					 return false;
}

bool Sphere::isPointOverCenter(ofVec2f p, float radiusMultiply)
{
    return p.distance(center) < SphereTones::sphereRadius * radiusMultiply;
}