@@ -32,181 +32,181 @@ Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
3232
3333namespace Box2D . NetStandard . Collision
3434{
35- /// <summary>
36- /// An axis aligned bounding box.
37- /// </summary>
38- public struct AABB
39- {
40- /// <summary>
41- /// The lower vertex
42- /// </summary>
43- internal Vector2 lowerBound ;
44-
45- /// <summary>
46- /// The upper vertex
47- /// </summary>
48- internal Vector2 upperBound ;
49-
50- public Vector2 LowerBound
51- {
52- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
53- get => lowerBound ;
54- }
55-
56- public Vector2 UpperBound
57- {
58- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
59- get => upperBound ;
60- }
61-
62- public Vector2 Size
63- {
64- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
65- get => upperBound - lowerBound ;
66- }
67-
68- /// Get the center of the AABB.
69- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
70- public Vector2 GetCenter ( ) => 0.5f * ( lowerBound + upperBound ) ;
71-
72- /// Get the extents of the AABB (half-widths).
73- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
74- public Vector2 GetExtents ( ) => 0.5f * ( upperBound - lowerBound ) ;
75-
76- /// Get the perimeter length
77- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
78- internal float GetPerimeter ( )
79- {
80- float wx = upperBound . X - lowerBound . X ;
81- float wy = upperBound . Y - lowerBound . Y ;
82- return 2.0f * ( wx + wy ) ;
83- }
84-
85- /// Combine an AABB into this one.
86- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
87- internal void Combine ( in AABB aabb )
88- {
89- lowerBound = Vector2 . Min ( lowerBound , aabb . lowerBound ) ;
90- upperBound = Vector2 . Max ( upperBound , aabb . upperBound ) ;
91- }
92-
93- /// Combine two AABBs into this one.
94- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
95- internal static AABB Combine ( in AABB aabb1 , in AABB aabb2 )
96- {
97- AABB result = default ;
98- result . lowerBound = Vector2 . Min ( aabb1 . lowerBound , aabb2 . lowerBound ) ;
99- result . upperBound = Vector2 . Max ( aabb1 . upperBound , aabb2 . upperBound ) ;
100- return result ;
101- }
102-
103- internal AABB Enlarged ( float amount )
104- {
105- Vector2 vecAmt = new Vector2 ( amount ) ;
106- return new AABB ( lowerBound - vecAmt , upperBound + vecAmt ) ;
107- }
108-
109- internal bool Intersects ( in AABB other )
110- {
111- return other . lowerBound . Y <= this . upperBound . Y &&
112- other . upperBound . Y >= this . lowerBound . Y &&
113- other . upperBound . X >= this . lowerBound . X &&
114- other . lowerBound . X <= this . upperBound . X ;
115- }
116-
117- /// Does this aabb contain the provided AABB.
118- [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
119- internal bool Contains ( in AABB aabb )
120- {
121- var result = true ;
122- result = result && lowerBound . X <= aabb . lowerBound . X ;
123- result = result && lowerBound . Y <= aabb . lowerBound . Y ;
124- result = result && aabb . upperBound . X <= upperBound . X ;
125- result = result && aabb . upperBound . Y <= upperBound . Y ;
126- return result ;
127- }
128-
129- private bool RayCast ( out RayCastOutput output , in RayCastInput input )
130- {
131- output = default ;
132- float tmin = float . MinValue ;
133- float tmax = float . MaxValue ;
134-
135- Vector2 p = input . p1 ;
136- Vector2 d = input . p2 - input . p1 ;
137- var absD = Vector2 . Abs ( d ) ;
138-
139- Vector2 normal = Vector2 . Zero ;
140-
141- for ( var i = 0 ; i < 2 ; ++ i )
142- {
143- if ( absD . GetIdx ( i ) < Settings . FLT_EPSILON )
144- {
145- // Parallel.
146- if ( p . GetIdx ( i ) < lowerBound . GetIdx ( i ) || upperBound . GetIdx ( i ) < p . GetIdx ( i ) )
147- {
148- return false ;
149- }
150- }
151- else
152- {
153- float inv_d = 1.0f / d . GetIdx ( i ) ;
154- float t1 = ( lowerBound . GetIdx ( i ) - p . GetIdx ( i ) ) * inv_d ;
155- float t2 = ( upperBound . GetIdx ( i ) - p . GetIdx ( i ) ) * inv_d ;
156-
157- // Sign of the normal vector.
158- float s = - 1.0f ;
159-
160- if ( t1 > t2 )
161- {
162- float temp = t1 ;
163- t1 = t2 ;
164- t2 = temp ;
165- s = 1.0f ;
166- }
167-
168- // Push the min up
169- if ( t1 > tmin )
170- {
171- normal = new Vector2 ( i == 0 ? s : 0 , i == 1 ? s : 0 ) ;
172- tmin = t1 ;
173- }
174-
175- // Pull the max down
176- tmax = MathF . Min ( tmax , t2 ) ;
177-
178- if ( tmin > tmax )
179- {
180- return false ;
181- }
182- }
183- }
184-
185- // Does the ray start inside the box?
186- // Does the ray intersect beyond the max fraction?
187- if ( tmin < 0.0f || input . maxFraction < tmin )
188- {
189- return false ;
190- }
191-
192- // Intersection.
193- output . fraction = tmin ;
194- output . normal = normal ;
195- return true ;
196- }
197-
198- private bool IsValid ( )
199- {
200- Vector2 d = upperBound - lowerBound ;
201- bool valid = d . X >= 0.0f && d . Y >= 0.0f ;
202- valid = valid && lowerBound . IsValid ( ) && upperBound . IsValid ( ) ;
203- return valid ;
204- }
205-
206- public AABB ( Vector2 lowerBound , Vector2 upperBound )
207- {
208- this . lowerBound = lowerBound ;
209- this . upperBound = upperBound ;
210- }
211- }
35+ /// <summary>
36+ /// An axis aligned bounding box.
37+ /// </summary>
38+ public struct AABB
39+ {
40+ /// <summary>
41+ /// The lower vertex
42+ /// </summary>
43+ internal Vector2 lowerBound ;
44+
45+ /// <summary>
46+ /// The upper vertex
47+ /// </summary>
48+ internal Vector2 upperBound ;
49+
50+ public Vector2 LowerBound
51+ {
52+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
53+ get => lowerBound ;
54+ }
55+
56+ public Vector2 UpperBound
57+ {
58+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
59+ get => upperBound ;
60+ }
61+
62+ public Vector2 Size
63+ {
64+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
65+ get => upperBound - lowerBound ;
66+ }
67+
68+ /// Get the center of the AABB.
69+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
70+ public Vector2 GetCenter ( ) => 0.5f * ( lowerBound + upperBound ) ;
71+
72+ /// Get the extents of the AABB (half-widths).
73+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
74+ public Vector2 GetExtents ( ) => 0.5f * ( upperBound - lowerBound ) ;
75+
76+ /// Get the perimeter length
77+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
78+ internal float GetPerimeter ( )
79+ {
80+ float wx = upperBound . X - lowerBound . X ;
81+ float wy = upperBound . Y - lowerBound . Y ;
82+ return 2.0f * ( wx + wy ) ;
83+ }
84+
85+ /// Combine an AABB into this one.
86+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
87+ internal void Combine ( in AABB aabb )
88+ {
89+ lowerBound = Vector2 . Min ( lowerBound , aabb . lowerBound ) ;
90+ upperBound = Vector2 . Max ( upperBound , aabb . upperBound ) ;
91+ }
92+
93+ /// Combine two AABBs into this one.
94+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
95+ internal static AABB Combine ( in AABB aabb1 , in AABB aabb2 )
96+ {
97+ AABB result = default ;
98+ result . lowerBound = Vector2 . Min ( aabb1 . lowerBound , aabb2 . lowerBound ) ;
99+ result . upperBound = Vector2 . Max ( aabb1 . upperBound , aabb2 . upperBound ) ;
100+ return result ;
101+ }
102+
103+ internal AABB Enlarged ( float amount )
104+ {
105+ Vector2 vecAmt = new Vector2 ( amount ) ;
106+ return new AABB ( lowerBound - vecAmt , upperBound + vecAmt ) ;
107+ }
108+
109+ internal bool Intersects ( in AABB other )
110+ {
111+ return other . lowerBound . Y <= this . upperBound . Y &&
112+ other . upperBound . Y >= this . lowerBound . Y &&
113+ other . upperBound . X >= this . lowerBound . X &&
114+ other . lowerBound . X <= this . upperBound . X ;
115+ }
116+
117+ /// Does this aabb contain the provided AABB.
118+ [ MethodImpl ( MethodImplOptions . AggressiveInlining ) ]
119+ internal bool Contains ( in AABB aabb )
120+ {
121+ var result = true ;
122+ result = result && lowerBound . X <= aabb . lowerBound . X ;
123+ result = result && lowerBound . Y <= aabb . lowerBound . Y ;
124+ result = result && aabb . upperBound . X <= upperBound . X ;
125+ result = result && aabb . upperBound . Y <= upperBound . Y ;
126+ return result ;
127+ }
128+
129+ private bool RayCast ( out RayCastOutput output , in RayCastInput input )
130+ {
131+ output = default ;
132+ float tmin = float . MinValue ;
133+ float tmax = float . MaxValue ;
134+
135+ Vector2 p = input . p1 ;
136+ Vector2 d = input . p2 - input . p1 ;
137+ var absD = Vector2 . Abs ( d ) ;
138+
139+ Vector2 normal = Vector2 . Zero ;
140+
141+ for ( var i = 0 ; i < 2 ; ++ i )
142+ {
143+ if ( absD . GetIdx ( i ) < Settings . FLT_EPSILON )
144+ {
145+ // Parallel.
146+ if ( p . GetIdx ( i ) < lowerBound . GetIdx ( i ) || upperBound . GetIdx ( i ) < p . GetIdx ( i ) )
147+ {
148+ return false ;
149+ }
150+ }
151+ else
152+ {
153+ float inv_d = 1.0f / d . GetIdx ( i ) ;
154+ float t1 = ( lowerBound . GetIdx ( i ) - p . GetIdx ( i ) ) * inv_d ;
155+ float t2 = ( upperBound . GetIdx ( i ) - p . GetIdx ( i ) ) * inv_d ;
156+
157+ // Sign of the normal vector.
158+ float s = - 1.0f ;
159+
160+ if ( t1 > t2 )
161+ {
162+ float temp = t1 ;
163+ t1 = t2 ;
164+ t2 = temp ;
165+ s = 1.0f ;
166+ }
167+
168+ // Push the min up
169+ if ( t1 > tmin )
170+ {
171+ normal = new Vector2 ( i == 0 ? s : 0 , i == 1 ? s : 0 ) ;
172+ tmin = t1 ;
173+ }
174+
175+ // Pull the max down
176+ tmax = MathF . Min ( tmax , t2 ) ;
177+
178+ if ( tmin > tmax )
179+ {
180+ return false ;
181+ }
182+ }
183+ }
184+
185+ // Does the ray start inside the box?
186+ // Does the ray intersect beyond the max fraction?
187+ if ( tmin < 0.0f || input . maxFraction < tmin )
188+ {
189+ return false ;
190+ }
191+
192+ // Intersection.
193+ output . fraction = tmin ;
194+ output . normal = normal ;
195+ return true ;
196+ }
197+
198+ private bool IsValid ( )
199+ {
200+ Vector2 d = upperBound - lowerBound ;
201+ bool valid = d . X >= 0.0f && d . Y >= 0.0f ;
202+ valid = valid && lowerBound . IsValid ( ) && upperBound . IsValid ( ) ;
203+ return valid ;
204+ }
205+
206+ public AABB ( Vector2 lowerBound , Vector2 upperBound )
207+ {
208+ this . lowerBound = lowerBound ;
209+ this . upperBound = upperBound ;
210+ }
211+ }
212212}
0 commit comments