Верле: разрешаем коллизии (часть 1) / Хабр

Всех приветствую! Сегодня мы попробуем написать некое подобие простейшего физического движка.

Введение

Из жизни мы знаем, что если точка в момент времени имеет координаты $\vec x = (x;y)$ , и двигается в этот момент времени со скоростью $\vec v$ , то через $\Delta t \rightarrow 0$ времени координаты у точки будут "примерно" $\vec x = (x + \vec v_x \Delta t;y + \vec v_y \Delta t)$ :

$\vec{x}(t+\Delta t) = \vec{x}(t) + \vec{v}(t)\Delta t$

Мы пишем код, поэтому нам нужна эта формула в дискретной форме. Минимальным отрезком времени (ближе всего к нулю) будет время, за которое проходит одна итерация "игрового" цикла (где - номер итерации цикла, а $\Delta t$ - время, за которое проходит итерациия):

$\vec{x}_{n+1} = \vec{x}_{n} + \vec{v}_{n} \Delta t$

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

$\vec{x}_{n+1} = \vec{x}_n + (\vec{v}_{n-1} + \vec{a}_n \Delta t )\Delta t =\vec{x}_n + \vec{v}_{n-1} \Delta t + \vec{a}_n \Delta t^2 = \vec{x}_n + \vec{x}_{n} - \vec{x}_{n-1} + \vec{a}_n \Delta t^2$

Получаем такую формулу:

$\vec{x}_{n+1} = 2\vec{x}_n - \vec{x}_{n-1} + \vec{a}_n \Delta t^2$

Если интересно, «как оно на самом деле», можно ознакомиться со статьей на Википедии.

Начинаем писать код

Для начала напишем простенький класс вектора (x;y) для $\vec x, \vec a$ . Нам нужно, чтобы эти вектора можно было складывать/вычитать между собой, умножать/делить на число:

#pragma once
#include <cmath>

namespace eng {
template <typename T> struct Vec2 {
  T x, y;

  Vec2() : x{0}, y{0} {};
  Vec2(T _x, T _y) : x{_x}, y{_y} {};

  T length() const { return std::sqrt(x * x + y * y); }

  Vec2 &operator=(const Vec2 &other) {
    x = other.x;
    y = other.y;
    return *this;
  }

  Vec2 operator+(const Vec2 &other) const {
    return Vec2{x + other.x, y + other.y};
  }
  Vec2 operator-(const Vec2 &other) const {
    return Vec2{x - other.x, y - other.y};
  }

  void operator+=(const Vec2 &other) {
    x += other.x;
    y += other.y;
  }
  void operator-=(const Vec2 &other) {
    x -= other.x;
    y -= other.y;
  }

  Vec2 operator*(const T value) const { return Vec2{x * value, y * value}; }
  Vec2 operator/(const T value) const { return Vec2{x / value, y / value}; }
};
}

Такого функционала нам будет достаточно. Вообще говоря, даже не нужно было делать класс шаблонным.

Теперь пропишем константы - ускорение свободного падения, размеры экрана, область, которая будет "стеной" для наших объектов (можно было взять окно в качестве такой области, но с окружностью проще понять, находимся ли мы за ее пределами):

#pragma once
#include <Vector2.hpp>

namespace constants {
// ускорение свободного падения
const inline eng::Vec2 gravity = {0.0f, 1000.0f};

// размеры экрана
const inline int screenWidth = 1280;
const inline int screenHeight = 720;

// область, которую нельзя покидать нашим объектам
const inline float areaRadius = 300.f;
const inline float areaX = constants::screenWidth / 2.f;
const inline float areaY = constants::screenHeight / 2.f;
} // namespace constants

Перейдем к написанию класса для сущностей нашего движка, в его объектах мы будем хранить текущую позицию, предыдущую позицию, ускорение тела (все в векторах) + методы для обновления позиции по формуле из начала статьи (и еще объект, который будем использовать для отображения нашего абстрактного тела на экране):

#pragma once
#include "Constants.hpp"
#include "Vector2.hpp"
#include <SFML/Graphics.hpp>
#include <iostream>

namespace eng {
struct VerletObject {
  // вектора из формулы
  Vec2<float> positionCurrent;
  Vec2<float> positionOld;
  Vec2<float> acceleration;

  // в качестве графической библиотеки будем использовать СФМЛ
  sf::CircleShape sfShape;
  float radius;

  // не забываем сделать центр окружности центром шейпа, по умолчанию 
  // им является левый верхний угол
  VerletObject(float xPos, float yPos, float _radius, sf::Color color)
      : positionCurrent{xPos, yPos}, positionOld{xPos, yPos}, radius{_radius} {
    sfShape.setRadius(radius);
    sfShape.setOrigin(radius, radius);
    sfShape.setPosition(xPos, yPos);
    sfShape.setFillColor(color);
  }
  

  // все в соответствии с формулой
  void updatePosition(float dt) {
    Vec2<float> velocity = positionCurrent - positionOld;
    positionOld = positionCurrent;

    positionCurrent += velocity + constants::gravity * dt * dt;

    sfShape.setPosition(positionCurrent.x, positionCurrent.y);
  }

};
}

Начинаем реализовывать класс, который будет хранить в себе все VerletObject, обновлять им позиции, искать коллизии, разрешать их - словом, движок. Для начала напишем метод, вызовом которого мы будем не давать покидать заданную область нашим телам и метод, который будет вызывать метод обновления позиции каждому из тел:

#pragma once
#include "Constants.hpp"
#include "Vector2.hpp"
#include "VerletObject.hpp"
#include <SFML/Window.hpp>
#include <vector>

namespace eng{
class Game {
private:
  //в массиве храним объекты движка
  std::vector<VerletObject *> objects;
  // указатель на окно, в котором мы будем показывать тела
  sf::RenderWindow *window;

  // так обновляем позицию им всем 
  void updatePositions(float dt) {
    for (auto *object : objects) {
      object->updatePosition(dt);
    }
  }

  // так не даем объекту покидать разрешенную область
  void applyConstraint() {
    const Vec2 centerPosition{constants::areaX, constants::areaY};
    
    // для каждого объекта
    for (auto *object : objects) {
      // считаем радиус-вектор от центра допустимой области к объекту
      // ищем его модуль
      const Vec2 vecToObj = object->positionCurrent - centerPosition;
      const float distToObj = vecToObj.length();

      // если объект выходит за границы области
      if (distToObj > constants::areaRadius - object->radius) {

        // берем единичный вектор (направление от ц. области к ц. объекта) 
        const Vec2<float> normalized = vecToObj / distToObj;
        // обновляем позицию так, чтобы наш объект был внутри области
        // по-сути, мы двигаем его ближе к центру области по прямой, проходящей
        // через центр объекта и центр области
        object->positionCurrent =
            centerPosition +
            normalized * (constants::areaRadius - object->radius);
      }
    }
  }
  }
}

Чек-поинт

Пора посмотреть, что у нас получается.

Для этого напишем конструктор и два метода: один для добавления объектов, второй вызвающий два описанных выше (в него мы будем передавать $\Delta t$ ) и отображающий их на экране :

//...
public:
  Game(sf::RenderWindow *_window) : window{_window} {};

  void addObject(float xPos, float yPos, float radius,
                 sf::Color color = sf::Color(sf::Color::Blue)) {
    VerletObject *obj = new VerletObject(xPos, yPos, radius, color);
    objects.push_back(obj);
  }

  void update(float dt) {
    applyConstraint();
    updatePositions(dt);

    for (auto *object : objects) {
      window->draw(object->sfShape);
    }
  }
//...

И наконец, main.cpp:

#include "Constants.hpp"
#include "Game.hpp"
#include "Random.hpp"
#include "Vector2.hpp"
#include <SFML/System.hpp>


int main() {
  
  // создаем окно
  sf::RenderWindow window(
      sf::VideoMode(constants::screenWidth, constants::screenHeight), "Verlet");
  window.setVerticalSyncEnabled(1);

  // создаем 'движок'
  eng::Game game(&window);

  // та самая область, которую нельзя покидать
  sf::CircleShape area;
  area.setOrigin(constants::areaRadius, constants::areaRadius);
  area.setPosition(constants::areaX, constants::areaY);
  area.setRadius(constants::areaRadius);
  area.setFillColor(sf::Color::White);
  area.setPointCount(200);

  
  // для получения Delta t
  sf::Clock deltaClock;
  sf::Time dt;
  while (window.isOpen()) {

    // закрываем окно, если мы его закрываем (обычно, нажимая на крестик в углу)
    sf::Event event;
    while (window.pollEvent(event)) {
      if (event.type == sf::Event::Closed)
        window.close();
    }

    // будем генерировать объекты по нажатию ПКМ в месте, где находится курсор
    if (sf::Mouse::isButtonPressed(sf::Mouse::Right)) {
      sf::Vector2i position = sf::Mouse::getPosition(window);

      // добавляем тело
      game.addObject(position.x, position.y, eng::getRandomInt(5, 30),
                     sf::Color(eng::getRandomInt(0, 255),
                               eng::getRandomInt(0, 255),
                               eng::getRandomInt(0, 255)));
    }


    window.clear();
    // сначала рисуем область, потом уже объекты, иначе мы их не увидим
    window.draw(area);
    game.update(dt.asSeconds());
    // отображаем что получилось
    window.display();

    // записываем время итерации
    dt = deltaClock.restart();
  }
  return 0;
}

Забыл еще одну вспомогательную функцию, рандомные цвета у наших тел для разнообразия:

int getRandomInt(int l, int r) {
  std::random_device rd;
  std::uniform_int_distribution<int> gen(l, r);
  return gen(rd);
}

Результат следующий:

Перейдем к написанию коллизии. Пока что мы будем использовать примитивный алгоритм, заключающийся в переборе всех пар объектов и сравнению расстояния между их центрами и суммой их радиусов: если оно меньше суммы радиусов (т.е. они пересекаются, что невозможно), то окружности надо "растолкнуть" на разницу расстояния между их центрами и суммой их радиусов вдоль прямой, соединяющей их центры. Этого достаточно ведь мы изменили координаты тела, что означает: оно приобрело скорость $\vec v = \vec x_n - \vec x_{n-1}$ и продолжит двигаться.

//...
void solveCollisions() {
    // перебираем все пары объектов
    for (int i = 0; i < objects.size(); ++i) {
      for (int j = 0; j < objects.size(); ++j) {
        // самому с собой столкнуться невозможно
        if (j == i)
          continue;

        // вектор от центра первой окр. к центру второй
        Vec2<float> collisionAxis =
            objects[i]->positionCurrent - objects[j]->positionCurrent;

        // если расстояние между ними больше, чем сумма радиусов
        // то они не контактируют
        const float dist = collisionAxis.length();
        if (dist > objects[i]->radius + objects[j]->radius)
          continue;

        // единичная версия нашего вектора от ц. первой окр. к ц. второй окр
        Vec2<float> normalized = collisionAxis / dist;
        // расстояние, на которое нам нужно отодвинуть друг от друга окружности
        // чтобы одна не была в другой
        const float delta = objects[i]->radius + objects[j]->radius - dist;
        
        // рассталкиваем их вдоль прямой, проходящей через их центры
        // соблюдая некое подобие закона сохранения импульса
         float weightDiff =
            objects[j]->radius / (objects[i]->radius + objects[j]->radius);
        objects[i]->positionCurrent += normalized * delta * weightDiff;
        objects[j]->positionCurrent -= normalized * delta * (1 - weightDiff);
      }
    }
  }
//...

Осталось добавить вызов этого метода в update.

И запускаем!

Получилось неплохо, хоть и не идеально (зато очень просто).

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