1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#include "Math/BsCapsule.h"
4#include "Math/BsRay.h"
5
6namespace bs
7{
8 Capsule::Capsule(const LineSegment3& segment, float radius)
9 :mSegment(segment), mRadius(radius)
10 { }
11
12 std::pair<bool, float> Capsule::intersects(const Ray& ray) const
13 {
14 const Vector3& org = ray.getOrigin();
15 const Vector3& dir = ray.getDirection();
16
17 Vector3 segDir = mSegment.end - mSegment.start;
18 float segExtent = segDir.normalize() * 0.5f;
19 Vector3 segCenter = mSegment.start + segDir * segExtent;
20
21 Vector3 basis[3];
22 basis[0] = segDir;
23 basis[0].orthogonalComplement(basis[1], basis[2]);
24
25 float rSqr = mRadius * mRadius;
26
27 Vector3 diff = org - segCenter;
28 Vector3 P(basis[1].dot(diff), basis[2].dot(diff), basis[0].dot(diff));
29
30 // Get the z-value, in capsule coordinates, of the incoming line's
31 // unit-length direction.
32 float dz = basis[0].dot(dir);
33 if (std::abs(dz) == 1.0f)
34 {
35 // The line is parallel to the capsule axis. Determine whether the
36 // line intersects the capsule hemispheres.
37 float radialSqrDist = rSqr - P[0] * P[0] - P[1] * P[1];
38 if (radialSqrDist < 0.0f)
39 {
40 // The line is outside the cylinder of the capsule, so there is no
41 // intersection.
42 return std::make_pair(false, 0.0f);
43 }
44
45 // The line intersects the hemispherical caps.
46 float zOffset = std::sqrt(radialSqrDist) + segExtent;
47 if (dz > 0.0f)
48 return std::make_pair(true, -P[2] - zOffset);
49 else
50 return std::make_pair(true, P[2] - zOffset);
51 }
52
53 // Convert the incoming line unit-length direction to capsule coordinates.
54 Vector3 D(basis[1].dot(dir), basis[2].dot(dir), dz);
55
56 // Test intersection of line with infinite cylinder
57 float a0 = P[0] * P[0] + P[1] * P[1] - rSqr;
58 float a1 = P[0] * D[0] + P[1] * D[1];
59 float a2 = D[0] * D[0] + D[1] * D[1];
60 float discr = a1*a1 - a0*a2;
61
62 if (discr < 0.0f)
63 {
64 // The line does not intersect the infinite cylinder.
65 return std::make_pair(false, 0.0f);
66 }
67
68 float root, inv, tValue, zValue;
69 float nearestT = std::numeric_limits<float>::max();
70 bool foundOneIntersection = false;
71
72 if (discr > 0.0f)
73 {
74 // The line intersects the infinite cylinder in two places.
75 root = std::sqrt(discr);
76 inv = 1.0f / a2;
77
78 tValue = (-a1 - root)*inv;
79 zValue = P[2] + tValue*D[2];
80 if (std::abs(zValue) <= segExtent)
81 {
82 nearestT = tValue;
83 foundOneIntersection = true;
84 }
85
86 tValue = (-a1 + root)*inv;
87 zValue = P[2] + tValue*D[2];
88 if (std::abs(zValue) <= segExtent)
89 {
90 if (foundOneIntersection)
91 return std::make_pair(true, nearestT);
92 else
93 {
94 nearestT = tValue;
95 foundOneIntersection = true;
96 }
97 }
98 }
99 else
100 {
101 // The line is tangent to the infinite cylinder but intersects the
102 // cylinder in a single point.
103 tValue = -a1 / a2;
104 zValue = P[2] + tValue*D[2];
105 if (std::abs(zValue) <= segExtent)
106 return std::make_pair(true, tValue);
107 }
108
109 // Test intersection with bottom hemisphere.
110 float PZpE = P[2] + segExtent;
111 a1 += PZpE*D[2];
112 a0 += PZpE*PZpE;
113 discr = a1*a1 - a0;
114 if (discr > 0)
115 {
116 root = sqrt(discr);
117 tValue = -a1 - root;
118 zValue = P[2] + tValue*D[2];
119 if (zValue <= -segExtent)
120 {
121 if (foundOneIntersection)
122 return std::make_pair(true, nearestT < tValue ? nearestT : tValue);
123 else
124 {
125 nearestT = tValue;
126 foundOneIntersection = true;
127 }
128 }
129
130 tValue = -a1 + root;
131 zValue = P[2] + tValue*D[2];
132 if (zValue <= -segExtent)
133 {
134 if (foundOneIntersection)
135 return std::make_pair(true, nearestT < tValue ? nearestT : tValue);
136 else
137 {
138 nearestT = tValue;
139 foundOneIntersection = true;
140 }
141 }
142 }
143 else if (discr == 0.0f)
144 {
145 tValue = -a1;
146 zValue = P[2] + tValue*D[2];
147 if (zValue <= -segExtent)
148 {
149 if (foundOneIntersection)
150 return std::make_pair(true, nearestT < tValue ? nearestT : tValue);
151 else
152 {
153 nearestT = tValue;
154 foundOneIntersection = true;
155 }
156 }
157 }
158
159 // Test intersection with top hemisphere
160 a1 -= 2.0f*segExtent*D[2];
161 a0 -= 4.0f*segExtent*P[2];
162 discr = a1*a1 - a0;
163 if (discr > 0.0f)
164 {
165 root = sqrt(discr);
166 tValue = -a1 - root;
167 zValue = P[2] + tValue*D[2];
168 if (zValue >= segExtent)
169 {
170 if (foundOneIntersection)
171 return std::make_pair(true, nearestT < tValue ? nearestT : tValue);
172 else
173 {
174 nearestT = tValue;
175 foundOneIntersection = true;
176 }
177 }
178
179 tValue = -a1 + root;
180 zValue = P[2] + tValue*D[2];
181 if (zValue >= segExtent)
182 {
183 if (foundOneIntersection)
184 return std::make_pair(true, nearestT < tValue ? nearestT : tValue);
185 else
186 {
187 nearestT = tValue;
188 foundOneIntersection = true;
189 }
190 }
191 }
192 else if (discr == 0.0f)
193 {
194 tValue = -a1;
195 zValue = P[2] + tValue*D[2];
196 if (zValue >= segExtent)
197 {
198 if (foundOneIntersection)
199 return std::make_pair(true, nearestT < tValue ? nearestT : tValue);
200 else
201 {
202 nearestT = tValue;
203 foundOneIntersection = true;
204 }
205 }
206 }
207
208 if (foundOneIntersection)
209 return std::make_pair(true, nearestT);
210
211 return std::make_pair(false, 0.0f);
212 }
213}
214