1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
// SPDX-FileCopyrightText: 2023 Erin Catto
// SPDX-License-Identifier: MIT
#include "aabb.h"
#include "box2d/math_functions.h"
#include <float.h>
bool b2AABB_IsValid( b2AABB a )
{
b2Vec2 d = b2Sub( a.upperBound, a.lowerBound );
bool valid = d.x >= 0.0f && d.y >= 0.0f;
valid = valid && b2Vec2_IsValid( a.lowerBound ) && b2Vec2_IsValid( a.upperBound );
return valid;
}
// From Real-time Collision Detection, p179.
b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 )
{
// Radius not handled
b2CastOutput output = { 0 };
float tmin = -FLT_MAX;
float tmax = FLT_MAX;
b2Vec2 p = p1;
b2Vec2 d = b2Sub( p2, p1 );
b2Vec2 absD = b2Abs( d );
b2Vec2 normal = b2Vec2_zero;
// x-coordinate
if ( absD.x < FLT_EPSILON )
{
// parallel
if ( p.x < a.lowerBound.x || a.upperBound.x < p.x )
{
return output;
}
}
else
{
float inv_d = 1.0f / d.x;
float t1 = ( a.lowerBound.x - p.x ) * inv_d;
float t2 = ( a.upperBound.x - p.x ) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if ( t1 > t2 )
{
float tmp = t1;
t1 = t2;
t2 = tmp;
s = 1.0f;
}
// Push the min up
if ( t1 > tmin )
{
normal.y = 0.0f;
normal.x = s;
tmin = t1;
}
// Pull the max down
tmax = b2MinFloat( tmax, t2 );
if ( tmin > tmax )
{
return output;
}
}
// y-coordinate
if ( absD.y < FLT_EPSILON )
{
// parallel
if ( p.y < a.lowerBound.y || a.upperBound.y < p.y )
{
return output;
}
}
else
{
float inv_d = 1.0f / d.y;
float t1 = ( a.lowerBound.y - p.y ) * inv_d;
float t2 = ( a.upperBound.y - p.y ) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if ( t1 > t2 )
{
float tmp = t1;
t1 = t2;
t2 = tmp;
s = 1.0f;
}
// Push the min up
if ( t1 > tmin )
{
normal.x = 0.0f;
normal.y = s;
tmin = t1;
}
// Pull the max down
tmax = b2MinFloat( tmax, t2 );
if ( tmin > tmax )
{
return output;
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if ( tmin < 0.0f || 1.0f < tmin )
{
return output;
}
// Intersection.
output.fraction = tmin;
output.normal = normal;
output.point = b2Lerp( p1, p2, tmin );
output.hit = true;
return output;
}
#if 0
bool b2TestOverlap( const b2Shape* shapeA, int32_t indexA,
const b2Shape* shapeB, int32_t indexB,
b2Transform xfA, b2Transform xfB)
{
b2DistanceInput input;
input->proxyA.Set(shapeA, indexA);
input->proxyB.Set(shapeB, indexB);
input->transformA = xfA;
input->transformB = xfB;
input->useRadii = true;
b2DistanceCache cache;
cache.count = 0;
b2DistanceOutput output;
b2Distance(&output, &cache, &input);
return output.distance < 10.0f * b2_epsilon;
}
#endif