GfRay
GfFrustum::ComputeRay(const GfVec3d &worldSpacePos) const
{
GfVec3d camSpaceToPos = ComputeViewMatrix().Transform(worldSpacePos);
// Compute the camera-space starting point (the viewpoint) and
// direction (toward the point camSpaceToPos).
GfVec3d pos;
GfVec3d dir;
if (_projectionType == Perspective) {
pos = GfVec3d(0);
dir = camSpaceToPos.GetNormalized();
}
else {
pos.Set(camSpaceToPos[0], camSpaceToPos[1], 0.0);
dir = -GfVec3d::ZAxis();
}
// Transform these by the inverse of the view matrix.
const GfMatrix4d &viewInverse = ComputeViewInverse();
GfVec3d rayFrom = viewInverse.Transform(pos);
GfVec3d rayDir = viewInverse.TransformDir(dir);
// Build and return the ray
return GfRay(rayFrom, rayDir);
}
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;
}
GfRay
GfFrustum::ComputePickRay(const GfVec3d &worldSpacePos) const
{
GfVec3d camSpaceToPos = ComputeViewMatrix().Transform(worldSpacePos);
// Compute the camera-space starting point (the viewpoint) and
// direction (toward the point camSpaceToPos).
GfVec3d pos;
GfVec3d dir;
if (_projectionType == Perspective) {
pos = GfVec3d(0);
dir = camSpaceToPos.GetNormalized();
}
else {
pos.Set(camSpaceToPos[0], camSpaceToPos[1], 0.0);
dir = -GfVec3d::ZAxis();
}
return _ComputePickRayOffsetToNearPlane(pos, dir);
}
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);
}
void
GfPlane::Set(const GfVec3d &normal, const GfVec3d &point)
{
_normal = normal.GetNormalized();
_distance = GfDot(_normal, point);
}
