Commit 301d1891 authored by Alexander Kreibich's avatar Alexander Kreibich

Changes to simulate Steady states

parent a07790b0
......@@ -62,11 +62,11 @@ public class CollisionTestPerformer : MonoBehaviour
simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
}
simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForce;
simulationLoop.sphereExternalForces[0] = pullForce;
yield return new WaitForSeconds(applyForceTime);
simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = Vector3.zero;
simulationLoop.sphereExternalForces[0] = Vector3.zero;
float timeDiff = Time.time - startTime;
Debug.Log("Elapsed time of collision test: " + timeDiff);
......
......@@ -12,6 +12,9 @@ namespace GuidewireSim
*/
public class ConstraintSolvingStep : MonoBehaviour
{
private SimulationLoop simulationLoop; // Declare simulationLoop
private float rodElementLength; // Declare rodElementLength
MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
Vector3 deltaPositionOne = new Vector3(); //!< The correction of @p particlePositionOne in method SolveStretchConstraint().
......@@ -19,15 +22,15 @@ public class ConstraintSolvingStep : MonoBehaviour
BSM.Quaternion deltaOrientation = new BSM.Quaternion(); //!< The correction of @p orientation in method SolveStretchConstraint().
BSM.Quaternion deltaOrientationOne = new BSM.Quaternion(); //!< The correction of @p orientationOne in method SolveBendTwistConstraint().
BSM.Quaternion deltaOrientationTwo = new BSM.Quaternion(); //!< The correction of @p orientationTwo in method SolveBendTwistConstraint().
private List<Vector3> allDeltaPositionOnes = new List<Vector3>();
private List<Vector3> allDeltaPositionTwos = new List<Vector3>();
private List<BSM.Quaternion> allDeltaOrientations = new List<BSM.Quaternion>();
private List<BSM.Quaternion> allDeltaOrientationOnes = new List<BSM.Quaternion>();
private List<BSM.Quaternion> allDeltaOrientationTwos = new List<BSM.Quaternion>();
// private List<Vector3> allDeltaPositionOnes = new List<Vector3>();
//private List<Vector3> allDeltaPositionTwos = new List<Vector3>();
//private List<BSM.Quaternion> allDeltaOrientations = new List<BSM.Quaternion>();
//private List<BSM.Quaternion> allDeltaOrientationOnes = new List<BSM.Quaternion>();
// private List<BSM.Quaternion> allDeltaOrientationTwos = new List<BSM.Quaternion>();
float stretchStiffness = 0.1f;
float bendStiffness = 0.1f;
float stretchStiffness = 0.9f;
float bendStiffness = 0.9f;
[Tooltip("Whether to solve both constraints in bilateral interleaving order. Naive order is used when false.")]
[SerializeField] bool executeInBilateralOrder = false; //!< Whether to solve both constraints in bilateral interleaving order. Naive order is used when false.
......@@ -36,6 +39,8 @@ public class ConstraintSolvingStep : MonoBehaviour
{
mathHelper = GetComponent<MathHelper>();
Assert.IsNotNull(mathHelper);
simulationLoop = GetComponent<SimulationLoop>(); // Initialize simulationLoop
rodElementLength = simulationLoop.GetRodElementLength(); // Initialize rodElementLength
}
/**
......@@ -288,18 +293,20 @@ public class ConstraintSolvingStep : MonoBehaviour
*/
public void SolveStretchConstraint(Vector3 particlePositionOne, Vector3 particlePositionTwo, BSM.Quaternion orientation,
BSM.Quaternion e_3, float rodElementLength, out Vector3 deltaPositionOne,
out Vector3 deltaPositionTwo, out BSM.Quaternion deltaOrientation, float inverseMassOne = 1000f,
float inverseMassTwo = 1000f, float inertiaWeight = 1f)
out Vector3 deltaPositionTwo, out BSM.Quaternion deltaOrientation, float inverseMassOne = 1f,
float inverseMassTwo = 1f, float inertiaWeight = 1f)
{
Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(orientation), tolerance: 0.01f);
Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(e_3), tolerance: 0.01f);
float inverseMassValue = ((1000/rodElementLength)+1)/10f;
Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(orientation), tolerance: 0.001f);
Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(e_3), tolerance: 0.001f);
Assert.IsTrue(rodElementLength > 0f);
Assert.IsTrue(inverseMassOne >= 0f && inverseMassOne <= 1f);
Assert.IsTrue(inverseMassTwo >= 0f && inverseMassTwo <= 1f);
Assert.IsTrue(inertiaWeight >= 0f && inertiaWeight <= 1f);
Vector3 thirdDirector = mathHelper.ImaginaryPart(orientation * e_3 * BSM.Quaternion.Conjugate(orientation));
float denominator = inverseMassOne + inverseMassTwo + 4 * inertiaWeight * rodElementLength * rodElementLength;
float denominator = inverseMassValue + inverseMassValue + 4 * inertiaWeight * rodElementLength * rodElementLength;
Vector3 factor = (1f / rodElementLength) * (particlePositionTwo - particlePositionOne) - thirdDirector;
BSM.Quaternion embeddedFactor = mathHelper.EmbeddedVector(factor);
......@@ -307,9 +314,12 @@ public class ConstraintSolvingStep : MonoBehaviour
float quaternionScalarFactor = 2f * inertiaWeight * rodElementLength * rodElementLength / denominator;
BSM.Quaternion quaternionProduct = embeddedFactor * orientation * BSM.Quaternion.Conjugate(e_3);
deltaPositionOne = inverseMassOne * rodElementLength * factor / denominator;
deltaPositionTwo = - inverseMassTwo * rodElementLength * factor / denominator;
deltaPositionOne = inverseMassValue * rodElementLength * factor / denominator;
deltaPositionTwo = - inverseMassValue * rodElementLength * factor / denominator;
deltaOrientation = quaternionScalarFactor * quaternionProduct;
//string debugFilePath = "/home/akreibich/TestRobinCode37/DebugConstraint.txt";
//string content = $"inverseMassValue: {inverseMassValue}, denominator: {denominator}\n";
//File.AppendAllText(debugFilePath, content);
}
/**
......@@ -347,6 +357,9 @@ public class ConstraintSolvingStep : MonoBehaviour
deltaOrientationOne = inertiaWeightOne * orientationTwo * embeddedDarbouxDifference / denominator;
deltaOrientationTwo = - inertiaWeightTwo * orientationOne * embeddedDarbouxDifference / denominator;
}
/**
......@@ -363,14 +376,6 @@ private void CorrectStretchPredictions(int sphereIndex, Vector3[] spherePosition
Assert.IsTrue(sphereIndex >= 0);
Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[sphereIndex]), tolerance: 0.01f);
string path = "/home/akreibich/TestRobinCode37/LogConstraints.txt";
using (StreamWriter writer = new StreamWriter(path, true)) // true to append data to the file
{
// Log the deltaPositionOne and deltaPositionTwo to the file
writer.WriteLine($"Delta Position One: {deltaPositionOne}");
writer.WriteLine($"Delta Position Two: {deltaPositionTwo}");
}
spherePositionPredictions[sphereIndex] += stretchStiffness * deltaPositionOne;
spherePositionPredictions[sphereIndex + 1] += stretchStiffness * deltaPositionTwo;
......@@ -379,6 +384,14 @@ private void CorrectStretchPredictions(int sphereIndex, Vector3[] spherePosition
cylinderOrientationPredictions[sphereIndex].Normalize();
Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[sphereIndex]), tolerance: 0.01f);
string path = "/home/akreibich/TestRobinCode37/LogConstraints.txt";
using (StreamWriter writer = new StreamWriter(path, true)) // true to append data to the file
{
// Log the deltaPositionOne and deltaPositionTwo to the file
//writer.WriteLine($"Delta Position One: {1000 * deltaPositionOne}");
//writer.WriteLine($"Delta Position Two: {1000 * deltaPositionTwo}");
}
}
/**
......@@ -403,13 +416,13 @@ private void CorrectStretchPredictions(int sphereIndex, Vector3[] spherePosition
Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[cylinderIndex]), tolerance: 0.01f);
Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[cylinderIndex + 1]), tolerance: 0.01f);
allDeltaOrientationOnes.Add(deltaOrientationOne);
allDeltaOrientationTwos.Add(deltaOrientationTwo);
//allDeltaOrientationOnes.Add(deltaOrientationOne);
//allDeltaOrientationTwos.Add(deltaOrientationTwo);
}
public List<Vector3> GetAllDeltaPositionOnes() => allDeltaPositionOnes;
public List<Vector3> GetAllDeltaPositionTwos() => allDeltaPositionTwos;
public List<BSM.Quaternion> GetAllDeltaOrientations() => allDeltaOrientations;
public List<BSM.Quaternion> GetAllDeltaOrientationOnes() => allDeltaOrientationOnes;
public List<BSM.Quaternion> GetAllDeltaOrientationTwos() => allDeltaOrientationTwos;
// public List<Vector3> GetAllDeltaPositionOnes() => allDeltaPositionOnes;
//public List<Vector3> GetAllDeltaPositionTwos() => allDeltaPositionTwos;
//public List<BSM.Quaternion> GetAllDeltaOrientations() => allDeltaOrientations;
//public List<BSM.Quaternion> GetAllDeltaOrientationOnes() => allDeltaOrientationOnes;
//public List<BSM.Quaternion> GetAllDeltaOrientationTwos() => allDeltaOrientationTwos;
}
}
using UnityEngine;
using System.IO;
using GuidewireSim;
using System;
public class CreationScript : MonoBehaviour
{
private string logFilePath = "";
public GameObject spherePrefab; //This is for choosing a Prefab for the sphere element. One can directly gain this in Robins code from: Assets/Guidewire_Assets/Prefabs
public GameObject cylinderPrefab; //This is for choosing a Prefab for the cylinder element. One can directly gain this in Robins code from: Assets/Guidewire_Assets/Prefabs
private int spheresCount; //This variable is used to store the number of spheres in the guidewire, so the script can keep knowledge on how many spheres were created
private GameObject[] spheres; //This array is used to store references to the sphere game objects in the guidewire. //This array is used to store references to the sphere game objects in the guidewire.
private GameObject[] cylinders; //-''-
//Declare several private variables (in order to later use the other script/ a path....)
private PredictionStep predictionStep;
private string logFilePath = "";
private Vector3 lastSpherePreviousPosition = Vector3.zero;
public GameObject spherePrefab;
public GameObject cylinderPrefab;
private int spheresCount;
private GameObject[] spheres;
private GameObject[] cylinders;
private SimulationLoop simulationLoop;
private float initialCheckDelay = 3f;
private int firstCallResetCounter = 0;
private const int MaxFirstCallResets = 1;
private Vector3[] lastSpherePositions;
private Vector3[] lastSphereVelocities;
void Start()
{
//get several components/ scripts from a GameObject called Simulation. This GameObject holds all the relevant scripts, among others the SimulationLoop, that is responsible for organizing and calling all parts of the simulation.
predictionStep = GameObject.Find("Simulation").GetComponent<PredictionStep>();
simulationLoop = GameObject.Find("Simulation").GetComponent<SimulationLoop>();
InvokeRepeating("SavePositionsToFile", 0f, 1f);
InvokeRepeating("SavePositionsToFile", 0f, 0.01f);
}
private void Awake()
{
//Find file path in order to be able to log information later when running it as a Unity standalone application and not being able to rely on the console in Unity anymore.
string[] args = System.Environment.GetCommandLineArgs();
for (int i = 0; i < args.Length; i++)
{
......@@ -32,22 +47,29 @@ public class CreationScript : MonoBehaviour
void FixedUpdate()
{
//SavePositionsToFile();
if (Time.time > initialCheckDelay)
{
//After an inital delay (in order to give the system enough time to actually realize the displacement and not kill the simulation before even applying the first displacement, the velocity is checked. (if the magntidude is below a treshhold, then another displacement is applied, or if all wanted displacements have been applied, the application is quit)
CheckVelocityDifference();
}
}
//This code segment is responsible for creating and positioning the spheres and cylinders
//this is the guidewire creation method, it calls the spheres and cylinders elements and creates them with an aquidistant spacing in a row.
//The starting position is chosen by the Vector in line 70, also the
public void CreateGuidewire(int numberElements)
{
spheres = new GameObject[numberElements];
cylinders = new GameObject[numberElements - 1];
//Here we use GetComponent to recieve the value of the variable rodElementLength that is stored in the SimulationLoop script. As this variable needs to be private, we use a getter Method and therefore also call this instead of calling the variable directly.
float rEL = simulationLoop.GetRodElementLength();
lastSpherePositions = new Vector3[numberElements];
for (int i = 0; i < numberElements; ++i)
{
GameObject sphere = Instantiate(spherePrefab);
sphere.transform.position = new Vector3(0, 0, -2000 + i * rEL);
sphere.transform.position = new Vector3(0, 0, 44.5f + i * rEL); //-444 fuer 1000 lang, -123 fur 2?, 44.5f fur 2!
sphere.transform.parent = this.transform;
spheres[i] = sphere;
if (i < numberElements - 1)
......@@ -55,16 +77,17 @@ public class CreationScript : MonoBehaviour
GameObject cylinder = Instantiate(cylinderPrefab);
cylinder.layer = 6;
cylinder.transform.parent = this.transform;
cylinders[i] = cylinder;
}
}
spheresCount = numberElements;
simulationLoop.SetSpheres(spheres);
simulationLoop.SetCylinders(cylinders);
}
//Here I save the positions of the spheres to a .txt file. The file path has to be adapted to the local network for the file one wants to save the positons to
public void SavePositionsToFile()
{
......@@ -83,9 +106,63 @@ public class CreationScript : MonoBehaviour
}
}
}
public void UpdateSphereVelocities(Vector3[] velocities)
{
lastSphereVelocities = velocities;
}
private void CheckVelocityDifference()
{
string debugVelocityFilePath = "/home/akreibich/TestRobinCode37/DebugVelocities.txt";
string positionFilePath = "/home/akreibich/TestRobinCode37/Position#N.txt";
if (spheres != null && spheres.Length > 1 && lastSphereVelocities != null)
{
bool allSpheresBelowVelocityThreshold = true;
float epsilon = 0.000001f;
float velocityDifferenceThreshold = 1f; // Define your velocity threshold here
using (StreamWriter velocityWriter = new StreamWriter(debugVelocityFilePath, true),
positionWriter = new StreamWriter(positionFilePath, true))
{
for (int i = 1; i < spheres.Length; i++)
{
float velocityMagnitude = lastSphereVelocities[i].magnitude;
//velocityWriter.WriteLine("Sphere " + i + " Velocity Magnitude: " + velocityMagnitude);
if (velocityMagnitude >= velocityDifferenceThreshold + epsilon)
{
allSpheresBelowVelocityThreshold = false;
break;
}
}
if (allSpheresBelowVelocityThreshold)
{
if (firstCallResetCounter < MaxFirstCallResets)
{
predictionStep.ResetFirstCall();
firstCallResetCounter++;
DateTime now = DateTime.Now;
string timestamp = now.ToString("yyyy-MM-dd HH:mm:ss.fff");
velocityWriter.WriteLine($"Threshold reached at {timestamp}, triggering displacement.");
positionWriter.WriteLine($"Threshold reached at {timestamp}, triggering displacement.");
}
if (firstCallResetCounter >= MaxFirstCallResets)
{
Application.Quit(1);
}
}
}
}
}
//This code segment is responsible for passing the spheres and cylinders
public GameObject GetLastSphere()
{
if (spheres != null && spheres.Length > 0)
......@@ -104,4 +181,4 @@ public class CreationScript : MonoBehaviour
{
return cylinders;
}
}
}
......@@ -72,7 +72,7 @@ namespace GuidewireSim
public void InitSphereInverseMasses(int spheresCount, out float[] sphereInverseMasses)
{
sphereInverseMasses = new float[spheresCount];
float inverseMassValue = ((2000/rodElementLength)+1)/50;
float inverseMassValue = ((1000/rodElementLength)+1)/10f;
for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
{
......
//import to use functionalities
using UnityEngine;
using System.IO;
using Dummiesman;
......@@ -7,17 +8,19 @@ public class MeshImporter3 : MonoBehaviour
{
void Awake()
{
//Path for the log file, in order to see if everything worked as expected. If e.g. no mesh can be seen in a GUI test, then check the log, if the path to the mesh was found and maybe the positioning is off (not in the view of the camera)...
string logFilePath = "/home/akreibich/TestRobinCode37/DebugLogs.txt";
File.AppendAllText(logFilePath, "Awake started.\n");
try
{
//Here we initialize the mesh path variables, for this we need a way to parse the name from the command line environment that gets simulated by the python script.
string[] args = Environment.GetCommandLineArgs();
File.AppendAllText(logFilePath, "Received arguments: " + string.Join(", ", args) + "\n");
string meshPath ="";
string secondMeshPath = "";
//if run via the python loop, it should be like this
//If one runs via the python loop, it should be like this: (so here we initialize transformation vectors to zero), if the objective is to test it in the scene itself, set the position manually below by commenting it out and placing it correctly).
Vector3 position = Vector3.zero;
Vector3 scale = Vector3.one;
Vector3 rotation = Vector3.zero;
......@@ -27,9 +30,11 @@ public class MeshImporter3 : MonoBehaviour
//Vector3 scale = new Vector3(27.2f, 27.2f, 27.2f);
//Vector3 rotation = new Vector3(0f, -86.6f, 0f);
//Iterate through command line arguments to set the above initialized mesh paths and transformations for the positioning of the mesh.
//Adding a second .obj file is possible, as a second mesh if the goal is to not allow collisions from the outside and from the inside. Then the second mesh is the same mesh as the first but with its normals inverted.
for (int i = 0; i < args.Length; i++)
{
if (args[i] == "-objPath")
if (args[i] == "-objPath")//This checks if the argument is for object path, so for the position, scale, paths... and everytime it finds the name, it takes the next values as the argument.
{
meshPath = args[i + 1];
}
......@@ -39,16 +44,19 @@ public class MeshImporter3 : MonoBehaviour
}
else if (args[i] == "-position")
{
//Split the argument into components and create a vector
string[] posArgs = args[i + 1].Split(',');
position = new Vector3(float.Parse(posArgs[0]), float.Parse(posArgs[1]), float.Parse(posArgs[2]));
}
else if (args[i] == "-scale")
{
//Split the argument into components and create a vector
string[] scaleArgs = args[i + 1].Split(',');
scale = new Vector3(float.Parse(scaleArgs[0]), float.Parse(scaleArgs[1]), float.Parse(scaleArgs[2]));
}
else if (args[i] == "-rotation")
{
//Again we split the argument into components and create a vector
string[] rotArgs = args[i + 1].Split(',');
rotation = new Vector3(float.Parse(rotArgs[0]), float.Parse(rotArgs[1]), float.Parse(rotArgs[2]));
}
......@@ -108,11 +116,14 @@ public class MeshImporter3 : MonoBehaviour
return;
}
//This creates a new GameObject and sets its transformation properties: (these are defined either above or parsed from the outside loop)
GameObject obj = new GameObject(primaryMesh.name);
obj.transform.position = position;
obj.transform.localScale = scale;
obj.transform.eulerAngles = rotation;
//Add the MeshFilter component and assign the primary mesh, do the same below for the MeshRenderer component below. Then the material is loaded, in order for the artery to have the typical optic and seem realistic
MeshFilter filter = obj.AddComponent<MeshFilter>();
filter.mesh = primaryMesh;
......@@ -128,6 +139,7 @@ public class MeshImporter3 : MonoBehaviour
Debug.LogError("Failed to load material");
}
//Now add a MeshCollider and the primary mesh, in order to enable collision detection between guidewire and artery that can be resolved in the constraint solving.
MeshCollider primaryCollider = obj.AddComponent<MeshCollider>();
primaryCollider.sharedMesh = primaryMesh;
......@@ -137,10 +149,12 @@ public class MeshImporter3 : MonoBehaviour
secondaryCollider.sharedMesh = secondaryMesh;
}
//Rigidbody component needs to be added as well in order to enable the collision detection, use isKinematic on, for all colliding objects, then the collision data will be output and not corrected by the Unity engine itself
Rigidbody rb = obj.AddComponent<Rigidbody>();
rb.useGravity = false;
rb.isKinematic = true;
//adding the correct layer for the layer matrix to have the correct effect
int layerIndex = LayerMask.NameToLayer("Blood Vessel");
if (layerIndex == -1)
......
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Assertions;
using BSM = BulletSharp.Math;
using System.IO;
namespace GuidewireSim
{
/**
* This class implements the prediction step of the algorithm.
*/
public class PredictionStep : MonoBehaviour
{
MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
private float zDisplacement = 0.0f;
private void Awake()
{
mathHelper = GetComponent<MathHelper>();
Assert.IsNotNull(mathHelper);
string[] args = System.Environment.GetCommandLineArgs();
for (int i = 0; i < args.Length; i++)
{
if (args[i] == "-zDisplacement")
{
zDisplacement = float.Parse(args[i + 1]);
}
}
}
/**
......@@ -27,6 +39,11 @@ public class PredictionStep : MonoBehaviour
* @return The predictions of the positions of the spheres, i.e. @p spherePositionPredictions.
* @note The predictions are again stored in @p sphereVelocities.
*/
public void ResetFirstCall()
{
firstCall = true;
}
public Vector3[] PredictSphereVelocities(Vector3[] sphereVelocities, float[] sphereInverseMasses, Vector3[] sphereExternalForces)
{
Vector3 calc = Time.deltaTime * sphereInverseMasses[1] * sphereExternalForces[1];
......@@ -47,17 +64,57 @@ public class PredictionStep : MonoBehaviour
* @param sphereVelocities The velocity of the current frame of each sphere.
* @return The prediction of the position at the current frame of each sphere, i.e. spherePositionPredictions.
*/
public Vector3[] PredictSpherePositions(Vector3[] spherePositionPredictions, int spheresCount, Vector3[] spherePositions,
Vector3[] sphereVelocities)
{
for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
private bool firstCall = true; // Add this line to keep track of the first call
public Vector3[] PredictSpherePositions(Vector3[] spherePositionPredictions, int spheresCount, Vector3[] spherePositions,
Vector3[] sphereVelocities)
{
string filePath = "/home/akreibich/TestRobinCode37/debugPredictPosition"; // File path
for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
{
// If this is the first call and we are at the first sphere, apply the special offset
if (firstCall && sphereIndex == 0)
{
spherePositionPredictions[0] = spherePositions[0] + new Vector3(0, 0, zDisplacement); // Used zDisplacement here
using (StreamWriter writer = new StreamWriter(filePath, true)) // 'true' parameter appends to the file
{
//writer.WriteLine("First call, first sphere: special offset applied.");
}
}
else
{
spherePositionPredictions[sphereIndex] = spherePositions[sphereIndex] + Time.deltaTime * sphereVelocities[sphereIndex];
spherePositionPredictions[sphereIndex] =spherePositions[sphereIndex] + Time.deltaTime * sphereVelocities[sphereIndex];
if (firstCall && sphereIndex == 0) // This condition should never be true, but adding for completeness
{
using (StreamWriter writer = new StreamWriter(filePath, true))
{
//writer.WriteLine("First call, first sphere: special offset NOT applied.");
}
}
else
{
using (StreamWriter writer = new StreamWriter(filePath, true))
{
//writer.WriteLine($"Subsequent calls or other spheres: regular update applied for sphere {sphereIndex}.");
}
}
}
using (StreamWriter writer = new StreamWriter("/home/akreibich/TestRobinCode37/Position#N.txt", true))
{
//writer.WriteLine("PredictSpherePositions called. firstCall: " + firstCall);
// Add additional logging if needed
}
}
return spherePositionPredictions;
// Update the flag so the special case won't be applied again
if (firstCall)
{
firstCall = false;
}
return spherePositionPredictions;
}
/**
* Calculates the predictions for the angular velocities for the prediction step of the algorithm.
* @param cylinderAngularVelocities The angular velocity of the current frame of each orientation element/ cylinder.
......@@ -108,4 +165,4 @@ public class PredictionStep : MonoBehaviour
return cylinderOrientationPredictions;
}
}
}
\ No newline at end of file
}
......@@ -14,6 +14,7 @@ namespace GuidewireSim
public class SimulationLoop : MonoBehaviour
{
private bool isFirstCall = true;
//public int count; //hier aus CreationScript
//CreationScript creationScript; //hier -""-
private Stopwatch stopwatch;
......@@ -162,7 +163,7 @@ namespace GuidewireSim
else
{
Debug.LogError("Failed to parse rodElementLength from command-line arguments. Using default value.");
rodElementLength = 10f;
rodElementLength = 50f;
}
}
}
......@@ -211,52 +212,66 @@ namespace GuidewireSim
Time.fixedDeltaTime = timeStep;
PerformInitializationStep();
//InvokeRepeating("SavePositionsToFile", 0f, 1f);
//InvokeRepeating("SavePositionsToFile", 0f, 0.01f);
objectSetter.SetCylinderPositions(cylinders, CylinderCount, cylinderPositions);
objectSetter.SetCylinderOrientations(cylinders, CylinderCount, cylinderOrientations, directors);
}
private void FixedUpdate()
{
stopwatch.Restart();
if (ExecuteSingleLoopTest) return;
PerformSimulationLoop();
UpdateCameraPosition();
stopwatch.Stop();
long elapsedMilliseconds = stopwatch.ElapsedMilliseconds;
UnityEngine.Debug.Log($"FixedUpdate took {elapsedMilliseconds} ms");
string filePath = "/home/akreibich/TestRobinCode37/TimeCalculationsFixedUpdate.txt";
using (StreamWriter writer = new StreamWriter(filePath, true)) //The 'true" parameter appends to the file
{
writer.WriteLine($"FixedUpdate took {elapsedMilliseconds} ms");
}
string filePath2 = "/home/akreibich/TestRobinCode37/LogConstraints.txt";
using (StreamWriter writer = new StreamWriter(filePath2, true)) //same as above
{
writer.WriteLine($"FixedUpdate took place");
}
}
//private static int fixedUpdateCounter = 0;
private void FixedUpdate()
{
stopwatch.Restart();
SavePositionsToFile();
if (ExecuteSingleLoopTest) return;
PerformSimulationLoop();
UpdateCameraPosition();
stopwatch.Stop();
long elapsedMilliseconds = stopwatch.ElapsedMilliseconds;
UnityEngine.Debug.Log($"FixedUpdate took {elapsedMilliseconds} ms");
string filePath = "/home/akreibich/TestRobinCode37/TimeCalculationsFixedUpdate.txt";
using (StreamWriter writer = new StreamWriter(filePath, true))
{
//writer.WriteLine($"FixedUpdate took {elapsedMilliseconds} ms");
}
string filePath2 = "/home/akreibich/TestRobinCode37/LogConstraints.txt";
using (StreamWriter writer = new StreamWriter(filePath2, true))
{
//writer.WriteLine($"FixedUpdate took place");
}
}
public void SavePositionsToFile()
public void SavePositionsToFile()
{
string logFilePath1 = $"/home/akreibich/TestRobinCode37/Position#All.txt";
using (StreamWriter writer = new StreamWriter(logFilePath1, true))
{
for (int i = 0; i < spheres.Length; i++)
{
using (StreamWriter writer = new StreamWriter(logFilePath, true))
{
for (int i = 0; i < spheres.Length; i++)
{
Vector3 spherePosition = spheres[i].transform.position;
writer.WriteLine("Sphere " + (i + 1) + " Position: " + spherePosition.x + "," + spherePosition.y + "," + spherePosition.z);
}
}
Vector3 spherePosition = spheres[i].transform.position;
writer.WriteLine("Sphere " + (i + 1) + " Position: " + spherePosition.x + "," + spherePosition.y + "," + spherePosition.z);
}
}
//fixedUpdateCounter++; // Increment the counter for the next file name
}
private void UpdateCameraPosition()
{
if (spheres != null && spheres.Length > 0)
{
GameObject lastSphere = spheres[0];
GameObject lastSphere = spheres[spheres.Length-1];
Vector3 newCameraPosition = lastSphere.transform.position + cameraOffset;
followingCamera.transform.position = newCameraPosition;
}
......@@ -298,10 +313,10 @@ namespace GuidewireSim
SetCollidersStep();
if (solveStretchConstraints)
{
List<Vector3> allDeltaPositionOnes = constraintSolvingStep.GetAllDeltaPositionOnes();
List<Vector3> allDeltaPositionTwos = constraintSolvingStep.GetAllDeltaPositionTwos();
List<BSM.Quaternion> allDeltaOrientations = constraintSolvingStep.GetAllDeltaOrientations();
Debug.Log($"All Stretch Corrections - DeltaPositionOnes: {string.Join(", ", allDeltaPositionOnes)}, DeltaPositionTwos: {string.Join(", ", allDeltaPositionTwos)}, DeltaOrientations: {string.Join(", ", allDeltaOrientations)}");
//List<Vector3> allDeltaPositionOnes = constraintSolvingStep.GetAllDeltaPositionOnes();
//List<Vector3> allDeltaPositionTwos = constraintSolvingStep.GetAllDeltaPositionTwos();
//List<BSM.Quaternion> allDeltaOrientations = constraintSolvingStep.GetAllDeltaOrientations();
//Debug.Log($"All Stretch Corrections - DeltaPositionOnes: {string.Join(", ", allDeltaPositionOnes)}, DeltaPositionTwos: {string.Join(", ", allDeltaPositionTwos)}, DeltaOrientations: {string.Join(", ", allDeltaOrientations)}");
}
}
......@@ -330,7 +345,7 @@ namespace GuidewireSim
string filePath = "/home/akreibich/TestRobinCode37/LastSphereConstraintPositions";
using (StreamWriter writer = new StreamWriter(filePath, true)) // 'true' parameter appends to the file
{
writer.WriteLine($"Start of Constraint Solving - Last sphere position: {spherePositionPredictions[0]}");
//writer.WriteLine($"Start of Constraint Solving - Last sphere position: {spherePositionPredictions[0]}");
}
Vector3 initialLastSpherePosition = spherePositionPredictions[0];
if (solveStretchConstraints)
......@@ -347,20 +362,35 @@ namespace GuidewireSim
{
collisionSolvingStep.SolveCollisionConstraints(spherePositionPredictions, solverStep, ConstraintSolverSteps);
}
spherePositionPredictions[0] = initialLastSpherePosition;
using (StreamWriter writer = new StreamWriter(filePath, true)) // 'true' parameter appends to the file
{
writer.WriteLine($"End of Constraint Solving - Last sphere position: {spherePositionPredictions[0]}");
}
if (isFirstCall)
{
spherePositionPredictions[0] = initialLastSpherePosition;
using (StreamWriter writer = new StreamWriter(filePath, true)) // 'true' parameter appends to the file
{
//writer.WriteLine($"FirstCall");
}
}
using (StreamWriter writer = new StreamWriter(filePath, true)) // 'true' parameter appends to the file
{
//writer.WriteLine($"End of Constraint Solving - Last sphere position: {spherePositionPredictions[0]}");
}
}
// Set flag to false after the first call
if (isFirstCall)
{
isFirstCall = true;
}
}
if (solveCollisionConstraints)
{
collisionHandler.ResetRegisteredCollisions();
}
}
}
private void PerformUpdateStep()
{
......
......@@ -3,6 +3,7 @@ using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Assertions;
using BSM = BulletSharp.Math;
using System.IO;
namespace GuidewireSim
{
......@@ -12,6 +13,7 @@ namespace GuidewireSim
public class UpdateStep : MonoBehaviour
{
MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
private Vector3[] lastSphereVelocities;
private void Awake()
{
......@@ -27,17 +29,36 @@ public class UpdateStep : MonoBehaviour
* @param spherePositions The position at the current frame of each sphere.
* @return The velocity of the current frame of each sphere, i.e. @p sphereVelocities.
*/
public Vector3[] UpdateSphereVelocities(Vector3[] sphereVelocities, int spheresCount, Vector3[] spherePositionPredictions,
Vector3[] spherePositions)
public Vector3[] UpdateSphereVelocities(Vector3[] sphereVelocities, int spheresCount, Vector3[] spherePositionPredictions, Vector3[] spherePositions)
{
string debugFilePath = "/home/akreibich/TestRobinCode37/DebugVelocities.txt";
using (StreamWriter writer = new StreamWriter(debugFilePath, true))
{
for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
{
sphereVelocities[sphereIndex] = (spherePositionPredictions[sphereIndex] - spherePositions[sphereIndex]) / Time.deltaTime;
}
if (sphereIndex == 0) // Check if it's the first sphere
{
sphereVelocities[sphereIndex] = Vector3.zero; // Set velocity to (0, 0, 0)
}
else
{
// Update the velocity for other spheres
Vector3 newVelocity = (spherePositionPredictions[sphereIndex] - spherePositions[sphereIndex]) / Time.deltaTime;
sphereVelocities[sphereIndex] = newVelocity;
}
return sphereVelocities;
writer.WriteLine($"After Update: Sphere Index: {sphereIndex}, {1* sphereVelocities[sphereIndex]}");
}
CreationScript creationScript = GameObject.Find("GameObject (1)").GetComponent<CreationScript>();
if (creationScript != null)
{
creationScript.UpdateSphereVelocities(sphereVelocities);
}
}
return sphereVelocities;
}
/**
* Updates the sphere positions given the current position predictions.
* @param spherePositions The position at the current frame of each sphere.
......@@ -45,16 +66,37 @@ public class UpdateStep : MonoBehaviour
* @param spherePositionPredictions The prediction of the position at the current frame of each sphere (in this case of the last frame).
* @return The position at the current frame of each sphere, i.e. @p spherePositions.
*/
public Vector3[] UpdateSpherePositions(Vector3[] spherePositions, int spheresCount, Vector3[] spherePositionPredictions)
public Vector3[] UpdateSpherePositions(Vector3[] spherePositions, int spheresCount, Vector3[] spherePositionPredictions)
{
for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
// Initialize StreamWriter to write to the specified file.
// This will append to the file if it already exists.
using (StreamWriter writer = new StreamWriter("/home/akreibich/TestRobinCode37/UpdateStepDebug.txt", true))
{
spherePositions[sphereIndex] = spherePositionPredictions[sphereIndex];
for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
{
// Write the position before updating to the file.
//writer.WriteLine($"Before Update: Sphere Index: {sphereIndex}, {spherePositions[sphereIndex].ToString()}");
// Calculate the difference between the new and old positions.
//Vector3 difference = 1000 * (spherePositionPredictions[sphereIndex] - spherePositions[sphereIndex]);
// Update the position.
spherePositions[sphereIndex] = spherePositionPredictions[sphereIndex];
// Write the updated position and difference to the file.
// writer.WriteLine($"After Update: Sphere Index: {sphereIndex}, {spherePositions[sphereIndex].ToString()}");
// writer.WriteLine($"Difference: Sphere Index: {sphereIndex}, {difference.ToString()}");
}
}
return spherePositions;
}
/**
* Updates the cylinder angular velocities for the update step of the simulation.
* @param cylinderAngularVelocities The angular velocity of the current frame of each orientation element/ cylinder.
......@@ -88,7 +130,7 @@ public class UpdateStep : MonoBehaviour
public BSM.Quaternion[] UpdateCylinderOrientations(BSM.Quaternion[] cylinderOrientations, int cylinderCount,
BSM.Quaternion[] cylinderOrientationPredictions)
{
for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
for (int cylinderIndex = 1; cylinderIndex < cylinderCount; cylinderIndex++)
{
cylinderOrientations[cylinderIndex] = cylinderOrientationPredictions[cylinderIndex];
}
......@@ -96,4 +138,4 @@ public class UpdateStep : MonoBehaviour
return cylinderOrientations;
}
}
}
\ No newline at end of file
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment