示例#1
0
文件: ray.cpp 项目: JT-a/USD
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;
}
示例#2
0
文件: ray.cpp 项目: JT-a/USD
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;
}
示例#3
0
文件: ray.cpp 项目: JT-a/USD
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);
}
示例#4
0
文件: line2d.cpp 项目: JT-a/USD
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 );
}
示例#5
0
文件: plane.cpp 项目: 400dama/USD
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;
}
示例#6
0
文件: vec3f.cpp 项目: JT-a/USD
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);
}
示例#7
0
文件: vec3f.cpp 项目: JT-a/USD
/*
 * 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;
}
示例#8
0
文件: line2d.cpp 项目: JT-a/USD
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;
}
示例#9
0
文件: plane.cpp 项目: 400dama/USD
void
GfPlane::Set(const GfVec3d &p0, const GfVec3d &p1, const GfVec3d &p2)
{
    _normal = GfCross(p1 - p0, p2 - p0).GetNormalized();
    _distance = GfDot(_normal, p0);
}
示例#10
0
文件: plane.cpp 项目: 400dama/USD
void
GfPlane::Set(const GfVec3d &normal, const GfVec3d &point)
{
    _normal = normal.GetNormalized();
    _distance = GfDot(_normal, point);
}