矩形旋转碰撞,OBB方向包围盒算法实现

原博客地址：http://blog.csdn.net/tom_221x/article/details/38457757

版权归原博客作者所有

如何进行2D旋转矩形的碰撞检测，可以使用一种叫OBB的检测算法(Oriented bounding box)方向包围盒。这个算法是基于SAT(Separating Axis Theorem)分离轴定律的。而OBB不仅仅是计算矩形的碰撞检测，而是一种算法模型。简单解释一下概念，包围盒和分离轴定律。

包围盒：是根据物体的集合形状，来决定盒子的大小和方向，这样可以选择最紧凑的盒子来代表物体。见下图

黑色的就是包围盒，可以是凸多边形，最贴近检测物体即可。

 

分离轴定律：两个凸多边形物体，如果我们能找到一个轴，使得两个在物体在该轴上的投影互不重叠，则这两个物体之间没有碰撞发生，该轴为Separating Axis

那么用一般去检测那些轴呢，垂直于多边形每条边的轴。如下图：

所以，分离轴定律变成，两个多边形在所有轴上的投影都发生重叠，则判定为碰撞；否则，没有发生碰撞。

 

 

下面，我只考虑矩形的情况，如何检测分离轴。

很明显，矩形4条边，有4条检测轴，那么2个矩形就有8个。但是矩形有2个轴是重复的，所以只需要检测2条轴就可以了，既是矩形的两条互相垂直的边所在的轴。

如上图，判断碰撞，我们需要判断2个矩形在4个轴上的投影是否重叠。这里有2种可能的方式。第一种，把每个矩形的4个顶点投影到一个轴上，这样算出4个顶点最长的连线距离，以后同样对待第二个矩形，最后判断2个矩形投影距离是否重叠。

第二种方式，把2个矩形的半径距离投影到轴上，以后把2个矩形的中心点连线投影到轴上，以后判断2个矩形的中心连线投影，和2个矩形的半径投影之和的大小。本文使用这种方式。

 

这里用到一些向量的数学知识。如下图：

P点为矩形在X轴上的投影点，矩形在垂直轴上的投影点为原点。这里也能看出来，点P所在的矩形轴， 在X轴上的投影长度为OP，如果矩形逆时针绕远点O旋转，OP在X轴上的投影长度变小，直到为0，OP垂直于X轴。也就是，OP在X轴上的投影长度的最大与最小值。这也解释了，为什么我们选择检测轴为垂直于多边形边的轴，因为在这些轴上我们能取到极值，中间的那些轴就没必要检测了。

如何表示轴，我们需要用向量，正确的使用单位向量，能够简化模型，降低思考的难度。如下图：

假设P点的坐标为(px, py), 那么向量P就是(px, py)，点P在X轴上的投影点Q坐标是(qx, qy)，那么向量Q就是(qx, qy)。我们假设X轴上的单位向量是(1, 0)。那么向量P和X轴上单位向量点乘有：

向量P * X轴单位向量 = |P| * |X轴单位向量| * cosPQ  = px * 1 + py * 0 = px

又因为单位向量的长度等于1所以，px就是向量Q的长度。这是非常有意义的，我们就得到一个规律，就是把一个向量点乘一个单位向量，我们得到的是这个向量在这个单位向量上的投影长度。用代码表示为：

[cpp] view plain copy

1. /** 
2.  * dot-multiply 
3.  */  
4. private float dot(float[] axisA, float[] axisB) {  
5.     return Math.abs(axisA[0] * axisB[0] + axisA[1] * axisB[1]);  
6. }  

这里float[] 存放的是一个点的x ,y 坐标。axisB 为单位向量，这个结果就是axisA向量在，单位向量axisB上投影的长度。

 

 

下面我们看一下，单位向量如何表示：

单位向量是用单位圆来描述的。假设这个圆的半径为1，那么圆上的任何一个坐标到原点构成的向量都可以看作一个单位向量，并且长度为1。这般，明显的点P就是一个单位向量。点P在单位圆上移动，那么这个单位向量就在旋转，向量P就和角A建立了关系。很明显的得出，cosA 就是向量P的X坐标，sinA 就是向量P的Y坐标。

这样我们就可以得出，单位向量P为(cosA，sinA)。这个模型的意义就是把单位向量P可以看成矩形的条边。如下图：

那么矩形的另一个边对应的单位向量S如何表示呢，向量S和向量P是垂直的，我们可以得出， S(-sinA, cosA)， 向量S 点乘 向量P  = 0

至此，我们就可以通过一个旋转角度，得到一个矩形的2个检测轴的单位向量。代码如下：

[cpp] view plain copy

1. // unit vector of x axis  
2. private float[] axisX;  
3. // unit vector of y axis  
4. private float[] axisY;  
5.    
6. // 0 -360  
7. private float rotation;  
8.    
9. /** 
10.  * Set axis x and y by rotation 
11.  *  
12.  * @param rotation float 0 - 360  
13.  */  
14. public OBB setRotation(float rotation) {  
15.     this.rotation = rotation;  
16.    
17.     this.axisX[0] = MathUtils.cos(rotation);  
18.     this.axisX[1] = MathUtils.sin(rotation);  
19.    
20.     this.axisY[0] = -MathUtils.sin(rotation);  
21.     this.axisY[1] = MathUtils.cos(rotation);  
22.    
23.     return this;  
24. }  

下一步如何计算矩形的半径投影呢，什么又是半径投影呢，看下图：

橙色线段，是矩形的2条检测轴，3张图是矩形旋转的3个特殊位置截图。蓝色线段就是矩形半径投影。其实就是，矩形在X轴上最远处的交点，数学上意义就是2条检测轴的投影之和。

2条检测轴的向量和就是中心点到矩形一个顶点的向量，所以投影半径也是中心点到矩形顶点的向量投影长度。注意向量的方向会影响投影长度。按照中间那幅图，2条检测轴向量和的投影是，2条检测轴投影的差值。如果把其中一个轴，旋转180度，那么2个检测轴和的投影就是，2条轴投影的和值。

 

至此，如果我们把矩形在任意角度的2条轴向量投影到单位向量上，根据前面的单位向量规律。我们就得到了轴向量在单位向量上投影的长度，而单位向量的长度为1，那么我们得到的就是轴向量与单位向量的比例。在用这个比例乘以轴向量的长度，就得到了轴的投影长度，就能求出轴半径的长度了。如图

2个矩形检测过程中，每次以一个矩形的检测轴为坐标系，投影另一个矩形的检测轴。图中，蓝色线段为左边矩形的半径投影，黄色线段为右边矩形检测轴。我们需要把右边2条检测轴投影到蓝色线段所在X轴的单位向量，得到投影比例，以后在乘以2条检测轴的长度，就可以得到右边矩形的半径投影。

红色线段为2个矩形的中心点连心，计算其在X轴的投影长度。比较中心点连线的投影长度与2矩形的半径投影长度之和，如果连线投影大，那么在这条轴上没有碰撞，否则碰撞。半径投影代码如下：

[cpp] view plain copy

1. private float halfWidth;  
2.    
3. private float halfHeight;  
4.    
5. /** 
6.  * Get axisX and axisY projection radius distance on axis 
7.  */  
8. public float getProjectionRadius(float[] axis) {  
9.    
10.     // axis, axisX and axisY are unit vector  
11.    
12.     // projected axisX to axis  
13.     float projectionAxisX = this.dot(axis, this.axisX);  
14.     // projected axisY to axis  
15.     float projectionAxisY = this.dot(axis, this.axisY);  
16.    
17.     return this.halfWidth * projectionAxisX + this.halfHeight * projectionAxisY;  
18. }  

判断2矩形最终是否碰撞，需要依次检测4个分离轴，如果在一个轴上没有碰撞，则2个矩形就没有碰撞。代码如下：

[cpp] view plain copy

1. /** 
2.  * OBB is collision with other OBB 
3.  */  
4. public boolean isCollision(OBB obb) {  
5.     // two OBB center distance vector  
6.     float[] centerDistanceVertor = {  
7.             this.centerPoint[0] - obb.centerPoint[0],  
8.             this.centerPoint[1] - obb.centerPoint[1]  
9.     };  
10.    
11.     float[][] axes = {  
12.             this.axisX,  
13.             this.axisY,  
14.             obb.axisX,  
15.             obb.axisY,  
16.     };  
17.    
18.     for(int i = 0; i < axes.length; i++) {  
19.         // compare OBB1 radius projection add OBB2 radius projection to centerDistance projection  
20.         if(this.getProjectionRadius(axes[i]) + obb.getProjectionRadius(axes[i])   
21.                 <= this.dot(centerDistanceVertor, axes[i])) {  
22.             return false;  
23.         }  
24.     }  
25.    
26.     return true;  
27. }  

最后，给出OBB完整的代码封装

Java代码：

[java] view plain copy

1. /** 
2.  * @author scott.cgi 
3.  * @since  2012-11-19 
4.  *   
5.  * Oriented bounding box  
6.  */  
7. public class OBB {  
8.       
9.     private float[] centerPoint;  
10.       
11.     private float halfWidth;  
12.       
13.     private float halfHeight;  
14.       
15.     // unit vector of x axis  
16.     private float[] axisX;  
17.     // unit vector of y axis  
18.     private float[] axisY;  
19.       
20.     // 0 -360  
21.     private float rotation;  
22.       
23.     private float scaleX;  
24.     private float scaleY;  
25.       
26.       
27.     private float offsetAxisPointDistance;  
28.       
29.       
30.     /** 
31.      * Create default OBB 
32.      *  
33.      * @param x bornCenterX x 
34.      * @param y bornCenterY Y 
35.      * @param halfWidth 
36.      * @param halfHeight 
37.      */  
38.     public OBB(float bornCenterX, float bornCenterY, float halfWidth, float halfHeight) {  
39.           
40.         this.halfWidth  = halfWidth;  
41.         this.halfHeight = halfHeight;  
42.           
43.         this.scaleX = 1.0f;  
44.         this.scaleY = 1.0f;  
45.           
46.         this.centerPoint = new float[] {  
47.                         bornCenterX,  
48.                         bornCenterY  
49.         };  
50.           
51.         this.axisX = new float[2];  
52.         this.axisY = new float[2];  
53.           
54.         float[] offsetAxisPoint = new float[] {  
55.                 bornCenterX - Director.getHalfScreenWidth(),  
56.                 bornCenterY - Director.getHalfScreenHeight()  
57.         };  
58.           
59.         this.offsetAxisPointDistance = (float) Math.sqrt(this.dot(offsetAxisPoint, offsetAxisPoint));  
60.           
61.         this.setRotation(0.0f);  
62.     }  
63.       
64.     /** 
65.      * Create default OBB with born in center screen 
66.      *  
67.      * @param halfWidth 
68.      * @param halfHeight 
69.      */  
70.     public OBB(float halfWidth, float halfHeight) {  
71.         this(Director.getHalfScreenWidth(), Director.getHalfScreenHeight(), halfWidth, halfHeight);  
72.     }  
73.       
74.     /** 
75.      * Get axisX and axisY projection radius distance on axis 
76.      */  
77.     public float getProjectionRadius(float[] axis) {  
78.           
79.         // axis, axisX and axisY are unit vector  
80.           
81.         // projected axisX to axis  
82.         float projectionAxisX = this.dot(axis, this.axisX);  
83.         // projected axisY to axis  
84.         float projectionAxisY = this.dot(axis, this.axisY);  
85.           
86.           
87.         return this.halfWidth * this.scaleX * projectionAxisX + this.halfHeight * this.scaleY * projectionAxisY;  
88.     }  
89.       
90.     /** 
91.      * OBB is collision with other OBB 
92.      */  
93.     public boolean isCollision(OBB obb) {  
94.         // two OBB center distance vector  
95.         float[] centerDistanceVertor = {  
96.                 this.centerPoint[0] - obb.centerPoint[0],  
97.                 this.centerPoint[1] - obb.centerPoint[1]  
98.         };  
99.           
100.           
101.         float[][] axes = {  
102.                 this.axisX,  
103.                 this.axisY,  
104.                 obb.axisX,  
105.                 obb.axisY,  
106.         };  
107.           
108.         for(int i = 0; i < axes.length; i++) {  
109.             // compare OBB1 radius projection add OBB2 radius projection to centerDistance projection  
110.             if(this.getProjectionRadius(axes[i]) + obb.getProjectionRadius(axes[i])   
111.                     <= this.dot(centerDistanceVertor, axes[i])) {  
112.                 return false;  
113.             }  
114.         }  
115.           
116.         return true;  
117.     }  
118.       
119.       
120.     /** 
121.      * dot-multiply 
122.      */  
123.     private float dot(float[] axisA, float[] axisB) {  
124.         return Math.abs(axisA[0] * axisB[0] + axisA[1] * axisB[1]);  
125.     }  
126.       
127.     /** 
128.      * Set axis x and y by rotation 
129.      *  
130.      * @param rotation float 0 - 360  
131.      */  
132.     public OBB setRotation(float rotation) {  
133.         this.rotation = rotation;  
134.           
135.         this.axisX[0] = MathUtils.cos(rotation);  
136.         this.axisX[1] = MathUtils.sin(rotation);  
137.           
138.         this.axisY[0] = -MathUtils.sin(rotation);  
139.         this.axisY[1] = MathUtils.cos(rotation);  
140.           
141.         this.setCenter(this.centerPoint[0], this.centerPoint[1]);  
142.   
143.         return this;  
144.     }  
145.       
146.     /** 
147.      * Set OBB center point and will add offsetAxis value 
148.      */  
149.     public OBB setCenter(float x, float y) {  
150.         float offsetX = this.offsetAxisPointDistance * MathUtils.cos(this.rotation);  
151.         float offsetY = this.offsetAxisPointDistance * MathUtils.sin(this.rotation);  
152.           
153.         this.centerPoint[0] = x + offsetX;  
154.         this.centerPoint[1] = y + offsetY;  
155.           
156.         return this;  
157.     }  
158.       
159.     /** 
160.      * Set OBB scale x, y 
161.      */  
162.     public OBB setScale(float scaleX, float scaleY) {  
163.         this.scaleX = scaleX;  
164.         this.scaleY = scaleY;  
165.                           
166.         return this;  
167.     }  
168.       
169.       
170.     public float getRotation() {  
171.         return this.rotation;  
172.     }  
173.       
174.     public float getCenterX() {  
175.         return this.centerPoint[0];  
176.     }  
177.       
178.     public float getCenterY() {  
179.         return this.centerPoint[1];  
180.     }  
181.       
182.     public float getHalfWidth() {  
183.         return this.halfWidth * this.scaleX;  
184.     }  
185.       
186.     public float getHalfHeight() {  
187.         return this.halfHeight * this.scaleY;  
188.     }  
189.       
190. }  

c 代码：

[cpp] view plain copy

1. /* 
2.  * OBBRect.h 
3.  * 
4.  *  Created on: 2013-2-11 
5.  *  Author: scott.cgi 
6.  */  
7.   
8. #ifndef OBBRect_Rect_H_  
9. #define OBBRect_Rect_H_  
10.   
11. #include <stdbool.h>  
12.   
13. #include "Mojoc/Graphics/Draw/Rect.h"  
14. #include "Mojoc/Toolkit/Def/CodeStyle.h"  
15. #include "Mojoc/Toolkit/Utils/AMathUtils.h"  
16. #include "Mojoc/Toolkit/Def/StructMember.h"  
17.   
18.   
19. #ifdef __cplusplus  
20. extern "C" {  
21. #endif  
22.   
23.   
24. typedef struct OBBRect OBBRect;  
25.   
26. /** 
27.  * Oriented bounding box in Rect shape 
28.  * Use openGL world coordinate system 
29.  */  
30. struct OBBRect {  
31.   
32.     float   centerX;  
33.     float   centerY;  
34.   
35.     /** Set origin(0,0) when obbRect create */  
36.     float   originX;  
37.     float   originY;  
38.   
39.     /** Clockwise [0 - 360] */  
40.     float   rotation;  
41.     float   scaleX;  
42.     float   scaleY;  
43.   
44.   
45.         Get(  
46.                 /** Unit vector of x axis */  
47.                 StructMember(Vector2, xAxis);  
48.   
49.                 /** Unit vector of y axis */  
50.                 StructMember(Vector2, yAxis);  
51.   
52.                 Rect*   rect;  
53.   
54.                 /** Distance of center point to origin(0, 0 */  
55.                 float   offsetDistance;  
56.                 /** Degree of vector which center point to origin(0, 0 */  
57.                 float   offsetDegree;  
58.   
59.                 float   halfWidth;  
60.                 float   halfHeight;  
61.         );  
62.   
63. };  
64.   
65.   
66.   
67.   
68. typedef struct {  
69.         OBBRect* (*create) (Rect* rect);  
70.         void     (*init)   (Rect* rect, Out(OBBRect* obbRect));  
71.   
72.     /** 
73.      * Set obbRect rotation or origin x,y called updateCenter for 
74.      * update obbRect center x,y 
75.      */  
76.     void (*updateCenter)(OBBRect* obbRect);  
77.   
78.   
79.     /** 
80.      * Set obbRect rotation 
81.      */  
82.     void (*setRotation) (OBBRect* obbRect, float rotation);  
83.   
84.   
85.     /** 
86.      * OBBRect is collision with other OBBRect 
87.      */  
88.     bool (*isCollision) (OBBRect* obbRect, OBBRect* otherObb);  
89.   
90.   
91.     /** 
92.      * Get real width with scaleX 
93.      */  
94.     float (*getWidth)   (OBBRect* obbRect);  
95.   
96.   
97.     /** 
98.      * Get real height with scaleY 
99.      */  
100.     float (*getHeight)  (OBBRect* obbRect);  
101.   
102. } _AOBBRect;  
103.   
104.   
105. extern _AOBBRect AOBBRect;  
106.   
107.   
108.   
109. #ifdef __cplusplus  
110. }  
111. #endif  
112.   
113. #endif /* OBBRect_H_ */  

[cpp] view plain copy

1. /* 
2.  * OBBRect.c 
3.  * 
4.  *  Created on: 2013-2-11 
5.  *  Author: scott.cgi 
6.  */  
7.   
8. #include <malloc.h>  
9. #include <math.h>  
10.   
11. #include "Mojoc/Physics/OBBRect.h"  
12. #include "Mojoc/Toolkit/Utils/AMathUtils.h"  
13. #include "Mojoc/Toolkit/Platform/Log.h"  
14.   
15.   
16. static void updateCenter(OBBRect* obbRect) {  
17.         obbRect->centerX = obbRect->originX +  
18.                                    obbRect->offsetDistance * obbRect->scaleX *  
19.                                            AMathUtils_Cos(obbRect->rotation + obbRect->offsetDegree);  
20.           
21.         obbRect->centerY = obbRect->originY +  
22.                                    obbRect->offsetDistance * obbRect->scaleY *  
23.                                            AMathUtils_Sin(obbRect->rotation + obbRect->offsetDegree);  
24. }  
25.   
26. static void setRotation(OBBRect* obbRect, float rotation) {  
27.         obbRect->xAxis->x = AMathUtils_Cos(rotation);  
28.         obbRect->xAxis->y = AMathUtils_Sin(rotation);  
29.   
30.         obbRect->yAxis->x = -AMathUtils_Sin(rotation);  
31.         obbRect->yAxis->y =  AMathUtils_Cos(rotation);  
32.   
33.         obbRect->rotation = rotation;  
34. }  
35.   
36. /** 
37.  * Get axisX and axisY projection radius distance on unit axis 
38.  */  
39. static inline float getProjectionRadius(OBBRect* obbRect, Vector2* axis) {  
40.   
41.     // axis, axisX and axisY are unit vector  
42.   
43.     // projected axisX to axis  
44.     float projectionAxisX = AMathUtils_DotMultiply2(axis->x, axis->y, obbRect->xAxis->x, obbRect->xAxis->y);  
45.     // projected axisY to axis  
46.     float projectionAxisY = AMathUtils_DotMultiply2(axis->x, axis->y, obbRect->yAxis->x, obbRect->yAxis->y);  
47.   
48.   
49.     return obbRect->halfWidth  * obbRect->scaleX * projectionAxisX +  
50.            obbRect->halfHeight * obbRect->scaleY * projectionAxisY;  
51.   
52. }  
53.   
54. static bool isCollision(OBBRect* obbRect, OBBRect* otherObb) {  
55.     // two OBBRect center distance vector  
56.         Vector2 distanceVec = {  
57.         obbRect->centerX - otherObb->centerX,  
58.         obbRect->centerY - otherObb->centerY,  
59.     };  
60.   
61.   
62.         Vector2* axes[] = {  
63.                 obbRect->xAxis,  
64.                 obbRect->yAxis,  
65.             otherObb->xAxis,  
66.                 otherObb->yAxis,  
67.     };  
68.   
69.     for (int i = 0; i < 4; i++) {  
70.         // compare OBBRect1 radius projection add OBBRect2 radius projection to centerDistance projection  
71.         if (getProjectionRadius(obbRect, axes[i]) + getProjectionRadius(otherObb, axes[i]) <=  
72.            AMathUtils_DotMultiply2(distanceVec.x, distanceVec.y, axes[i]->x, axes[i]->y)) {  
73.   
74.             return false;  
75.         }  
76.     }  
77.   
78.     return true;  
79. }  
80.   
81. static float getWidth(OBBRect* obbRect) {  
82.         return (obbRect->halfWidth * 2) * obbRect->scaleX;  
83. }  
84.   
85. static float getHeight(OBBRect* obbRect) {  
86.         return (obbRect->halfHeight * 2) * obbRect->scaleY;  
87. }  
88.   
89. static inline void initOBBRect(OBBRect* obbRect, Rect* rect) {  
90.         obbRect->rect           = rect;  
91.   
92.         obbRect->halfWidth      = (rect->right   - rect->left) / 2;  
93.         obbRect->halfHeight     = (rect->bottom  - rect->top)  / 2;  
94.   
95.         obbRect->scaleX         = 1.0f;  
96.         obbRect->scaleY         = 1.0f;  
97.   
98.     obbRect->originX        = 0.0f;  
99.     obbRect->originY        = 0.0f;  
100.   
101.         obbRect->centerX        = rect->left + obbRect->halfWidth;  
102.         obbRect->centerY        = rect->top  - obbRect->halfHeight;  
103.           
104.         obbRect->offsetDistance = sqrtf(AMathUtils_DotMultiply2(obbRect->centerX, obbRect->centerY, obbRect->centerX, obbRect->centerY));  
105.         obbRect->offsetDegree   = AMathUtils_Atan2(obbRect->centerX, obbRect->centerY);  
106.           
107.         LogD("centerX = %f, centerY = %f", obbRect->centerX, obbRect->centerY);  
108.         LogD("offsetDistance = %f, offsetDegree = %f",  
109.                   obbRect->offsetDistance, obbRect->offsetDegree);  
110.           
111.     setRotation(obbRect, 0.0f);  
112. }  
113.   
114. static OBBRect* create(Rect* rect) {  
115.         OBBRect* obbRect = (OBBRect*) malloc(sizeof(OBBRect));  
116.         initOBBRect(obbRect, rect);  
117.   
118.         return obbRect;  
119. }  
120.   
121. static void init(Rect* rect, Out(OBBRect* obbRect)) {  
122.         initOBBRect(obbRect, rect);  
123. }  
124.   
125.   
126. _AOBBRect AOBBRect = {  
127.         create,  
128.         init,  
129.   
130.         updateCenter,  
131.         setRotation,  
132.         isCollision,  
133.         getWidth,  
134.         getHeight,  
135. };  

以上为原作者所著，本人根据作者所述，根据编译环境的不同，改写成PLC的Codesys-ST语言来实现：

1.两点之间的距离函数：Dist_2pts()

2. 向量的点乘函数dot()

3. 投影半径函数 GetProjectionRadius()

4. 产品的数据结构L_RPP_ProdInfo

5.主程序Main()