c++matrixvectorrenderingphysics-engine

Precision of rotation operation of vectors


I'm currently making a simple 2D physics engine and have been having trouble with accurate rotations. I have a matrix/vector library that I wrote that can rotate vectors by creating a rotation matrix. I am using SFML to render the shapes, and I notice that when I rotate the shapes, which takes in the shapes local coordinates, multiplies it with the rotation matrix and adds it to the current position of the shape (taken from the centre of it), the shape is no longer correct as in the vertices are slightly off from where they are meant to be. I have spent a while trying to fix it but I am not sure what the fault is. enter image description here enter image description here

I have tried switching from float to double but the problem is still present. I thought that the error from such operation is small and unnoticeable but it is very clear. Also at rotation of 0 radians it is fine, but if I rotate the shape by a few rotations and go back to 0 the inaccuracy is also visible for 0. My renderer scales the coord by a factor, if this is set to 10 the error is more clear. Here is the code for the components most important to this issue, if more is needed I'll be glad to add more:

/*! 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);
    }
}

edit:

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;
}

The rotation is applied only to the (local) vertices once, each iteration

enter image description here

note I increased the size of this shape so it's easy to see


Solution

  • My matrix multiplication uses int as the running total causing rounding error, simply changing this to double/float fixes the issue. Thank you everyone for the help.

    /*! 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);
        double 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;
    }