Esempio n. 1
0
/*
  Calculate the line segment PaPb that is the shortest route between
  two lines P1P2 and P3P4. Calculate also the values of mua and mub where
  Pa = P1 + mua (P2 - P1)
  Pb = P3 + mub (P4 - P3)
  Return FALSE if no solution exists.
*/
Segment3D get_shortest_segment(const Segment3D &sa,
                               const Segment3D &sb) {
  const double eps= .0001;
  algebra::Vector3D va= sa.get_point(1) - sa.get_point(0);
  algebra::Vector3D vb= sb.get_point(1) - sb.get_point(0);
  double ma= va*va;
  double mb= vb*vb;
  // if one of them is too short to have a well defined direction
  // just look at an endpoint
  if (ma < eps && mb < eps) {
    return Segment3D(sa.get_point(0), sb.get_point(0));
  } else if (ma < eps) {
    return get_reversed(get_shortest_segment(sb, sa.get_point(0)));
  } else if (mb < eps) {
    return get_shortest_segment(sa, sb.get_point(0));
  }

  algebra::Vector3D vfirst = sa.get_point(0)- sb.get_point(0);

  IMP_LOG_VERBOSE( vfirst << " | " << va << " | " << vb << std::endl);

  double dab = vb*va;

  double denom = ma * mb - dab * dab;
  // they are parallel or anti-parallel
  if (std::abs(denom) < eps) {
    return get_shortest_segment_parallel(sa, sb);
  }
  double dfb = vfirst*vb;
  double dfa = vfirst*va;
  double numer = dfb * dab - dfa * mb;

  double fa = numer / denom;
  double fb = (dfb + dab * fa) / mb;

  /*
    denom = d2121 * d4343 - d4321 * d4321;
    numer = d1343 * d4321 - d1321 * d4343;

    *mua = numer / denom;
    *mub = (d1343 + d4321 * (*mua)) / d4343;

    pa->x = p1.x + *mua * p21.x;
    pa->y = p1.y + *mua * p21.y;
    pa->z = p1.z + *mua * p21.z;
    pb->x = p3.x + *mub * p43.x;
    pb->y = p3.y + *mub * p43.y;
    pb->z = p3.z + *mub * p43.z;
  */

  algebra::Vector3D ra=get_clipped_point(sa, fa);
  algebra::Vector3D rb=get_clipped_point(sb, fb);

  IMP_LOG_VERBOSE( fa << " " << fb << std::endl);

  return Segment3D(ra, rb);
}
Esempio n. 2
0
/*
  get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
 */
float SRV_Channel::get_output_norm(void)
{
    uint16_t mid = (servo_max + servo_min) / 2;
    float ret;
    if (mid <= servo_min) {
        return 0;
    }
    if (output_pwm < mid) {
        ret = (float)(output_pwm - mid) / (float)(mid - servo_min);
    } else if (output_pwm > mid) {
        ret = (float)(output_pwm - mid) / (float)(servo_max  - mid);
    } else {
        ret = 0;
    }
    if (get_reversed()) {
           ret = -ret;
    }
    return ret;
}