// From Graphics Gems - intersection of sphere with ray
void Foam::searchableSphere::findLineAll
(
const point& start,
const point& end,
pointIndexHit& near,
pointIndexHit& far
) const
{
near.setMiss();
far.setMiss();
vector dir(end-start);
scalar magSqrDir = magSqr(dir);
if (magSqrDir > ROOTVSMALL)
{
const vector toCentre(centre_-start);
scalar magSqrToCentre = magSqr(toCentre);
dir /= Foam::sqrt(magSqrDir);
scalar v = (toCentre & dir);
scalar disc = sqr(radius_) - (magSqrToCentre - sqr(v));
if (disc >= 0)
{
scalar d = Foam::sqrt(disc);
scalar nearParam = v-d;
if (nearParam >= 0 && sqr(nearParam) <= magSqrDir)
{
near.setHit();
near.setPoint(start + nearParam*dir);
near.setIndex(0);
}
scalar farParam = v+d;
if (farParam >= 0 && sqr(farParam) <= magSqrDir)
{
far.setHit();
far.setPoint(start + farParam*dir);
far.setIndex(0);
}
}
}
}
void Foam::planeSearchableSurface::findLineAll
(
const point& start,
const point& end,
pointIndexHit& hit
) const
{
hit.setMiss();
vector dir=end-start;
scalar f=plane_.normalIntersect(start,dir);
if(0<=f && f<=1) {
hit.rawPoint()=start+f*dir;
hit.setHit();
hit.setIndex(0);
}
}
void Foam::searchableDisk::findLine
(
const point& start,
const point& end,
pointIndexHit& info
) const
{
info = pointIndexHit(false, Zero, -1);
vector v(start - origin_);
// Decompose sample-origin into normal and parallel component
scalar parallel = (v & normal_);
if (sign(parallel) == sign((end - origin_) & normal_))
{
return;
}
// Remove the parallel component and normalise
v -= parallel*normal_;
scalar magV = mag(v);
if (magV < ROOTVSMALL)
{
v = Zero;
}
else
{
v /= magV;
}
// Set (hit or miss) to intersection of ray and plane of disk
info.setPoint(origin_ + magV*v);
if (magV <= radius_)
{
info.setHit();
info.setIndex(0);
}
}
// From http://www.gamedev.net/community/forums/topic.asp?topic_id=467789 -
// intersection of cylinder with ray
void Foam::searchableCylinder::findLineAll
(
const point& start,
const point& end,
pointIndexHit& near,
pointIndexHit& far
) const
{
near.setMiss();
far.setMiss();
vector point1Start(start-point1_);
vector point2Start(start-point2_);
vector point1End(end-point1_);
// Quick rejection of complete vector outside endcaps
scalar s1 = point1Start&unitDir_;
scalar s2 = point1End&unitDir_;
if ((s1 < 0 && s2 < 0) || (s1 > magDir_ && s2 > magDir_))
{
return;
}
// Line as P = start+t*V where V is unit vector and t=[0..mag(end-start)]
vector V(end-start);
scalar magV = mag(V);
if (magV < ROOTVSMALL)
{
return;
}
V /= magV;
// We now get the nearest intersections to start. This can either be
// the intersection with the end plane or with the cylinder side.
// Get the two points (expressed in t) on the end planes. This is to
// clip any cylinder intersection against.
scalar tPoint1;
scalar tPoint2;
// Maintain the two intersections with the endcaps
scalar tNear = VGREAT;
scalar tFar = VGREAT;
{
scalar s = (V&unitDir_);
if (mag(s) > VSMALL)
{
tPoint1 = -s1/s;
tPoint2 = -(point2Start&unitDir_)/s;
if (tPoint2 < tPoint1)
{
Swap(tPoint1, tPoint2);
}
if (tPoint1 > magV || tPoint2 < 0)
{
return;
}
// See if the points on the endcaps are actually inside the cylinder
if (tPoint1 >= 0 && tPoint1 <= magV)
{
if (radius2(start+tPoint1*V) <= sqr(radius_))
{
tNear = tPoint1;
}
}
if (tPoint2 >= 0 && tPoint2 <= magV)
{
if (radius2(start+tPoint2*V) <= sqr(radius_))
{
// Check if already have a near hit from point1
if (tNear <= 1)
{
tFar = tPoint2;
}
else
{
tNear = tPoint2;
}
}
}
}
else
{
// Vector perpendicular to cylinder. Check for outside already done
// above so just set tpoint to allow all.
tPoint1 = -VGREAT;
tPoint2 = VGREAT;
}
}
const vector x = point1Start ^ unitDir_;
const vector y = V ^ unitDir_;
const scalar d = sqr(radius_);
// Second order equation of the form a*t^2 + b*t + c
const scalar a = (y&y);
const scalar b = 2*(x&y);
const scalar c = (x&x)-d;
const scalar disc = b*b-4*a*c;
scalar t1 = -VGREAT;
scalar t2 = VGREAT;
if (disc < 0)
{
// Fully outside
return;
}
else if (disc < ROOTVSMALL)
{
// Single solution
if (mag(a) > ROOTVSMALL)
{
t1 = -b/(2*a);
//Pout<< "single solution t:" << t1
// << " for start:" << start << " end:" << end
// << " c:" << c << endl;
if (t1 >= 0 && t1 <= magV && t1 >= tPoint1 && t1 <= tPoint2)
{
// valid. Insert sorted.
if (t1 < tNear)
{
tFar = tNear;
tNear = t1;
}
else if (t1 < tFar)
{
tFar = t1;
}
}
else
{
return;
}
}
else
{
// Aligned with axis. Check if outside radius
//Pout<< "small discriminant:" << disc
// << " for start:" << start << " end:" << end
// << " magV:" << magV
// << " c:" << c << endl;
if (c > 0)
{
return;
}
}
}
else
{
if (mag(a) > ROOTVSMALL)
{
scalar sqrtDisc = sqrt(disc);
t1 = (-b - sqrtDisc)/(2*a);
t2 = (-b + sqrtDisc)/(2*a);
if (t2 < t1)
{
Swap(t1, t2);
}
if (t1 >= 0 && t1 <= magV && t1 >= tPoint1 && t1 <= tPoint2)
{
// valid. Insert sorted.
if (t1 < tNear)
{
tFar = tNear;
tNear = t1;
}
else if (t1 < tFar)
{
tFar = t1;
}
}
if (t2 >= 0 && t2 <= magV && t2 >= tPoint1 && t2 <= tPoint2)
{
// valid. Insert sorted.
if (t2 < tNear)
{
tFar = tNear;
tNear = t2;
}
else if (t2 < tFar)
{
tFar = t2;
}
}
//Pout<< "two solutions t1:" << t1 << " t2:" << t2
// << " for start:" << start << " end:" << end
// << " magV:" << magV
// << " c:" << c << endl;
}
else
{
// Aligned with axis. Check if outside radius
//Pout<< "large discriminant:" << disc
// << " small a:" << a
// << " for start:" << start << " end:" << end
// << " magV:" << magV
// << " c:" << c << endl;
if (c > 0)
{
return;
}
}
}
// Check tNear, tFar
if (tNear >= 0 && tNear <= magV)
{
near.setPoint(start+tNear*V);
near.setHit();
near.setIndex(0);
if (tFar <= magV)
{
far.setPoint(start+tFar*V);
far.setHit();
far.setIndex(0);
}
}
else if (tFar >= 0 && tFar <= magV)
{
near.setPoint(start+tFar*V);
near.setHit();
near.setIndex(0);
}
}