D:/Zythum/DinoKod/Common/Math3D.cpp

#include "Common/Assert.h"
#include "Common/Error.h"
#include "Common/Console.h"
#include "Common/Vector.h"
#include "Common/Matrix.h"
#include "Common/Point.h"
#include "Common/Plane.h"

#include "Common/Math3D.h"

#define EPSILON 0.1f
#define COLL_TEST_CULL

//---------------------------------------------------------------------------------------------------------------------
void GetLocalAngleFromNormal( KVector& Normal, float& AngleX, float& AngleY, float& AngleZ )
{
        // Angle X
        if( ( Normal.y > -0.001f ) && ( Normal.y < 0.001f ) )
                if( ( Normal.z > -0.001f ) && ( Normal.z < 0.001f ) )
                        AngleX = 0.0f;
                else
                        if( Normal.z > 0.0f )
                                AngleX = PI / 2.0f;
                        else
                                AngleX = 3.0f * PI / 2.0f;
        else
                if( Normal.y > 0.0f )
                        AngleX = atanf( Normal.z / Normal.y );
                else
                        AngleX = atanf( Normal.z / Normal.y ) + PI;

        AngleX = fmodf( AngleX + 2.0f * PI, 2.0f * PI );

        // TODO : Angle Y
        AngleY = 0.0f;
/*
        // Angle Z
        if( ( Normal.y > -0.001f ) && ( Normal.y < 0.001f ) )
                if( ( Normal.x > -0.001f ) && ( Normal.x < 0.001f ) )
                        AngleZ = 0.0f;
                else
                        if( Normal.x > 0.0f )
                                AngleZ = PI / 2.0f;
                        else
                                AngleZ = 3.0f * PI / 2.0f;
        else
                if( Normal.y > 0.0f )
                        AngleZ = atanf( Normal.x / Normal.y );
                else
                        AngleZ = atanf( Normal.x / Normal.y ) + PI;

//      AngleZ = -AngleZ;
        AngleZ = fmodf( AngleZ + 2.0f * PI, 2.0f * PI );
*/
        // Angle Z
        if( ( Normal.x > -0.001f ) && ( Normal.x < 0.001f ) )
                if( ( Normal.y > -0.001f ) && ( Normal.y < 0.001f ) )
                        AngleZ = 0.0f;
                else
                        if( Normal.y > 0.0f )
                                AngleZ = PI / 2.0f;
                        else
                                AngleZ = 3.0f * PI / 2.0f;
        else
                if( Normal.x > 0.0f )
                        AngleZ = atanf( Normal.y / Normal.x );
                else
                        AngleZ = atanf( Normal.y / Normal.x ) + PI;

        AngleZ -= PI / 2.0f;
        AngleZ = fmodf( AngleZ + 2.0f * PI, 2.0f * PI );

        KVector NormalZ = Normal;

        NormalZ.x = Normal.x * cosf( -AngleZ ) + Normal.y * -sinf( -AngleZ );
        NormalZ.y = Normal.x * sinf( -AngleZ ) + Normal.y * cosf( -AngleZ );
        NormalZ.z = Normal.z;

        // Angle X
        if( ( NormalZ.y > -0.001f ) && ( NormalZ.y < 0.001f ) )
                if( ( NormalZ.z > -0.001f ) && ( NormalZ.z < 0.001f ) )
                        AngleX = 0.0f;
                else
                        if( NormalZ.z > 0.0f )
                                AngleX = PI / 2.0f;
                        else
                                AngleX = 3.0f * PI / 2.0f;
        else
                if( NormalZ.y > 0.0f )
                        AngleX = atanf( NormalZ.z / NormalZ.y );
                else
                        AngleX = atanf( NormalZ.z / NormalZ.y ) + PI;

        AngleX = fmodf( AngleX + 2.0f * PI, 2.0f * PI );
}

//---------------------------------------------------------------------------------------------------------------------
bool IntersectTriangle( KVector& orig, KVector& dir, KVector& v0, KVector& v1, KVector& v2, float* t, float* u, float* v )
{
    // Find vectors for two edges sharing vert0
    KVector edge1 = v1 - v0;
    KVector edge2 = v2 - v0;

    // Begin calculating determinant - also used to calculate U parameter
    KVector pvec;
    pvec = KVector::CrossProduct( dir, edge2 );

        KVector Normal = KVector::CrossProduct( edge1, edge2 );
        Normal.Normalize();
        KPlane  Plane( Normal, KVector::DotProduct( Normal, v0 ) );

        float   fOrig = KVector::DotProduct( Plane.m_Normal, orig ) - Plane.m_Distance;
        float   fDir = KVector::DotProduct( Plane.m_Normal, orig + dir ) - Plane.m_Distance;


//      g_pConsole << "DISTANCE : " << fOrig << " " << fDir << KENDL;
//      if( ( fabs( fOrig ) < 0.5f ) || ( fabs( fDir ) < 0.5f ) )
//              goto test;
//
//      if( !( ( fOrig > 0.0f ) && ( fDir < 0.0f ) ) )
//              return false;
        if( ( ( fOrig > 0.0f ) && ( fDir > 0.0f ) ) || ( ( fOrig < 0.0f ) && ( fDir < 0.0f ) ) )
                return false;


//test:
    // If determinant is near zero, ray lies in plane of triangle
    float det = KVector::DotProduct( edge1, pvec );

#ifdef COLL_TEST_CULL
        
        if( det < EPSILON )
                return false;

    // Calculate distance from vert0 to ray origin
    KVector tvec = orig - v0;

    // Calculate U parameter and test bounds
    *u = KVector::DotProduct( tvec, pvec );
    if( *u < 0.0f || *u > det )
        return false;

    // Prepare to test V parameter
    KVector qvec;
        qvec = KVector::CrossProduct( tvec, edge1 );

    // Calculate V parameter and test bounds
    *v = KVector::DotProduct( dir, qvec );
    if( *v < 0.0f || *u + *v > det )
        return false;

    // Calculate t, scale parameters, ray intersects triangle
    *t = KVector::DotProduct( edge2, qvec );
    float fInvDet = 1.0f / det;
    *t *= fInvDet;
    *u *= fInvDet;
    *v *= fInvDet;

//      g_pConsole << *t << KENDL;
        
//      KASSERT( *t >= 0.0f );
//      if( ( *t > 0.4f ) || ( *t < 10.0f ) )
//              return false;

#else

        if ((det > -EPSILON) && (det < EPSILON))
                return false; 
        
        float   inv_det = 1.0f / det;
        
        // calcule la distance entre l'origine et le premier vertex du triangle
        KVector tvec = orig - v0;
        
        // calcule l'élement u
        *u = KVector::DotProduct( tvec, pvec ) * inv_det;
        
        if ((*u < 0.0f) || (*u > 1.0f))
                return false;
        
        // Prepare to test v parameter
        KVector qvec = KVector::CrossProduct( tvec, edge1 );
        
        // Calcule le paramètre v
        *v = KVector::DotProduct( dir, qvec ) * inv_det;
        
        if ((*v < 0.0f) || ((*u + *v) > 1.0f))
                return false;
           
        //calcule la distance entre l'origine et le point de collision.
        *t = KVector::DotProduct( edge2, qvec ) * inv_det;

#endif
 
        return true;
}

//---------------------------------------------------------------------------------------------------------------------
bool IntersectTriangleCullNone( KVector& orig, KVector& dir, KVector& v0, KVector& v1, KVector& v2, float* t, float* u, float* v )
{
    // Find vectors for two edges sharing vert0
    KVector edge1 = v1 - v0;
    KVector edge2 = v2 - v0;

    // Begin calculating determinant - also used to calculate U parameter
    KVector pvec;
    pvec = KVector::CrossProduct( dir, edge2 );

    // If determinant is near zero, ray lies in plane of triangle
    float det = KVector::DotProduct( edge1, pvec );

        if ((det > -EPSILON) && (det < EPSILON))
                return false; 
        
        float   inv_det = 1.0f / det;
        
        // calcule la distance entre l'origine et le premier vertex du triangle
        KVector tvec = orig - v0;
        
        // calcule l'élement u
        *u = KVector::DotProduct( tvec, pvec ) * inv_det;
        
        if ((*u < 0.0f) || (*u > 1.0f))
                return false;
        
        // Prepare to test v parameter
        KVector qvec = KVector::CrossProduct( tvec, edge1 );
        
        // Calcule le paramètre v
        *v = KVector::DotProduct( dir, qvec ) * inv_det;
        
        if ((*v < 0.0f) || ((*u + *v) > 1.0f))
                return false;
           
        //calcule la distance entre l'origine et le point de collision.
        *t = KVector::DotProduct( edge2, qvec ) * inv_det;

        return true;
}


//---------------------------------------------------------------------------------------------------------------------
bool IntersectTriangle( KVector& center, float radius, KVector& v0, KVector& v1, KVector& v2, KVector* pPoint )
{
    // Find vectors for two edges sharing vert0
    KVector edge1 = v1 - v0;
    KVector edge2 = v2 - v0;
    KVector edge3 = v1 - v2;

        KVector Normal = KVector::CrossProduct( edge1, edge2 );
        Normal.Normalize();
        KPlane  Plane( Normal, KVector::DotProduct( Normal, v0 ) );

        float   fDist = (KVector::DotProduct( Plane.m_Normal, center ) - Plane.m_Distance);

        // La sphere est suffisament loin du plan, pas de collision
        if( fDist > radius )
                return false;

        if( fDist < 0.0f )
                return false;

        // La sphere touche le plan
//      *t = -fDist;

        *pPoint = center + Normal * -fDist;

        // Edge 1
        KVector ve1 = KVector::CrossProduct( Normal, v1 - v0 );
        ve1.Normalize();
        KPlane  Plane1( ve1, KVector::DotProduct( ve1, v0 ) );
        float   d1 = KVector::DotProduct( Plane1.m_Normal, *pPoint ) - Plane1.m_Distance;
        if( d1 < 0.0f )
                return false;

        // Edge 2
        KVector ve2 = KVector::CrossProduct( Normal, v0 - v2 );
        ve2.Normalize();
        KPlane  Plane2( ve2, KVector::DotProduct( ve2, v0 ) );
        float   d2 = KVector::DotProduct( Plane2.m_Normal, *pPoint ) - Plane2.m_Distance;
        if( d2 < 0.0f )
                return false;

        // Edge 3
        KVector ve3 = KVector::CrossProduct( Normal, v2 - v1 );
        ve3.Normalize();
        KPlane  Plane3( ve3, KVector::DotProduct( ve3, v1 ) );
        float   d3 = KVector::DotProduct( Plane3.m_Normal, *pPoint ) - Plane3.m_Distance;
        if( d3 < 0.0f )
                return false;

        return true;
 /*   // Begin calculating determinant - also used to calculate U parameter
    KVector pvec;
    pvec = KVector::CrossProduct( dir, edge2 );

    // If determinant is near zero, ray lies in plane of triangle
    float det = KVector::DotProduct( edge1, pvec );

        if ((det > -EPSILON) && (det < EPSILON))
                return false; 
        
        float   inv_det = 1.0f / det;
        
        // calcule la distance entre l'origine et le premier vertex du triangle
        KVector tvec = orig - v0;
        
        // calcule l'élement u
        *u = KVector::DotProduct( tvec, pvec ) * inv_det;
        
        if ((*u < 0.0f) || (*u > 1.0f))
                return false;
        
        // Prepare to test v parameter
        KVector qvec = KVector::CrossProduct( tvec, edge1 );
        
        // Calcule le paramètre v
        *v = KVector::DotProduct( dir, qvec ) * inv_det;
        
        if ((*v < 0.0f) || ((*u + *v) > 1.0f))
                return false;
           
        //calcule la distance entre l'origine et le point de collision.
        *t = KVector::DotProduct( edge2, qvec ) * inv_det;
        return true;*/
}

//---------------------------------------------------------------------------------------------------------------------
void ComputePickVector( KMatrix& MatProj, KMatrix& MatView, KPt& PickPoint, KPt& ViewportSize, KVector* PickRayOrig, KVector* PickRayDir )
{
        // Compute the vector of the pick ray in screen space
        KVector v;
        v.x =  ( ( ( 2.0f * PickPoint.x ) / ViewportSize.x ) - 1 ) / MatProj._11;
        v.y = -( ( ( 2.0f * PickPoint.y ) / ViewportSize.y ) - 1 ) / MatProj._22;
        v.z =  1.0f;

        // Get the inverse view matrix
        KMatrix m = MatView;
        m.Inverse();
//      m = MatrixInverse( MatView );

        // Transform the screen space pick ray into 3D space
        PickRayDir->x  = v.x * m._11 + v.y * m._21 + v.z * m._31;
        PickRayDir->y  = v.x * m._12 + v.y * m._22 + v.z * m._32;
        PickRayDir->z  = v.x * m._13 + v.y * m._23 + v.z * m._33;
        PickRayOrig->x = m._41;
        PickRayOrig->y = m._42;
        PickRayOrig->z = m._43;
}


//---------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------
KVector ClosestPointOnLine( KVector& Point, KVector& v0, KVector& v1 )
{
        KVector c = Point - v0;
        KVector V = v1 - v0; 
        float d = V.Magnitude();

        V.Normalize();
        float t = KVector::DotProduct( V, c );

        if( t < 0.0f ) return v0;
        if( t > d ) return v1;

        V *= t;

        return v0 + V;  
}

//---------------------------------------------------------------------------------------------------------------------
KVector ClosestPointOnTriangle( KVector& Point, KVector& v0, KVector& v1, KVector& v2 )
{
        KVector r01 = ClosestPointOnLine( Point, v0, v1 );
        KVector r12 = ClosestPointOnLine( Point, v1, v2 );
        KVector r20 = ClosestPointOnLine( Point, v2, v0 );
        KVector m01 = Point - r01;
        KVector m12 = Point - r12;
        KVector m20 = Point - r20;
                                                                         
        float d01 = m01.Magnitude();
        float d12 = m12.Magnitude();
        float d20 = m20.Magnitude();

        float min = d01;
        KVector Result = r01;

        if( d12 < min )
        {
                min = d12;
                Result = r12;
        }

        if( d20 < min )
                Result = r20;

        return Result;  
}

//---------------------------------------------------------------------------------------------------------------------
bool CheckPointInTriangle( KVector& Point, KVector& v0, KVector& v1, KVector& v2 )
{
        float TotalAngles = 0.0f;

        // Calcul les 3 vecteurs
        KVector VecteurA = Point - v0;
        KVector VecteurB = Point - v1;
        KVector VecteurC = Point - v2;

        VecteurA.Normalize();
        VecteurB.Normalize();
        VecteurC.Normalize();

        TotalAngles += acosf( KVector::DotProduct( VecteurA, VecteurB ) );   
        TotalAngles += acosf( KVector::DotProduct( VecteurB, VecteurC ) );
        TotalAngles += acosf( KVector::DotProduct( VecteurC, VecteurA ) ); 

        // Teste avec une marge d'erreur
        if( fabsf( TotalAngles - 2.0f * PI ) <= 0.005 )
                return true;

        return false;
}

//---------------------------------------------------------------------------------------------------------------------
bool CheckPointInSphere( KVector Point, KVector& sO, float sR )
{
        Point -= sO;
        float d = Point.Magnitude();

        if( d <= sR )
                return true;

   return false;
}
/*
//---------------------------------------------------------------------------------------------------------------------
float IntersectRayPlaneDistancef( KVector& RayOrigin, KVector& RayVector, KVector& PointOrigin, KVector& PointNormal )
{
        float d = -KVector::DotProduct( PointNormal, PointOrigin );
        float numer = KVector::DotProduct( PointNormal, RayOrigin ) + d;
        float denom = KVector::DotProduct( PointNormal, RayVector );

        if( denom == 0 )  // La normale est orthogonale au vector, pas d'intersection
                return -1.0f;
        
        return -( numer / denom );      
}
*/
//---------------------------------------------------------------------------------------------------------------------
float IntersectRayPlaneDistance( KVector& RayOrigin, KVector& RayVector, KVector& PointOrigin, KVector& PointNormal )
{
        float d = -KVector::DotProduct( PointNormal, PointOrigin );
        float numer = KVector::DotProduct( PointNormal, RayOrigin ) + d;
        float denom = KVector::DotProduct( PointNormal, RayVector );

        if( denom == 0 )        // La normale est orthogonale au vector, pas d'intersection
                return -1.0f;

        return -( numer / denom );      
}

//---------------------------------------------------------------------------------------------------------------------
float IntersectRaySphere( KVector& RayOrigin, KVector& RayVector, KVector& SphereOrigin, float SphereRadius )
{
        KVector Q = SphereOrigin - RayOrigin;
        float   c = Q.Magnitude();
        float   v = KVector::DotProduct( Q, RayVector );
        float   d = SphereRadius * SphereRadius - ( c * c - v * v );

        if( d < 0.0f )
                return -1.0f;

        return v - sqrtf( d );
}

//---------------------------------------------------------------------------------------------------------------------
float IntersectRayPlaneDistance( KVector& RayOrigin, KVector& RayVector, KVector& PlaneNormal, float PlaneDist )
{
        float   CosAlpha;
        float   Delta;

        CosAlpha = KVector::DotProduct( RayVector, PlaneNormal );

        // parallel to the plane (alpha=90)
        if( CosAlpha == 0 )
                return -1.0f;

        Delta = PlaneDist - KVector::DotProduct( RayOrigin, PlaneNormal );

        return ( Delta / CosAlpha );
}

//---------------------------------------------------------------------------------------------------------------------
bool GetLowestRoot( float a, float b, float c, float maxR, float* root )
{
        // Check if a solution exists
        float determinant = b * b - 4.0f * a * c;

        // If determinant is negative it means no solutions.
        if( determinant < 0.0f )
                return false;

        // calculate the two roots: (if determinant == 0 then
        // x1==x2 but let’s disregard that slight optimization)
        float sqrtD = sqrtf( determinant );
        float r1 = (-b - sqrtD) / (2*a);
        float r2 = (-b + sqrtD) / (2*a);

        // Sort so x1 <= x2
        if (r1 > r2)
        {
                float temp = r2;
                r2 = r1;
                r1 = temp;
        }

        // Get lowest root:
        if (r1 > 0 && r1 < maxR)
        {
                *root = r1;
                return true;
        }

        // It is possible that we want x2 - this can happen
        // if x1 < 0
        if (r2 > 0 && r2 < maxR)
        {
                *root = r2;
                return true;
        }
        // No (valid) solutions
        return false;
}
/*
//---------------------------------------------------------------------------------------------------------------------
bool CheckSphereCollision( KVector& Center, float Radius, KVector& Velocity, KVector& v0, KVector& v1, KVector& v2, KVector* pPoint, KVector* pNormal )
{
        // Normalize la vélocité
        KVector NormalizedVelocity = Velocity;
        NormalizedVelocity.Normalize();

        // Calcule les vecteurs des cotés
    KVector Edge1 = v1 - v0;
    KVector Edge2 = v2 - v0;

        // Calcule la normale du triangle
        *pNormal = KVector::CrossProduct( Edge1, Edge2 );
        pNormal->Normalize();

        // Calcule le plan du triangle
        KPlane  Plane( *pNormal, KVector::DotProduct( v0, *pNormal ) );

        // Calcule le point d'intersection de la sphere
        KVector SpherePoint = Center - (*pNormal * Radius);

        // Determine la position du point d'intersection de la sphere par rapport au plan du triangle
        KVector PlanePoint;
        float   Distance;
        
        if( Plane.GetPointSide( SpherePoint ) == KPPS_FRONTSIDE )
        {
                // Devant
                // Calcule le point d'intersection du plan
                Distance        = IntersectRayPlaneDistance( SpherePoint, NormalizedVelocity, v0, *pNormal );
                PlanePoint  = SpherePoint + NormalizedVelocity * Distance;
        }
        else
        {
                // Derrière
                // Calcule le point d'intersection du plan
                Distance        = IntersectRayPlaneDistance( SpherePoint, *pNormal, v0, *pNormal );
                PlanePoint      = SpherePoint + *pNormal * Distance;
        }

        // Determine le point d'intersection de la sphere avec le triangle s'il y en a 1
        KVector TrianglePoint = PlanePoint;

        // Teste si le point d'intersection est dans le triangle
        if( !CheckPointInTriangle( PlanePoint, v0, v1, v2 ) ) 
        {
                return false;

                // Calcule le point le plus proche
                TrianglePoint = ClosestPointOnTriangle( PlanePoint, v0, v1, v2 );
                Distance = IntersectRayPlaneDistance( TrianglePoint, -NormalizedVelocity, Center, KVector( 1.0f, 1.0f, 1.0f ) );

                if( Distance > 0.0f )
                {
                        SpherePoint = TrianglePoint + (-NormalizedVelocity) * Distance;
                }
        }

        if( ( Distance >= 0.0f ) && ( Distance <= Velocity.Magnitude() ) )
        {
                *pPoint = TrianglePoint;
                return true;
        }

        return false;
}
*/

//#include "Common/Console.h"
//---------------------------------------------------------------------------------------------------------------------
bool CheckSphereCollision( KCollisionData* pData, KVector v0, KVector v1, KVector v2, float Epsilon )
{
        // Scale les vertex dans l'espace de l'ellipse
        v0 /= pData->m_Radius;
        v1 /= pData->m_Radius;
        v2 /= pData->m_Radius;

        // Normalize la vélocité
        KVector NormalizedVelocity = pData->m_Velocity;
        NormalizedVelocity.Normalize();

        // Calcule les vecteurs des cotés
    KVector Edge1 = v1 - v0;
    KVector Edge2 = v2 - v0;

        // Calcule la normale du triangle
        KVector Normal = KVector::CrossProduct( Edge1, Edge2 );
        Normal.Normalize();

        // Calcule le plan du triangle
        KPlane  Plane( Normal, KVector::DotProduct( v0, Normal ) );

        // Calcule le point d'intersection de la sphere
        KVector SpherePoint = pData->m_Position - Normal;

        // Determine la position du point d'intersection de la sphere par rapport au plan du triangle
        KVector PlanePoint;
        float   Distance;
        
        if( Plane.GetPointSide( SpherePoint, Epsilon ) == KPPS_BACKSIDE )
        {
                // Derrière
                // Calcule le point d'intersection du plan
                Distance        = IntersectRayPlaneDistance( SpherePoint, Normal, v0, Normal );
                PlanePoint      = SpherePoint + Normal * Distance;
//              g_Console << "KPPS_BACKSIDE" << KENDL;
        }
        else
        {
                // Devant
                // Calcule le point d'intersection du plan
                Distance        = IntersectRayPlaneDistance( SpherePoint, NormalizedVelocity, v0, Normal );
                PlanePoint  = SpherePoint + NormalizedVelocity * Distance;
//              g_Console << "KPPS_FRONTSIDE" << KENDL;
        }

        // Determine le point d'intersection de la sphere avec le triangle s'il y en a 1
        KVector TrianglePoint = PlanePoint;

        // Teste si le point d'intersection est dans le triangle
        if( !CheckPointInTriangle( PlanePoint, v0, v1, v2 ) ) 
        {
                // Calcule le point le plus proche
                TrianglePoint = ClosestPointOnTriangle( PlanePoint, v0, v1, v2 );
                Distance = IntersectRaySphere( TrianglePoint, -NormalizedVelocity, pData->m_Position, 1.0f );

                if( Distance > 0.0f )
                {
                        SpherePoint = TrianglePoint - NormalizedVelocity * Distance;
                }
        }

        // Gestion de l'erreur
        if( CheckPointInSphere( TrianglePoint, pData->m_Position, 1.0f ) )
                pData->m_bStuck = true;

        // Resultat
        if( ( Distance > 0.0f ) && ( Distance <= pData->m_Velocity.Magnitude() ) )
        {
                if( ( pData->m_bFoundCollision == false ) || ( Distance < pData->m_NearestDistance ) )
                {
                        pData->m_NearestDistance                                        = Distance;
                        pData->m_NearestIntersectionPoint                       = SpherePoint;
                        pData->m_NearestPolygonIntersectionPoint        = TrianglePoint;
                        pData->m_bFoundCollision                                        = true;
                        return true;
                }
        }

        return false;
}

//---------------------------------------------------------------------------------------------------------------------
// Improved Collision detection and Response
// Kasper Fauerby
// kasper@peroxide.dk
// http://www.peroxide.dk
// 25th July 2003
//---------------------------------------------------------------------------------------------------------------------
void CheckSphereCollision2( KCollisionData2* pData, KVector v0, KVector v1, KVector v2 )
{
        v0 /= pData->m_eRadius;
        v1 /= pData->m_eRadius;
        v2 /= pData->m_eRadius;

        // Make the plane containing this triangle.
        KPlane  TrianglePlane( v0, v1, v2 );

        // Is triangle front-facing to the velocity vector?
        // We only check front-facing triangles
        // (your choice of course)
        if( TrianglePlane.IsFrontFacingTo( pData->m_NormalizedVelocity ) )
        {
                // Get interval of plane intersection:
                float   t0, t1;
                bool    bEmbeddedInPlane = false;
                
                // Calculate the signed distance from sphere
                // position to triangle plane
                float SignedDistToTrianglePlane = (float)TrianglePlane.SignedDistanceTo( pData->m_BasePoint );

                // cache this as we’re going to use it a few times below:
                float NormalDotVelocity = TrianglePlane.m_Normal.DotProductf( pData->m_Velocity );
                
                // if sphere is travelling parrallel to the plane:
                if( NormalDotVelocity == 0.0f )
                {
                        if( fabs( SignedDistToTrianglePlane ) >= 1.0f )
                        {
                                // Sphere is not embedded in plane.
                                // No collision possible:
                                return;
                        }
                        else
                        {
                                // sphere is embedded in plane.
                                // It intersects in the whole range [0..1]
                                bEmbeddedInPlane = true;
                                t0 = 0.0;
                                t1 = 1.0;
                        }
                }
                else
                {
                        // N dot D is not 0. Calculate intersection interval:
                        t0 = ( -1.0f - SignedDistToTrianglePlane ) / NormalDotVelocity;
                        t1 = ( 1.0f - SignedDistToTrianglePlane ) / NormalDotVelocity;
                        
                        // Swap so t0 < t1
                        if( t0 > t1 )
                        {
                                float temp = t1;
                                t1 = t0;
                                t0 = temp;
                        }

                        // Check that at least one result is within range:
                        if( t0 > 1.0f || t1 < 0.0f )
                        {
                                // Both t values are outside values [0,1]
                                // No collision possible:
                                return;
                        }

                        // Clamp to [0,1]
                        if (t0 < 0.0) t0 = 0.0;
                        if (t1 < 0.0) t1 = 0.0;
                        if (t0 > 1.0) t0 = 1.0;
                        if (t1 > 1.0) t1 = 1.0;
                }

                // OK, at this point we have two time values t0 and t1
                // between which the swept sphere intersects with the
                // triangle plane. If any collision is to occur it must
                // happen within this interval.
                KVector CollisionPoint;
                bool    bFoundCollison = false;
                float   t = 1.0;
                // First we check for the easy case - collision inside
                // the triangle. If this happens it must be at time t0
                // as this is when the sphere rests on the front side
                // of the triangle plane. Note, this can only happen if
                // the sphere is not embedded in the triangle plane.
                if( !bEmbeddedInPlane )
                {
                        KVector PlaneIntersectionPoint = ( pData->m_BasePoint - TrianglePlane.m_Normal ) + pData->m_Velocity * t0;

                        if( CheckPointInTriangle( PlaneIntersectionPoint, v0, v1, v2 ) )
                        {
                                bFoundCollison = true;
                                t = t0;
                                CollisionPoint = PlaneIntersectionPoint;
                        }
                }
                // if we haven’t found a collision already we’ll have to
                // sweep sphere against points and edges of the triangle.
                // Note: A collision inside the triangle (the check above)
                // will always happen before a vertex or edge collision!
                // This is why we can skip the swept test if the above
                // gives a collision!
                if( bFoundCollison == false )
                {
                        // some commonly used terms:
                        KVector Velocity = pData->m_Velocity;
                        KVector Base = pData->m_BasePoint;
                        float VelocitySquaredLength = Velocity.SquareMagnitude();
                        float a,b,c; // Params for equation
                        float newT;
                        // For each vertex or edge a quadratic equation have to
                        // be solved. We parameterize this equation as
                        // a*t^2 + b*t + c = 0 and below we calculate the
                        // parameters a,b and c for each test.
                        // Check against points:
                        a = VelocitySquaredLength;
        
                        // P1
                        b = 2.0f * Velocity.DotProductf( Base - v0 );
                        c = ( v0 - Base ).SquareMagnitude() - 1.0f;
                        if( GetLowestRoot( a, b, c, t, &newT ) )
                        {
                                t = newT;
                                bFoundCollison = true;
                                CollisionPoint = v0;
                        }

                        // P2
                        b = 2.0f * Velocity.DotProductf( Base - v1 );
                        c = ( v1 - Base ).SquareMagnitude() - 1.0f;
                        if( GetLowestRoot( a, b, c, t, &newT ) )
                        {
                                t = newT;
                                bFoundCollison = true;
                                CollisionPoint = v1;
                        }

                        // P3
                        b = 2.0f * Velocity.DotProductf( Base - v2 );
                        c = ( v2 - Base ).SquareMagnitude() - 1.0f;
                        if( GetLowestRoot( a, b, c, t, &newT ) )
                        {
                                t = newT;
                                bFoundCollison = true;
                                CollisionPoint = v2;
                        }

                        // Check agains edges:
                        // p1 -> p2:
                        KVector Edge                            = v1 - v0;
                        KVector BaseToVertex            = v0 - Base;
                        float EdgeSquaredLength         = Edge.SquareMagnitude();
                        float EdgeDotVelocity           = Edge.DotProductf( Velocity );
                        float EdgeDotBaseToVertex       = Edge.DotProductf( BaseToVertex );

                        // Calculate parameters for equation
                        a = EdgeSquaredLength * -VelocitySquaredLength + EdgeDotVelocity * EdgeDotVelocity;
                        b = EdgeSquaredLength * ( 2.0f * Velocity.DotProductf( BaseToVertex ) ) - 2.0f * EdgeDotVelocity * EdgeDotBaseToVertex;
                        c = EdgeSquaredLength * ( 1.0f - BaseToVertex.SquareMagnitude() ) + EdgeDotBaseToVertex * EdgeDotBaseToVertex;

                        // Does the swept sphere collide against infinite edge?
                        if( GetLowestRoot( a, b, c, t, &newT ) )
                        {
                                // Check if intersection is within line segment:
                                float f = ( EdgeDotVelocity * newT - EdgeDotBaseToVertex ) / EdgeSquaredLength;
                                if( f >= 0.0f && f <= 1.0f )
                                {
                                        // intersection took place within segment.
                                        t = newT;
                                        bFoundCollison = true;
                                        CollisionPoint = v0 + Edge * f;
                                }
                        }

                        // p2 -> p3:
                        Edge                            = v2 - v1;
                        BaseToVertex            = v1 - Base;
                        EdgeSquaredLength       = Edge.SquareMagnitude();
                        EdgeDotVelocity         = Edge.DotProductf( Velocity );
                        EdgeDotBaseToVertex     = Edge.DotProductf( BaseToVertex );
                        a = EdgeSquaredLength * -VelocitySquaredLength + EdgeDotVelocity * EdgeDotVelocity;
                        b = EdgeSquaredLength * ( 2.0f * Velocity.DotProductf( BaseToVertex ) ) - 2.0f * EdgeDotVelocity * EdgeDotBaseToVertex;
                        c = EdgeSquaredLength * ( 1.0f - BaseToVertex.SquareMagnitude() ) + EdgeDotBaseToVertex * EdgeDotBaseToVertex;
                        
                        if( GetLowestRoot( a, b, c, t, &newT ) )
                        {
                                float f = ( EdgeDotVelocity * newT - EdgeDotBaseToVertex ) / EdgeSquaredLength;
                                if( f >= 0.0f && f <= 1.0f )
                                {
                                        t = newT;
                                        bFoundCollison = true;
                                        CollisionPoint = v1 + Edge * f;
                                }
                        }

                        // v2 -> v0:
                        Edge                            = v0 - v2;
                        BaseToVertex            = v2 - Base;
                        EdgeSquaredLength       = Edge.SquareMagnitude();
                        EdgeDotVelocity         = Edge.DotProductf( Velocity );
                        EdgeDotBaseToVertex = Edge.DotProductf( BaseToVertex );
                        a = EdgeSquaredLength * -VelocitySquaredLength + EdgeDotVelocity * EdgeDotVelocity;
                        b = EdgeSquaredLength * ( 2.0f * Velocity.DotProductf( BaseToVertex ) ) - 2.0f * EdgeDotVelocity * EdgeDotBaseToVertex;
                        c = EdgeSquaredLength * ( 1.0f - BaseToVertex.SquareMagnitude() ) + EdgeDotBaseToVertex * EdgeDotBaseToVertex;

                        if ( GetLowestRoot( a, b, c, t, &newT ) )
                        {
                                float f = ( EdgeDotVelocity * newT - EdgeDotBaseToVertex ) / EdgeSquaredLength;
                                if( f >= 0.0f && f <= 1.0f )
                                {
                                        t = newT;
                                        bFoundCollison = true;
                                        CollisionPoint = v2 + Edge * f;
                                }
                        }
                }

                // Set result:
                if( bFoundCollison == true )
                {
                        // distance to collision: ’t’ is time of collision
                        float DistToCollision = t * pData->m_Velocity.Magnitude();

                        // Does this triangle qualify for the closest hit?
                        // it does if it’s the first hit or the closest
                        if( pData->m_bFoundCollision == false || DistToCollision < pData->m_NearestDistance )
                        {
                                // Collision information nessesary for sliding
                                pData->m_NearestDistance        = DistToCollision;
                                pData->m_IntersectionPoint      = CollisionPoint;
                                pData->m_CollisionNormal        = TrianglePlane.m_Normal;
                                pData->m_bFoundCollision        = true;
                        }
                }
        } // if not backface
}

/*
//---------------------------------------------------------------------------------------------------------------------
bool CheckSphereCollision( KCollisionData* pData, KVector v0, KVector v1, KVector v2 )
{
        double  DistanceToTravel = pData->m_Velocity.Magnitude();

        // Plan du poly
        KVector Origin = v0;
        KVector Normal = KVector::CrossProduct( v1 - v0, v2 - v0 );
        Normal.Normalize();
        
        // Distance entre le plan et le centre de la sphere
        double  Distance = IntersectRayPlaneDistance( Origin, Normal, pData->m_Position, -Normal );
        KVector SphereIntersection;
        KVector PlaneIntersection;

        if( Distance < 0.0f )
        {
                return false;
        }
        else if( Distance <= 1.0f )
        {
                PlaneIntersection = pData->m_Position - Normal * Distance;
        }
        else
        {
                SphereIntersection = pData->m_Position + Normal;
                
                KVector Velocity = pData->m_Velocity;
                Velocity.Normalize();
                double  t = IntersectRayPlaneDistance( Origin, Normal, SphereIntersection, Velocity );

                if( t < 0.0f )
                        return false;

                PlaneIntersection = SphereIntersection + Velocity * t;
        }

        KVector PolyIntersection = PlaneIntersection;

        // Teste si le point d'intersection est dans le triangle
        if( !CheckPointInTriangle( PlaneIntersection, v0, v1, v2 ) ) 
        { 
                // Calcule le point le plus proche
                PolyIntersection = ClosestPointOnTriangle( PlaneIntersection, v0, v1, v2 );
        }

        double  t = IntersectRaySphere( PolyIntersection, -pData->m_Velocity, pData->m_Position, 1.0f );

        if( t >= 0.0f && t <= DistanceToTravel )
        {
                KVector Velocity = pData->m_Velocity;
                Velocity.Normalize();
                KVector Intersection = PolyIntersection - Velocity * t;

                if( ( !pData->m_bFoundCollision ) || ( t < pData->m_NearestDistance ) )
                {
                        pData->m_NearestDistance                                        = t;
                        pData->m_NearestIntersectionPoint                       = Intersection;
                        pData->m_NearestPolygonIntersectionPoint        = PolyIntersection;
                        pData->m_bFoundCollision                                        = true;
                        return true;
                }
        }

        return false;
}

*/

//---------------------------------------------------------------------------------------------------------------------
bool CheckRayEllipse( KVector& vRayOrigin, KVector& vRayDestination, KVector& vEllipseOrigin, KVector& vEllipseRadius, float* pFraction )
{
        return CheckRaySphere( vRayOrigin / vEllipseRadius, vRayDestination / vEllipseRadius, vEllipseOrigin / vEllipseRadius, 1.0f, pFraction );
}

//---------------------------------------------------------------------------------------------------------------------
bool CheckRaySphere( KVector& vRayOrigin, KVector& vRayDestination, KVector& vSphereCenter, float SphereRadius, float* pFraction )
{
        KVector RayParam = vRayDestination - vRayOrigin;
        // a = i² + j² + k²
        // b = 2i(x1 - l) + 2j(y1 - m) + 2k(z1 - n)
        // c = l² + m² + n² + x1² + y1² + z1² + 2(-lx1 - my1 - nz1) - r²
        float   a = SQR( RayParam.x ) + SQR( RayParam.y ) + SQR( RayParam.z );
        float   b = 2.0f * RayParam.x * ( vRayOrigin.x - vSphereCenter.x ) + 2.0f * RayParam.y * ( vRayOrigin.y - vSphereCenter.y ) + 2.0f * RayParam.z * ( vRayOrigin.z - vSphereCenter.z );
        float   c = SQR( vSphereCenter.x ) + SQR( vSphereCenter.y ) + SQR( vSphereCenter.z )
                                + SQR( vRayOrigin.x ) + SQR( vRayOrigin.y ) + SQR( vRayOrigin.z )
                                + 2.0f * ( -vSphereCenter.x * vRayOrigin.x - vSphereCenter.y * vRayOrigin.y - vSphereCenter.z * vRayOrigin.z )
                                - SQR( SphereRadius );

        // Calcul le déterminant d = b²-4ac
        float   d = SQR( b ) - 4.0f * a * c;

        // Pas de solutions
        if( d < 0.0f )
        {
                *pFraction = 1.0f;
                return false;
        }

        // 1 solution -b/2a
        if( d == 0.0f )
        {
                *pFraction = -b / (2.0f * a);
                return true;
        }

        // 2 solutions -b-Va/2a -b+Va/2a
        float   sqrta = sqrtf( a );
        float   t0 = (-b - sqrta) / (2.0f * a);
        float   t1 = (-b + sqrta) / (2.0f * a);
        *pFraction = (t0 < t1) ? t0 : t1;
        
        return true;
}

---