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