//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Line3& rkLine, const Box3& rkBox)
{
    Real fAWdU[3], fAWxDdU[3], fRhs;

    Vector3 kDiff = rkLine.Origin() - rkBox.Center();
    Vector3 kWxD = rkLine.Direction().Cross(kDiff);

    fAWdU[1] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(1)));
    fAWdU[2] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(2)));
    fAWxDdU[0] = Math::FAbs(kWxD.Dot(rkBox.Axis(0)));
    fRhs = rkBox.Extent(1)*fAWdU[2] + rkBox.Extent(2)*fAWdU[1];
    if ( fAWxDdU[0] > fRhs )
        return false;

    fAWdU[0] = Math::FAbs(rkLine.Direction().Dot(rkBox.Axis(0)));
    fAWxDdU[1] = Math::FAbs(kWxD.Dot(rkBox.Axis(1)));
    fRhs = rkBox.Extent(0)*fAWdU[2] + rkBox.Extent(2)*fAWdU[0];
    if ( fAWxDdU[1] > fRhs )
        return false;

    fAWxDdU[2] = Math::FAbs(kWxD.Dot(rkBox.Axis(2)));
    fRhs = rkBox.Extent(0)*fAWdU[1] + rkBox.Extent(1)*fAWdU[0];
    if ( fAWxDdU[2] > fRhs )
        return false;

    return true;
}
Example #2
0
bool Plane::intersectLine(const Line3& line, Vector3* result) const
{
    const Vector3 direction = line.delta();

    float denominator = normal().dot( direction );

    if ( denominator == 0 ) {
        // line is coplanar, return origin
        if ( distanceToPoint( line.start() ) == 0 ) {
            if (result) {
                result->copy( line.start() );
            }
            return true;
        }

        // Unsure if this is the correct method to handle this case.
        return false;
    }

    float t = - ( line.start().dot( normal() ) + constant() ) / denominator;

    if( (t < 0 || t > 1) ) {
        return false;
    }

    if (result)
        result->copy( direction ).multiplyScalar( t ).add( line.start() );

    return true;
}
Example #3
0
Real Mgc::SqrDistance (const Vector3& rkPoint, const Line3& rkLine,

    Real* pfParam)

{

    Vector3 kDiff = rkPoint - rkLine.Origin();

    Real fSqrLen = rkLine.Direction().SquaredLength();

    Real fT = kDiff.Dot(rkLine.Direction())/fSqrLen;

    kDiff -= fT*rkLine.Direction();



    if ( pfParam )

        *pfParam = fT;



    return kDiff.SquaredLength();

}
Example #4
0
Real
closestPtLineTriangle(Line3 const & line, Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02, Real & s,
                      Vector3 & c1, Vector3 & c2)
{
  if (lineIntersectsTriangle(line, v0, edge01, edge02, &s))
  {
    c1 = c2 = line.getPoint() + s * line.getDirection();
    return 0;
  }

  // Either (1) the line is not parallel to the triangle and the point of
  // intersection of the line and the plane of the triangle is outside the
  // triangle or (2) the line and triangle are parallel.  Regardless, the
  // closest point on the triangle is on an edge of the triangle.  Compare
  // the line to all three edges of the triangle.
  Real min_sqdist = -1;
  Vector3 v[3] = { v0, v0 + edge01, v0 + edge02 };
  Vector3 tmp_c1, tmp_c2;
  float tmp_s, t;
  for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
  {
    Real sqdist = closestPtSegmentLine(v[i0], v[i1], line.getPoint(), line.getPoint() + line.getDirection(),
                                       t, tmp_s, tmp_c2, tmp_c1);
    if (min_sqdist < 0 || sqdist < min_sqdist)
    {
      min_sqdist = sqdist;
      s = tmp_s;
      c1 = tmp_c1;
      c2 = tmp_c2;
    }
  }

  return min_sqdist;
}
Example #5
0
bool Plane::isIntersectionLine( const Line3& line ) const
{
    // Note: this tests if a line intersects the plane, not whether it (or its end-points) are coplanar with it.

    float startSign = distanceToPoint( line.start() );
    float endSign = distanceToPoint( line.end() );

    return ( startSign < 0 && endSign > 0 ) || ( endSign < 0 && startSign > 0 );
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Segment3& rkSeg, const Box3& rkBox,
    Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
#ifdef _DEBUG
    // The four parameters pointers are either all non-null or all null.
    if ( pfLParam )
    {
        assert( pfBParam0 && pfBParam1 && pfBParam2 );
    }
    else
    {
        assert( !pfBParam0 && !pfBParam1 && !pfBParam2 );
    }
#endif

    Line3 kLine;
    kLine.Origin() = rkSeg.Origin();
    kLine.Direction() = rkSeg.Direction();

    Real fLP, fBP0, fBP1, fBP2;
    Real fSqrDistance = SqrDistance(kLine,rkBox,&fLP,&fBP0,&fBP1,&fBP2);
    if ( fLP >= 0.0f )
    {
        if ( fLP <= 1.0f )
        {
            if ( pfLParam )
            {
                *pfLParam = fLP;
                *pfBParam0 = fBP0;
                *pfBParam1 = fBP1;
                *pfBParam2 = fBP2;
            }

            return fSqrDistance;
        }
        else
        {
            fSqrDistance = SqrDistance(rkSeg.Origin()+rkSeg.Direction(),
                rkBox,pfBParam0,pfBParam1,pfBParam2);

            if ( pfLParam )
                *pfLParam = 1.0f;

            return fSqrDistance;
        }
    }
    else
    {
        fSqrDistance = SqrDistance(rkSeg.Origin(),rkBox,pfBParam0,
            pfBParam1,pfBParam2);

        if ( pfLParam )
            *pfLParam = 0.0f;

        return fSqrDistance;
    }
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Line3& rkLine, const Triangle3& rkTriangle,
    Vector3& rkPoint)
{
    Real fLinP;
    if ( SqrDistance(rkLine,rkTriangle,&fLinP) <= gs_fEpsilon )
    {
        rkPoint = rkLine.Origin() + fLinP*rkLine.Direction();
        return true;
    }
    return false;
}
Example #8
0
bool
lineIntersectsTriangle(Line3 const & line, Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02, Real * time)
{
  // The code is taken from the ray-triangle intersection test in Dave Eberly's Wild Magic library, v5.3, released under the
  // Boost license: http://www.boost.org/LICENSE_1_0.txt .

  static Real const EPS = 1e-30f;

  Vector3 diff = line.getPoint() - v0;
  Vector3 normal = edge01.cross(edge02);

  // Solve Q + t*D = b1*E1 + b2*E2 (Q = diff, D = line direction, E1 = edge01, E2 = edge02, N = Cross(E1,E2)) by
  //   |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
  //   |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
  //   |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)

  Real DdN = line.getDirection().dot(normal);
  int sign;
  if (DdN > EPS)
    sign = 1;
  else if (DdN < -EPS)
  {
    sign = -1;
    DdN = -DdN;
  }
  else
  {
    // Line and triangle are parallel, call it a "no intersection" even if the line does intersect
    return false;
  }

  Real DdQxE2 = sign * line.getDirection().dot(diff.cross(edge02));
  if (DdQxE2 >= 0)
  {
    Real DdE1xQ = sign * line.getDirection().dot(edge01.cross(diff));
    if (DdE1xQ >= 0)
    {
      if (DdQxE2 + DdE1xQ <= DdN)
      {
        // Line intersects triangle
        Real QdN = -sign * diff.dot(normal);
        if (time) *time = QdN / DdN;
        return true;
      }
      // else: b1 + b2 > 1, no intersection
    }
    // else: b2 < 0, no intersection
  }
  // else: b1 < 0, no intersection

  return false;
}
Example #9
0
//----------------------------------------------------------------------------
// @ BoundingSphere::Intersect()
// ---------------------------------------------------------------------------
// Determine intersection between sphere and line
//-----------------------------------------------------------------------------
bool
BoundingSphere::Intersect( const Line3& line ) const
{
    // compute intermediate values
    Vector3 w = mCenter - line.GetOrigin();
    float wsq = w.Dot(w);
    float proj = w.Dot(line.GetDirection());
    float rsq = mRadius*mRadius;
    float vsq = line.GetDirection().Dot(line.GetDirection());

    // test length of difference vs. radius
    return ( vsq*wsq - proj*proj <= vsq*rsq );
}
Example #10
0
optional<Vector3> Plane::intersectLine( const Line3& line, Vector3& target ) {

  auto direction = line.delta();
  auto denominator = normal.dot( direction );

  if ( denominator == 0 ) {
    // line is coplanar, return origin
    if( distanceToPoint( line.start ) == 0 ) {

      target.copy( line.start );
      return target;

    }

    // Unsure if this is the correct method to handle this case.
    return nullptr;

  }

  auto t = - ( line.start.dot( normal ) + constant ) / denominator;

  if( t < 0 || t > 1 ) {

    return nullptr;

  }

  return target.copy( direction ).multiplyScalar( t ).add( line.start );
}
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Line3& rkLine, const Circle3& rkCircle,
                       Vector3* pkLineClosest, Vector3* pkCircleClosest)
{
    Vector3 kDiff = rkLine.Origin() - rkCircle.Center();
    Real fDSqrLen = kDiff.SquaredLength();
    Real fMdM = rkLine.Direction().SquaredLength();
    Real fDdM = kDiff.Dot(rkLine.Direction());
    Real fNdM = rkCircle.N().Dot(rkLine.Direction());
    Real fDdN = kDiff.Dot(rkCircle.N());

    Real fA0 = fDdM;
    Real fA1 = fMdM;
    Real fB0 = fDdM - fNdM*fDdN;
    Real fB1 = fMdM - fNdM*fNdM;
    Real fC0 = fDSqrLen - fDdN*fDdN;
    Real fC1 = fB0;
    Real fC2 = fB1;
    Real fRSqr = rkCircle.Radius()*rkCircle.Radius();

    Real fA0Sqr = fA0*fA0;
    Real fA1Sqr = fA1*fA1;
    Real fTwoA0A1 = 2.0f*fA0*fA1;
    Real fB0Sqr = fB0*fB0;
    Real fB1Sqr = fB1*fB1;
    Real fTwoB0B1 = 2.0f*fB0*fB1;
    Real fTwoC1 = 2.0f*fC1;

    // The minimum point B+t*M occurs when t is a root of the quartic
    // equation whose coefficients are defined below.
    Polynomial kPoly(5);
    kPoly[0] = fA0Sqr*fC0 - fB0Sqr*fRSqr;
    kPoly[1] = fTwoA0A1*fC0 + fA0Sqr*fTwoC1 - fTwoB0B1*fRSqr;
    kPoly[2] = fA1Sqr*fC0 + fTwoA0A1*fTwoC1 + fA0Sqr*fC2 - fB1Sqr*fRSqr;
    kPoly[3] = fA1Sqr*fTwoC1 + fTwoA0A1*fC2;
    kPoly[4] = fA1Sqr*fC2;

    int iNumRoots;
    Real afRoot[4];
    kPoly.GetAllRoots(iNumRoots,afRoot);

    Real fMinSqrDist = Math::MAX_REAL;
    for (int i = 0; i < iNumRoots; i++)
    {
        // compute distance from P(t) to circle
        Vector3 kP = rkLine.Origin() + afRoot[i]*rkLine.Direction();
        Vector3 kCircleClosest;
        Real fSqrDist = SqrDistance(kP,rkCircle,&kCircleClosest);
        if ( fSqrDist < fMinSqrDist )
        {
            fMinSqrDist = fSqrDist;
            if ( pkLineClosest )
                *pkLineClosest = kP;
            if ( pkCircleClosest )
                *pkCircleClosest = kCircleClosest;
        }
    }

    return fMinSqrDist;
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Plane& rkPlane0, const Plane& rkPlane1,
    Line3& rkLine)
{
    // If Cross(N0,N1) is zero, then either planes are parallel and separated
    // or the same plane.  In both cases, 'false' is returned.  Otherwise,
    // the intersection line is
    //
    //   L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
    //
    // for some coefficients c0 and c1 and for t any real number (the line
    // parameter).  Taking dot products with the normals,
    //
    //   d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
    //   d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
    //
    // which are two equations in two unknowns.  The solution is
    //
    //   c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
    //   c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
    //
    // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.

    Real fN00 = rkPlane0.Normal().SquaredLength();
    Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
    Real fN11 = rkPlane1.Normal().SquaredLength();
    Real fDet = fN00*fN11 - fN01*fN01;

    if ( Math::FAbs(fDet) < gs_fEpsilon )
        return false;

    Real fInvDet = 1.0f/fDet;
    Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
    Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;

    rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
    rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
    return true;
}
Example #13
0
static T
distanceToTuple(Line3<T> line, const tuple &t)
{
    Vec3<T> v;
    if(t.attr("__len__")() == 3)
    {
        v.x = extract<T>(t[0]);
        v.y = extract<T>(t[1]);
        v.z = extract<T>(t[2]);
        
        return line.distanceTo(v);
    }
    else
        THROW(Iex::LogicExc, "Line3 expects tuple of length 3");      
}
Example #14
0
static Vec3<T>
closestPointToTuple(Line3<T> line, const tuple &t)
{
    MATH_EXC_ON;
    Vec3<T> v;
    if(t.attr("__len__")() == 3)
    {
        v.x = extract<T>(t[0]);
        v.y = extract<T>(t[1]);
        v.z = extract<T>(t[2]);
        
        return line.closestPointTo(v);
    }
    else
        THROW(Iex::LogicExc, "Line3 expects tuple of length 3");      
}
Example #15
0
static void
setTuple(Line3<T> &line, const tuple &t0, const tuple &t1)
{
    MATH_EXC_ON;
    Vec3<T> v0, v1;
    if(t0.attr("__len__")() == 3 && t1.attr("__len__")() == 3)
    {
        v0.x = extract<T>(t0[0]);
        v0.y = extract<T>(t0[1]);
        v0.z = extract<T>(t0[2]);

        v1.x = extract<T>(t1[0]);
        v1.y = extract<T>(t1[1]);
        v1.z = extract<T>(t1[2]);
        
        line.set(v0, v1);
    }
    else
        THROW(Iex::LogicExc, "Line3 expects tuple of length 3");    
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Line3& rkLine, const Box3& rkBox,
    int& riQuantity, Vector3 akPoint[2])
{
    // convert line to box coordinates
    Vector3 kDiff = rkLine.Origin() - rkBox.Center();
    Vector3 kOrigin(
        kDiff.Dot(rkBox.Axis(0)),
        kDiff.Dot(rkBox.Axis(1)),
        kDiff.Dot(rkBox.Axis(2))
    );
    Vector3 kDirection(
        rkLine.Direction().Dot(rkBox.Axis(0)),
        rkLine.Direction().Dot(rkBox.Axis(1)),
        rkLine.Direction().Dot(rkBox.Axis(2))
    );

    Real fT0 = -Math::MAX_REAL, fT1 = Math::MAX_REAL;
    bool bIntersects = FindIntersection(kOrigin,kDirection,rkBox.Extents(),
        fT0,fT1);

    if ( bIntersects )
    {
        if ( fT0 != fT1 )
        {
            riQuantity = 2;
            akPoint[0] = rkLine.Origin() + fT0*rkLine.Direction();
            akPoint[1] = rkLine.Origin() + fT1*rkLine.Direction();
        }
        else
        {
            riQuantity = 1;
            akPoint[0] = rkLine.Origin() + fT0*rkLine.Direction();
        }
    }
    else
    {
        riQuantity = 0;
    }

    return bIntersects;
}
Example #17
0
bool Wml::FindIntersection (const Line3<Real>& rkLine,
    const Cylinder3<Real>& rkCylinder, int& riQuantity,
    Vector3<Real> akPoint[2])
{
    Real afT[2];

    if ( rkCylinder.Capped() )
    {
        riQuantity = Find(rkLine.Origin(),rkLine.Direction(),rkCylinder,afT);
    }
    else
    {
        riQuantity = FindHollow(rkLine.Origin(),rkLine.Direction(),
            rkCylinder,afT);
    }

    for (int i = 0; i < riQuantity; i++)
        akPoint[i] = rkLine.Origin() + afT[i]*rkLine.Direction();

    return riQuantity > 0;
}
Example #18
0
static Vec3<T>
pointAt(Line3<T> &line, T t)
{
    MATH_EXC_ON;
    return line.operator()(t);
}
Example #19
0
static T
distanceTo1(Line3<T> &line, Vec3<T> &p)
{
    MATH_EXC_ON;
    return line.distanceTo(p);
}
Example #20
0
static Vec3<T>
closestPointTo1(Line3<T> line, const Vec3<T> &p)
{
    MATH_EXC_ON;
    return line.closestPointTo(p);
}
Example #21
0
bool Mgc::FindIntersection (const Line3& rkLine, const Sphere& rkSphere,

    int& riQuantity, Vector3 akPoint[2])

{

    // set up quadratic Q(t) = a*t^2 + 2*b*t + c

    Vector3 kDiff = rkLine.Origin() - rkSphere.Center();

    Real fA = rkLine.Direction().SquaredLength();

    Real fB = kDiff.Dot(rkLine.Direction());

    Real fC = kDiff.SquaredLength() -

        rkSphere.Radius()*rkSphere.Radius();



    Real afT[2];

    Real fDiscr = fB*fB - fA*fC;

    if ( fDiscr < 0.0f )

    {

        riQuantity = 0;

        return false;

    }

    else if ( fDiscr > 0.0f )

    {

        Real fRoot = Math::Sqrt(fDiscr);

        Real fInvA = 1.0f/fA;

        riQuantity = 2;

        afT[0] = (-fB - fRoot)*fInvA;

        afT[1] = (-fB + fRoot)*fInvA;

        akPoint[0] = rkLine.Origin() + afT[0]*rkLine.Direction();

        akPoint[1] = rkLine.Origin() + afT[1]*rkLine.Direction();

        return true;

    }

    else

    {

        riQuantity = 1;

        afT[0] = -fB/fA;

        akPoint[0] = rkLine.Origin() + afT[0]*rkLine.Direction();

        return true;

    }

}
Example #22
0
 vec::Vector3 Normal( Line3 const& i_line, vec::Vector3 const& i_vector )
 {
     return i_line.Normal( i_vector );
 }
Example #23
0
 vec::Vector3 DistanceToPoint( Line3 const& i_line, vec::Vector3 const& i_point )
 {
     return i_line.DistanceToPoint( i_point );
 }
//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Line3& rkLine, const Box3& rkBox,
    Real* pfLParam, Real* pfBParam0, Real* pfBParam1, Real* pfBParam2)
{
#ifdef _DEBUG
    // The four parameters pointers are either all non-null or all null.
    if ( pfLParam )
    {
        assert( pfBParam0 && pfBParam1 && pfBParam2 );
    }
    else
    {
        assert( !pfBParam0 && !pfBParam1 && !pfBParam2 );
    }
#endif

    // compute coordinates of line in box coordinate system
    Vector3 kDiff = rkLine.Origin() - rkBox.Center();
    Vector3 kPnt(kDiff.Dot(rkBox.Axis(0)),kDiff.Dot(rkBox.Axis(1)),
        kDiff.Dot(rkBox.Axis(2)));
    Vector3 kDir(rkLine.Direction().Dot(rkBox.Axis(0)),
        rkLine.Direction().Dot(rkBox.Axis(1)),
        rkLine.Direction().Dot(rkBox.Axis(2)));

    // Apply reflections so that direction vector has nonnegative components.
    bool bReflect[3];
    int i;
    for (i = 0; i < 3; i++)
    {
        if ( kDir[i] < 0.0f )
        {
            kPnt[i] = -kPnt[i];
            kDir[i] = -kDir[i];
            bReflect[i] = true;
        }
        else
        {
            bReflect[i] = false;
        }
    }

    Real fSqrDistance = 0.0f;

    if ( kDir.x > 0.0f )
    {
        if ( kDir.y > 0.0f )
        {
            if ( kDir.z > 0.0f )
            {
                // (+,+,+)
                CaseNoZeros(kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (+,+,0)
                Case0(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
        else
        {
            if ( kDir.z > 0.0f )
            {
                // (+,0,+)
                Case0(0,2,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (+,0,0)
                Case00(0,1,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
    }
    else
    {
        if ( kDir.y > 0.0f )
        {
            if ( kDir.z > 0.0f )
            {
                // (0,+,+)
                Case0(1,2,0,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (0,+,0)
                Case00(1,0,2,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
        }
        else
        {
            if ( kDir.z > 0.0f )
            {
                // (0,0,+)
                Case00(2,0,1,kPnt,kDir,rkBox,pfLParam,fSqrDistance);
            }
            else
            {
                // (0,0,0)
                Case000(kPnt,rkBox,fSqrDistance);
                if ( pfLParam )
                    *pfLParam = 0.0f;
            }
        }
    }

    if ( pfLParam )
    {
        // undo reflections
        for (i = 0; i < 3; i++)
        {
            if ( bReflect[i] )
                kPnt[i] = -kPnt[i];
        }

        *pfBParam0 = kPnt.x;
        *pfBParam1 = kPnt.y;
        *pfBParam2 = kPnt.z;
    }

    return fSqrDistance;
}
Example #25
0
static void
set1(Line3<T> &line, const Vec3<T> &p0, const Vec3<T> &p1)
{
    MATH_EXC_ON;
    line.set (p0, p1);
}
Example #26
0
static T
distanceTo2(Line3<T> &line, Line3<T> &other)
{
    MATH_EXC_ON;
    return line.distanceTo(other);
}
Example #27
0
bool Wisp::isClicked(Line3 line)
{
	bool clicked = 1 + 1 > line.distanceTo(location + Vector3(0, 0, TERRAINMAP.getHeight(location) + 1 + 1));
	return clicked;
}
Example #28
0
static Vec3<T>
closestPointTo2(Line3<T> line, const Line3<T> &other)
{
    MATH_EXC_ON;
    return line.closestPointTo(other);
}