void
SbMatrix::multLineMatrix(const SbLine &src, SbLine &dst) const
{
SbVec3f pos, dir;
multVecMatrix(src.getPosition(), pos);
multDirMatrix(src.getDirection(), dir);
dst.setValue(pos, pos+dir);
}
bool
XipGeomUtils::intersect(const SbLine &line, const SbPlane &plane, SbVec3f &pt)
{
float t, denom;
// solve for t:
// n . (l.p + t * l.d) - d == 0
denom = plane.getNormal().dot(line.getDirection());
if ( fabs(denom) < XIP_EPSILON )
return FALSE;
// t = - (n . l.p - d) / (n . l.d)
t = - (plane.getNormal().dot(line.getPosition()) - plane.getDistanceFromOrigin()) / denom;
pt = line.getPosition() + t * line.getDirection();
return TRUE;
}
//////////////////////////////////////////////////////////////////////////////
//
// Sphere line intersection - this sets the parameter intersection,
// and returns TRUE if the line and sphere really do intersect.
//
// line-sphere intersection algorithm lifted from Eric Haines chapter in
// Glassner's "Introduction to Ray Tracing", pp. 35-7
//
SbBool
SbSphere::intersect(const SbLine &l, SbVec3f &intersection) const
//
//////////////////////////////////////////////////////////////////////////////
{
float B,C; // At^2 + Bt + C = 0, but A is 1 since we normalize Rd
float discr; // discriminant (B^2 - 4AC)
SbVec3f v;
float t,sqroot;
SbBool doesIntersect = TRUE;
// setup B,C
v = l.getPosition() - center;
B = 2.0 * (l.getDirection().dot(v));
C = v.dot(v) - (radius * radius);
// compute discriminant
// if negative, there is no intersection
discr = B*B - 4.0*C;
if (discr < 0.0) {
// line and sphere do not intersect
doesIntersect = FALSE;
}
else {
// compute t0: (-B - sqrt(B^2 - 4AC)) / 2A (A = 1)
sqroot = sqrtf(discr);
t = (-B - sqroot) * 0.5;
if (t < 0.0) {
// no intersection, try t1: (-B + sqrt(B^2 - 4AC)) / 2A (A = 1)
t = (-B + sqroot) * 0.5;
}
if (t < 0.0) {
// line and sphere do not intersect
doesIntersect = FALSE;
}
else {
// intersection! point is (point + (dir * t))
intersection = l.getPosition() + (l.getDirection() * t);
}
}
return doesIntersect;
}
//////////////////////////////////////////////////////////////////////////////
//
// Sphere line intersection - this sets the parameter intersection,
// and returns TRUE if the line and sphere really do intersect.
//
// line-sphere intersection algorithm lifted from Eric Haines chapter in
// Glassner's "Introduction to Ray Tracing", pp. 35-7
//
SbBool
SbSphere::intersect(const SbLine &l, SbVec3f &enter, SbVec3f &exit) const
//
//////////////////////////////////////////////////////////////////////////////
{
float B,C; // At^2 + Bt + C = 0, but A is 1 since we normalize Rd
float discr; // discriminant (B^2 - 4AC)
SbVec3f v;
float sqroot;
SbBool doesIntersect = TRUE;
// setup B,C
v = l.getPosition() - center;
B = 2.0 * (l.getDirection().dot(v));
C = v.dot(v) - (radius * radius);
// compute discriminant
// if negative, there is no intersection
discr = B*B - 4.0*C;
if (discr < 0.0) {
// line and sphere do not intersect
doesIntersect = FALSE;
}
else {
sqroot = sqrtf(discr);
float t0 = (-B - sqroot) * 0.5;
enter = l.getPosition() + (l.getDirection() * t0);
float t1 = (-B + sqroot) * 0.5;
exit = l.getPosition() + (l.getDirection() * t1);
}
return doesIntersect;
}
SbBool
SbCylinder::intersect(const SbLine &line, SbVec3f &enter, SbVec3f &exit) const
//
////////////////////////////////////////////////////////////////////////
{
// The intersection will actually be done on a radius 1 cylinder
// aligned with the y axis, so we transform the line into that
// space, then intersect, then transform the results back.
// rotation to y axis
SbRotation rotToYAxis(axis.getDirection(), SbVec3f(0,1,0));
SbMatrix mtxToYAxis;
mtxToYAxis.setRotate(rotToYAxis);
// scale to unit space
float scaleFactor = 1.0f/radius;
SbMatrix toUnitCylSpace;
toUnitCylSpace.setScale(SbVec3f(scaleFactor, scaleFactor, scaleFactor));
toUnitCylSpace.multLeft(mtxToYAxis);
// find the given line un-translated
SbVec3f origin = line.getPosition();
origin -= axis.getPosition();
SbLine noTranslationLine(origin, origin + line.getDirection());
// find the un-translated line in unit cylinder's space
SbLine cylLine;
toUnitCylSpace.multLineMatrix(noTranslationLine, cylLine);
// find the intersection on the unit cylinder
SbVec3f cylEnter, cylExit;
SbBool intersected = unitCylinderIntersect(cylLine, cylEnter, cylExit);
if (intersected) {
// transform back to original space
SbMatrix fromUnitCylSpace = toUnitCylSpace.inverse();
fromUnitCylSpace.multVecMatrix(cylEnter, enter);
enter += axis.getPosition();
fromUnitCylSpace.multVecMatrix(cylExit, exit);
exit += axis.getPosition();
}
return intersected;
}
SbBool
SbCylinder::unitCylinderIntersect(const SbLine &l,
SbVec3f &enter, SbVec3f &exit)
//
////////////////////////////////////////////////////////////////////////
{
float A, B, C, discr, sqroot, t0, t1;
const SbVec3f &pos = l.getPosition(), &dir = l.getDirection();
SbBool doesIntersect = TRUE;
A = dir[0] * dir[0] + dir[2] * dir[2];
B = 2.0f * (pos[0] * dir[0] + pos[2] * dir[2]);
C = pos[0] * pos[0] + pos[2] * pos[2] - 1;
// discriminant = B^2 - 4AC
discr = B*B - 4.0f*A*C;
// if discriminant is negative, no intersection
if (discr < 0.0) {
doesIntersect = FALSE;
}
else {
sqroot = float(sqrtf(discr));
// magic to stabilize the answer
if (B > 0.0) {
t0 = -(2.0f * C) / (sqroot + B);
t1 = -(sqroot + B) / (2.0f * A);
}
else {
t0 = (2.0f * C) / (sqroot - B);
t1 = (sqroot - B) / (2.0f * A);
}
enter = pos + (dir * t0);
exit = pos + (dir * t1);
}
return doesIntersect;
}
SbVec3f
SbLineProjector::project(const SbVec2f &point)
//
////////////////////////////////////////////////////////////////////////
{
// Convert two line points to world space
SbLine worldLine;
workingToWorld.multLineMatrix( line, worldLine );
SbVec3f wldPt1 = worldLine.getPosition();
SbVec3f wldDir = worldLine.getDirection();
SbVec3f wldPt2 = wldPt1 + wldDir;
// Convert two line points to normalized screen space.
SbVec3f nrmScnPt1, nrmScnPt2;
viewVol.projectToScreen( wldPt1, nrmScnPt1 );
viewVol.projectToScreen( wldPt2, nrmScnPt2 );
// Convert two line points and input point
// to viewPlane space, a screen space that's got view plane's aspect ratio:
float vvW = (viewVol.getWidth() == 0.0) ? 1 : viewVol.getWidth();
float vvH = (viewVol.getHeight() == 0.0) ? 1 : viewVol.getHeight();
SbVec3f vpPt1( nrmScnPt1[0] * vvW, nrmScnPt1[1] * vvH, 0);
SbVec3f vpPt2( nrmScnPt2[0] * vvW, nrmScnPt2[1] * vvH, 0);
SbVec3f vpInPoint( point[0] * vvW, point[1] * vvH, 0);
// Create the viewPlaneLine -- our line expressed in viewPlane space:
SbLine viewPlaneLine( vpPt1, vpPt2 );
// In viewplane space, find the closest point on our line to the cursor.
SbVec3f vpClosestPt = viewPlaneLine.getClosestPoint( vpInPoint );
vpClosestPt.setValue( vpClosestPt[0], vpClosestPt[1], 0 );
// If we've got a perspective view, we may need to clamp the point we
// choose so that it's not too close to the vanishing point.
// Otherwise we'll just use our vpClosestPt
SbVec3f vpClampedPt = vpClosestPt;
if ( viewVol.getProjectionType() == SbViewVolume::PERSPECTIVE ) {
// Find the vanishing point of our line in viewPlane space:
// Convert the direction of our line from world space into space
// after the affine matrix (i.e. just before the projection matrix)
SbMatrix vvAffine, vvProj;
viewVol.getMatrices( vvAffine, vvProj );
SbVec3f postAffineDir;
vvAffine.multDirMatrix( wldDir, postAffineDir );
// If the direction of the line is parallel to the view plane,
// then the z component of postAffineDir is 0.
// In this case, we will not need to clamp our point and moreover,
// if we try we'll wind up dividing by zero pretty soon.
if ( postAffineDir[2] != 0.0 ) {
// If we send a line out from (0,0,0) into the viewVolume towards
// postAffineDir, it will vanish at the same point as any other line
// parallel to this direction. Also, all points along this line
// will project to the same point on the near (or far) plane.
// So a line connecting (0,0,0) and the point at postAffineDir will
// intersect the near plane at the vanishing point. Transforming
// any point on this line by vvProj will yield the same x,y result
// and the z component will vary with depth.
// So multiply the postAffineDir as a vector through the projection
// matrix and use the x,y for the vanishing point.
SbVec3f projVanish;
vvProj.multVecMatrix( postAffineDir, projVanish );
// Convert from [-1,1] range to [0,1] range for normalized coords.
SbVec3f nrmScnVanish;
nrmScnVanish[0] = (1.0 + projVanish[0]) * 0.5;
nrmScnVanish[1] = (1.0 + projVanish[1]) * 0.5;
// Finally, get the vanishing point in viewPlane coords:
SbVec3f vpVanish( nrmScnVanish[0] * vvW, nrmScnVanish[1] * vvH, 0 );
#if 0
// Check that the vanishing point is correct:
// Project nrmScnVanish on the plane to see if it goes along wldDir:
SbVec2f nrmScnVanish2( nrmScnVanish[0], nrmScnVanish[1] );
SbLine vanishWorldLine;
viewVol.projectPointToLine( nrmScnVanish2, vanishWorldLine );
SbVec3f test = vanishWorldLine.getDirection();
fprintf(stderr,"wldDir = %f %f %f\n",wldDir[0],wldDir[1],wldDir[2]);
fprintf(stderr,"checkDir = %f %f %f\n", test[0], test[1],test[2]);
#endif
// The points vpPt1 and vpPt2 define the line in viewPlane space.
// We can't go on the other side of the vanishing point from these
// defining points in screen space or the point will be undefined when
// we cast it into world space.
// So clamp our selected point to lie on vpPt1's side of the vanishing
// point. Since points near the vanishing point will also be incredibly
// far away, introduce an (arbitrary) metric, VANISH_DELTA.
// Our selection must be more than VANISH_DELTA times the average of
// viewVolumeHeight and viewVolumeWidth from the vanishing point.
#define VANISH_DELTA .01
float vanishSafetyDist = VANISH_DELTA * .5 * (vvW + vvH);
#undef VANISH_DELTA
// Make pt0, the point from which we measure distances along vpLine.
// It will be one extra unit away from vpVanish than safetyDist
SbVec3f pt0 = viewPlaneLine.getPosition();
pt0.setValue( pt0[0], pt0[1], 0 );
SbVec3f pt0ToVanishDir = vpVanish - pt0;
pt0ToVanishDir.normalize();
float pt0ToVanishDist = vanishSafetyDist + 1.0;
pt0 = vpVanish - pt0ToVanishDist * pt0ToVanishDir;
// Get vector and dist from pt0 to vpClosestPt
SbVec3f pt0ToClosest = vpClosestPt - pt0;
float pt0ToClosestDist = pt0ToClosest.length();
// If vpClosestPt is too far from pt0, clamp it:
float clampDist = pt0ToVanishDist - vanishSafetyDist;
if ( (pt0ToClosestDist > clampDist)
&& (pt0ToClosest.dot(pt0ToVanishDir) > 0.0) ) {
vpClampedPt = pt0 + clampDist * pt0ToVanishDir;
}
}
}
// Convert result back into normalized screen space:
SbVec2f nrmScnClampedPt( vpClampedPt[0] / vvW, vpClampedPt[1] / vvH);
// Create a line in working space by projecting our point into the scene:
SbVec3f result, whoCares;
SbLine workingLine = getWorkingLine( nrmScnClampedPt );
// Find point on the projector line closest to workingLine
if (! line.getClosestPoints(workingLine, result, whoCares)) {
#ifdef DEBUG
SoDebugError::post("SbLineProjector::project",
"Couldn't get closest point");
#endif
}
return result;
}