old/src/main42a.cpp
double follow_neighbour(double x_end, double y_end,\
                        FiducialProxy &neighborFinder, int iD)
{     double total_dist_to_neighbor = 1.0;
      for(int i =0; i< neighborFinder.GetCount(); i++){
            double x_dist = neighborFinder.GetFiducialItem(i).pose.px - x_end;
            double y_dist = neighborFinder.GetFiducialItem(i).pose.py - y_end;
// Assume Hawk is faster than Swan, and Swan is faster than Sparrow, we have:
//            total_dist_to_neighbor = total_dist_to_neighbor + sqrt(x_dist*x_dist*x_dist*x_dist+y_dist*y_dist*y_dist*y_dist)/abs(x_dist*y_dist); // Hawk Finsler distance
            if (iD >= SWARM_SIZE/7)
              total_dist_to_neighbor = total_dist_to_neighbor + sqrt(x_dist*x_dist*x_dist*x_dist+y_dist*y_dist*y_dist*y_dist-x_dist*y_dist*x_dist*y_dist)/2; // Accurate Swan Finsler distance
            else
 total_dist_to_neighbor = total_dist_to_neighbor + pow(abs(x_dist*x_dist*x_dist-x_dist*x_dist*y_dist+x_dist*y_dist*y_dist-y_dist*y_dist*y_dist),2/3)/(6/3)/2; // Accurate Sparrow Finsler distance
      }
      return total_dist_to_neighbor;
}
/**
 * determine the anarchism-authoritism  distance away from all neighbors
 **/
determine the anarchism-authoritism  distance away from all neighbors
determine the anarchism - authoritism distance away from all neighbors
double follow_neighbour(double x_end, double y_end,\
                        FiducialProxy &neighborFinder, int iD)
{     double total_dist_to_neighbor = 1.0;
      for(int i =0; i< neighborFinder.GetCount(); i++){
            double x_dist = neighborFinder.GetFiducialItem(i).pose.px - x_end;
            double y_dist = neighborFinder.GetFiducialItem(i).pose.py - y_end;
            if (iD >= SWARM_SIZE/7)
              total_dist_to_neighbor = total_dist_to_neighbor + sqrt(x_dist*x_dist*x_dist*x_dist+y_dist*y_dist*y_dist*y_dist-x_dist*y_dist*x_dist*y_dist)/2; 
            else
 total_dist_to_neighbor = total_dist_to_neighbor + pow(abs(x_dist*x_dist*x_dist-x_dist*x_dist*y_dist+x_dist*y_dist*y_dist-y_dist*y_dist*y_dist),2/3)/(6/3)/2; 
      }
      return total_dist_to_neighbor;
}
double follow_neighbour ( double x_end , double y_end , FiducialProxy & neighborFinder , int iD ) { double total_dist_to_neighbor = 1.0 ; for ( int i = 0 ; i < neighborFinder . GetCount ( ) ; i ++ ) { double x_dist = neighborFinder . GetFiducialItem ( i ) . pose . px - x_end ; double y_dist = neighborFinder . GetFiducialItem ( i ) . pose . py - y_end ; if ( iD >= SWARM_SIZE / 7 ) total_dist_to_neighbor = total_dist_to_neighbor + sqrt ( x_dist * x_dist * x_dist * x_dist + y_dist * y_dist * y_dist * y_dist - x_dist * y_dist * x_dist * y_dist ) / 2 ; else total_dist_to_neighbor = total_dist_to_neighbor + pow ( abs ( x_dist * x_dist * x_dist - x_dist * x_dist * y_dist + x_dist * y_dist * y_dist - y_dist * y_dist * y_dist ) , 2 / 3 ) / ( 6 / 3 ) / 2 ; } return total_dist_to_neighbor ; }
determine the anarchism-authoritism  distance away from all neighbors
determine the anarchism - authoritism distance away from all neighbors
// Assume Hawk is faster than Swan, and Swan is faster than Sparrow, we have: //            total_dist_to_neighbor = total_dist_to_neighbor + sqrt(x_dist*x_dist*x_dist*x_dist+y_dist*y_dist*y_dist*y_dist)/abs(x_dist*y_dist); // Hawk Finsler distance // Accurate Swan Finsler distance // Accurate Sparrow Finsler distance