@@ -32,181 +32,181 @@ Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
32
32
33
33
namespace Box2D . NetStandard . Collision
34
34
{
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
+ }
212
212
}
0 commit comments