1 /* This file is part of the Spring engine (GPL v2 or later), see LICENSE.html */
2
3
4 #include "CollisionHandler.h"
5 #include "CollisionVolume.h"
6 #include "Rendering/Models/3DModel.h"
7 #include "Sim/Units/Unit.h"
8 #include "Sim/Features/Feature.h"
9 #include "Sim/Misc/GroundBlockingObjectMap.h"
10 #include "Sim/Misc/GlobalSynced.h"
11 #include "Sim/Misc/GlobalConstants.h"
12 #include "System/FastMath.h"
13 #include "System/Matrix44f.h"
14 #include "System/Log/ILog.h"
15
16 unsigned int CCollisionHandler::numDiscTests = 0;
17 unsigned int CCollisionHandler::numContTests = 0;
18
19
20
PrintStats()21 void CCollisionHandler::PrintStats()
22 {
23 LOG("[CCollisionHandler] dis-/continuous tests: %i/%i", numDiscTests, numContTests);
24 }
25
26
27
DetectHit(const CSolidObject * o,const float3 p0,const float3 p1,CollisionQuery * cq,bool forceTrace)28 bool CCollisionHandler::DetectHit(const CSolidObject* o, const float3 p0, const float3 p1, CollisionQuery* cq, bool forceTrace)
29 {
30 return (DetectHit(o->collisionVolume, o, p0, p1, cq, forceTrace));
31 }
32
DetectHit(const CollisionVolume * v,const CSolidObject * o,const float3 p0,const float3 p1,CollisionQuery * cq,bool forceTrace)33 bool CCollisionHandler::DetectHit(const CollisionVolume* v, const CSolidObject* o, const float3 p0, const float3 p1, CollisionQuery* cq, bool forceTrace)
34 {
35 bool hit = false;
36
37 if (cq != NULL)
38 cq->Reset();
39
40 if (o->IsInVoid())
41 return hit;
42
43 // test *only* for ray intersections with the piece tree
44 // (whether or not the unit's regular volume is disabled)
45 //
46 // overrides forceTrace, which itself overrides testType
47 // needs a unit because only units have a LocalModel atm
48 //
49 // NOTE:
50 // usePieceCollisionVolumes is parsed in SolidObjectDef
51 // so features *can* also set it, which will crash here
52 // (prevented by FeatureHandler)
53 if (v->DefaultToPieceTree())
54 return (CCollisionHandler::IntersectPieceTree(static_cast<const CUnit*>(o), p0, p1, cq));
55 if (v->IgnoreHits())
56 return hit;
57
58 if (forceTrace || v->UseContHitTest()) {
59 hit = CCollisionHandler::Intersect(v, o, p0, p1, cq);
60 } else {
61 // Collision() does not need p1 (no ray, no ray-endpoint)
62 hit = CCollisionHandler::Collision(v, o, p0, cq);
63 }
64
65 return hit;
66 }
67
68
69
Collision(const CollisionVolume * v,const CSolidObject * o,const float3 p,CollisionQuery * cq)70 bool CCollisionHandler::Collision(const CollisionVolume* v, const CSolidObject* o, const float3 p, CollisionQuery* cq)
71 {
72 bool hit = false;
73
74 // if <v> is a sphere, then the bounding radius is just its own radius -->
75 // we do not need to test the COLVOL_TYPE_SPHERE case again when this fails
76 if ((v->GetWorldSpacePos(o) - p).SqLength() > v->GetBoundingRadiusSq())
77 return hit;
78
79 if (v->DefaultToFootPrint()) {
80 hit = CCollisionHandler::CollisionFootPrint(o, p);
81 } else {
82 switch (v->GetVolumeType()) {
83 case CollisionVolume::COLVOL_TYPE_SPHERE: {
84 hit = true;
85 } break;
86 default: {
87 // NOTE: we have to translate by relMidPos to get to midPos
88 // (which is where the collision volume gets drawn) because
89 // GetTransformMatrix() only uses pos (UNITS AND FEATURES)
90 CMatrix44f m = o->GetTransformMatrix(true);
91 m.Translate(o->relMidPos * WORLD_TO_OBJECT_SPACE);
92 m.Translate(v->GetOffsets());
93
94 hit = CCollisionHandler::Collision(v, m, p);
95 }
96 }
97 }
98
99 if (cq != NULL && hit) {
100 // same as the special cases for the continuous tests
101 // (but here p is a valid coordinate and safe to use)
102 cq->b0 = CQ_POINT_IN_VOL; cq->t0 = 0.0f; cq->p0 = p;
103 }
104
105 return hit;
106 }
107
108
CollisionFootPrint(const CSolidObject * o,const float3 & p)109 bool CCollisionHandler::CollisionFootPrint(const CSolidObject* o, const float3& p)
110 {
111 // If the object isn't marked on blocking map, or if it is flying,
112 // effecively only the early-out sphere check (in Collision(CUnit*)
113 // or Collision(CFeature*)) is performed (which we already passed).
114 if (!o->IsBlocking())
115 return false;
116 if (o->IsInAir())
117 return false;
118
119 // this is semi-equivalent to testing if <p> is inside the rectangular
120 // collision volume in the COLVOL_TYPE_BOX case, but takes non-blocking
121 // yardmap squares into account (even though this is a discrete test so
122 // projectile might have tunneled across blocking squares to get to <p>)
123 // note: if we get here <v> is always a box
124 const float invSquareSize = 1.0f / SQUARE_SIZE;
125 const int squareIdx = int(p.x * invSquareSize) + int(p.z * invSquareSize) * gs->mapx;
126
127 if (squareIdx < 0 || squareIdx >= gs->mapSquares)
128 return false;
129
130 const BlockingMapCell& cell = groundBlockingObjectMap->GetCell(squareIdx);
131
132 return (cell.find(o->GetBlockingMapID()) != cell.end());
133 }
134
135
Collision(const CollisionVolume * v,const CMatrix44f & m,const float3 & p)136 bool CCollisionHandler::Collision(const CollisionVolume* v, const CMatrix44f& m, const float3& p)
137 {
138 numDiscTests += 1;
139
140 // get the inverse volume transformation matrix and
141 // apply it to the projectile's position, then test
142 // if the transformed position lies within the axis-
143 // aligned collision volume
144 CMatrix44f mInv = m.Invert();
145 float3 pi = mInv.Mul(p);
146
147 bool hit = false;
148
149 switch (v->GetVolumeType()) {
150 case CollisionVolume::COLVOL_TYPE_SPHERE: {
151 // normally, this code is never executed, because the higher level
152 // Collision(CFeature*) and Collision(CUnit*) already optimize via
153 // early-out tests
154 hit = (pi.dot(pi) <= v->GetHSqScales().x);
155
156 // test for arbitrary ellipsoids (no longer supported)
157 // const float f1 = (pi.x * pi.x) / v->GetHSqScales().x;
158 // const float f2 = (pi.y * pi.y) / v->GetHSqScales().y;
159 // const float f3 = (pi.z * pi.z) / v->GetHSqScales().z;
160 // hit = ((f1 + f2 + f3) <= 1.0f);
161 } break;
162 case CollisionVolume::COLVOL_TYPE_CYLINDER: {
163 switch (v->GetPrimaryAxis()) {
164 case CollisionVolume::COLVOL_AXIS_X: {
165 const bool xPass = (math::fabs(pi.x) < v->GetHScales().x);
166 const float yRat = (pi.y * pi.y) / v->GetHSqScales().y;
167 const float zRat = (pi.z * pi.z) / v->GetHSqScales().z;
168 hit = (xPass && (yRat + zRat <= 1.0f));
169 } break;
170 case CollisionVolume::COLVOL_AXIS_Y: {
171 const bool yPass = (math::fabs(pi.y) < v->GetHScales().y);
172 const float xRat = (pi.x * pi.x) / v->GetHSqScales().x;
173 const float zRat = (pi.z * pi.z) / v->GetHSqScales().z;
174 hit = (yPass && (xRat + zRat <= 1.0f));
175 } break;
176 case CollisionVolume::COLVOL_AXIS_Z: {
177 const bool zPass = (math::fabs(pi.z) < v->GetHScales().z);
178 const float xRat = (pi.x * pi.x) / v->GetHSqScales().x;
179 const float yRat = (pi.y * pi.y) / v->GetHSqScales().y;
180 hit = (zPass && (xRat + yRat <= 1.0f));
181 } break;
182 }
183 } break;
184 case CollisionVolume::COLVOL_TYPE_BOX: {
185 const bool b1 = (math::fabs(pi.x) < v->GetHScales().x);
186 const bool b2 = (math::fabs(pi.y) < v->GetHScales().y);
187 const bool b3 = (math::fabs(pi.z) < v->GetHScales().z);
188 hit = (b1 && b2 && b3);
189 } break;
190 }
191
192 return hit;
193 }
194
195
MouseHit(const CUnit * u,const float3 & p0,const float3 & p1,const CollisionVolume * v,CollisionQuery * cq)196 bool CCollisionHandler::MouseHit(const CUnit* u, const float3& p0, const float3& p1, const CollisionVolume* v, CollisionQuery* cq)
197 {
198 if (!u->IsInVoid()) {
199 if (v->DefaultToPieceTree()) {
200 return (CCollisionHandler::IntersectPieceTree(u, p0, p1, cq));
201 }
202 if (!v->IgnoreHits()) {
203 // note: should mouse-rays care about
204 // IgnoreHits if unit is not in void?
205 CMatrix44f m = u->GetTransformMatrix(false, true);
206 m.Translate(u->relMidPos * WORLD_TO_OBJECT_SPACE);
207 m.Translate(v->GetOffsets());
208
209 return (CCollisionHandler::Intersect(v, m, p0, p1, cq));
210 }
211 }
212
213 return false;
214 }
215
216
217 /*
218 bool CCollisionHandler::IntersectPieceTreeHelper(
219 LocalModelPiece* lmp,
220 const CMatrix44f& mat,
221 const float3& p0,
222 const float3& p1,
223 std::vector<CollisionQuery>* cqs
224 ) {
225 bool ret = false;
226
227 CollisionVolume* lmpVol = lmp->GetCollisionVolume();
228 CMatrix44f volMat = lmp->GetModelSpaceMatrix() * mat;
229
230 if (lmp->scriptSetVisible && !lmpVol->IgnoreHits()) {
231 volMat.Translate(lmpVol->GetOffsets());
232
233 CollisionQuery cq;
234
235 if ((ret = CCollisionHandler::Intersect(lmpVol, volMat, p0, p1, &cq))) {
236 cq.SetHitPiece(lmp); cqs->push_back(cq);
237 }
238
239 volMat.Translate(-lmpVol->GetOffsets());
240 }
241
242 for (unsigned int i = 0; i < lmp->children.size(); i++) {
243 ret |= IntersectPieceTreeHelper(lmp->children[i], mat, p0, p1, cqs);
244 }
245
246 return ret;
247 }
248 */
249
IntersectPiecesHelper(const CUnit * u,const float3 & p0,const float3 & p1,CollisionQuery * cq)250 bool CCollisionHandler::IntersectPiecesHelper(
251 const CUnit* u,
252 const float3& p0,
253 const float3& p1,
254 CollisionQuery* cq
255 ) {
256 CMatrix44f unitMat = u->GetTransformMatrix(true);
257 CMatrix44f volMat;
258
259 CollisionQuery cqt;
260
261 if (cq == NULL)
262 cq = &cqt;
263
264 float minDistSq = std::numeric_limits<float>::max();
265 float curDistSq = std::numeric_limits<float>::max();
266
267 for (unsigned int n = 0; n < u->localModel->pieces.size(); n++) {
268 const LocalModelPiece* lmp = u->localModel->GetPiece(n);
269 const CollisionVolume* lmpVol = lmp->GetCollisionVolume();
270
271 if (!lmp->scriptSetVisible || lmpVol->IgnoreHits())
272 continue;
273
274 volMat = unitMat * lmp->GetModelSpaceMatrix();
275 volMat.Translate(lmpVol->GetOffsets());
276
277 if (!CCollisionHandler::Intersect(lmpVol, volMat, p0, p1, cq))
278 continue;
279 // skip if neither an ingress nor an egress hit
280 if (cq->GetHitPos() == ZeroVector)
281 continue;
282
283 cq->SetHitPiece(const_cast<LocalModelPiece*>(lmp));
284
285 // save the closest intersection (others are not needed)
286 if ((curDistSq = (cq->GetHitPos() - p0).SqLength()) >= minDistSq)
287 continue;
288
289 minDistSq = curDistSq;
290 }
291
292 // true iff at least one piece was intersected
293 // (query must have been reset by calling code)
294 return (cq->GetHitPiece() != NULL);
295 }
296
297
IntersectPieceTree(const CUnit * u,const float3 & p0,const float3 & p1,CollisionQuery * cq)298 bool CCollisionHandler::IntersectPieceTree(const CUnit* u, const float3& p0, const float3& p1, CollisionQuery* cq)
299 {
300 // TODO:
301 // needs an early-out test, but gets complicated because
302 // pieces can move --> no clearly defined bounding volume
303 return (IntersectPiecesHelper(u, p0, p1, cq));
304 }
305
Intersect(const CollisionVolume * v,const CSolidObject * o,const float3 p0,const float3 p1,CollisionQuery * cq)306 inline bool CCollisionHandler::Intersect(const CollisionVolume* v, const CSolidObject* o, const float3 p0, const float3 p1, CollisionQuery* cq)
307 {
308 CMatrix44f m = o->GetTransformMatrix(true);
309 m.Translate(o->relMidPos * WORLD_TO_OBJECT_SPACE);
310 m.Translate(v->GetOffsets());
311
312 return (CCollisionHandler::Intersect(v, m, p0, p1, cq));
313 }
314
315 /*
316 bool CCollisionHandler::IntersectAlt(const collisionVolume* d, const CMatrix44f& m, const float3& p0, const float3& p1, CollisionQuery*)
317 {
318 // alternative numerical integration method (unused)
319 const float delta = 1.0f;
320 const float length = (p1 - p0).Length();
321 const float3 dir = (p1 - p0).Normalize();
322
323 for (float t = 0.0f; t <= length; t += delta) {
324 if (::Collision(d, m, p0 + dir * t)) return true;
325 }
326
327 return false;
328 }
329 */
330
331
Intersect(const CollisionVolume * v,const CMatrix44f & m,const float3 & p0,const float3 & p1,CollisionQuery * q)332 bool CCollisionHandler::Intersect(const CollisionVolume* v, const CMatrix44f& m, const float3& p0, const float3& p1, CollisionQuery* q)
333 {
334 numContTests += 1;
335
336 CMatrix44f mInv = m.Invert();
337 const float3 pi0 = mInv.Mul(p0);
338 const float3 pi1 = mInv.Mul(p1);
339 bool intersect = false;
340
341 // minimum and maximum (x, y, z) coordinates of transformed ray
342 const float rminx = std::min(pi0.x, pi1.x), rminy = std::min(pi0.y, pi1.y), rminz = std::min(pi0.z, pi1.z);
343 const float rmaxx = std::max(pi0.x, pi1.x), rmaxy = std::max(pi0.y, pi1.y), rmaxz = std::max(pi0.z, pi1.z);
344
345 // minimum and maximum (x, y, z) coordinates of (bounding box around) volume
346 const float vminx = -v->GetHScales().x, vminy = -v->GetHScales().y, vminz = -v->GetHScales().z;
347 const float vmaxx = v->GetHScales().x, vmaxy = v->GetHScales().y, vmaxz = v->GetHScales().z;
348
349 // check if ray segment misses (bounding box around) volume
350 // (if so, then no further intersection tests are necessary)
351 if (rmaxx < vminx || rminx > vmaxx) { return false; }
352 if (rmaxy < vminy || rminy > vmaxy) { return false; }
353 if (rmaxz < vminz || rminz > vmaxz) { return false; }
354
355 switch (v->GetVolumeType()) {
356 case CollisionVolume::COLVOL_TYPE_SPHERE: {
357 // sphere is special case of ellipsoid, reuse code
358 intersect = CCollisionHandler::IntersectEllipsoid(v, pi0, pi1, q);
359 } break;
360 case CollisionVolume::COLVOL_TYPE_CYLINDER: {
361 intersect = CCollisionHandler::IntersectCylinder(v, pi0, pi1, q);
362 } break;
363 case CollisionVolume::COLVOL_TYPE_BOX: {
364 // also covers footprints, but without taking the blocking-map into account
365 // TODO: this would require stepping ray across non-blocking yardmap squares?
366 //
367 // intersect = CCollisionHandler::IntersectFootPrint(v, pi0, pi1, q);
368 intersect = CCollisionHandler::IntersectBox(v, pi0, pi1, q);
369 } break;
370 }
371
372 if (q != NULL) {
373 // transform intersection points (iff not a special
374 // case, otherwise calling code should not use them)
375 if (q->b0 == CQ_POINT_ON_RAY) { q->p0 = m.Mul(q->p0); }
376 if (q->b1 == CQ_POINT_ON_RAY) { q->p1 = m.Mul(q->p1); }
377 }
378
379 return intersect;
380 }
381
IntersectEllipsoid(const CollisionVolume * v,const float3 & pi0,const float3 & pi1,CollisionQuery * q)382 bool CCollisionHandler::IntersectEllipsoid(const CollisionVolume* v, const float3& pi0, const float3& pi1, CollisionQuery* q)
383 {
384 // transform the volume-space points into (unit) sphere-space (requires fewer
385 // float-ops than solving the surface equation for arbitrary ellipsoid volumes)
386 const float3 pii0 = float3(pi0.x * v->GetHIScales().x, pi0.y * v->GetHIScales().y, pi0.z * v->GetHIScales().z);
387 const float3 pii1 = float3(pi1.x * v->GetHIScales().x, pi1.y * v->GetHIScales().y, pi1.z * v->GetHIScales().z);
388 const float rSq = 1.0f;
389
390 if (pii0.dot(pii0) <= rSq) {
391 if (q != NULL) {
392 // terminate early in the special case
393 // that ray-segment originated *in* <v>
394 // (these points are NOT transformed!)
395 q->b0 = CQ_POINT_IN_VOL; q->p0 = ZeroVector;
396 q->b1 = CQ_POINT_IN_VOL; q->p1 = ZeroVector;
397 }
398 return true;
399 }
400
401 // get the ray direction in unit-sphere space
402 const float3 dir = (pii1 - pii0).SafeNormalize();
403
404 // solves [ x^2 + y^2 + z^2 == r^2 ] for t
405 // (<A> represents dir.dot(dir), equal to 1
406 // since ray direction already normalized)
407 const float A = 1.0f;
408 const float B = 2.0f * (pii0).dot(dir);
409 const float C = pii0.dot(pii0) - rSq;
410 const float D = (B * B) - (4.0f * A * C);
411
412 if (D < -COLLISION_VOLUME_EPS) {
413 return false;
414 } else {
415 // get the length of the ray segment in volume-space
416 const float segLenSq = (pi1 - pi0).SqLength();
417
418 if (D < COLLISION_VOLUME_EPS) {
419 // one solution for t
420 const float t0 = -B * 0.5f;
421 // const float t0 = -B / (2.0f * A);
422 // get the intersection point in sphere-space
423 const float3 pTmp = pii0 + (dir * t0);
424 // get the intersection point in volume-space
425 const float3 p0 = pTmp * v->GetHScales();
426 // get the distance from the start of the segment
427 // to the intersection point in volume-space
428 const float dSq0 = (p0 - pi0).SqLength();
429 // if the intersection point is closer to p0 than
430 // the end of the ray segment, the hit is valid
431 const int b0 = (t0 > 0.0f && dSq0 <= segLenSq) * CQ_POINT_ON_RAY;
432
433 if (q != NULL) {
434 q->b0 = b0; q->b1 = CQ_POINT_NO_INT;
435 q->t0 = t0; q->t1 = 0.0f;
436 q->p0 = p0; q->p1 = ZeroVector;
437 }
438
439 return (b0 == CQ_POINT_ON_RAY);
440 } else {
441 // two solutions for t
442 const float rD = fastmath::apxsqrt(D);
443 const float t0 = (-B - rD) * 0.5f;
444 const float t1 = (-B + rD) * 0.5f;
445 // const float t0 = (-B + rD) / (2.0f * A);
446 // const float t1 = (-B - rD) / (2.0f * A);
447 // get the intersection points in sphere-space
448 const float3 pTmp0 = pii0 + (dir * t0);
449 const float3 pTmp1 = pii0 + (dir * t1);
450 // get the intersection points in volume-space
451 const float3 p0 = pTmp0 * v->GetHScales();
452 const float3 p1 = pTmp1 * v->GetHScales();
453 // get the distances from the start of the ray
454 // to the intersection points in volume-space
455 const float dSq0 = (p0 - pi0).SqLength();
456 const float dSq1 = (p1 - pi0).SqLength();
457 // if one of the intersection points is closer to p0
458 // than the end of the ray segment, the hit is valid
459 const int b0 = (t0 > 0.0f && dSq0 <= segLenSq) * CQ_POINT_ON_RAY;
460 const int b1 = (t1 > 0.0f && dSq1 <= segLenSq) * CQ_POINT_ON_RAY;
461
462 if (q != NULL) {
463 q->b0 = b0; q->b1 = b1;
464 q->t0 = t0; q->t1 = t1;
465 q->p0 = p0; q->p1 = p1;
466 }
467
468 return (b0 == CQ_POINT_ON_RAY || b1 == CQ_POINT_ON_RAY);
469 }
470 }
471
472 return false;
473 }
474
IntersectCylinder(const CollisionVolume * v,const float3 & pi0,const float3 & pi1,CollisionQuery * q)475 bool CCollisionHandler::IntersectCylinder(const CollisionVolume* v, const float3& pi0, const float3& pi1, CollisionQuery* q)
476 {
477 const int pAx = v->GetPrimaryAxis();
478 const int sAx0 = v->GetSecondaryAxis(0);
479 const int sAx1 = v->GetSecondaryAxis(1);
480 const float3& ahs = v->GetHScales();
481 const float3& ahsq = v->GetHSqScales();
482 const float ratio =
483 ((pi0[sAx0] * pi0[sAx0]) / ahsq[sAx0]) +
484 ((pi0[sAx1] * pi0[sAx1]) / ahsq[sAx1]);
485
486 if ((math::fabs(pi0[pAx]) < ahs[pAx]) && (ratio <= 1.0f)) {
487 if (q != NULL) {
488 // terminate early in the special case
489 // that ray-segment originated within v
490 q->b0 = CQ_POINT_IN_VOL; q->p0 = ZeroVector;
491 q->b1 = CQ_POINT_IN_VOL; q->p1 = ZeroVector;
492 }
493 return true;
494 }
495
496 // ray direction in volume-space
497 const float3 dir = (pi1 - pi0).SafeNormalize();
498
499 // ray direction in (unit) cylinder-space
500 float3 diir = ZeroVector;
501
502 // ray terminals in (unit) cylinder-space
503 float3 pii0 = pi0;
504 float3 pii1 = pi1;
505
506 // end-cap plane normals in volume-space
507 float3 n0 = ZeroVector;
508 float3 n1 = ZeroVector;
509
510 // (unit) cylinder-space to volume-space transformation
511 float3 inv(1.0f, 1.0f, 1.0f);
512
513 // unit-cylinder surface equation params
514 float a = 0.0f;
515 float b = 0.0f;
516 float c = 0.0f;
517
518 switch (pAx) {
519 case CollisionVolume::COLVOL_AXIS_X: {
520 pii0.y = pi0.y * v->GetHIScales().y;
521 pii0.z = pi0.z * v->GetHIScales().z;
522 pii1.y = pi1.y * v->GetHIScales().y;
523 pii1.z = pi1.z * v->GetHIScales().z;
524
525 inv.y = v->GetHScales().y;
526 inv.z = v->GetHScales().z;
527 diir = (pii1 - pii0).SafeNormalize();
528
529 n0.x = -1.0f; // left
530 n1.x = 1.0f; // right
531
532 // yz-surface equation params
533 a = (diir.y * diir.y) + (diir.z * diir.z);
534 b = ((pii0.y * diir.y) + (pii0.z * diir.z)) * 2.0f;
535 c = (pii0.y * pii0.y) + (pii0.z * pii0.z) - 1.0f;
536 } break;
537 case CollisionVolume::COLVOL_AXIS_Y: {
538 pii0.x = pi0.x * v->GetHIScales().x;
539 pii0.z = pi0.z * v->GetHIScales().z;
540 pii1.x = pi1.x * v->GetHIScales().x;
541 pii1.z = pi1.z * v->GetHIScales().z;
542
543 inv.x = v->GetHScales().x;
544 inv.z = v->GetHScales().z;
545 diir = (pii1 - pii0).SafeNormalize();
546
547 n0.y = 1.0f; // top
548 n1.y = -1.0f; // bottom
549
550 // xz-surface equation params
551 a = (diir.x * diir.x) + (diir.z * diir.z);
552 b = ((pii0.x * diir.x) + (pii0.z * diir.z)) * 2.0f;
553 c = (pii0.x * pii0.x) + (pii0.z * pii0.z) - 1.0f;
554 } break;
555 case CollisionVolume::COLVOL_AXIS_Z: {
556 pii0.x = pi0.x * v->GetHIScales().x;
557 pii0.y = pi0.y * v->GetHIScales().y;
558 pii1.x = pi1.x * v->GetHIScales().x;
559 pii1.y = pi1.y * v->GetHIScales().y;
560
561 inv.x = v->GetHScales().x;
562 inv.y = v->GetHScales().y;
563 diir = (pii1 - pii0).SafeNormalize();
564
565 n0.z = 1.0f; // front
566 n1.z = -1.0f; // back
567
568 // xy-surface equation params
569 a = (diir.x * diir.x) + (diir.y * diir.y);
570 b = ((pii0.x * diir.x) + (pii0.y * diir.y)) * 2.0f;
571 c = (pii0.x * pii0.x) + (pii0.y * pii0.y) - 1.0f;
572 } break;
573 }
574
575 // volume-space intersection points
576 float3 p0 = ZeroVector;
577 float3 p1 = ZeroVector;
578
579 int b0 = CQ_POINT_NO_INT;
580 int b1 = CQ_POINT_NO_INT;
581 float
582 d = (b * b) - (4.0f * a * c),
583 rd = 0.0f, // math::sqrt(d) or 1/dp
584 dp = 0.0f, // dot(n{0, 1}, dir)
585 ra = 0.0f; // ellipsoid ratio of p{0, 1}
586 float s0 = 0.0f, t0 = 0.0f;
587 float s1 = 0.0f, t1 = 0.0f;
588
589 // get the length of the ray segment in volume-space
590 const float segLenSq = (pi1 - pi0).SqLength();
591
592 if (d >= -COLLISION_VOLUME_EPS) {
593 if (a != 0.0f) {
594 // quadratic eq.; one or two surface intersections
595 if (d < COLLISION_VOLUME_EPS) {
596 t0 = -b / (2.0f * a);
597 p0 = (pii0 + (diir * t0)) * inv;
598 s0 = (p0 - pi0).SqLength();
599 b0 = (s0 < segLenSq && math::fabs(p0[pAx]) < ahs[pAx]) * CQ_POINT_ON_RAY;
600 } else {
601 rd = fastmath::apxsqrt(d);
602 t0 = (-b - rd) / (2.0f * a);
603 t1 = (-b + rd) / (2.0f * a);
604 p0 = (pii0 + (diir * t0)) * inv;
605 p1 = (pii0 + (diir * t1)) * inv;
606 s0 = (p0 - pi0).SqLength();
607 s1 = (p1 - pi0).SqLength();
608 b0 = (s0 < segLenSq && math::fabs(p0[pAx]) < ahs[pAx]) * CQ_POINT_ON_RAY;
609 b1 = (s1 < segLenSq && math::fabs(p1[pAx]) < ahs[pAx]) * CQ_POINT_ON_RAY;
610 }
611 } else {
612 if (b != 0.0f) {
613 // linear eq.; one surface intersection
614 t0 = -c / b;
615 p0 = (pii0 + (diir * t0)) * inv;
616 s0 = (p0 - pi0).SqLength();
617 b0 = (s0 < segLenSq && math::fabs(p0[pAx]) < ahs[pAx]) * CQ_POINT_ON_RAY;
618 }
619 }
620 }
621
622 if (b0 == CQ_POINT_NO_INT) {
623 // p0 does not lie on ray segment, or does not fall
624 // between cylinder end-caps: check if segment goes
625 // through front cap (plane)
626 // NOTE: normal n0 and dir should not be orthogonal
627 dp = n0.dot(dir);
628 rd = (dp != 0.0f)? 1.0f / dp: 0.01f;
629
630 t0 = -(n0.dot(pi0) - ahs[pAx]) * rd;
631 p0 = pi0 + (dir * t0);
632 s0 = (p0 - pi0).SqLength();
633 ra =
634 (((p0[sAx0] * p0[sAx0]) / ahsq[sAx0]) +
635 ((p0[sAx1] * p0[sAx1]) / ahsq[sAx1]));
636 b0 = (t0 >= 0.0f && ra <= 1.0f && s0 <= segLenSq) * CQ_POINT_ON_RAY;
637 }
638 if (b1 == CQ_POINT_NO_INT) {
639 // p1 does not lie on ray segment, or does not fall
640 // between cylinder end-caps: check if segment goes
641 // through rear cap (plane)
642 // NOTE: normal n1 and dir should not be orthogonal
643 dp = n1.dot(dir);
644 rd = (dp != 0.0f)? 1.0f / dp: 0.01f;
645
646 t1 = -(n1.dot(pi0) - ahs[pAx]) * rd;
647 p1 = pi0 + (dir * t1);
648 s1 = (p1 - pi0).SqLength();
649 ra =
650 (((p1[sAx0] * p1[sAx0]) / ahsq[sAx0]) +
651 ((p1[sAx1] * p1[sAx1]) / ahsq[sAx1]));
652 b1 = (t1 >= 0.0f && ra <= 1.0f && s1 <= segLenSq) * CQ_POINT_ON_RAY;
653 }
654
655 if (q != NULL) {
656 q->b0 = b0; q->b1 = b1;
657 q->t0 = t0; q->t1 = t1;
658 q->p0 = p0; q->p1 = p1;
659 }
660
661 return (b0 == CQ_POINT_ON_RAY || b1 == CQ_POINT_ON_RAY);
662 }
663
IntersectBox(const CollisionVolume * v,const float3 & pi0,const float3 & pi1,CollisionQuery * q)664 bool CCollisionHandler::IntersectBox(const CollisionVolume* v, const float3& pi0, const float3& pi1, CollisionQuery* q)
665 {
666 const bool ba = (math::fabs(pi0.x) < v->GetHScales().x);
667 const bool bb = (math::fabs(pi0.y) < v->GetHScales().y);
668 const bool bc = (math::fabs(pi0.z) < v->GetHScales().z);
669
670 if (ba && bb && bc) {
671 // terminate early in the special case
672 // that ray-segment originated within v
673 if (q != NULL) {
674 q->b0 = CQ_POINT_IN_VOL; q->p0 = ZeroVector;
675 q->b1 = CQ_POINT_IN_VOL; q->p1 = ZeroVector;
676 }
677
678 return true;
679 }
680
681 float tn = -9999999.9f;
682 float tf = 9999999.9f;
683 float t0 = 0.0f;
684 float t1 = 0.0f;
685 float t2 = 0.0f;
686
687 const float3 dir = (pi1 - pi0).SafeNormalize();
688
689 if (math::fabs(dir.x) < COLLISION_VOLUME_EPS) {
690 if (math::fabs(pi0.x) > v->GetHScales().x) {
691 return false;
692 }
693 } else {
694 if (dir.x > 0.0f) {
695 t0 = (-v->GetHScales().x - pi0.x) / dir.x;
696 t1 = ( v->GetHScales().x - pi0.x) / dir.x;
697 } else {
698 t1 = (-v->GetHScales().x - pi0.x) / dir.x;
699 t0 = ( v->GetHScales().x - pi0.x) / dir.x;
700 }
701
702 if (t0 > t1) { t2 = t1; t1 = t0; t0 = t2; }
703 if (t0 > tn) { tn = t0; }
704 if (t1 < tf) { tf = t1; }
705 if (tn > tf) { return false; }
706 if (tf < 0.0f) { return false; }
707 }
708
709 if (math::fabs(dir.y) < COLLISION_VOLUME_EPS) {
710 if (math::fabs(pi0.y) > v->GetHScales().y) {
711 return false;
712 }
713 } else {
714 if (dir.y > 0.0f) {
715 t0 = (-v->GetHScales().y - pi0.y) / dir.y;
716 t1 = ( v->GetHScales().y - pi0.y) / dir.y;
717 } else {
718 t1 = (-v->GetHScales().y - pi0.y) / dir.y;
719 t0 = ( v->GetHScales().y - pi0.y) / dir.y;
720 }
721
722 if (t0 > t1) { t2 = t1; t1 = t0; t0 = t2; }
723 if (t0 > tn) { tn = t0; }
724 if (t1 < tf) { tf = t1; }
725 if (tn > tf) { return false; }
726 if (tf < 0.0f) { return false; }
727 }
728
729 if (math::fabs(dir.z) < COLLISION_VOLUME_EPS) {
730 if (math::fabs(pi0.z) > v->GetHScales().z) {
731 return false;
732 }
733 } else {
734 if (dir.z > 0.0f) {
735 t0 = (-v->GetHScales().z - pi0.z) / dir.z;
736 t1 = ( v->GetHScales().z - pi0.z) / dir.z;
737 } else {
738 t1 = (-v->GetHScales().z - pi0.z) / dir.z;
739 t0 = ( v->GetHScales().z - pi0.z) / dir.z;
740 }
741
742 if (t0 > t1) { t2 = t1; t1 = t0; t0 = t2; }
743 if (t0 > tn) { tn = t0; }
744 if (t1 < tf) { tf = t1; }
745 if (tn > tf) { return false; }
746 if (tf < 0.0f) { return false; }
747 }
748
749 // get the intersection points in volume-space
750 const float3 p0 = pi0 + (dir * tn);
751 const float3 p1 = pi0 + (dir * tf);
752 // get the length of the ray segment in volume-space
753 const float segLenSq = (pi1 - pi0).SqLength();
754 // get the distances from the start of the ray
755 // to the intersection points in volume-space
756 const float dSq0 = (p0 - pi0).SqLength();
757 const float dSq1 = (p1 - pi0).SqLength();
758 // if one of the intersection points is closer to p0
759 // than the end of the ray segment, the hit is valid
760 const int b0 = (dSq0 <= segLenSq) * CQ_POINT_ON_RAY;
761 const int b1 = (dSq1 <= segLenSq) * CQ_POINT_ON_RAY;
762
763 if (q != NULL) {
764 q->b0 = b0; q->b1 = b1;
765 q->t0 = tn; q->t1 = tf;
766 q->p0 = p0; q->p1 = p1;
767 }
768
769 return (b0 == CQ_POINT_ON_RAY || b1 == CQ_POINT_ON_RAY);
770 }
771
772