Looking for efficient OBB - AABB 3D Collider Solutions.

Hi galls and folks.

I am learning on ECS with Job system and trying to implement simple OBB (Oriented Bounding Box) and AABB (Axis Alligned Bounding Box) collision detection for 3D Boxes. This means, I can not use native Unity Colliders components.

At the moment, I have written my own script, but it seams is not most performant algorithm. I can collide (intersect) 800 boxes pairs at the same time, with 25-30 fps on single core using OOP. Yet I do wonder, if anybody has any details, on Unity Collision system, to squeeze most of it.

Or maybe, what should I change, in my scripts. I am sure it can be optimized. Yet not entirely sure how.

Here is my quite vauge test version of script.
It dosen't have to be complex.

Spoiler

Code (CSharp):

1. using UnityEngine;
2.  
3. /// <summary>
4. /// 3D Box collision
5. /// </summary>
6. public class SeprateAxisTheorem : MonoBehaviour {
7.  
8.     /// <summary>
9.     /// AABB - Axis Alligned Bounding Box
10.     /// </summary>
11.     Transform tr_cubeAABB ;
12.  
13.     /// OBB - Oriented Bounding Box
14.     Transform tr_cubeOBB ;
15.  
16.     // BoxCollider col_cubeAABB ;
17.     BoxCollider col_cubeOBB ;
18.  
19.     public class BoundingsAxis
20.     {
21.         public Vector3 V3_min ;
22.         public Vector3 V3_max ;
23.     }
24.  
25.     // Use this for initialization
26.     void Start ()
27.     {
28.         tr_cubeAABB = this.transform.Find ( "StaticCube" ) ;
29.         tr_cubeOBB = this.transform.Find ( "DynamicCube" ) ;
30.  
31.         BoundingsAxis axisAABB = new BoundingsAxis () ;
32.         BoundingsAxis axisOBB = new BoundingsAxis () ;
33.          
34.         // col_cubeAABB = tr_cubeAABB.GetComponent <BoxCollider> () ;
35.         col_cubeOBB = tr_cubeOBB.GetComponent <BoxCollider> () ;
36.  
37.     }
38.  
39.     // Update is called once per frame
40.     void FixedUpdate ()
41.     {
42.         Vector3 [] V3_minMaxAABB = _MinMaxAABB ( tr_cubeAABB ) ;
43.         Vector3 [] V3_minMaxOBB = _MinMaxOBB ( tr_cubeOBB, col_cubeOBB.bounds ) ;
44.  
45.         // Debug.Log ( _AABBCollision3D ( V3_minMaxAABB, V3_minMaxOBB ) ? "1" : "0" ) ;
46.  
47.  
48.         // check first, if Simple AAB2OOB collision occures
49.         if ( _AABB2AABBCollisionTest ( V3_minMaxAABB, V3_minMaxOBB ) )
50.         {
51.  
52.             Vector3[] aabbCorners = _CornersAABB ( tr_cubeAABB ) ;
53.             Vector3[] obbCorners = _CornersOBB ( tr_cubeOBB ) ;
54.  
55.             // then consider axis rotation in calculations
56.             bool isColliding = _ABB2OBBCollisionTest ( aabbCorners, obbCorners ) ;
57.  
58.             if ( isColliding )
59.             {
60.                 // Debug.Log ( "AABB + OBB" ) ;
61.             }
62.             else
63.             {
64.                 // Debug.Log ( "ABB + AABB" ) ;
65.             }
66.         }
67.  
68.     }
69.  
70.     private Vector3 [] _CornersAABB ( Transform tr )
71.     {
72.         //BoxCollider boxCollider = tr.GetComponent <BoxCollider> () ;
73.         //Bounds bunds = boxCollider.bounds ;
74.  
75.         Vector3 [] a_corners = new Vector3 [8] ;
76.  
77.         a_corners[0] = tr.position + new Vector3 (-tr.localScale.x, -tr.localScale.y, -tr.localScale.z) * 0.5f ;
78.         a_corners[1] = tr.position + new Vector3 (tr.localScale.x, -tr.localScale.y, -tr.localScale.z) * 0.5f ;
79.         a_corners[2] = tr.position + new Vector3 (tr.localScale.x, -tr.localScale.y, tr.localScale.z) * 0.5f ;
80.         a_corners[3] = tr.position + new Vector3 (-tr.localScale.x, -tr.localScale.y, tr.localScale.z) * 0.5f ;
81.         a_corners[4] = tr.position + new Vector3 (-tr.localScale.x, tr.localScale.y, -tr.localScale.z) * 0.5f ;
82.         a_corners[5] = tr.position + new Vector3 (tr.localScale.x, tr.localScale.y, -tr.localScale.z) * 0.5f ;
83.         a_corners[6] = tr.position + new Vector3 (tr.localScale.x, tr.localScale.y, tr.localScale.z) * 0.5f ;
84.         a_corners[7] = tr.position + new Vector3 (-tr.localScale.x, tr.localScale.y, tr.localScale.z) * 0.5f ;
85.  
86.         return a_corners ;
87.     }
88.  
89.     private Vector3 [] _CornersOBB ( Transform tr )
90.     {
91.         //BoxCollider boxCollider = tr.GetComponent <BoxCollider> () ;
92.         //Bounds bunds = boxCollider.bounds ;
93.  
94.         Vector3 [] a_corners = new Vector3 [8] ;
95.  
96.         a_corners[0] = tr.position + tr.rotation * ( new Vector3 (-tr.localScale.x, -tr.localScale.y, -tr.localScale.z) * 0.5f ) ;
97.         a_corners[1] = tr.position + tr.rotation * ( new Vector3 (tr.localScale.x, -tr.localScale.y, -tr.localScale.z) * 0.5f ) ;
98.         a_corners[2] = tr.position + tr.rotation * ( new Vector3 (tr.localScale.x, -tr.localScale.y, tr.localScale.z) * 0.5f ) ;
99.         a_corners[3] = tr.position + tr.rotation * ( new Vector3 (-tr.localScale.x, -tr.localScale.y, tr.localScale.z) * 0.5f ) ;
100.         a_corners[4] = tr.position + tr.rotation * ( new Vector3 (-tr.localScale.x, tr.localScale.y, -tr.localScale.z) * 0.5f ) ;
101.         a_corners[5] = tr.position + tr.rotation * ( new Vector3 (tr.localScale.x, tr.localScale.y, -tr.localScale.z) * 0.5f ) ;
102.         a_corners[6] = tr.position + tr.rotation * ( new Vector3 (tr.localScale.x, tr.localScale.y, tr.localScale.z) * 0.5f ) ;
103.         a_corners[7] = tr.position + tr.rotation * ( new Vector3 (-tr.localScale.x, tr.localScale.y, tr.localScale.z) * 0.5f ) ;
104.  
105.         return a_corners ;
106.     }
107.  
108.     private Vector3 [] _MinMaxAABB ( Transform tr )
109.     {
110.  
111.         //Vector3 V3_min = bounds.min ;
112.         //Vector3 V3_max = bounds.max ;
113.         Vector3 V3_min = tr.position + new Vector3 (-tr.localScale.x, -tr.localScale.y, -tr.localScale.z) * 0.5f ;
114.         Vector3 V3_max = tr.position + new Vector3 (tr.localScale.x, tr.localScale.y, tr.localScale.z) * 0.5f ;
115.  
116.         // Debug.DrawLine ( V3_min - Vector3.forward * 50, V3_min + Vector3.forward * 100, Color.red ) ;
117.         //Debug.DrawLine ( V3_max - Vector3.forward * 50, V3_max + Vector3.forward * 100, Color.red ) ;
118.         /// Debug.DrawLine ( V3_min - Vector3.right * 50, V3_min + Vector3.right * 100, Color.blue ) ;
119.         //Debug.DrawLine ( V3_max - Vector3.right * 50, V3_max + Vector3.right * 100, Color.blue ) ;
120.  
121.         // pass properties to an array
122.         Vector3 [] V3_a = new Vector3 [2] ;
123.         V3_a [0] = V3_min ;
124.         V3_a [1] = V3_max ;
125.  
126.         return V3_a ;
127.  
128.     }
129.     private Vector3 [] _MinMaxOBB ( Transform tr, Bounds bounds )
130.     {
131.  
132.         Vector3 V3_min = bounds.min ;
133.         Vector3 V3_max = bounds.max ;
134.         //Vector3 V3_min = tr.position +  tr.rotation * ( new Vector3 (-tr.localScale.x, -tr.localScale.y, -tr.localScale.z) * 0.5f ) ;
135.         //Vector3 V3_max = tr.position +  tr.rotation * ( new Vector3 (tr.localScale.x, tr.localScale.y, tr.localScale.z) * 0.5f ) ;
136.  
137.         // Debug.DrawLine ( V3_min - Vector3.forward * 50, V3_min + Vector3.forward * 100, Color.red ) ;
138.         // Debug.DrawLine ( V3_max - Vector3.forward * 50, V3_max + Vector3.forward * 100, Color.red ) ;
139.         // Debug.DrawLine ( V3_min - Vector3.right * 50, V3_min + Vector3.right * 100, Color.blue ) ;
140.         // Debug.DrawLine ( V3_max - Vector3.right * 50, V3_max + Vector3.right * 100, Color.blue ) ;
141.  
142.         // pass properties to an array
143.         Vector3 [] V3_a = new Vector3 [2] ;
144.         V3_a [0] = V3_min ;
145.         V3_a [1] = V3_max ;
146.  
147.         return V3_a ;
148.     }
149.  
150.     /// <summary>
151.     /// Axis Alligned Bounding Box collision 3D
152.     /// Returns true if collides
153.     /// </summary>
154.     /// <param name="V3_minMaxAABB"></param>
155.     /// <param name="V3_minMaxOBB"></param>
156.     /// <returns></returns>
157.     private bool _AABB2AABBCollisionTest ( Vector3 [] V3_minMaxAABB, Vector3 [] V3_minMaxOBB )
158.     {
159.         // 0 index is a min bounding vector
160.         // 1 index is a max bounding vector
161.                  
162.         if ( !_ABBAxisIsColliding ( V3_minMaxOBB [0].x, V3_minMaxOBB [1].x, V3_minMaxAABB [0].x, V3_minMaxAABB [1].x ) // is not colliding X
163.             || !_ABBAxisIsColliding ( V3_minMaxOBB [0].y, V3_minMaxOBB [1].y, V3_minMaxAABB [0].y, V3_minMaxAABB [1].y ) // is not colliding Y
164.             || !_ABBAxisIsColliding ( V3_minMaxOBB [0].z, V3_minMaxOBB [1].z, V3_minMaxAABB [0].z, V3_minMaxAABB [1].z ) // is not colliding Z
165.             )
166.         {
167.             return false ;
168.         }
169.          
170.         // is colliding
171.         return true ;
172.     }
173.  
174.     // is not colliding with selected axis
175.     private bool _ABBAxisIsColliding ( float f_minAABB, float f_maxAABB, float f_minOBBx, float f_maxOBB )
176.     {
177.         if ( f_maxOBB >= f_minAABB && f_minOBBx <= f_maxAABB )
178.         {
179.             return true ;
180.         }
181.  
182.         return false ;
183.     }
184.  
185.     // aCorn and bCorn are arrays containing all corners (vertices) of the two OBBs
186.     private static bool _IntersectsWhenProjected ( Vector3[] aCorn, Vector3[] bCorn, Vector3 V3_axis )
187.     {
188.  
189.         // Handles the cross product = {0,0,0} case
190.         if( V3_axis == Vector3.zero )
191.             return true;
192.  
193.         float aMin = float.MaxValue;
194.         float aMax = float.MinValue;
195.         float bMin = float.MaxValue;
196.         float bMax = float.MinValue;
197.  
198.         // Define two intervals, a and b. Calculate their min and max values
199.         for( int i = 0; i < 8; i++ )
200.         {
201.             //Debug.DrawLine ( aCorn [i], aCorn [i] + V3_axis * 100, Color.blue ) ;
202.             //Debug.DrawLine ( bCorn [i], bCorn [i] + V3_axis * 100, Color.red ) ;
203.  
204.             float aDist = Vector3.Dot( aCorn[i], V3_axis );
205.             aMin = ( aDist < aMin ) ? aDist : aMin;
206.             aMax = ( aDist > aMax ) ? aDist : aMax;
207.             float bDist = Vector3.Dot( bCorn[i], V3_axis );
208.             bMin = ( bDist < bMin ) ? bDist : bMin;
209.             bMax = ( bDist > bMax ) ? bDist : bMax;
210.         }
211.  
212.         // One-dimensional intersection test between a and b
213.         float longSpan = Mathf.Max( aMax, bMax ) - Mathf.Min( aMin, bMin );
214.         float sumSpan = aMax - aMin + bMax - bMin;
215.                  
216.  
217.         return longSpan < sumSpan; // Change this to <= if you want the case were they are touching but not overlapping, to count as an intersection
218.     }
219.  
220.  
221.     private bool _ABB2OBBCollisionTest ( Vector3 [] aabbCorners, Vector3 [] obbCorners )
222.     {
223.  
224.         Vector3 V3_frwAABB = Vector3.forward ;
225.         Vector3 V3_frwOBB = tr_cubeOBB.rotation * Vector3.forward ;
226.         Vector3 V3_frwFrw = Vector3.Cross ( V3_frwAABB, V3_frwOBB ) ;
227.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_frwFrw ) ) return false ;
228.         // Debug.Log ( "1" ) ;
229.  
230.         Vector3 V3_upAABB = Vector3.up ;
231.         Vector3 V3_upFrw = Vector3.Cross ( V3_upAABB, V3_frwOBB ) ;
232.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_upFrw ) ) return false ;
233.         // Debug.Log ( "2" ) ;
234.  
235.         Vector3 V3_rightAABB = Vector3.right ;
236.         Vector3 V3_rightFrw = Vector3.Cross ( V3_rightAABB, V3_frwOBB ) ;
237.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_rightFrw ) ) return false ;
238.         // Debug.Log ( "3" ) ;
239.  
240.         Vector3 V3_upOBB = tr_cubeOBB.rotation * Vector3.up ;
241.         Vector3 V3_upUp = Vector3.Cross ( V3_upAABB, V3_upOBB ) ;
242.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_upUp ) ) return false ;
243.         //Debug.Log ( "4" ) ;
244.          
245.         Vector3 V3_frwUp = Vector3.Cross ( V3_frwAABB, V3_upOBB ) ;
246.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_frwUp ) ) return false ;
247.         //Debug.Log ( "5" ) ;
248.  
249.         Vector3 V3_rightUp = Vector3.Cross ( V3_rightAABB, V3_upOBB ) ;
250.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_rightUp ) ) return false ;
251.         //Debug.Log ( "6" ) ;
252.  
253.         Vector3 V3_rightOBB = tr_cubeOBB.rotation * Vector3.right ;
254.         Vector3 V3_frwRight = Vector3.Cross ( V3_frwAABB, V3_rightOBB ) ;
255.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_frwRight ) ) return false ;
256.         //Debug.Log ( "7" ) ;
257.  
258.         Vector3 V3_upRight = Vector3.Cross ( V3_upAABB, V3_rightOBB ) ;
259.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_upRight ) ) return false ;
260.         //Debug.Log ( "8" ) ;
261.          
262.         Vector3 V3_rightRight = Vector3.Cross ( V3_rightAABB, V3_rightOBB ) ;
263.         if ( !_IntersectsWhenProjected ( aabbCorners, obbCorners, V3_rightRight ) ) return false ;
264.         //Debug.Log ( "9" ) ;
265.          
266.         return true ;
267.     }

[\spoiler]

Any assistance would be most welcome

Edit:
What I have should mention, I am using SAT (Separate Axis Theorem)