CN102609321B - Rapid and continuous collision detection method based on K_DOPs (k-discrete orientation polytopes) - Google Patents

Abstract

The invention provides a rapid and continuous collision detection method based on K_DOPs (k-discrete orientation polytopes), which includes the steps: (1) presetting an initial time point and a terminal time point, detecting whether two objects are collided or not at the initial time point and the terminal time point by means of static K_DOPs collision detection technology, exiting if the objects are collided and going to a second step if not; (2) detecting dynamic intersection of bounding boxes according to motion paths of the two objects; (3) exiting if the K_DOPs bounding boxes of two subintervals are not intersected and going to a fourth step if the K_DOPs bounding boxes are intersected; (4) detecting collision of basic elements of the colliding bounding boxes; and (5) reporting collision and exiting if the basic elements are collided, and reporting no collision and exiting if not. By the aid of the rapid and continuous collision detection method, collision of two rigid bodies during continuous movement can be detected in real time, omission and piercing in discrete detection can be avoided, detection results can be more reliable, and precision and performance are high.

Description

Based on the quick method for detecting continuous collision of K_DOPs

Technical field

The invention belongs to robot motion field, particularly a kind of method of carrying out in real time collision detection in the time of robot motion.

Background technology

Collision detection algorithm has very long research history in robot motion field, and real time collision detection is a very crucial problem in robot field.There are at present many feasible collision detection algorithm, but along with the emerging field such as such as virtual reality emerge in large numbers and thing followed people to mutual real-time, what scene authenticity required improves constantly, the problem that Collision Detection faces also becomes increasingly conspicuous, and wherein most crucial problem is how effectively to improve the speed of collision detection.

Conventional method is mainly divided into two classes at present: discrete method and continuity method.Discrete method is to calculate each time point state by timing to determine collision situation, but this method there will be and pierces through and undetected phenomenon.In order to overcome the shortcoming of discrete method, continuity method by calculating object the state on whole motion path determine collision situation, thereby avoided occurring piercing through and undetected phenomenon, certainly continuity method is slow with respect to discrete method.

K-DOPs full name is K-Discrete Orientation Polytopes, and K-DOPs algorithm belongs to a kind of convex polyhedron collision detection algorithm.Wherein, the implementation procedure of static K-DOPs algorithm is the static K-DOPs bounding box of model, thereby judging two objects, the crossing test of then carrying out bounding box on discrete time point whether has collision possibility (James T.Klosowski, Martin Held, JosephS.B.Mitchell, Henry Sowizral, Karel Zikan.Efficient Collision Detection Using Bounding VolumeHierarchies of k-DOPs[J] and Stephane Redon, Abderrahmane Kheddary, Sabine Coquillart.FastContinuous Collision Detection between Rigid Bodies[J]).The present invention realizes based on static K-DOPs algorithm, and all bounding boxs that traversal detects are in the present invention all the path bounding boxs that utilizes the static K-DOPs bounding box having calculated to calculate, instead of static bounding box.The present invention has just used ergodic algorithm and the partitioning algorithm in static bounding box detection technique.Wherein, path bounding box be one comprise object in this path motion process the minimum K-DOPs bounding box in space of process.

Summary of the invention

The object of the invention is to overcome prior art above shortcomings, provide a kind of can effectively avoid in discrete detection algorithm pierce through high with undetected phenomenon and precision and performance is good based on the quick method for detecting continuous collision of K_DOPs.The present invention is based on interval interpolation technique and static K_DOPs Collision Detection, first the motion path of two objects is carried out to interval interpolation fitting, then utilize static K_DOPs detection algorithm to calculate the collision sub-range after interpolation, continuous interpolation fitting is carried out in collision sub-range to be detected again, until be less than a set-point between impact zone, then this smallest interval is carried out to the right continuous detecting of triangular plate, concrete technical scheme is as follows.

Based on the quick method for detecting continuous collision of K_DOPs, comprise the steps:

Whether S1, given initial time point and termination time point, detect two objects with static K_DOPs algorithm and bump at initial point and terminating point, if bumped, exits.Otherwise, go to step 2.

S2, according to the motion path of two objects carry out bounding box dynamically intersect detect.

If the K_DOPs bounding box of two objects of S3 is non-intersect, exits, otherwise go to step 4.

S4, the fundamental element in the bounding box bumping is carried out to collision detection.

If S5 fundamental element bumps, report is collided and is exited, otherwise report does not bump and exits.

In above-mentioned steps S1, first build the static K_DOPs bounding box of two objects, whether have the collision may thereby the crossing test of then carrying out bounding box on discrete time point judges two objects.

Described step S2 comprises the following steps:

S21, establish the K_DOPs bounding box Q of collision centering colliding object one ₁for: B ₁={ b ₁₁, b ₁₂... b _1j... b _1K, the K_DOPs bounding box Q of collision centering colliding object two ₂for: B ₂={ b ₂₁, b ₂₂... b _2j..., b _2K, wherein b _1jj direction projection of the bounding box of expression colliding object one put the distance of initial point, b _2jj direction projection of the bounding box of expression colliding object two put the distance of initial point, and K is the bounding box number of axle; The crossing detection of K_Dops bounding box is actually intersects detection to the projection on K bar direction of principal axis.If the vector of unit length of i axle is P _i(p _xi, p _yi, p _zi), B ₁with the intersection point of axle i be F1=b _{1, i}(p _x ⁱ, p _y ⁱ, p _z ⁱ), the intersection point of B2 and axle i is F2=b _{2, i}(p _xi, p _yi, p _zi), i is 1～K, for any time t, Q ₁module and carriage transformation matrix be M ₁(t), the module and carriage transformation matrix of Q2 is M ₂(t), the bounding box after object of which movement be by after object bounding box motion on K direction of principal axis again projection approximation obtain.Can obtain intersection point post exercise coordinate according to attitude transformation matrices so.

$\left\{\begin{array}{c}{F^{\′}}_1=M_1\left(t\right)\·F_1=M_1\left(t\right)\·(b_{1,i}\·{({p^i}_x,{p^i}_y,{p^i}_z)}^T)\\ {F^{\′}}_2=M_2\left(t\right)\·F_2=M_2\left(t\right)\·(b_{2,i}\·{({p^i}_x,{p^i}_y,{p^i}_z)}^T)\end{array}\right.---\left(1\right)$

S22, note initial point be on zero, a i axle certain any be (p _x ⁱ, p _y ⁱ, p _z ⁱ), F ₁' at axle p ⁱon subpoint be designated as F ₁", can obtain the subpoint on i direction of principal axis after object bounding box motion, thereby obtain the distance of the subpoint of colliding object one post exercise bounding box on i axle to initial point.In like manner can obtain the distance of the subpoint of colliding object two post exercise bounding boxs on i axle to initial point.

S23, because each axle of K_Dops bounding box axle is to there being an axle contrary with this direction of principal axis, here the axle that hypothesis is contrary with i direction of principal axis is j, and basis principle above can obtain colliding object one and the subpoint of colliding object two post exercise bounding boxs on j the axle distance to initial point equally.

S24, the Projection Line Segment on two reciprocal axles of direction is intersected to test.So for axle i and the j of a pair of opposite direction, two line segments only need to meet formula below any one can judge that two line segments are non-crossing:

$\left\{\begin{array}{c}{b}_{1,i}\left(t\right)<b_{2,j}\left(t\right)\\ b_{2,j}\left(t\right)<b_{1,j}\left(t\right)\end{array}---\left(2\right)\right.$

Described step S4 comprises: the crossing detection for limit with limit: suppose that a (t) b (t) represents a limit, c (t) d (t) represents an other limit, if equation below has root, intersecting appears in these two limits:

a(t)c(t)·(a(t)b(t)×c(t)d(t))＝0 (3)

Just know whether equation has root with the equation that finding roots of complex functional equation method goes to solve above.Joining and if only if on limit root is in (0,1) interval.In limit and the crossing detection of face or the crossing detection on face and limit: the intersection point a (t) that first detects limit and face, suppose a (t) representative vector, b (t) c (t) d (t) represents triangular plate, if equation below has root, limit is crossing with face appearance, and fundamental element bumps.

a(t)b(t)·(b(t)c(t)×b(t)d(t))＝0 (4)

Just know whether equation has root with the equation that extraction of root goes to solve above.And if only if in face on limit root is in (0,1) interval.

The present invention has following advantage and effect with respect to prior art:

Quick continuous collision detection algorithm based on K_DOPs can detect the collision situation of two rigid bodies in continuous motion process in real time, obtain approaching the broken line of rigid motion track by interval interpolation technique, and then rigid body is carried out in broken line motion process to collision detection, finally utilize the dynamic detection technology of fundamental element in minimum broken line motion process, to carry out detection of dynamic to triangular plate.Experimental results show that this algorithm can avoid occurring the undetected and diapirism in discrete detection, make testing result more reliable, and precision is high and performance is good, and have nothing to do because K_DOPs detects with triangular plate quantity and the topological structure of rigid body, so make this algorithm there is equally this good character.

Brief description of the drawings

Fig. 1 is the crossing figure of axial projection of two objects while intersecting test in embodiment.

Fig. 2 is the disjoint figure of axial projection of two objects while intersecting test in embodiment.

Fig. 3 is the process flow diagram based on the quick method for detecting continuous collision of K_DOPs in embodiment.

Embodiment

Below in conjunction with embodiment, the present invention is described in further detail, but embodiments of the present invention are not limited to this.

Embodiment:

The present invention is based on the quick method for detecting continuous collision of K_DOPs comprises the steps:

Whether S1, given initial time point and termination time point, detect two objects with static K_DOPs Collision Detection and bump at initial point and terminating point, if bumped, exits.Otherwise, go to step 2.

S2, according to the motion path of two objects carry out bounding box dynamically intersect detect.

If the K_DOPs bounding box of two objects of S3 is non-intersect, exits, otherwise go to step 4.

S4, the bounding box of collision is carried out to the collision detection of fundamental element.

If S5 fundamental element bumps, report is collided and is exited, otherwise report does not bump and exits.

Described step S1 comprises the following steps:

Whether first S11, static K_DOPs algorithm build the static K_DOPs bounding box of two objects, have the collision may thereby the crossing test of then carrying out bounding box on discrete time point judges two objects.

Described step S2 comprises the following steps:

S21, establish the bounding box Q of collision centering colliding object one ₁for: B ₁={ b ₁₁, b ₁₂..., b _1K, the bounding box Q of collision centering colliding object two ₂for: B ₂={ b ₂₁, b ₂₂..., b _2K, wherein b _ijj direction projection of the bounding box of expression colliding object i put the distance of initial point, and K is the bounding box number of axle.The crossing detection of K_Dops bounding box is actually intersects and detects the projection on K bar direction of principal axis.If the vector of unit length of i axle is P ⁱ(p _x ⁱ, p _y ⁱ, p _z ⁱ), B ₁with the intersection point of axle i be F ₁=b _{1, i}(p _x ⁱ, p _y ⁱ, p _z ⁱ), B ₂with the intersection point of axle i be F ₂=b _{2, i}(p _x ⁱ, p _y ⁱ, p _z ⁱ), for any time t, Q ₁module and carriage transformation matrix be M ₁(t), the module and carriage transformation matrix of Q2 is M ₂(t), the bounding box after object of which movement be by after object bounding box motion on K direction of principal axis again projection approximation obtain.Can obtain intersection point post exercise coordinate according to attitude transformation matrices so.

$\left\{\begin{array}{c}{F^{\&prime;}}_1=M_1\left(t\right)\&CenterDot;F_1=M_1\left(t\right)\&CenterDot;(b_{1,i}\&CenterDot;{({p^i}_x,{p^i}_y,{p^i}_z)}^T)\\ {F^{\&prime;}}_2=M_2\left(t\right)\&CenterDot;F_2=M_2\left(t\right)\&CenterDot;(b_{2,i}\&CenterDot;{({p^i}_x,{p^i}_y,{p^i}_z)}^T)\end{array}\right.---\left(1\right)$

Certain on S22, an i axle is a bit (p _x ⁱ, p _y ⁱ, p _z ⁱ), F ₁' at axle p ⁱon subpoint be designated as F ₁", can obtain the subpoint on i direction of principal axis after object bounding box motion and be:

F” ₁＝d·P ⁱ (2)

Wherein d is that subpoint is to the distance of initial point

$d=\frac{\stackrel{\&RightArrow;}{{\mathrm{OP}}^1}\&CenterDot;\stackrel{\&RightArrow;}{{{\mathrm{OF}}_1}^{\&prime;}}}{\left|\stackrel{\&RightArrow;}{{\mathrm{OP}}^1}\right|}=\frac{(p_x^i,p_y^i,p_z^i)\&CenterDot;M_1\left(t\right)\&CenterDot;(b_{1,i}\&CenterDot;{(p_x^i,p_y^i,p_z^i)}^T)}{\left|\stackrel{\&RightArrow;}{{\mathrm{OP}}^1}\right|}---\left(3\right)$

Due to P ⁱfor vector of unit length, formula of reduction has:

$d=\stackrel{\&RightArrow;}{{\mathrm{OP}}^1}\&CenterDot;\stackrel{\&RightArrow;}{{{\mathrm{OF}}_1}^{\&prime;}}=({p^i}_x,{p^i}_y,{p^i}_z)\&CenterDot;M_1\left(t\right)\&CenterDot;(b_{1,i}\&CenterDot;{({p^i}_x,{p^i}_y,{p^i}_z)}^T)---\left(4\right)$

Thereby obtain the distance of the subpoint of colliding object one post exercise bounding box on i axle to initial point:

b _1，i(t)＝(p _x ⁱ，p _y ⁱ，p _z ⁱ)·M ₁(t)·(b _1，i·(p _x ⁱ，p _y ⁱ，p _z ⁱ) ^T) (5)

In like manner can obtain the distance of the subpoint of colliding object two post exercise bounding boxs on i axle to initial point:

b _2，i(t)＝(p _x ⁱ，p _y ⁱ，p _z ⁱ)·M ₂(t)·(b _2，i·(p _x ⁱ，p _y ⁱ，p _z ⁱ) ^T) (6)

S23, because each axle of K_Dops bounding box axle is to there being an axle contrary with this direction of principal axis, here the axle that hypothesis is contrary with i direction of principal axis is j, and basis principle above can obtain colliding object one and the subpoint of colliding object two post exercise bounding boxs on j the axle distance to initial point equally:

$\left\{\begin{array}{c}{b}_{1,j}\left(t\right)=({p_x}^j,{p_y}^j,{p_z}^j)\&CenterDot;M_1\left(t\right)\&CenterDot;(b_{1,j}\&CenterDot;({p_x}^j,{o_y}^j,{p_z}^j{)}^T)\\ \end{array}\right.$

$b_{2,j}\left(t\right)=({p_x}^j,{p_y}^j,{p_z}^j)\&CenterDot;M_2\left(t\right)\&CenterDot;(b_{2,j}\&CenterDot;{({p_x}^j,{p_y}^j,{p_z}^j)}^T)---\left(7\right)$

S24, the Projection Line Segment on two reciprocal axles of direction is intersected to detection.

So for axle i and the j of a pair of opposite direction, two line segments only need to meet formula below any one can judge that two line segments are non-crossing:

$\left\{\begin{array}{c}{b}_{1,i}\left(t\right)<b_{2,j}\left(t\right)\\ b_{2,i}\left(t\right)<b_{1,j}\left(t\right)\end{array}---\left(8\right)\right.$

Launch b _{1, i}(t) < b _{2, j}(t) obtain:

(p _x ⁱ，p _y ⁱ，p _z ⁱ)·M ₁(t)·(b _1，i·(p _x ⁱ，p _y ⁱ，p _z ⁱ) ^T)＜(p _x ^j，p _y ^j，p _z ^j)·M ₂(t)·(b _2，j·(p _x ^j，p _y ^j，p _z ^j) ^T) (9)

(p _x ⁱ，p _y ⁱ，p _z ⁱ)·M ₁(t)·(b _1，i·(p _x ⁱ，p _y ⁱ，p _z ⁱ) ^T)-(p _x ^j，p _y ^j，p _z ^j)·M ₂(t)·(b _2，j·(p _x ^j，p _y ^j，p _z ^j) ^T)＜0 (10)

Due to (p _x ^j, p _y ^j, p _z ^j(the p of)=- _x ⁱ, p _y ⁱ, p _z ⁱ), simplifying above-mentioned formula has:

p ⁱ·(M ₁(t)·b _1，i-M ₂(t)·b _2，j)·(p ⁱ) ^T＜0 (11)

Described step S4 comprises the following steps:

S41, crossing detection for limit with limit: suppose that a (t) b (t) represents a limit, c (t) d (t) represents an other limit, if equation below has root, intersecting appears in these two limits:

a(t)c(t)·(a(t)b(t)×c(t)d(t))＝0 (12)

Just know whether equation has root with the equation that finding roots of complex functional equation method goes to solve above.Joining and if only if on limit root is in (0,1) interval.

For limit and the crossing detection of face or the crossing detection on face and limit: the intersection point a (t) that first detects limit and face, suppose a (t) representative vector, b (t) c (t) d (t) represents triangular plate, if equation below has root, limit is crossing with face appearance, and two objects bump.

a(t)b(t)·(b(t)c(t)×b(t)d(t))＝0 (13)

Just know whether equation has root with the equation that extraction of root goes to solve above.And if only if in face on limit root is in (0,1) interval.

Above-described embodiment is preferably embodiment of the present invention; but embodiments of the present invention are not restricted to the described embodiments; other any do not deviate from change, the modification done under Spirit Essence of the present invention and principle, substitutes, combination, simplify; all should be equivalent substitute mode, within being included in protection scope of the present invention.

Claims (3)

1. based on the quick method for detecting continuous collision of K_DOPs, it is characterized in that comprising the following steps:

Whether S1, given initial time point and termination time point, detect two objects with static K_DOPs algorithm and bump at initial time point and termination time point, if bumped, exit, otherwise, go to step S2;

S2, according to the motion path of two objects carry out bounding box dynamically intersect detect, specifically comprise the following steps:

S21, establish the K_DOPs bounding box Q of collision centering colliding object one ₁for: B ₁={ b ₁₁, b ₁₂... b _1j... b _1K, the K_DOPs bounding box Q of collision centering colliding object two ₂for: B ₂={ b ₂₁, b ₂₂... b _2j..., b _2K, wherein b _1jj direction projection of the bounding box of expression colliding object one put the distance of initial point, b _2jj direction projection of the bounding box of expression colliding object two put the distance of initial point, and K is the bounding box number of axle; The crossing detection of K_Dops bounding box is actually intersects detection to the projection on K bar direction of principal axis; If the vector of unit length of i axle is Pi (p _x ⁱ, p _y ⁱ, p _z ⁱ), B ₁with the intersection point of axle i be F1=b _{1, i}(p _x ⁱ, p _y ⁱ, p _z ⁱ), the intersection point of B2 and axle i is F2=b _{2, i}(p _x ⁱ, p _y ⁱ, p _z ⁱ), i is 1～K, for any time t, Q ₁module and carriage transformation matrix be M ₁(t), the module and carriage transformation matrix of Q2 is M ₂(t), the bounding box after object of which movement be by after object bounding box motion on K direction of principal axis again projection obtain, can obtain intersection point post exercise coordinate according to attitude transformation matrices so:

$\left\{\begin{array}{c}{F^{\&prime;}}_1=M_1\left(t\right)\&CenterDot;F_1=M_1\left(t\right)\&CenterDot;(b_{1,i}\&CenterDot;{({p^i}_x,{p^i}_y,{p^i}_z)}^T)\\ {F^{\&prime;}}_2=M_2\left(t\right)\&CenterDot;F_2=M_2\left(t\right)\&CenterDot;(b_{2,i}\&CenterDot;{({p^i}_x,{p^i}_y,{p^i}_z)}^T)\end{array}\right.---\left(1\right);$

S22, note initial point are O, and certain on i axle is a bit (p _x ⁱ, p _y ⁱ, p _z ⁱ), F ₁' subpoint on i axle is designated as F ₁"; can obtain the subpoint on i direction of principal axis after object bounding box motion; thus the distance of the subpoint of colliding object one post exercise bounding box on i axle to initial point obtained, in like manner can obtain the distance of the subpoint of colliding object two post exercise bounding boxs on i axle to initial point;

S23, because each axle of K_Dops bounding box axle is to there being an axle contrary with this each direction of principal axis, here the axle that hypothesis is contrary with i direction of principal axis is j, and basis principle above can obtain colliding object one and the subpoint of colliding object two post exercise bounding boxs on j the axle distance to initial point equally;

S24, the Projection Line Segment on two reciprocal axles of direction is intersected to test, so for axle i and the j of a pair of opposite direction, two line segments only need to meet formula below any one can judge that two line segments are non-crossing:

$\left\{\begin{array}{c}{b}_{1,i}\left(t\right)<b_{2,j}\left(t\right)\\ b_{2,i}\left(t\right)<b_{1,j}\left(t\right)\end{array}\right.---\left(2\right);$

If the K_DOPs bounding box of two objects of S3 is non-intersect, exits, otherwise go to step S4;

S4, the fundamental element in the bounding box bumping is carried out to collision detection;

If S5 fundamental element bumps, report is collided and is exited, otherwise report does not bump and exits.

2. according to claim 1 based on the quick method for detecting continuous collision of K_DOPs, it is characterized in that

In step S1, first build the static K_DOPs bounding box of two objects, whether have the collision may thereby the crossing test of then carrying out bounding box on discrete time point judges two objects.

3. according to claim 1 based on the quick method for detecting continuous collision of K_DOPs, it is characterized in that described step S4 comprises: the crossing detection for limit with limit: suppose that a (t) b (t) represents a limit, c (t) d (t) represents an other limit, if equation below has root, intersecting appears in these two limits:

a(t)c(t)·(a(t)b(t)×c(t)d(t))＝0 (3)，

Just know whether equation has root with the equation that finding roots of complex functional equation method goes to solve above, joining and if only if on limit root is in (0,1) interval; For limit and the crossing detection of face or the crossing detection on face and limit: the intersection point a (t) that first detects limit and face, suppose a (t) representative vector, b (t) c (t) d (t) represents triangular plate, if equation below has root, limit occurs crossing with face, fundamental element bumps

a(t)b(t)·(b(t)c(t)×b(t)d(t))＝0 (4)，

Just know whether equation has root with the equation that extraction of root goes to solve above, and if only if in face on limit root is in (0,1) interval.