/** Returns f, the 'relative' projection of a point p onto the line
that contains s.
Formally, the projection of p onto the line through s is s[0]+f*(s[1]-s[0])
f is in the range [0..1] if the projection of p is inside s.
@param s segment in 3D
@param p point in 3D
@return the 'relative' project of p onto the line containing s
*/
double get_relative_projection_on_segment(const Segment3D &s,
const algebra::Vector3D &p) {
algebra::Vector3D vs = s.get_point(1) - s.get_point(0);
algebra::Vector3D vps = p - s.get_point(0);
double f = vps * vs / (vs * vs);
return f;
}
bool Box3D::intersects(const Segment3D& s) const
{
Segment3D sloc;
toLocal(s,sloc);
AABB3D bbloc;
bbloc.bmin.setZero();
bbloc.bmax=dims;
return sloc.intersects(bbloc);
}
/*
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);
}
void Segment3D::closestPoint(const Segment3D& s, Real& t, Real& u) const
{
Line3D l1,l2;
getLine(l1);
s.getLine(l2);
l1.closestPoint(l2,t,u);
t=Clamp(t,Zero,One);
u=Clamp(u,Zero,One);
}
Real Segment3D::distance(const Segment3D& s) const
{
Real t,u;
closestPoint(s,t,u);
Vector3 p1,p2;
eval(t,p1);
s.eval(u,p2);
return p1.distance(p2);
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Creates a bounding sphere.
///
/// \param[in] segment Segment to create bounding sphere from.
///
////////////////////////////////////////////////////////////////////////////////////
BoundingSphere BoundingSphere::CreateBoundingSphere(const Segment3D& segment)
{
return BoundingSphere(segment.GetMidpoint(), segment.GetMagnitude() + segment.mWidth/2.0);
}
double get_distance(const Segment3D &a, const Segment3D &b) {
Segment3D s = get_shortest_segment(a, b);
return (s.get_point(0) - s.get_point(1)).get_magnitude();
}
double get_distance(const Segment3D &s, const Vector3D &p) {
Segment3D ss = get_shortest_segment(s, p);
return (ss.get_point(0) - ss.get_point(1)).get_magnitude();
}
//TODO have to clean up
void Draw2D::drawSegment(SDL_Surface *screen,Segment3D _segment, Uint8 r, Uint8 g, Uint8 b)
{
drawSegment(screen,(*_segment.getPointA()),(*_segment.getPointB()),r,g,b);
}