/*
* BuildOrthonormalFrame constructs two unit vectors *v1 and *v2,
* with *v1 and *v2 perpendicular to each other and (*this).
* We arbitrarily cross *this with the X axis to form *v1,
* and if the result is degenerate, we set *v1 = (Y axis) X *this.
* If L = length(*this) < eps, we shrink v1 and v2 to be of
* length L/eps.
*/
void
GfBuildOrthonormalFrame(GfVec3f const &v0,
GfVec3f* v1,
GfVec3f* v2, float eps)
{
float len = v0.GetLength();
if (len == 0.) {
*v1 = *v2 = GfVec3f(0);
}
else {
GfVec3f unitDir = v0 / len;
*v1 = GfVec3f::XAxis() ^ unitDir;
if (GfSqr(*v1) < GfSqr(1e-4))
*v1 = GfVec3f::YAxis() ^ unitDir;
GfNormalize(v1);
*v2 = unitDir ^ *v1; // this is of unit length
if (len < eps) {
double desiredLen = len / eps;
*v1 *= desiredLen;
*v2 *= desiredLen;
}
}
}
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;
}
double
GfRange3f::GetDistanceSquared(const GfVec3f &p) const
{
double dist = 0.0;
if (p[0] < _min[0]) {
// p is left of box
dist += GfSqr(_min[0] - p[0]);
}
else if (p[0] > _max[0]) {
// p is right of box
dist += GfSqr(p[0] - _max[0]);
}
if (p[1] < _min[1]) {
// p is front of box
dist += GfSqr(_min[1] - p[1]);
}
else if (p[1] > _max[1]) {
// p is back of box
dist += GfSqr(p[1] - _max[1]);
}
if (p[2] < _min[2]) {
// p is below of box
dist += GfSqr(_min[2] - p[2]);
}
else if (p[2] > _max[2]) {
// p is above of box
dist += GfSqr(p[2] - _max[2]);
}
return dist;
}
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);
}
/*
* 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
GfRay::_SolveQuadratic(const double a,
const double b,
const double c,
double *enterDistance,
double *exitDistance) const
{
if (GfIsClose(a, 0.0, tolerance)) {
if (GfIsClose(b, 0.0, tolerance)) {
// Degenerate solution
return false;
}
double t = -c / b;
if (t < 0.0) {
return false;
}
if (enterDistance) {
*enterDistance = t;
}
if (exitDistance) {
*exitDistance = t;
}
return true;
}
// Discriminant
double disc = GfSqr(b) - 4.0 * a * c;
if (GfIsClose(disc, 0.0, tolerance)) {
// Tangent
double t = -b / (2.0 * a);
if (t < 0.0) {
return false;
}
if (enterDistance) {
*enterDistance = t;
}
if (exitDistance) {
*exitDistance = t;
}
return true;
}
if (disc < 0.0) {
// No intersection
return false;
}
// Two intersection points
double q = -0.5 * (b + copysign(1.0, b) * GfSqrt(disc));
double t0 = q / a;
double t1 = c / q;
if (t0 > t1) {
std::swap(t0, t1);
}
if (t1 >= 0) {
if (enterDistance) {
*enterDistance = t0;
}
if (exitDistance) {
*exitDistance = t1;
}
return true;
}
return false;
}
