//----------------------------------------------------------------------------
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;
}
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;
}
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();
}
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;
}
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;
}
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;
}
//----------------------------------------------------------------------------
// @ 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 );
}
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;
}
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");
}
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");
}
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;
}
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;
}
static Vec3<T>
pointAt(Line3<T> &line, T t)
{
MATH_EXC_ON;
return line.operator()(t);
}
static T
distanceTo1(Line3<T> &line, Vec3<T> &p)
{
MATH_EXC_ON;
return line.distanceTo(p);
}
static Vec3<T>
closestPointTo1(Line3<T> line, const Vec3<T> &p)
{
MATH_EXC_ON;
return line.closestPointTo(p);
}
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;
}
}
vec::Vector3 Normal( Line3 const& i_line, vec::Vector3 const& i_vector )
{
return i_line.Normal( i_vector );
}
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;
}
static void
set1(Line3<T> &line, const Vec3<T> &p0, const Vec3<T> &p1)
{
MATH_EXC_ON;
line.set (p0, p1);
}
static T
distanceTo2(Line3<T> &line, Line3<T> &other)
{
MATH_EXC_ON;
return line.distanceTo(other);
}
bool Wisp::isClicked(Line3 line)
{
bool clicked = 1 + 1 > line.distanceTo(location + Vector3(0, 0, TERRAINMAP.getHeight(location) + 1 + 1));
return clicked;
}
static Vec3<T>
closestPointTo2(Line3<T> line, const Line3<T> &other)
{
MATH_EXC_ON;
return line.closestPointTo(other);
}