本发明提供了基于K_DOPs快速连续碰撞检测方法，包括步骤：（1）给定初始时间点和终止时间点，用静态K_DOPs碰撞检测技术检测两个对象在初始点和终止点是否发生碰撞，如果有发生碰撞，则退出。否则，转步骤2；（2）根据两个对象的运动路径进行包围盒动态相交检测；（3）若两个子区间的K_DOPs包围盒不相交，退出，否则转步骤4；（4）对碰撞的包围盒进行基本元素的碰撞检测；（5）若基本元素发生碰撞，则报告碰撞并退出，否则报告没有发生碰撞并退出。本发明可以实时地检测两个刚体在连续运动过程中的碰撞情况，可以避免出现离散检测中的漏检和刺穿现象，使得检测结果更可靠，并且精度高和性能好。
1.基于K_DOPs快速连续碰撞检测方法，其特征在于包括以下步骤：
S1、给定初始时间点和终止时间点，用静态K_DOPs算法检测两个对象在初始时间点和终止时间点是否发生碰撞，如果有发生碰撞，则退出，否则，转步骤S2；
S2、根据两个对象的运动路径进行包围盒动态相交检测，具体包括以下步骤：
S21、设碰撞对中碰撞对象一的K_DOPs包围盒Q1为：B1＝{b11,b12,…b1j,…b1K}，碰撞对中碰撞对象二的K_DOPs包围盒Q2为：B2＝{b21,b22,…b2j,…,b2K}，其中b1j表示碰撞对象一的包围盒的第j个方向投影点到原点的距离，b2j表示碰撞对象二的包围盒的第j个方向投影点到原点的距离，K为包围盒轴数；K_Dops包围盒的相交检测实际上是对K条轴方i i i向上的投影进行相交检测；设第i个轴的单位向量为Pi(px,py,pz)，B1与轴i的交点为F1i i i i i i＝b1,i·(px,py,pz)，B2与轴i的交点为F2＝b2,i·(px,py,pz)，i为1～K，则对于任意时刻t，Q1的位姿变换矩阵为M1(t)，Q2的位姿变换矩阵为M2(t)，物体运动后的包围盒是通过对物体包围盒运动后在K个轴方向上重新投影得到，那么根据姿态变化矩阵可以得到交点运动后的坐标：i i i
S22、记原点为O，第i个轴上的某一点为(px,py,pz)，F1'在第i个轴上的投影点记为F1″，则可以得到物体包围盒运动后在第i个轴方向上的投影点，从而得到碰撞对象一运动后的包围盒在第i个轴上的投影点到原点的距离，同理可以得到碰撞对象二运动后的包围盒在第i个轴上的投影点到原点的距离；
S23、由于K_Dops包围盒轴的每一个轴都对应有一条与该每一个轴方向相反的轴，这里假设与第i个轴方向相反的轴为j，则根据上面的原理同样可以得到碰撞对象一与碰撞对象二运动后的包围盒在第j个轴上的投影点到原点的距离；
S24、对方向互反的两根轴上的投影线段进行相交测试，那么对于一对方向相反的轴i和j，两根线段只需要满足下面的公式的任意一个则可以判断两根线段是非相交的：S3、若两个对象的K_DOPs包围盒不相交，退出，否则转步骤S4；
S4、对发生碰撞的包围盒中的基本元素进行碰撞检测；
S5、若基本元素发生碰撞，则报告碰撞并退出，否则报告没有发生碰撞并退出。
2.根据权利要求1所述的基于K_DOPs快速连续碰撞检测方法，其特征在于步骤S1中首先构建两个对象的静态K_DOPs包围盒，然后在离散时间点上进行包围盒的相交测试从而判断两个物体是否有碰撞可能。
3.根据权利要求1所述的基于K_DOPs快速连续碰撞检测方法，其特征在于所述步骤S4包括：对于边与边的相交检测：假设a(t)b(t)代表一条边，c(t)d(t)代表另外一条边，如果下面的方程有根，则这两条边出现相交：a(t)c(t)·(a(t)b(t)×c(t)d(t))＝0 (3)，用方程求根法去求解上面的方程就知道方程是否有根，相交点在边上当且仅当根在(0,1)区间；对于边与面的相交检测或面与边的相交检测：首先检测边与面的交点a(t)，假设a(t)代表矢量，b(t)c(t)d(t)代表三角片，如果下面的方程有根，则边与面出现相交，基本元素发生碰撞，a(t)b(t)·(b(t)c(t)×b(t)d(t))＝0 (4)，用求根法去求解上面的方程就知道方程是否有根，边在面里当且仅当根在(0,1)区间。
基于K_DOPs快速连续碰撞检测方法
技术领域
[0001] 本发明属于机器人运动领域，特别涉及一种在机器人运动时实时进行碰撞检测的方法。
背景技术
[0002] 碰撞检测算法在机器人运动领域有很长的研究历史，实时碰撞检测是机器人领域中一个非常关键的问题。目前已经有比较多可行的碰撞检测算法，但随着诸如虚拟现实等新兴领域的涌现及随之而来的人们对交互实时性，场景真实性要求的不断提高，碰撞检测技术所面临的问题也日益突出，其中最核心的问题是如何有效地提高碰撞检测的速度。
[0003] 目前常用的方法主要分为两类：离散法和连续法。离散法是通过定时计算每个时间点状态来确定碰撞情况，但这种方法会出现刺穿和漏检现象。为了克服离散法的缺点，连续法通过计算对象在整个运动路径上的状态来确定碰撞情况，从而避免了出现刺穿和漏检现象，当然连续法相对于离散法慢。
[0004] K-DOPs全称是K-Discrete Orientation Polytopes，K-DOPs算法属于一种凸多面体碰撞检测算法。其中，静态K-DOPs算法的实现过程是首先建立静态K-DOPs包围盒，然后在离散时间点上进行包围盒的相交测试从而判断两个物体是否有碰撞可能(James T.Klosowski，Martin Held，JosephS.B.Mitchell，Henry Sowizral，Karel Zikan.Efficient Collision Detection Using Bounding VolumeHierarchies of k-DOPs[J]和 Stephane Redon，Abderrahmane Kheddary，Sabine Coquillart.FastContinuous Collision Detection between Rigid Bodies[J])。本发明就是基于静态K-DOPs算法实现的，在本发明中遍历检测的所有包围盒都是利用已经计算好的静态K-DOPs包围盒所计算出来的路径包围盒，而不是静态包围盒。本发明只是借用了静态包围盒检测技术中的遍历算法和分割算法。其中，路径包围盒是一个包含对象在该路径运动过程中所经过的空间的最小K-DOPs包围盒。
发明内容
[0005] 本发明的目的是克服现有技术存在的上述不足，提供一种可以有效地避免离散检测算法中的刺穿和漏检现象并且精度高和性能好的基于K_DOPs快速连续碰撞检测方法。本发明基于间隔插值技术和静态K_DOPs碰撞检测技术，先对两个对象的运动路径进行间隔插值拟合，然后利用静态K_DOPs检测算法计算插值后的碰撞子区间，对碰撞子区间进行不断的插值拟合再检测，直到碰撞区间小于一个给定值，然后对这个最小区间进行三角片对的连续检测，具体技术方案如下。
[0006] 基于K_DOPs快速连续碰撞检测方法，包括如下步骤：
[0007] S1、给定初始时间点和终止时间点，用静态K_DOPs算法检测两个对象在初始点和终止点是否发生碰撞，如果有发生碰撞，则退出。否则，转步骤2。
[0008] S2、根据两个对象的运动路径进行包围盒动态相交检测。
[0009] S3、若两个对象的K_DOPs包围盒不相交，退出，否则转步骤4。
[0010] S4、对发生碰撞的包围盒中的基本元素进行碰撞检测。
[0011] S5、若基本元素发生碰撞，则报告碰撞并退出，否则报告没有发生碰撞并退出。
[0012] 上述步骤S1中首先构建两个对象的静态K_DOPs包围盒，然后在离散时间点上进行包围盒的相交测试从而判断两个物体是否有碰撞可能。
[0013] 所述步骤S2包括以下步骤：
[0014] S21、设碰撞对中碰撞对象一的K_DOPs包围盒Q1为：B1＝{b11，b12，...b1j，...b1K}，碰撞对中碰撞对象二的K_DOPs包围盒Q2为：B2＝{b21，b22，...b2j，...，b2K}，其中b1j表示碰撞对象一的包围盒的第j个方向投影点到原点的距离，b2j表示碰撞对象二的包围盒的第j个方向投影点到原点的距离，K为包围盒轴数；K_Dops包围盒的相交检测实际上是对K条轴方向上的投影进行相交检测。设第i个轴的单位向量为Pi(pxi，pyi，pzi)，B1与轴i的交点i i i为F1＝b1，i·(px，py，pz)，B2与轴i的交点为F2＝b2，i·(pxi，pyi，pzi)，i为1～K，则对于任意时刻t，Q1的位姿变换矩阵为M1(t)，Q2的位姿变换矩阵为M2(t)，物体运动后的包围盒是通过对物体包围盒运动后在K个轴方向上重新投影近似得到。那么根据姿态变化矩阵可以得到交点运动后的坐标。
[0015]
[0016] S22、记原点为○，第i个轴上的某一点为(pxi，pyi，pzi)，F1′在轴pi上的投影点记为F1”，则可以得到物体包围盒运动后在第i个轴方向上的投影点，从而得到碰撞对象一运动后的包围盒在第i个轴上的投影点到原点的距离。同理可以得到碰撞对象二运动后的包围盒在第i个轴上的投影点到原点的距离。
[0017] S23、由于K_Dops包围盒轴的每一个轴都对应有一条与该轴方向相反的轴，这里假设与第i个轴方向相反的轴为j，则根据上面的原理同样可以得到碰撞对象一与碰撞对象二运动后的包围盒在第j个轴上的投影点到原点的距离。
[0018] S24、对方向互反的两根轴上的投影线段进行相交测试。那么对于一对方向相反的轴i和j，两根线段只需要满足下面的公式的任意一个则可以判断两根线段是非相交的：
[0019]
[0020] 所述步骤S4包括：对于边与边的相交检测：假设a(t)b(t)代表一条边，c(t)d(t)代表另外一条边，如果下面的方程有根，则这两条边出现相交：
[0021] a(t)c(t)·(a(t)b(t)×c(t)d(t))＝0 (3)
[0022] 用方程求根法去求解上面的方程就知道方程是否有根。相交点在边上当且仅当根在(0，1)区间。于边与面的相交检测或面与边的相交检测：首先检测边与面的交点a(t)，假设a(t)代表矢量，b(t)c(t)d(t)代表三角片，如果下面的方程有根，则边与面出现相交，基本元素发生碰撞。
[0023] a(t)b(t)·(b(t)c(t)×b(t)d(t))＝0 (4)
[0024] 用求根法去求解上面的方程就知道方程是否有根。边在面里当且仅当根在(0，1)区间。
[0025] 本发明相对于现有技术具有如下的优点及效果：
[0026] 基于K_DOPs的快速连续碰撞检测算法可以实时地检测两个刚体在连续运动过程中的碰撞情况，通过间隔插值技术得到逼近刚体运动轨迹的折线，进而对刚体在折线运动过程中进行碰撞检测，最后利用基本元素的动态检测技术对三角片在最小折线运动过程中进行动态检测。实验证明该算法可以避免出现离散检测中的漏检和刺穿现象，使得检测结果更可靠，并且精度高和性能好，而且由于K_DOPs检测跟刚体的三角片数量和拓扑结构无关，所以使得本算法同样具有这个良好的性质。
附图说明
[0027] 图1是实施方式中进行相交测试时的两个对象相交的轴投影图。
[0028] 图2是实施方式中进行相交测试时的两个对象不相交的轴投影图。
[0029] 图3是实施方式中基于K_DOPs快速连续碰撞检测方法的流程图。
具体实施方式
[0030] 下面结合实施例对本发明作进一步详细的描述，但本发明的实施方式不限于此。
[0031] 实施例：
[0032] 本发明基于K_DOPs快速连续碰撞检测方法包括如下步骤：
[0033] S1、给定初始时间点和终止时间点，用静态K_DOPs碰撞检测技术检测两个对象在初始点和终止点是否发生碰撞，如果有发生碰撞，则退出。否则，转步骤2。
[0034] S2、根据两个对象的运动路径进行包围盒动态相交检测。
[0035] S3、若两个对象的K_DOPs包围盒不相交，退出，否则转步骤4。
[0036] S4、对碰撞的包围盒进行基本元素的碰撞检测。
[0037] S5、若基本元素发生碰撞，则报告碰撞并退出，否则报告没有发生碰撞并退出。
[0038] 所述步骤S1包括以下步骤：
[0039] S11、静态K_DOPs算法首先构建两个对象的静态K_DOPs包围盒，然后在离散时间点上进行包围盒的相交测试从而判断两个对象是否有碰撞可能。
[0040] 所述步骤S2包括以下步骤：
[0041] S21、设碰撞对中碰撞对象一的包围盒Q1为：B1＝{b11，b12，...，b1K}，碰撞对中碰撞对象二的包围盒Q2为：B2＝{b21，b22，…，b2K}，其中bij表示碰撞对象i的包围盒的第j个方向投影点到原点的距离，K为包围盒轴数。K_Dops包围盒的相交检测实际上是对K条i i i i轴方向上的投影相交检测。设第i个轴的单位向量为P(px，py，pz)，B1与轴i的交点为i i i i i i
F1＝b1，i·(px，py，pz)，B2与轴i的交点为F2＝b2，i·(px，py，pz)，则对于任意时刻t，Q1的位姿变换矩阵为M1(t)，Q2的位姿变换矩阵为M2(t)，物体运动后的包围盒是通过对物体包围盒运动后在K个轴方向上重新投影近似得到。那么根据姿态变化矩阵可以得到交点运动后的坐标。
[0042]
[0043] S22、第i个轴上的某一点为(pxi，pyi，pzi)，F1′在轴pi上的投影点记为F1”，则可以得到物体包围盒运动后在第i个轴方向上的投影点为：
[0044] F”1＝d·Pi (2)
[0045] 其中d为投影点到原点的距离为
[0046]
[0047] 由于Pi为单位向量，则简化公式有：
[0048]
[0049] 从而得到碰撞对象一运动后的包围盒在第i个轴上的投影点到原点的距离：
[0050] b1，i(t)＝(pxi，pyi，pzi)·M1(t)·(b1，i·(pxi，pyi，pzi)T) (5)[0051] 同理可以得到碰撞对象二运动后的包围盒在第i个轴上的投影点到原点的距离：
[0052] b2，i(t)＝(pxi，pyi，pzi)·M2(t)·(b2，i·(pxi，pyi，pzi)T) (6)[0053] S23、由于K_Dops包围盒轴的每一个轴都对应有一条与该轴方向相反的轴，这里假设与第i个轴方向相反的轴为j，则根据上面的原理同样可以得到碰撞对象一与碰撞对象二运动后的包围盒在第j个轴上的投影点到原点的距离：
[0054]
[0055]
[0056] S24、对方向互反的两根轴上的投影线段进行相交检测。
[0057] 那么对于一对方向相反的轴i和j，两根线段只需要满足下面的公式的任意一个则可以判断两根线段是非相交的：
[0058]
[0059] 展开b1，i(t)＜b2，j(t)得到：
[0060] (pxi，pyi，pzi)·M1(t)·(b1，i·(pxi，pyi，pzi)T)＜(pxj，pyj，pzj)·M2(t)·(b2，j·(pxj，j j Tpy，pz)) (9)
[0061] 即
[0062] (pxi，pyi，pzi)·M1(t)·(b1，i·(pxi，pyi，pzi)T)-(pxj，pyj，pzj)·M2(t)·(b2，j·(pxj，j j Tpy，pz))＜0 (10)
[0063] 由于(pxj，pyj，pzj)＝-(pxi，pyi，pzi)，则简化上述公式有：
[0064] pi·(M1(t)·b1，i-M2(t)·b2，j)·(pi)T＜0 (11)
[0065] 所述步骤S4包括以下步骤：
[0066] S41、对于边与边的相交检测：假设a(t)b(t)代表一条边，c(t)d(t)代表另外一条边，如果下面的方程有根，则这两条边出现相交：
[0067] a(t)c(t)·(a(t)b(t)×c(t)d(t))＝0 (12)
[0068] 用方程求根法去求解上面的方程就知道方程是否有根。相交点在边上当且仅当根在(0，1)区间。
[0069] 对于边与面的相交检测或面与边的相交检测：首先检测边与面的交点a(t)，假设a(t)代表矢量，b(t)c(t)d(t)代表三角片，如果下面的方程有根，则边与面出现相交，两个对象发生碰撞。
[0070] a(t)b(t)·(b(t)c(t)×b(t)d(t))＝0 (13)
[0071] 用求根法去求解上面的方程就知道方程是否有根。边在面里当且仅当根在(0，1)区间。
[0072] 上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
