From f4ace0f0b17e894e3c9981d47545834a83351578 Mon Sep 17 00:00:00 2001 From: Erich Schubert Date: Tue, 12 Nov 2024 16:39:23 +0100 Subject: [PATCH] Collision with acceleration --- src/collision.cpp | 171 +++++++++++++++++--------------- src/collision.h | 2 +- src/unittest/test_collision.cpp | 45 ++++++--- 3 files changed, 120 insertions(+), 98 deletions(-) diff --git a/src/collision.cpp b/src/collision.cpp index 2f44696fe3bcc..29811d8ede49f 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -90,107 +90,116 @@ inline v3f rangelimv(const v3f vec, const f32 low, const f32 high) } } +const f32 NO_COLLISION_TIME = -std::numeric_limits::infinity(); + +// Time to collision in a one dimensional setting, with acceleration. +// We have `pos = vel * dtime + 0.5 * acc * dtime²`. +// This yields `dtime = (-vel ± sqrt(vel² + 2 * acc * pos)) / acc` +inline f32 timeToCollision1D(f32 pos, f32 vel, f32 acc) { + if (pos == 0.f) + return 0.f; + // Base case, dtime = pos / vel, if in the same direction + if (acc == 0.f) + return pos * vel > 0.f ? pos / vel : NO_COLLISION_TIME; + // use f64 because of squared values to avoid catastrophic cancelation + f64 sq = vel * (f64) vel + 2. * acc * pos; + if (sq < 0.0 && sq > -1e-6) + sq = 0.0; // allow some rounding error + if (sq < 0.0) + return NO_COLLISION_TIME; + f32 sqrt = (f32) std::sqrt(sq); // back to original precision + // beware: both vel and acc may be negative here + f32 sol_a = (-vel + sqrt) / acc; + f32 sol_b = (-vel - sqrt) / acc; + if (sol_a >= 0.f && (sol_b < 0.f || sol_a <= sol_b)) + return sol_a; + if (sol_b >= 0.f && (sol_a < 0.f || sol_b <= sol_a)) + return sol_b; + return NO_COLLISION_TIME; +} + // Helper function: // Checks for collision of a moving aabbox with a static aabbox // Returns -1 if no collision, 0 if X collision, 1 if Y collision, 2 if Z collision // The time after which the collision occurs is stored in dtime. CollisionAxis axisAlignedCollision( const aabb3f &staticbox, const aabb3f &movingbox, - const v3f speed, f32 *dtime) + const v3f speed, const v3f accel, f32 *dtime) { //TimeTaker tt("axisAlignedCollision"); - aabb3f relbox( - (movingbox.MaxEdge.X - movingbox.MinEdge.X) + (staticbox.MaxEdge.X - staticbox.MinEdge.X), // sum of the widths - (movingbox.MaxEdge.Y - movingbox.MinEdge.Y) + (staticbox.MaxEdge.Y - staticbox.MinEdge.Y), - (movingbox.MaxEdge.Z - movingbox.MinEdge.Z) + (staticbox.MaxEdge.Z - staticbox.MinEdge.Z), - std::max(movingbox.MaxEdge.X, staticbox.MaxEdge.X) - std::min(movingbox.MinEdge.X, staticbox.MinEdge.X), //outer bounding 'box' dimensions - std::max(movingbox.MaxEdge.Y, staticbox.MaxEdge.Y) - std::min(movingbox.MinEdge.Y, staticbox.MinEdge.Y), - std::max(movingbox.MaxEdge.Z, staticbox.MaxEdge.Z) - std::min(movingbox.MinEdge.Z, staticbox.MinEdge.Z) - ); - - const f32 dtime_max = *dtime; - f32 inner_margin; // the distance of clipping recovery - f32 distance; - f32 time; - - - if (speed.Y) { - distance = relbox.MaxEdge.Y - relbox.MinEdge.Y; - // FIXME: The dtime calculation is inaccurate without acceleration information. - // Exact formula: `dtime = (-vel ± sqrt(vel² + 2 * acc * distance)) / acc` - *dtime = distance / std::abs(speed.Y); - time = std::max(*dtime, 0.0f); - - if (*dtime <= dtime_max) { - inner_margin = std::max(-0.5f * (staticbox.MaxEdge.Y - staticbox.MinEdge.Y), -2.0f); - - if ((speed.Y > 0 && staticbox.MinEdge.Y - movingbox.MaxEdge.Y > inner_margin) || - (speed.Y < 0 && movingbox.MinEdge.Y - staticbox.MaxEdge.Y > inner_margin)) { - if ( - (std::max(movingbox.MaxEdge.X + speed.X * time, staticbox.MaxEdge.X) - - std::min(movingbox.MinEdge.X + speed.X * time, staticbox.MinEdge.X) - - relbox.MinEdge.X < 0) && - (std::max(movingbox.MaxEdge.Z + speed.Z * time, staticbox.MaxEdge.Z) - - std::min(movingbox.MinEdge.Z + speed.Z * time, staticbox.MinEdge.Z) - - relbox.MinEdge.Z < 0) - ) - return COLLISION_AXIS_Y; - } + if (speed.Y || accel.Y) { + f32 time; + if (movingbox.MaxEdge.Y < staticbox.MinEdge.Y) { + time = timeToCollision1D(staticbox.MinEdge.Y - movingbox.MaxEdge.Y, speed.Y, accel.Y); + } else if (movingbox.MinEdge.Y > staticbox.MaxEdge.Y) { + time = timeToCollision1D(staticbox.MaxEdge.Y - movingbox.MinEdge.Y, speed.Y, accel.Y); + // overlapping, allow to separate: + } else if (movingbox.MaxEdge.Y - staticbox.MinEdge.Y > staticbox.MaxEdge.Y - movingbox.MinEdge.Y) { + time = speed.Y > 0.f || (speed.Y == 0.f && accel.Y > 0.f) ? NO_COLLISION_TIME : 0; + } else { + time = speed.Y < 0.f || (speed.Y == 0.f && accel.Y < 0.f) ? NO_COLLISION_TIME : 0; } - else { - return COLLISION_AXIS_NONE; + if (time != NO_COLLISION_TIME && time <= *dtime) { + v3f aspeed = speed + 0.5 * accel; + if (movingbox.MaxEdge.X + aspeed.X * time > staticbox.MinEdge.X && + movingbox.MinEdge.X + aspeed.X * time < staticbox.MaxEdge.X && + movingbox.MaxEdge.Z + aspeed.Z * time > staticbox.MinEdge.Z && + movingbox.MinEdge.Z + aspeed.Z * time < staticbox.MaxEdge.Z) { + *dtime = time; + return COLLISION_AXIS_Y; + } } } // NO else if here - if (speed.X) { - distance = relbox.MaxEdge.X - relbox.MinEdge.X; - *dtime = distance / std::abs(speed.X); - time = std::max(*dtime, 0.0f); - - if (*dtime <= dtime_max) { - inner_margin = std::max(-0.5f * (staticbox.MaxEdge.X - staticbox.MinEdge.X), -2.0f); - - if ((speed.X > 0 && staticbox.MinEdge.X - movingbox.MaxEdge.X > inner_margin) || - (speed.X < 0 && movingbox.MinEdge.X - staticbox.MaxEdge.X > inner_margin)) { - if ( - (std::max(movingbox.MaxEdge.Y + speed.Y * time, staticbox.MaxEdge.Y) - - std::min(movingbox.MinEdge.Y + speed.Y * time, staticbox.MinEdge.Y) - - relbox.MinEdge.Y < 0) && - (std::max(movingbox.MaxEdge.Z + speed.Z * time, staticbox.MaxEdge.Z) - - std::min(movingbox.MinEdge.Z + speed.Z * time, staticbox.MinEdge.Z) - - relbox.MinEdge.Z < 0) - ) - return COLLISION_AXIS_X; - } + if (speed.X || accel.X) { + f32 time; + if (movingbox.MaxEdge.X < staticbox.MinEdge.X) { + time = timeToCollision1D(staticbox.MinEdge.X - movingbox.MaxEdge.X, speed.X, accel.X); + } else if (movingbox.MinEdge.X > staticbox.MaxEdge.X) { + time = timeToCollision1D(staticbox.MaxEdge.X - movingbox.MinEdge.X, speed.X, accel.X); + // overlapping, allow to separate: + } else if (movingbox.MaxEdge.X - staticbox.MinEdge.X > staticbox.MaxEdge.X - movingbox.MinEdge.X) { + time = speed.X > 0.f || (speed.X == 0.f && accel.X > 0.f) ? NO_COLLISION_TIME : 0; } else { - return COLLISION_AXIS_NONE; + time = speed.X < 0.f || (speed.X == 0.f && accel.X < 0.f) ? NO_COLLISION_TIME : 0; + } + if (time != NO_COLLISION_TIME && time <= *dtime) { + v3f aspeed = speed + 0.5 * accel; + if (movingbox.MaxEdge.Y + aspeed.Y * time > staticbox.MinEdge.Y && + movingbox.MinEdge.Y + aspeed.Y * time < staticbox.MaxEdge.Y && + movingbox.MaxEdge.Z + aspeed.Z * time > staticbox.MinEdge.Z && + movingbox.MinEdge.Z + aspeed.Z * time < staticbox.MaxEdge.Z) { + *dtime = time; + return COLLISION_AXIS_X; + } } } // NO else if here - if (speed.Z) { - distance = relbox.MaxEdge.Z - relbox.MinEdge.Z; - *dtime = distance / std::abs(speed.Z); - time = std::max(*dtime, 0.0f); - - if (*dtime <= dtime_max) { - inner_margin = std::max(-0.5f * (staticbox.MaxEdge.Z - staticbox.MinEdge.Z), -2.0f); - - if ((speed.Z > 0 && staticbox.MinEdge.Z - movingbox.MaxEdge.Z > inner_margin) || - (speed.Z < 0 && movingbox.MinEdge.Z - staticbox.MaxEdge.Z > inner_margin)) { - if ( - (std::max(movingbox.MaxEdge.X + speed.X * time, staticbox.MaxEdge.X) - - std::min(movingbox.MinEdge.X + speed.X * time, staticbox.MinEdge.X) - - relbox.MinEdge.X < 0) && - (std::max(movingbox.MaxEdge.Y + speed.Y * time, staticbox.MaxEdge.Y) - - std::min(movingbox.MinEdge.Y + speed.Y * time, staticbox.MinEdge.Y) - - relbox.MinEdge.Y < 0) - ) - return COLLISION_AXIS_Z; + if (speed.Z || accel.Z) { + f32 time; + if (movingbox.MaxEdge.Z < staticbox.MinEdge.Z) { + time = timeToCollision1D(staticbox.MinEdge.Z - movingbox.MaxEdge.Z, speed.Z, accel.Z); + } else if (movingbox.MinEdge.Z > staticbox.MaxEdge.Z) { + time = timeToCollision1D(staticbox.MaxEdge.Z - movingbox.MinEdge.Z, speed.Z, accel.Z); + // overlapping, allow to separate: + } else if (movingbox.MaxEdge.Z - staticbox.MinEdge.Z > staticbox.MaxEdge.Z - movingbox.MinEdge.Z) { + time = speed.Z > 0.f || (speed.Z == 0.f && accel.Z > 0.f) ? NO_COLLISION_TIME : 0; + } else { + time = speed.Z < 0.f || (speed.Z == 0.f && accel.Z < 0.f) ? NO_COLLISION_TIME : 0; + } + if (time != NO_COLLISION_TIME && time <= *dtime) { + v3f aspeed = speed + 0.5 * accel; + if (movingbox.MaxEdge.X + aspeed.X * time > staticbox.MinEdge.X && + movingbox.MinEdge.X + aspeed.X * time < staticbox.MaxEdge.X && + movingbox.MaxEdge.Y + aspeed.Y * time > staticbox.MinEdge.Y && + movingbox.MinEdge.Y + aspeed.Y * time < staticbox.MaxEdge.Y) { + *dtime = time; + return COLLISION_AXIS_Z; } } } @@ -449,7 +458,7 @@ collisionMoveResult collisionMoveSimple(Environment *env, IGameDef *gamedef, // Find nearest collision of the two boxes (raytracing-like) f32 dtime_tmp = nearest_dtime; CollisionAxis collided = axisAlignedCollision(box_info.box, - movingbox, aspeed_f, &dtime_tmp); + movingbox, *speed_f, accel_f, &dtime_tmp); if (collided == -1 || dtime_tmp >= nearest_dtime) continue; diff --git a/src/collision.h b/src/collision.h index 5306cdd8ac376..0be3b66cbe864 100644 --- a/src/collision.h +++ b/src/collision.h @@ -76,7 +76,7 @@ bool collision_check_intersection(Environment *env, IGameDef *gamedef, // dtime receives time until first collision, invalid if -1 is returned CollisionAxis axisAlignedCollision( const aabb3f &staticbox, const aabb3f &movingbox, - v3f speed, f32 *dtime); + v3f speed, v3f accel, f32 *dtime); // Helper function: // Checks if moving the movingbox up by the given distance would hit a ceiling. diff --git a/src/unittest/test_collision.cpp b/src/unittest/test_collision.cpp index e30afc02f6a01..c2bfdc7dfafce 100644 --- a/src/unittest/test_collision.cpp +++ b/src/unittest/test_collision.cpp @@ -73,38 +73,43 @@ void TestCollision::testAxisAlignedCollision() aabb3f s(bx, by, bz, bx+1, by+1, bz+1); aabb3f m(bx-2, by, bz, bx-1, by+1, bz+1); v3f v(1, 0, 0); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 0); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 0); UASSERT(fabs(dtime - 1.000) < 0.001); } { aabb3f s(bx, by, bz, bx+1, by+1, bz+1); aabb3f m(bx-2, by, bz, bx-1, by+1, bz+1); v3f v(-1, 0, 0); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == -1); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == -1); } { aabb3f s(bx, by, bz, bx+1, by+1, bz+1); aabb3f m(bx-2, by+1.5, bz, bx-1, by+2.5, bz+1); v3f v(1, 0, 0); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == -1); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == -1); } { aabb3f s(bx, by, bz, bx+1, by+1, bz+1); aabb3f m(bx-2, by-1.5, bz, bx-1.5, by+0.5, bz+1); v3f v(0.5, 0.1, 0); + v3f a(0., 0., 0.); f32 dtime = 3.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 0); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 0); UASSERT(fabs(dtime - 3.000) < 0.001); } { aabb3f s(bx, by, bz, bx+1, by+1, bz+1); aabb3f m(bx-2, by-1.5, bz, bx-1.5, by+0.5, bz+1); v3f v(0.5, 0.1, 0); + v3f a(0., 0., 0.); f32 dtime = 3.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 0); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 0); UASSERT(fabs(dtime - 3.000) < 0.001); } @@ -113,23 +118,26 @@ void TestCollision::testAxisAlignedCollision() aabb3f s(bx, by, bz, bx+1, by+1, bz+1); aabb3f m(bx+2, by, bz, bx+3, by+1, bz+1); v3f v(-1, 0, 0); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 0); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 0); UASSERT(fabs(dtime - 1.000) < 0.001); } { aabb3f s(bx, by, bz, bx+1, by+1, bz+1); aabb3f m(bx+2, by, bz, bx+3, by+1, bz+1); v3f v(1, 0, 0); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == -1); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == -1); } { aabb3f s(bx, by, bz, bx+1, by+1, bz+1); aabb3f m(bx+2, by, bz+1.5, bx+3, by+1, bz+3.5); v3f v(-1, 0, 0); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == -1); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == -1); } { aabb3f s(bx, by, bz, bx+1, by+1, bz+1); @@ -137,7 +145,7 @@ void TestCollision::testAxisAlignedCollision() v3f v(-0.5, 0.2, 0); // 0.200000003 precisely v3f a(0., 0., 0.); f32 dtime = 2.51f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 1); // Y, not X! + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 1); // Y, not X! UASSERT(fabs(dtime - 2.500) < 0.001); } { @@ -146,7 +154,7 @@ void TestCollision::testAxisAlignedCollision() v3f v(-0.5, 0.3, 0); // 0.300000012 precisely v3f a(0., 0., 0.); f32 dtime = 2.1f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 0); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 0); UASSERT(fabs(dtime - 2.000) < 0.001); } @@ -157,24 +165,27 @@ void TestCollision::testAxisAlignedCollision() aabb3f s(bx, by, bz, bx+2, by+2, bz+2); aabb3f m(bx+2.3, by+2.29, bz+2.29, bx+4.2, by+4.2, bz+4.2); v3f v(-1./3, -1./3, -1./3); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 0); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 0); UASSERT(fabs(dtime - 0.9) < 0.001); } { aabb3f s(bx, by, bz, bx+2, by+2, bz+2); aabb3f m(bx+2.29, by+2.3, bz+2.29, bx+4.2, by+4.2, bz+4.2); v3f v(-1./3, -1./3, -1./3); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 1); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 1); UASSERT(fabs(dtime - 0.9) < 0.001); } { aabb3f s(bx, by, bz, bx+2, by+2, bz+2); aabb3f m(bx+2.29, by+2.29, bz+2.3, bx+4.2, by+4.2, bz+4.2); v3f v(-1./3, -1./3, -1./3); + v3f a(0., 0., 0.); f32 dtime = 1.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 2); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 2); UASSERT(fabs(dtime - 0.9) < 0.001); } { @@ -183,23 +194,25 @@ void TestCollision::testAxisAlignedCollision() v3f v(1./7, 1./7, 1./7); v3f a(0., 0., 0.); f32 dtime = 17.1f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 0); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 0); UASSERT(fabs(dtime - 16.1) < 0.001); } { aabb3f s(bx, by, bz, bx+2, by+2, bz+2); aabb3f m(bx-4.2, by-4.2, bz-4.2, bx-2.29, by-2.3, bz-2.29); v3f v(1./7, 1./7, 1./7); + v3f a(0., 0., 0.); f32 dtime = 17.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 1); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 1); UASSERT(fabs(dtime - 16.1) < 0.001); } { aabb3f s(bx, by, bz, bx+2, by+2, bz+2); aabb3f m(bx-4.2, by-4.2, bz-4.2, bx-2.29, by-2.29, bz-2.3); v3f v(1./7, 1./7, 1./7); + v3f a(0., 0., 0.); f32 dtime = 17.0f; - UASSERT(axisAlignedCollision(s, m, v, &dtime) == 2); + UASSERT(axisAlignedCollision(s, m, v, a, &dtime) == 2); UASSERT(fabs(dtime - 16.1) < 0.001); } }