1// tGeometry.cpp 
2// 
3// Idealized geometric shapes. These include triangles, circles, boxes, spheres, rays, lines, planes, cylinders, 
4// capsules, frustums, and ellipses. When the shape primitives have a number in their name, it refers to the space the 
5// shape is in, not the dimensionality of the shape itself. eg. A tCircle3 is a (2D) circle in R3 while a tCircle2 is a 
6// 2D circle in R2. For shapes that are in R3 we drop the 3 because the R3 primitives are more general. 
7// 
8// Copyright (c) 2006, 2016 Tristan Grimmer. 
9// Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby 
10// granted, provided that the above copyright notice and this permission notice appear in all copies. 
11// 
12// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL 
13// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, 
14// INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN 
15// AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR 
16// PERFORMANCE OF THIS SOFTWARE. 
17 
18#include "Math/tGeometry.h" 
19 
20 
21namespace tMath 
22{ 
23 static tIntersectResult IntersectFindLineLineHelper(const tLine2& a, const tLine2& b, float& ua, float& ub); 
24}; 
25 
26 
27bool tMath::tORect2::IsInside(const tVector2& point) const 
28{ 
29 tVector2 d = point - Center; 
30 for (int a = 0; a < 2; a++) 
31 { 
32 float dot = tAbs(d * Axes[a]); 
33 if (dot > Extents.E[a]) 
34 return false; 
35 } 
36 
37 return true; 
38} 
39 
40 
41void tMath::tABox::Transform(const tMatrix4& t) 
42{ 
43 // Transform the 8 box points to the new space and grow a new box around them. 
44 tABox box; 
45 box.AddPoint( (t * tVector4(Min.x, Min.y, Min.z, 1.0f)).GetCartesian() ); 
46 box.AddPoint( (t * tVector4(Max.x, Min.y, Min.z, 1.0f)).GetCartesian() ); 
47 box.AddPoint( (t * tVector4(Min.x, Max.y, Min.z, 1.0f)).GetCartesian() ); 
48 box.AddPoint( (t * tVector4(Max.x, Max.y, Min.z, 1.0f)).GetCartesian() ); 
49 box.AddPoint( (t * tVector4(Min.x, Min.y, Max.z, 1.0f)).GetCartesian() ); 
50 box.AddPoint( (t * tVector4(Max.x, Min.y, Max.z, 1.0f)).GetCartesian() ); 
51 box.AddPoint( (t * tVector4(Min.x, Max.y, Max.z, 1.0f)).GetCartesian() ); 
52 box.AddPoint( (t * tVector4(Max.x, Max.y, Max.z, 1.0f)).GetCartesian() ); 
53 
54 *this = box; 
55} 
56 
57 
58bool tMath::tOBox::IsInside(const tVector3& point) const 
59{ 
60 tVector3 d = point - Center; 
61 for (int a = 0; a < 3; a++) 
62 { 
63 float dot = tAbs(d * Axes[a]); 
64 if (dot > Extents.E[a]) 
65 return false; 
66 } 
67 
68 return true; 
69} 
70 
71 
72void tMath::tPlane::ComputeOrthogonalBasisVectors(tVector3& u, tVector3& v) const 
73{ 
74 // The algorithm is 'householder orthogonalization' as described here: 
75 // http://math.stackexchange.com/questions/64430/find-extra-arbitrary-two-points-for-a-plane-given-the-normal-and-a-point-that-l 
76 tVector3 n = Normal; 
77 n.Normalize(); 
78 
79 // Compute the 'mirror' vector w = (nx+1,ny,nz). 
80 tVector3 w = n; 
81 w.x += 1.0f; 
82 
83 // Compute the 3x3 Householder matrix H = I - 2(wwT/wTw) where T means transpose. 
84 tMatrix4 wwT = w.MulByTranspose(w); 
85 float wTw = w*w; 
86 tMatrix4 H = tMatrix4::identity - 2.0f*(wwT/wTw); 
87 
88 // Row1 of H will be a unit vector parallel to n. Row 2 and 3 will be unit vectors orthogonal to n and each other. 
89 // We transpose to get the rows out easily. 
90 H.Transpose(); 
91 u = H.C2; 
92 v = H.C3; 
93} 
94 
95 
96void tMath::tFrustum::Set(const tMatrix4& mat) 
97{  
98 // We put the matrix in row major format so we can do the trick described 
99 // in Akenine-Moller&Haines, Real-Time Rendering, 2nd Ed., pp.613-614. 
100 tMatrix4 m(mat); 
101 m.Transpose(); 
102 tVector4 t; 
103 
104 tSub(t, m.C4, m.C1); Planes[Plane_Right].Set(t); 
105 tAdd(t, m.C4, m.C1); Planes[Plane_Left].Set(t); 
106 tSub(t, m.C4, m.C2); Planes[Plane_Top].Set(t); 
107 tAdd(t, m.C4, m.C2); Planes[Plane_Bottom].Set(t); 
108 tSub(t, m.C4, m.C3); Planes[Plane_Far].Set(t); 
109 tAdd(t, m.C4, m.C3); Planes[Plane_Near].Set(t); 
110 
111 for (int p = 0; p < Plane_NumPlanes; p++) 
112 Planes[p].Normalize(); 
113} 
114 
115 
116bool tMath::tComputeSmallEnclosingSphere(tSphere& sphere, const tVector3* points, int numPoints, float minRadius) 
117{ 
118 if (!points || (numPoints <= 0)) 
119 return false; 
120 
121 if (minRadius < Epsilon) 
122 minRadius = Epsilon; 
123 
124 float& radius = sphere.Radius; 
125 tVector3& center = sphere.Center; 
126 
127 radius = minRadius; 
128 center = points[0]; 
129 if (numPoints == 1) 
130 return true; 
131 
132 for (int t = 0; t < 2; t++) 
133 { 
134 for (int p = 0; p < numPoints; p++) 
135 { 
136 float distSq = (points[p] - center).LengthSq(); 
137 float radSq = radius*radius; 
138 if (distSq > radSq) 
139 { 
140 float dist = tSqrt(distSq); 
141 float param = dist / radius; 
142 float paramInv = 1.0f/param; 
143 float paramInvSq = paramInv*paramInv; 
144 float onePlus = 1.0f + paramInvSq; 
145 float oneMinus = 1.0f - paramInvSq; 
146 
147 radius = 0.5f * (param + paramInv) * radius; 
148 center = (onePlus*center + oneMinus*points[p]) / 2.0f; 
149 } 
150 } 
151 } 
152 
153 for (int p = 0; p < numPoints; p++) 
154 { 
155 tVector3 diff = points[p] - center; 
156 float distSq = diff.LengthSq(); 
157 float radSq = radius*radius; 
158 if (distSq > radSq) 
159 { 
160 float dist = tSqrt(distSq); 
161 radius = (radius + dist) / 2.0f; 
162 center += diff * (dist - radius) / dist; 
163 } 
164 } 
165 
166 return true; 
167} 
168 
169 
170bool tMath::tComputeSmallEnclosingSphere_Unstable(tSphere& sphere, const tVector3* points, int numPoints) 
171{ 
172 if (!points || (numPoints <= 0)) 
173 return false; 
174 
175 float& radius = sphere.Radius; 
176 tVector3& center = sphere.Center; 
177 if (numPoints == 1) 
178 { 
179 radius = 0.0f; 
180 center = points[0]; 
181 return true; 
182 } 
183 
184 tVector3 xmin(PosInfinity, PosInfinity, PosInfinity); 
185 tVector3 xmax(NegInfinity, NegInfinity, NegInfinity); 
186 tVector3 ymin(PosInfinity, PosInfinity, PosInfinity); 
187 tVector3 ymax(NegInfinity, NegInfinity, NegInfinity); 
188 tVector3 zmin(PosInfinity, PosInfinity, PosInfinity); 
189 tVector3 zmax(NegInfinity, NegInfinity, NegInfinity); 
190 for (int p = 0; p < numPoints; p++) 
191 { 
192 const tVector3& point = points[p]; 
193 if (xmin.x > point.x) 
194 xmin = point; 
195 
196 if (xmax.x < point.x) 
197 xmax = point; 
198 
199 if (ymin.y > point.y) 
200 ymin = point; 
201 
202 if (ymax.y < point.y) 
203 ymax = point; 
204 
205 if (zmin.z > point.z) 
206 zmin = point; 
207 
208 if (zmax.z < point.z) 
209 zmax = point; 
210 } 
211 
212 // Span the distance between min and max squared. 
213 float xspan = (xmax - xmin).LengthSq(); 
214 float yspan = (ymax - ymin).LengthSq(); 
215 float zspan = (zmax - zmin).LengthSq(); 
216 
217 // Find maximally separated pair. 
218 tVector3 diam1(xmin); 
219 tVector3 diam2(xmax); 
220 float maxspan = xspan; 
221 
222 if (yspan > maxspan) 
223 { 
224 maxspan = yspan; 
225 diam1 = ymin; 
226 diam2 = ymax; 
227 } 
228 
229 if (zspan > maxspan) 
230 { 
231 diam1 = zmin; 
232 diam2 = zmax; 
233 } 
234 
235 // Calculate initial center. 
236 center.Set( (diam1.x+diam2.x)*0.5f, (diam1.y+diam2.y)*0.5f, (diam1.z+diam2.z)*0.5f ); 
237 
238 // Calculate initial radius. 
239 tVector3 radVec(diam2.x - center.x, diam2.y - center.y, diam2.z - center.z); 
240 float radSq = radVec.LengthSq(); 
241 radius = tSqrt(radSq); 
242 
243 // Second pass. Increment current sphere. 
244 for (int p = 0; p < numPoints; p++) 
245 { 
246 const tVector3& point = points[p]; 
247 tVector3 diff = point - center; 
248 float distSq = diff.LengthSq(); 
249 
250 // DistSq is guaranteed to be > 0 so the divide is safe. 
251 if (distSq > radSq) 
252 { 
253 float dist = tSqrt(distSq); 
254 radius = (radius + dist) / 2.0f; 
255 float diff = dist - radius; 
256 center = (center*radius + point*diff) / dist; 
257 } 
258 } 
259 
260 return true; 
261} 
262 
263 
264float tMath::tDistanceToLine(const tVector3& p, const tLine& l) 
265{ 
266 tVector3 u = l.B - l.A; 
267 u.Normalize(); 
268 tVector3 w = p - l.A; 
269 u %= w; // Cross product. 
270 
271 return u.Length(); 
272} 
273 
274 
275tMath::tIntersectResult tMath::IntersectFindLineLineHelper(const tLine2& a, const tLine2& b, float& ua, float& ub) 
276{ 
277 // Based on http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/ 
278 float x4mx3 = b.B.x - b.A.x; 
279 float y1my3 = a.A.y - b.A.y; 
280 float y4my3 = b.B.y - b.A.y; 
281 float x1mx3 = a.A.x - b.A.x; 
282 float x2mx1 = a.B.x - a.A.x; 
283 float y2my1 = a.B.y - a.A.y; 
284 
285 float d = y4my3*x2mx1 - x4mx3*y2my1; 
286 float na = x4mx3*y1my3 - y4my3*x1mx3; 
287 float nb = x2mx1*y1my3 - y2my1*x1mx3; 
288 if (d == 0.0f) 
289 { 
290 // We're parallel, possibly coincident. 
291 if ((na == 0.0f) || (nb == 0.0f)) 
292 return tIntersectResult::Coincident; 
293 else 
294 return tIntersectResult::Parallel; 
295 } 
296 
297 ua = na/d; 
298 ub = nb/d; 
299 return tIntersectResult::Point; 
300} 
301 
302 
303tMath::tIntersectResult tMath::tIntersectFindLineLine(const tLine2& a, const tLine2& b, tVector2& i) 
304{ 
305 float ua, ub; 
306 tIntersectResult r = IntersectFindLineLineHelper(a, b, ua, ub); 
307 if (r != tIntersectResult::Point) 
308 return r; 
309 
310 i = a.A + ua*(a.B - a.A); 
311 return tIntersectResult::Point; 
312} 
313 
314 
315tMath::tIntersectResult tMath::tIntersectFindSegSeg(const tLineSeg2& a, const tLineSeg2& b, tVector2& i) 
316{ 
317 float ua, ub; 
318 tIntersectResult r = IntersectFindLineLineHelper(a, b, ua, ub); 
319 if (r != tIntersectResult::Point) 
320 return r; 
321 
322 if (tInRange(ua, 0.0f, 1.0f) && tInRange(ub, 0.0f, 1.0f)) 
323 { 
324 i = a.A + ua*(a.B - a.A); 
325 return tIntersectResult::Point; 
326 } 
327 
328 return tIntersectResult::None; 
329} 
330 
331 
332tMath::tIntersectResult tMath::tIntersectFindRaySeg(const tRay2& r, const tLineSeg2& b, tVector2& i) 
333{ 
334 tLine2 a(r.Start, r.Start+r.Dir); 
335 
336 float ua, ub; 
337 tIntersectResult res = IntersectFindLineLineHelper(a, b, ua, ub); 
338 if (res != tIntersectResult::Point) 
339 return res; 
340 
341 if ((ua >= 0.0f) && tInRange(ub, 0.0f, 1.0f)) 
342 { 
343 i = a.A + ua*(a.B - a.A); 
344 return tIntersectResult::Point; 
345 } 
346 
347 return tIntersectResult::None; 
348} 
349 
350 
351int tMath::tIntersectFindRayRect(const tRay2& r, const tARect2& b, tVector2 ia[2], uint32* sides) 
352{ 
353 int numIntersections = 0; 
354 
355 // The box is made of 4 segments that we test in turn. 
356 tVector2 tl(b.Min.x, b.Max.y); 
357 tVector2 br(b.Max.x, b.Min.y); 
358 tLineSeg2 seg; 
359 tVector2 i; 
360 if (sides) 
361 *sides = 0; 
362 
363 // Bottom segment. 
364 seg.A = b.Min; 
365 seg.B = br; 
366 if (tIntersectFindRaySeg(r, seg, i) == tIntersectResult::Point) 
367 { 
368 ia[numIntersections++] = i; 
369 if (sides) 
370 *sides |= tARect2::Side_Bottom; 
371 } 
372 
373 // Right segment. 
374 seg.A = br; 
375 seg.B = b.Max; 
376 if (tIntersectFindRaySeg(r, seg, i) == tIntersectResult::Point) 
377 { 
378 ia[numIntersections++] = i; 
379 if (sides) 
380 *sides |= tARect2::Side_Right; 
381 } 
382 if (numIntersections == 2) 
383 return numIntersections; 
384 
385 // Top segment. 
386 seg.A = b.Max; 
387 seg.B = tl; 
388 if (tIntersectFindRaySeg(r, seg, i) == tIntersectResult::Point) 
389 { 
390 ia[numIntersections++] = i; 
391 if (sides) 
392 *sides |= tARect2::Side_Top; 
393 } 
394 if (numIntersections == 2) 
395 return numIntersections; 
396 
397 // Left segment. 
398 seg.A = tl; 
399 seg.B = b.Min; 
400 if (tIntersectFindRaySeg(r, seg, i) == tIntersectResult::Point) 
401 { 
402 ia[numIntersections++] = i; 
403 if (sides) 
404 *sides |= tARect2::Side_Left; 
405 } 
406 
407 return numIntersections; 
408} 
409 
410 
411bool tMath::tIntersectFindRaySphere(float& t, const tRay& ray, const tSphere& sphere) 
412{ 
413 const tVector3& p = ray.Start; 
414 const tVector3& d = ray.Dir; 
415 const tVector3& c = sphere.Center; 
416 const float& r = sphere.Radius; 
417 
418 // rayDir must have length 1. Solution is quadratic equation with A=1 and B, C as below. 
419 float B = 2.0f*( d.x*(p.x-c.x) + d.y*(p.y-c.y) + d.z*(p.z-c.z) ); 
420 float C = (p.x-c.x)*(p.x-c.x) + (p.y-c.y)*(p.y-c.y) + (p.z-c.z)*(p.z-c.z) - r*r; 
421 
422 float discriminant = B*B - 4.0f*C; 
423 if (discriminant < 0.0f) 
424 return false; 
425 
426 float root = tSqrt(discriminant); 
427 
428 t = (-B - root)/2.0f; 
429 
430 // May need the other root. 
431 if (t < 0.0f) 
432 t = (-B + root)/2.0f; 
433 
434 if (t < 0.0f) 
435 // The sphere is behind us. 
436 return false; 
437 
438 // The intersection = p + d*t. 
439 // t0, t1 = (- B + (B^2 - 4*C)^1/2) / 2 where t0 is for (-) and t1 is for (+). 
440 // If the discriminant is < 0.0 then there is no real root and no intersection. If there is a real root 
441 // (discriminant > = 0.0) then the smaller positive root is the closest intersection point. We compute t0 and if it 
442 // is positive we are done. If it is negative we compute t1. The intersection point is given by:  
443 // Ri = [xi, yi, zi] = [x0 + xd * ti , y0 + yd * ti, z0 + zd * ti] 
444 return true; 
445} 
446 
447 
448bool tMath::tIntersectTestLineSegSphere(const tLineSeg& segmentAB, const tSphere& sphere) 
449{ 
450 tRay rayAB(segmentAB); 
451 tLineSeg segmentBA(segmentAB.B, segmentAB.A); 
452 tRay rayBA(segmentBA); 
453 
454 float u, v; 
455 if (tIntersectFindRaySphere(u, rayAB, sphere) && tIntersectFindRaySphere(v, rayBA, sphere)) 
456 return true; 
457 else 
458 return false; 
459} 
460 
461 
462bool tMath::tIntersectFindRayEllipse(float& t, const tRay2& ray, const tEllipse2& ellipse) 
463{ 
464 float r1 = ellipse.RadiusX; 
465 float r2 = ellipse.RadiusY; 
466 tAssert(r1 && r2); 
467 
468 float dx = ray.Dir.x; 
469 float dy = ray.Dir.y; 
470 tAssert(dx || dy); 
471 
472 float discriminant = 1.0f/( (dx*dx/(r1*r1)) + (dy*dy/(r2*r2)) ); 
473 if (discriminant < 0.0f) 
474 return false; 
475 
476 t = tSqrt(discriminant); 
477 return true; 
478} 
479 
480 
481bool tMath::tIntersectTestRayTriangle(const tRay& ray, const tTriangle& tri) 
482{ 
483 // A reconditioned version of the Segura-Feito method. 
484 tVector3 a = tri.A - ray.Start; 
485 tVector3 b = tri.B - ray.Start; 
486 tVector3 c = tri.C - ray.Start; 
487 
488 tVector3 t; 
489 tCross(t, a, ray.Dir); 
490 if (c*t > 0.0f) 
491 return false; 
492 
493 if (b*t < 0.0f) 
494 return false; 
495 
496 tCross(t, b, ray.Dir); 
497 if (c*t < 0.0f) 
498 return false; 
499 
500 return true; 
501} 
502 
503 
504bool tMath::tIntersectTestFrustumSphere(const tFrustum& f, const tSphere& s) 
505{ 
506 for (int p = 0; p < int(tFrustum::Plane_NumPlanes); p++) 
507 { 
508 // Use the sphere's center in the normalized plane equation to get the projected distance to plane. 
509 if (s.Center * f.Planes[p].Normal + f.Planes[p].Distance < -s.Radius) 
510 return false; 
511 } 
512 
513 return true; 
514} 
515