Cocos2d-x教程(34)－三维物体OBB碰撞检测算法

    Vec3 _center;   // 中心点
    /*
      以下三个变量为正交单位向量，
      定义了当前OBB包围盒的x,y,z轴
      用于计算矢量投影
     */
    Vec3 _xAxis;    // 包围盒x轴方向单位矢量
    Vec3 _yAxis;    // 包围盒y轴方向单位矢量
    Vec3 _zAxis;    // 包围盒z轴方向单位矢量
    Vec3 _extents;  // 3个1/2边长,半长、半宽、半高

 AABB aabb = _sprite->getAABB();//获取一个Sprite3D对象的aabb包围盒
 OBB  _obbt = OBB(aabb);//创建obb包围盒

2. OBB包围盒的碰撞检测方法

#ifndef __CC_OBB_H__
#define __CC_OBB_H__

#include "CCAABB.h"
#include "3d/3dExport.h"

NS_CC_BEGIN

class CC_3D_DLL OBB
{
public:
    OBB();//默认构造函数

    /*
     * 构造函数 根据一个AABB包围盒初始化一个OBB包围盒
     */
    OBB(const AABB& aabb);
    
    /*
     * 构造函数 根据点信息初始化一个OBB包围盒
     */
    OBB(const Vec3* verts, int num);
    
    /*
     * 判断点是否在一个OBB包围盒内
     */
    bool containPoint(const Vec3& point) const;

    /*
     * 指定OBB各成员变量的值
     */
    void set(const Vec3& center, const Vec3& _xAxis, const Vec3& _yAxis, const Vec3& _zAxis, const Vec3& _extents);
    
    /*
     * 复位函数 将当前OBB对象所占用的内存块置零
     */ 
    void reset();

    /* 面向包围盒z轴负方向
     * verts[0] : 左上点坐标
     * verts[1] : 左下点坐标
     * verts[2] : 右下点坐标
     * verts[3] : 右上点坐标
     *
     * 面向包围盒z轴正方向
     * verts[4] : 右上点坐标
     * verts[5] : 右下点坐标
     * verts[6] : 左下点坐标
     * verts[7] : 左上点坐标
     */
    void getCorners(Vec3* verts) const;
    
    /*
     *检测是否和其他OBB盒碰撞
     */
    bool intersects(const OBB& box) const;
    
	/**
     * 由给定的变换矩阵变换OBB包围盒
     */
    void transform(const Mat4& mat);
    
protected:
    /*
     * 将点投影到目标轴
     */
    float projectPoint(const Vec3& point, const Vec3& axis) const;
    
    /*
     * 计算最大和最小投影值
     */
    void getInterval(const OBB& box, const Vec3& axis, float &min, float &max) const;
    
    /*
     * 取边的矢量 获取分离轴使用
     */
    Vec3 getEdgeDirection(int index) const;
    
    /*
     * 取面的方向矢量 获取分离轴时使用
     */
    Vec3 getFaceDirection(int index) const;

public:
    Vec3 _center;   // 中心点
    /*
      以下三个变量为正交单位向量，
      定义了当前OBB包围盒的x,y,z轴
      用于计算矢量投影
     */
    Vec3 _xAxis;    // 包围盒x轴方向单位矢量
    Vec3 _yAxis;    // 包围盒y轴方向单位矢量
    Vec3 _zAxis;    // 包围盒z轴方向单位矢量
    Vec3 _extents;  // 3个1/2边长,半长、半宽、半高
};

NS_CC_END

#endif

#include "3d/CCOBB.h"

NS_CC_BEGIN

#define ROTATE(a,i,j,k,l) g=a.m[i + 4 * j]; h=a.m[k + 4 * l]; a.m[i + 4 * j]=(float)(g-s*(h+g*tau)); a.m[k + 4 * l]=(float)(h+s*(g-h*tau));

//生成协方差矩阵
static Mat4 _getConvarianceMatrix(const Vec3* vertPos, int vertCount)
{
    int i;
    Mat4 Cov;

    double S1[3];
    double S2[3][3];

    S1[0] = S1[1] = S1[2] = 0.0;
    S2[0][0] = S2[1][0] = S2[2][0] = 0.0;
    S2[0][1] = S2[1][1] = S2[2][1] = 0.0;
    S2[0][2] = S2[1][2] = S2[2][2] = 0.0;

    // get center of mass
    for(i=0; i<vertCount; i++)
    {
        S1[0] += vertPos[i].x;
        S1[1] += vertPos[i].y;
        S1[2] += vertPos[i].z;

        S2[0][0] += vertPos[i].x * vertPos[i].x;
        S2[1][1] += vertPos[i].y * vertPos[i].y;
        S2[2][2] += vertPos[i].z * vertPos[i].z;
        S2[0][1] += vertPos[i].x * vertPos[i].y;
        S2[0][2] += vertPos[i].x * vertPos[i].z;
        S2[1][2] += vertPos[i].y * vertPos[i].z;
    }

    float n = (float)vertCount;
    // now get covariances
    Cov.m[0] = (float)(S2[0][0] - S1[0]*S1[0] / n) / n;
    Cov.m[5] = (float)(S2[1][1] - S1[1]*S1[1] / n) / n;
    Cov.m[10] = (float)(S2[2][2] - S1[2]*S1[2] / n) / n;
    Cov.m[4] = (float)(S2[0][1] - S1[0]*S1[1] / n) / n;
    Cov.m[9] = (float)(S2[1][2] - S1[1]*S1[2] / n) / n;
    Cov.m[8] = (float)(S2[0][2] - S1[0]*S1[2] / n) / n;
    Cov.m[1] = Cov.m[4];
    Cov.m[2] = Cov.m[8];
    Cov.m[6] = Cov.m[9];

    return Cov;
}
//获取一个矢量在某个轴的分量
static float& _getElement( Vec3& point, int index)
{
    if (index == 0)
        return point.x;
    if (index == 1)
        return point.y;
    if (index == 2)
        return point.z;

    CC_ASSERT(0);
    return point.x;
}
//获取特征向量
static void _getEigenVectors(Mat4* vout, Vec3* dout, Mat4 a)
{
    int n = 3;
    int j,iq,ip,i;
    double tresh, theta, tau, t, sm, s, h, g, c;
    int nrot;
    Vec3 b;
    Vec3 z;
    Mat4 v;
    Vec3 d;

    v = Mat4::IDENTITY;
    for(ip = 0; ip < n; ip++)
    {
        _getElement(b, ip) = a.m[ip + 4 * ip];
        _getElement(d, ip) = a.m[ip + 4 * ip];
        _getElement(z, ip) = 0.0;
    }

    nrot = 0;

    for(i = 0; i < 50; i++)
    {
        sm = 0.0;
        for(ip = 0; ip < n; ip++) for(iq = ip+1; iq < n; iq++) sm += fabs(a.m[ip + 4 * iq]);
        if( fabs(sm) < FLT_EPSILON )
        {
            v.transpose();
            *vout = v;
            *dout = d;
            return;
        }

        if (i < 3)
            tresh = 0.2 * sm / (n*n);
        else 
            tresh = 0.0;

        for(ip = 0; ip < n; ip++)
        {
            for(iq = ip + 1; iq < n; iq++)
            {
                g = 100.0 * fabs(a.m[ip + iq * 4]);
                float dmip = _getElement(d, ip);
                float dmiq = _getElement(d, iq);

                if( i>3 && fabs(dmip) + g == fabs(dmip) && fabs(dmiq) + g == fabs(dmiq) )
                {
                    a.m[ip + 4 * iq] = 0.0;
                }
                else if (fabs(a.m[ip + 4 * iq]) > tresh)
                {
                    h = dmiq - dmip;
                    if (fabs(h) + g == fabs(h))
                    {
                        t=(a.m[ip + 4 * iq])/h;
                    }
                    else
                    {
                        theta = 0.5 * h / (a.m[ip + 4 * iq]);
                        t=1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
                        if (theta < 0.0) t = -t;
                    }
                    c = 1.0 / sqrt(1+t*t);
                    s = t*c;
                    tau = s / (1.0+c);
                    h = t * a.m[ip + 4 * iq];
                    _getElement(z, ip) -= (float)h;
                    _getElement(z, iq) += (float)h;
                    _getElement(d, ip) -= (float)h;
                    _getElement(d, iq) += (float)h;
                    a.m[ip + 4 * iq]=0.0;
                    for(j = 0; j < ip; j++) { ROTATE(a,j,ip,j,iq); }
                    for(j = ip + 1; j < iq; j++) { ROTATE(a,ip,j,j,iq); }
                    for(j = iq + 1; j < n; j++) { ROTATE(a,ip,j,iq,j); }
                    for(j = 0; j < n; j++) { ROTATE(v,j,ip,j,iq); }
                    nrot++;
                }
            }
        }

        for(ip = 0; ip < n; ip++)
        {
            _getElement(b, ip) += _getElement(z, ip);
            _getElement(d, ip) = _getElement(b, ip);
            _getElement(z, ip) = 0.0f;
        }
    }

    v.transpose();
    *vout = v;
    *dout = d;
    return;
}
//创建OBB包围盒取向矩阵
static Mat4 _getOBBOrientation(const Vec3* vertPos, int num)
{
    Mat4 Cov; //创建一个 4*4 矩阵

    if (num <= 0)
        return Mat4::IDENTITY; //返回单位矩阵

    Cov = _getConvarianceMatrix(vertPos, num); //创建协方差矩阵

    // now get eigenvectors
    Mat4 Evecs;
    Vec3 Evals;
    _getEigenVectors(&Evecs, &Evals, Cov); //求特征向量

    Evecs.transpose(); //转置

    return Evecs;
}
//默认构造函数
OBB::OBB()
{
    //数据复位
    reset();
}
//由一个AABB包围盒生成OBB包围盒
OBB::OBB(const AABB& aabb)
{
    //数据复位
    reset();
    
    //中心点
    _center = (aabb._min + aabb._max);
    _center.scale(0.5f);
    
    //各轴旋转矩阵的单位矩阵
    _xAxis = Vec3(1.0f, 0.0f, 0.0f);
    _yAxis = Vec3(0.0f, 1.0f, 0.0f);
    _zAxis = Vec3(0.0f, 0.0f, 1.0f);
    
    //半尺存 半长 半宽 半高
    _extents = aabb._max - aabb._min;
    _extents.scale(0.5f);
}
//构造函数 根据点信息初始化一个OBB包围盒
OBB::OBB(const Vec3* verts, int num)
{
    if (!verts) return; //如果verts不存在 返回
    
    reset(); //数据复位
    
    Mat4 matTransform = _getOBBOrientation(verts, num); //创建包围盒取向矩阵
    
    /*	
        matTransform是一个正交矩阵,所以它的逆矩阵就是它的转置;
        AA‘=E（E为单位矩阵，A‘表示“矩阵A的转置矩阵”） A称为正交矩阵
    */
    matTransform.transpose(); //计算matTransform矩阵的转置(此处相当于求逆矩）
    
    Vec3 vecMax = matTransform * Vec3(verts[0].x, verts[0].y, verts[0].z);
    
    Vec3 vecMin = vecMax;
    
    for (int i = 1; i < num; i++)
    {
        Vec3 vect = matTransform * Vec3(verts[i].x, verts[i].y, verts[i].z);
        
        vecMax.x = vecMax.x > vect.x ? vecMax.x : vect.x;
        vecMax.y = vecMax.y > vect.y ? vecMax.y : vect.y;
        vecMax.z = vecMax.z > vect.z ? vecMax.z : vect.z;
        
        vecMin.x = vecMin.x < vect.x ? vecMin.x : vect.x;
        vecMin.y = vecMin.y < vect.y ? vecMin.y : vect.y;
        vecMin.z = vecMin.z < vect.z ? vecMin.z : vect.z;
    }
    
    matTransform.transpose();
    
    _xAxis = Vec3(matTransform.m[0], matTransform.m[1], matTransform.m[2]);
    _yAxis = Vec3(matTransform.m[4], matTransform.m[5], matTransform.m[6]);
    _zAxis = Vec3(matTransform.m[8], matTransform.m[9], matTransform.m[10]);
    
    _center	= 0.5f * (vecMax + vecMin);
    _center *= matTransform;
    
    _xAxis.normalize();
    _yAxis.normalize();
    _zAxis.normalize();
    
    _extents = 0.5f * (vecMax - vecMin);
}
//判断一点是否在OBB包围盒内
bool OBB::containPoint(const Vec3& point) const
{
    //相当于将点坐标从世界坐标系中转换到了OBB包围盒的物体坐标系中
    Vec3 vd = point - _center;
    
    /*
      dot方法为求点积
      由于_xAxis为单位矢量
      vd与_xAxis的点击即为在_xAxis方向的投影
    */
    float d = vd.dot(_xAxis); //计算x方向投影d
    
    //判断投影是否大于x正方向的半长或小于x负方向半长
    if (d > _extents.x || d < -_extents.x)
        return false;//满足条件说明不在包围盒内

    d = vd.dot(_yAxis); //计算y方向投影
    //同理
    if (d > _extents.y || d < -_extents.y)
        return false;

    d = vd.dot(_zAxis);//计算z方向投影
    if (d > _extents.z || d < -_extents.z)
        return false;

    return true;
}
//指定OBB包围盒的变量值
void OBB::set(const Vec3& center, const Vec3& xAxis, const Vec3& yAxis, const Vec3& zAxis, const Vec3& extents)
{
    _center = center;
    _xAxis = xAxis;
    _yAxis = yAxis;
    _zAxis = zAxis;
    _extents = extents;
}
//复位
void OBB::reset()
{
    memset(this, 0, sizeof(OBB)); //将OBB所在内存块置零
}
//获取顶点信息
void OBB::getCorners(Vec3* verts) const
{
    Vec3 extX = _xAxis * _extents.x;  //x方向分量
    Vec3 extY = _yAxis * _extents.y;  //y方向分量
    Vec3 extZ = _zAxis * _extents.z;  //z方向分量
    
    //z轴正方向的面
    verts[0] = _center - extX + extY + extZ;     // 左上顶点坐标
    verts[1] = _center - extX - extY + extZ;     // 左下顶点坐标
    verts[2] = _center + extX - extY + extZ;     // 右下顶点坐标
    verts[3] = _center + extX + extY + extZ;     // 右上顶点坐标
    
    //z轴负方向的面
    verts[4] = _center + extX + extY - extZ;     // 右上顶点坐标
    verts[5] = _center + extX - extY - extZ;     // 右下顶点坐标
    verts[6] = _center - extX - extY - extZ;     // 左下顶点坐标
    verts[7] = _center - extX + extY - extZ;     // 左上顶点坐标
}
//将点投影到坐标轴
float OBB::projectPoint(const Vec3& point, const Vec3& axis)const
{
    float dot = axis.dot(point); //点积
    float ret = dot * point.length();
    return ret;
}
//计算最大最小投影值
void OBB::getInterval(const OBB& box, const Vec3& axis, float &min, float &max)const
{
    Vec3 corners[8];
    box.getCorners(corners);//获取包围盒顶点信息
    float value;
    //分别投影八个点，取最大和最小值
    min = max = projectPoint(axis, corners[0]);
    for(int i = 1; i < 8; i++)
    {
        value = projectPoint(axis, corners[i]);
        min = MIN(min, value);
        max = MAX(max, value);
    }
}
//取边的矢量
Vec3 OBB::getEdgeDirection(int index)const
{
    Vec3 corners[8];
    getCorners(corners); //获取八个顶点信息
    
    Vec3 tmpLine;
    switch(index)
    {
        case 0:// x轴方向
            tmpLine = corners[5] - corners[6];
            tmpLine.normalize(); //归一化
            break;
        case 1:// y轴方向
            tmpLine = corners[7] - corners[6];
            tmpLine.normalize();
            break;
        case 2:// z轴方向
            tmpLine = corners[1] - corners[6];
            tmpLine.normalize();
            break;
        default:
            CCASSERT(0, "Invalid index!");
            break;
    }
    return tmpLine;
}
//取面的方向矢量
Vec3 OBB::getFaceDirection(int index) const
{
    Vec3 corners[8];
    getCorners(corners); //获取八个顶点信息
    
    Vec3 faceDirection, v0, v1;
    switch(index)
    {
        case 0://前/后 计算结果为一个与z轴平行的矢量
            v0 = corners[2] - corners[1]; //朝向+z的面 左下点->右下点的矢量
            v1 = corners[0] - corners[1]; //          左下点->左上点的矢量
            /*
               两个矢量的叉积得到的结果
               是垂直于原来两个相乘矢量的矢量
             */
            Vec3::cross(v0, v1, &faceDirection); //计算v0,v1的叉积 结果存储到faceDirection
            /*
                归一化
                此处相当于求x,y轴所在平面的法矢量
             */
            faceDirection.normalize();
            break;
        case 1:// 左/右 计算结果为一个与x轴平行的矢量
            v0 = corners[5] - corners[2];
            v1 = corners[3] - corners[2];
            Vec3::cross(v0, v1, &faceDirection);
            faceDirection.normalize();
            break;
        case 2:// 上/下 计算结果为一个与y轴平行的矢量
            v0 = corners[1] - corners[2];
            v1 = corners[5] - corners[2];
            Vec3::cross(v0, v1, &faceDirection);
            faceDirection.normalize();
            break;
        default:
            CCASSERT(0, "Invalid index!");
            break;
    }
    return faceDirection; //返回方向矢量
}
//检测两个OBB包围盒是否重合
bool OBB::intersects(const OBB& box) const
{
    float min1, max1, min2, max2;
    //当前包围盒的三个面方向 相当于取包围盒的三个坐标轴为分离轴并计算投影作比较
    for (int i = 0; i < 3; i++)
    {
        getInterval(*this, getFaceDirection(i), min1, max1);//计算当前包围盒在某轴上的最大最小投影值
        getInterval(box, getFaceDirection(i), min2, max2);//计算另一个包围盒在某轴上的最大最小投影值
        if (max1 < min2 || max2 < min1) return false; //判断分离轴上投影是否重合
    }
    //box包围盒的三个面方向
    for (int i = 0; i < 3; i++)
    {
        getInterval(*this, box.getFaceDirection(i), min1, max1);
        getInterval(box, box.getFaceDirection(i), min2, max2);
        if (max1 < min2 || max2 < min1) return false;
    }
    
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            Vec3 axis;
            //Vec3::cross(getFaceDirection(i), box.getFaceDirection(j), &axis); //2d-x源代码
            Vec3::cross(getEdgeDirection(i), box.getEdgeDirection(j), &axis); //修改，这里应该边的矢量并做叉积
            getInterval(*this, axis, min1, max1);
            getInterval(box, axis, min2, max2);
            if (max1 < min2 || max2 < min1) return false;
        }
    }
    
    return true;
}

//由一个给定矩阵对OBB包围盒进行变换
void OBB::transform(const Mat4& mat)
{
    // 新的中心点
    Vec4 newcenter = mat * Vec4(_center.x, _center.y, _center.z, 1.0f);
    _center.x = newcenter.x;
    _center.y = newcenter.y;
    _center.z = newcenter.z;

    //变换向量
    _xAxis = mat * _xAxis;
    _yAxis = mat * _yAxis;
    _zAxis = mat * _zAxis;

    _xAxis.normalize();  //归一化
    _yAxis.normalize();
    _zAxis.normalize();

    Vec3 scale, trans; 
    Quaternion quat;  //四元数 单位长度的四元数可以表示三维旋转
    mat.decompose(&scale, &quat, &trans);

    //半长 半宽 半高 
    _extents.x *= scale.x;
    _extents.y *= scale.y;
    _extents.z *= scale.z;
}

NS_CC_END