find peaks PP

gistfile1.cpp

		static vector <int> getPeaks( ofPolyline & l, float angleThresh, int numToLook, bool bInvert, bool bGroup, int numToSkip = 0 ){
			vector <int> results; 			
			if( l.size() >= 3 ){
			
				for(int k = 0; k < l.size(); k++){ 
				
					ofPoint normal; 
					bool bPeak = true;
					
					for(int j = 1; j < numToLook; j++){
						int L = k - j; 
						int R = k + j; 
						
						int Lp = L+1;
						int Rp = R-1;
						
						if( L < 0 ) L += l.size(); 
						if( R >= l.size() ) R -= l.size(); 
						if( Lp < 0 ) Lp += l.size(); 
						if( Rp >= l.size() ) Rp -= l.size(); 
						if( Rp < 0 ) Rp += l.size(); 
						if( Lp >= l.size() ) Lp -= l.size(); 
						
						if( j == 1 ){
							ofPoint delta = l[R] - l[L]; 
							normal.set(-delta.y, delta.x);
							if( bInvert ){
								normal *= -1.0;
							}
						}
						
						ofPoint vecL = l[L] - l[Lp];
						ofPoint vecR = l[R] - l[Rp];

						float angle1 = normal.angle(vecL);						
						float angle2 = normal.angle(vecR);
						
						//cout << " normal is " << normal << " vecL " << vecL << " angle1 = " << angle1 << "    vecR is " << vecR << " angle 2 = " << angle2 << endl; 
						
						if( fabs(angle1) < 90+angleThresh || fabs(angle2) < 90+angleThresh ){
							bPeak = false;
							break;
						}
						
					}
						
					if(  bPeak )results.push_back(k);
				}
			}
			
			if( results.size() && bGroup ){
			
				bool bCollecting = false;
				
				int findGoodStart = 0;
				for(int k = 0; k < l.size(); k++){ 
					bool bFound = false; 
					for(int j = 0; j < results.size(); j++){
						if( results[j] == k ){
							bFound = true; 
							break; 
						}
					}
					if( bFound == false){
						findGoodStart = k;
						break; 
					}
				}
				
				vector <int> newResults; 
				vector <int> curCollection;
				
				int notFoundCount = 0;
				
				for(int k = 0; k < l.size(); k++){ 
					int d = k + findGoodStart; 
					if( d >= l.size() ) d -= l.size(); 
					
					bool bFound = false; 
					for(int j = 0; j < results.size(); j++){
						if( results[j] == d ){
							bFound = true; 
							//TODO: erase from here 
							break; 
						}
					}
					
					if( bFound && bCollecting == false ){
						bCollecting = true;
						curCollection.clear();
						notFoundCount = 0;
					}
					
					if( bCollecting ){
						if( bFound ){
							curCollection.push_back(d); 
						}
						if( !bFound ){
							notFoundCount++;
						}
						if( ( !bFound && notFoundCount > numToSkip ) || k == l.size()-1 ){
							if( curCollection.size() == 1 ){
								newResults.push_back(curCollection[0]); 
							}else if( curCollection.size() > 1 ){
								int res = curCollection[curCollection.size()/2]; 
								newResults.push_back(res); 
							}
							bCollecting = false;
						}
					}
					
				}
				
				results = newResults;
				
			}
			
			return results; 
		}