Bezier Bezier::join(const Bezier * other) const
{
Bezier bezier;
bool otherIsEmpty = (other == NULL || other->isEmpty());
if (isEmpty() && otherIsEmpty) {
return bezier;
}
else {
if (isEmpty()) {
bezier.set_cp0(m_endpoint0);
bezier.set_cp1(other->cp1());
}
else if (other->isEmpty()) {
bezier.set_cp1(other->m_endpoint1);
bezier.set_cp0(cp0());
}
else {
bezier.set_cp0(cp0());
bezier.set_cp1(other->cp1());
}
}
return bezier;
}
double SegSegDist(const hrp::Vector3& u0, const hrp::Vector3& u1,
const hrp::Vector3& v0, const hrp::Vector3& v1)
{
hrp::Vector3 u(u1 - u0);
hrp::Vector3 v(v1 - v0);
hrp::Vector3 w(u0 - v0);
double a = u.dot(u); // always >= 0
double b = u.dot(v);
double c = v.dot(v); // always >= 0
double d = u.dot(w);
double e = v.dot(w);
double D = a*c - b*b; // always >= 0
double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
#define EPS 1e-8
if (D < EPS) { // the lines are almost parallel
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else { // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0)
sN = 0.0;
else if (-d > a)
sN = sD;
else {
sN = -d;
sD = a;
}
}
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (fabsf(sN) < EPS ? 0.0f : sN / sD);
tc = (fabsf(tN) < EPS ? 0.0f : tN / tD);
hrp::Vector3 cp0(u0 + sc * u);
hrp::Vector3 cp1(v0 + tc * v);
// get the difference of the two closest points
hrp::Vector3 dP(cp0 - cp1);
return dP.norm(); // return the closest distance
}
