Точность операции вращения векторов

В настоящее время я создаю простой физический 2D-движок, и у меня возникли проблемы с точным вращением. У меня есть написанная мной библиотека матриц/векторов, которая может вращать векторы, создавая матрицу вращения. Я использую SFML для рендеринга фигур, и я заметил, что когда я вращаю фигуры, которые принимают локальные координаты фигур, умножают их на матрицу вращения и добавляют их к текущему положению фигуры (взятому из ее центра). ), форма больше не правильная, так как вершины немного не соответствуют тому месту, где они должны быть. Я потратил некоторое время, пытаясь это исправить, но не знаю, в чем проблема.

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

/*! Rotates matrix by theta radians
 */
physicsEngine::Matrix physicsEngine::rotationMat2D(double theta)
{
    double cTheta = std::cos(theta); double sTheta = std::sin(theta);
    return physicsEngine::Matrix(2, 2, { cTheta, -sTheta, sTheta, cTheta });
}

/*! Matrix multiplication operator
 */
physicsEngine::Matrix physicsEngine::Matrix::operator*(const Matrix& other) const {
    if (this->cols != other.rows) {
        throw std::invalid_argument("left matrix must have same number of columns as left matrix rows");
    }
    physicsEngine::Matrix t(this->rows, other.cols);
    int runningTot;
    for (int leftRow = 0; leftRow < this->rows; leftRow++) {
        for (int rightCol = 0; rightCol < other.cols; rightCol++) {
            runningTot = 0;
            for (int leftCol = 0; leftCol < this->cols; leftCol++) {
                runningTot += (*this)(leftRow, leftCol) * other(leftCol, rightCol);
            }
            t(leftRow, rightCol) = runningTot;
        }
    }

    return t;
}

#ifndef physicsEngine_RENDER_HPP
#define physicsEngine_RENDER_HPP

#include <vector>
#include <SFML/Window.hpp>
#include <SFML/Graphics.hpp>
#include <SFML/Graphics/Drawable.hpp>
#include "world.hpp"



namespace physicsEngine {
    constexpr int SCALE = 1;
    class ShapeGroup : public sf::Drawable {
        private:
        public:
            std::vector<sf::Shape*> shapes;
            ShapeGroup(std::initializer_list<sf::Shape*> shapes);
            void addShape(sf::Shape* shape);
            virtual void draw(sf::RenderTarget& target, sf::RenderStates states) const;
            virtual void setPosition(sf::Vector2f position);
            virtual void setOrigin(sf::Vector2f position);
            virtual void setRotation(double angle);
    };


    class Renderer {
        public:
            World* sim;
            int width, height;
            std::vector<physicsEngine::ShapeGroup*> shapes;
            bool showInfo;
            sf::Vector2f sfPosition(const RigidBody& body) const;
            Renderer(World* sim, bool showFPS);
            void update(const double& dt);
            sf::RenderWindow window;
            sf::Text info;
            sf::Font font;
    };
}
#endif // physicsEngine_RENDER_HPP

// todo:
//      choose a good scale
//      adding/removing objects to renderer after construction

#include "physicsEngine/render.hpp"
#include "physicsEngine/world.hpp"
#include "physicsEngine/constants.hpp"
#include <string>
#include <numbers>
#include <stdlib.h>

/*! Converts position of body in physicsEngine::vector to sf::Vector2f
 *  Uses physicsEngine::SCALE as scaling factor
 */
sf::Vector2f physicsEngine::Renderer::sfPosition(const physicsEngine::RigidBody& body) const {
    Matrix pos = body.getPos();
    return sf::Vector2f(pos(0, 0) * physicsEngine::SCALE, this->height - pos(1, 0) * physicsEngine::SCALE);
}

/*! Constructor for renderer
 *
 * Takes a world, which is our virtual representation of a simulation manager
 * Convert all world dimensions to the dimensions for rendering
 * Flag showInfo for diagnostics
 */
physicsEngine::Renderer::Renderer(World* sim, bool showInfo) {
    std::srand(time(NULL));
    this->showInfo= showInfo;
    this->sim = sim;
    this->width = sim->X * physicsEngine::SCALE; this->height = sim->Y * physicsEngine::SCALE;
    this->window.create(sf::VideoMode(this->width, this->height), "Renderer", sf::Style::Default);

    // not sure about paths, this doenst work right now and its late, maybe require user to put in PATH vars, also maybe average it so its somewhat visible
    if (this->font.loadFromFile("/home/hirok/dev/physicsEngine/arial.ttf")) {
        this->info.setFont(this->font);
        this->info.setCharacterSize(20);
        this->info.setPosition(0, this->height - 20);
        this->info.setFillColor(sf::Color::White);
    }
    for (physicsEngine::RigidBody* body : sim->bodies) {
        if (body->getType() == physicsEngine::Rectangle) {
            this->shapes.push_back(new physicsEngine::ShapeGroup({ new sf::RectangleShape(sf::Vector2f(body->getW() * physicsEngine::SCALE, body->getH() * physicsEngine::SCALE)) }));
        }
        else {
            this->shapes.push_back(new physicsEngine::ShapeGroup({ new sf::CircleShape(body->getR() * physicsEngine::SCALE) }));
        }
        this->shapes.back()->shapes.back()->setFillColor(sf::Color(std::rand() % 256, std::rand() % 256, std::rand() % 256));
        if (body->getType() == physicsEngine::Rectangle) {
            this->shapes.back()->setOrigin(sf::Vector2((float)(physicsEngine::SCALE * body->getW() / 2), (float)(physicsEngine::SCALE * body->getH() / 2)));
        }
        else {
            // ugly and not centered, fix later for aesthetics
            this->shapes.back()->addShape(new sf::CircleShape(10, 3));
            this->shapes.back()->shapes.back()->setPosition(sf::Vector2f(physicsEngine::SCALE * body->getR(), 0));
            this->shapes.back()->shapes.back()->setFillColor(sf::Color::Blue);
            this->shapes.back()->setOrigin(sf::Vector2f(physicsEngine::SCALE * body->getR(), physicsEngine::SCALE * body->getR()));
            
        }
    }

}

/*! Redraws the shapes on the screen with new scaled position
 *  Called every render cycle
 *  Can show time delta from processing
 */
void physicsEngine::Renderer::update(const double& dt) {
    this->window.clear();
    for (int i = 0; i < this->shapes.size(); i++) {
        this->shapes[i]->setPosition(this->sfPosition(*this->sim->bodies[i]));
        this->shapes[i]->setRotation(this->sim->bodies[i]->getT() * 180 / std::numbers::pi);
        this->window.draw(*this->shapes[i]);
    }
    if (this->showInfo) {
        this->info.setString(std::to_string(1 / dt));
        this->window.draw(this->info);
    }
    // this->window.display();
}



physicsEngine::ShapeGroup::ShapeGroup(std::initializer_list<sf::Shape*> shapes) {
    for (int i = 0; i < shapes.size(); i++) {
        this->shapes.push_back(*(shapes.begin() + i)); //pointer/iterator magic
    }
}
void physicsEngine::ShapeGroup::addShape(sf::Shape* shape) {
    this->shapes.push_back(shape);
    // this->shapes.back()->setOrigin(this->shapes[0]->getOrigin());
}

void physicsEngine::ShapeGroup::draw(sf::RenderTarget& target, sf::RenderStates states) const {
    for (int i = 0; i < this->shapes.size(); i++) {
        target.draw(*this->shapes[i], states);
    }
}

void physicsEngine::ShapeGroup::setPosition(sf::Vector2f position) {
    for (int i = 0; i < this->shapes.size(); i++) {
        this->shapes[i]->setPosition(position);
    }
}

void physicsEngine::ShapeGroup::setOrigin(sf::Vector2f position) {
    this->shapes[0]->setOrigin(position);
}

void physicsEngine::ShapeGroup::setRotation(double angle) {
    for (int i = 0; i < this->shapes.size(); i++) {
        this->shapes[i]->setRotation(angle);
    }
}

редактировать:

void physicsEngine::RigidBody::getVerticesWorld(physicsEngine::Matrix(&res)[4]) {
    physicsEngine::Matrix rotMat = physicsEngine::rotationMat2D(this->theta);

    for (int i = 0; i < 4; i++) {
        res[i] = (rotMat * this->vertices[i]) + this->pos;
    }
}

int main() {
    // setup
    double movementUnit = 2;
    double zoomFactor = 1.1;
    physicsEngine::World sim(1500, 1000);
    physicsEngine::Matrix coords[4];
    sim.addBody(new physicsEngine::RigidBody(100, 100, 1, 0.5, 50, 0, 0, 0));
    sim.addBody(new physicsEngine::RigidBody(500, 500, 0, 0, 400, 400, 0, 0, 0));
    sim.addBody(new physicsEngine::RigidBody(10, 10, 1, 0, 1, 0, 0, 0));
    physicsEngine::Renderer ren(&sim, true);
    double dt = 0;

    // Initial view setup
    sf::View view = ren.window.getView();
    sf::Vector2f viewCenter = view.getCenter();

    while (ren.window.isOpen()) {
        auto startTime = std::chrono::high_resolution_clock::now();
        physicsEngine::Matrix pos = sim.bodies[0]->getPos();

        sf::Event event;
        while (ren.window.pollEvent(event)) {
            if (event.type == sf::Event::Closed) {
                ren.window.close();
            }
            if (event.type == sf::Event::KeyPressed) {
                if (event.key.code == sf::Keyboard::Left)
                    sim.bodies[0]->addPos(physicsEngine::Matrix(2, 1, { -movementUnit, 0 }));
                else if (event.key.code == sf::Keyboard::Right)
                    sim.bodies[0]->addPos(physicsEngine::Matrix(2, 1, { movementUnit, 0 }));
                else if (event.key.code == sf::Keyboard::Up)
                    sim.bodies[0]->addPos(physicsEngine::Matrix(2, 1, { 0, movementUnit }));
                else if (event.key.code == sf::Keyboard::Down)
                    sim.bodies[0]->addPos(physicsEngine::Matrix(2, 1, { 0, -movementUnit }));
                if (event.key.code == sf::Keyboard::A)
                    sim.bodies[1]->addTheta(-0.125);
                else if (event.key.code == sf::Keyboard::D)
                    sim.bodies[1]->addTheta(0.125);

            }
            if (event.type == sf::Event::MouseWheelScrolled) {
                if (event.mouseWheelScroll.delta > 0) {
                    // Zoom in
                    view.zoom(1.0f / zoomFactor);

                    // Adjust view center based on cursor position
                    sf::Vector2i mousePos = sf::Mouse::getPosition(ren.window);
                    sf::Vector2f worldPos = ren.window.mapPixelToCoords(mousePos);
                    view.setCenter(worldPos);
                }
                else if (event.mouseWheelScroll.delta < 0) {
                    // Zoom out
                    view.zoom(zoomFactor);

                    // Adjust view center based on cursor position
                    sf::Vector2i mousePos = sf::Mouse::getPosition(ren.window);
                    sf::Vector2f worldPos = ren.window.mapPixelToCoords(mousePos);
                    view.setCenter(worldPos);
                }
            }
        }

        ren.update(dt);

        // Apply the updated view
        ren.window.setView(view);

        // Draw points and lines
        sim.bodies[1]->getVerticesWorld(coords);
        sf::VertexArray lines(sf::LinesStrip, 5);
        for (int i = 0; i < 4; ++i) {
            lines[i].position = sf::Vector2f(static_cast<float>(coords[i](0, 0) * physicsEngine::SCALE),
                static_cast<float>(coords[i](1, 0) * physicsEngine::SCALE));
            lines[i].color = sf::Color::Red;

            sf::CircleShape circle(2); // radius of 2
            circle.setFillColor(sf::Color::Red);
            circle.setPosition(lines[i].position);
            ren.window.draw(circle);
        }
        // Close the loop by connecting the last point to the first
        lines[4].position = sf::Vector2f(static_cast<float>(coords[0](0, 0) * physicsEngine::SCALE),
            static_cast<float>(coords[0](1, 0) * physicsEngine::SCALE));
        lines[4].color = sf::Color::Red;

        ren.window.draw(lines);
        ren.window.display();
        std::cout << sim.bodies[1]->getT() << "\n";
        auto endTime = std::chrono::high_resolution_clock::now();
        dt = std::chrono::duration_cast<std::chrono::duration<double>>(endTime - startTime).count();
    }

    return 0;
}

Вращение применяется только к (локальным) вершинам один раз, на каждой итерации.

обратите внимание: я увеличил размер этой фигуры, чтобы ее было легче увидеть