Feature/pendulum physics

.gitignore

@@ -12,5 +12,4 @@
 *.obj
 *.out
 *.exe
-*.sh
 *.ini

App/CMakeLists.txt

@@ -1,4 +1,17 @@
-set(APP_NAME PrototypeApp)
+set(APP_NAME Simulation)
+
+#Shaders and test folder path
+set(CIRCLE_SHADER "${CMAKE_SOURCE_DIR}/res/shaders/circle.shader")
+add_compile_definitions(CIRCLE_SHADER="${CIRCLE_SHADER}")
+
+set(LINE_SHADER "${CMAKE_SOURCE_DIR}/res/shaders/line.shader")
+add_compile_definitions(LINE_SHADER="${LINE_SHADER}")
+
+set(TRAIL_SHADER "${CMAKE_SOURCE_DIR}/res/shaders/trail.shader")
+add_compile_definitions(TRAIL_SHADER="${TRAIL_SHADER}")
+
+set(TEST_FOLDER_PATH "${CMAKE_SOURCE_DIR}/tests_results/")
+add_compile_definitions(TEST_FOLDER_PATH="${TEST_FOLDER_PATH}")
 
 add_executable( ${APP_NAME} app.cpp simulation2D.cpp pendulum.cpp numerical_eq.cpp)
 

App/app.cpp

@@ -2,41 +2,22 @@
 #include "numerical_eq.hpp"
 // #include "simulation3D.hpp"
 
-#include <cstdio> //fprintf
 #include <memory>
 
-void Error(const char* message){ fprintf(stderr, message); }
+int main(){
 
-int main(int argc, char* argv[]){
 
-    if (argc != 2) {
-        Error("Wrong number of arguments!\n");
-        return 1;
-    }
-
-    if (*argv[1] != '1' && *argv[1] != '2') {
-        Error("Wrong option! 1 - 2D simulation, 2 - 3D simulation\n");
-        return 1;
-    }
 
     /*
-        TEST FIRST
+        NUMERICAL TESTS FIRST
     */
     RunTests();
 
+    /*
+        SIMULATION APP
+    */
+    std::unique_ptr<Simulation> sim = std::make_unique<Simulation2D>();
 
-    char choise = *argv[1];
-    std::unique_ptr<Simulation> sim;
-
-    switch (choise)
-    {
-        case '1': sim = std::make_unique<Simulation2D>();  break;
-        // case '2': sim = std::make_unique<Simulation3D>();  break;
-
-        default: break;
-    }
-
-
     sim->initialize();
 
     sim->run();

App/numerical_eq.cpp

@@ -4,15 +4,13 @@
 #include <iostream>
 #include <cmath>
 #include <fstream>
-#include <iomanip>  // Do formatowania wyników (np. zaokrąglanie)
+#include <iomanip>
 
 //Constanst
 static double g = 9.81;
 static double m = 1.0; 
 static double l = 1.0;
 
-#define TEST_FOLDER_PATH "/home/marek/Dev/Projects/pulsarEngine/tests_results/"
-
 
 void RunTests(){
 

App/pendulum.cpp

@@ -21,6 +21,14 @@ void SinglePendulum::displayParameters(){
 
 }
 
+double SinglePendulum::getKinematicEnergy(){
+     return (0.5 * m * pow(l,2) * pow(thetadot,2));
+}
+
+double SinglePendulum::getPotencialEnergy(){
+     return ( m * g * l * (1 - cos(theta)));
+}
+
 
 DoublePendulum::DoublePendulum( float x0, float y0, double m1, double m2, double l1, double l2, double init_theta1,double init_theta2,double init_thetadot1, double init_thetadot2)
 :m1(m1), m2(m2), l1(l1), l2(l2),theta1(glm::radians(init_theta1)), theta2(glm::radians(init_theta2)), thetadot1(init_thetadot1), thetadot2(init_thetadot2), x0(x0), y0(y0)
@@ -35,6 +43,19 @@ DoublePendulum::DoublePendulum( float x0, float y0, double m1, double m2, double
 
 }
 
+
+double DoublePendulum::getKinematicEnergy(){
+    return ( ( 0.5 * m1 * l1 * l1 * thetadot1 * thetadot1 ) + 
+             ( 0.5 * m2 * ( (l1 * l1 * thetadot1 * thetadot1) + (l2 * l2 * thetadot2 * thetadot2) + (2 * l1 * l2 * thetadot1 * thetadot2 * cos(theta1 - theta2)) ) )
+            );
+}
+
+double DoublePendulum::getPotencialEnergy(){
+    return (   (-1.0 * m1 * g * l1 * cos(theta1)) - ( m2 * g * ( (l1 * cos(theta1)) + (l2 * cos(theta2)) ) )
+
+             );
+}
+
 void DoublePendulum::displayParameters(){
 
     std::cout << "Double Pendulum parameters: \n";
@@ -45,44 +66,115 @@ void ResetPendulum(SinglePendulum& pendulum){}
 
 void ResetPendulum(DoublePendulum& pendulum){}
 
-void SimulatePendulumEuler(SinglePendulum& pendulum){
+void SimulatePendulumApprox(SinglePendulum& pendulum){
 
     //Calculate properties in polar coordinates
-    double thetaddot = -(g / pendulum.l) * sin(pendulum.theta);
+    double thetaddot = -(g / pendulum.l) * pendulum.theta;
     pendulum.thetadot += (thetaddot * dt);    
     pendulum.theta += (pendulum.thetadot * dt);
 
     //Update cartesian coordinates based on polar coordinates values
     pendulum.x = pendulum.x0 + ( pendulum.l * sin(pendulum.theta));
     pendulum.y = pendulum.y0 - ( pendulum.l * cos(pendulum.theta));
+
 }
 
-void SimulatePendulumApprox(SinglePendulum& pendulum){
+void SimulatePendulumEuler(SinglePendulum& pendulum){
 
     //Calculate properties in polar coordinates
-    double thetaddot = -(g / pendulum.l) * pendulum.theta;
+    double thetaddot = -(g / pendulum.l) * sin(pendulum.theta);
     pendulum.thetadot += (thetaddot * dt);    
     pendulum.theta += (pendulum.thetadot * dt);
 
     //Update cartesian coordinates based on polar coordinates values
     pendulum.x = pendulum.x0 + ( pendulum.l * sin(pendulum.theta));
     pendulum.y = pendulum.y0 - ( pendulum.l * cos(pendulum.theta));
+}
+
+void SimulatePendulumHeun(SinglePendulum& pendulum){
+
+    //Calcualte k1,k2 parameters for theta and thetadot
+    double k1_theta = dt * pendulum.thetadot;
+    double k1_thetadot = dt * (-(g / pendulum.l) * sin(pendulum.theta));
+
+    double k2_theta = dt * ( pendulum.thetadot + (0.5 * k1_thetadot) );
+    double k2_thetadot = dt * ( -(g / pendulum.l) * sin(pendulum.theta + 0.5 * k1_theta) );
+
+    pendulum.thetadot = pendulum.thetadot + k2_thetadot;
+    pendulum.theta = pendulum.theta + k2_theta;
+
+    //Update cartesian coordinates based on polar coordinates values
+    pendulum.x = pendulum.x0 + ( pendulum.l * sin(pendulum.theta));
+    pendulum.y = pendulum.y0 - ( pendulum.l * cos(pendulum.theta));
+
 
 }
 
-void SimulatePendulumEuler(DoublePendulum& pendulum){
+void SimulatePendulumRK4(SinglePendulum& pendulum){
+
+    //Calcualte k1,k2 parameters for theta and thetadot
+    double k1_theta = dt * pendulum.thetadot;
+    double k1_thetadot = dt * (-(g / pendulum.l) * sin(pendulum.theta));
+
+    double k2_theta = dt * ( pendulum.thetadot + (0.5 * k1_thetadot) );
+    double k2_thetadot = dt * ( -(g / pendulum.l) * sin(pendulum.theta + 0.5 * k1_theta) );
+
+    double k3_theta = dt * ( pendulum.thetadot + (0.5 * k2_thetadot) );
+    double k3_thetadot = dt * ( -(g / pendulum.l) * sin(pendulum.theta + 0.5 * k2_theta) );
+
+    double k4_theta = dt * ( pendulum.thetadot + k3_thetadot);
+    double k4_thetadot = dt * ( -(g / pendulum.l) * sin(pendulum.theta + k3_theta) );
+
+    pendulum.thetadot = pendulum.thetadot + ((k1_thetadot + 2 * k2_thetadot + 2 * k3_thetadot + k4_thetadot) / 6 );
+    pendulum.theta = pendulum.theta + ((k1_theta + 2 * k2_theta + 2 * k3_theta + k4_theta) / 6 );
+
+    //Update cartesian coordinates based on polar coordinates values
+    pendulum.x = pendulum.x0 + ( pendulum.l * sin(pendulum.theta));
+    pendulum.y = pendulum.y0 - ( pendulum.l * cos(pendulum.theta));
+
+
+}
+
+/*
+    Helpfull functions for double pendulum calculation
+*/
+
+double calculateThetaddot2(double l1, double l2, double m1, double m2, double theta1, double theta2, double thetadot1, double thetadot2){
+
+    //helpful expressions 
+    double a = ( -1 * (m1 + m2) * g * sin(theta1) ) -( m2 * l2 * thetadot2 * thetadot2 * sin(theta1 - theta2) );
+    double c = m2 * l2 * cos(theta1 - theta2);
+    double d = -1 * g * sin(theta2) + pow(thetadot1,2) * l1 * sin(theta1 - theta2);
+    double e = l1 * cos(theta1 - theta2);
+    double f = l2;
 
-    //Calculation helpful expressions 
-    double a = ( -1 * (pendulum.m1 + pendulum.m2) * g * sin(pendulum.theta1) ) -( pendulum.m2 * pendulum.l2 * pendulum.thetadot2 * pendulum.thetadot2 * sin(pendulum.theta1 - pendulum.theta2) );
-    double b = (pendulum.m1 + pendulum.m2) * pendulum.l1;
-    double c = pendulum.m2 * pendulum.l2 * cos(pendulum.theta1 - pendulum.theta2);
-    double d = -1 * g * sin(pendulum.theta2) + pow(pendulum.thetadot1,2) * pendulum.l1 * sin(pendulum.theta1 - pendulum.theta2);
-    double e = pendulum.l1 * cos(pendulum.theta1 - pendulum.theta2);
-    double f = pendulum.l2;
 
-    //Calculate properties in polar coordinates for both masses
     double thetaddot2 = (d - (e * a)) / (f - (e*c));
+    return thetaddot2;
+
+}
+
+double calculateThetaddot1(double l1, double l2, double m1, double m2, double theta1, double theta2, double thetadot2, double thetaddot2){
+
+    //helpful expressions 
+    double a = ( -1 * (m1 + m2) * g * sin(theta1) ) -( m2 * l2 * thetadot2 * thetadot2 * sin(theta1 - theta2) );
+    double b = (m1 + m2) * l1;
+    double c = m2 * l2 * cos(theta1 - theta2);
+
+
     double thetaddot1 = (a - (c * thetaddot2)) / b;
+    return thetaddot1;
+
+}
+
+void SimulatePendulumEuler(DoublePendulum& pendulum){
+
+    //Calculate properties in polar coordinates for both masses
+    double thetaddot2 = calculateThetaddot2(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2, pendulum.theta1, pendulum.theta2,
+                            pendulum.thetadot1, pendulum.thetadot2);
+
+    double thetaddot1 = calculateThetaddot1(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2, pendulum.theta1, pendulum.theta2,
+                                            pendulum.thetadot2, thetaddot2);                        
 
     pendulum.thetadot2 = pendulum.thetadot2 + dt * thetaddot2;
     pendulum.theta2 = pendulum.theta2 + dt * pendulum.thetadot2;
@@ -96,4 +188,129 @@ void SimulatePendulumEuler(DoublePendulum& pendulum){
 
     pendulum.x2 = pendulum.x0 + ( pendulum.l1 * sin(pendulum.theta1)) + ( pendulum.l2 * sin(pendulum.theta2));
     pendulum.y2 = pendulum.y0 - ( pendulum.l1 * cos(pendulum.theta1)) - ( pendulum.l2 * cos(pendulum.theta2));
+}
+
+/*
+    Posible to not create another double variables
+*/
+void SimulatePendulumHeun(DoublePendulum& pendulum){
+
+    //Calculate thetaddots for k1
+    double thetaddot2 = calculateThetaddot2(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2, pendulum.theta1, pendulum.theta2,
+                            pendulum.thetadot1, pendulum.thetadot2);
+
+    double thetaddot1 = calculateThetaddot1(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2, pendulum.theta1, pendulum.theta2,
+                                            pendulum.thetadot2, thetaddot2);   
+
+
+    //Calculate k1s
+    double k1_theta2 = dt * pendulum.thetadot2;
+    double k1_thetadot2 = dt * thetaddot2;
+    double k1_theta1 = dt * pendulum.thetadot1;
+    double k1_thetadot1 = dt * thetaddot1;
+
+    //Calculate thetaddots for k2
+    double thetaddot2_k2 = calculateThetaddot2(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2,
+                            pendulum.theta1 + 0.5 * k1_theta1, pendulum.theta2 + 0.5 * k1_theta2,
+                            pendulum.thetadot1 + 0.5 * k1_thetadot1, pendulum.thetadot2  + 0.5 * k1_thetadot2);
+
+    double thetaddot1_k2 = calculateThetaddot1(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2,
+                                               pendulum.theta1 + 0.5 * k1_theta1, pendulum.theta2 + 0.5 * k1_theta2,
+                                               pendulum.thetadot2  + 0.5 * k1_thetadot2, thetaddot2_k2);  
+
+    //Calculate k2s
+    double k2_theta2 = dt * (pendulum.thetadot2 + 0.5 * k1_thetadot2);
+    double k2_thetadot2 = dt * thetaddot2_k2;
+    double k2_theta1 = dt * (pendulum.thetadot1 + 0.5 * k1_thetadot1);
+    double k2_thetadot1 = dt * thetaddot1_k2;
+
+
+    //Update properties based on RK2                                        
+    pendulum.thetadot2 = pendulum.thetadot2 + k2_thetadot2;
+    pendulum.theta2 = pendulum.theta2 + k2_theta2;
+
+    pendulum.thetadot1 = pendulum.thetadot1 + k2_thetadot1;
+    pendulum.theta1 = pendulum.theta1 + k2_theta1;
+
+    //Update cartesian coordinates based on polar coordinates values for both masses
+    pendulum.x1 = pendulum.x0 + ( pendulum.l1 * sin(pendulum.theta1)); 
+    pendulum.y1 = pendulum.y0 - ( pendulum.l1 * cos(pendulum.theta1));
+
+    pendulum.x2 = pendulum.x0 + ( pendulum.l1 * sin(pendulum.theta1)) + ( pendulum.l2 * sin(pendulum.theta2));
+    pendulum.y2 = pendulum.y0 - ( pendulum.l1 * cos(pendulum.theta1)) - ( pendulum.l2 * cos(pendulum.theta2));
+}
+
+void SimulatePendulumRK4(DoublePendulum& pendulum){
+
+    //Calculate thetaddots for k1
+    double thetaddot2 = calculateThetaddot2(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2, pendulum.theta1, pendulum.theta2,
+                            pendulum.thetadot1, pendulum.thetadot2);
+
+    double thetaddot1 = calculateThetaddot1(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2, pendulum.theta1, pendulum.theta2,
+                                            pendulum.thetadot2, thetaddot2);   
+
+
+    //Calculate k1s
+    double k1_theta2 = dt * pendulum.thetadot2;
+    double k1_thetadot2 = dt * thetaddot2;
+    double k1_theta1 = dt * pendulum.thetadot1;
+    double k1_thetadot1 = dt * thetaddot1;
+
+    //Calculate thetaddots for k2
+    thetaddot2 = calculateThetaddot2(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2,
+                            pendulum.theta1 + 0.5 * k1_theta1, pendulum.theta2 + 0.5 * k1_theta2,
+                            pendulum.thetadot1 + 0.5 * k1_thetadot1, pendulum.thetadot2  + 0.5 * k1_thetadot2);
+
+    thetaddot1 = calculateThetaddot1(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2,
+                                               pendulum.theta1 + 0.5 * k1_theta1, pendulum.theta2 + 0.5 * k1_theta2,
+                                               pendulum.thetadot2  + 0.5 * k1_thetadot2, thetaddot2);  
+
+    //Calculate k2s
+    double k2_theta2 = dt * (pendulum.thetadot2 + 0.5 * k1_thetadot2);
+    double k2_thetadot2 = dt * thetaddot2;
+    double k2_theta1 = dt * (pendulum.thetadot1 + 0.5 * k1_thetadot1);
+    double k2_thetadot1 = dt * thetaddot1;
+
+    //Calculate thetaddots for k3
+    thetaddot2 = calculateThetaddot2(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2,
+                            pendulum.theta1 + 0.5 * k2_theta1, pendulum.theta2 + 0.5 * k2_theta2,
+                            pendulum.thetadot1 + 0.5 * k2_thetadot1, pendulum.thetadot2  + 0.5 * k2_thetadot2);
+
+    thetaddot1 = calculateThetaddot1(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2,
+                                               pendulum.theta1 + 0.5 * k2_theta1, pendulum.theta2 + 0.5 * k2_theta2,
+                                               pendulum.thetadot2  + 0.5 * k2_thetadot2, thetaddot2); 
+
+    double k3_theta2 = dt * (pendulum.thetadot2 + 0.5 * k2_thetadot2);
+    double k3_thetadot2 = dt * thetaddot2;
+    double k3_theta1 = dt * (pendulum.thetadot1 + 0.5 * k2_thetadot1);
+    double k3_thetadot1 = dt * thetaddot1;
+
+    //Calculate thetaddots for k4
+    thetaddot2 = calculateThetaddot2(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2,
+                            pendulum.theta1 + k3_theta1, pendulum.theta2 + k3_theta2,
+                            pendulum.thetadot1 + k3_thetadot1, pendulum.thetadot2  + k3_thetadot2);
+
+    thetaddot1 = calculateThetaddot1(pendulum.l1, pendulum.l2, pendulum.m1, pendulum.m2,
+                                               pendulum.theta1 + k3_theta1, pendulum.theta2 + k3_theta2,
+                                               pendulum.thetadot2  + k3_thetadot2, thetaddot2); 
+
+    double k4_theta2 = dt * (pendulum.thetadot2 + k3_thetadot2);
+    double k4_thetadot2 = dt * thetaddot2;
+    double k4_theta1 = dt * (pendulum.thetadot1 + k3_thetadot1);
+    double k4_thetadot1 = dt * thetaddot1;
+
+    //Update properties based on RK4                                     
+    pendulum.thetadot2 = pendulum.thetadot2 + ((k1_thetadot2 + 2 * k2_thetadot2 + 2 * k3_thetadot2 + k4_thetadot2) / 6);
+    pendulum.theta2 = pendulum.theta2 + ((k1_theta2 + 2 * k2_theta2 + 2 * k3_theta2 + k4_theta2) / 6);
+
+    pendulum.thetadot1 = pendulum.thetadot1 + ((k1_thetadot1 + 2 * k2_thetadot1 + 2 * k3_thetadot1 + k4_thetadot1) / 6);
+    pendulum.theta1 = pendulum.theta1 + ((k1_theta1 + 2 * k2_theta1 + 2 * k3_theta1 + k4_theta1) / 6);
+
+    //Update cartesian coordinates based on polar coordinates values for both masses
+    pendulum.x1 = pendulum.x0 + ( pendulum.l1 * sin(pendulum.theta1)); 
+    pendulum.y1 = pendulum.y0 - ( pendulum.l1 * cos(pendulum.theta1));
+
+    pendulum.x2 = pendulum.x0 + ( pendulum.l1 * sin(pendulum.theta1)) + ( pendulum.l2 * sin(pendulum.theta2));
+    pendulum.y2 = pendulum.y0 - ( pendulum.l1 * cos(pendulum.theta1)) - ( pendulum.l2 * cos(pendulum.theta2));
+
 }