bool RaySphereIntersect( const Vector3<T> &vSphereCenter, T radius,
const Vector3<T> &vRayStart, const Vector3<T> &vRayDir,
T distMax, T &distHit, bool bDiscardInner )
{
T a,b,c;
Vector3<T> vRayOrig = vRayStart - vSphereCenter;
T fRayOrigLengthSq = vRayOrig.LengthSq();
T radiusSq = radius*radius;
if( bDiscardInner && fRayOrigLengthSq < radiusSq )
{
distHit = T(-1.0);
return false;
}
a = vRayDir.Dot(vRayDir);
b = T(2.0) * vRayDir.Dot(vRayOrig);
c = vRayOrig.Dot(vRayOrig) - radiusSq;
T d = b*b - T(4.0)*a*c;
if( d < T(0.0) )
{
distHit = T(-1.0);
return false;
}
//Solution for p = eye + view * t
T t = ( -b - sqrt(d) ) / (T(2.0) * a);
if( t < 0.0f )
t = ( -b + sqrt(d) ) / (T(2.0) * a);
distHit = t;
return distHit >= T(0.0) && distHit <= distMax;
}
// 線分と線分の距離の平方を返す(カプセル用)
float Collision::DistanceSegmentSegmentSq( const Vector3& l1p1, const Vector3& l1p2, const Vector3& l2p1, const Vector3& l2p2 )
{
// ねじれの位置の判定
Vector3 v1 = l1p2 - l1p1;
Vector3 v2 = l2p2 - l2p1;
Vector3 n;
Vector3Cross( n, v1, v2 );
float nn = n.LengthSq();
if ( nn ) {
// 平行ではない
Vector3 v12 = l2p1 - l1p1;
float nv12 = Vector3Dot( n, v12 );
Vector3 vd = n * ( nv12 / nn );
Vector3 q1 = l2p1 - vd;
Vector3 q2 = l2p2 - vd;
Vector3 p1q1 = q1 - l1p1;
Vector3 p1q2 = q2 - l1p1;
Vector3 r1, r2;
Vector3Cross( r1, v1, p1q1 );
Vector3Cross( r2, v1, p1q2 );
if ( Vector3Dot( r1, r2 ) < 0 ) {
Vector3 v3 = q2 - q1;
Vector3 q1p1 = l1p1 - q1;
Vector3 q1p2 = l1p2 - q1;
Vector3Cross( r1, v3, q1p1 );
Vector3Cross( r2, v3, q1p2 );
if (Vector3Dot( r1, r2 ) < 0 ) {
// ねじれの位置
return nv12 * nv12 / nn;
}
}
}
// ねじれじゃない位置
return min(
min( DistancePointSegmentSq( l1p1, l2p1, l2p2 ),
DistancePointSegmentSq( l1p2, l2p1, l2p2 ) ),
min(DistancePointSegmentSq( l2p1, l1p1, l1p2 ),
DistancePointSegmentSq( l2p2, l1p1, l1p2 ) )
);
}
bool Sphere::Trace(const Ray& ray, float& distance) const
{
Vector3 rayToSphereCenter = Centre - ray.Origin;
float lengthRTSC2 = rayToSphereCenter.LengthSq();
float closestApproach = Vector3::Dot(rayToSphereCenter, ray.Direction);
if (closestApproach < 0.0f) // the intersection is behind the ray
return false;
// halfCord2 = the distance squared from the closest approach of the ray to a perpendicular to the ray
// through the center of the sphere to the place where the ray actually intersects the sphere
float halfCord2 = (Radius * Radius) - lengthRTSC2 + (closestApproach * closestApproach);
if(halfCord2 < 0.0f)
return false; // the ray missed the sphere
distance = closestApproach - (float)sqrt((float)halfCord2);
return true;
/*
Vector3 v = ray.Origin - Centre;
float b = - Vector3::Dot( v, ray.Direction );
float det = (b*b) - Vector3::Dot( v, v ) + sqrtf( Radius );
if ( det > 0.0f )
{
det = sqrtf( det );
float i1 = b - det;
float i2 = b + det;
if ( i2 > 0 )
{
if ( i1 < 0 )
{
// inside sphere
distance = i2;
return true;
}
else
{
// hit sphere
distance = i1;
return true;
}
}
}
return false;
*/
/*
vector3 v = a_Ray.GetOrigin() - m_Centre;
float b = -DOT( v, a_Ray.GetDirection() );
float det = (b * b) - DOT( v, v ) + m_SqRadius;
int retval = MISS;
if (det > 0)
{
det = sqrtf( det );
float i1 = b - det;
float i2 = b + det;
if (i2 > 0)
{
if (i1 < 0)
{
if (i2 < a_Dist)
{
a_Dist = i2;
retval = INPRIM;
}
}
else
{
if (i1 < a_Dist)
{
a_Dist = i1;
retval = HIT;
}
}
}
}
return retval;
*/
}