How can I make my force calculations better?

Question

I am making a simple physics solver (haven't gotten to collision resolution or detection yet, just finished making it mt'ed), and when I apply forces through "W", "A", "S" and "D", the movement of the entity feels a bit weird.

I would appreciate for someone to look at my force code and check if it is correct. Specifically the code pertaining to forces, because there are no syntax errors or anything like that, just a small semantic error. It's difficult to find anything about friction in games online and how to implement it, so I kind of just guessed. Here are the scripts: math.hpp:

#pragma once
//static
class Math {
public:
    static inline float Sign(float num) {
        return (num >= .0f) * 2.f - 1.f;
    }
    static inline float SignOrZero(float num) {
        return Sign(num) - (num == .0f);
    }
    //special case where i don't use camel case because it's such a commonly used function
    static inline float abs(float orig) {
        return orig * Sign(orig);
    }
    static inline float Min(float a, float b) {
        return a < b ? a : b;
    }
    static inline float Max(float a, float b) {
        return a > b ? a : b;
    }
    template<typename T2>
    static inline Vector2<T2> Sign(Vector2<T2> v) {
        return Vector2<T2>(Sign(v.x), Sign(v.y));
    }
    template<typename T2>
    static inline Vector2<T2> SignOrZero(Vector2<T2> v) {
        return Vector2<T2>(SignOrZero(v.x), SignOrZero(v.y));
    }
    template<typename T2>
    static inline Vector2<T2> abs(Vector2<T2> orig) {
        return orig * Sign(orig);
    }
    template<typename T2>
    static inline Vector2<T2> Min(Vector2<T2> a, Vector2<T2> b) {
        return Vector2<T2>(Min(a.x, b.x), Min(a.y, b.y));
    }
    template<typename T2>
    static inline Vector2<T2> Max(Vector2<T2> a, Vector2<T2> b) {
        return Vector2<T2>(Max(a.x, b.x), Max(a.y, b.y));
    }
};

physics.cpp:

#include "physics.hpp"
#include "main.hpp"
#include "math.hpp"
#define IS_MULTI_THREADED
#define SubEntBP atomicNumEntities++;\
return Node<RigidBody*>::AddAtHead(rb, &entityHead)
Node<RigidBody*> *Physics::entityHead = nullptr;
std::atomic<int> volatile Physics::atomicNumEntities = 0;
Node<RigidBody*> *Physics::SubscribeEntity (const char* texturePath, FVector2 startPos, IntVec2 size, FVector2 initVel, float angle, float mass){
    RigidBody *rb = new RigidBody(startPos, initVel, angle, Images::LoadTexture(texturePath), size, mass);
    SubEntBP;
}
Node<RigidBody*> *Physics::SubscribeEntity (RigidBody *rb){
    SubEntBP;
}
static Node<RigidBody*>* curNode;
const FVector2 FVector2::One = FVector2(1.f, 1.f);
const FVector2 FVector2::Zero = {};
const FVector2 FVector2::Right = FVector2(1.f, .0f);
const FVector2 FVector2::Left = FVector2(-1.f, .0f);
const FVector2 FVector2::Up = FVector2(.0f, 1.f);
const FVector2 FVector2::Down = FVector2(.0f, -1.f);
const float Physics::fricCoef = 95.f;
const FVector2 Physics::frictionVec = (FVector2)FVector2::One * fricCoef;
const float Physics::frictionMagnitude = GetFrictionVec().Magnitude();
static FVector2 curVelSign;
//should be called after all behaviours' updates are called so transformations from this frame's position and rotation changes, and force calculations can be enacted
static FVector2 curFriction;
static float frictionDT;
static float fricCoefByDT;
void Physics::HandleRigidBody(RigidBody *rb) {
    frictionDT = Main::DeltaTime();
    //F_net=m*a_net
    //a_net=F_net/m
    //v=u+a_net*t
    //v=u+F_net/m*t
    //let t be the time since the last frame, a_net is the acceleration calculated from the mass and the net force applied since the first frame
    //then
    //v=velocity from last frame + net force applied from last frame / mass of entity * delta time
    //v=velocity from last frame + net force applied from last frame * inverse mass of entity * delta time
    //can't add the force after the total force is added to the velocity, otherwise the force will be reset before the start of the next frame.
    if (rb->velocity.Magnitude() > (fricCoefByDT = fricCoef * frictionDT)) rb->velocity -= Math::SignOrZero(rb->velocity) * fricCoefByDT;
    else rb->velocity = FVector2::Zero;
    rb->velocity += rb->force * rb->invMass * Main::DeltaTime();
    rb->position += rb->velocity;
    rb->position.IntoRectXY(rb->rect);
}
void Physics::ThreadFunc(int index) {
    Node<RigidBody*> *curEntity;
    while (1) {
        {
            std::unique_lock<std::mutex> lock(threadFuncMutexes[index]);
            while (!canRunThread[index] && !stopThread[index]) threadFuncConds[index].wait(lock);
            if (stopThread[index]) return;
        }
        curEntity = entityHead;
        for (int i = 0; i < index; i++) Node<RigidBody*>::Advance(&curEntity);
        HandleRigidBody(curEntity->value);
        {
            std::unique_lock<std::mutex> lock(mainWaitMutexes[index]);
            canRunThread[index] = false;
        }
        mainWaitConds[index].notify_one();
    }
}
void Physics::Init() {
#ifdef IS_MULTI_THREADED
    for (int i = 0; i < MAX_THREAD_COUNT; i++) {
        workers[i] = std::thread(ThreadFunc, i);
    }
#endif
}
static RigidBody* curRigidBody;
static int threadIndex;
std::atomic<bool> Physics::canRunThread[MAX_THREAD_COUNT] = { 0 };
bool Physics::stopThread[MAX_THREAD_COUNT] = { 0 };
std::condition_variable Physics::threadFuncConds[MAX_THREAD_COUNT];
std::condition_variable Physics::mainWaitConds[MAX_THREAD_COUNT];
std::mutex Physics::threadFuncMutexes[MAX_THREAD_COUNT];
std::mutex Physics::mainWaitMutexes[MAX_THREAD_COUNT];
std::thread Physics::workers[MAX_THREAD_COUNT];
void Physics::Update(float dt) {
    curNode = entityHead;
    threadIndex = 0;
    while (curNode) {
#ifdef IS_MULTI_THREADED
        {
            std::unique_lock<std::mutex> threadFuncLG(threadFuncMutexes[threadIndex]);
            canRunThread[threadIndex] = true;
        }
        threadFuncConds[threadIndex].notify_one();
        threadIndex++;
#else 
        HandleRigidBody(curNode->value);
#endif
        Node<RigidBody*>::Advance(&curNode);
    }
    curNode = entityHead;
    threadIndex = 0;
    while (curNode) {
        curRigidBody = curNode->value;
#ifdef IS_MULTI_THREADED
        {
            std::unique_lock<std::mutex> lock(mainWaitMutexes[threadIndex]);
            while (canRunThread[threadIndex]) {
                mainWaitConds[threadIndex].wait(lock);
            }
            threadIndex++;
        }
#endif
        SDL_RenderCopy(Main::renderer, curRigidBody->texture, nullptr, curRigidBody->rect);
        curRigidBody->force = FVector2::Zero;
        Node<RigidBody*>::Advance(&curNode);
    }
}
//TODO: make each of the cleanups for this multithreaded
void Physics::Finalize() {
    curNode = entityHead;
    threadIndex = 0;
    while (curNode) {
        curNode->value->Finalize();//change this to finalizer function with deletion of node value
        Node<RigidBody*>::Advance(&curNode);
    }
#ifdef IS_MULTI_THREADED
    for (int i = 0; i < MAX_THREAD_COUNT; i++) {
        {
            std::unique_lock<std::mutex> lock(threadFuncMutexes[i]);
            stopThread[i] = true;
        }
        threadFuncConds[i].notify_one();
        cout << "trying to join threads...\n";
        workers[i].join();
        cout << "done\n";
        char* line = new char[MAX_PATH], * file = new char[MAX_PATH];
        sprintf(line, "%d", __LINE__); sprintf(file, "%s", __FILE__); cout << (string("to get rid of this text and the \"triyng to join threads... done\" text, goto ") + file + ": " + line + '\n').c_str();//all on the same line so there is no offset between the line printed and where the original source code lies.
    }
#endif
}

and player.cpp:

#include "textures.hpp"
#include "winMgr.hpp"
#include <tuple>
#include "create shapes.hpp"
#include "array.hpp"
#define PLAYER_WIDTH 100
#define PLAYER_HEIGHT 100
float Player::accel = 200.f;
float Player::speed = 1500.f;
static RigidBody *player;
static Node<RigidBody*> *plrNode;
void Player::Update() {
    player->AddForce(Main::fInputVec * accel);
    if (player->velocity.Magnitude() > speed) player->velocity = player->velocity.Normalized() * speed;
}
void Player::Init() {
    plrNode = Physics::SubscribeEntity(Shapes::squarePath.c_str(), Main::DisplaySize / 2.0f, IntVec2(PLAYER_WIDTH, PLAYER_HEIGHT));
#define NUM_SHAPES_TEMP 5U
    Node<RigidBody*>** shapes = Shapes::CreateShapes(NUM_SHAPES_TEMP, 1.f);
    Node<RigidBody*>* shapesArr[NUM_SHAPES_TEMP];
    Array<Node<RigidBody*>*>::CopyTo(shapes, shapesArr, sizeof(shapesArr));
    for (auto i : shapesArr) {
        i->value->position = Main::DisplaySize / 2.f;
    }
    player = plrNode->value;
}

From here you can clearly see how I'm applying forces. This is in SDL2 c++.

Answer 1

Don't implement everything from scratch

Your goal is to make a physics solver. While that might require mathematics and 2D vectors, it is not your goal to implement those other things. I strongly recommend that you use the standard library, and where appropriate, some external libraries to avoid having to implement everything from scratch.

For example, for regular floating point variables, use std::abs(), std::min() from <cmath>. Your response to Harith's question about why you wrote your own is “mine are inlined”, but any compiler with optimizations enabled will inline the standard math functions just as well.

The standard library also comes with containers such as std::list and std::vector, which will do memory allocation of the items you store in them for you, so no need for manual calls to new and accidentally forgetting delete.

For 2D vectors, I recommend that you use a vector math library such as Eigen or GLM. The few functions you have implemented so far are not going to be enough.

For entity management, consider using an entity component system such as EnTT.

With all that out of the way, you can concentrate on the actual physics engine. Of course, if your actual end goal is to make a game, then I'd go so far as to recommend you use an existing physics engine instead of writing your own.

Avoid macros wherever possible

Macros are problematic for various reasons. Consider SubEntBP: what if you wanted to call this conditionally, and wrote:

if (some_condition)
    SubEntBP;

This immediately introduces a hard to find bug where an entity gets added but the atomic counter is not incremented accordingly.

Whenever possible, use static constexpr variables instead of #defineing a constant, and in case of statements, write regular functions:

static constexpr bool isMultithreaded = true;
static auto SubEntBP(auto* rb) {
    atomicNumEntities++;
    return Node<RigidBody*>::AddAtHead(rb, &entityHead);
}

Or since Physics::SubscribeEntity(RigidBody *rb) just calls SubEntBP, why not move all the code into that function, and call this overload from the other one?

Node<RigidBody*> *Physics::SubscribeEntity (RigidBody *rb){
    atomicNumEntities++;
    return Node<RigidBody*>::AddAtHead(rb, &entityHead);
}

Node<RigidBody*> *Physics::SubscribeEntity (const char* texturePath, FVector2 startPos, IntVec2 size, FVector2 initVel, float angle, float mass){
    SubscribeEntity(new RigidBody(startPos, initVel, angle, Images::LoadTexture(texturePath), size, mass));
}

Be aware of the scope of atomic operations

While Physics::atomicNumEntities is an atomic integer, calling SubEntBP will not both add an entity to the list and increment atomicNumEntities in one single atomic operation. To me this looks like a problem waiting to happen. If this whole action should be atomic, just use a std::mutex.

Issues with the thread pool

In Physics::Update(), I see array out of bounds accesses happen whenever the number of entities is larger than MAX_THREAD_COUNT.

Conversely, in Physics::ThreadFunc(), each thread will only ever process a single entity before setting its own canRunThread to false.

Use more features from C++20

In the comments you mention you target C++20. In that case, consider using std::jthread to avoid having to manually join threads, std::format() to format strings, std::source_location to avoid having to use the __LINE__ and __FILE__ macros,

Avoid doing too much on one line of code

This line of code is doing three things:

if (rb->velocity.Magnitude() > (fricCoefByDT = fricCoef * frictionDT)) rb->velocity -= Math::SignOrZero(rb->velocity) * fricCoefByDT;

First it does a calculation and stores it in fricCoefByDT, then it compares that against rb->velocity.Magnitude(), and then it update rb->velocity. It is very easy to miss that first thing. I suggest splitting this into three line:

fricCoefByDT = fricCoef * frictionDT;

if (rb->velocity.Magnitude() > fricCoefByDT)
    rb->velocity -= Math::SignOrZero(rb->velocity) * fricCoefByDT;

Avoid global variables

You have several global variables that are unnecessary. For example, in physics.cpp, frictionDT and fricCoefByDT should just be local variables inside Phsyics::HandleRigidBody(). There are other variables that I don't see used anywhere else in your code, but even if they are, it looks like they should be local variables as well, like curVelSign and curFriction.

There are dangers with having global variables. In a large program, it's easy to create conflicts with global variables with the same name defined in multiple source files. There is also a risk of reusing the same variable from multiple threads.