Double-Pendulum Equation Bug-CodePudding

I have been making a double-pendulum simulator in C using Raylib and I have finished everything other than correctly implementing the equations. When doing so, however, no matter what I try, I can't seem to figure out why my implementation of the equations is not working. Most likely I am missing something fundamental but it's so hectic as is that it's difficult for me to understand. How would I implement these equations successfully? For reference:

Angular acceleration for pendulum 1 is angularA1

Angular acceleration for pendulum 2 is angularA2

I split the numerators in the equations into parts because it's simply too hard to keep track of it on one line. The problem is in main.cpp and the file generator is completely separate from the pendulum.

pendulum.h:


#ifndef DOUBLE_PENDULUM_SIM_PENDULUM_H
#define DOUBLE_PENDULUM_SIM_PENDULUM_H


class pendulum {
public:

    //Pendulum length
    float pLength{};

    //Pendulum mass
    float pMass{};

    //Pendulum's x component
    float x{};

    //Pendulum's y component
    float y{};

    //Pendulum's angular displacement relative to its origin
    float pAngle{};


public:

    void setAngularV(float angV);
    void setAngularA(float angA);

    [[nodiscard]] float getX() const ;
    [[nodiscard]] float getY() const ;

    [[nodiscard]] float getAngle() const ;
    [[nodiscard]] float getMass() const;
    [[nodiscard]] float getLength() const;

    void setX(float length, float angle);
    void setY(float length, float angle);

    void setLength(float length);
    void setMass(float mass);
    void setAngle(float angle);

    //Default constructor
    pendulum();

    //Easier way to initialize pendulum
    pendulum(float length, float mass, float angle) : pLength(length), pMass(mass), pAngle(angle) {}

    //De-constructor to make sure pendulum objects are destroyed
    ~pendulum();


};

#endif //DOUBLE_PENDULUM_SIM_PENDULUM_H

pendulum.cpp:


#include "includes/pendulum.h"
#include <cmath>
#include <iostream>
#include <raylib.h>


void pendulum::setX(float length, float angle) {
    x = length * sin(angle);
}

void pendulum::setY(float length, float angle) {
    y = length * cos(angle);
}

float pendulum::getX() const {
    return x;
}

float pendulum::getY() const {
    return y;
}

void pendulum::setLength(float length) {
    pLength = length;
}

void pendulum::setMass(float mass) {
    pMass = mass;
}


float pendulum::getAngle() const {
    return pAngle;
}

pendulum::~pendulum() {
    std::cout << "\nPendulum destroyed" << std::endl;
}

void pendulum::setAngle(float angle) {
    pAngle = angle * DEG2RAD;
}

float pendulum::getMass() const {
    return pMass;
}

float pendulum::getLength() const {
    return pLength;
}

//Default constructor
pendulum::pendulum() = default;

main.cpp:

#include <iostream>
#include <fstream>
#include "raylib.h"
#include "includes/pendulum.h"
#include "includes/GenerateFile.h"
#include <cmath>

void testPendulum();

GenerateFile generator;

pendulum pen1;
pendulum pen2;

//The universal constant for gravity, for these purposes however, it can be any value as long as it works
const float g = 1;

int main() {

    //Prompt to make the initial values themselves
    float uLength1, uLength2, uMass1, uMass2, uAngle1, uAngle2;

    //Recommendation: 100 200 for length, 20 25 for mass, 45 0 for angle
    try {

        std::cout << "Please choose the length of each pendulum, starting with Pendulum 1, then Pendulum 2. Each value provided can be up to 7 decimal digits, " << "\n" << "length MUST BE greater than 50 and less than 200" << "\n";
        std::cin >> uLength1 >> uLength2;
        std::cout << "Please choose the mass of each pendulum, starting with Pendulum 1, then Pendulum 2. Each value provided can be up to 7 decimal digits, " << "\n" << "mass MUST BE greater than 20 and less than 100" << "\n";
        std::cin >> uMass1 >> uMass2;
        std::cout << "Please choose the starting angle of each pendulum, starting with Pendulum 1, then Pendulum 2. Each value provided can be up to 7 decimal digits" << "\n";
        std::cin >> uAngle1 >> uAngle2;

    } catch (const std::exception & e) {
        std::cout << e.what();
    }

    //Init angular acceleration and angular velocity for pendulums

    float angularV1 = 0;
    float angularV2 = 0;

    //Pendulum 1 settings
    pen1.setMass(uMass1);
    pen1.setLength(uLength1);
    pen1.setAngle(uAngle1);
    pen1.setX(pen1.pLength,pen1.getAngle());
    pen1.setY(pen1.pLength, pen1.getAngle());

    std::cout << "X coord: " << pen1.getX() << " Y coord: " << pen1.getY() << std::endl;

    //Pendulum 2 settings
    pen2.setMass(uMass2);
    pen2.setLength(uLength2);
    pen2.setAngle(uAngle2); //Can only set this once and cant anywhere else, why?
    pen2.setX( pen2.pLength,pen2.getAngle());
    pen2.setY( pen2.pLength,pen2.getAngle());
    pen2.x = pen1.getX()   pen2.getX();
    pen2.y = pen1.getY()   pen2.getY();

    std::cout << "X coord: " << pen2.getX() << " Y coord: " << pen2.getY() << std::endl;

    //Window settings
    const double screenWidth = 1440;
    const double screenHeight = 1080;

    Vector2 origin{(float) screenWidth/2,(float) screenHeight/3};

    InitWindow((int) screenWidth, (int) screenHeight, "Double-Pendulum-Sim");

    int frameCounter = 0;

    SetTargetFPS(60);

    //Set coords for pendulums
    float px1 = pen1.getX()   origin.x;
    float py1 = pen1.getY()   origin.y;

    float px2 = pen2.getX()   origin.x;
    float py2 = pen2.getY()   origin.y;

    //Load & start pathing
    RenderTexture2D target = LoadRenderTexture((int) screenWidth, (int) screenHeight);

    BeginTextureMode(target);
    ClearBackground(RAYWHITE);
    EndTextureMode();

    /****Write data to a file*****/

    //Init new Values.txt file
    generator.file.open("Values.txt");

    if(!generator.file) {
        perror("Error finding or opening file");
    } else {
        std::cout << "File opened successfully" << std::endl;
    }
    generator.file << "Angle 1    | X_1         | Y_1         | Angle 2    | X_2         | Y_2         | Frame # " << std::endl;



    //Main while loop
    while (!WindowShouldClose()) {
        Vector2 rod1{px1,py1};
        Vector2 rod2 {px2, py2};

        //Implement angular acceleration for first pendulum equations:
        float num1 = -g * (2 * pen1.getMass()   pen2.getMass()) * sin(pen1.getAngle());
        float num2 = -pen2.getMass() * g * sin(pen1.getAngle() - 2 * pen2.getAngle());
        float num3 = -2 * sin(pen1.getAngle() - pen2.getAngle()) * pen2.getMass();
        float num4 = pow(angularV2, 2) * pen2.getLength()   pow(angularV1,2) * pen1.getLength() * cos(pen1.getAngle() - pen2.getAngle());
        float den1 = pen1.getLength() * (2*pen1.getMass()   pen2.getMass() - pen2.getMass() * cos(2*pen1.getAngle() - 2 * pen2.getAngle()));

        float angularA1 = (num1   num2   num3*num4) / den1;

        //Angular acceleration for second pendulum:
         num1 = 2 * sin(pen1.getAngle() - pen2.getAngle());
         num2 = (pow(angularV1,2.0) * pen1.getLength() * (pen1.getMass()   pen2.getMass()));
         num3 = g * (pen1.getMass()   pen2.getMass()) * cos(pen1.getAngle());
         num4 = pow(angularV2,2.0) * pen2.getLength() * pen2.getMass() * cos(pen1.getAngle() - pen2.getAngle());
         den1 = pen2.getLength() * (2*pen1.getMass()   pen2.getMass() - pen2.getMass() * cos(2*pen1.getAngle() - 2*pen2.getAngle()));

        float angularA2 = (num1 * (num2   num3   num4)) / den1;


        /**------------------Update------------------*/

            frameCounter  ;
            uAngle1  = angularV1;
            angularV1  = angularA1;
            pen1.setAngle(uAngle1); //Can only set this once and cant anywhere else, why?
            pen1.setX(pen1.pLength,pen1.getAngle());
            pen1.setY(pen1.pLength, pen1.getAngle());

            px1 = pen1.getX()   origin.x;
            py1 = pen1.getY()   origin.y;

            uAngle2  = angularV2;
            angularV2  = angularA2;
            pen2.setAngle(uAngle2); //Can only set this once and cant anywhere else, why?
            pen2.setX( pen2.pLength,pen2.getAngle());
            pen2.setY( pen2.pLength,pen2.getAngle());
            pen2.x = pen1.getX()   pen2.getX();
            pen2.y = pen1.getY()   pen2.getY();

            px2 = pen2.getX()   origin.x;
            py2 = pen2.getY()   origin.y;

            //Write data to file by first getting the values of the pendulums:
            generator.getData(std::to_string(frameCounter),std::to_string(uAngle1),std::to_string(px1),std::to_string(px2),std::to_string(uAngle2),std::to_string(px2),std::to_string(py2));

    /**---------------------------------Draw-Pendulums & Path---------------------------------- */

    BeginDrawing();


            BeginTextureMode(target);
            DrawCircleV(rod2, 2.0f, RED);
//            DrawPixelV(rod2, RED);
            EndTextureMode();

            DrawTextureRec(target.texture, (Rectangle){0,0, (float) target.texture.width, (float) -target.texture.height}, (Vector2){0,0}, WHITE);

            ClearBackground(RAYWHITE);

            DrawFPS(100, 100);


            DrawLineEx(origin, rod1, 5.0f, BLACK);
            DrawCircle( px1,py1,pen1.pMass,BLACK);

            DrawLineEx(rod1, rod2, 5.0f, BLACK);
            DrawCircle(px2,py2,pen2.pMass,BLACK);

            std::cout << "Frame #: " <<  frameCounter << std::endl;
            std::cout << "Pendulum 1 Angle: " << uAngle1 << std::endl;
            std::cout << "Pendulum 2 Angle: " << uAngle2 << std::endl;
            std::cout << "Pendulum 1 X & Y: " << pen1.getX() << " " << pen1.getY() << std::endl;
            std::cout << "Pendulum 2 X & Y: " << pen2.getX() << " " << pen2.getY() << std::endl;

    EndDrawing();

    }

    CloseWindow();

    generator.file.close();

    return 0;
}

//Test function
void testPendulum() {

    try {
        pen1.setMass(20.0f);
        pen1.setLength(150.0f);
        pen1.setAngle(0.0f);
        pen1.setX(pen1.pLength,pen1.getAngle());
        pen1.setY(pen1.pLength, pen1.getAngle());

        std::cout << "X coord: " << pen1.getX() << " Y coord: " << pen1.getY() << std::endl;

        pen2.setMass(50.0f);
        pen2.setLength(150.0f);
        pen2.setAngle(0.0f);
        pen2.setX( pen2.pLength,pen2.getAngle());
        pen2.setY( pen2.pLength,pen2.getAngle());
        pen2.x = pen1.getX()   pen2.getX();
        pen2.y = pen1.getY()   pen2.getY();


        std::cout << "X coord: " << pen2.getX() << " Y coord: " << pen2.getY() << std::endl;

    } catch (const std::exception & e) {
        std::cout << e.what();
    }

}

These are the Double Pendulum Equations I'm trying to use:

CodePudding user response：

Be careful, you've made no attempt to use sane units in your code. You've mismatched degrees and radians in at least one spot too by doing uAngle1 = angularV1. uAngle1 is in degrees, but the angularV1 should likely be produced in radians per second.

That brings in the missing aspect of time in your numerical integration.

Numerical integration typically works by assuming velocity is linear under short time intervals.

A typical numerical integration inner loop calculation looks like this:

acceleration = netForce / mass;
velocity  = acceleration * timestep;
position  = velocity * timestep;

Whereas you just do:

velocity  = acceleration;
position  = velocity;

...which is like using a timestep of 1. The timestep must be small. For simulations that make gradual changes, it may be sufficient to just use the frame interval (e.g.: use a timestep of 1/60th of a second for a 60 fps simulation). Some simulations will actually subdivide the frame interval into a number of simulations steps (e.g. you could perform 100 updates per frame, each with a very small timestep of ((1/60) / 100).

Whatever happens, you should reconcile your units. Make sure your gravitational constant makes sense given the scale of your world. g = 1 is likely much too large. (@Gene made an excellent comment to this effect.) Also make sure your timesteps are reasonable. An implicit timestep may be interpreted as a time interval of 1 second (instead of 1 frame) and your simulation may be running an additional 60 times faster than you expect. All this leads to large values and numerical instability in the integration calculation, which depends on small changes.

You're currently using a distance unit of pixels, but perhaps it makes more sense to use a unit of meters and then scale your objects to fit the screen with a sane camera and viewport calculation.
You're currently using a time unit of frames, but perhaps it makes more sense to use a unit of seconds and make use of either the known frames-per-second rate of your simulation, or keep track of the actual elapsed time with a clock so that the simulation doesn't stutter if frames are dropped.
You're mismatching degrees and radians. It makes sense to use degrees when interacting with the user and radians in your physics calculations, but be careful not to mix them up during your calculations.