Untitled

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO; // Add this line for file I/O

public class Pendulum : MonoBehaviour
{
    // parameters of ths simulation (are specified through the Unity's interface)
    public float gravity_acceleration = 9.81f;
    public float mass = 1.0f;
    public float friction_coeficient = 0.0f;
    public float initial_angular_velocity = 0.0f;
    public float time_step_h = 0.05f;
    public string ode_method = "euler";

    // parameters that will be populated automatically from the geometry/components of the scene
    private float rod_length = 0.0f;
    private float c = 0.0f;
    private float omega_sqr = 0.0f;
    private GameObject pendulum = null;
    private string outputDirectory;
    private string fileName;

    // the state vector stores two entries:
    // state_vector[0] is the angle of pendulum (\theta) in radians
    // state_vector[1] is the angular velocity of pendulum
    private Vector2 state_vector;

    // Use this for initialization
    void Start()
    {
        outputDirectory = Application.dataPath + "/PendulumData/";
        // Create the output directory if it doesn't exist
        if (!Directory.Exists(outputDirectory))
        {
            Directory.CreateDirectory(outputDirectory);
        }
        Time.fixedDeltaTime = time_step_h;      // set the simulation step - FixedUpdate is called every 'time_step_h' seconds 
        state_vector = new Vector2(0.0f, 0.0f); // initialization of the state vector
        pendulum = GameObject.Find("Pendulum");
        if (pendulum == null)
        {
            Debug.LogError("Sphere not found! Did you delete it from the starter scene?");
        }
        GameObject rod = GameObject.Find("Quad");
        if (rod == null)
        {
            Debug.LogError("Rod not found! Did you delete it from the starter scene?");
        }
        rod_length = rod.transform.localScale.y; // finds rod length (based on quad's scale)

        state_vector[0] = pendulum.transform.eulerAngles.z * Mathf.Deg2Rad; // initial angle is set from the starter scene
        state_vector[1] = initial_angular_velocity;

        c = friction_coeficient / mass;        // following the ODE specification
        omega_sqr = gravity_acceleration / rod_length;
    }

    // Update is called once per Time.fixedDeltaTime  sec
    void FixedUpdate()
    {
        // complete this function (measure kinetic, potential, total energy)
        float kinetic_energy = 0.5f * mass * rod_length * rod_length * state_vector.y * state_vector.y;
        float potential_energy = mass * gravity_acceleration * rod_length * (1 - Mathf.Cos(state_vector.x));
        float total_energy = kinetic_energy + potential_energy;
        string fileName = $"{outputDirectory}pendulum_data_{ode_method}_{initial_angular_velocity}_{friction_coeficient}_{time_step_h}.txt";
        string data = Time.time + "\t" + kinetic_energy + "\t" + potential_energy + "\t" + total_energy + "\n";
        Debug.Log(data);
        File.AppendAllText(fileName, data);
        OdeStep();
        pendulum.transform.eulerAngles = new Vector3(0.0f, 0.0f, state_vector[0] * Mathf.Rad2Deg);
    }

    // complete this function!
    // it should return the right side of the two ODEs (in other words, the derivative of the pendulum's angle and angular velocity)
    // and you should use it in OdeStep!
    Vector2 PendulumDynamics(Vector2 current_state_vector)
    {
        // Unpack the current state
        float angle = current_state_vector.x;
        float angular_velocity = current_state_vector.y;

        // Calculate the derivatives based on equations 2 & 3
        float g_v = -gravity_acceleration * Mathf.Sin(angle);
        float h_y = -c * angular_velocity - omega_sqr * Mathf.Sin(angle);

        // Return the derivatives
        return new Vector2(angular_velocity, g_v + h_y);
    }

    void OdeStep()
    {
        // update the state_vector (both entries) properly depending on the specified ode_method 
        if (ode_method == "euler")
        {
            // implement the forward's Euler method
            Vector2 k1 = PendulumDynamics(state_vector);
            state_vector += time_step_h * k1;
        }
        else if (ode_method == "trapezoidal")
        {
            // implement the trapezoidal method (also known as Heun's method or improved Euler method)
            Vector2 k1 = PendulumDynamics(state_vector);
            Vector2 k2 = PendulumDynamics(state_vector + k1 * time_step_h);
            state_vector += (time_step_h / 2) * (k1 + k2);
        }
        else if (ode_method == "rk")
        {
            // implement the Runge-Kutta variant (RK3)
            Vector2 k1 = time_step_h * PendulumDynamics(state_vector);
            Vector2 k2 = time_step_h * PendulumDynamics(state_vector + k1);
            Vector2 k3 = time_step_h * PendulumDynamics(state_vector + k1 / 4 + k2 / 4);
            state_vector += (k1 + k2 + 4 * k3) / 6;
        }
        else if (ode_method == "semi-implicit")
        {
            // implement the symplectic method (also known as semi-implicit Euler method)
            state_vector[1] += time_step_h * PendulumDynamics(state_vector)[1];
            // Update angle using the updated angular velocity
            state_vector[0] += time_step_h * state_vector[1];
        }
        else
        {
            Debug.LogError("ODE method should be one of the: euler, trapezoidal, rk, semi-implicit");
        }
    }
}