330 lines
8.3 KiB
C++
330 lines
8.3 KiB
C++
/****************************************************************************
|
||
Copyright (c) 2010,RadonLabs GmbH
|
||
Copyright (c) 2011-2013,WebJet Business Division,CYOU
|
||
|
||
http://www.genesis-3d.com.cn
|
||
|
||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
of this software and associated documentation files (the "Software"), to deal
|
||
in the Software without restriction, including without limitation the rights
|
||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
copies of the Software, and to permit persons to whom the Software is
|
||
furnished to do so, subject to the following conditions:
|
||
|
||
The above copyright notice and this permission notice shall be included in
|
||
all copies or substantial portions of the Software.
|
||
|
||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||
THE SOFTWARE.
|
||
****************************************************************************/
|
||
#include "stdneb.h"
|
||
#include "math/intersection.h"
|
||
#include <algorithm>
|
||
#include <limits>
|
||
|
||
|
||
namespace Math
|
||
{
|
||
|
||
//------------------------------------------------------------------------
|
||
// 该算法直接结果为直线是否与三角形相交，判断射线与三角形相交切记判断fOut大于0 2014-4-28
|
||
bool
|
||
Intersection::Intersect( const Ray& ray, const float3& v0, const float3& v1, const float3& v2, scalar& fOut, scalar fTolerance )
|
||
{
|
||
// Find vectors for two edges sharing vert0
|
||
float3 edge1 = v1 - v0;
|
||
float3 edge2 = v2 - v0;
|
||
|
||
// Begin calculating determinant - also used to calculate U parameter
|
||
float3 pvec = ray.Direction().crossProduct(edge2);
|
||
|
||
// If determinant is near zero, ray lies in plane of triangle
|
||
scalar det = edge1.dotProduct(pvec);
|
||
|
||
float3 tvec;
|
||
|
||
if( det > 0 )
|
||
{
|
||
tvec = ray.Start() - v0;
|
||
}
|
||
else
|
||
{
|
||
tvec = v0 - ray.Start();
|
||
det = -det;
|
||
}
|
||
|
||
if( det < fTolerance )
|
||
return false;
|
||
|
||
// Calculate U parameter and test bounds
|
||
scalar u = tvec.dotProduct(pvec);
|
||
if( u < 0.0f || u > det )
|
||
return false;
|
||
|
||
// Prepare to test V parameter
|
||
float3 qvec = tvec.crossProduct( edge1 );
|
||
|
||
// Calculate V parameter and test bounds
|
||
scalar v = ray.Direction().dotProduct( qvec);
|
||
if( v < 0.0f || u + v > det )
|
||
return false;
|
||
|
||
// Calculate t, scale parameters, ray intersects triangle
|
||
fOut =edge2.dotProduct( qvec );
|
||
scalar fInvDet = 1.0f / det;
|
||
fOut *= fInvDet;
|
||
|
||
return true;
|
||
|
||
}
|
||
//------------------------------------------------------------------------
|
||
|
||
bool
|
||
Intersection::Intersect( const Ray& ray1, const Ray& ray2, scalar& fOut1, scalar& fOut2, scalar fTolerance )
|
||
{
|
||
const float3& dir1 = ray1.Direction();
|
||
float3 c = dir1.crossProduct( ray2.Direction() );
|
||
|
||
scalar clq = c.lengthsq();
|
||
|
||
if ( n_fequal(clq, 0.0f, fTolerance ) )
|
||
{
|
||
// 平行或者重合
|
||
return false;
|
||
}
|
||
|
||
// 解方程组，得到两点射线上的最近点
|
||
float3 pDiff = ray2.Start() - ray1.Start();
|
||
|
||
float3 pDis_1 = pDiff.crossProduct( ray1.Direction() );
|
||
float3 pDis_2 = pDiff.crossProduct( ray2.Direction() );
|
||
|
||
scalar out1 = ( pDis_2.dotProduct( c ) ) / clq;
|
||
scalar Out2 = ( pDis_1.dotProduct( c ) ) / clq;
|
||
|
||
// 两个最近点的距离为0， 则相交
|
||
float3 p1 = ray1.PointAt( out1 );
|
||
float3 p2 = ray2.PointAt( Out2 );
|
||
float3 lineseg = p2 - p1;
|
||
scalar lineLength = lineseg.lengthsq();
|
||
|
||
if ( n_fequal( lineLength, 0.0f, fTolerance) )
|
||
{
|
||
// 是否在射线的反方向
|
||
if ( out1 < 0.0f || Out2 < 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
// intersect
|
||
fOut1 = out1;
|
||
fOut2 = Out2;
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
|
||
}
|
||
//------------------------------------------------------------------------
|
||
bool
|
||
Intersection::Intersect( const Ray& ray1, const float3& lineBegin, const float3& lineEnd, scalar& fOut, scalar fTolerance )
|
||
{
|
||
// 构建中一个射线
|
||
float3 lineseg = lineEnd - lineBegin;
|
||
|
||
if ( n_fequal(lineseg.lengthsq(), 0.0, fTolerance ) )
|
||
{
|
||
// 线退化为一个点。使用点的拾取
|
||
if( Intersect(ray1, lineBegin, fTolerance ) )
|
||
{
|
||
fOut = 0.0;
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
|
||
float3 normalDir = lineseg;
|
||
normalDir.normalise();
|
||
Ray ray2(lineBegin, normalDir );
|
||
|
||
scalar fOut1;
|
||
scalar fout2;
|
||
|
||
if( Intersect(ray1, ray2, fOut1, fout2, fTolerance) )
|
||
{
|
||
// 判断是在线段上还是在延长线上
|
||
scalar linelength = lineseg.length();
|
||
if( fout2 < (linelength + fTolerance) )
|
||
{
|
||
fOut = fOut1;
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
//------------------------------------------------------------------------
|
||
bool
|
||
Intersection::Intersect( const Ray& ray, const float3& p, scalar fTolerance )
|
||
{
|
||
float3 dir = p - ray.Start();
|
||
|
||
// 射线的反方向
|
||
if ( dir.dotProduct( ray.Direction() ) < 0.0f )
|
||
{
|
||
return false;
|
||
}
|
||
|
||
float3 c = dir.crossProduct( ray.Direction() );
|
||
scalar clq = c.lengthsq();
|
||
|
||
if ( n_fequal(clq, 0.0f, fTolerance ) )
|
||
{
|
||
// 平行或者重合
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
|
||
//------------------------------------------------------------------------
|
||
// Returns true if the ray stats inside the box or in front of the box
|
||
// if intersect, fOut1 is the first intersection;fOut2 is the second intersection
|
||
// from Ogre
|
||
bool
|
||
Intersection::Intersect(const Ray& ray, const bbox& box, scalar& fOut1, scalar& fOut2)
|
||
{
|
||
float3 bmin(box.pmin.x(), box.pmin.y(),box.pmin.z());
|
||
float3 bmax(box.pmax.x(), box.pmax.y(),box.pmax.z());
|
||
float3 rayorig = ray.Start();
|
||
float3 raydir = ray.Direction();
|
||
|
||
float3 absDir;
|
||
absDir[0] = n_abs(raydir[0]);
|
||
absDir[1] = n_abs(raydir[1]);
|
||
absDir[2] = n_abs(raydir[2]);
|
||
|
||
// Sort the axis, ensure check minimise floating error axis first
|
||
int imax = 0, imid = 1, imin = 2;
|
||
if (absDir[0] < absDir[2])
|
||
{
|
||
imax = 2;
|
||
imin = 0;
|
||
}
|
||
if (absDir[1] < absDir[imin])
|
||
{
|
||
imid = imin;
|
||
imin = 1;
|
||
}
|
||
else if (absDir[1] > absDir[imax])
|
||
{
|
||
imid = imax;
|
||
imax = 1;
|
||
}
|
||
|
||
scalar start = 0, end = N_INFINITY;
|
||
|
||
#define _CALC_AXIS(i) \
|
||
do { \
|
||
scalar denom = 1 / raydir[i]; \
|
||
scalar newstart = (bmin[i] - rayorig[i]) * denom; \
|
||
scalar newend = (bmax[i] - rayorig[i]) * denom; \
|
||
if (newstart > newend) std::swap(newstart, newend); \
|
||
if (newstart > end || newend < start) return false; \
|
||
if (newstart > start) start = newstart; \
|
||
if (newend < end) end = newend; \
|
||
} while(0)
|
||
|
||
// Check each axis in turn
|
||
|
||
_CALC_AXIS(imax);
|
||
|
||
if (absDir[imid] < std::numeric_limits<scalar>::epsilon())
|
||
{
|
||
// Parallel with middle and minimise axis, check bounds only
|
||
if (rayorig[imid] < bmin[imid] || rayorig[imid] > bmax[imid] ||
|
||
rayorig[imin] < bmin[imin] || rayorig[imin] > bmax[imin])
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
_CALC_AXIS(imid);
|
||
|
||
if (absDir[imin] < std::numeric_limits<scalar>::epsilon())
|
||
{
|
||
// Parallel with minimise axis, check bounds only
|
||
if (rayorig[imin] < bmin[imin] || rayorig[imin] > bmax[imin])
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
_CALC_AXIS(imin);
|
||
}
|
||
}
|
||
#undef _CALC_AXIS
|
||
|
||
fOut1 = start;
|
||
fOut2 = end;
|
||
|
||
return true;
|
||
}
|
||
//------------------------------------------------------------------------
|
||
// return true if the sphere 0 intersect sphere 1
|
||
bool
|
||
Intersection::Intersect(const Math::sphere& s0, const Math::sphere& s1 )
|
||
{
|
||
point c_ = s0.p - s1.p;
|
||
float3 c(c_.x(),c_.y(),c_.z() );
|
||
|
||
float lengthsq = c.lengthsq();
|
||
|
||
if ( n_sqrt( s0.r + s1.r ) > lengthsq )
|
||
return true;
|
||
else
|
||
return false;
|
||
}
|
||
|
||
// return true if intersect
|
||
bool
|
||
Intersection::Intersect(const sphere& sph, const frustum& fru)
|
||
{
|
||
return Intersect (sph, fru.planes, frustum::NumPlanes);
|
||
}
|
||
|
||
bool
|
||
Intersection::Intersect(const sphere& sph, const plane* p, const int planeCount)
|
||
{
|
||
for (int i = 0; i < planeCount; ++i)
|
||
{
|
||
float dist = p[i].distance(sph.p);
|
||
if (dist - sph.r > 0)
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool
|
||
Intersection::Intersect(const bbox& box, const sphere& sph)
|
||
{
|
||
float4 vec4 = float4::minimize(float4::maximize(sph.p, box.pmin), box.pmax);
|
||
|
||
float num3 = sph.p.x() - vec4.x();
|
||
float num2 = sph.p.y() - vec4.y();
|
||
float num = sph.p.z() - vec4.z();
|
||
float num4 = ((num3 * num3) + (num2 * num2)) + (num * num);//sph 和 vec4的距离的平方
|
||
|
||
return num <= (sph.r * sph.r);
|
||
}
|
||
|
||
} |