bool JointHitDetector::DetectHit(ofxVec3f& vDir)
{
	int numTrackedPoints = mPoints.size();
	if (numTrackedPoints < 3)
		return false;
   
	ofxVec3f A = mPoints[1] - mPoints[0];
	ofxVec3f B = mPoints[2] - mPoints[1];
   
	for (int i=2; i<=numTrackedPoints; ++i)
	{
      //printf("*** i = %i\n", i);
      //printf("A = (%f,%f,%f)   B = (%f,%f,%f)\n", A.x, A.y, A.z, B.x, B.y, B.z);
		ofxVec3f C = mPoints[numTrackedPoints > i ? i : i-1] - mPoints[i-1];
      ofxVec3f normA = A.getNormalized();
      ofxVec3f normB = B.getNormalized();
      
      static float _shortEnough = 30.0f;
      if (LongEnough(A) && (normA.dot(normB) < .1f || B.length() < _shortEnough))
      {
         vDir = A;
         for (int j=0; j<i; ++j) //erase the A part, keep the B
            mPoints.erase(mPoints.begin());
         return true;
      }
		else if (normA.dot(normB) > .8f || (LongEnough(A) && normA.dot(normB) > .4f))
		{
			A = A+B; B = C;
		}
		else
		{
			A = B; B = C;
		}
	}
   
	return false;
}

bool JointHitDetector::LongEnough(const ofxVec3f& vec) const
{
   return vec.lengthSquared() > mRequiredLength * mRequiredLength;
}