Vector3f TrackballManipulator::SphereLineIntersection(const Vector3f& pStart, const Vector3f& pEnd,
const Vector3f& pSpherePos, Float pRadius)
{
// pStart P1 coordinates (point of line)
// pEnd P2 coordinates (point of line)
// pSpherePos, pRadius P3 coordinates and radius (sphere)
Vector3f interVector;
Float a, b, c, mu, i ;
a = (pEnd - pStart).GetLengthSqr();
b = 2 * ( (pEnd.x - pStart.x)*(pStart.x - pSpherePos.x) +
(pEnd.y - pStart.y)*(pStart.y - pSpherePos.y) +
(pEnd.z - pStart.z)*(pStart.z - pSpherePos.z) );
c = pSpherePos.GetLengthSqr() +
pStart.GetLengthSqr() -
2 * ( pSpherePos | pStart ) -
pRadius * pRadius;
i = b * b - 4 * a * c ;
if( i < 0.0 )
{
// no intersection
interVector = Closest(pStart, pEnd, pSpherePos);
}
if( i == 0.0 )
{
// one intersection
mu = -b/(2*a) ;
interVector = pStart + ((pEnd - pStart) * mu);
}
if ( i > 0.0 )
{
// two intersections
// first intersection
mu = (-b + Maths::Sqrt( b*b - 4*a*c )) / (2*a);
Vector3f inter1 = pStart + ((pEnd - pStart) * mu);
// second intersection
mu = (-b - Maths::Sqrt( b*b - 4*a*c )) / (2*a);
Vector3f inter2 = pStart + ((pEnd - pStart) * mu);
// Get the camera.
Camera* camera = mEditor->GetWorldManager().GetCurrentCamera();
if((inter1 - camera->GetPosition()).GetLength() <
(inter2 - camera->GetPosition()).GetLength())
{
interVector = inter1;
}
else
{
interVector = inter2;
}
}
return interVector;
}
// Return the closest point on the line segment start_end to point.
Vector3f TrackballManipulator::Closest(const Vector3f& pStart,
const Vector3f& pEnd,
const Vector3f& pPoint)
{
// Acquire a vector from the start point to the point in question.
Vector3f A;
A.x = pPoint.x - pStart.x;
A.y = pPoint.y - pStart.y;
A.z = pPoint.z - pStart.z;
// Acquire a vector from the start point to the end point.
Vector3f B;
B.x = pEnd.x - pStart.x;
B.y = pEnd.y - pStart.y;
B.z = pEnd.z - pStart.z;
// Determine the length of the segment start_end.
Float d = B.GetLengthSqr();
// Normalize the vector.
B.Normalize();
// Calculate the dot product between the two vectors.
Float t = A.x * B.x + A.y * B.y + A.z * B.z;
// Rule out some special cases.
// t <= 0, meaning behind the start point.
if(t*t <= 0)
return pStart;
// t >= the length of start_end, meaning past the end point.
else if(t*t >= d) // t squared, to avoid an unnecessary square root operation.
return pEnd;
// If the closest point on start_end to point is somewhere along the segment, determine exactly where.
Vector3f C;
C.x = B.x * t + pStart.x;
C.y = B.y * t + pStart.y;
C.z = B.z * t + pStart.z;
// Return the final value.
return C;
}