Skip to content

Commit

Permalink
Collision with acceleration
Browse files Browse the repository at this point in the history
  • Loading branch information
@kno10
kno10 committed Nov 13, 2024
1 parent 50b6935 commit 00989ce
Show file tree
Hide file tree
Showing 3 changed files with 98 additions and 81 deletions.
132 changes: 68 additions & 64 deletions src/collision.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,106 +89,110 @@ inline v3f rangelimv(const v3f vec, const f32 low, const f32 high)
}
}

// 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`
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 : -10000.f;
// use f64 because of squared values to avoid catastrophic cancelation
f64 sq = vel * (f64) vel + 2. * acc * pos;
if (sq < 0.f /*&& sq > -0.001f*/) sq = 0.f; // allow some tolerance
if (sq < 0.f) return -10000.f;
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 -10000.f;
}

// 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)
v3f sums = v3f( // sum of the widths
(movingbox.MaxEdge.X - movingbox.MinEdge.X) + (staticbox.MaxEdge.X - staticbox.MinEdge.X),
(movingbox.MaxEdge.Y - movingbox.MinEdge.Y) + (staticbox.MaxEdge.Y - staticbox.MinEdge.Y),
(movingbox.MaxEdge.Z - movingbox.MinEdge.Z) + (staticbox.MaxEdge.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 (speed.Y || accel.Y) {
const f32 distance = movingbox.MaxEdge.Y < staticbox.MaxEdge.Y ? staticbox.MinEdge.Y - movingbox.MaxEdge.Y :
movingbox.MinEdge.Y > staticbox.MinEdge.Y ? staticbox.MaxEdge.Y - movingbox.MinEdge.Y: 0.;
*dtime = timeToCollision1D(distance, speed.Y, accel.Y);

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)
)
f32 time = std::max(*dtime, 0.0f);
f32 inner_margin = std::max(-0.5f * (staticbox.MaxEdge.Y - staticbox.MinEdge.Y), -2.0f);

if (((speed.Y > 0 || accel.Y > 0) && staticbox.MinEdge.Y - movingbox.MaxEdge.Y > inner_margin) ||
((speed.Y < 0 || accel.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)
- sums.X < 0) &&
( std::max(movingbox.MaxEdge.Z + speed.Z * time, staticbox.MaxEdge.Z)
- std::min(movingbox.MinEdge.Z + speed.Z * time, staticbox.MinEdge.Z)
- sums.Z < 0))
return COLLISION_AXIS_Y;
}
}
else {
return COLLISION_AXIS_NONE;
}
}

// 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 (speed.X || accel.X) {
const f32 distance = movingbox.MaxEdge.X < staticbox.MaxEdge.X ? staticbox.MinEdge.X - movingbox.MaxEdge.X :
movingbox.MinEdge.X > staticbox.MinEdge.X ? staticbox.MaxEdge.X - movingbox.MinEdge.X: 0.;
*dtime = timeToCollision1D(distance, speed.X, accel.X);

if (*dtime <= dtime_max) {
inner_margin = std::max(-0.5f * (staticbox.MaxEdge.X - staticbox.MinEdge.X), -2.0f);
f32 time = std::max(*dtime, 0.0f);
f32 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)
)
(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)
- sums.Y < 0) &&
( std::max(movingbox.MaxEdge.Z + speed.Z * time, staticbox.MaxEdge.Z)
- std::min(movingbox.MinEdge.Z + speed.Z * time, staticbox.MinEdge.Z)
- sums.Z < 0))
return COLLISION_AXIS_X;
}
} else {
return COLLISION_AXIS_NONE;
}
}

// 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 (speed.Z || accel.Z) {
const f32 distance = movingbox.MaxEdge.Z < staticbox.MaxEdge.Z ? staticbox.MinEdge.Z - movingbox.MaxEdge.Z :
movingbox.MinEdge.Z > staticbox.MinEdge.Z ? staticbox.MaxEdge.Z - movingbox.MinEdge.Z: 0.;
*dtime = timeToCollision1D(distance, speed.Z, accel.Z);

if (*dtime <= dtime_max) {
inner_margin = std::max(-0.5f * (staticbox.MaxEdge.Z - staticbox.MinEdge.Z), -2.0f);
f32 time = std::max(*dtime, 0.0f);
f32 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)
)
(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)
- sums.X < 0) &&
( std::max(movingbox.MaxEdge.Y + speed.Y * time, staticbox.MaxEdge.Y)
- std::min(movingbox.MinEdge.Y + speed.Y * time, staticbox.MinEdge.Y)
- sums.Y < 0))
return COLLISION_AXIS_Z;
}
}
Expand Down Expand Up @@ -448,7 +452,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;

Expand Down
2 changes: 1 addition & 1 deletion src/collision.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
45 changes: 29 additions & 16 deletions src/unittest/test_collision.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand All @@ -113,31 +118,34 @@ 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);
aabb3f m(bx+2, by-1.5, bz, bx+2.5, by-0.5, bz+1);
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);
}
{
Expand All @@ -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);
}

Expand All @@ -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);
}
{
Expand All @@ -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);
}
}
Expand Down

0 comments on commit 00989ce

Please sign in to comment.