Коллайдеры

Чтобы избежать столкновений роботов с окружающими объектами и друг с другом, используют коллайдеры.

В данном фреймворке коллайдеры реализованы в следующей логике:

коллайдер инициализируется моделями коллизий

/**
 * инициализация коллайдера
 *
 * @param groupedModelPaths вектор векторов путей к моделям. это связано с тем,
 * что сцена строится на основе набора описаний роботов
 * каждый из них содержит список путей к своим моделям
 * @param subColliders флаг, нудно ли составить виртуальные сцены для
 * каждого подмножества объектов сцены
*/
virtual void init(std::vector<std::vector<std::string>> groupedModelPaths, bool subColliders) = 0;

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

/**
 * проверка соответствует ли состояние сцены столкновению
 * @param matrices список матриц преобразований звеньев
 * @return флаг, соответствует ли состояние сцены столкновению
*/
virtual bool isCollided(std::vector<Eigen::Matrix4d> matrices) = 0;

Иными словами, коллайдер оперирует только 3D объектами и их матрицами преобразований.

Коллизией называется столкновение полигонов моделей (обычно для коллизий хранят упрощённые модели, а для рисования - полноценные).

Все коллайдеры должны расширять базовый класс core.collider.base.Collider. В нём все методы чисто виртуальные, т.е. не имеют реализации.

Коллайдеры строятся по принципу, что внутри себя они хранят списки звеньев для каждого робота сцены, поэтому инициализация выполняется вектором векторов строк (по вектору строк для каждого робота).

Также для режима multiRobot коллайдер строит подколлайдеры для выбранного робота и статических объектов.

/**
 * @brief проверка соответствует ли состояние сцены столкновению
 * проверка соответствует ли состояние сцены (список матриц преобразований звеньев
 * только для задействованных объектов)
 * столкновению при этом учитываются только объекты с индексами из robotIndexes
 * @param matrices
 * @param robotIndexes
 * @return флаг, соответствует ли состояние сцены столкновению
*/
virtual bool isCollided(std::vector<Eigen::Matrix4d> matrices, std::vector<int> robotIndexes) = 0;

Solid Коллайдер

Первый коллайдер использует openSource библиотеку solid3. Он расширяет базовый класс Collider.

Библиотека solid построена так, что нужно сначала задать матрицы преобразования, а потом выполнить то или иное действие: нарисовать, проверить коллизии и т.д. Поэтому метод в начале пытается заблокировать мьютекс _setTransformMutex

Этот мьютекс разблокируется командой _makeFree()

Каждое звено описывается объектом класса project.core.collider.base.Solid3DObject. Внутри этого класса содержится как логика для работы с объектами библиотеки solid3, так и класс project.core.collider.base.StlShape.

Для работы в многопоточных программах написан класс синхронного коллайдера project.core.collider.SolidSyncCollider. Он является надстройкой над массивом однопоточных коллайдеров заданной длины. Каждому коллайдеру соответствует свой мьютекс.

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