/*
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);
}
/*
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;
}
