double R3Distance(const R3Point& point, const R3Segment& segment)
{
// Check if start point is closest
R3Vector v1 = point - segment.Start();
double dir1 = v1.Dot(segment.Vector());
if (dir1 < 0) return v1.Length();
// Check if end point is closest
R3Vector v2 = point - segment.End();
double dir2 = v2.Dot(segment.Vector());
if (dir2 < 0) return v2.Length();
// Return distance from point to segment line
return R3Distance(point, segment.Line());
}
double R3Distance(const R3Ray& ray, const R3Segment& segment)
{
// There's got to be a better way ???
// Get vectors in more convenient form
const R3Vector v1 = ray.Vector();
const R3Vector v2 = segment.Vector();
// Compute useful intermediate values
const double v1v1 = 1.0; // v1.Dot(v1);
const double v2v2 = 1.0; // v2.Dot(v2);
double v1v2 = v1.Dot(v2);
double denom = v1v2*v1v2 - v1v1*v2v2;
// Check if ray and segment are parallel
if (denom == 0) {
// Not right ???
// Look at directions of vectors, then check relative starts and stops
return R3Distance(segment.Line(), ray.Line());
}
else {
// Find closest points
const R3Vector p1 = ray.Start().Vector();
const R3Vector p2 = segment.Start().Vector();
double p1v1 = v1.Dot(p1);
double p2v2 = v2.Dot(p2);
double p1v2 = v2.Dot(p1);
double p2v1 = v1.Dot(p2);
double ray_t = (v1v2*p2v2 + v2v2*p1v1 - v1v2*p1v2 - v2v2*p2v1) / denom;
double segment_t = (v1v2*p1v1 + v1v1*p2v2 - v1v2*p2v1 - v1v1*p1v2) / denom;
R3Point ray_point = (ray_t <= 0.0) ? ray.Start() : ray.Point(ray_t);
R3Point segment_point = (segment_t <= 0.0) ? segment.Start() :
(segment_t >= segment.Length()) ? segment.End() : segment.Ray().Point(segment_t);
double distance = R3Distance(ray_point, segment_point);
return distance;
}
}
