alignment

/* compute alignment control vector */
double align_vector_x = 0, align_vector_y = 0;

if (neighbors->heading.size() > 0){
  double alignment_real = 0, alignment_imaginary = 0;

  /* use focal robot heading */
  double focal_heading = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
  alignment_real       += cos(focal_heading);
  alignment_imaginary  += sin(focal_heading);

  /* use neighbors heading */
  for(int i = 0; i < neighbors->heading.size(); i++){
    alignment_real      += cos(neighbors->heading[i]);
    alignment_imaginary += sin(neighbors->heading[i]);
  }

  /* normalize */
  double len = sqrt(pow(alignment_real, 2) + pow(alignment_imaginary, 2));
  align_vector_x = (len == 0) ? 0 : alignment_real / len;
  align_vector_y = (len == 0) ? 0 : alignment_imaginary / len;
}