bool
GfRay::Intersect(const GfPlane &plane,
double *distance, bool *frontFacing) const
{
// The dot product of the ray direction and the plane normal
// indicates the angle between them. Reject glancing
// intersections. Note: this also rejects ill-formed planes with
// zero normals.
double d = GfDot(_direction, plane.GetNormal());
if (d < GF_MIN_VECTOR_LENGTH && d > -GF_MIN_VECTOR_LENGTH)
return false;
// Get a point on the plane.
GfVec3d planePoint = plane.GetDistanceFromOrigin() * plane.GetNormal();
// Compute the parametric distance t to the plane. Reject
// intersections outside the ray bounds.
double t = GfDot(planePoint - _startPoint, plane.GetNormal()) / d;
if (t < 0.0)
return false;
if (distance)
*distance = t;
if (frontFacing)
*frontFacing = (d < 0.0);
return true;
}
bool
GfRay::Intersect(const GfVec3d &origin,
const GfVec3d &axis,
const double radius,
const double height,
double *enterDistance,
double *exitDistance) const
{
GfVec3d unitAxis = axis.GetNormalized();
// Apex of cone
GfVec3d apex = origin + height * unitAxis;
GfVec3d delta = _startPoint - apex;
GfVec3d u =_direction - GfDot(_direction, unitAxis) * unitAxis;
GfVec3d v = delta - GfDot(delta, unitAxis) * unitAxis;
double p = GfDot(_direction, unitAxis);
double q = GfDot(delta, unitAxis);
double cos2 = GfSqr(height) / (GfSqr(height) + GfSqr(radius));
double sin2 = 1 - cos2;
double a = cos2 * GfDot(u, u) - sin2 * GfSqr(p);
double b = 2.0 * (cos2 * GfDot(u, v) - sin2 * p * q);
double c = cos2 * GfDot(v, v) - sin2 * GfSqr(q);
if (!_SolveQuadratic(a, b, c, enterDistance, exitDistance)) {
return false;
}
// Eliminate any solutions on the double cone
bool enterValid = GfDot(unitAxis, GetPoint(*enterDistance) - apex) <= 0.0;
bool exitValid = GfDot(unitAxis, GetPoint(*exitDistance) - apex) <= 0.0;
if ((!enterValid) && (!exitValid)) {
// Solutions lie only on double cone
return false;
}
if (!enterValid) {
*enterDistance = *exitDistance;
}
else if (!exitValid) {
*exitDistance = *enterDistance;
}
return true;
}
bool
GfRay::Intersect(const GfVec3d &origin,
const GfVec3d &axis,
const double radius,
double *enterDistance,
double *exitDistance) const
{
GfVec3d unitAxis = axis.GetNormalized();
GfVec3d delta = _startPoint - origin;
GfVec3d u = _direction - GfDot(_direction, unitAxis) * unitAxis;
GfVec3d v = delta - GfDot(delta, unitAxis) * unitAxis;
// Quadratic equation for implicit infinite cylinder
double a = GfDot(u, u);
double b = 2.0 * GfDot(u, v);
double c = GfDot(v, v) - GfSqr(radius);
return _SolveQuadratic(a, b, c, enterDistance, exitDistance);
}
GfVec2d
GfLine2d::FindClosestPoint(const GfVec2d &point, double *t) const
{
// Compute the vector from the start point to the given point.
GfVec2d v = point - _p0;
// Find the length of the projection of this vector onto the line.
double lt = GfDot(v, _dir);
if (t)
*t = lt;
return GetPoint( lt );
}
GfPlane &
GfPlane::Transform(const GfMatrix4d &matrix)
{
// Compute the point on the plane along the normal from the origin.
GfVec3d pointOnPlane = _distance * _normal;
// Transform the plane normal by the adjoint of the matrix to get
// the new normal. The adjoint (inverse transpose) is used to
// multiply normals so they are not scaled incorrectly.
GfMatrix4d adjoint = matrix.GetInverse().GetTranspose();
_normal = adjoint.TransformDir(_normal).GetNormalized();
// Transform the point on the plane by the matrix.
pointOnPlane = matrix.Transform(pointOnPlane);
// The new distance is the projected distance of the vector to the
// transformed point onto the (unit) transformed normal. This is
// just a dot product.
_distance = GfDot(pointOnPlane, _normal);
return *this;
}
GfVec3f
GfSlerp(double alpha, const GfVec3f &v0, const GfVec3f &v1)
{
// determine the angle between the two lines going from the center of
// the sphere to v0 and v1. the projection (dot prod) of one onto the
// other gives us the arc cosine of the angle between them.
double angle = acos(GfClamp((double)GfDot(v0, v1), -1.0, 1.0));
// Check for very small angle between the vectors, and if so, just lerp them.
// XXX: This value for epsilon is somewhat arbitrary, and if
// someone can derive a more meaningful value, that would be fine.
if ( fabs(angle) < 0.001 ) {
return GfLerp(alpha, v0, v1);
}
// compute the sin of the angle, we need it a couple of places
double sinAngle = sin(angle);
// Check if the vectors are nearly opposing, and if so,
// compute an arbitrary orthogonal vector to interpolate across.
// XXX: Another somewhat arbitrary test for epsilon, but trying to stay
// within reasonable float precision.
if ( fabs(sinAngle) < 0.00001 ) {
GfVec3f vX, vY;
v0.BuildOrthonormalFrame(&vX, &vY);
GfVec3f v = v0 * cos(alpha*M_PI) + vX * sin(alpha*M_PI);
return v;
}
// interpolate
double oneOverSinAngle = 1.0 / sinAngle;
return
v0 * (sin((1.0-alpha)*angle) * oneOverSinAngle) +
v1 * (sin( alpha *angle) * oneOverSinAngle);
}
/*
* Given 3 basis vectors *tx, *ty, *tz, orthogonalize and optionally normalize
* them.
*
* This uses an iterative method that is very stable even when the vectors
* are far from orthogonal (close to colinear). The number of iterations
* and thus the computation time does increase as the vectors become
* close to colinear, however.
*
* If the iteration fails to converge, returns false with vectors as close to
* orthogonal as possible.
*/
bool
GfOrthogonalizeBasis(GfVec3f *tx, GfVec3f *ty, GfVec3f *tz,
bool normalize, double eps)
{
GfVec3f ax,bx,cx,ay,by,cy,az,bz,cz;
if (normalize) {
GfNormalize(tx);
GfNormalize(ty);
GfNormalize(tz);
ax = *tx;
ay = *ty;
az = *tz;
} else {
ax = *tx;
ay = *ty;
az = *tz;
ax.Normalize();
ay.Normalize();
az.Normalize();
}
/* Check for colinear vectors. This is not only a quick-out: the
* error computation below will evaluate to zero if there's no change
* after an iteration, which can happen either because we have a good
* solution or because the vectors are colinear. So we have to check
* the colinear case beforehand, or we'll get fooled in the error
* computation.
*/
if (GfIsClose(ax,ay,eps) || GfIsClose(ax,az,eps) || GfIsClose(ay,az,eps)) {
return false;
}
const int MAX_ITERS = 20;
int iter;
for (iter = 0; iter < MAX_ITERS; ++iter) {
bx = *tx;
by = *ty;
bz = *tz;
bx -= GfDot(ay,bx) * ay;
bx -= GfDot(az,bx) * az;
by -= GfDot(ax,by) * ax;
by -= GfDot(az,by) * az;
bz -= GfDot(ax,bz) * ax;
bz -= GfDot(ay,bz) * ay;
cx = 0.5*(*tx + bx);
cy = 0.5*(*ty + by);
cz = 0.5*(*tz + bz);
if (normalize) {
cx.Normalize();
cy.Normalize();
cz.Normalize();
}
GfVec3f xDiff = *tx - cx;
GfVec3f yDiff = *ty - cy;
GfVec3f zDiff = *tz - cz;
double error =
GfDot(xDiff,xDiff) + GfDot(yDiff,yDiff) + GfDot(zDiff,zDiff);
// error is squared, so compare to squared tolerance
if (error < GfSqr(eps))
break;
*tx = cx;
*ty = cy;
*tz = cz;
ax = *tx;
ay = *ty;
az = *tz;
if (!normalize) {
ax.Normalize();
ay.Normalize();
az.Normalize();
}
}
return iter < MAX_ITERS;
}
bool
GfFindClosestPoints( const GfLine2d &l1, const GfLine2d &l2,
GfVec2d *closest1, GfVec2d *closest2,
double *t1, double *t2 )
{
// Define terms:
// p1 = line 1's position
// d1 = line 1's direction
// p2 = line 2's position
// d2 = line 2's direction
const GfVec2d &p1 = l1._p0;
const GfVec2d &d1 = l1._dir;
const GfVec2d &p2 = l2._p0;
const GfVec2d &d2 = l2._dir;
// We want to find points closest1 and closest2 on each line.
// Their parametric definitions are:
// closest1 = p1 + t1 * d1
// closest2 = p2 + t2 * d2
//
// We know that the line connecting closest1 and closest2 is
// perpendicular to both the ray and the line segment. So:
// d1 . (closest2 - closest1) = 0
// d2 . (closest2 - closest1) = 0
//
// Substituting gives us:
// d1 . [ (p2 + t2 * d2) - (p1 + t1 * d1) ] = 0
// d2 . [ (p2 + t2 * d2) - (p1 + t1 * d1) ] = 0
//
// Rearranging terms gives us:
// t2 * (d1.d2) - t1 * (d1.d1) = d1.p1 - d1.p2
// t2 * (d2.d2) - t1 * (d2.d1) = d2.p1 - d2.p2
//
// Substitute to simplify:
// a = d1.d2
// b = d1.d1
// c = d1.p1 - d1.p2
// d = d2.d2
// e = d2.d1 (== a, if you're paying attention)
// f = d2.p1 - d2.p2
double a = GfDot(d1, d2);
double b = GfDot(d1, d1);
double c = GfDot(d1, p1) - GfDot(d1, p2);
double d = GfDot(d2, d2);
double e = a;
double f = GfDot(d2, p1) - GfDot(d2, p2);
// And we end up with:
// t2 * a - t1 * b = c
// t2 * d - t1 * e = f
//
// Solve for t1 and t2:
// t1 = (c * d - a * f) / (a * e - b * d)
// t2 = (c * e - b * f) / (a * e - b * d)
//
// Note the identical denominators...
double denom = a * e - b * d;
// Denominator == 0 means the lines are parallel; no intersection.
if ( GfIsClose( denom, 0, 1e-6 ) )
return false;
double lt1 = (c * d - a * f) / denom;
double lt2 = (c * e - b * f) / denom;
if ( closest1 )
*closest1 = l1.GetPoint( lt1 );
if ( closest2 )
*closest2 = l2.GetPoint( lt2 );
if ( t1 )
*t1 = lt1;
if ( t2 )
*t2 = lt2;
return true;
}
void
GfPlane::Set(const GfVec3d &p0, const GfVec3d &p1, const GfVec3d &p2)
{
_normal = GfCross(p1 - p0, p2 - p0).GetNormalized();
_distance = GfDot(_normal, p0);
}
void
GfPlane::Set(const GfVec3d &normal, const GfVec3d &point)
{
_normal = normal.GetNormalized();
_distance = GfDot(_normal, point);
}
