Swept AABB Обнаружение столкновений и проблемы реагирования

Я работаю над простой 2D-игрой, и у меня возникают проблемы с обнаружением столкновений и реагированием на них. Я использую код, адаптированный из этой статьи: http://www.gamedev.net/page/resources/_/technical/game-programming/swept-aabb-collision-detection-and-response-r3084

Я пробовал много изменений в коде, но я не могу заставить его вести себя так, как ожидалось. Мой алгоритм пытается обнаружить столкновения между одним движущимся объектом и одним или несколькими статическими блоками. Он использует следующую логику:

1.
Обнаружение столкновения в широкой фазе:

Создайте AABB, которая инкапсулирует движение объекта, а затем обнаружите перекрытия с AABB статических блоков. Добавьте эти события столкновения в список и отсортируйте этот список по времени воздействия.

2.
Обнаружение узкофазных столкновений:

Рассчитайте время удара и нормальное столкновение для каждого события столкновения и переместите объект в правильную позицию вне каждого статического блока. Используйте нормальное столкновение, чтобы переместить объект в правильное положение.

Вот код, который я использую для расчета времени удара для события столкновения:

float Collision::collisionTime(QVector2D &normal)
{
// _one is our moving entity, _two is a static block

// Calculate the distance to entry and distance to exit
QVector2D distToEntry, distToExit;
QVector2D velocity = _one->velocity();

if (velocity.x() > 0)
{
distToEntry.setX(_two->rect().left()  - _one->rect().right());
distToExit.setX( _two->rect().right() - _one->rect().left());
}
else
{
distToEntry.setX(_two->rect().right() - _one->rect().left());
distToExit.setX( _two->rect().left()  - _one->rect().right());
}

if (velocity.y() > 0)
{
distToEntry.setY(_two->rect().top()    - _one->rect().bottom());
distToExit.setY( _two->rect().bottom() - _one->rect().top());
}
else
{
distToEntry.setY(_two->rect().bottom() - _one->rect().top());
distToExit.setY( _two->rect().top()    - _one->rect().bottom());
}

// Calculate the entry and exit times.
QVector2D entry, exit;
if (velocity.x() == 0)
{
entry.setX(-std::numeric_limits<float>::infinity());
exit.setX(std::numeric_limits<float>::infinity());
}
else
{
entry.setX(distToEntry.x() / velocity.x());
exit.setX(distToExit.x() / velocity.x());
}

if (velocity.y() == 0)
{
entry.setY(-std::numeric_limits<float>::infinity());
exit.setY(std::numeric_limits<float>::infinity());
}
else
{
entry.setY(distToEntry.y() / velocity.y());
exit.setY(distToExit.y() / velocity.y());
}

if (entry.x() > 1.0)
{
entry.setX(-std::numeric_limits<float>::infinity());
}
if (entry.y() > 1.0)
{
entry.setY(-std::numeric_limits<float>::infinity());
}

// Find the earliest / latest times of collision.
float entryTime = std::max(entry.x(), entry.y());
float exitTime  = std::min(exit.x(),  exit.y());

// If there was no collision...
if ((entryTime > exitTime) ||
(entry.x() < 0 && entry.y() < 0))
{
normal = QVector2D(0, 0);
return 1.0;
}

// If there was a collision,
// set the proper normal and return.
if (entry.x() >= entry.y())
{
if (distToEntry.x() < 0)
{
normal = QVector2D(1.0, 0);
}
else
{
normal = QVector2D(-1.0, 0);
}
}
else
{
if (distToEntry.y() < 0)
{
normal = QVector2D(0, 1.0);
}
else
{
normal = QVector2D(0, -1.0);
}
}
return entryTime;
}

А вот код, который генерирует ответ о столкновении:

bool Collision::resolve()
{
QVector2D collisionNormal;

// Calculate the collision time and normal.
float time = collisionTime(collisionNormal);

// Move to the point of collision.
_one->setPos(_one->pos().x() + _one->velocity().x() * time,
_one->pos().y() + _one->velocity().y() * time);

float remainingTime = 1.0 - time;

// If remainingTime > 0, there was a collision.
// Apply a slide effect.
if (remainingTime > 0)
{
float dotProduct = (_one->velocity().x() * collisionNormal.y() + _one->velocity().y() * collisionNormal.x()) * remainingTime;
_one->setVelocity(QVector2D(dotProduct * collisionNormal.y(), dotProduct * collisionNormal.x()));
return true;
}
return false;
}

Теперь проблемы — В следующих ситуациях движущаяся сущность полностью останавливается. Я ожидал, что он будет скользить горизонтально в обоих случаях. Мне не удалось определить, является ли это проблемой с кодом обнаружения столкновения или кодом ответа на столкновение:

1.
|¯¯¯¯¯¯¯¯|
| Entity |
|        |
|________|
|¯¯¯¯¯¯¯¯|     ⇲ velocity
| Block  |
|        |
|________|

2.
|¯¯¯¯¯¯¯¯|
| Entity |
|        | ⇲ velocity
|________|
|¯¯¯¯¯¯¯¯‖¯¯¯¯¯¯¯¯|
| Block  ‖ Block  |
|        ‖        |
|________‖________|