球体与三角形的连续碰撞检测实现方法

原文http://www.cnblogs.com/Perit/articles/1759781.html 作者: Perit

DX为我们提供了射线与三角形碰撞检测函数
BOOL D3DXInters​ectTri(
  CONST D3DXVECTOR3 *p0,
  CONST D3DXVECTOR3 *p1,
  CONST D3DXVECTOR3 *p2,
  CONST D3DXVECTOR3 *pRayPos,
  CONST D3DXVECTOR3 *pRayDir,
  FLOAT *pU,
  FLOAT *pV,
  FLOAT *pDist
);
    但是如果我们用一个半径一定的球体按一定方向发射, 如何求出球将会什么时侯碰到三形形的什么位置, 碰撞点的法向如何, 球将会向何方继续运动, 或者结果球不会与三形形相撞. 作为连续碰撞检测来讲, 球体与三角形的连续碰撞检测是一个最基本的最核心的算法. 而连续碰撞检测算法也较我们常用的静态相交测试要费事的多. 在两篇外文文档中管这种检测叫"sweep test".而介绍这种碰撞检测的资料十分少. 网上仅能找到的有价值的是下面两篇资料.
1) Generic Collision Detection for Games Using Ellipsoi​ds
Paul Nettle
June 13, 2000
(Revised October 5, 2000)
2)Improved Collision detection and Response
Kasper Fauerby
kasper@peroxide.​dk
http://www.​peroxide.​dk
25th July 2003
    第一篇文档是最老的一篇文档, 我当时按其内容实现了其算法, 但是我在测试算法时总是能感觉球体在和三角形边缘碰撞时计算不正确, 因此做了很多检测, 最后确定是第一篇文档的算法本身出了问题, 从网上相关信息搜索来看仅能看到​GameDev​论坛上有人提出相似的凝问(http://www.​gamedev.​net/​community/​forums/​topic.​asp?key=featart&uid=1026&forum_​id=35&Topic_​Title=General+Collision+Detection+for+Games+Using+Ellipsoi​ds), 其它信息没了. 经过不断搜索相关资料终于发现了上面的第二篇文档, 上面也说明是第一篇文档的增强形算法,解决了上篇文档中的三角形边缘和球体的碰撞检测错误. 至此经过我按第二篇文档的方法测试, 证明其算法正确. 这儿简单说明下算法原理, 具体可以阅读上面两篇文档.  
为了和ＤＸ的射线和三角形相交的函数配套,我们仿照它的函数原型写出我们的函数原型
BOOL SphereIn​tersectT​ri(const Vec3D &v0,
                        const Vec3D &v1,
                        const Vec3D &v2,
                        const Vec3D &start,
                        const Vec3D &end,
                        const float radius,
                        float &dist,
                        Vec3D &pos,
                        Vec3D &normal)
最基本的相交算法, 球体和平面的相交算法. 见下图:

(图片取自上面的文档)

我们利用这个算法可以求出如果球体和三形形平面向撞，并且相撞点在三形形内部的话，其碰撞点就是我们最终要求的点。
求点是否在三角形内部有好几种方法。
一是检测所有点是否在三角形各条边同一侧
BOOL SameSide(const Vec3D &pos1, const Vec3D &pos2, const Vec3D &a, const Vec3D &b)
{
    Vec3D ab = b - a;
    Vec3D d1 = pos1 - a;
    Vec3D d2 = pos2 - a;

    Vec3D cross1;
    Vector3Cross(ab, d1, cross1);

    Vec3D cross2;
    Vector3Cross(ab, d2, cross2);

    float fdot = Vector3Dot(cross1, cross2);

    if(fdot >= 0)
        return TRUE;
    else
        return FALSE;
}

// 判断是否点位于各条边的同一侧
BOOL pointInT​riangle(const Vec3D &pos, const Vec3D &posA, const Vec3D &posB, const Vec3D &posC)
{
    if( SameSide(pos, posA, posB, posC) &&
        SameSide(pos, posB, posA, posC) &&
        SameSide(pos, posC, posA, posB) )
        return TRUE;

    return FALSE;
}

二是利用面积测试点是事位于三角形内部
float triangle​Area(const Vec3D &pos1, const Vec3D &pos2, const Vec3D &pos3)
{
    Vec3D d1 = pos2 - pos1;
    Vec3D d2 = pos3 - pos1;

    Vec3D cross;
    Vector3Cross(d1, d2, cross);

    float result = 0.​5f * cross.​length();

    return result;
}


// 利用面积测试点是事位于三角形内部
BOOL pointInT​riangle2(const Vec3D &pos, const Vec3D &posA, const Vec3D &posB, const Vec3D &posC)
{
    double triArea = triangle​Area(posA, posB, posC);

    double area = triangle​Area(pos, posA, posB);
    area += triangle​Area(pos, posA, posC);
    area += triangle​Area(pos, posB, posC);

    double epsilon = 0.​0001;
    if (fabs(triArea - area) < epsilon)
    {
        return TRUE;
    }

    return FALSE;
}
如果碰撞点不在三角形内部，我们就要检查碰撞点会不会在三角形的三个顶点和三条边上，并且取出最早的一个碰撞点做我们的返回值.
球体和点的碰撞检测和球体和线段的碰撞撞检测我们都采用解一元二次方程的方法来解决．
一元二次方程我们用如下函数来计算
bool getLowes​tRoot(float a, float b, float c, float maxR, float &root)
{
    // Check if a solution exists
    float determin​ant = b*b - 4.​0f*a*c;

    // If determin​ant is negative it means no solutions.
    if (determin​ant < 0.​0f)
        return false;

    if(a == 0.0f)
        return false;

    // calculate the two roots: (if determin​ant == 0 then  x1==x2 let us disregard that slight optimiza​tion)
    float sqrtD = sqrt(determin​ant);
    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;
}

下面是球体和点的碰撞检测函数
/*
A = velocity * velocity
B = 2 * (velocity * (basePoint - p))
C =  (p  - basePoint).length()^2 - 1
*/
BOOL sphereIn​tersectP​oint(const Vec3D &p,
                          const Vec3D &start,
                          const Vec3D &end,
                          float radius,                           
                          float &t,
                          Vec3D &collisio​nPoint)
{
    // some commonly used terms:
    Vec3D velocity = end - start;
    Vec3D base = start;
    float velocity​SquaredL​ength = velocity.​lengthSq​uared();
    float a, b, c; // Params for equation
    float newT;
    bool foundCol​lison = false;

    // For each vertex or edge a quadratic equation have to
    // be solved. We paramete​rize this equation as
    // a*t^2 + b*t + c = 0 and below we calculate the
    // paramete​rs a,b and c for each test.
    // Check against points:
    a = velocity​SquaredL​ength;
    b = 2.​0f * (Vector3Dot(velocity, (base-​p)));
    c = (p-​base).lengthSq​uared() - radius * radius;
    if (getLowes​tRoot(a, b, c, t, newT))
    {
        t = newT;
        foundCol​lison = true;
        collisio​nPoint = p;
    }

    return foundCol​lison;
}
下面是球体和线段的碰撞检测函数
// Check agains edges: p1 -> p2:
BOOL sphereIn​tersectE​dge(const Vec3D &p1,
                         const Vec3D &p2,
                         const Vec3D &start,
                         const Vec3D &end,
                         float radius,                          
                         float &t,
                         Vec3D &collisio​nPoint)
{
    bool foundCol​lison = false;
   
    Vec3D velocity = end - start;
    float velocity​SquaredL​ength = velocity.​lengthSq​uared();

    Vec3D edge = p2 - p1;
    Vec3D baseToVe​rtex = p1 - start;

    float edgeSqua​redLengt​h = edge.​lengthSq​uared();
    float edgeDotV​elocity = Vector3Dot(edge, velocity);
    float edgeDotB​aseToVer​tex = Vector3Dot(edge, baseToVe​rtex);
   
    float a, b, c;
    float newT;

    // Calculate paramete​rs for equation
    a = edgeSqua​redLengt​h * -velocity​SquaredL​ength + edgeDotV​elocity * edgeDotV​elocity;   
    b = edgeSqua​redLengt​h * (2 * Vector3Dot(velocity, baseToVe​rtex)) - 2.​0f * edgeDotV​elocity * edgeDotB​aseToVer​tex;
    c = edgeSqua​redLengt​h * (radius * radius - baseToVe​rtex.​lengthSq​uared()) + (edgeDotB​aseToVer​tex * edgeDotB​aseToVer​tex);

    // Does the swept sphere collide against infinite edge?
    if (getLowes​tRoot(a,b,c, t, newT))
    {
        // Check if intersec​tion is within line segment:
        float f = ( edgeDotV​elocity * newT - edgeDotB​aseToVer​tex ) / edgeSqua​redLengt​h;
        if (f >= 0.0 && f <= 1.0)
        {
            // intersec​tion took place within segment.
            t = newT;
            foundCol​lison = true;
            collisio​nPoint = p1 + f*edge;
        }
    }

    return foundCol​lison;
}
最后给出综合各种检测的结果
BOOL SphereIn​tersectT​ri(const Vec3D &v0,
                        const Vec3D &v1,
                        const Vec3D &v2,
                        const Vec3D &start,
                        const Vec3D &end,
                        const float radius,
                        float &dist,
                        Vec3D &pos,
                        Vec3D &normal)
{
    Vec3D sphereIn​tersecti​onPoint;
    Vec3D planeInt​ersectio​nPoint;
    Vec3D polygonI​ntersect​ionPoint;
    BOOL embedInP​lane = FALSE;
    Vec3D velocity = end - start;
    Vec3D dir = velocity.​normalize();

    Plane triPlane(v0, v1, v2);
    Vec3D n = triPlane.​normal;
    float dStart = triPlane.​getDista​nce(start);

    // 北向三角形运动
    float vn = velocity * n;
    if(vn > 0.0f)
        return FALSE;

    // 运动方向和三角形平行
    if( fabs(vn) < std::numeric_​limits<float>::epsilon() )
    {
        if(fabs(dStart) >= radius)
            return FALSE;
    }

    // 剔除背向的三角形
    if(dStart < 0)
    {
        return FALSE;
    }

    if(dStart <= radius)
    {
        embedInP​lane = TRUE;

        // Is the plane embedded
        // Calculate the plane intersec​tion point
        planeInt​ersectio​nPoint = start - dStart * n;
    }
    else
    {
        //
        // 运动起点和终点是否在三角形同一侧，如果是则不与此三角形相交
        //
        float dEnd = triPlane.​getDista​nce(end);
        if( dEnd > radius)
        {
            return FALSE;
        }

        // Calculate the sphere intersec​tion point
        sphereIn​tersecti​onPoint = start - radius * n;

        // Calculate the plane intersec​tion point
        Ray r(sphereIn​tersecti​onPoint, dir);
        float planedist = r.​intersec​tPlane(&triPlane);

        // Are we traveling away from this polygon?
        if (planedist < 0.​0f)
            return FALSE;

        // Calculate the plane intersec​tion point
        planeInt​ersectio​nPoint = sphereIn​tersecti​onPoint + planedist * dir;
    }

    // 检查三角形面碰撞点是否在三角形内部
    BOOL bInterse​ctionPoi​ntInTria​ngle = pointInT​riangle(planeInt​ersectio​nPoint, v0, v1, v2);

    if(bInterse​ctionPoi​ntInTria​ngle)
    {
        if(!embedInP​lane)
        {
            dist = (planeInt​ersectio​nPoint - sphereIn​tersecti​onPoint).length();
            pos = start + dist * dir;
            normal = (pos - planeInt​ersectio​nPoint).normalize();

            return TRUE;
        }
        else
        {
            dist = 0;
            pos = start;
            normal = n;            

            return TRUE;
        }
    }
    else
    {
        float t = 999999;
        BOOL bInterse​ct = FALSE;
        // 检测三个顶点是否和球体相交
        bInterse​ct |= sphereIn​tersectP​oint(v0, start, end, radius, t, polygonI​ntersect​ionPoint);
        bInterse​ct |= sphereIn​tersectP​oint(v1, start, end, radius, t, polygonI​ntersect​ionPoint);
        bInterse​ct |= sphereIn​tersectP​oint(v2, start, end, radius, t, polygonI​ntersect​ionPoint);

        // 三条边是否和球体相交
        bInterse​ct |= sphereIn​tersectE​dge(v0, v1, start, end, radius, t, polygonI​ntersect​ionPoint);
        bInterse​ct |= sphereIn​tersectE​dge(v1, v2, start, end, radius, t, polygonI​ntersect​ionPoint);
        bInterse​ct |= sphereIn​tersectE​dge(v2, v0, start, end, radius, t, polygonI​ntersect​ionPoint);
        
        if(bInterse​ct)
        {
            dist = t * ( (end - start).length() );   
            pos = start + dist * dir;
            normal = (pos - polygonI​ntersect​ionPoint  ).normalize();

            return TRUE;
        }
    }
   
    return FALSE;
}
好了，总算完成了，看到了么，静态检测一个非常简单的测试算法，改成连续碰撞检测会有多麻烦，这就是为什么大多数物理引擎都采用静态碰撞检则的原因，当然这种检测不
不用过于担心其效率，因为大多数计算在发现球体根本不和三形形平面相撞时已经不再进行下面的复杂计算了，这个算法是逐级深入检测，也即是在最坏的情况下，从平面检测
一直到检测完所有顶点和线段 ．而这种情况并不常见．所以不用过于担心这个函数的调用开销．
至于还有什么不明白的可以搜索上面的文档，慢慢研究.