//---------------------------------------------------------------------------- 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); }