Full code of the loop generating a long guidewire

CameraSwitcherGUI.cs

+using UnityEngine;
+using UnityEngine.UI;
+
+public class CameraSwitcherGUI : MonoBehaviour
+{
+    public Camera camera1;
+    public Camera camera2;
+    public Button switchButton;  // Reference to the UI button
+
+    private void Start()
+    {
+        // Initialize camera states
+        camera1.enabled = true;
+        camera2.enabled = false;
+
+        // Add a click listener to the button
+        switchButton.onClick.AddListener(SwitchCameras);
+    }
+
+    public void SwitchCameras()
+    {
+        camera1.enabled = !camera1.enabled;
+        camera2.enabled = !camera2.enabled;
+    }
+}
+

CameraSwitcherGUI.cs.meta

+fileFormatVersion: 2
+guid: d8499b1ef342bc95586efa57c5ce1cee
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

CollisionDetectionPrimitive.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+
+namespace GuidewireSim
+{
+    /**
+     * This class is responsible for tracking collisions of the object it is attached to. Attach this component only to sphere objects
+     * of the guidewire.
+     */
+    public class CollisionDetectionPrimitive : MonoBehaviour
+    {
+        SimulationLoop simulationLoop; //!< The SimulationLoop component in the Simulation GameObject
+        CollisionHandler collisionHandler; //!< The CollisionHandler component in the Simulation GameObject
+
+        public int sphereID; /**< The unique ID of the sphere that this component is attached to. Matches the position in @p spheres in #SimulationLoop.
+                              *   @note Should also match the position in @p spherePositions, @p sphereVelocities, @p sphereExternalForces,
+                              *   @p spherePositionPredictions in #SimulationLoop.
+                              */
+
+        private void Awake()
+        {
+            simulationLoop = FindObjectOfType<SimulationLoop>();
+            Assert.IsNotNull(simulationLoop);
+
+            collisionHandler = FindObjectOfType<CollisionHandler>();
+            Assert.IsNotNull(collisionHandler);
+        }
+
+        private void Start()
+        {
+            AssignSphereID();
+        }
+
+        /**
+         * Assigns the unique ID of the object sphere it is attached to to #sphereID.
+         */
+        private void AssignSphereID()
+        {
+            GameObject thisSphere = this.transform.gameObject;
+
+            for (int sphereIndex = 0; sphereIndex < simulationLoop.SpheresCount; sphereIndex++)
+            {
+                if (thisSphere == simulationLoop.spheres[sphereIndex])
+                {
+                    sphereID = sphereIndex;
+                    return;
+                }
+            }
+
+            Debug.LogWarning("No sphereID could be assigned.");
+        }
+
+        /**
+         * Registers a collision that Unity's collision detection detected.
+         */
+        private void OnCollisionEnter(Collision other)
+        {
+            ContactPoint collisionContact = other.GetContact(0);
+
+            Vector3 contactPoint = collisionContact.point;
+            Vector3 collisionNormal = collisionContact.normal;
+
+            collisionHandler.RegisterCollision(this.transform, sphereID, contactPoint, collisionNormal);
+        }
+
+        /**
+         * Registers a collision that Unity's collision detection detected.
+         */
+        private void OnCollisionStay(Collision other)
+        {
+            ContactPoint collisionContact = other.GetContact(0);
+
+            Vector3 contactPoint = collisionContact.point;
+            Vector3 collisionNormal = collisionContact.normal;
+
+            collisionHandler.RegisterCollision(this.transform, sphereID, contactPoint, collisionNormal);
+        }
+    }
+}
\ No newline at end of file

CollisionDetectionPrimitive.cs.meta

+fileFormatVersion: 2
+guid: 22ca87514938f7140bded0cf819e6cfa
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 1300
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

CollisionDetectionVessel.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+namespace GuidewireSim
+{
+    /**
+     * Similarly to CollisionDetectionPrimitive, this class is responsible for tracking collisions of the object it is attached to.
+     * Attach this component only to blood vessel objects.
+     */
+    public class CollisionDetectionVessel : MonoBehaviour
+    {
+        SimulationLoop simulationLoop; //!< The SimulationLoop component in the Simulation GameObject
+        CollisionHandler collisionHandler; //!< The CollisionHandler component in the Simulation GameObject
+
+        private void Awake()
+        {
+            simulationLoop = FindObjectOfType<SimulationLoop>();
+            Assert.IsNotNull(simulationLoop);
+
+            collisionHandler = FindObjectOfType<CollisionHandler>();
+            Assert.IsNotNull(collisionHandler);
+        }
+
+        // private void OnCollisionEnter(Collision other)
+        // {
+        //     // ContactPoint contactPoint = other.GetContact(0);
+        //     // DebugExtension.DrawPoint(contactPoint.point, Color.black);
+        //     int sphereID = 1;
+
+        //     collisionHandler.RegisterCollision(other.transform, other, sphereID);
+        // }
+
+        // private void OnCollisionStay(Collision other)
+        // {
+        //     // ContactPoint contactPoint = other.GetContact(0);
+        //     // DebugExtension.DrawPoint(contactPoint.point, Color.black);
+
+        //     int sphereID = 1;
+
+        //     collisionHandler.RegisterCollision(other.transform, other, sphereID);
+        // }
+
+        private int YieldSphereID(Transform sphereTransform)
+        {
+            GameObject sphereGO = sphereTransform.gameObject;
+
+            for (int sphereIndex = 0; sphereIndex < simulationLoop.SpheresCount; sphereIndex++)
+            {
+                if (sphereGO == simulationLoop.spheres[sphereIndex])
+                {
+                    return sphereIndex;
+                }
+            }
+
+            Debug.LogWarning("No sphereID could be assigned.");
+            return 0;
+        }
+    }
+}
\ No newline at end of file

CollisionDetectionVessel.cs.meta

+fileFormatVersion: 2
+guid: bb0903acb76666a4a9257caa76e71835
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

CollisionHandler.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+namespace GuidewireSim
+{
+    /**
+     * This class manages all collisions that should be resolved, i.e. the collisions of the last frame.
+     */
+    public class CollisionHandler : MonoBehaviour
+    {
+        public List<CollisionPair> registeredCollisions; //!< All collisions that occured between the last and the current frame in OnTriggerEnter.
+
+        public SphereCollider[] sphereColliders; /**< Each element stores a reference to the SpherCollider of the respective element in @p spheres
+                                                      *   in SimulationLoop.
+                                                      *   @exampletext The second element in this list is the SphereCollider corresponding to the
+                                                      *   sphere GameObject that is referenced in the second element of @p spheres in SimulationLoop.
+                                                      */
+
+        float sphereRadius = 5f; //!< The radius of the sphere elements of the guidewire.
+
+        private void Start()
+        {
+            registeredCollisions = new List<CollisionPair>();    
+        }
+
+        /**
+         * Registers a collision by adding it to #registeredCollisions.
+         * @param sphere The sphere of the guidewire that collided.
+         * @param sphereID The unique ID of @p sphere.
+         * @param contactPoint The contact point of the collision.
+         * @param collisionNormal The normal of the collision.
+         */
+        public void RegisterCollision(Transform sphere, int sphereID, Vector3 contactPoint, Vector3 collisionNormal)
+        {
+            CollisionPair registeredCollision = new CollisionPair(sphere, sphereID, contactPoint, collisionNormal);
+            registeredCollisions.Add(registeredCollision);
+        }
+
+        /**
+         * Clears the list of all registered collisions.
+         */
+        public void ResetRegisteredCollisions()
+        {
+            registeredCollisions.Clear();
+        }
+
+        /**
+         * Sets the position of the collider of each sphere to the sphere's position prediction.
+         * @note The position of the collider is implicitly set by setting the colliders center argument.
+         * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+         * @param spherePositionPredictions The prediction of the position at the current frame of each sphere (in this case of the last frame).
+         * @param spherePositions The position at the current frame of each sphere.
+         */
+        public void SetCollidersToPredictions(int spheresCount, Vector3[] spherePositionPredictions, Vector3[] spherePositions)
+        {
+            for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
+            {
+                Vector3 centerPosition = (spherePositionPredictions[sphereIndex] - spherePositions[sphereIndex]) / (2 * sphereRadius);
+                sphereColliders[sphereIndex].center = centerPosition;
+            }
+        }
+    }
+}
\ No newline at end of file

CollisionHandler.cs.meta

+fileFormatVersion: 2
+guid: 9f7f474e600043f489e35ce209adf4b9
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

CollisionPair.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace GuidewireSim
+{
+    /**
+     * Carries all information of a collision that occured.
+     */
+    public struct CollisionPair
+    {
+        public Transform sphere; //!< The sphere object of the guidewire that was part of the collision.
+        public Vector3 contactPoint; //!< The contact point of the collision.
+        public Vector3 collisionNormal; //!< The normal of the collision.
+        public int sphereID; //!< The ID of the sphere object of the guidewire that was part of the collision.
+
+        public CollisionPair(Transform sphere, int sphereID, Vector3 contactPoint, Vector3 collisionNormal)
+        {
+            this.sphere = sphere;
+            this.sphereID = sphereID;
+            this.contactPoint = contactPoint;
+            this.collisionNormal = collisionNormal;
+        }
+    }
+}
\ No newline at end of file

CollisionPair.cs.meta

+fileFormatVersion: 2
+guid: e348a30119928ff4eba43e7678df2244
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

CollisionSolvingStep.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+namespace GuidewireSim
+{
+    /**
+     * This class implements the collision solving that is part of the constraint solving step.
+     */
+    public class CollisionSolvingStep : MonoBehaviour
+    {
+        MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
+        CollisionHandler collisionHandler; //!< The component CollisionHandler that tracks all collisions.
+
+        Vector3 deltaPosition = new Vector3(); //!< The correction of @p spherePositionPrediction in method SolveCollisionConstraint().
+        Vector3 initialPositionPrediction = new Vector3();
+
+        float sphereRadius = 5f; //!< The radius of a sphere of the guidewire.
+        [SerializeField] float collisionMargin = 0.1f; /**< A margin by which a colliding element of the guidewire is set away from the object
+                                                        *   colliding with in the direction of the normal.
+                                                        *   @note Without this margin, the colliding element of the guidewire (e.g. a sphere) is
+                                                        *   corrected such that its surface exactly touches the object colliding with, which
+                                                        *   results in the guidewire still penetrating the object.
+                                                        */
+        [Range(0.00001f, 1f)] float collisionStiffness = 0.001f; //!< The collision constraint stiffness parameter.
+
+        private void Awake()
+        {
+            mathHelper = GetComponent<MathHelper>();
+            Assert.IsNotNull(mathHelper);
+
+            collisionHandler = GetComponent<CollisionHandler>();
+            Assert.IsNotNull(collisionHandler);
+        }
+
+        /**
+         * Is responsible for executing one iteration of the constraint solving step for the collision constraint
+         * of each collision of this frame.
+         * @param spherePositionPredictions The prediction of the position at the current frame of each sphere (in this case of the last frame).
+         * @param solverStep The current iteration of the constraint solving step.
+         * @param constraintSolverSteps The total number of solver steps of the constraint solving step.
+         */
+        public void SolveCollisionConstraints(Vector3[] spherePositionPredictions, int solverStep, int constraintSolverSteps)
+        {
+
+            for (int collisionIndex = 0; collisionIndex < collisionHandler.registeredCollisions.Count; collisionIndex++)
+            {
+                CollisionPair collisionPair = collisionHandler.registeredCollisions[collisionIndex];
+                int sphereID = collisionPair.sphereID;
+                Vector3 spherePositionPrediction = spherePositionPredictions[sphereID];
+
+                SolveCollisionConstraint(spherePositionPrediction, collisionPair.contactPoint, collisionPair.collisionNormal,
+                                         solverStep, out deltaPosition);
+                CorrectCollisionPredictions(sphereID, spherePositionPredictions, solverStep, constraintSolverSteps);
+            }
+        }
+
+        /**
+         * Solves the collision constraint for one collision that occured this frame.
+         * @param spherePositionPredictions The prediction of the position at the current frame of each sphere (in this case of the last frame).
+         * @param contactPoint The contact point of the collision.
+         * @param collisionNormal The normal of the collision.
+         * @param solverStep The current iteration of the constraint solving step.
+         * @attention Current calculation of the normal only works for spheres.
+         */
+        private void SolveCollisionConstraint(Vector3 spherePositionPrediction, Vector3 contactPoint, Vector3 collisionNormal,
+                                             int solverStep, out Vector3 deltaPosition)
+        {
+            if (solverStep == 0)
+            {
+                DrawCollisionInformation(spherePositionPrediction, contactPoint, collisionNormal);
+            }
+
+            deltaPosition = CalculateDeltaPosition(spherePositionPrediction, contactPoint, collisionNormal);
+        }
+
+        /**
+         * Draws the contact point, collision normal, and displacement corrections into the scene of the collision that occured.
+         * @param spherePositionPrediction The position prediction of the sphere that collided.
+         * @param contactPoint The contact point of the collision.
+         * @param collisionNormal The normal of the collision.
+         */
+        private void DrawCollisionInformation(Vector3 spherePositionPrediction, Vector3 contactPoint, Vector3 collisionNormal)
+        {
+            Debug.DrawLine(contactPoint, contactPoint + 10f * collisionNormal, Color.blue, 2f);
+            DebugExtension.DrawPoint(spherePositionPrediction, Color.white);
+            DebugExtension.DrawPoint(contactPoint, Color.yellow);
+        }
+
+        /**
+         * Calculates the displacement of the collision constraint.
+         * @param spherePositionPredictions The prediction of the position at the current frame of each sphere (in this case of the last frame).
+         * @param closestSurfacePoint The contact point of the collision.
+         * @param normalVector The collision normal.
+         * @return The delta position, i.e. the calculated displacement.
+         */
+        private Vector3 CalculateDeltaPosition(Vector3 spherePositionPrediction, Vector3 closestSurfacePoint, Vector3 normalVector)
+        {
+            return - (spherePositionPrediction - sphereRadius * normalVector - closestSurfacePoint - collisionMargin * normalVector);
+        }
+
+        /**
+         * Corrects the position prediction of the sphere of @p sphereIndex with the calculated displacement.
+         * @param sphereIndex The sphere ID of the colliding sphere.
+         * @param spherePositionPredictions The prediction of the position at the current frame of each sphere (in this case of the last frame).
+         * @param solverStep The current iteration of the constraint solving step.
+         * @param constraintSolverSteps The total number of solver steps of the constraint solving step.
+         */
+        private void CorrectCollisionPredictions(int sphereIndex, Vector3[] spherePositionPredictions, int solverStep, int constraintSolverSteps)
+        {
+            Assert.IsTrue(sphereIndex >= 0);
+
+            if (solverStep == 0)
+            {
+                initialPositionPrediction = spherePositionPredictions[sphereIndex];
+            }
+
+            spherePositionPredictions[sphereIndex] += collisionStiffness * deltaPosition;
+
+            if (solverStep == constraintSolverSteps - 1)
+            {
+                DebugExtension.DrawPoint(spherePositionPredictions[sphereIndex], Color.red);
+            }
+        }
+    }
+}
\ No newline at end of file

CollisionSolvingStep.cs.meta

+fileFormatVersion: 2
+guid: 70b3fde2a73919f49b9b11e9deeeef0d
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

CollisionTestPerformer.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+namespace GuidewireSim
+{
+public class CollisionTestPerformer : MonoBehaviour
+{
+    SimulationLoop simulationLoop; //!< The SimulationLoop component that executes all steps of the simulation loop.
+
+    [SerializeField] Vector3 pullForce = new Vector3(0f, 0f, 5f); //!< External force that is applied in Force Test Three.
+
+    [SerializeField] bool doCollisionTestOne = false;
+    [SerializeField] bool doCollisionTestTwo = false;
+    [SerializeField] bool doCollisionTestThree = false;
+    [SerializeField] bool doCollisionTestFour = false;
+    float startTime = 0f;
+
+    private void Awake()
+    {
+        simulationLoop = GetComponent<SimulationLoop>();
+        Assert.IsNotNull(simulationLoop);
+    }
+
+    private void Start()
+    {
+        startTime = Time.time;
+        PerformCollisionTests();
+    }
+
+    /**
+     * Performs each Torque Test whose respective serialized boolean is set to true in the Unity inspector.
+     */
+    private void PerformCollisionTests()
+    {
+        if (doCollisionTestOne) PerformCollisionTestOne();
+        else if (doCollisionTestTwo) StartCoroutine(PerformCollisionTestTwo());
+        else if (doCollisionTestThree) StartCoroutine(PerformCollisionTestThree());
+        else if (doCollisionTestFour) StartCoroutine(PerformCollisionTestFour());
+    }
+
+    /**
+     * Performs torque test one. This test applies an external force to one end of the guidewire.
+     */
+    private void PerformCollisionTestOne()
+    {
+        for (int sphereIndex = 0; sphereIndex < (simulationLoop.SpheresCount - 1); sphereIndex++)
+        {
+            simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
+        }
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForce;
+    }
+
+    // force gets applied for a fixed time
+    private IEnumerator PerformCollisionTestTwo(float applyForceTime = 1.5f)
+    {
+        for (int sphereIndex = 0; sphereIndex < (simulationLoop.SpheresCount - 1); sphereIndex++)
+        {
+            simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
+        }
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForce;
+
+        yield return new WaitForSeconds(applyForceTime);
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = Vector3.zero;
+
+        float timeDiff = Time.time - startTime;
+        Debug.Log("Elapsed time of collision test: " + timeDiff);
+        Debug.Log("Velocity at test end: " + simulationLoop.sphereVelocities[1].ToString("e2"));
+        Debug.Log("End of Pull Phase of Collision Test Two");
+    }
+
+    // force gets applied until a fixed velocity is reached
+    private IEnumerator PerformCollisionTestThree(float exitVelocity = 4f)
+    {
+        for (int sphereIndex = 0; sphereIndex < (simulationLoop.SpheresCount - 1); sphereIndex++)
+        {
+            simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
+        }
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForce;
+
+
+        yield return new WaitUntil(() => simulationLoop.sphereVelocities[simulationLoop.SpheresCount - 1].z >= exitVelocity);
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = Vector3.zero;
+
+        float timeDiff = Time.time - startTime;
+        Debug.Log("Elapsed time of collision test: " + timeDiff);
+        Debug.Log("Velocity at test end: " + simulationLoop.sphereVelocities[1].ToString("e2"));
+        Debug.Log("End of Pull Phase of Collision Test Three");
+    }
+
+    // force gets applied for the whole time
+    private IEnumerator PerformCollisionTestFour(float pullForceFactor = 0.3f)
+    {
+        float appliedPullForce = pullForceFactor * pullForce.z;
+        
+        Debug.Log("Start of Collision Test Four");
+        Debug.Log("Pull Force: " + appliedPullForce);
+
+        for (int sphereIndex = 0; sphereIndex < (simulationLoop.SpheresCount - 1); sphereIndex++)
+        {
+            simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
+        }
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForceFactor * pullForce;
+
+        yield return null;
+    }
+}
+}

CollisionTestPerformer.cs.meta

+fileFormatVersion: 2
+guid: ad1da93e65d612f4bb67479904956eca
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 1200
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

ConstraintSolvingStep.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+using BSM = BulletSharp.Math;
+
+namespace GuidewireSim
+{
+/**
+ * This class executes and implements various algorithms of the constraint solving step of the algorithm and manages all coherent data.
+ */
+public class ConstraintSolvingStep : MonoBehaviour
+{
+    MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
+
+    Vector3 deltaPositionOne = new Vector3(); //!< The correction of @p particlePositionOne in method SolveStretchConstraint().
+    Vector3 deltaPositionTwo = new Vector3(); //!< The correction of @p particlePositionTwo in method SolveStretchConstraint().
+    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().
+
+    float stretchStiffness = 0.1f;
+    float bendStiffness = 0.1f;
+
+    [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.
+
+    private void Awake()
+    {
+        mathHelper = GetComponent<MathHelper>();
+        Assert.IsNotNull(mathHelper);
+    }
+
+    /**
+     * Is responsible for executing one iteration of the constraint solving step for the stretch constraint, i.e. corrects each particle
+     * position prediction one time and also each orientation prediction one time.
+     * @note Can be executed in naive order or bilateral interleaving order.
+     * @param spherePositionPredictions The array of position predictions that get corrected in this step.
+     * @param cylinderOrientationPredictions The array of orientation predictions that get corrected in this step.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param worldSpaceBasis The three basis vectors of the world coordinate system as embedded quaternions with scalar part 0.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @req @p spheresCount should be at least one.
+     * @req @p rodElementLength should be positive.
+     * @req Executes the constraint solving step in bilateral interleaving order if #executeInBilateralOrder and otherwise in naive order.
+     */
+    public void SolveStretchConstraints(Vector3[] spherePositionPredictions, BSM.Quaternion[] cylinderOrientationPredictions,
+                                        int spheresCount, BSM.Quaternion[] worldSpaceBasis, float rodElementLength)
+    {
+        Assert.IsTrue(spheresCount >= 1);
+        Assert.IsTrue(rodElementLength > 0f);
+
+        BSM.Quaternion e_3 = worldSpaceBasis[2];
+
+        if (executeInBilateralOrder)
+        {
+            SolveStretchConstraintsInBilateralOrder(spherePositionPredictions, cylinderOrientationPredictions, spheresCount, rodElementLength, e_3);
+        }
+        else
+        {
+            SolveStretchConstraintsInNaiveOrder(spherePositionPredictions, cylinderOrientationPredictions, spheresCount, rodElementLength, e_3);
+        }
+    }
+
+    /**
+     * Is responsible for executing one iteration of the constraint solving step for the bend twist constraint, i.e. corrects each orientation
+     * prediction one time.
+     * @note Can be executed in naive order or bilateral interleaving order.
+     * @param cylinderOrientationPredictions The array of orientation predictions that get corrected in this step.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param discreteRestDarbouxVectors The array of all discrete Darboux Vectors at the rest configuration, i.e. at frame 0. Has (n-1) elements,
+     * if n is the number of orientations of the guidewire, because the darboux vector is taken of two adjacent orientations.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @req @p cylinderCount should be at least one.
+     * @req @p rodElementLength should be positive.
+     * @req Executes the constraint solving step in bilateral interleaving order if #executeInBilateralOrder and otherwise in naive order.
+     */
+    public void SolveBendTwistConstraints(BSM.Quaternion[] cylinderOrientationPredictions, int cylinderCount, Vector3[] discreteRestDarbouxVectors,
+                                          float rodElementLength)
+    {
+        Assert.IsTrue(cylinderCount >= 1);
+        Assert.IsTrue(rodElementLength > 0f);
+
+        if (executeInBilateralOrder)
+        {
+            SolveBendTwistConstraintsInBilateralOrder(cylinderOrientationPredictions, cylinderCount, discreteRestDarbouxVectors, rodElementLength);
+        }
+        else
+        {
+            SolveBendTwistConstraintsInNaiveOrder(cylinderOrientationPredictions, cylinderCount, discreteRestDarbouxVectors, rodElementLength);
+        }
+    }
+
+    /**
+     * Executes one iteration of the constraint solving step for the stretch constraint in bilateral order, i.e. corrects each particle
+     * position prediction one time and also each orientation prediction one time.
+     * @note You can read more about bilateral order in the 2016 paper "Position and Orientation Based Cosserat Rods".
+     * @attention The index shifting of this algorithm is not easy to understand, but got deeply tested.
+     * @param spherePositionPredictions The array of position predictions that get corrected in this step.
+     * @param cylinderOrientationPredictions The array of orientation predictions that get corrected in this step.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @param e_3 The third basis vector of the world coordinate system as embedded quaternions with scalar part 0.
+     * @req @p spheresCount should be at least one.
+     * @req @p rodElementLength should be positive.
+     */
+    private void SolveStretchConstraintsInBilateralOrder(Vector3[] spherePositionPredictions, BSM.Quaternion[] cylinderOrientationPredictions,
+                                                         int spheresCount, float rodElementLength, BSM.Quaternion e_3)
+    {
+        Assert.IsTrue(spheresCount >= 1);
+        Assert.IsTrue(rodElementLength > 0f);
+        
+        int upcountingIndex = 0;
+        int downcountingIndex;
+        int lastAscendingIndex = spheresCount - 2;
+
+        if (spheresCount % 2 == 0) // e.g. spheresCount is an even number
+        {
+            downcountingIndex = lastAscendingIndex - 1;
+
+            SolveStretchConstraint(spherePositionPredictions[lastAscendingIndex], spherePositionPredictions[lastAscendingIndex + 1],
+                                   cylinderOrientationPredictions[lastAscendingIndex], e_3, rodElementLength, out deltaPositionOne,
+                                   out deltaPositionTwo, out deltaOrientation);
+            CorrectStretchPredictions(lastAscendingIndex, spherePositionPredictions, cylinderOrientationPredictions);
+        }
+        else // spheresCount % 2 == 1, e.g. spheresCount is an odd number
+        {
+            downcountingIndex = lastAscendingIndex;
+        }
+
+        while (upcountingIndex < lastAscendingIndex)
+        {
+            // upcounting
+            SolveStretchConstraint(spherePositionPredictions[upcountingIndex], spherePositionPredictions[upcountingIndex + 1],
+                                   cylinderOrientationPredictions[upcountingIndex], e_3, rodElementLength, out deltaPositionOne,
+                                   out deltaPositionTwo, out deltaOrientation);
+            CorrectStretchPredictions(upcountingIndex, spherePositionPredictions, cylinderOrientationPredictions);
+
+            // downcounting
+            SolveStretchConstraint(spherePositionPredictions[downcountingIndex], spherePositionPredictions[downcountingIndex + 1],
+                                   cylinderOrientationPredictions[downcountingIndex], e_3, rodElementLength, out deltaPositionOne,
+                                   out deltaPositionTwo, out deltaOrientation);
+            CorrectStretchPredictions(downcountingIndex, spherePositionPredictions, cylinderOrientationPredictions);
+
+            upcountingIndex += 2;
+            downcountingIndex -= 2;
+        }
+    }
+
+    /**
+     * Executes one iteration of the constraint solving step for the stretch constraint in naive order, i.e. corrects each particle
+     * position prediction one time and also each orientation prediction one time.
+     * @note Naive order means the predictions are updated beginning from one end of the guidewire to the other end of the guidewire.
+     * @param spherePositionPredictions The array of position predictions that get corrected in this step.
+     * @param cylinderOrientationPredictions The array of orientation predictions that get corrected in this step.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @param e_3 The third basis vector of the world coordinate system as embedded quaternions with scalar part 0.
+     * @req @p spheresCount should be at least one.
+     * @req @p rodElementLength should be positive.
+     */
+    private void SolveStretchConstraintsInNaiveOrder(Vector3[] spherePositionPredictions, BSM.Quaternion[] cylinderOrientationPredictions,
+                                                     int spheresCount, float rodElementLength, BSM.Quaternion e_3)
+    {
+        Assert.IsTrue(spheresCount >= 1);
+        Assert.IsTrue(rodElementLength > 0f);
+
+        for (int sphereIndex = 0; sphereIndex < spheresCount - 1; sphereIndex++)
+        {
+            SolveStretchConstraint(spherePositionPredictions[sphereIndex], spherePositionPredictions[sphereIndex + 1],
+                                   cylinderOrientationPredictions[sphereIndex], e_3, rodElementLength, out deltaPositionOne,
+                                   out deltaPositionTwo, out deltaOrientation);
+            CorrectStretchPredictions(sphereIndex, spherePositionPredictions, cylinderOrientationPredictions);
+        }
+    }
+
+    /**
+     * Is responsible for executing one iteration of the constraint solving step for the bend twist constraint in bilateral order,
+     * i.e. corrects each orientation prediction one time.
+     * @note You can read more about bilateral order in the 2016 paper "Position and Orientation Based Cosserat Rods".
+     * @attention The index shifting of this algorithm is not easy to understand, but got deeply tested.
+     * @param cylinderOrientationPredictions The array of orientation predictions that get corrected in this step.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param discreteRestDarbouxVectors The array of all discrete Darboux Vectors at the rest configuration, i.e. at frame 0. Has (n-1) elements,
+     * if n is the number of orientations of the guidewire, because the darboux vector is taken of two adjacent orientations.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @req @p cylinderCount should be at least one.
+     * @req @p rodElementLength should be positive.
+     */
+    private void SolveBendTwistConstraintsInBilateralOrder(BSM.Quaternion[] cylinderOrientationPredictions, int cylinderCount,
+                                                           Vector3[] discreteRestDarbouxVectors, float rodElementLength)
+    {
+        Assert.IsTrue(cylinderCount >= 1);
+        Assert.IsTrue(rodElementLength > 0f);
+
+        int upcountingIndex = 0;
+        int downcountingIndex;
+        int lastAscendingIndex = cylinderCount - 2;
+
+        if (cylinderCount % 2 == 0) // e.g. cylinderCount is an even number
+        {
+            downcountingIndex = lastAscendingIndex - 1;
+
+            SolveBendTwistConstraint(cylinderOrientationPredictions[lastAscendingIndex], cylinderOrientationPredictions[lastAscendingIndex + 1],
+                                     discreteRestDarbouxVectors[lastAscendingIndex], rodElementLength, out deltaOrientationOne,
+                                     out deltaOrientationTwo);
+            CorrectBendTwistPredictions(lastAscendingIndex, cylinderOrientationPredictions);
+        }
+        else // cylinderCount % 2 == 1, e.g. sphereLength is an odd number
+        {
+            downcountingIndex = lastAscendingIndex;
+        }
+
+        while (upcountingIndex < lastAscendingIndex)
+        {
+            // upcounting
+            SolveBendTwistConstraint(cylinderOrientationPredictions[upcountingIndex], cylinderOrientationPredictions[upcountingIndex + 1],
+                                     discreteRestDarbouxVectors[upcountingIndex], rodElementLength, out deltaOrientationOne,
+                                     out deltaOrientationTwo);
+            CorrectBendTwistPredictions(upcountingIndex, cylinderOrientationPredictions);
+
+            // downcounting
+            SolveBendTwistConstraint(cylinderOrientationPredictions[downcountingIndex], cylinderOrientationPredictions[downcountingIndex + 1],
+                                     discreteRestDarbouxVectors[downcountingIndex], rodElementLength, out deltaOrientationOne,
+                                     out deltaOrientationTwo);
+            CorrectBendTwistPredictions(downcountingIndex, cylinderOrientationPredictions);
+
+            upcountingIndex += 2;
+            downcountingIndex -= 2;
+        }
+    }
+
+    /**
+     * Is responsible for executing one iteration of the constraint solving step for the bend twist constraint in naive order,
+     * i.e. corrects each orientation prediction one time.
+     * @note Naive order means the predictions are updated beginning from one end of the guidewire to the other end of the guidewire.
+     * @param cylinderOrientationPredictions The array of orientation predictions that get corrected in this step.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param discreteRestDarbouxVectors The array of all discrete Darboux Vectors at the rest configuration, i.e. at frame 0. Has (n-1) elements,
+     * if n is the number of orientations of the guidewire, because the darboux vector is taken of two adjacent orientations.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @req @p cylinderCount should be at least one.
+     * @req @p rodElementLength should be positive.
+     */
+    private void SolveBendTwistConstraintsInNaiveOrder(BSM.Quaternion[] cylinderOrientationPredictions, int cylinderCount,
+                                                       Vector3[] discreteRestDarbouxVectors, float rodElementLength)
+    {
+        Assert.IsTrue(cylinderCount >= 1);
+        Assert.IsTrue(rodElementLength > 0f);
+
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount - 1; cylinderIndex++)
+        {
+            Vector3 discreteRestDarbouxVector = discreteRestDarbouxVectors[cylinderIndex];
+
+            SolveBendTwistConstraint(cylinderOrientationPredictions[cylinderIndex], cylinderOrientationPredictions[cylinderIndex + 1],
+                                     discreteRestDarbouxVector, rodElementLength, out deltaOrientationOne,
+                                     out deltaOrientationTwo);
+            CorrectBendTwistPredictions(cylinderIndex, cylinderOrientationPredictions);
+        }
+    }
+
+    /**
+     * Solves the stretch constraint by calculating the corrections @p deltaPositionOne and @p deltaPositionTwo, @p deltaOrientation.
+     * @note To be more precise, the stretch constraint is not solved but minimized, i.e. the constraint will after
+     * correcting with the corrections be closer to zero.
+     * @param particlePositionOne The first particle position prediction of the centerline element to be corrected.
+     * @param particlePositionTwo The second particle position prediction of the centerline element to be corrected.
+     * @param orientation The  orientation quaternion prediction of the orientation element between the particle positions to be corrected.
+     * @param e_3 The third basis vector of the world space coordinates embedded as a quaternion with scalar part 0.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @param[out] deltaPositionOne The correction of @p particlePositionOne.
+     * @param[out] deltaPositionTwo The correction of @p particlePositionTwo.
+     * @param[out] deltaOrientation The correction of @p orientation.
+     * @param inverseMassOne The inverse mass scalar for @p particlePositionOne. Use a value of 1 for a moving particle and 0 for a fixed particle.
+     * @param inverseMassTwo The inverse mass scalar for @p particlePositionTwo. Use a value of 1 for a moving particle and 0 for a fixed particle.
+     * @param inertiaWeight The inertia weight scalar for @p orientation. Use a value of 1 for a moving orientation and 0 for a fixed orientation.
+     * @req @p orientation should be a unit quaternions, i.e. have length approximately equal to one.
+     * @req @p e_3 should be a unit quaternions, i.e. have length approximately equal to one.
+     * @req @p rodElementLength should be positive.
+     * @req @p inverseMassOne, @p inverseMassTwo and @p inertiaWeight should be values between 0 and 1.
+     */
+    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 = 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);
+        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;
+
+        Vector3 factor = (1f / rodElementLength) * (particlePositionTwo - particlePositionOne) - thirdDirector;
+        BSM.Quaternion embeddedFactor = mathHelper.EmbeddedVector(factor);
+        
+        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;
+        deltaOrientation = quaternionScalarFactor * quaternionProduct;
+    }
+
+    /**
+     * Solves the bend twist constraint by calculating the corrections @p deltaOrientationOne and @p deltaOrientationTwo.
+     * @note To be more precise, the bend twist constraint is not solved but minimized, i.e. the constraint will after
+     * correcting with the corrections be closer to zero.
+     * @param orientationOne The first orientation quaternion prediction of the orientation element to be corrected.
+     * @param orientationTwo The second orientation quaternion prediction of the orientation element to be corrected.
+     * @param discreteRestDarbouxVector The discrete Darboux Vector at the rest configuration, i.e. at frame 0.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @param[out] deltaOrientationOne The correction of @p orientationOne.
+     * @param[out] deltaOrientationTwo The correction of @p orientationTwo.
+     * @param inertiaWeightOne The inertia weight scalar for @p orientationOne. Use a value of 1 for a moving orientation and 0 for a fixed orientation.
+     * @param inertiaWeightTwo The inertia weight scalar for @p orientationTwo. Use a value of 1 for a moving orientation and 0 for a fixed orientation.
+     * @req @p orientationOne and @p orientationTwo should be unit quaternions, i.e. have length approximately equal to one.
+     * @req @p rodElementLength should be positive.
+     * @req @p inertiaWeightOne and @p inertiaWeightTwo should be values between 0 and 1.
+     */
+    public void SolveBendTwistConstraint(BSM.Quaternion orientationOne, BSM.Quaternion orientationTwo, Vector3 discreteRestDarbouxVector,
+                                         float rodElementLength, out BSM.Quaternion deltaOrientationOne,
+                                         out BSM.Quaternion deltaOrientationTwo, float inertiaWeightOne = 1f, float inertiaWeightTwo = 1f)
+    {
+        Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(orientationOne), tolerance: 0.01f);
+        Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(orientationTwo), tolerance: 0.01f);
+        Assert.IsTrue(rodElementLength > 0f);
+        Assert.IsTrue(inertiaWeightOne >= 0f && inertiaWeightOne <= 1f);
+        Assert.IsTrue(inertiaWeightTwo >= 0f && inertiaWeightTwo <= 1f);
+
+        float denominator = inertiaWeightOne + inertiaWeightTwo;
+        Vector3 discreteDarbouxVector = mathHelper.DiscreteDarbouxVector(orientationOne, orientationTwo, rodElementLength);
+        float darbouxSignFactor = mathHelper.DarbouxSignFactor(discreteDarbouxVector, discreteRestDarbouxVector);
+        
+        Vector3 darbouxDifference = discreteDarbouxVector - darbouxSignFactor * discreteRestDarbouxVector;
+        BSM.Quaternion embeddedDarbouxDifference = mathHelper.EmbeddedVector(darbouxDifference);
+
+        deltaOrientationOne = inertiaWeightOne * orientationTwo * embeddedDarbouxDifference / denominator;
+        deltaOrientationTwo = - inertiaWeightTwo * orientationOne * embeddedDarbouxDifference / denominator;
+    }
+
+    /**
+     * Corrects the predictions of the stretch constraint by adding @p deltaPositionOne, @p deltaPositionTwo and @p deltaOrientation.
+     * @note Note that @p deltaOrientation may has a length unequal one by definition.
+     * @param sphereIndex The index of the first element of @p spherePositionPredictions that gets corrected.
+     * @param spherePositionPredictions The array of position predictions of which two positions get corrected in this method.
+     * @param cylinderOrientationPredictions The array of orientation predictions of which one quaternions gets corrected in this method.
+     * @req The relevant entries of @p cylinderOrientationPredictions should be unit quaternions, i.e. have length approximately equal to one.
+     * @req After the quaternion prediction got corrected, it should again be a unit quaternions, i.e. have length approximately equal to one.
+     */
+    private void CorrectStretchPredictions(int sphereIndex, Vector3[] spherePositionPredictions, BSM.Quaternion[] cylinderOrientationPredictions)
+    {
+        Assert.IsTrue(sphereIndex >= 0);
+        Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[sphereIndex]), tolerance: 0.01f);
+
+        spherePositionPredictions[sphereIndex] += stretchStiffness * deltaPositionOne;
+        spherePositionPredictions[sphereIndex + 1] += stretchStiffness * deltaPositionTwo;
+        cylinderOrientationPredictions[sphereIndex] += stretchStiffness * deltaOrientation;
+
+        cylinderOrientationPredictions[sphereIndex].Normalize();
+
+        Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[sphereIndex]), tolerance: 0.01f);
+    }
+
+    /**
+     * Corrects the predictions of the bend twist constraint by adding @p deltaOrientationOne and @p deltaOrientationTwo.
+     * @note Note that @p deltaOrientationOne and @p deltaOrientationTwo may have a length unequal one by definition.
+     * @param cylinderIndex The index of the first element of @p cylinderOrientationPredictions that gets corrected.
+     * @param cylinderOrientationPredictions The array of orientation predictions of which two quaternions get corrected in this method.
+     * @req The relevant entries of @p cylinderOrientationPredictions should be unit quaternions, i.e. have length approximately equal to one.
+     * @req After the quaternion predictions got corrected, they should again be unit quaternions, i.e. have length approximately equal to one.
+     */
+    private void CorrectBendTwistPredictions(int cylinderIndex, BSM.Quaternion[] cylinderOrientationPredictions)
+    {
+        Assert.IsTrue(cylinderIndex >= 0);
+        Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[cylinderIndex]), tolerance: 0.01f);
+        Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[cylinderIndex + 1]), tolerance: 0.01f);
+
+        cylinderOrientationPredictions[cylinderIndex] += bendStiffness * deltaOrientationOne;
+        cylinderOrientationPredictions[cylinderIndex + 1] += bendStiffness * deltaOrientationTwo;
+
+        cylinderOrientationPredictions[cylinderIndex].Normalize();
+        cylinderOrientationPredictions[cylinderIndex + 1].Normalize();
+
+        Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[cylinderIndex]), tolerance: 0.01f);
+        Assert.AreApproximatelyEqual(1f, mathHelper.QuaternionLength(cylinderOrientationPredictions[cylinderIndex + 1]), tolerance: 0.01f);
+    }
+}
+}
\ No newline at end of file

ConstraintSolvingStep.cs.meta

+fileFormatVersion: 2
+guid: 44f385d20ea9f8d49a21d6275cc8dafe
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

CreationScript.cs

+using UnityEngine;
+using System.IO;
+using GuidewireSim;
+
+public class CreationScript : MonoBehaviour
+{
+    public GameObject spherePrefab;
+    public GameObject cylinderPrefab;
+    private int spheresCount;
+    private int cylinderCount;
+    private GameObject[] spheres;
+    private GameObject[] cylinders;
+
+    private SimulationLoop simulationLoop;
+
+    void Start()
+    {
+        simulationLoop = GameObject.Find("Simulation").GetComponent<SimulationLoop>();
+    }
+
+    void FixedUpdate()
+    {
+        SavePositionsToFile();
+    }
+
+    public void CreateGuidewire(int numberElements)
+    {
+        spheres = new GameObject[numberElements];
+        cylinders = new GameObject[numberElements - 1];
+        float rEL = simulationLoop.GetRodElementLength();
+
+        for (int i = 0; i < numberElements; ++i)
+        {
+            GameObject sphere = Instantiate(spherePrefab);
+            sphere.transform.position = new Vector3(0, 7, -1000 + i * rEL);
+            sphere.transform.parent = this.transform;
+
+            spheres[i] = sphere;
+
+            if (i < numberElements - 1)
+            {
+                GameObject cylinder = Instantiate(cylinderPrefab);
+                cylinder.layer = 6;
+                cylinder.transform.parent = this.transform;
+
+                cylinders[i] = cylinder;
+            }
+        }
+
+        spheresCount = numberElements;
+
+        simulationLoop.SetSpheres(spheres);
+        simulationLoop.SetCylinders(cylinders);
+    }
+
+    public void SavePositionsToFile()
+    {
+    	string path = "/home/akreibich/TestRobinCode22/PositionTest22_2.txt";
+    	StreamWriter writer = new StreamWriter(path, true);
+
+    	if (spheresCount > 0)
+    	{
+        	Vector3 firstSpherePosition = spheres[0].transform.position;
+        	writer.WriteLine("First Sphere: " + firstSpherePosition.x + "," + firstSpherePosition.y + "," + firstSpherePosition.z);
+
+        	if (spheresCount > 1)
+        	{
+            	Vector3 lastSpherePosition = spheres[spheresCount - 1].transform.position;
+            	writer.WriteLine("Last Sphere: " + lastSpherePosition.x + "," + lastSpherePosition.y + "," + lastSpherePosition.z);
+        	}
+    	}
+
+    	writer.Close();
+    }
+
+
+    
+
+    public GameObject GetLastSphere()
+    {
+        if (spheres != null && spheres.Length > 0)
+        {
+            return spheres[spheres.Length - 1];
+        }
+        return null;
+    }
+
+    public GameObject[] GetSpheres()
+    {
+        return spheres;
+    }
+
+    public GameObject[] GetCylinders()
+    {
+        return cylinders;
+    }
+}

CreationScript.cs.meta

+fileFormatVersion: 2
+guid: 0db5d8d9e5b58013aada8da779d68927
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

DebugExtension.cs

+using UnityEngine;
+using System.Collections;
+using System.Reflection;
+
+/// <summary>
+/// Debug Extension
+/// 	- Static class that extends Unity's debugging functionallity.
+/// 	- Attempts to mimic Unity's existing debugging behaviour for ease-of-use.
+/// 	- Includes gizmo drawing methods for less memory-intensive debug visualization.
+/// </summary>
+
+public static class DebugExtension
+{
+	#region DebugDrawFunctions
+	
+	/// <summary>
+	/// 	- Debugs a point.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The point to debug.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the point.
+	/// </param>
+	/// <param name='scale'>
+	/// 	- The size of the point.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the point.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not this point should be faded when behind other objects.
+	/// </param>
+	public static void DebugPoint(Vector3 position, Color color, float scale = 1.0f, float duration = 0, bool depthTest = true)
+	{
+		color = (color == default(Color)) ? Color.white : color;
+		
+		Debug.DrawRay(position+(Vector3.up*(scale*0.5f)), -Vector3.up*scale, color, duration, depthTest);
+		Debug.DrawRay(position+(Vector3.right*(scale*0.5f)), -Vector3.right*scale, color, duration, depthTest);
+		Debug.DrawRay(position+(Vector3.forward*(scale*0.5f)), -Vector3.forward*scale, color, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a point.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The point to debug.
+	/// </param>
+	/// <param name='scale'>
+	/// 	- The size of the point.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the point.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not this point should be faded when behind other objects.
+	/// </param>
+	public static void DebugPoint(Vector3 position, float scale = 1.0f, float duration = 0, bool depthTest = true)
+	{
+		DebugPoint(position, Color.white, scale, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs an axis-aligned bounding box.
+	/// </summary>
+	/// <param name='bounds'>
+	/// 	- The bounds to debug.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the bounds.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the bounds.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the bounds should be faded when behind other objects.
+	/// </param>
+	public static void DebugBounds(Bounds bounds, Color color, float duration = 0, bool depthTest = true)
+	{
+		Vector3 center = bounds.center;
+		
+		float x = bounds.extents.x;
+		float y = bounds.extents.y;
+		float z = bounds.extents.z;
+		
+		Vector3 ruf = center+new Vector3(x,y,z);
+		Vector3 rub = center+new Vector3(x,y,-z);
+		Vector3 luf = center+new Vector3(-x,y,z);
+		Vector3 lub = center+new Vector3(-x,y,-z);
+		
+		Vector3 rdf = center+new Vector3(x,-y,z);
+		Vector3 rdb = center+new Vector3(x,-y,-z);
+		Vector3 lfd = center+new Vector3(-x,-y,z);
+		Vector3 lbd = center+new Vector3(-x,-y,-z);
+		
+		Debug.DrawLine(ruf, luf, color, duration, depthTest);
+		Debug.DrawLine(ruf, rub, color, duration, depthTest);
+		Debug.DrawLine(luf, lub, color, duration, depthTest);
+		Debug.DrawLine(rub, lub, color, duration, depthTest);
+		
+		Debug.DrawLine(ruf, rdf, color, duration, depthTest);
+		Debug.DrawLine(rub, rdb, color, duration, depthTest);
+		Debug.DrawLine(luf, lfd, color, duration, depthTest);
+		Debug.DrawLine(lub, lbd, color, duration, depthTest);
+		
+		Debug.DrawLine(rdf, lfd, color, duration, depthTest);
+		Debug.DrawLine(rdf, rdb, color, duration, depthTest);
+		Debug.DrawLine(lfd, lbd, color, duration, depthTest);
+		Debug.DrawLine(lbd, rdb, color, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs an axis-aligned bounding box.
+	/// </summary>
+	/// <param name='bounds'>
+	/// 	- The bounds to debug.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the bounds.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the bounds should be faded when behind other objects.
+	/// </param>
+	public static void DebugBounds(Bounds bounds, float duration = 0, bool depthTest = true)
+	{
+		DebugBounds(bounds, Color.white, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a local cube.
+	/// </summary>
+	/// <param name='transform'>
+	/// 	- The transform that the cube will be local to.
+	/// </param>
+	/// <param name='size'>
+	/// 	- The size of the cube.
+	/// </param>
+	/// <param name='color'>
+	/// 	- Color of the cube.
+	/// </param>
+	/// <param name='center'>
+	/// 	- The position (relative to transform) where the cube will be debugged.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cube.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cube should be faded when behind other objects.
+	/// </param>
+	public static void DebugLocalCube(Transform transform, Vector3 size, Color color, Vector3 center = default(Vector3), float duration = 0, bool depthTest = true)
+	{
+		Vector3 lbb = transform.TransformPoint(center+((-size)*0.5f));
+		Vector3 rbb = transform.TransformPoint(center+(new Vector3(size.x, -size.y, -size.z)*0.5f));
+		
+		Vector3 lbf = transform.TransformPoint(center+(new Vector3(size.x, -size.y, size.z)*0.5f));
+		Vector3 rbf = transform.TransformPoint(center+(new Vector3(-size.x, -size.y, size.z)*0.5f));
+		
+		Vector3 lub = transform.TransformPoint(center+(new Vector3(-size.x, size.y, -size.z)*0.5f));
+		Vector3 rub = transform.TransformPoint(center+(new Vector3(size.x, size.y, -size.z)*0.5f));
+		
+		Vector3 luf = transform.TransformPoint(center+((size)*0.5f));
+		Vector3 ruf = transform.TransformPoint(center+(new Vector3(-size.x, size.y, size.z)*0.5f));
+		
+		Debug.DrawLine(lbb, rbb, color, duration, depthTest);
+		Debug.DrawLine(rbb, lbf, color, duration, depthTest);
+		Debug.DrawLine(lbf, rbf, color, duration, depthTest);
+		Debug.DrawLine(rbf, lbb, color, duration, depthTest);
+		
+		Debug.DrawLine(lub, rub, color, duration, depthTest);
+		Debug.DrawLine(rub, luf, color, duration, depthTest);
+		Debug.DrawLine(luf, ruf, color, duration, depthTest);
+		Debug.DrawLine(ruf, lub, color, duration, depthTest);
+		
+		Debug.DrawLine(lbb, lub, color, duration, depthTest);
+		Debug.DrawLine(rbb, rub, color, duration, depthTest);
+		Debug.DrawLine(lbf, luf, color, duration, depthTest);
+		Debug.DrawLine(rbf, ruf, color, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a local cube.
+	/// </summary>
+	/// <param name='transform'>
+	/// 	- The transform that the cube will be local to.
+	/// </param>
+	/// <param name='size'>
+	/// 	- The size of the cube.
+	/// </param>
+	/// <param name='center'>
+	/// 	- The position (relative to transform) where the cube will be debugged.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cube.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cube should be faded when behind other objects.
+	/// </param>
+	public static void DebugLocalCube(Transform transform, Vector3 size, Vector3 center = default(Vector3), float duration = 0, bool depthTest = true)
+	{
+		DebugLocalCube(transform, size, Color.white, center, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a local cube.
+	/// </summary>
+	/// <param name='space'>
+	/// 	- The space the cube will be local to.
+	/// </param>
+	/// <param name='size'>
+	///		- The size of the cube.
+	/// </param>
+	/// <param name='color'>
+	/// 	- Color of the cube.
+	/// </param>
+	/// <param name='center'>
+	/// 	- The position (relative to transform) where the cube will be debugged.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cube.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cube should be faded when behind other objects.
+	/// </param>
+	public static void DebugLocalCube(Matrix4x4 space, Vector3 size, Color color, Vector3 center = default(Vector3), float duration = 0, bool depthTest = true)
+	{	
+		color = (color == default(Color)) ? Color.white : color;
+		
+		Vector3 lbb = space.MultiplyPoint3x4(center+((-size)*0.5f));
+		Vector3 rbb = space.MultiplyPoint3x4(center+(new Vector3(size.x, -size.y, -size.z)*0.5f));
+		
+		Vector3 lbf = space.MultiplyPoint3x4(center+(new Vector3(size.x, -size.y, size.z)*0.5f));
+		Vector3 rbf = space.MultiplyPoint3x4(center+(new Vector3(-size.x, -size.y, size.z)*0.5f));
+		
+		Vector3 lub = space.MultiplyPoint3x4(center+(new Vector3(-size.x, size.y, -size.z)*0.5f));
+		Vector3 rub = space.MultiplyPoint3x4(center+(new Vector3(size.x, size.y, -size.z)*0.5f));
+		
+		Vector3 luf = space.MultiplyPoint3x4(center+((size)*0.5f));
+		Vector3 ruf = space.MultiplyPoint3x4(center+(new Vector3(-size.x, size.y, size.z)*0.5f));
+		
+		Debug.DrawLine(lbb, rbb, color, duration, depthTest);
+		Debug.DrawLine(rbb, lbf, color, duration, depthTest);
+		Debug.DrawLine(lbf, rbf, color, duration, depthTest);
+		Debug.DrawLine(rbf, lbb, color, duration, depthTest);
+		
+		Debug.DrawLine(lub, rub, color, duration, depthTest);
+		Debug.DrawLine(rub, luf, color, duration, depthTest);
+		Debug.DrawLine(luf, ruf, color, duration, depthTest);
+		Debug.DrawLine(ruf, lub, color, duration, depthTest);
+		
+		Debug.DrawLine(lbb, lub, color, duration, depthTest);
+		Debug.DrawLine(rbb, rub, color, duration, depthTest);
+		Debug.DrawLine(lbf, luf, color, duration, depthTest);
+		Debug.DrawLine(rbf, ruf, color, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a local cube.
+	/// </summary>
+	/// <param name='space'>
+	/// 	- The space the cube will be local to.
+	/// </param>
+	/// <param name='size'>
+	///		- The size of the cube.
+	/// </param>
+	/// <param name='center'>
+	/// 	- The position (relative to transform) where the cube will be debugged.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cube.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cube should be faded when behind other objects.
+	/// </param>
+	public static void DebugLocalCube(Matrix4x4 space, Vector3 size, Vector3 center = default(Vector3), float duration = 0, bool depthTest = true)
+	{
+		DebugLocalCube(space, size, Color.white, center, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a circle.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- Where the center of the circle will be positioned.
+	/// </param>
+	/// <param name='up'>
+	/// 	- The direction perpendicular to the surface of the circle.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the circle.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the circle.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the circle.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the circle should be faded when behind other objects.
+	/// </param>
+	public static void DebugCircle(Vector3 position, Vector3 up, Color color, float radius = 1.0f, float duration = 0, bool depthTest = true)
+	{
+		Vector3 _up = up.normalized * radius;
+		Vector3 _forward = Vector3.Slerp(_up, -_up, 0.5f);
+		Vector3 _right = Vector3.Cross(_up, _forward).normalized*radius;
+		
+		Matrix4x4 matrix = new Matrix4x4();
+		
+		matrix[0] = _right.x;
+		matrix[1] = _right.y;
+		matrix[2] = _right.z;
+		
+		matrix[4] = _up.x;
+		matrix[5] = _up.y;
+		matrix[6] = _up.z;
+		
+		matrix[8] = _forward.x;
+		matrix[9] = _forward.y;
+		matrix[10] = _forward.z;
+		
+		Vector3 _lastPoint = position + matrix.MultiplyPoint3x4(new Vector3(Mathf.Cos(0), 0, Mathf.Sin(0)));
+		Vector3 _nextPoint = Vector3.zero;
+		
+		color = (color == default(Color)) ? Color.white : color;
+		
+		for(var i = 0; i < 91; i++){
+			_nextPoint.x = Mathf.Cos((i*4)*Mathf.Deg2Rad);
+			_nextPoint.z = Mathf.Sin((i*4)*Mathf.Deg2Rad);
+			_nextPoint.y = 0;
+			
+			_nextPoint = position + matrix.MultiplyPoint3x4(_nextPoint);
+			
+			Debug.DrawLine(_lastPoint, _nextPoint, color, duration, depthTest);
+			_lastPoint = _nextPoint;
+		}
+	}
+	
+	/// <summary>
+	/// 	- Debugs a circle.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- Where the center of the circle will be positioned.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the circle.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the circle.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the circle.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the circle should be faded when behind other objects.
+	/// </param>
+	public static void DebugCircle(Vector3 position, Color color, float radius = 1.0f, float duration = 0, bool depthTest = true)
+	{
+		DebugCircle(position, Vector3.up, color, radius, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a circle.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- Where the center of the circle will be positioned.
+	/// </param>
+	/// <param name='up'>
+	/// 	- The direction perpendicular to the surface of the circle.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the circle.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the circle.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the circle should be faded when behind other objects.
+	/// </param>
+	public static void DebugCircle(Vector3 position, Vector3 up, float radius = 1.0f, float duration = 0, bool depthTest = true)
+	{
+		DebugCircle(position, up, Color.white, radius, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a circle.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- Where the center of the circle will be positioned.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the circle.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the circle.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the circle should be faded when behind other objects.
+	/// </param>
+	public static void DebugCircle(Vector3 position, float radius = 1.0f, float duration = 0, bool depthTest = true)
+	{
+		DebugCircle(position, Vector3.up, Color.white, radius, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a wire sphere.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position of the center of the sphere.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the sphere.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the sphere.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the sphere.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the sphere should be faded when behind other objects.
+	/// </param>
+	public static void DebugWireSphere(Vector3 position, Color color, float radius = 1.0f, float duration = 0, bool depthTest = true)
+	{
+		float angle = 10.0f;
+		
+		Vector3 x = new Vector3(position.x, position.y + radius * Mathf.Sin(0), position.z + radius * Mathf.Cos(0));
+		Vector3 y = new Vector3(position.x + radius * Mathf.Cos(0), position.y, position.z + radius * Mathf.Sin(0));
+		Vector3 z = new Vector3(position.x + radius * Mathf.Cos(0), position.y + radius * Mathf.Sin(0), position.z);
+		
+		Vector3 new_x;
+		Vector3 new_y;
+		Vector3 new_z;
+		
+		for(int i = 1; i < 37; i++){
+			
+			new_x = new Vector3(position.x, position.y + radius * Mathf.Sin(angle*i*Mathf.Deg2Rad), position.z + radius * Mathf.Cos(angle*i*Mathf.Deg2Rad));
+			new_y = new Vector3(position.x + radius * Mathf.Cos(angle*i*Mathf.Deg2Rad), position.y, position.z + radius * Mathf.Sin(angle*i*Mathf.Deg2Rad));
+			new_z = new Vector3(position.x + radius * Mathf.Cos(angle*i*Mathf.Deg2Rad), position.y + radius * Mathf.Sin(angle*i*Mathf.Deg2Rad), position.z);
+			
+			Debug.DrawLine(x, new_x, color, duration, depthTest);
+			Debug.DrawLine(y, new_y, color, duration, depthTest);
+			Debug.DrawLine(z, new_z, color, duration, depthTest);
+		
+			x = new_x;
+			y = new_y;
+			z = new_z;
+		}
+	}
+	
+	/// <summary>
+	/// 	- Debugs a wire sphere.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position of the center of the sphere.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the sphere.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the sphere.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the sphere should be faded when behind other objects.
+	/// </param>
+	public static void DebugWireSphere(Vector3 position, float radius = 1.0f, float duration = 0, bool depthTest = true)
+	{
+		DebugWireSphere(position, Color.white, radius, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a cylinder.
+	/// </summary>
+	/// <param name='start'>
+	/// 	- The position of one end of the cylinder.
+	/// </param>
+	/// <param name='end'>
+	/// 	- The position of the other end of the cylinder.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the cylinder.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the cylinder.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cylinder.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cylinder should be faded when behind other objects.
+	/// </param>
+	public static void DebugCylinder(Vector3 start, Vector3 end, Color color, float radius = 1, float duration = 0, bool depthTest = true)
+	{
+		Vector3 up = (end-start).normalized*radius;
+		Vector3 forward = Vector3.Slerp(up, -up, 0.5f);
+		Vector3 right = Vector3.Cross(up, forward).normalized*radius;
+		
+		//Radial circles
+		DebugExtension.DebugCircle(start, up, color, radius, duration, depthTest);	
+		DebugExtension.DebugCircle(end, -up, color, radius, duration, depthTest);
+		DebugExtension.DebugCircle((start+end)*0.5f, up, color, radius, duration, depthTest);
+		
+		//Side lines
+		Debug.DrawLine(start+right, end+right, color, duration, depthTest);
+		Debug.DrawLine(start-right, end-right, color, duration, depthTest);
+		
+		Debug.DrawLine(start+forward, end+forward, color, duration, depthTest);
+		Debug.DrawLine(start-forward, end-forward, color, duration, depthTest);
+		
+		//Start endcap
+		Debug.DrawLine(start-right, start+right, color, duration, depthTest);
+		Debug.DrawLine(start-forward, start+forward, color, duration, depthTest);
+		
+		//End endcap
+		Debug.DrawLine(end-right, end+right, color, duration, depthTest);
+		Debug.DrawLine(end-forward, end+forward, color, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a cylinder.
+	/// </summary>
+	/// <param name='start'>
+	/// 	- The position of one end of the cylinder.
+	/// </param>
+	/// <param name='end'>
+	/// 	- The position of the other end of the cylinder.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the cylinder.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cylinder.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cylinder should be faded when behind other objects.
+	/// </param>
+	public static void DebugCylinder(Vector3 start, Vector3 end, float radius = 1, float duration = 0, bool depthTest = true)
+	{
+		DebugCylinder(start, end, Color.white, radius, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a cone.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position for the tip of the cone.
+	/// </param>
+	/// <param name='direction'>
+	/// 	- The direction for the cone gets wider in.
+	/// </param>
+	/// <param name='angle'>
+	/// 	- The angle of the cone.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the cone.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cone.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cone should be faded when behind other objects.
+	/// </param>
+	public static void DebugCone(Vector3 position, Vector3 direction, Color color, float angle = 45, float duration = 0, bool depthTest = true)
+	{
+		float length = direction.magnitude;
+		
+		Vector3 _forward = direction;
+		Vector3 _up = Vector3.Slerp(_forward, -_forward, 0.5f);
+		Vector3 _right = Vector3.Cross(_forward, _up).normalized*length;
+		
+		direction = direction.normalized;
+		
+		Vector3 slerpedVector = Vector3.Slerp(_forward, _up, angle/90.0f);
+		
+		float dist;
+		var farPlane = new Plane(-direction, position+_forward);
+		var distRay = new Ray(position, slerpedVector);
+	
+		farPlane.Raycast(distRay, out dist); 
+		
+		Debug.DrawRay(position, slerpedVector.normalized*dist, color);
+		Debug.DrawRay(position, Vector3.Slerp(_forward, -_up, angle/90.0f).normalized*dist, color, duration, depthTest);
+		Debug.DrawRay(position, Vector3.Slerp(_forward, _right, angle/90.0f).normalized*dist, color, duration, depthTest);
+		Debug.DrawRay(position, Vector3.Slerp(_forward, -_right, angle/90.0f).normalized*dist, color, duration, depthTest);
+		
+		DebugExtension.DebugCircle(position+_forward, direction, color, (_forward-(slerpedVector.normalized*dist)).magnitude, duration, depthTest);
+		DebugExtension.DebugCircle(position+(_forward*0.5f), direction, color, ((_forward*0.5f)-(slerpedVector.normalized*(dist*0.5f))).magnitude, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a cone.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position for the tip of the cone.
+	/// </param>
+	/// <param name='direction'>
+	/// 	- The direction for the cone gets wider in.
+	/// </param>
+	/// <param name='angle'>
+	/// 	- The angle of the cone.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cone.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cone should be faded when behind other objects.
+	/// </param>
+	public static void DebugCone(Vector3 position, Vector3 direction, float angle = 45, float duration = 0, bool depthTest = true)
+	{
+		DebugCone(position, direction, Color.white, angle, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a cone.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position for the tip of the cone.
+	/// </param>
+	/// <param name='angle'>
+	/// 	- The angle of the cone.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the cone.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cone.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cone should be faded when behind other objects.
+	/// </param>
+	public static void DebugCone(Vector3 position, Color color, float angle = 45, float duration = 0, bool depthTest = true)
+	{
+		DebugCone(position, Vector3.up, color, angle, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a cone.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position for the tip of the cone.
+	/// </param>
+	/// <param name='angle'>
+	/// 	- The angle of the cone.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the cone.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the cone should be faded when behind other objects.
+	/// </param>
+	public static void DebugCone(Vector3 position, float angle = 45, float duration = 0, bool depthTest = true)
+	{
+		DebugCone(position, Vector3.up, Color.white, angle, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs an arrow.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The start position of the arrow.
+	/// </param>
+	/// <param name='direction'>
+	/// 	- The direction the arrow will point in.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the arrow.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the arrow.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the arrow should be faded when behind other objects. 
+	/// </param>
+	public static void DebugArrow(Vector3 position, Vector3 direction, Color color, float duration = 0, bool depthTest = true)
+	{
+		Debug.DrawRay(position, direction, color, duration, depthTest);
+		DebugExtension.DebugCone(position+direction, -direction*0.333f, color, 15, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs an arrow.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The start position of the arrow.
+	/// </param>
+	/// <param name='direction'>
+	/// 	- The direction the arrow will point in.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the arrow.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the arrow should be faded when behind other objects. 
+	/// </param>
+	public static void DebugArrow(Vector3 position, Vector3 direction, float duration = 0, bool depthTest = true)
+	{
+		DebugArrow(position, direction, Color.white, duration, depthTest);
+	}
+	
+	/// <summary>
+	/// 	- Debugs a capsule.
+	/// </summary>
+	/// <param name='start'>
+	/// 	- The position of one end of the capsule.
+	/// </param>
+	/// <param name='end'>
+	/// 	- The position of the other end of the capsule.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the capsule.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the capsule.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the capsule.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the capsule should be faded when behind other objects.
+	/// </param>
+	public static void DebugCapsule(Vector3 start, Vector3 end, Color color, float radius = 1, float duration = 0, bool depthTest = true)
+	{
+		Vector3 up = (end-start).normalized*radius;
+		Vector3 forward = Vector3.Slerp(up, -up, 0.5f);
+		Vector3 right = Vector3.Cross(up, forward).normalized*radius;
+		
+		float height = (start-end).magnitude;
+		float sideLength = Mathf.Max(0, (height*0.5f)-radius);
+		Vector3 middle = (end+start)*0.5f;
+		
+		start = middle+((start-middle).normalized*sideLength);
+		end = middle+((end-middle).normalized*sideLength);
+		
+		//Radial circles
+		DebugExtension.DebugCircle(start, up, color, radius, duration, depthTest);	
+		DebugExtension.DebugCircle(end, -up, color, radius, duration, depthTest);
+		
+		//Side lines
+		Debug.DrawLine(start+right, end+right, color, duration, depthTest);
+		Debug.DrawLine(start-right, end-right, color, duration, depthTest);
+		
+		Debug.DrawLine(start+forward, end+forward, color, duration, depthTest);
+		Debug.DrawLine(start-forward, end-forward, color, duration, depthTest);
+		
+		for(int i = 1; i < 26; i++){
+			
+			//Start endcap
+			Debug.DrawLine(Vector3.Slerp(right, -up, i/25.0f)+start, Vector3.Slerp(right, -up, (i-1)/25.0f)+start, color, duration, depthTest);
+			Debug.DrawLine(Vector3.Slerp(-right, -up, i/25.0f)+start, Vector3.Slerp(-right, -up, (i-1)/25.0f)+start, color, duration, depthTest);
+			Debug.DrawLine(Vector3.Slerp(forward, -up, i/25.0f)+start, Vector3.Slerp(forward, -up, (i-1)/25.0f)+start, color, duration, depthTest);
+			Debug.DrawLine(Vector3.Slerp(-forward, -up, i/25.0f)+start, Vector3.Slerp(-forward, -up, (i-1)/25.0f)+start, color, duration, depthTest);
+			
+			//End endcap
+			Debug.DrawLine(Vector3.Slerp(right, up, i/25.0f)+end, Vector3.Slerp(right, up, (i-1)/25.0f)+end, color, duration, depthTest);
+			Debug.DrawLine(Vector3.Slerp(-right, up, i/25.0f)+end, Vector3.Slerp(-right, up, (i-1)/25.0f)+end, color, duration, depthTest);
+			Debug.DrawLine(Vector3.Slerp(forward, up, i/25.0f)+end, Vector3.Slerp(forward, up, (i-1)/25.0f)+end, color, duration, depthTest);
+			Debug.DrawLine(Vector3.Slerp(-forward, up, i/25.0f)+end, Vector3.Slerp(-forward, up, (i-1)/25.0f)+end, color, duration, depthTest);
+		}
+	}
+	
+	/// <summary>
+	/// 	- Debugs a capsule.
+	/// </summary>
+	/// <param name='start'>
+	/// 	- The position of one end of the capsule.
+	/// </param>
+	/// <param name='end'>
+	/// 	- The position of the other end of the capsule.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the capsule.
+	/// </param>
+	/// <param name='duration'>
+	/// 	- How long to draw the capsule.
+	/// </param>
+	/// <param name='depthTest'>
+	/// 	- Whether or not the capsule should be faded when behind other objects.
+	/// </param>
+	public static void DebugCapsule(Vector3 start, Vector3 end, float radius = 1, float duration = 0, bool depthTest = true)
+	{
+		DebugCapsule(start, end, Color.white, radius, duration, depthTest);	
+	}
+	
+	#endregion
+	
+	#region GizmoDrawFunctions
+	
+	/// <summary>
+	/// 	- Draws a point.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The point to draw.
+	/// </param>
+	///  <param name='color'>
+	/// 	- The color of the drawn point.
+	/// </param>
+	/// <param name='scale'>
+	/// 	- The size of the drawn point.
+	/// </param>
+	public static void DrawPoint(Vector3 position, Color color, float duration = 2f, float scale = 1.0f)
+	{
+	
+		// Gizmos.DrawRay(position+(Vector3.up*(scale*0.5f)), -Vector3.up*scale);
+		// Gizmos.DrawRay(position+(Vector3.right*(scale*0.5f)), -Vector3.right*scale);
+		// Gizmos.DrawRay(position+(Vector3.forward*(scale*0.5f)), -Vector3.forward*scale);
+
+		Vector3 finalPosition = position;
+
+		Debug.DrawLine(finalPosition+(Vector3.up*(scale*0.5f)), finalPosition-(Vector3.up*(scale*0.5f)), color, 2f);
+		Debug.DrawLine(finalPosition+(Vector3.right*(scale*0.5f)), finalPosition-(Vector3.right*(scale*0.5f)), color, 2f);
+		Debug.DrawLine(finalPosition+(Vector3.forward*(scale*0.5f)), finalPosition-(Vector3.forward*(scale*0.5f)), color, 2f);
+
+	}
+	
+	/// <summary>
+	/// 	- Draws a point.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The point to draw.
+	/// </param>
+	/// <param name='scale'>
+	/// 	- The size of the drawn point.
+	/// </param>
+	public static void DrawPoint(Vector3 position, float scale = 1.0f)
+	{
+		DrawPoint(position, Color.white, scale);
+	}
+	
+	/// <summary>
+	/// 	- Draws an axis-aligned bounding box.
+	/// </summary>
+	/// <param name='bounds'>
+	/// 	- The bounds to draw.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the bounds.
+	/// </param>
+	public static void DrawBounds(Bounds bounds, Color color)
+	{
+		Vector3 center = bounds.center;
+		
+		float x = bounds.extents.x;
+		float y = bounds.extents.y;
+		float z = bounds.extents.z;
+		
+		Vector3 ruf = center+new Vector3(x,y,z);
+		Vector3 rub = center+new Vector3(x,y,-z);
+		Vector3 luf = center+new Vector3(-x,y,z);
+		Vector3 lub = center+new Vector3(-x,y,-z);
+		
+		Vector3 rdf = center+new Vector3(x,-y,z);
+		Vector3 rdb = center+new Vector3(x,-y,-z);
+		Vector3 lfd = center+new Vector3(-x,-y,z);
+		Vector3 lbd = center+new Vector3(-x,-y,-z);
+		
+		Color oldColor = Gizmos.color;
+		Gizmos.color = color;
+		
+		Gizmos.DrawLine(ruf, luf);
+		Gizmos.DrawLine(ruf, rub);
+		Gizmos.DrawLine(luf, lub);
+		Gizmos.DrawLine(rub, lub);
+		
+		Gizmos.DrawLine(ruf, rdf);
+		Gizmos.DrawLine(rub, rdb);
+		Gizmos.DrawLine(luf, lfd);
+		Gizmos.DrawLine(lub, lbd);
+		
+		Gizmos.DrawLine(rdf, lfd);
+		Gizmos.DrawLine(rdf, rdb);
+		Gizmos.DrawLine(lfd, lbd);
+		Gizmos.DrawLine(lbd, rdb);
+		
+		Gizmos.color = oldColor;
+	}
+	
+	/// <summary>
+	/// 	- Draws an axis-aligned bounding box.
+	/// </summary>
+	/// <param name='bounds'>
+	/// 	- The bounds to draw.
+	/// </param>
+	public static void DrawBounds(Bounds bounds)
+	{
+		DrawBounds(bounds, Color.white);
+	}
+	
+	/// <summary>
+	/// 	- Draws a local cube.
+	/// </summary>
+	/// <param name='transform'>
+	/// 	- The transform the cube will be local to.
+	/// </param>
+	/// <param name='size'>
+	/// 	- The local size of the cube.
+	/// </param>
+	/// <param name='center'>
+	///		- The local position of the cube.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the cube.
+	/// </param>
+	public static void DrawLocalCube(Transform transform, Vector3 size, Color color, Vector3 center = default(Vector3))
+	{
+		Color oldColor = Gizmos.color;
+		Gizmos.color = color;
+		
+		Vector3 lbb = transform.TransformPoint(center+((-size)*0.5f));
+		Vector3 rbb = transform.TransformPoint(center+(new Vector3(size.x, -size.y, -size.z)*0.5f));
+		
+		Vector3 lbf = transform.TransformPoint(center+(new Vector3(size.x, -size.y, size.z)*0.5f));
+		Vector3 rbf = transform.TransformPoint(center+(new Vector3(-size.x, -size.y, size.z)*0.5f));
+		
+		Vector3 lub = transform.TransformPoint(center+(new Vector3(-size.x, size.y, -size.z)*0.5f));
+		Vector3 rub = transform.TransformPoint(center+(new Vector3(size.x, size.y, -size.z)*0.5f));
+		
+		Vector3 luf = transform.TransformPoint(center+((size)*0.5f));
+		Vector3 ruf = transform.TransformPoint(center+(new Vector3(-size.x, size.y, size.z)*0.5f));
+		
+		Gizmos.DrawLine(lbb, rbb);
+		Gizmos.DrawLine(rbb, lbf);
+		Gizmos.DrawLine(lbf, rbf);
+		Gizmos.DrawLine(rbf, lbb);
+		
+		Gizmos.DrawLine(lub, rub);
+		Gizmos.DrawLine(rub, luf);
+		Gizmos.DrawLine(luf, ruf);
+		Gizmos.DrawLine(ruf, lub);
+		
+		Gizmos.DrawLine(lbb, lub);
+		Gizmos.DrawLine(rbb, rub);
+		Gizmos.DrawLine(lbf, luf);
+		Gizmos.DrawLine(rbf, ruf);
+		
+		Gizmos.color = oldColor;
+	}
+	
+	/// <summary>
+	/// 	- Draws a local cube.
+	/// </summary>
+	/// <param name='transform'>
+	/// 	- The transform the cube will be local to.
+	/// </param>
+	/// <param name='size'>
+	/// 	- The local size of the cube.
+	/// </param>
+	/// <param name='center'>
+	///		- The local position of the cube.
+	/// </param>	
+	public static void DrawLocalCube(Transform transform, Vector3 size, Vector3 center = default(Vector3))
+	{
+		DrawLocalCube(transform, size, Color.white, center);
+	}
+	
+	/// <summary>
+	/// 	- Draws a local cube.
+	/// </summary>
+	/// <param name='space'>
+	/// 	- The space the cube will be local to.
+	/// </param>
+	/// <param name='size'>
+	/// 	- The local size of the cube.
+	/// </param>
+	/// <param name='center'>
+	/// 	- The local position of the cube.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the cube.
+	/// </param>
+	public static void DrawLocalCube(Matrix4x4 space, Vector3 size, Color color, Vector3 center = default(Vector3))
+	{
+		Color oldColor = Gizmos.color;
+		Gizmos.color = color;
+		
+		Vector3 lbb = space.MultiplyPoint3x4(center+((-size)*0.5f));
+		Vector3 rbb = space.MultiplyPoint3x4(center+(new Vector3(size.x, -size.y, -size.z)*0.5f));
+		
+		Vector3 lbf = space.MultiplyPoint3x4(center+(new Vector3(size.x, -size.y, size.z)*0.5f));
+		Vector3 rbf = space.MultiplyPoint3x4(center+(new Vector3(-size.x, -size.y, size.z)*0.5f));
+		
+		Vector3 lub = space.MultiplyPoint3x4(center+(new Vector3(-size.x, size.y, -size.z)*0.5f));
+		Vector3 rub = space.MultiplyPoint3x4(center+(new Vector3(size.x, size.y, -size.z)*0.5f));
+		
+		Vector3 luf = space.MultiplyPoint3x4(center+((size)*0.5f));
+		Vector3 ruf = space.MultiplyPoint3x4(center+(new Vector3(-size.x, size.y, size.z)*0.5f));
+		
+		Gizmos.DrawLine(lbb, rbb);
+		Gizmos.DrawLine(rbb, lbf);
+		Gizmos.DrawLine(lbf, rbf);
+		Gizmos.DrawLine(rbf, lbb);
+		
+		Gizmos.DrawLine(lub, rub);
+		Gizmos.DrawLine(rub, luf);
+		Gizmos.DrawLine(luf, ruf);
+		Gizmos.DrawLine(ruf, lub);
+		
+		Gizmos.DrawLine(lbb, lub);
+		Gizmos.DrawLine(rbb, rub);
+		Gizmos.DrawLine(lbf, luf);
+		Gizmos.DrawLine(rbf, ruf);
+		
+		Gizmos.color = oldColor;
+	}
+	
+	/// <summary>
+	/// 	- Draws a local cube.
+	/// </summary>
+	/// <param name='space'>
+	/// 	- The space the cube will be local to.
+	/// </param>
+	/// <param name='size'>
+	/// 	- The local size of the cube.
+	/// </param>
+	/// <param name='center'>
+	/// 	- The local position of the cube.
+	/// </param>
+	public static void DrawLocalCube(Matrix4x4 space, Vector3 size, Vector3 center = default(Vector3))
+	{
+		DrawLocalCube(space, size, Color.white, center);
+	}
+	
+	/// <summary>
+	/// 	- Draws a circle.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- Where the center of the circle will be positioned.
+	/// </param>
+	/// <param name='up'>
+	/// 	- The direction perpendicular to the surface of the circle.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the circle.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the circle.
+	/// </param>
+	public static void DrawCircle(Vector3 position, Vector3 up, Color color, float radius = 1.0f)
+	{
+		up = ((up == Vector3.zero) ? Vector3.up : up).normalized * radius;
+		Vector3 _forward = Vector3.Slerp(up, -up, 0.5f);
+		Vector3 _right = Vector3.Cross(up, _forward).normalized*radius;
+		
+		Matrix4x4 matrix = new Matrix4x4();
+		
+		matrix[0] = _right.x;
+		matrix[1] = _right.y;
+		matrix[2] = _right.z;
+		
+		matrix[4] = up.x;
+		matrix[5] = up.y;
+		matrix[6] = up.z;
+		
+		matrix[8] = _forward.x;
+		matrix[9] = _forward.y;
+		matrix[10] = _forward.z;
+		
+		Vector3 _lastPoint = position + matrix.MultiplyPoint3x4(new Vector3(Mathf.Cos(0), 0, Mathf.Sin(0)));
+		Vector3 _nextPoint = Vector3.zero;
+		
+		Color oldColor = Gizmos.color;
+		Gizmos.color = (color == default(Color)) ? Color.white : color;
+		
+		for(var i = 0; i < 91; i++){
+			_nextPoint.x = Mathf.Cos((i*4)*Mathf.Deg2Rad);
+			_nextPoint.z = Mathf.Sin((i*4)*Mathf.Deg2Rad);
+			_nextPoint.y = 0;
+			
+			_nextPoint = position + matrix.MultiplyPoint3x4(_nextPoint);
+			
+			Gizmos.DrawLine(_lastPoint, _nextPoint);
+			_lastPoint = _nextPoint;
+		}
+		
+		Gizmos.color = oldColor;
+	}
+	
+	/// <summary>
+	/// 	- Draws a circle.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- Where the center of the circle will be positioned.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the circle.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the circle.
+	/// </param>
+	public static void DrawCircle(Vector3 position, Color color, float radius = 1.0f)
+	{
+		DrawCircle(position, Vector3.up, color, radius);
+	}
+	
+	/// <summary>
+	/// 	- Draws a circle.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- Where the center of the circle will be positioned.
+	/// </param>
+	/// <param name='up'>
+	/// 	- The direction perpendicular to the surface of the circle.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the circle.
+	/// </param>
+	public static void DrawCircle(Vector3 position, Vector3 up, float radius = 1.0f)
+	{
+		DrawCircle(position, position, Color.white, radius);
+	}
+	
+	/// <summary>
+	/// 	- Draws a circle.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- Where the center of the circle will be positioned.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the circle.
+	/// </param>
+	public static void DrawCircle(Vector3 position, float radius = 1.0f)
+	{
+		DrawCircle(position, Vector3.up, Color.white, radius);
+	}
+	
+	//Wiresphere already exists
+	
+	/// <summary>
+	/// 	- Draws a cylinder.
+	/// </summary>
+	/// <param name='start'>
+	/// 	- The position of one end of the cylinder.
+	/// </param>
+	/// <param name='end'>
+	/// 	- The position of the other end of the cylinder.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the cylinder.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the cylinder.
+	/// </param>
+	public static void DrawCylinder(Vector3 start, Vector3 end, Color color, float radius = 1.0f){
+		Vector3 up = (end-start).normalized*radius;
+		Vector3 forward = Vector3.Slerp(up, -up, 0.5f);
+		Vector3 right = Vector3.Cross(up, forward).normalized*radius;
+		
+		//Radial circles
+		DebugExtension.DrawCircle(start, up, color, radius);	
+		DebugExtension.DrawCircle(end, -up, color, radius);
+		DebugExtension.DrawCircle((start+end)*0.5f, up, color, radius);
+		
+		Color oldColor = Gizmos.color;
+		Gizmos.color = color;
+		
+		//Side lines
+		Gizmos.DrawLine(start+right, end+right);
+		Gizmos.DrawLine(start-right, end-right);
+		
+		Gizmos.DrawLine(start+forward, end+forward);
+		Gizmos.DrawLine(start-forward, end-forward);
+		
+		//Start endcap
+		Gizmos.DrawLine(start-right, start+right);
+		Gizmos.DrawLine(start-forward, start+forward);
+		
+		//End endcap
+		Gizmos.DrawLine(end-right, end+right);
+		Gizmos.DrawLine(end-forward, end+forward);
+		
+		Gizmos.color = oldColor;
+	}
+	
+	/// <summary>
+	/// 	- Draws a cylinder.
+	/// </summary>
+	/// <param name='start'>
+	/// 	- The position of one end of the cylinder.
+	/// </param>
+	/// <param name='end'>
+	/// 	- The position of the other end of the cylinder.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the cylinder.
+	/// </param>
+	public static void DrawCylinder(Vector3 start, Vector3 end, float radius = 1.0f)
+	{
+		DrawCylinder(start, end, Color.white, radius);
+	}
+	
+	/// <summary>
+	/// 	- Draws a cone.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position for the tip of the cone.
+	/// </param>
+	/// <param name='direction'>
+	/// 	- The direction for the cone to get wider in.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the cone.
+	/// </param>
+	/// <param name='angle'>
+	/// 	- The angle of the cone.
+	/// </param>
+	public static void DrawCone(Vector3 position, Vector3 direction, Color color, float angle = 45)
+	{
+		float length = direction.magnitude;
+		
+		Vector3 _forward = direction;
+		Vector3 _up = Vector3.Slerp(_forward, -_forward, 0.5f);
+		Vector3 _right = Vector3.Cross(_forward, _up).normalized*length;
+		
+		direction = direction.normalized;
+		
+		Vector3 slerpedVector = Vector3.Slerp(_forward, _up, angle/90.0f);
+		
+		float dist;
+		var farPlane = new Plane(-direction, position+_forward);
+		var distRay = new Ray(position, slerpedVector);
+	
+		farPlane.Raycast(distRay, out dist); 
+		
+		Color oldColor = Gizmos.color;
+		Gizmos.color = color;
+		
+		Gizmos.DrawRay(position, slerpedVector.normalized*dist);
+		Gizmos.DrawRay(position, Vector3.Slerp(_forward, -_up, angle/90.0f).normalized*dist);
+		Gizmos.DrawRay(position, Vector3.Slerp(_forward, _right, angle/90.0f).normalized*dist);
+		Gizmos.DrawRay(position, Vector3.Slerp(_forward, -_right, angle/90.0f).normalized*dist);
+		
+		DebugExtension.DrawCircle(position+_forward, direction, color, (_forward-(slerpedVector.normalized*dist)).magnitude);
+		DebugExtension.DrawCircle(position+(_forward*0.5f), direction, color, ((_forward*0.5f)-(slerpedVector.normalized*(dist*0.5f))).magnitude);
+		
+		Gizmos.color = oldColor;
+	}
+	
+	/// <summary>
+	/// 	- Draws a cone.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position for the tip of the cone.
+	/// </param>
+	/// <param name='direction'>
+	/// 	- The direction for the cone to get wider in.
+	/// </param>
+	/// <param name='angle'>
+	/// 	- The angle of the cone.
+	/// </param>
+	public static void DrawCone(Vector3 position, Vector3 direction, float angle = 45)
+	{
+		DrawCone(position, direction, Color.white, angle);
+	}
+	
+	/// <summary>
+	/// 	- Draws a cone.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position for the tip of the cone.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the cone.
+	/// </param>
+	/// <param name='angle'>
+	/// 	- The angle of the cone.
+	/// </param>
+	public static void DrawCone(Vector3 position, Color color, float angle = 45)
+	{
+		DrawCone(position, Vector3.up, color, angle);
+	}
+	
+	/// <summary>
+	/// 	- Draws a cone.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The position for the tip of the cone.
+	/// </param>
+	/// <param name='angle'>
+	/// 	- The angle of the cone.
+	/// </param>
+	public static void DrawCone(Vector3 position, float angle = 45)
+	{
+		DrawCone(position, Vector3.up, Color.white, angle);
+	}
+	
+	/// <summary>
+	/// 	- Draws an arrow.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The start position of the arrow.
+	/// </param>
+	/// <param name='direction'>
+	/// 	- The direction the arrow will point in.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the arrow.
+	/// </param>
+	public static void DrawArrow(Vector3 position, Vector3 direction, Color color)
+	{	
+		Color oldColor = Gizmos.color;
+		Gizmos.color = color;
+		
+		Gizmos.DrawRay(position, direction);
+		DebugExtension.DrawCone(position+direction, -direction*0.333f, color, 15);
+		
+		Gizmos.color = oldColor;
+	}
+	
+	/// <summary>
+	/// 	- Draws an arrow.
+	/// </summary>
+	/// <param name='position'>
+	/// 	- The start position of the arrow.
+	/// </param>
+	/// <param name='direction'>
+	/// 	- The direction the arrow will point in.
+	/// </param>
+	public static void DrawArrow(Vector3 position, Vector3 direction)
+	{
+		DrawArrow(position, direction, Color.white);
+	}
+	
+	/// <summary>
+	/// 	- Draws a capsule.
+	/// </summary>
+	/// <param name='start'>
+	/// 	- The position of one end of the capsule.
+	/// </param>
+	/// <param name='end'>
+	/// 	- The position of the other end of the capsule.
+	/// </param>
+	/// <param name='color'>
+	/// 	- The color of the capsule.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the capsule.
+	/// </param>
+	public static void DrawCapsule(Vector3 start, Vector3 end, Color color, float radius = 1)
+	{
+		Vector3 up = (end-start).normalized*radius;
+		Vector3 forward = Vector3.Slerp(up, -up, 0.5f);
+		Vector3 right = Vector3.Cross(up, forward).normalized*radius;
+		
+		Color oldColor = Gizmos.color;
+		Gizmos.color = color;
+		
+		float height = (start-end).magnitude;
+		float sideLength = Mathf.Max(0, (height*0.5f)-radius);
+		Vector3 middle = (end+start)*0.5f;
+		
+		start = middle+((start-middle).normalized*sideLength);
+		end = middle+((end-middle).normalized*sideLength);
+		
+		//Radial circles
+		DebugExtension.DrawCircle(start, up, color, radius);	
+		DebugExtension.DrawCircle(end, -up, color, radius);
+		
+		//Side lines
+		Gizmos.DrawLine(start+right, end+right);
+		Gizmos.DrawLine(start-right, end-right);
+		
+		Gizmos.DrawLine(start+forward, end+forward);
+		Gizmos.DrawLine(start-forward, end-forward);
+		
+		for(int i = 1; i < 26; i++){
+			
+			//Start endcap
+			Gizmos.DrawLine(Vector3.Slerp(right, -up, i/25.0f)+start, Vector3.Slerp(right, -up, (i-1)/25.0f)+start);
+			Gizmos.DrawLine(Vector3.Slerp(-right, -up, i/25.0f)+start, Vector3.Slerp(-right, -up, (i-1)/25.0f)+start);
+			Gizmos.DrawLine(Vector3.Slerp(forward, -up, i/25.0f)+start, Vector3.Slerp(forward, -up, (i-1)/25.0f)+start);
+			Gizmos.DrawLine(Vector3.Slerp(-forward, -up, i/25.0f)+start, Vector3.Slerp(-forward, -up, (i-1)/25.0f)+start);
+			
+			//End endcap
+			Gizmos.DrawLine(Vector3.Slerp(right, up, i/25.0f)+end, Vector3.Slerp(right, up, (i-1)/25.0f)+end);
+			Gizmos.DrawLine(Vector3.Slerp(-right, up, i/25.0f)+end, Vector3.Slerp(-right, up, (i-1)/25.0f)+end);
+			Gizmos.DrawLine(Vector3.Slerp(forward, up, i/25.0f)+end, Vector3.Slerp(forward, up, (i-1)/25.0f)+end);
+			Gizmos.DrawLine(Vector3.Slerp(-forward, up, i/25.0f)+end, Vector3.Slerp(-forward, up, (i-1)/25.0f)+end);
+		}
+		
+		Gizmos.color = oldColor;
+	}
+	
+	/// <summary>
+	/// 	- Draws a capsule.
+	/// </summary>
+	/// <param name='start'>
+	/// 	- The position of one end of the capsule.
+	/// </param>
+	/// <param name='end'>
+	/// 	- The position of the other end of the capsule.
+	/// </param>
+	/// <param name='radius'>
+	/// 	- The radius of the capsule.
+	/// </param>
+	public static void DrawCapsule(Vector3 start, Vector3 end, float radius = 1)
+	{
+		DrawCapsule(start, end, Color.white, radius);
+	}
+	
+	#endregion
+	
+	#region DebugFunctions
+	
+	/// <summary>
+	/// 	- Gets the methods of an object.
+	/// </summary>
+	/// <returns>
+	/// 	- A list of methods accessible from this object.
+	/// </returns>
+	/// <param name='obj'>
+	/// 	- The object to get the methods of.
+	/// </param>
+	/// <param name='includeInfo'>
+	/// 	- Whether or not to include each method's method info in the list.
+	/// </param>
+	public static string MethodsOfObject(System.Object obj, bool includeInfo = false){
+		string methods = "";
+		MethodInfo[] methodInfos = obj.GetType().GetMethods();
+		for(int i = 0; i < methodInfos.Length; i++){
+			if(includeInfo){
+				methods += methodInfos[i]+"\n";
+			}
+			
+			else{
+				methods += methodInfos[i].Name+"\n";
+			}
+		}
+		
+		return (methods);
+	}
+	
+	/// <summary>
+	/// 	- Gets the methods of a type.
+	/// </summary>
+	/// <returns>
+	/// 	- A list of methods accessible from this type.
+	/// </returns>
+	/// <param name='type'>
+	/// 	- The type to get the methods of.
+	/// </param>
+	/// <param name='includeInfo'>
+	/// 	- Whether or not to include each method's method info in the list.
+	/// </param>
+	public static string MethodsOfType(System.Type type, bool includeInfo = false){
+		string methods = "";
+		MethodInfo[] methodInfos = type.GetMethods();
+		for(var i = 0; i < methodInfos.Length; i++){
+			if(includeInfo){
+				methods += methodInfos[i]+"\n";
+			}
+			
+			else{
+				methods += methodInfos[i].Name+"\n";
+			}
+		}
+		
+		return (methods);
+	}
+	
+	#endregion
+}

DebugExtension.cs.meta

+fileFormatVersion: 2
+guid: 54785b64fd02b0e429cca9e59cdcb28b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

DirectorsDrawer.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+namespace GuidewireSim
+{
+/**
+ * This class represents each orientation by drawing all of its directors as arrows in each frame.
+ */
+public class DirectorsDrawer : MonoBehaviour
+{
+    SimulationLoop simulationLoop; //!< The component SimulationLoop.
+
+    [SerializeField] [Range(0.1f, 50f)] float scaleFactor; //!< The scale factor that gets multiplied to the length of the respective director.
+    [SerializeField] [Range(1f, 90f)] float arrowHeadAngle; //!< The angle spread of the arrow head.
+    [SerializeField] [Range(0.1f, 1f)] float arrowHeadPercentage; //!< The percentage of the length of the arrow that the arrow head covers.
+    Color directorOneColor = Color.green; //!< The color that the lines representing the first director are drawn with.
+    Color directorTwoColor = Color.blue; //!< The color that the lines representing the second director are drawn with.
+    Color directorThreeColor = Color.red; //!< The color that the lines representing the third director are drawn with.
+    Color[] directorColors = new Color[3] {Color.red, Color.green, Color.blue}; /**< The color that the lines representing the
+                                                                                 *   three directors are drawn with.
+                                                                                 *   @note The i-th director is drawn in the i-th Color.
+                                                                                 */
+
+    private void Awake()
+    {
+        simulationLoop = GetComponent<SimulationLoop>();
+        Assert.IsNotNull(simulationLoop);
+    }
+
+    void Update()
+    {
+        DrawDirectors(simulationLoop.cylinderPositions, simulationLoop.directors);
+    }
+
+    /**
+     * Draws the director basis of each orientation element as arrows.
+     * @param cylinderPositions The center of mass of each cylinder, i.e. the position of each orientation element.
+     * @param directors The orthonormal basis of each orientation element / cylinder, also called directors.
+     */
+    private void DrawDirectors(Vector3[] cylinderPositions, Vector3[][] directors)
+    {
+        // iterates over each orientation element
+        for (int cylinderIndex = 0; cylinderIndex < cylinderPositions.Length; cylinderIndex++)
+        {
+            Vector3 startPosition = cylinderPositions[cylinderIndex];
+
+            // iterates over each director
+            for (int directorIndex = 0; directorIndex < 3; directorIndex++)
+            {
+                Vector3 endPosition = startPosition + scaleFactor * directors[directorIndex][cylinderIndex];
+
+                // Draws a line between the cylinder's center of mass and the director's end position.
+                Debug.DrawLine(startPosition, endPosition, directorColors[directorIndex]);
+
+                Vector3[] arrowHeadPositions = CalculateArrowHeadPositions(startPosition, endPosition);                    
+                DrawArrowHeadLines(directorIndex, endPosition, arrowHeadPositions);
+                DrawArrowHeadConnectionLines(directorIndex, arrowHeadPositions);
+            }
+        }
+    }
+
+    /**
+     * Calculates the end position of each line of each arrow head. E.g. an arrow head consists of four lines, each of them starting at
+     * @p endPosition and spreading in different directions to form the shape of an arrow tip.
+     * @param startPosition The start position of the director, i.e. the position of the orientation.
+     * @param endPosition The position of the tip of the arrow head.
+     * @return The end positions of the four lines that form the arrow head.
+     * @req @p arrowHeadPositions has a length of 4.
+     */
+    private Vector3[] CalculateArrowHeadPositions(Vector3 startPosition, Vector3 endPosition)
+    {
+        Vector3[] arrowHeadPositions = new Vector3[4];
+        Vector3 direction = endPosition - startPosition;
+
+        arrowHeadPositions[0] = endPosition + Quaternion.LookRotation(direction) * Quaternion.Euler(-arrowHeadAngle, 0, 0) * Vector3.back * arrowHeadPercentage;
+        arrowHeadPositions[1] = endPosition + Quaternion.LookRotation(direction) * Quaternion.Euler(0, arrowHeadAngle, 0) * Vector3.back * arrowHeadPercentage;
+        arrowHeadPositions[2] = endPosition + Quaternion.LookRotation(direction) * Quaternion.Euler(arrowHeadAngle, 0, 0) * Vector3.back * arrowHeadPercentage;
+        arrowHeadPositions[3] = endPosition + Quaternion.LookRotation(direction) * Quaternion.Euler(0, -arrowHeadAngle, 0) * Vector3.back * arrowHeadPercentage;
+
+        Assert.IsTrue(arrowHeadPositions.Length == 4);
+
+        return arrowHeadPositions;
+    }
+
+    /**
+     * Draws the four lines that form the arrow head for the director that corresponds to @p directorIndex.
+     * @param directorIndex The index of the director under consideration.
+     * @param endPosition The position of the tip of the arrow head.
+     * @param arrowHeadPositions The end positions of the four lines that form the arrow head.
+     * @req @p arrowHeadPositions has a length of 4.
+     */
+    private void DrawArrowHeadLines(int directorIndex, Vector3 endPosition, Vector3[] arrowHeadPositions)
+    {
+        Assert.IsTrue(arrowHeadPositions.Length == 4);
+
+        for (int positionIndex = 0; positionIndex < arrowHeadPositions.Length; positionIndex++)
+        {
+            Debug.DrawLine(endPosition, arrowHeadPositions[positionIndex], directorColors[directorIndex]);
+        }
+    }
+
+    /**
+     * Draws the four lines that connect the arrow head tips with each other. E.g. draws the line from @p arrowHeadPositions 0 and @p arrowHeadPositions 1.
+     * @param directorIndex The index of the director under consideration.
+     * @param arrowHeadPositions The end positions of the four lines that form the arrow head.
+     */
+    private void DrawArrowHeadConnectionLines(int directorIndex, Vector3[] arrowHeadPositions)
+    {
+        Debug.DrawLine(arrowHeadPositions[0], arrowHeadPositions[1], directorColors[directorIndex]);
+        Debug.DrawLine(arrowHeadPositions[1], arrowHeadPositions[2], directorColors[directorIndex]);
+        Debug.DrawLine(arrowHeadPositions[2], arrowHeadPositions[3], directorColors[directorIndex]);
+        Debug.DrawLine(arrowHeadPositions[3], arrowHeadPositions[0], directorColors[directorIndex]);
+    }
+    }
+}
\ No newline at end of file

DirectorsDrawer.cs.meta

+fileFormatVersion: 2
+guid: 58982b2c5257b7d4baa434eefb1460fb
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

ForceTestPerformer.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+namespace GuidewireSim
+{
+/**
+ * This class enables the user to test the impact of external forces with one button within the Unity inspector.
+ */
+public class ForceTestPerformer : MonoBehaviour
+{
+    SimulationLoop simulationLoop; //!< The SimulationLoop component that executes all steps of the simulation loop.
+
+    [Tooltip("Applies gravity to all spheres.")]
+    [SerializeField] bool doForceTestOne = false; //!< Whether to run Force Test One. This test applies gravity to all spheres.
+    [Tooltip("Applies an external force to one end of the guidewire.")]
+    [SerializeField] bool doForceTestTwo = false; //!< Whether to run Force Test Two. This test applies an external force to one end of the guidewire.
+    [Tooltip("Applies an external force to one end of the guidewire for a fixed amount of time and then the opposite force"
+    + "at the same sphere for the same amount of time.")]
+    [SerializeField] bool doForceTestThree = false; /**< Whether to run Force Test Three. This test applies an external force to one end of the guidewire
+                                                     *   for a fixed amount of time and then the opposite force at the same sphere for the same amount of time.
+                                                     */
+    [Tooltip("Applies an external force to one end of the guidewire and the opposite force at the other end of the guidewire.")]
+    [SerializeField] bool doForceTestFour = false; /**< Whether to run Force Test Four. This test applies an external force to one end of the guidewire
+                                                    *   and the opposite force at the other end of the guidewire.
+                                                    */
+    [Tooltip("Shifts one end of the guidewire and runs the simulation for exactly one loop iteration to test constraint solving.")]
+    [SerializeField] bool doSingleLoopTest = false; /**< Whether to run the Single Loop Test. This test shifts one end of the guidewire and
+                                                     *   runs the simulation for exactly one loop iteration to test constraint solving.
+                                                     */
+    [Tooltip("External force that is applied in Force Test Three.")]
+    [SerializeField] Vector3 pullForceTestThree = new Vector3(0f, 3f, 0f); //!< External force that is applied in Force Test Three.
+
+    private void Awake()
+    {
+        simulationLoop = GetComponent<SimulationLoop>();
+        Assert.IsNotNull(simulationLoop);
+    }
+
+    private void Start()
+    {
+        PerformForceTests();
+    }
+
+    /**
+     * Performs each Force Test whose respective serialized boolean is set to true in the Unity inspector.
+     */
+    private void PerformForceTests()
+    {
+        if (doForceTestOne) PerformForceTestOne();
+        else if (doForceTestTwo) PerformForceTestTwo();
+        else if (doForceTestThree) StartCoroutine(PerformForceTestThree(pullForceTestThree));
+        else if (doForceTestFour) PerformForceTestFour();
+        else if (doSingleLoopTest) PerformSingleLoopTest();
+    }
+
+    /**
+     * Performs force test one. This test applies gravity to all spheres.
+     */
+    private void PerformForceTestOne()
+    {
+        Vector3 gravity = new Vector3(0f, -9.81f, 0f);
+
+        for (int sphereIndex = 0; sphereIndex < simulationLoop.SpheresCount; sphereIndex++)
+        {
+            simulationLoop.sphereExternalForces[sphereIndex] = gravity;
+        }
+    }
+
+    /**
+     * Performs force test two. This test applies an external force to one end of the guidewire.
+     */
+    private void PerformForceTestTwo()
+    {
+        Vector3 pullForce = new Vector3(0f, 0.3f, 0f);
+
+        for (int sphereIndex = 0; sphereIndex < (simulationLoop.SpheresCount - 1); sphereIndex++)
+        {
+            simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
+        }
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForce;
+    }
+
+    /**
+     * Performs force test three. This test applies an external force to one end of the guidewire
+     * for a fixed amount of time and then the opposite force at the same sphere for the same amount of time.
+     * @param applyForceTime For how many seconds to apply the force to the particles.
+     */
+    private IEnumerator PerformForceTestThree(Vector3 pullForce, float applyForceTime = 1f)
+    {
+        for (int sphereIndex = 0; sphereIndex < (simulationLoop.SpheresCount - 1); sphereIndex++)
+        {
+            simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
+        }
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForce;
+
+        yield return new WaitForSeconds(applyForceTime);
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = -pullForce;
+
+        yield return new WaitForSeconds(applyForceTime);
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = Vector3.zero;
+
+        Debug.Log("End of Force Test Three");
+    }
+
+    /**
+     * Performs force test four. This test applies an external force to one end of the guidewire
+     * and the opposite force at the other end of the guidewire.
+     */
+    private void PerformForceTestFour()
+    {
+        Vector3 pullForce = new Vector3(0f, 1f, 0f);
+
+        simulationLoop.sphereExternalForces[0] = -pullForce;
+
+        if (simulationLoop.SpheresCount > 2)
+        {
+            for (int sphereIndex = 1; sphereIndex < (simulationLoop.SpheresCount - 1); sphereIndex++)
+            {
+                simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
+            }
+        }
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForce;
+    }
+
+    /**
+     * Performs the single loop test. This test shifts one end of the guidewire and runs the simulation for exactly
+     * one loop iteration to test constraint solving.
+     * @note Position of particle one stays at (0, 0, 0), while the section particle shifts to about (10, 2, 0). Expected result is that
+     * both particles move a bit towards each other and reestablish a distance of 10 between them.
+     */
+    private void PerformSingleLoopTest()
+    {
+        Assert.IsTrue(simulationLoop.sphereVelocities.Length >= 2);
+
+        // ARRANGE
+        simulationLoop.ExecuteSingleLoopTest = true;
+        simulationLoop.ConstraintSolverSteps = 1000;
+        simulationLoop.sphereVelocities[simulationLoop.SpheresCount - 1] = new Vector3(0f, 100f, 0f);
+
+        Debug.Log("Executing Single Loop Test.");
+        Debug.Log("Constraint Solving Steps: " + simulationLoop.ConstraintSolverSteps);
+        Debug.Log("The last sphere is displaced by velocity " + simulationLoop.sphereVelocities[simulationLoop.SpheresCount - 1]);
+        Debug.Log("The distance between both spheres at rest state is "
+                  + Vector3.Distance(simulationLoop.spherePositions[0], simulationLoop.spherePositions[simulationLoop.SpheresCount - 1]));
+
+        // ACT
+        simulationLoop.PerformSimulationLoop();
+
+        // ASSERT
+        Debug.Log("Sphere Positions after Update Step: " + simulationLoop.spherePositions[0] + simulationLoop.spherePositions[simulationLoop.SpheresCount - 1]);
+        Debug.Log("The distance between both spheres after the update step is "
+                  + Vector3.Distance(simulationLoop.spherePositions[0], simulationLoop.spherePositions[simulationLoop.SpheresCount - 1]));
+    }
+}
+}
\ No newline at end of file

ForceTestPerformer.cs.meta

+fileFormatVersion: 2
+guid: 08d386284a38d8446a35fc059bdcdac7
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 900
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

GuidewireCreateManager.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using GuidewireSim;
+
+public class GuidewireCreateManager : MonoBehaviour
+{
+    private CreationScript creationScript;
+    private SimulationLoop simulationLoop;
+
+    public GameObject Simulation;
+    public int L_0 = 100;
+    private float REL;
+
+    private void Start()
+    {
+        // Find the SimulationLoop component from the Simulation GameObject
+        simulationLoop = Simulation.GetComponent<SimulationLoop>();
+
+        if (simulationLoop == null)
+        {
+            Debug.LogError("SimulationLoop component not found in the Simulation GameObject!");
+            return; // Exit if SimulationLoop is not found
+        }
+
+        // Get the Rod Element Length
+        REL = simulationLoop.GetRodElementLength();
+        int numberOfElements = (int)(L_0 / REL) + 1; // Calculate the desired number of elements
+
+        // Find the CreationScript component in the scene
+        creationScript = FindObjectOfType<CreationScript>();
+        if (creationScript != null)
+        {
+            creationScript.CreateGuidewire(numberOfElements);
+
+            // Get the created spheres and cylinders from the CreationScript
+            GameObject[] createdSpheres = creationScript.GetSpheres();
+            GameObject[] createdCylinders = creationScript.GetCylinders();
+
+            // Link them to the arrays in the SimulationLoop script
+            simulationLoop.SetSpheres(createdSpheres);
+            simulationLoop.SetCylinders(createdCylinders);
+        }
+        else
+        {
+            Debug.LogError("CreationScript component not found in the scene!");
+        }
+    }
+}
+

GuidewireCreateManager.cs.meta

+fileFormatVersion: 2
+guid: 55bd9da85a19af534bb6a2238637280b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

InitializationStep.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using UnityEngine.Assertions;
+using BSM = BulletSharp.Math;
+namespace GuidewireSim
+{
+    [RequireComponent(typeof(MathHelper))]
+    public class InitializationStep : MonoBehaviour
+    {
+        private SimulationLoop simulationLoop; // Declare simulationLoop
+        private float rodElementLength; // Declare rodElementLength
+
+        private CollisionHandler collisionHandler;
+        private MathHelper mathHelper;
+
+        [Range(1000f, 10000f)]
+        private float materialDensity = 7860;
+
+        [Range(0.0001f, 1f)]
+        private float materialRadius = 0.001f;
+
+        private void Awake()
+        {
+            simulationLoop = GetComponent<SimulationLoop>(); // Initialize simulationLoop
+            rodElementLength = simulationLoop.GetRodElementLength(); // Initialize rodElementLength
+
+            collisionHandler = GetComponent<CollisionHandler>();
+            Assert.IsNotNull(collisionHandler);
+
+            mathHelper = GetComponent<MathHelper>();
+            Assert.IsNotNull(mathHelper);
+        }
+
+    /**
+     * Initializes @p spherePositions with the positions of @p spheres at the start of the simulation.
+     * @param spheres All spheres that are part of the guidewire.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param[out] spherePositions The position at the current frame of each sphere.
+     * @req @p spheresCount should be at least one.
+     */
+    public void InitSpherePositions(GameObject[] spheres, int spheresCount, out Vector3[] spherePositions)
+    {
+        Assert.IsTrue(spheresCount >= 1);
+
+        spherePositions = new Vector3[spheresCount];
+
+        for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
+        {
+            spherePositions[sphereIndex] = spheres[sphereIndex].transform.position;
+        }
+    }
+
+    /**
+     * Initializes @p sphereVelocities with the default value of zero at the start of the simulation.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param[out] sphereVelocities The velocity of the current frame of each sphere.
+     * @note Velocities are set to zero at the start of the simulation.
+     */
+    public void InitSphereVelocities(int spheresCount, out Vector3[] sphereVelocities)
+    {
+        sphereVelocities = new Vector3[spheresCount];
+    }
+
+    /**
+     * Initializes @p sphereInverseMasses with the default value of one at the start of the simulation.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param[out] sphereInverseMasses The constant inverse masses  of each sphere.
+     */
+    public void InitSphereInverseMasses(int spheresCount, out float[] sphereInverseMasses)
+    {
+        sphereInverseMasses = new float[spheresCount];
+        float inverseMassValue = ((2500/rodElementLength)+1)/2; 
+
+        for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
+        {
+            sphereInverseMasses[sphereIndex] = inverseMassValue;
+        }
+    }
+
+    /**
+     * Initializes @p cylinderPositions as middle points of the positions of @p spheres at the start of the simulation.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param[out] cylinderPositions The position/ center of mass of each cylinder.
+     */
+    public void InitCylinderPositions(int cylinderCount, Vector3[] spherePositions, out Vector3[] cylinderPositions)
+    {
+        cylinderPositions = new Vector3[cylinderCount];
+
+        mathHelper.CalculateCylinderPositions(cylinderCount, spherePositions, cylinderPositions);
+    }
+
+    /**
+     * Initializes @p cylinderOrientations with the default value of (0f, 0f, 0f, 1f) which equals the quaternion identity
+     * at the start of the simulation.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientations.
+     * @param[out] cylinderOrientations The orientation of each cylinder at its center of mass.
+     */
+    public void InitCylinderOrientations(int cylinderCount, out BSM.Quaternion[] cylinderOrientations)
+    {
+        cylinderOrientations = new BSM.Quaternion[cylinderCount];
+
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            cylinderOrientations[cylinderIndex] = new BSM.Quaternion(0f, 0f, 0f, 1f);
+        }
+
+    }
+
+    /**
+     * Calculates the discrete darboux vector for each orientation pair (two adjacent orientations) at its rest configuration,
+     * i.e. at frame 0.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param cylinderOrientations The orientation of each cylinder at its center of mass.
+     * @param[out] discreteRestDarbouxVectors The array of all discrete Darboux Vectors at the rest configuration, i.e. at frame 0. Has (n-1) elements,
+     * if n is the number of orientations of the guidewire, because the darboux vector is taken of two adjacent orientations.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @req @p cylinderCount should be at least one.
+     * @req @p rodElementLength should be positive.
+     * @note All cylinder orientations must be computed for frame 0 first.
+     */
+    public void InitDiscreteRestDarbouxVectors(int cylinderCount, BSM.Quaternion[] cylinderOrientations, out Vector3[] discreteRestDarbouxVectors,
+                                               float rodElementLength)
+    {
+        Assert.IsTrue(cylinderCount >= 1);
+        Assert.IsTrue(rodElementLength > 0f);
+
+        discreteRestDarbouxVectors = new Vector3[cylinderCount - 1];
+
+        for (int darbouxIndex = 0; darbouxIndex < cylinderCount - 1; darbouxIndex++)
+        {
+            discreteRestDarbouxVectors[darbouxIndex] = mathHelper.DiscreteDarbouxVector(cylinderOrientations[darbouxIndex],
+                                                                                        cylinderOrientations[darbouxIndex + 1],
+                                                                                        rodElementLength);
+        }
+    }
+
+    /**
+     * Initializes @p cylinderAngularVelocities with the default value of zero at the start of the simulation.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param[out] cylinderAngularVelocities The angular velocity of the current frame of each orientation element/ cylinder.
+     */
+    public void InitCylinderAngularVelocities(int cylinderCount, out Vector3[] cylinderAngularVelocities)
+    {
+        cylinderAngularVelocities = new Vector3[cylinderCount];
+    }
+
+    /**
+     * Initializes @p cylinderScalarWeights with the default value of one at the start of the simulation.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param[out] cylinderScalarWeights The constant scalar weights of each orientation/ quaternion similar to @p sphereInverseMasses.
+     */
+    public void InitCylinderScalarWeights(int cylinderCount, out float[] cylinderScalarWeights)
+    {
+        cylinderScalarWeights = new float[cylinderCount];
+
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            cylinderScalarWeights[cylinderIndex] = 1f;
+        }
+    }
+
+    /**
+     * Initializes @p sphereExternalForces with the default value of zero at the start of the simulation.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param[out] sphereExternalForces The sum of all current external forces that are applied per particle/ sphere.
+     */
+    public void InitSphereExternalForces(int spheresCount, out Vector3[] sphereExternalForces)
+    {
+        sphereExternalForces = new Vector3[spheresCount];
+    }
+
+    /**
+     * Initializes @p spherePositionPredictions with the default value of zero at the start of the simulation.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param[out] spherePositionPredictions The prediction of the position at the current frame of each sphere.
+     */
+    public void InitSpherePositionPredictions(GameObject[] spheres, int spheresCount, out Vector3[] spherePositionPredictions)
+    {
+        spherePositionPredictions = new Vector3[spheresCount];
+
+        for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
+        {
+            spherePositionPredictions[sphereIndex] = spheres[sphereIndex].transform.position;
+        }
+    }
+
+    /**
+     * Initializes @p cylinderOrientationPredictions with the default value of (0f, 0f, 0f, 1f) which equals the quaternion identity
+     * at the start of the simulation.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param[out] cylinderOrientationPredictions The prediction of the orientation of each cylinder at its center of mass.
+     */
+    public void InitCylinderOrientationPredictions(int cylinderCount, out BSM.Quaternion[] cylinderOrientationPredictions)
+    {
+        cylinderOrientationPredictions = new BSM.Quaternion[cylinderCount];
+
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            cylinderOrientationPredictions[cylinderIndex] = new BSM.Quaternion(0f, 0f, 0f, 1f);
+        }
+    }
+
+    /**
+     * Initializes @p inertiaTensor so that all elements except the diagonal ones are zero. The first and second diagonal entry equal
+     * \f$ \rho * \pi * \frac{r^{2}}{4} \f$, and the third diagonal entry equals \f$ \rho * \pi * \frac{r^{2}}{2} \f$.
+     * @param[out] inertiaTensor The inertia tensor. Entries are approximates as in the CoRdE paper.
+     */
+    public void InitInertiaTensor(out float[,] inertiaTensor)
+    {
+        inertiaTensor = new float[3,3];
+        inertiaTensor[0,0] = inertiaTensor[1,1] = materialDensity * Mathf.PI * materialRadius * materialRadius / 4f;
+        inertiaTensor[2,2] = materialDensity * Mathf.PI * materialRadius * materialRadius / 2f;
+    }
+
+    /**
+     * Initializes @p inverseInertiaTensor as the inverse of @p inertiaTensor.
+     * @param inertiaTensor The inertia tensor. Entries are approximates as in the CoRdE paper.
+     * @param[out] inverseInertiaTensor The inverse of @p inertiaTensor.
+     */    
+    public void InitInverseInertiaTensor(float[,] inertiaTensor, out float[,] inverseInertiaTensor)
+    {
+        Assert.IsTrue(inertiaTensor[0,0] != 0);
+        Assert.IsTrue(inertiaTensor[1,1] != 0);
+        Assert.IsTrue(inertiaTensor[2,2] != 0);
+
+        inverseInertiaTensor = new float[3,3];
+        inverseInertiaTensor[0,0] = 1f / inertiaTensor[0,0];
+        inverseInertiaTensor[1,1] = 1f / inertiaTensor[1,1];
+        inverseInertiaTensor[2,2] = 1f / inertiaTensor[2,2];
+    }
+
+    /**
+        * Initializes @p cylinderExternalTorques with the default value of zero at the start of the simulation.
+        * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+        * @param[out] cylinderExternalTorques The sum of all current external torques that are applied per orientation element/ cylinder.
+        */
+    public void InitCylinderExternalTorques(int cylinderCount, out Vector3[] cylinderExternalTorques)
+    {
+        cylinderExternalTorques = new Vector3[cylinderCount];
+    }
+
+    /**
+     * Initializes the world space basis vectors (1, 0, 0), (0, 1, 0), (0, 0, 1) as embedded quaternions with scalar part zero.
+     * @param[out] worldSpaceBasis The three basis vectors of the world coordinate system.
+     */
+    public void InitWorldSpaceBasis(out BSM.Quaternion[] worldSpaceBasis)
+    {
+        worldSpaceBasis = new BSM.Quaternion[3];
+
+        worldSpaceBasis[0] = new BSM.Quaternion(1f, 0f, 0f, 0f);
+        worldSpaceBasis[1] = new BSM.Quaternion(0f, 1f, 0f, 0f);
+        worldSpaceBasis[2] = new BSM.Quaternion(0f, 0f, 1f, 0f);
+    }
+
+    /**
+     * Initializes the @p directors array of arrays. The zero-th array defines all first directors of each director basis and so on.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param worldSpaceBasis The three basis vectors of the world coordinate system.
+     * @param[out] directors The orthonormal basis of each orientation element / cylinder, also called directors.
+     * @note Example: The (i, j)th element holds the (i-1)th director of orientation element j.
+     */
+    public void InitDirectors(int cylinderCount, BSM.Quaternion[] worldSpaceBasis, out Vector3[][] directors)
+    {
+        directors = new Vector3[3][];
+        directors[0] = new Vector3[cylinderCount];
+        directors[1] = new Vector3[cylinderCount];
+        directors[2] = new Vector3[cylinderCount];
+
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            directors[0][cylinderIndex] = mathHelper.ImaginaryPart(worldSpaceBasis[0]);
+            directors[1][cylinderIndex] = mathHelper.ImaginaryPart(worldSpaceBasis[1]);
+            directors[2][cylinderIndex] = mathHelper.ImaginaryPart(worldSpaceBasis[2]);
+        }
+    }
+
+    public void InitSphereColliders(int spheresCount, GameObject[] spheres)
+    {
+        collisionHandler.sphereColliders = new SphereCollider[spheresCount];
+
+        for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
+        {
+            SphereCollider sphereCollider = spheres[sphereIndex].GetComponent<SphereCollider>();
+            Assert.IsNotNull(sphereCollider);
+
+            collisionHandler.sphereColliders[sphereIndex] = sphereCollider;
+        }
+    }
+}
+}

InitializationStep.cs.meta

+fileFormatVersion: 2
+guid: 51e2964dfbfea2a4a86aa0885301a80c
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

MathHelper.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+using BSM = BulletSharp.Math;
+
+namespace GuidewireSim
+{
+/**
+ * This class provides various helper methods for calculation.
+ */
+public class MathHelper : MonoBehaviour
+{
+    /**
+     * Calculates @p cylinderPositions as the middle points of two adjacent spheres.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param spherePositions The position at the current frame of each sphere.
+     * @param cylinderPositions The position/ center of mass of each cylinder.
+     * @note @p cylinderPositions is not marked as an out parameter, since @p cylinderPositions is not initialized in this method, but its values
+     * are changed.
+     */
+    public void CalculateCylinderPositions(int cylinderCount, Vector3[] spherePositions, Vector3[] cylinderPositions)
+    {
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            cylinderPositions[cylinderIndex] = (spherePositions[cylinderIndex] + spherePositions[cylinderIndex + 1]) / 2;
+        }
+    }
+
+    /**
+     * Calculates the multiplication of \f$ M x \f$, where \f$ M \f$ is the @p matrix, and \f$ x \f$ is the @p vector input.
+     * @param matrix The matrix to be multiplied with the vector.
+     * @param vector The vector to be multiplied with the matrix.
+     * @return The multiplication \f$ M x \f$.
+     * @req @p matrix must be a \f$ 3 \times 3 \f$ matrix.
+     */
+    public Vector3 MatrixVectorMultiplication(float[,] matrix, Vector3 vector)
+    {
+        // must be 3x3 matrix
+        Assert.IsTrue(matrix.GetLength(0) == 3);
+        Assert.IsTrue(matrix.GetLength(1) == 3);
+
+        Vector3 result = new Vector3();
+        for (int index = 0; index < 3; index++)
+        {
+            result[index] = matrix[index, 0] * vector[index] + matrix[index, 1] * vector[1] + matrix[index, 2] * vector[2];
+        }
+
+        return result;
+    }
+
+    /**
+     * Calculates the multiplication of \f$ x^{T} M \f$, where \f$ M \f$ is the @p matrix, and \f$ x^{T} \f$ is the @p columnVector input.
+     * @param matrix The matrix to be multiplied with the vector.
+     * @param vector The column vector \f$ x^{T} \f$ to be multiplied with the matrix.
+     * @return The multiplication \f$ x^{T} M \f$.
+     * @req @p matrix must be a \f$ 3 \times 3 \f$ matrix.
+     * @attention The input vector and the output vector are both column vectors.
+     */
+    public Vector3 ColumnVectorMatrixMultiplication(Vector3 columnVector, float[,] matrix)
+    {
+        // input must be 3x3 matrix
+        Assert.IsTrue(matrix.GetLength(0) == 3);
+        Assert.IsTrue(matrix.GetLength(1) == 3);
+
+        Vector3 result = new Vector3();
+        for (int index = 0; index < 3; index++)
+        {
+            result[index] = columnVector[0] * matrix[0, index] + columnVector[1] * matrix[1, index] + columnVector[2] * matrix[2, index];
+        }
+
+        return result;
+    }
+
+    /**
+     * Calculates the multiplication of \f$ x v^{T} \f$, where \f$ x \f$ is a (row) vector , and \f$ v^{T} \f$ is a column vector.
+     * @param vectorOne The (row) vector to be multiplied.
+     * @param columnVectorTwo The column vector \f$ x^{T} \f$ to be multiplied.
+     * @return The resulting \f$ 3 \times 3 \f$ matrix of the multiplication \f$ x v^{T} \f$.
+     */
+    public float[,] VectorColumnVectorMultiplication(Vector3 vectorOne, Vector3 columnVectorTwo)
+    {
+        float[,] result = new float[3,3];
+        for (int rowIndex = 0; rowIndex < 3; rowIndex++)
+        {
+            for (int columnIndex = 0; columnIndex < 3; columnIndex++)
+            {
+                result[rowIndex, columnIndex] = vectorOne[rowIndex] * columnVectorTwo[columnIndex];
+            }
+        }
+
+        return result;
+    }
+
+    /**
+     * Calculates the multiplication of \f$ a M \f$, where \f$ a \f$ is a scalar , and \f$ M \f$ is a \f$ 3 \times 3 \f$ matrix.
+     * @param scalar The scalar to be multiplied.
+     * @param matrix The matrix \f$ M \f$ to be multiplied.
+     * @return The resulting \f$ 3 \times 3 \f$ matrix of the multiplication \f$ a M \f$.
+     */
+    public float[,] ScalarMatrixMultiplication(float scalar, float[,] matrix)
+    {
+        // input must be 3x3 matrix
+        Assert.IsTrue(matrix.GetLength(0) == 3);
+        Assert.IsTrue(matrix.GetLength(1) == 3);
+
+        float[,] result = new float[3,3];
+        for (int rowIndex = 0; rowIndex < 3; rowIndex++)
+        {
+            for (int columnIndex = 0; columnIndex < 3; columnIndex++)
+            {
+                result[rowIndex, columnIndex] = scalar * matrix[rowIndex, columnIndex];
+            }
+        }
+
+        return result;
+    }
+
+    /**
+     * Calculates the inverse of a \f$ 3 \times 3 \f$ matrix \f$ M \f$.
+     * @param matrix The matrix to 
+     * @param columnVectorTwo The column vector \f$ x^{T} \f$ to be multiplied.
+     * @return The resulting \f$ 3 \times 3 \f$ matrix of the multiplication \f$ x v^{T} \f$.
+     */
+    public float[,] MatrixInverse(float[,] matrix)
+    {
+        // input must be 3x3 matrix
+        Assert.IsTrue(matrix.GetLength(0) == 3);
+        Assert.IsTrue(matrix.GetLength(1) == 3);
+
+        float determinant = matrix[0,0] * (matrix[2,2] * matrix[1,1] - matrix[2,1] * matrix[1,2])
+                            - matrix[1,0] * (matrix[2,2] * matrix[0,1] - matrix[2,1] * matrix[0,2])
+                            + matrix[2,0] * (matrix[1,2] * matrix[0,1] - matrix[1,1] * matrix[0,2]);
+        Debug.Log("determinant" + determinant);
+        float inverseDeterminant = 1f / determinant;
+
+        float[,] tempMatrix = new float[3,3];
+        tempMatrix[0,0] = matrix[2,2] * matrix[1,1] - matrix[2,1] * matrix[1,2];
+        tempMatrix[0,1] = -(matrix[2,2] * matrix[0,1] - matrix[2,1] * matrix[0,2]);
+        tempMatrix[0,2] = matrix[1,2] * matrix[0,1] - matrix[1,1] * matrix[0,2];
+        tempMatrix[1,0] = -(matrix[2,2] * matrix[1,0] - matrix[2,0] * matrix[1,2]);
+        tempMatrix[1,1] = matrix[2,2] * matrix[0,0] - matrix[2,0] * matrix[0,2];
+        tempMatrix[1,2] = -(matrix[1,2] * matrix[0,0] - matrix[1,0] * matrix[0,2]);
+        tempMatrix[2,0] = matrix[2,1] * matrix[1,0] - matrix[2,0] * matrix[1,1];
+        tempMatrix[2,1] = -(matrix[2,1] * matrix[0,0] - matrix[2,0] * matrix[0,1]);
+        tempMatrix[2,2] = matrix[1,1] * matrix[0,0] - matrix[1,0] * matrix[0,1];
+
+        float[,] result = new float[3,3];
+        result = ScalarMatrixMultiplication(inverseDeterminant, tempMatrix); 
+
+        return result;
+    }
+
+    /**
+     * Returns a quaternion that is the embedded vector with scalar part zero.
+     * @exampletext \f$ (x, y, z) \mapsto (x, y, z, 0) \f$.
+     * @param vector The vector to be embedded.
+     * @return The quaternion that is the embedded vector with scalar part zero.
+     */
+    public BSM.Quaternion EmbeddedVector(Vector3 vector)
+    {
+        return new BSM.Quaternion(vector.x, vector.y, vector.z, 0f);
+    }
+
+    /**
+     * Returns the imaginary part of a @p quaternion.
+     * @exampletext \f$ (x, y, z, w) \mapsto (x, y, z) \f$.
+     * @param quaternion The quaternion whose imaginary part to return.
+     * @return The imaginary part of a @p quaternion.
+     */
+    public Vector3 ImaginaryPart(BSM.Quaternion quaternion)
+    {
+        return new Vector3(quaternion.X, quaternion.Y, quaternion.Z);
+    }
+
+    /**
+     * Takes as input a BSM.Quaternion and returns a UnityEngine.Quaternion.
+     * @param bsmQuaternion The BSM.Quaternion to be converted.
+     * @return The converted UnityEngine.Quaternion.
+     */
+    public Quaternion QuaternionConversionFromBSM(BSM.Quaternion bsmQuaternion)
+    {
+        return new Quaternion(bsmQuaternion.X, bsmQuaternion.Y, bsmQuaternion.Z, bsmQuaternion.W);
+    }
+
+    /**
+     * Takes as input a UnityEngine.Quaternion and returns a BSM.Quaternion.
+     * @param bsmQuaternion The UnityEngine.Quaternion to be converted.
+     * @return The converted BSM.Quaternion.
+     */
+    public BSM.Quaternion QuaternionConversionToBSM(Quaternion quaternion)
+    {
+        return new BSM.Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
+    }
+
+    /**
+     * Calculates the discrete Darboux Vector of two adjacent orientations @p orientationOne, @p orientationTwo.
+     * @param orientationOne The orientation with the lower index, e.g. \f$ i \f$.
+     * @param orientationTwo The orientation with the higher index, e.g. \f$ i + 1 \f$.
+     * @param rodElementLength The distance between two spheres, also the distance between two orientations.
+     * @return The discrete Darboux Vector between @p orientationOne and @p orientationTwo.
+     * @note There is only cylinderCount - 1 many darboux vectors. The i-th Darboux Vector
+     * is between orientation i and orientation i+1.
+     * @attention The order in which the orientations are entered matters. The Darboux Vector of \f$ q_1, q_2 \f$ is not the same as the
+     * Darboux Vector of \f$ q_2, q_1 \f$.
+     */
+    public Vector3 DiscreteDarbouxVector(BSM.Quaternion orientationOne, BSM.Quaternion orientationTwo, float rodElementLength)
+    {
+        BSM.Quaternion tempQuaternion = BSM.Quaternion.Conjugate(orientationOne) * orientationTwo;
+        Vector3 tempVector = ImaginaryPart(tempQuaternion);
+        float factor = 2f / rodElementLength;
+
+        return factor * tempVector;
+    }
+
+    /**
+     * Calculates the sign factor of the current discrete Darboux Vector and the rest Darboux Vector of the same orientations.
+     * @note Check the Position and Orientation Based Cosserat Rods Paper (2016) for more information on the sign factor.
+     * @param currentDarbouxVector The discrete Darboux Vector of two fixed orientations at the current frame.
+     * @param restDarbouxVector The rest Darboux Vector of the same two orientations at frame 0.
+     * @return The Sign Factor between these two entities.
+     */
+    public float DarbouxSignFactor(Vector3 currentDarbouxVector, Vector3 restDarbouxVector)
+    {
+        float difference = SquaredNorm(currentDarbouxVector - restDarbouxVector);
+        float summation = SquaredNorm(currentDarbouxVector + restDarbouxVector);
+        
+        if (difference <= summation) return 1f;
+        else return -1f;
+    }
+
+    /**
+     * Returns the squared norm of a @p vector.
+     * @param vector The vector whose squared norm to return.
+     * @return The Squared norm of @p vector.
+     */
+    private float SquaredNorm(Vector3 vector)
+    {
+        return vector.x * vector.x + vector.y * vector.y + vector.z * vector.z;
+    }
+
+    /**
+     * Returns the vector length of @p vector, i.e. \f$ \sqrt{x_1^2 + x_2^2 + x_3^2} \f$ for a three-dimensional vector.
+     * @param vector The vector whose length to return.
+     * @return The vector length of @p vector.
+     */
+    public float VectorLength(Vector3 vector)
+    {
+        return Mathf.Sqrt(SquaredNorm(vector));
+    }
+
+    /**
+     * Returns the quaternion length of @p quaternion, i.e. \f$ \sqrt{x^2 + y^2 + z^2 + w^2} \f$.
+     * @param quaternion The quaternion whose length to return.
+     * @return The quaternion length of @p quaternion.
+     */
+    public float QuaternionLength(BSM.Quaternion quaternion)
+    {
+        float q0Squared = Mathf.Pow(quaternion.W, 2);
+        Vector3 imaginaryPart = ImaginaryPart(quaternion);
+        float qTq = SquaredNorm(imaginaryPart);
+        float result = Mathf.Sqrt(q0Squared + qTq);
+
+        return result;
+    }
+
+    /**
+     * Returns the deviation between the actual distance of @p particlePositionOne and @p particlePositionTwo (current Rod Element Length) and
+     * the @p defaultRodElementLength.
+     * @param particlePositionOne The first particle under consideration for the rod element length.
+     * @param particlePositionTwo The first particle under consideration for the rod element length.
+     * @param defaultRodElementLength The rod element length at rest state (i.e. frame 0) between these two particles.
+     * @return The deviation between the actual rod element length and the default rod element length.
+     */
+    public float RodElementLengthDeviation(Vector3 particlePositionOne, Vector3 particlePositionTwo, float defaultRodElementLength)
+    {
+        float currentRodElementLength = RodElementLength(particlePositionOne, particlePositionTwo);
+        float deviation = Mathf.Abs(currentRodElementLength - defaultRodElementLength);
+
+        return deviation;
+    }
+
+    /**
+     * Returns the deviation of the stretch constraint from zero.
+     * @param particlePositionOne \f$ p_1 \f$ of the equation (31).
+     * @param particlePositionTwo \f$ p_2 \f$ of the equation (31).
+     * @param orientation \f$ q \f$ of the equation (31).
+     * @param e_3 \f$ e_3 \f$ of the equation (31).
+     * @param rodElementLength \f$ l \f$ of the equation (31).
+     * @param logIntermediateResults Whether to output several logs that contain intermediate results of the calculation. Default is false.
+     * @return The Deviation of the calculated stretch constraint and zero.
+     * @note Check the Position and Orientation Based Cosserat Rods Paper (2016), equation (31), for more information on the stretch constraint.
+     */
+    public float StretchConstraintDeviation(Vector3 particlePositionOne, Vector3 particlePositionTwo, BSM.Quaternion orientation,
+                                            BSM.Quaternion e_3, float rodElementLength, bool logIntermediateResults = false)
+    {
+        Vector3 firstTerm = (1f / rodElementLength) * (particlePositionTwo - particlePositionOne);
+        Vector3 secondTerm = ImaginaryPart(orientation * e_3 * BSM.Quaternion.Conjugate(orientation));
+
+        if (logIntermediateResults)
+        {
+            float normalizedDotProduct = Vector3.Dot(firstTerm, secondTerm) / (VectorLength(firstTerm) * VectorLength(firstTerm));
+            float angle = Mathf.Acos(normalizedDotProduct);
+
+            Debug.Log("Length of first term of C_s: " + VectorLength(firstTerm));
+            Debug.Log("Length of second term of C_s: " + VectorLength(secondTerm));
+            Debug.Log("Angle between first and second term of C_s: " + angle);
+
+        }
+
+        Vector3 constraintResult = firstTerm - secondTerm;
+
+        float deviation = Vector3.Distance(constraintResult, Vector3.zero);
+
+        return deviation;
+    }
+
+    /**
+     * Returns the deviation of the bend twist constraint from zero.
+     * @param orientationOne \f$ q \f$ of the equation (32).
+     * @param orientationTwo \f$ u \f$ of the equation (32).
+     * @param rodElementLength The Rod Element Length between @p orientationOne and @p orientationTwo.
+     * Used to calculate  \f$ \mathbb{\Omega} \f$ of the equation (32).
+     * @param discreteRestDarbouxVector \f$ \mathbb{\Omega}^0 \f$ of the equation (32).
+     * @param logIntermediateResults Whether to output several logs that contain intermediate results of the calculation. Default is false.
+     * @return The Deviation of the calculated bend twist constraint and zero.
+     * @note Check the Position and Orientation Based Cosserat Rods Paper (2016), equation (32), for more information on the bend twist constraint.
+     */
+    public float BendTwistConstraintDeviation(BSM.Quaternion orientationOne, BSM.Quaternion orientationTwo, float rodElementLength,
+                                              Vector3 discreteRestDarbouxVector, bool logIntermediateResults = false)
+    {
+        Vector3 discreteDarbouxVector = DiscreteDarbouxVector(orientationOne, orientationTwo, rodElementLength);
+        float darbouxSignFactor = DarbouxSignFactor(discreteDarbouxVector, discreteRestDarbouxVector);
+
+        if (logIntermediateResults)
+        {
+            Debug.Log("Discrete Darboux Vector: " + discreteDarbouxVector);
+            Debug.Log("Discrete Rest Darboux Vector: " + discreteRestDarbouxVector);
+            Debug.Log("Darboux Sign Factor: " + darbouxSignFactor);
+        }
+
+        Vector3 constraintResult = discreteDarbouxVector - darbouxSignFactor * discreteRestDarbouxVector;
+
+        float deviation = Vector3.Distance(constraintResult, Vector3.zero);
+
+        return deviation;
+    }
+
+    /**
+     * Calculates the rod element length between @p particlePositionOne and @p particlePositionTwo.
+     * @param particlePositionOne The first particle of the rod element under consideration.
+     * @param particlePositionTwo The first particle of the rod element under consideration.
+     * @return The rod element length.
+     */
+    public float RodElementLength(Vector3 particlePositionOne, Vector3 particlePositionTwo)
+    {
+        return Vector3.Distance(particlePositionOne, particlePositionTwo);
+    }
+
+    /**
+     * Updates all directors of each orientation at the update step of the simulation loop.
+     * @exampletext The directors \f$ d_1, d_2, d_3 \f$ are calculated as \f$ d_i = q \cdot e_i \cdot \bar{q} \f$ for each orientation \f$ q\f$,
+     * where \f$ e_i \f$ is the i-th world space basis vector. In quaternion calculus, this means rotating \f$ e_i \f$ by \f$ q \f$.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param cylinderOrientations The orientation of each cylinder at its center of mass.
+     * @param directors The orthonormal basis of each orientation element / cylinder, also called directors.
+     * @param worldSpaceBasis The three basis vectors of the world coordinate system.
+     * @return All directors of each orientation.
+     */
+    public Vector3[][] UpdateDirectors(int cylinderCount, BSM.Quaternion[] cylinderOrientations, Vector3[][] directors, BSM.Quaternion[] worldSpaceBasis)
+    {
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            for (int axisIndex = 0; axisIndex < 3; axisIndex++)
+            {
+                directors[axisIndex][cylinderIndex] = ImaginaryPart(cylinderOrientations[cylinderIndex] * worldSpaceBasis[axisIndex] * BSM.Quaternion.Conjugate(cylinderOrientations[cylinderIndex]));
+            }
+        }
+
+        return directors;
+    }
+
+    /**
+     * Provides a random unit quaternion drawn from a gaussian distribution.
+     * @return A random unit quaternion drawn from a gaussian distribution.
+     * @note This works by drawing four random, gaussian distributed, numbers, and filling the components of the quaternion with these numbers.
+     * Mathematically, this is equal to drawing a quaternion from a gaussian distribution in \f$ \mathcal{R}^{4} \f$, since the joint distribution
+     * of gaussian samples is again gaussian.
+     * @req The length of the drawn quaternion is approximately equal to one.
+     */
+    public BSM.Quaternion RandomUnitQuaternion()
+    {
+        float x = GetGaussianRandomNumber();
+        float y = GetGaussianRandomNumber();
+        float z = GetGaussianRandomNumber();
+        float w = GetGaussianRandomNumber();
+
+        BSM.Quaternion quaternion = new BSM.Quaternion(x, y, z, w);
+
+        BSM.Quaternion unitQuaternion = BSM.Quaternion.Normalize(quaternion);
+
+        Assert.AreApproximatelyEqual(1f, QuaternionLength(unitQuaternion), tolerance: 0.01f);
+
+        return unitQuaternion;
+    }
+
+    /**
+     * Provides a sample from \f$ \mathcal{N}(0,1)\f$ by using the Marsaglia polar method to transform a uniform distribution to a normal distribution.
+     * @return A sample from \f$ \mathcal{N}(0,1)\f$.
+     * @note To understand this method, google the Marsaglia polar method. Note that unity does not provide a function to
+     * generate a random number following a gaussian distribution.
+     */
+    public float GetGaussianRandomNumber()
+    {
+        float v1, v2, s;
+
+        do
+        {
+            v1 = 2.0f * Random.Range(0f,1f) - 1.0f;
+            v2 = 2.0f * Random.Range(0f,1f) - 1.0f;
+            s = v1 * v1 + v2 * v2;
+        } while (s >= 1.0f || s == 0f);
+
+        s = Mathf.Sqrt((-2.0f * Mathf.Log(s)) / s);
+    
+        return v1 * s;
+    }
+}
+}
\ No newline at end of file

MathHelper.cs.meta

+fileFormatVersion: 2
+guid: 247d58bf6cc854e409f370d46a6646f4
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport.meta

+fileFormatVersion: 2
+guid: 9a007ccd78fa34e7d8072ff3761a6868
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/CharWordReader.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using System.IO;
+using System.Text;
+
+namespace Dummiesman {
+	public class CharWordReader {
+		public char[] word;
+		public int wordSize;
+		public bool endReached;
+
+		private StreamReader reader;
+		private int bufferSize;
+		private char[] buffer;
+		
+		public char currentChar;
+		private int currentPosition = 0;
+		private int maxPosition = 0;
+
+		public CharWordReader(StreamReader reader, int bufferSize) {
+			this.reader = reader;
+			this.bufferSize = bufferSize;
+
+			this.buffer = new char[this.bufferSize];
+			this.word = new char[this.bufferSize];
+
+			this.MoveNext();
+		}
+
+		public void SkipWhitespaces() {
+			while (char.IsWhiteSpace(this.currentChar)) {
+				this.MoveNext();
+			}
+		}
+
+		public void SkipWhitespaces(out bool newLinePassed) {
+			newLinePassed = false;
+			while (char.IsWhiteSpace(this.currentChar)) {
+				if (this.currentChar == '\r' || this.currentChar == '\n') {
+					newLinePassed = true;
+				}
+				this.MoveNext();
+			}
+		}
+
+		public void SkipUntilNewLine() {
+			while (this.currentChar != char.MinValue && this.currentChar != '\n' && this.currentChar != '\r') {
+				this.MoveNext();
+			}
+			this.SkipNewLineSymbols();
+		}
+
+		public void ReadUntilWhiteSpace() {
+			this.wordSize = 0;
+			while (this.currentChar != char.MinValue && char.IsWhiteSpace(this.currentChar) == false) {
+				this.word[this.wordSize] = this.currentChar;
+				this.wordSize++;
+				this.MoveNext();
+			}
+		}
+
+		public void ReadUntilNewLine() {
+			this.wordSize = 0;
+			while (this.currentChar != char.MinValue && this.currentChar != '\n' && this.currentChar != '\r') {
+				this.word[this.wordSize] = this.currentChar;
+				this.wordSize++;
+				this.MoveNext();
+			}
+			this.SkipNewLineSymbols();
+		}
+
+		public bool Is(string other) {
+			if (other.Length != this.wordSize) {
+				return false;
+			}
+
+			for (int i=0; i<this.wordSize; i++) {
+				if (this.word[i] != other[i]) {
+					return false;
+				}
+			}
+
+			return true;
+		}
+        public string GetString(int startIndex = 0) {
+            if (startIndex >= this.wordSize - 1) {
+                return string.Empty;
+            }
+            return new string(this.word, startIndex, this.wordSize - startIndex);
+        }
+		
+		public Vector3 ReadVector() {
+			this.SkipWhitespaces();
+			float x = this.ReadFloat();
+			this.SkipWhitespaces();
+			float y = this.ReadFloat();
+			this.SkipWhitespaces(out var newLinePassed);
+			float z = 0f;
+			if (newLinePassed == false) {
+				z = this.ReadFloat();
+			}
+			return new Vector3(x, y, z);
+		}
+
+		public int ReadInt() {
+			int result = 0;
+			bool isNegative = this.currentChar == '-';
+			if (isNegative == true) {
+				this.MoveNext();
+			}
+			
+			while (this.currentChar >= '0' && this.currentChar <= '9') {
+				var digit = this.currentChar - '0';
+				result = result * 10 + digit;
+				this.MoveNext();
+			}
+
+			return (isNegative == true) ? -result : result;
+		}
+
+		public float ReadFloat() {
+			bool isNegative = this.currentChar == '-';
+			if (isNegative) {
+				this.MoveNext();
+			}
+
+			var num = (float)this.ReadInt();
+			if (this.currentChar == '.' || this.currentChar == ',') {
+				this.MoveNext();
+				num +=  this.ReadFloatEnd();
+
+				if (this.currentChar == 'e' || this.currentChar == 'E') {
+					this.MoveNext();
+					var exp = this.ReadInt();
+					num = num * Mathf.Pow(10f, exp);
+				}
+			}
+			if (isNegative == true) {
+				num = -num;
+			}
+
+			return num;
+		}
+
+		private float ReadFloatEnd() {
+			float result = 0f;
+
+			var exp = 0.1f;
+			while (this.currentChar >= '0' && this.currentChar <= '9') {
+				var digit = this.currentChar - '0';
+				result += digit * exp;
+
+				exp *= 0.1f;
+
+				this.MoveNext();
+			}
+
+			return result;
+		}
+
+		private void SkipNewLineSymbols() {
+			while (this.currentChar == '\n' || this.currentChar == '\r') {
+				this.MoveNext();
+			}
+		}
+
+		public void MoveNext() {
+			this.currentPosition++;
+			if (this.currentPosition >= this.maxPosition) {
+				if (this.reader.EndOfStream == true) {
+					this.currentChar = char.MinValue;
+					this.endReached = true;
+					return;
+				}
+
+				this.currentPosition = 0;
+				this.maxPosition = this.reader.Read(this.buffer, 0, this.bufferSize);
+			}
+			this.currentChar = this.buffer[this.currentPosition];
+		}
+	}
+}
\ No newline at end of file

OBJImport/CharWordReader.cs.meta

+fileFormatVersion: 2
+guid: 37ed5e34a98d98669ac1a63bf547afa3
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/MTLLoader.cs

+/*
+ * Copyright (c) 2019 Dummiesman
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+*/
+
+using Dummiesman;
+using System.Collections.Generic;
+using System.IO;
+using UnityEngine;
+
+public class MTLLoader {
+    public List<string> SearchPaths = new List<string>() { "%FileName%_Textures", string.Empty};
+
+    private FileInfo _objFileInfo = null;
+
+    /// <summary>
+    /// The texture loading function. Overridable for stream loading purposes.
+    /// </summary>
+    /// <param name="path">The path supplied by the OBJ file, converted to OS path seperation</param>
+    /// <param name="isNormalMap">Whether the loader is requesting we convert this into a normal map</param>
+    /// <returns>Texture2D if found, or NULL if missing</returns>
+    public virtual Texture2D TextureLoadFunction(string path, bool isNormalMap)
+    {
+        //find it
+        foreach (var searchPath in SearchPaths)
+        {
+            //replace varaibles and combine path
+            string processedPath = (_objFileInfo != null) ? searchPath.Replace("%FileName%", Path.GetFileNameWithoutExtension(_objFileInfo.Name)) 
+                                                          : searchPath;
+            string filePath = Path.Combine(processedPath, path);
+
+            //return if eists
+            if (File.Exists(filePath))
+            {
+                var tex = ImageLoader.LoadTexture(filePath);
+
+                if(isNormalMap)
+                    tex = ImageUtils.ConvertToNormalMap(tex);
+
+                return tex;
+            }
+        }
+
+        //not found
+        return null;
+    }
+
+    private Texture2D TryLoadTexture(string texturePath, bool normalMap = false)
+    {
+        //swap directory seperator char
+        texturePath = texturePath.Replace('\\', Path.DirectorySeparatorChar);
+        texturePath = texturePath.Replace('/', Path.DirectorySeparatorChar);
+
+        return TextureLoadFunction(texturePath, normalMap);
+    }
+    
+    private int GetArgValueCount(string arg)
+    {
+        switch (arg)
+        {
+            case "-bm":
+            case "-clamp":
+            case "-blendu":
+            case "-blendv":
+            case "-imfchan":
+            case "-texres":
+                return 1;
+            case "-mm":
+                return 2;
+            case "-o":
+            case "-s":
+            case "-t":
+                return 3;
+        }
+        return -1;
+    }
+
+    private int GetTexNameIndex(string[] components)
+    {
+        for(int i=1; i < components.Length; i++)
+        {
+            var cmpSkip = GetArgValueCount(components[i]);
+            if(cmpSkip < 0)
+            {
+                return i;
+            }
+            i += cmpSkip;
+        }
+        return -1;
+    }
+
+    private float GetArgValue(string[] components, string arg, float fallback = 1f)
+    {
+        string argLower = arg.ToLower();
+        for(int i=1; i < components.Length - 1; i++)
+        {
+            var cmp = components[i].ToLower();
+            if(argLower == cmp)
+            {
+                return OBJLoaderHelper.FastFloatParse(components[i+1]);
+            }
+        }
+        return fallback;
+    }
+
+    private string GetTexPathFromMapStatement(string processedLine, string[] splitLine)
+    {
+        int texNameCmpIdx = GetTexNameIndex(splitLine);
+        if(texNameCmpIdx < 0)
+        {
+            Debug.LogError($"texNameCmpIdx < 0 on line {processedLine}. Texture not loaded.");
+            return null;
+        }
+
+        int texNameIdx = processedLine.IndexOf(splitLine[texNameCmpIdx]);
+        string texturePath = processedLine.Substring(texNameIdx);
+
+        return texturePath;
+    }
+
+    /// <summary>
+    /// Loads a *.mtl file
+    /// </summary>
+    /// <param name="input">The input stream from the MTL file</param>
+    /// <returns>Dictionary containing loaded materials</returns>
+    public Dictionary<string, Material> Load(Stream input)
+    {
+        var inputReader = new StreamReader(input);
+        var reader = new StringReader(inputReader.ReadToEnd());
+
+        Dictionary<string, Material> mtlDict = new Dictionary<string, Material>();
+        Material currentMaterial = null;
+
+        for (string line = reader.ReadLine(); line != null; line = reader.ReadLine())
+        {
+            if (string.IsNullOrWhiteSpace(line))
+                continue;
+
+            string processedLine = line.Clean();
+            string[] splitLine = processedLine.Split(' ');
+
+            //blank or comment
+            if (splitLine.Length < 2 || processedLine[0] == '#')
+                continue;
+
+            //newmtl
+            if (splitLine[0] == "newmtl")
+            {
+                string materialName = processedLine.Substring(7);
+
+                var newMtl = new Material(Shader.Find("Standard (Specular setup)")) { name = materialName };
+                mtlDict[materialName] = newMtl;
+                currentMaterial = newMtl;
+
+                continue;
+            }
+
+            //anything past here requires a material instance
+            if (currentMaterial == null)
+                continue;
+
+            //diffuse color
+            if (splitLine[0] == "Kd" || splitLine[0] == "kd")
+            {
+                var currentColor = currentMaterial.GetColor("_Color");
+                var kdColor = OBJLoaderHelper.ColorFromStrArray(splitLine);
+
+                currentMaterial.SetColor("_Color", new Color(kdColor.r, kdColor.g, kdColor.b, currentColor.a));
+                continue;
+            }
+
+            //diffuse map
+            if (splitLine[0] == "map_Kd" || splitLine[0] == "map_kd")
+            {
+                string texturePath = GetTexPathFromMapStatement(processedLine, splitLine);
+                if(texturePath == null)
+                {
+                    continue; //invalid args or sth
+                }
+
+                var KdTexture = TryLoadTexture(texturePath);
+                currentMaterial.SetTexture("_MainTex", KdTexture);
+
+                //set transparent mode if the texture has transparency
+                if(KdTexture != null && (KdTexture.format == TextureFormat.DXT5 || KdTexture.format == TextureFormat.ARGB32))
+                {
+                    OBJLoaderHelper.EnableMaterialTransparency(currentMaterial);
+                }
+
+                //flip texture if this is a dds
+                if(Path.GetExtension(texturePath).ToLower() == ".dds")
+                {
+                    currentMaterial.mainTextureScale = new Vector2(1f, -1f);
+                }
+
+                continue;
+            }
+
+            //bump map
+            if (splitLine[0] == "map_Bump" || splitLine[0] == "map_bump")
+            {
+                string texturePath = GetTexPathFromMapStatement(processedLine, splitLine);
+                if(texturePath == null)
+                {
+                    continue; //invalid args or sth
+                }
+
+                var bumpTexture = TryLoadTexture(texturePath, true);
+                float bumpScale = GetArgValue(splitLine, "-bm", 1.0f);
+
+                if (bumpTexture != null) {
+                    currentMaterial.SetTexture("_BumpMap", bumpTexture);
+                    currentMaterial.SetFloat("_BumpScale", bumpScale);
+                    currentMaterial.EnableKeyword("_NORMALMAP");
+                }
+
+                continue;
+            }
+
+            //specular color
+            if (splitLine[0] == "Ks" || splitLine[0] == "ks")
+            {
+                currentMaterial.SetColor("_SpecColor", OBJLoaderHelper.ColorFromStrArray(splitLine));
+                continue;
+            }
+
+            //emission color
+            if (splitLine[0] == "Ka" || splitLine[0] == "ka")
+            {
+                currentMaterial.SetColor("_EmissionColor", OBJLoaderHelper.ColorFromStrArray(splitLine, 0.05f));
+                currentMaterial.EnableKeyword("_EMISSION");
+                continue;
+            }
+
+            //emission map
+            if (splitLine[0] == "map_Ka" || splitLine[0] == "map_ka")
+            {
+                string texturePath = GetTexPathFromMapStatement(processedLine, splitLine);
+                if(texturePath == null)
+                {
+                    continue; //invalid args or sth
+                }
+
+                currentMaterial.SetTexture("_EmissionMap", TryLoadTexture(texturePath));
+                continue;
+            }
+
+            //alpha
+            if (splitLine[0] == "d" || splitLine[0] == "Tr")
+            {
+                float visibility = OBJLoaderHelper.FastFloatParse(splitLine[1]);
+                
+                //tr statement is just d inverted
+                if(splitLine[0] == "Tr")
+                    visibility = 1f - visibility;  
+
+                if(visibility < (1f - Mathf.Epsilon))
+                {
+                    var currentColor = currentMaterial.GetColor("_Color");
+
+                    currentColor.a = visibility;
+                    currentMaterial.SetColor("_Color", currentColor);
+
+                    OBJLoaderHelper.EnableMaterialTransparency(currentMaterial);
+                }
+                continue;
+            }
+
+            //glossiness
+            if (splitLine[0] == "Ns" || splitLine[0] == "ns")
+            {
+                float Ns = OBJLoaderHelper.FastFloatParse(splitLine[1]);
+                Ns = (Ns / 1000f);
+                currentMaterial.SetFloat("_Glossiness", Ns);
+            }
+        }
+
+        //return our dict
+        return mtlDict;
+    }
+
+    /// <summary>
+    /// Loads a *.mtl file
+    /// </summary>
+    /// <param name="path">The path to the MTL file</param>
+    /// <returns>Dictionary containing loaded materials</returns>
+	public Dictionary<string, Material> Load(string path)
+    {
+        _objFileInfo = new FileInfo(path); //get file info
+        SearchPaths.Add(_objFileInfo.Directory.FullName); //add root path to search dir
+
+        using (var fs = new FileStream(path, FileMode.Open))
+        {
+            return Load(fs); //actually load
+        }
+        
+    }
+}

OBJImport/MTLLoader.cs.meta

+fileFormatVersion: 2
+guid: 11b3883c0720fb8409b86b54877b58ea
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/OBJLoader.cs

+/*
+ * Copyright (c) 2019 Dummiesman
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+*/
+
+using System.Collections.Generic;
+using System.IO;
+using UnityEngine;
+using System;
+using Dummiesman;
+#if UNITY_EDITOR
+using UnityEditor;
+#endif
+
+namespace Dummiesman
+{
+    public enum SplitMode {
+        None,
+        Object,
+        Material
+    }
+    
+    public class OBJLoader
+    {
+        //options
+        /// <summary>
+        /// Determines how objects will be created
+        /// </summary>
+        public SplitMode SplitMode = SplitMode.Object;
+
+        //global lists, accessed by objobjectbuilder
+        internal List<Vector3> Vertices = new List<Vector3>();
+        internal List<Vector3> Normals = new List<Vector3>();
+        internal List<Vector2> UVs = new List<Vector2>();
+
+        //materials, accessed by objobjectbuilder
+        internal Dictionary<string, Material> Materials;
+
+        //file info for files loaded from file path, used for GameObject naming and MTL finding
+        private FileInfo _objInfo;
+
+#if UNITY_EDITOR
+        [MenuItem("GameObject/Import From OBJ")]
+        static void ObjLoadMenu()
+        {
+            string pth =  EditorUtility.OpenFilePanel("Import OBJ", "", "obj");
+            if (!string.IsNullOrEmpty(pth))
+            {
+                System.Diagnostics.Stopwatch s = new System.Diagnostics.Stopwatch();
+                s.Start();
+
+                var loader = new OBJLoader
+                {
+                    SplitMode = SplitMode.Object,
+                };
+                loader.Load(pth);
+
+                Debug.Log($"OBJ import time: {s.ElapsedMilliseconds}ms");
+                s.Stop();
+            }
+        }
+#endif
+
+        /// <summary>
+        /// Helper function to load mtllib statements
+        /// </summary>
+        /// <param name="mtlLibPath"></param>
+        private void LoadMaterialLibrary(string mtlLibPath)
+        {
+            if (_objInfo != null)
+            {
+                if (File.Exists(Path.Combine(_objInfo.Directory.FullName, mtlLibPath)))
+                {
+                    Materials = new MTLLoader().Load(Path.Combine(_objInfo.Directory.FullName, mtlLibPath));
+                    return;
+                }
+            }
+
+            if (File.Exists(mtlLibPath))
+            {
+                Materials = new MTLLoader().Load(mtlLibPath);
+                return;
+            }
+        }
+
+        /// <summary>
+        /// Load an OBJ file from a stream. No materials will be loaded, and will instead be supplemented by a blank white material.
+        /// </summary>
+        /// <param name="input">Input OBJ stream</param>
+        /// <returns>Returns a GameObject represeting the OBJ file, with each imported object as a child.</returns>
+        public GameObject Load(Stream input)
+        {
+            var reader = new StreamReader(input);
+            //var reader = new StringReader(inputReader.ReadToEnd());
+
+            Dictionary<string, OBJObjectBuilder> builderDict = new Dictionary<string, OBJObjectBuilder>();
+            OBJObjectBuilder currentBuilder = null;
+            string currentMaterial = "default";
+
+            //lists for face data
+            //prevents excess GC
+            List<int> vertexIndices = new List<int>();
+            List<int> normalIndices = new List<int>();
+            List<int> uvIndices = new List<int>();
+
+            //helper func
+            Action<string> setCurrentObjectFunc = (string objectName) =>
+            {
+                if (!builderDict.TryGetValue(objectName, out currentBuilder))
+                {
+                    currentBuilder = new OBJObjectBuilder(objectName, this);
+                    builderDict[objectName] = currentBuilder;
+                }
+            };
+
+            //create default object
+            setCurrentObjectFunc.Invoke("default");
+
+			//var buffer = new DoubleBuffer(reader, 256 * 1024);
+			var buffer = new CharWordReader(reader, 4 * 1024);
+
+			//do the reading
+			while (true)
+            {
+				buffer.SkipWhitespaces();
+
+				if (buffer.endReached == true) {
+					break;
+				}
+
+				buffer.ReadUntilWhiteSpace();
+				
+                //comment or blank
+                if (buffer.Is("#"))
+                {
+					buffer.SkipUntilNewLine();
+                    continue;
+                }
+				
+				if (Materials == null && buffer.Is("mtllib")) {
+					buffer.SkipWhitespaces();
+					buffer.ReadUntilNewLine();
+					string mtlLibPath = buffer.GetString();
+					LoadMaterialLibrary(mtlLibPath);
+					continue;
+				}
+				
+				if (buffer.Is("v")) {
+					Vertices.Add(buffer.ReadVector());
+					continue;
+				}
+
+				//normal
+				if (buffer.Is("vn")) {
+                    Normals.Add(buffer.ReadVector());
+                    continue;
+                }
+
+                //uv
+				if (buffer.Is("vt")) {
+                    UVs.Add(buffer.ReadVector());
+                    continue;
+                }
+
+                //new material
+				if (buffer.Is("usemtl")) {
+					buffer.SkipWhitespaces();
+					buffer.ReadUntilNewLine();
+					string materialName = buffer.GetString();
+                    currentMaterial = materialName;
+
+                    if(SplitMode == SplitMode.Material)
+                    {
+                        setCurrentObjectFunc.Invoke(materialName);
+                    }
+                    continue;
+                }
+
+                //new object
+                if ((buffer.Is("o") || buffer.Is("g")) && SplitMode == SplitMode.Object) {
+                    buffer.ReadUntilNewLine();
+                    string objectName = buffer.GetString(1);
+                    setCurrentObjectFunc.Invoke(objectName);
+                    continue;
+                }
+
+                //face data (the fun part)
+                if (buffer.Is("f"))
+                {
+                    //loop through indices
+                    while (true)
+                    {
+						bool newLinePassed;
+						buffer.SkipWhitespaces(out newLinePassed);
+						if (newLinePassed == true) {
+							break;
+						}
+
+                        int vertexIndex = int.MinValue;
+                        int normalIndex = int.MinValue;
+                        int uvIndex = int.MinValue;
+
+						vertexIndex = buffer.ReadInt();
+						if (buffer.currentChar == '/') {
+							buffer.MoveNext();
+							if (buffer.currentChar != '/') {
+								uvIndex = buffer.ReadInt();
+							}
+							if (buffer.currentChar == '/') {
+								buffer.MoveNext();
+								normalIndex = buffer.ReadInt();
+							}
+						}
+
+                        //"postprocess" indices
+                        if (vertexIndex > int.MinValue)
+                        {
+                            if (vertexIndex < 0)
+                                vertexIndex = Vertices.Count - vertexIndex;
+                            vertexIndex--;
+                        }
+                        if (normalIndex > int.MinValue)
+                        {
+                            if (normalIndex < 0)
+                                normalIndex = Normals.Count - normalIndex;
+                            normalIndex--;
+                        }
+                        if (uvIndex > int.MinValue)
+                        {
+                            if (uvIndex < 0)
+                                uvIndex = UVs.Count - uvIndex;
+                            uvIndex--;
+                        }
+
+                        //set array values
+                        vertexIndices.Add(vertexIndex);
+                        normalIndices.Add(normalIndex);
+                        uvIndices.Add(uvIndex);
+                    }
+
+                    //push to builder
+                    currentBuilder.PushFace(currentMaterial, vertexIndices, normalIndices, uvIndices);
+
+                    //clear lists
+                    vertexIndices.Clear();
+                    normalIndices.Clear();
+                    uvIndices.Clear();
+
+					continue;
+                }
+
+				buffer.SkipUntilNewLine();
+            }
+
+            //finally, put it all together
+            GameObject obj = new GameObject(_objInfo != null ? Path.GetFileNameWithoutExtension(_objInfo.Name) : "WavefrontObject");
+            obj.transform.localScale = new Vector3(-1f, 1f, 1f);
+
+            foreach (var builder in builderDict)
+            {
+                //empty object
+                if (builder.Value.PushedFaceCount == 0)
+                    continue;
+
+                var builtObj = builder.Value.Build();
+                builtObj.transform.SetParent(obj.transform, false);
+            }
+
+            return obj;
+        }
+
+        /// <summary>
+        /// Load an OBJ and MTL file from a stream.
+        /// </summary>
+        /// <param name="input">Input OBJ stream</param>
+        /// /// <param name="mtlInput">Input MTL stream</param>
+        /// <returns>Returns a GameObject represeting the OBJ file, with each imported object as a child.</returns>
+        public GameObject Load(Stream input, Stream mtlInput)
+        {
+            var mtlLoader = new MTLLoader();
+            Materials = mtlLoader.Load(mtlInput);
+
+            return Load(input);
+        }
+
+        /// <summary>
+        /// Load an OBJ and MTL file from a file path.
+        /// </summary>
+        /// <param name="path">Input OBJ path</param>
+        /// /// <param name="mtlPath">Input MTL path</param>
+        /// <returns>Returns a GameObject represeting the OBJ file, with each imported object as a child.</returns>
+        public GameObject Load(string path, string mtlPath)
+        {
+            _objInfo = new FileInfo(path);
+            if (!string.IsNullOrEmpty(mtlPath) && File.Exists(mtlPath))
+            {
+                var mtlLoader = new MTLLoader();
+                Materials = mtlLoader.Load(mtlPath);
+
+                using (var fs = new FileStream(path, FileMode.Open))
+                {
+                    return Load(fs);
+                }
+            }
+            else
+            {
+                using (var fs = new FileStream(path, FileMode.Open))
+                {
+                    return Load(fs);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Load an OBJ file from a file path. This function will also attempt to load the MTL defined in the OBJ file.
+        /// </summary>
+        /// <param name="path">Input OBJ path</param>
+        /// <returns>Returns a GameObject represeting the OBJ file, with each imported object as a child.</returns>
+        public GameObject Load(string path)
+        {
+            return Load(path, null);
+        }
+    }
+}
\ No newline at end of file

OBJImport/OBJLoader.cs.meta

+fileFormatVersion: 2
+guid: 9951190d0789ee64599dc374c1f81ce5
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/OBJLoaderHelper.cs

+using System.Globalization;
+using UnityEngine;
+
+namespace Dummiesman 
+{
+    public static class OBJLoaderHelper  
+    {
+        /// <summary>
+        /// Enables transparency mode on standard materials
+        /// </summary>
+        public static void EnableMaterialTransparency(Material mtl)
+        {
+            mtl.SetFloat("_Mode", 3f);
+            mtl.SetInt("_SrcBlend", (int)UnityEngine.Rendering.BlendMode.SrcAlpha);
+            mtl.SetInt("_DstBlend", (int)UnityEngine.Rendering.BlendMode.OneMinusSrcAlpha);
+            mtl.SetInt("_ZWrite", 0);
+            mtl.DisableKeyword("_ALPHATEST_ON");
+            mtl.EnableKeyword("_ALPHABLEND_ON");
+            mtl.DisableKeyword("_ALPHAPREMULTIPLY_ON");
+            mtl.renderQueue = 3000;
+        }
+
+        /// <summary>
+        /// Modified from https://codereview.stackexchange.com/a/76891. Faster than float.Parse
+        /// </summary>
+        public static float FastFloatParse(string input)
+        {
+            if (input.Contains("e") || input.Contains("E"))
+                return float.Parse(input, CultureInfo.InvariantCulture);
+
+            float result = 0;
+            int pos = 0;
+            int len = input.Length;
+
+            if (len == 0) return float.NaN;
+            char c = input[0];
+            float sign = 1;
+            if (c == '-')
+            {
+                sign = -1;
+                ++pos;
+                if (pos >= len) return float.NaN;
+            }
+
+            while (true) // breaks inside on pos >= len or non-digit character
+            {
+                if (pos >= len) return sign * result;
+                c = input[pos++];
+                if (c < '0' || c > '9') break;
+                result = (result * 10.0f) + (c - '0');
+            }
+
+            if (c != '.' && c != ',') return float.NaN;
+            float exp = 0.1f;
+            while (pos < len)
+            {
+                c = input[pos++];
+                if (c < '0' || c > '9') return float.NaN;
+                result += (c - '0') * exp;
+                exp *= 0.1f;
+            }
+            return sign * result;
+        }
+
+        /// <summary>
+        /// Modified from http://cc.davelozinski.com/c-sharp/fastest-way-to-convert-a-string-to-an-int. Faster than int.Parse
+        /// </summary>
+        public static int FastIntParse(string input)
+        {
+            int result = 0;
+            bool isNegative = (input[0] == '-');
+            
+            for (int i = (isNegative) ? 1 : 0; i < input.Length; i++)
+                result = result * 10 + (input[i] - '0');
+            return (isNegative) ? -result : result;
+        }
+
+        public static Material CreateNullMaterial()
+        {
+            return new Material(Shader.Find("Standard (Specular setup)"));
+        }
+
+        public static Vector3 VectorFromStrArray(string[] cmps)
+        {
+            float x = FastFloatParse(cmps[1]);
+            float y = FastFloatParse(cmps[2]);
+            if (cmps.Length == 4)
+            {
+                float z = FastFloatParse(cmps[3]);
+                return new Vector3(x, y, z);
+            }
+            return new Vector2(x, y);
+        }
+
+        public static Color ColorFromStrArray(string[] cmps, float scalar = 1.0f)
+        {
+            float Kr = FastFloatParse(cmps[1]) * scalar;
+            float Kg = FastFloatParse(cmps[2]) * scalar;
+            float Kb = FastFloatParse(cmps[3]) * scalar;
+            return new Color(Kr, Kg, Kb);
+        }
+    }
+}
\ No newline at end of file

OBJImport/OBJLoaderHelper.cs.meta

+fileFormatVersion: 2
+guid: 6083f989375f6104aafb31a9e82ee153
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/OBJObjectBuilder.cs

+/*
+ * Copyright (c) 2019 Dummiesman
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+*/
+
+using Dummiesman;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Dummiesman {
+public class OBJObjectBuilder {
+	//
+	public int PushedFaceCount { get; private set; } = 0;
+
+	//stuff passed in by ctor
+	private OBJLoader _loader;
+	private string _name;
+
+	private Dictionary<ObjLoopHash, int> _globalIndexRemap = new Dictionary<ObjLoopHash, int>();
+	private Dictionary<string, List<int>> _materialIndices = new Dictionary<string, List<int>>();
+	private List<int> _currentIndexList;
+	private string _lastMaterial = null;
+
+	//our local vert/normal/uv
+	private List<Vector3> _vertices = new List<Vector3>();
+	private List<Vector3> _normals = new List<Vector3>();
+	private List<Vector2> _uvs = new List<Vector2>();
+
+	//this will be set if the model has no normals or missing normal info
+	private bool recalculateNormals = false;
+
+	/// <summary>
+	/// Loop hasher helper class
+	/// </summary>
+	private class ObjLoopHash {
+		public int vertexIndex;
+		public int normalIndex;
+		public int uvIndex;
+
+		public override bool Equals(object obj) {
+			if (!(obj is ObjLoopHash))
+				return false;
+
+			var hash = obj as ObjLoopHash;
+			return (hash.vertexIndex == vertexIndex) && (hash.uvIndex == uvIndex) && (hash.normalIndex == normalIndex);
+		}
+
+		public override int GetHashCode() {
+			int hc = 3;
+			hc = unchecked(hc * 314159 + vertexIndex);
+			hc = unchecked(hc * 314159 + normalIndex);
+			hc = unchecked(hc * 314159 + uvIndex);
+			return hc;
+		}
+	}
+
+	public GameObject Build() {
+		var go = new GameObject(_name);
+
+		//add meshrenderer
+		var mr = go.AddComponent<MeshRenderer>();
+		int submesh = 0;
+
+
+		//locate the material for each submesh
+		Material[] materialArray = new Material[_materialIndices.Count];
+		foreach (var kvp in _materialIndices) {
+			Material material = null;
+			if (_loader.Materials == null) {
+				material = OBJLoaderHelper.CreateNullMaterial();
+				material.name = kvp.Key;
+			} else {
+				if (!_loader.Materials.TryGetValue(kvp.Key, out material)) {
+					material = OBJLoaderHelper.CreateNullMaterial();
+					material.name = kvp.Key;
+					_loader.Materials[kvp.Key] = material;
+				}
+			}
+			materialArray[submesh] = material;
+			submesh++;
+		}
+		mr.sharedMaterials = materialArray;
+
+		//add meshfilter
+		var mf = go.AddComponent<MeshFilter>();
+		submesh = 0;
+
+		var msh = new Mesh() {
+			name = _name, 
+			indexFormat = (_vertices.Count > 65535) ? UnityEngine.Rendering.IndexFormat.UInt32 : UnityEngine.Rendering.IndexFormat.UInt16,
+			subMeshCount = _materialIndices.Count 
+		};
+
+		//set vertex data
+		msh.SetVertices(_vertices);
+		msh.SetNormals(_normals);
+		msh.SetUVs(0, _uvs);
+
+		//set faces
+		foreach (var kvp in _materialIndices) {
+			msh.SetTriangles(kvp.Value, submesh);
+			submesh++;
+		}
+
+		//recalculations
+		if (recalculateNormals)
+			msh.RecalculateNormals();
+		msh.RecalculateTangents();
+		msh.RecalculateBounds();
+
+		mf.sharedMesh = msh;
+
+		//
+		return go;
+	}
+
+    public void SetMaterial(string name) {
+		if (!_materialIndices.TryGetValue(name, out _currentIndexList))
+		{
+			_currentIndexList = new List<int>();
+			_materialIndices[name] = _currentIndexList;
+		}
+	}
+
+
+	public void PushFace(string material, List<int> vertexIndices, List<int> normalIndices, List<int> uvIndices) {
+		//invalid face size?
+		if (vertexIndices.Count < 3) {
+			return;
+		}
+
+		//set material
+		if (material != _lastMaterial) {
+			SetMaterial(material);
+			_lastMaterial = material;
+		}
+
+		//remap
+		int[] indexRemap = new int[vertexIndices.Count];
+		for (int i = 0; i < vertexIndices.Count; i++) {
+			int vertexIndex = vertexIndices[i];
+			int normalIndex = normalIndices[i];
+			int uvIndex = uvIndices[i];
+
+			var hashObj = new ObjLoopHash() {
+				vertexIndex = vertexIndex,
+				normalIndex = normalIndex,
+				uvIndex = uvIndex
+			};
+			int remap = -1;
+
+			if (!_globalIndexRemap.TryGetValue(hashObj, out remap)) {
+				//add to dict
+				_globalIndexRemap.Add(hashObj, _vertices.Count);
+				remap = _vertices.Count;
+
+				//add new verts and what not
+				_vertices.Add((vertexIndex >= 0 && vertexIndex < _loader.Vertices.Count) ? _loader.Vertices[vertexIndex] : Vector3.zero);
+				_normals.Add((normalIndex >= 0 && normalIndex < _loader.Normals.Count) ? _loader.Normals[normalIndex] : Vector3.zero);
+				_uvs.Add((uvIndex >= 0 && uvIndex < _loader.UVs.Count) ? _loader.UVs[uvIndex] : Vector2.zero);
+
+				//mark recalc flag
+				if (normalIndex < 0)
+					recalculateNormals = true;
+			}
+
+			indexRemap[i] = remap;
+		}
+
+
+		//add face to our mesh list
+		if (indexRemap.Length == 3) {
+			_currentIndexList.AddRange(new int[] { indexRemap[0], indexRemap[1], indexRemap[2] });
+		} else if (indexRemap.Length == 4) {
+			_currentIndexList.AddRange(new int[] { indexRemap[0], indexRemap[1], indexRemap[2] });
+			_currentIndexList.AddRange(new int[] { indexRemap[2], indexRemap[3], indexRemap[0] });
+		} else if (indexRemap.Length > 4) {
+			for (int i = indexRemap.Length - 1; i >= 2; i--) {
+				_currentIndexList.AddRange(new int[] { indexRemap[0], indexRemap[i - 1], indexRemap[i] });
+			}
+		}
+
+		PushedFaceCount++;
+	}
+
+	public OBJObjectBuilder(string name, OBJLoader loader) {
+		_name = name;
+		_loader = loader;
+	}
+}
+}
\ No newline at end of file

OBJImport/OBJObjectBuilder.cs.meta

+fileFormatVersion: 2
+guid: 8242329223027ef40a531fc4dc2efd1c
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/README.HTML

+<!DOCTYPE html>
+<html>
+
+<head>
+    <title>Dummiesman OBJ Importer Readme</title>
+</head>
+
+<body>
+    <h2>Dummiesman's OBJ Importer</h2>
+    <p>Works during runtime*, and in editor! A couple of code samples are provided in the Samples folder.</p>
+    <br />
+    <h2>*Runtime Import Note</h2>
+    <p>You must go to the "Graphics" tab in your project, and add "Standard (Specular Setup)" to the always included shaders. This will make your next build take a while.</p>
+    <br />
+    <h2>Features</h2>
+    <ul>
+        <li>Material file loading</li>
+        <li>Runtime texture library supporting BMP, TGA, JPG, PNG, DDS, and CRN textures.</li>
+        <li>Decent import speed (~750k triangles in about 10 seconds)</li>
+        <li>Support for loading obj and mtl from stream, as well as overridable callback for texture loads</li>
+        <li>Works in editor, and during runtime</li>
+        <li>Loads triangles, quads, and ngons</li>
+    </ul>
+</body>
+
+</html>
\ No newline at end of file

OBJImport/README.HTML.meta

+fileFormatVersion: 2
+guid: 127e58e7a0d489046a128e511967dea8
+TextScriptImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/Samples.meta

+fileFormatVersion: 2
+guid: 1e292531b556307479437e7763c8b31f
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/Samples/MeshImporter3.cs

+using UnityEngine;
+using System.IO;
+using Dummiesman;
+using System;
+
+public class MeshImporter3 : MonoBehaviour
+{
+    void Awake()
+    {
+        string logFilePath = "/home/akreibich/TestRobinCode37/DebugLogs.txt";
+        File.AppendAllText(logFilePath, "Awake started.\n");
+
+        try
+        {
+            string[] args = Environment.GetCommandLineArgs();
+            File.AppendAllText(logFilePath, "Received arguments: " + string.Join(", ", args) + "\n");
+
+            string meshPath ="/home/akreibich/Desktop/Bachelor/Data (Binary)/Input/Input_MeshOption/DataForMeshes/Data10/Component1_normals1010_pos.obj";
+            string secondMeshPath =  "/home/akreibich/Desktop/Bachelor/Data (Binary)/Input/Input_MeshOption/DataForMeshes/Data10/Component1_1010_pos.obj";
+		//if run via the python loop, it should be like this
+		//Vector3 position = Vector3.zero;
+		//Vector3 scale = Vector3.one;
+		//Vector3 rotation = Vector3.zero;
+
+		//Change it to this for testing
+		Vector3 position = new Vector3(1311f, -392f, -1197f);
+		Vector3 scale = new Vector3(27.2f, 27.2f, 27.2f);
+		Vector3 rotation = new Vector3(0f, -86.6f, 0f);
+
+            for (int i = 0; i < args.Length; i++)
+            {
+                if (args[i] == "-objPath")
+                {
+                    meshPath = args[i + 1];
+                }
+                else if (args[i] == "-secondObjPath")
+                {
+                    secondMeshPath = args[i + 1];
+                }
+                else if (args[i] == "-position")
+                {
+                    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")
+                {
+                    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")
+                {
+                    string[] rotArgs = args[i + 1].Split(',');
+                    rotation = new Vector3(float.Parse(rotArgs[0]), float.Parse(rotArgs[1]), float.Parse(rotArgs[2]));
+                }
+            }
+
+            if (string.IsNullOrEmpty(meshPath))
+            {
+                Debug.LogError("Primary mesh path not specified");
+                File.AppendAllText(logFilePath, "Primary mesh path not specified\n");
+                return;
+            }
+
+            Mesh mesh = LoadMesh(meshPath, logFilePath);
+            Mesh secondMesh = string.IsNullOrEmpty(secondMeshPath) ? null : LoadMesh(secondMeshPath, logFilePath);
+
+            ImportMesh(mesh, secondMesh, position, scale, rotation);
+        }
+        catch (Exception e)
+        {
+            Debug.LogError("An exception occurred: " + e.ToString());
+            File.AppendAllText(logFilePath, "An exception occurred: " + e.ToString() + "\n");
+        }
+    }
+
+    Mesh LoadMesh(string meshPath, string logFilePath)
+    {
+        OBJLoader objLoader = new OBJLoader();
+        GameObject loadedObj = objLoader.Load(meshPath);
+
+        if (loadedObj == null)
+        {
+            Debug.LogError("Failed to load mesh from path: " + meshPath);
+            File.AppendAllText(logFilePath, "Failed to load mesh from path: " + meshPath + "\n");
+            return null;
+        }
+
+        File.AppendAllText(logFilePath, "Successfully loaded mesh from path: " + meshPath + "\n");
+
+        MeshFilter meshFilter = loadedObj.GetComponentInChildren<MeshFilter>();
+        Destroy(loadedObj);
+
+        if (meshFilter == null)
+        {
+            Debug.LogError("No MeshFilter component found in loaded GameObject");
+            File.AppendAllText(logFilePath, "No MeshFilter component found in loaded GameObject\n");
+            return null;
+        }
+
+        return meshFilter.sharedMesh;
+    }
+
+    void ImportMesh(Mesh primaryMesh, Mesh secondaryMesh, Vector3 position, Vector3 scale, Vector3 rotation)
+    {
+        if (primaryMesh == null)
+        {
+            Debug.LogError("Primary mesh is null");
+            return;
+        }
+
+        GameObject obj = new GameObject(primaryMesh.name);
+        obj.transform.position = position;
+        obj.transform.localScale = scale;
+        obj.transform.eulerAngles = rotation;
+
+        MeshFilter filter = obj.AddComponent<MeshFilter>();
+        filter.mesh = primaryMesh;
+
+        MeshRenderer renderer = obj.AddComponent<MeshRenderer>();
+        Material mat = Resources.Load<Material>("Aorta Material");
+
+        if (mat != null)
+        {
+            renderer.material = mat;
+        }
+        else
+        {
+            Debug.LogError("Failed to load material");
+        }
+
+        MeshCollider primaryCollider = obj.AddComponent<MeshCollider>();
+        primaryCollider.sharedMesh = primaryMesh;
+
+        if (secondaryMesh != null)
+        {
+            MeshCollider secondaryCollider = obj.AddComponent<MeshCollider>();
+            secondaryCollider.sharedMesh = secondaryMesh;
+        }
+
+        Rigidbody rb = obj.AddComponent<Rigidbody>();
+        rb.useGravity = false;
+        rb.isKinematic = true;
+
+        int layerIndex = LayerMask.NameToLayer("Blood Vessel");
+
+        if (layerIndex == -1)
+        {
+            Debug.LogError("Layer 'Blood Vessel' not found");
+        }
+        else
+        {
+            obj.layer = layerIndex;
+        }
+    }
+}
+

OBJImport/Samples/MeshImporter3.cs.meta

+fileFormatVersion: 2
+guid: df0f3cf90c6b7f24b99437ae8c8dc721
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/Samples/ObjFromFile.cs

+using Dummiesman;
+using System.IO;
+using UnityEngine;
+
+public class ObjFromFile : MonoBehaviour
+{
+    string objPath = string.Empty;
+    string error = string.Empty;
+    GameObject loadedObject;
+
+    void OnGUI() {
+        objPath = GUI.TextField(new Rect(0, 0, 256, 32), objPath);
+
+        GUI.Label(new Rect(0, 0, 256, 32), "Obj Path:");
+        if(GUI.Button(new Rect(256, 32, 64, 32), "Load File"))
+        {
+            //file path
+            if (!File.Exists(objPath))
+            {
+                error = "File doesn't exist.";
+            }else{
+                if(loadedObject != null)            
+                    Destroy(loadedObject);
+                loadedObject = new OBJLoader().Load(objPath);
+                error = string.Empty;
+            }
+        }
+
+        if(!string.IsNullOrWhiteSpace(error))
+        {
+            GUI.color = Color.red;
+            GUI.Box(new Rect(0, 64, 256 + 64, 32), error);
+            GUI.color = Color.white;
+        }
+    }
+}

OBJImport/Samples/ObjFromFile.cs.meta

+fileFormatVersion: 2
+guid: e668e28abcfb0ae439f4ead584aba85b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/Samples/ObjFromFile.unity

+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!29 &1
+OcclusionCullingSettings:
+  m_ObjectHideFlags: 0
+  serializedVersion: 2
+  m_OcclusionBakeSettings:
+    smallestOccluder: 5
+    smallestHole: 0.25
+    backfaceThreshold: 100
+  m_SceneGUID: 00000000000000000000000000000000
+  m_OcclusionCullingData: {fileID: 0}
+--- !u!104 &2
+RenderSettings:
+  m_ObjectHideFlags: 0
+  serializedVersion: 9
+  m_Fog: 0
+  m_FogColor: {r: 0.5, g: 0.5, b: 0.5, a: 1}
+  m_FogMode: 3
+  m_FogDensity: 0.01
+  m_LinearFogStart: 0
+  m_LinearFogEnd: 300
+  m_AmbientSkyColor: {r: 0.212, g: 0.227, b: 0.259, a: 1}
+  m_AmbientEquatorColor: {r: 0.114, g: 0.125, b: 0.133, a: 1}
+  m_AmbientGroundColor: {r: 0.047, g: 0.043, b: 0.035, a: 1}
+  m_AmbientIntensity: 1
+  m_AmbientMode: 0
+  m_SubtractiveShadowColor: {r: 0.42, g: 0.478, b: 0.627, a: 1}
+  m_SkyboxMaterial: {fileID: 10304, guid: 0000000000000000f000000000000000, type: 0}
+  m_HaloStrength: 0.5
+  m_FlareStrength: 1
+  m_FlareFadeSpeed: 3
+  m_HaloTexture: {fileID: 0}
+  m_SpotCookie: {fileID: 10001, guid: 0000000000000000e000000000000000, type: 0}
+  m_DefaultReflectionMode: 0
+  m_DefaultReflectionResolution: 128
+  m_ReflectionBounces: 1
+  m_ReflectionIntensity: 1
+  m_CustomReflection: {fileID: 0}
+  m_Sun: {fileID: 0}
+  m_IndirectSpecularColor: {r: 0.44657898, g: 0.4964133, b: 0.5748178, a: 1}
+  m_UseRadianceAmbientProbe: 0
+--- !u!157 &3
+LightmapSettings:
+  m_ObjectHideFlags: 0
+  serializedVersion: 11
+  m_GIWorkflowMode: 0
+  m_GISettings:
+    serializedVersion: 2
+    m_BounceScale: 1
+    m_IndirectOutputScale: 1
+    m_AlbedoBoost: 1
+    m_TemporalCoherenceThreshold: 1
+    m_EnvironmentLightingMode: 0
+    m_EnableBakedLightmaps: 1
+    m_EnableRealtimeLightmaps: 1
+  m_LightmapEditorSettings:
+    serializedVersion: 10
+    m_Resolution: 2
+    m_BakeResolution: 40
+    m_AtlasSize: 1024
+    m_AO: 0
+    m_AOMaxDistance: 1
+    m_CompAOExponent: 1
+    m_CompAOExponentDirect: 0
+    m_Padding: 2
+    m_LightmapParameters: {fileID: 0}
+    m_LightmapsBakeMode: 1
+    m_TextureCompression: 1
+    m_FinalGather: 0
+    m_FinalGatherFiltering: 1
+    m_FinalGatherRayCount: 256
+    m_ReflectionCompression: 2
+    m_MixedBakeMode: 2
+    m_BakeBackend: 1
+    m_PVRSampling: 1
+    m_PVRDirectSampleCount: 32
+    m_PVRSampleCount: 500
+    m_PVRBounces: 2
+    m_PVRFilterTypeDirect: 0
+    m_PVRFilterTypeIndirect: 0
+    m_PVRFilterTypeAO: 0
+    m_PVRFilteringMode: 1
+    m_PVRCulling: 1
+    m_PVRFilteringGaussRadiusDirect: 1
+    m_PVRFilteringGaussRadiusIndirect: 5
+    m_PVRFilteringGaussRadiusAO: 2
+    m_PVRFilteringAtrousPositionSigmaDirect: 0.5
+    m_PVRFilteringAtrousPositionSigmaIndirect: 2
+    m_PVRFilteringAtrousPositionSigmaAO: 1
+    m_ShowResolutionOverlay: 1
+  m_LightingDataAsset: {fileID: 0}
+  m_UseShadowmask: 1
+--- !u!196 &4
+NavMeshSettings:
+  serializedVersion: 2
+  m_ObjectHideFlags: 0
+  m_BuildSettings:
+    serializedVersion: 2
+    agentTypeID: 0
+    agentRadius: 0.5
+    agentHeight: 2
+    agentSlope: 45
+    agentClimb: 0.4
+    ledgeDropHeight: 0
+    maxJumpAcrossDistance: 0
+    minRegionArea: 2
+    manualCellSize: 0
+    cellSize: 0.16666667
+    manualTileSize: 0
+    tileSize: 256
+    accuratePlacement: 0
+    debug:
+      m_Flags: 0
+  m_NavMeshData: {fileID: 0}
+--- !u!1 &1373862594
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 1373862596}
+  - component: {fileID: 1373862595}
+  m_Layer: 0
+  m_Name: Directional Light
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!108 &1373862595
+Light:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 1373862594}
+  m_Enabled: 1
+  serializedVersion: 8
+  m_Type: 1
+  m_Color: {r: 1, g: 0.95686275, b: 0.8392157, a: 1}
+  m_Intensity: 1
+  m_Range: 10
+  m_SpotAngle: 30
+  m_CookieSize: 10
+  m_Shadows:
+    m_Type: 2
+    m_Resolution: -1
+    m_CustomResolution: -1
+    m_Strength: 1
+    m_Bias: 0.05
+    m_NormalBias: 0.4
+    m_NearPlane: 0.2
+  m_Cookie: {fileID: 0}
+  m_DrawHalo: 0
+  m_Flare: {fileID: 0}
+  m_RenderMode: 0
+  m_CullingMask:
+    serializedVersion: 2
+    m_Bits: 4294967295
+  m_Lightmapping: 4
+  m_LightShadowCasterMode: 0
+  m_AreaSize: {x: 1, y: 1}
+  m_BounceIntensity: 1
+  m_ColorTemperature: 6570
+  m_UseColorTemperature: 0
+  m_ShadowRadius: 0
+  m_ShadowAngle: 0
+--- !u!4 &1373862596
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 1373862594}
+  m_LocalRotation: {x: 0.40821788, y: -0.23456968, z: 0.10938163, w: 0.8754261}
+  m_LocalPosition: {x: 0, y: 3, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 1
+  m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0}
+--- !u!1 &1374413049
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 1374413051}
+  - component: {fileID: 1374413050}
+  m_Layer: 0
+  m_Name: Scripts
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!114 &1374413050
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 1374413049}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: e668e28abcfb0ae439f4ead584aba85b, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+--- !u!4 &1374413051
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 1374413049}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 2
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!1 &1513737669
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 1513737672}
+  - component: {fileID: 1513737671}
+  - component: {fileID: 1513737670}
+  m_Layer: 0
+  m_Name: Main Camera
+  m_TagString: MainCamera
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!81 &1513737670
+AudioListener:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 1513737669}
+  m_Enabled: 1
+--- !u!20 &1513737671
+Camera:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 1513737669}
+  m_Enabled: 1
+  serializedVersion: 2
+  m_ClearFlags: 1
+  m_BackGroundColor: {r: 0.19215687, g: 0.3019608, b: 0.4745098, a: 0}
+  m_projectionMatrixMode: 1
+  m_SensorSize: {x: 36, y: 24}
+  m_LensShift: {x: 0, y: 0}
+  m_FocalLength: 50
+  m_NormalizedViewPortRect:
+    serializedVersion: 2
+    x: 0
+    y: 0
+    width: 1
+    height: 1
+  near clip plane: 0.3
+  far clip plane: 1000
+  field of view: 60
+  orthographic: 0
+  orthographic size: 5
+  m_Depth: -1
+  m_CullingMask:
+    serializedVersion: 2
+    m_Bits: 4294967295
+  m_RenderingPath: -1
+  m_TargetTexture: {fileID: 0}
+  m_TargetDisplay: 0
+  m_TargetEye: 3
+  m_HDR: 1
+  m_AllowMSAA: 1
+  m_AllowDynamicResolution: 0
+  m_ForceIntoRT: 0
+  m_OcclusionCulling: 1
+  m_StereoConvergence: 10
+  m_StereoSeparation: 0.022
+--- !u!4 &1513737672
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 1513737669}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 1, z: -10}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 0
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}

OBJImport/Samples/ObjFromFile.unity.meta

+fileFormatVersion: 2
+guid: e96535515ee47d04589da7981f6650a2
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/Samples/ObjFromStream.cs

+using Dummiesman;
+using System.IO;
+using System.Text;
+using UnityEngine;
+
+public class ObjFromStream : MonoBehaviour {
+	void Start () {
+        //make www
+        var www = new WWW("https://people.sc.fsu.edu/~jburkardt/data/obj/lamp.obj");
+        while (!www.isDone)
+            System.Threading.Thread.Sleep(1);
+        
+        //create stream and load
+        var textStream = new MemoryStream(Encoding.UTF8.GetBytes(www.text));
+        var loadedObj = new OBJLoader().Load(textStream);
+	}
+}

OBJImport/Samples/ObjFromStream.cs.meta

+fileFormatVersion: 2
+guid: daba88a30f38f3f4d8df0d146acfcd08
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/Samples/ObjFromStream.unity

+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!29 &1
+OcclusionCullingSettings:
+  m_ObjectHideFlags: 0
+  serializedVersion: 2
+  m_OcclusionBakeSettings:
+    smallestOccluder: 5
+    smallestHole: 0.25
+    backfaceThreshold: 100
+  m_SceneGUID: 00000000000000000000000000000000
+  m_OcclusionCullingData: {fileID: 0}
+--- !u!104 &2
+RenderSettings:
+  m_ObjectHideFlags: 0
+  serializedVersion: 9
+  m_Fog: 0
+  m_FogColor: {r: 0.5, g: 0.5, b: 0.5, a: 1}
+  m_FogMode: 3
+  m_FogDensity: 0.01
+  m_LinearFogStart: 0
+  m_LinearFogEnd: 300
+  m_AmbientSkyColor: {r: 0.212, g: 0.227, b: 0.259, a: 1}
+  m_AmbientEquatorColor: {r: 0.114, g: 0.125, b: 0.133, a: 1}
+  m_AmbientGroundColor: {r: 0.047, g: 0.043, b: 0.035, a: 1}
+  m_AmbientIntensity: 1
+  m_AmbientMode: 0
+  m_SubtractiveShadowColor: {r: 0.42, g: 0.478, b: 0.627, a: 1}
+  m_SkyboxMaterial: {fileID: 10304, guid: 0000000000000000f000000000000000, type: 0}
+  m_HaloStrength: 0.5
+  m_FlareStrength: 1
+  m_FlareFadeSpeed: 3
+  m_HaloTexture: {fileID: 0}
+  m_SpotCookie: {fileID: 10001, guid: 0000000000000000e000000000000000, type: 0}
+  m_DefaultReflectionMode: 0
+  m_DefaultReflectionResolution: 128
+  m_ReflectionBounces: 1
+  m_ReflectionIntensity: 1
+  m_CustomReflection: {fileID: 0}
+  m_Sun: {fileID: 0}
+  m_IndirectSpecularColor: {r: 0.44657898, g: 0.4964133, b: 0.5748178, a: 1}
+  m_UseRadianceAmbientProbe: 0
+--- !u!157 &3
+LightmapSettings:
+  m_ObjectHideFlags: 0
+  serializedVersion: 11
+  m_GIWorkflowMode: 0
+  m_GISettings:
+    serializedVersion: 2
+    m_BounceScale: 1
+    m_IndirectOutputScale: 1
+    m_AlbedoBoost: 1
+    m_TemporalCoherenceThreshold: 1
+    m_EnvironmentLightingMode: 0
+    m_EnableBakedLightmaps: 1
+    m_EnableRealtimeLightmaps: 1
+  m_LightmapEditorSettings:
+    serializedVersion: 10
+    m_Resolution: 2
+    m_BakeResolution: 40
+    m_AtlasSize: 1024
+    m_AO: 0
+    m_AOMaxDistance: 1
+    m_CompAOExponent: 1
+    m_CompAOExponentDirect: 0
+    m_Padding: 2
+    m_LightmapParameters: {fileID: 0}
+    m_LightmapsBakeMode: 1
+    m_TextureCompression: 1
+    m_FinalGather: 0
+    m_FinalGatherFiltering: 1
+    m_FinalGatherRayCount: 256
+    m_ReflectionCompression: 2
+    m_MixedBakeMode: 2
+    m_BakeBackend: 1
+    m_PVRSampling: 1
+    m_PVRDirectSampleCount: 32
+    m_PVRSampleCount: 500
+    m_PVRBounces: 2
+    m_PVRFilterTypeDirect: 0
+    m_PVRFilterTypeIndirect: 0
+    m_PVRFilterTypeAO: 0
+    m_PVRFilteringMode: 1
+    m_PVRCulling: 1
+    m_PVRFilteringGaussRadiusDirect: 1
+    m_PVRFilteringGaussRadiusIndirect: 5
+    m_PVRFilteringGaussRadiusAO: 2
+    m_PVRFilteringAtrousPositionSigmaDirect: 0.5
+    m_PVRFilteringAtrousPositionSigmaIndirect: 2
+    m_PVRFilteringAtrousPositionSigmaAO: 1
+    m_ShowResolutionOverlay: 1
+  m_LightingDataAsset: {fileID: 0}
+  m_UseShadowmask: 1
+--- !u!196 &4
+NavMeshSettings:
+  serializedVersion: 2
+  m_ObjectHideFlags: 0
+  m_BuildSettings:
+    serializedVersion: 2
+    agentTypeID: 0
+    agentRadius: 0.5
+    agentHeight: 2
+    agentSlope: 45
+    agentClimb: 0.4
+    ledgeDropHeight: 0
+    maxJumpAcrossDistance: 0
+    minRegionArea: 2
+    manualCellSize: 0
+    cellSize: 0.16666667
+    manualTileSize: 0
+    tileSize: 256
+    accuratePlacement: 0
+    debug:
+      m_Flags: 0
+  m_NavMeshData: {fileID: 0}
+--- !u!1 &167273857
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 167273859}
+  - component: {fileID: 167273858}
+  m_Layer: 0
+  m_Name: Scripts
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!114 &167273858
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 167273857}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: daba88a30f38f3f4d8df0d146acfcd08, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+--- !u!4 &167273859
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 167273857}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 2
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!1 &560136681
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 560136683}
+  - component: {fileID: 560136682}
+  m_Layer: 0
+  m_Name: Directional Light
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!108 &560136682
+Light:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 560136681}
+  m_Enabled: 1
+  serializedVersion: 8
+  m_Type: 1
+  m_Color: {r: 1, g: 0.95686275, b: 0.8392157, a: 1}
+  m_Intensity: 1
+  m_Range: 10
+  m_SpotAngle: 30
+  m_CookieSize: 10
+  m_Shadows:
+    m_Type: 2
+    m_Resolution: -1
+    m_CustomResolution: -1
+    m_Strength: 1
+    m_Bias: 0.05
+    m_NormalBias: 0.4
+    m_NearPlane: 0.2
+  m_Cookie: {fileID: 0}
+  m_DrawHalo: 0
+  m_Flare: {fileID: 0}
+  m_RenderMode: 0
+  m_CullingMask:
+    serializedVersion: 2
+    m_Bits: 4294967295
+  m_Lightmapping: 4
+  m_LightShadowCasterMode: 0
+  m_AreaSize: {x: 1, y: 1}
+  m_BounceIntensity: 1
+  m_ColorTemperature: 6570
+  m_UseColorTemperature: 0
+  m_ShadowRadius: 0
+  m_ShadowAngle: 0
+--- !u!4 &560136683
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 560136681}
+  m_LocalRotation: {x: 0.40821788, y: -0.23456968, z: 0.10938163, w: 0.8754261}
+  m_LocalPosition: {x: 0, y: 3, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 1
+  m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0}
+--- !u!1 &2142185484
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 2142185487}
+  - component: {fileID: 2142185486}
+  - component: {fileID: 2142185485}
+  m_Layer: 0
+  m_Name: Main Camera
+  m_TagString: MainCamera
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!81 &2142185485
+AudioListener:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 2142185484}
+  m_Enabled: 1
+--- !u!20 &2142185486
+Camera:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 2142185484}
+  m_Enabled: 1
+  serializedVersion: 2
+  m_ClearFlags: 1
+  m_BackGroundColor: {r: 0.19215687, g: 0.3019608, b: 0.4745098, a: 0}
+  m_projectionMatrixMode: 1
+  m_SensorSize: {x: 36, y: 24}
+  m_LensShift: {x: 0, y: 0}
+  m_FocalLength: 50
+  m_NormalizedViewPortRect:
+    serializedVersion: 2
+    x: 0
+    y: 0
+    width: 1
+    height: 1
+  near clip plane: 0.3
+  far clip plane: 1000
+  field of view: 60
+  orthographic: 0
+  orthographic size: 5
+  m_Depth: -1
+  m_CullingMask:
+    serializedVersion: 2
+    m_Bits: 4294967295
+  m_RenderingPath: -1
+  m_TargetTexture: {fileID: 0}
+  m_TargetDisplay: 0
+  m_TargetEye: 3
+  m_HDR: 1
+  m_AllowMSAA: 1
+  m_AllowDynamicResolution: 0
+  m_ForceIntoRT: 0
+  m_OcclusionCulling: 1
+  m_StereoConvergence: 10
+  m_StereoSeparation: 0.022
+--- !u!4 &2142185487
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInternal: {fileID: 0}
+  m_GameObject: {fileID: 2142185484}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 1, z: -10}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 0
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}

OBJImport/Samples/ObjFromStream.unity.meta

+fileFormatVersion: 2
+guid: 0f928d9ce1b64b94f9b3d349956f8cd3
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/StringExtensions.cs

+namespace Dummiesman
+{
+    public static class StringExtensions
+    {
+        public static string Clean(this string str)
+        {
+            string rstr = str.Replace('\t', ' ');
+            while (rstr.Contains("  "))
+                rstr = rstr.Replace("  ", " ");
+            return rstr.Trim();
+        }
+    }
+}

OBJImport/StringExtensions.cs.meta

+fileFormatVersion: 2
+guid: 3ef7489c6f646f4448ffe979ea9857da
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader.meta

+fileFormatVersion: 2
+guid: ce592b27d670b994b8150b9ddd9fa341
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader/BMPLoader.cs

+#region License and Information
+/*****
+*
+* BMPLoader.cs
+* 
+* This is a simple implementation of a BMP file loader for Unity3D.
+* Formats it should support are:
+*  - 1 bit monochrome indexed
+*  - 2-8 bit indexed
+*  - 16 / 24 / 32 bit color (including "BI_BITFIELDS")
+*  - RLE-4 and RLE-8 support has been added.
+* 
+* Unless the type is "BI_ALPHABITFIELDS" the loader does not interpret alpha
+* values by default, however you can set the "ReadPaletteAlpha" setting to
+* true to interpret the 4th (usually "00") value as alpha for indexed images.
+* You can also set "ForceAlphaReadWhenPossible" to true so it will interpret
+* the "left over" bits as alpha if there are any. It will also force to read
+* alpha from a palette if it's an indexed image, just like "ReadPaletteAlpha".
+* 
+* It's not tested well to the bone, so there might be some errors somewhere.
+* However I tested it with 4 different images created with MS Paint
+* (1bit, 4bit, 8bit, 24bit) as those are the only formats supported.
+* 
+* 2017.02.05 - first version 
+* 2017.03.06 - Added RLE4 / RLE8 support
+* 
+* Copyright (c) 2017 Markus Göbel (Bunny83)
+* 
+* Permission is hereby granted, free of charge, to any person obtaining a copy
+* of this software and associated documentation files (the "Software"), to
+* deal in the Software without restriction, including without limitation the
+* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+* sell copies of the Software, and to permit persons to whom the Software is
+* furnished to do so, subject to the following conditions:
+* 
+* The above copyright notice and this permission notice shall be included in
+* all copies or substantial portions of the Software.
+* 
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+* IN THE SOFTWARE.
+* 
+*****/
+#endregion License and Information
+using UnityEngine;
+using System.Collections;
+using System.Collections.Generic;
+using System.IO;
+using System;
+
+namespace B83.Image.BMP
+{
+    public enum BMPComressionMode : int
+    {
+        BI_RGB = 0x00,
+        BI_RLE8 = 0x01,
+        BI_RLE4 = 0x02,
+        BI_BITFIELDS = 0x03,
+        BI_JPEG = 0x04,
+        BI_PNG = 0x05,
+        BI_ALPHABITFIELDS = 0x06,
+
+        BI_CMYK = 0x0B,
+        BI_CMYKRLE8 = 0x0C,
+        BI_CMYKRLE4 = 0x0D,
+
+    }
+    public struct BMPFileHeader
+    {
+        public ushort magic; // "BM"
+        public uint filesize;
+        public uint reserved;
+        public uint offset;
+    }
+    public struct BitmapInfoHeader
+    {
+        public uint size;
+        public int width;
+        public int height;
+        public ushort nColorPlanes; // always 1
+        public ushort nBitsPerPixel; // [1,4,8,16,24,32]
+        public BMPComressionMode compressionMethod;
+        public uint rawImageSize; // can be "0"
+        public int xPPM;
+        public int yPPM;
+        public uint nPaletteColors;
+        public uint nImportantColors;
+
+        public int absWidth { get { return Mathf.Abs(width); } }
+        public int absHeight { get { return Mathf.Abs(height); } }
+
+    }
+
+    public class BMPImage
+    {
+        public BMPFileHeader header;
+        public BitmapInfoHeader info;
+        public uint rMask = 0x00FF0000;
+        public uint gMask = 0x0000FF00;
+        public uint bMask = 0x000000FF;
+        public uint aMask = 0x00000000;
+        public List<Color32> palette;
+        public Color32[] imageData;
+        public Texture2D ToTexture2D()
+        {
+            var tex = new Texture2D(info.absWidth, info.absHeight);
+            tex.SetPixels32(imageData);
+            tex.Apply();
+            return tex;
+        }
+    }
+
+
+    public class BMPLoader
+    {
+        const ushort MAGIC = 0x4D42; // "BM" little endian
+        public bool ReadPaletteAlpha = false;
+        public bool ForceAlphaReadWhenPossible = false;
+
+        public BMPImage LoadBMP(string aFileName)
+        {
+            using (var file = File.OpenRead(aFileName))
+                return LoadBMP(file);
+        }
+        public BMPImage LoadBMP(byte[] aData)
+        {
+            using (var stream = new MemoryStream(aData))
+                return LoadBMP(stream);
+        }
+
+        public BMPImage LoadBMP(Stream aData)
+        {
+            using (var reader = new BinaryReader(aData))
+                return LoadBMP(reader);
+
+        }
+        public BMPImage LoadBMP(BinaryReader aReader)
+        {
+            BMPImage bmp = new BMPImage();
+            if (!ReadFileHeader(aReader, ref bmp.header))
+            {
+                Debug.LogError("Not a BMP file");
+                return null;
+            }
+            if (!ReadInfoHeader(aReader, ref bmp.info))
+            {
+                Debug.LogError("Unsupported header format");
+                return null;
+            }
+            if (bmp.info.compressionMethod != BMPComressionMode.BI_RGB
+                && bmp.info.compressionMethod != BMPComressionMode.BI_BITFIELDS
+                && bmp.info.compressionMethod != BMPComressionMode.BI_ALPHABITFIELDS
+                && bmp.info.compressionMethod != BMPComressionMode.BI_RLE4
+                && bmp.info.compressionMethod != BMPComressionMode.BI_RLE8
+                )
+            {
+                Debug.LogError("Unsupported image format: " + bmp.info.compressionMethod);
+                return null;
+            }
+            long offset = 14 + bmp.info.size;
+            aReader.BaseStream.Seek(offset, SeekOrigin.Begin);
+            if (bmp.info.nBitsPerPixel < 24)
+            {
+                bmp.rMask = 0x00007C00;
+                bmp.gMask = 0x000003E0;
+                bmp.bMask = 0x0000001F;
+            }
+
+            if (bmp.info.compressionMethod == BMPComressionMode.BI_BITFIELDS || bmp.info.compressionMethod == BMPComressionMode.BI_ALPHABITFIELDS)
+            {
+                bmp.rMask = aReader.ReadUInt32();
+                bmp.gMask = aReader.ReadUInt32();
+                bmp.bMask = aReader.ReadUInt32();
+            }
+            if (ForceAlphaReadWhenPossible)
+                bmp.aMask = GetMask(bmp.info.nBitsPerPixel) ^ (bmp.rMask | bmp.gMask | bmp.bMask);
+
+            if (bmp.info.compressionMethod == BMPComressionMode.BI_ALPHABITFIELDS)
+                bmp.aMask = aReader.ReadUInt32();
+
+            if (bmp.info.nPaletteColors > 0 || bmp.info.nBitsPerPixel <= 8)
+                bmp.palette = ReadPalette(aReader, bmp, ReadPaletteAlpha || ForceAlphaReadWhenPossible);
+
+
+            aReader.BaseStream.Seek(bmp.header.offset, SeekOrigin.Begin);
+            bool uncompressed = bmp.info.compressionMethod == BMPComressionMode.BI_RGB ||
+                bmp.info.compressionMethod == BMPComressionMode.BI_BITFIELDS ||
+                bmp.info.compressionMethod == BMPComressionMode.BI_ALPHABITFIELDS;
+            if (bmp.info.nBitsPerPixel == 32 && uncompressed)
+                Read32BitImage(aReader, bmp);
+            else if (bmp.info.nBitsPerPixel == 24 && uncompressed)
+                Read24BitImage(aReader, bmp);
+            else if (bmp.info.nBitsPerPixel == 16 && uncompressed)
+                Read16BitImage(aReader, bmp);
+            else if (bmp.info.compressionMethod == BMPComressionMode.BI_RLE4 && bmp.info.nBitsPerPixel == 4 && bmp.palette != null)
+                ReadIndexedImageRLE4(aReader, bmp);
+            else if (bmp.info.compressionMethod == BMPComressionMode.BI_RLE8 && bmp.info.nBitsPerPixel == 8 && bmp.palette != null)
+                ReadIndexedImageRLE8(aReader, bmp);
+            else if (uncompressed && bmp.info.nBitsPerPixel <= 8 && bmp.palette != null)
+                ReadIndexedImage(aReader, bmp);
+            else
+            {
+                Debug.LogError("Unsupported file format: " + bmp.info.compressionMethod + " BPP: " + bmp.info.nBitsPerPixel);
+                return null;
+            }
+            return bmp;
+        }
+
+
+        private static void Read32BitImage(BinaryReader aReader, BMPImage bmp)
+        {
+            int w = Mathf.Abs(bmp.info.width);
+            int h = Mathf.Abs(bmp.info.height);
+            Color32[] data = bmp.imageData = new Color32[w * h];
+            if (aReader.BaseStream.Position + w * h * 4 > aReader.BaseStream.Length)
+            {
+                Debug.LogError("Unexpected end of file.");
+                return;
+            }
+            int shiftR = GetShiftCount(bmp.rMask);
+            int shiftG = GetShiftCount(bmp.gMask);
+            int shiftB = GetShiftCount(bmp.bMask);
+            int shiftA = GetShiftCount(bmp.aMask);
+            byte a = 255;
+            for (int i = 0; i < data.Length; i++)
+            {
+                uint v = aReader.ReadUInt32();
+                byte r = (byte)((v & bmp.rMask) >> shiftR);
+                byte g = (byte)((v & bmp.gMask) >> shiftG);
+                byte b = (byte)((v & bmp.bMask) >> shiftB);
+                if (bmp.bMask != 0)
+                    a = (byte)((v & bmp.aMask) >> shiftA);
+                data[i] = new Color32(r, g, b, a);
+            }
+        }
+
+
+        private static void Read24BitImage(BinaryReader aReader, BMPImage bmp)
+        {
+            int w = Mathf.Abs(bmp.info.width);
+            int h = Mathf.Abs(bmp.info.height);
+            int rowLength = ((24 * w + 31) / 32) * 4;
+            int count = rowLength * h;
+            int pad = rowLength - w * 3;
+            Color32[] data = bmp.imageData = new Color32[w * h];
+            if (aReader.BaseStream.Position + count > aReader.BaseStream.Length)
+            {
+                Debug.LogError("Unexpected end of file. (Have " + (aReader.BaseStream.Position + count) + " bytes, expected " + aReader.BaseStream.Length + " bytes)");
+                return;
+            }
+            int shiftR = GetShiftCount(bmp.rMask);
+            int shiftG = GetShiftCount(bmp.gMask);
+            int shiftB = GetShiftCount(bmp.bMask);
+            for (int y = 0; y < h; y++)
+            {
+                for (int x = 0; x < w; x++)
+                {
+                    uint v = aReader.ReadByte() | ((uint)aReader.ReadByte() << 8) | ((uint)aReader.ReadByte() << 16);
+                    byte r = (byte)((v & bmp.rMask) >> shiftR);
+                    byte g = (byte)((v & bmp.gMask) >> shiftG);
+                    byte b = (byte)((v & bmp.bMask) >> shiftB);
+                    data[x + y * w] = new Color32(r, g, b, 255);
+                }
+                for (int i = 0; i < pad; i++)
+                    aReader.ReadByte();
+            }
+        }
+
+        private static void Read16BitImage(BinaryReader aReader, BMPImage bmp)
+        {
+            int w = Mathf.Abs(bmp.info.width);
+            int h = Mathf.Abs(bmp.info.height);
+            int rowLength = ((16 * w + 31) / 32) * 4;
+            int count = rowLength * h;
+            int pad = rowLength - w * 2;
+            Color32[] data = bmp.imageData = new Color32[w * h];
+            if (aReader.BaseStream.Position + count > aReader.BaseStream.Length)
+            {
+                Debug.LogError("Unexpected end of file. (Have " + (aReader.BaseStream.Position + count) + " bytes, expected " + aReader.BaseStream.Length + " bytes)");
+                return;
+            }
+            int shiftR = GetShiftCount(bmp.rMask);
+            int shiftG = GetShiftCount(bmp.gMask);
+            int shiftB = GetShiftCount(bmp.bMask);
+            int shiftA = GetShiftCount(bmp.aMask);
+            byte a = 255;
+            for (int y = 0; y < h; y++)
+            {
+                for (int x = 0; x < w; x++)
+                {
+                    uint v = aReader.ReadByte() | ((uint)aReader.ReadByte() << 8);
+                    byte r = (byte)((v & bmp.rMask) >> shiftR);
+                    byte g = (byte)((v & bmp.gMask) >> shiftG);
+                    byte b = (byte)((v & bmp.bMask) >> shiftB);
+                    if (bmp.aMask != 0)
+                        a = (byte)((v & bmp.aMask) >> shiftA);
+                    data[x + y * w] = new Color32(r, g, b, a);
+                }
+                for (int i = 0; i < pad; i++)
+                    aReader.ReadByte();
+            }
+        }
+
+        private static void ReadIndexedImage(BinaryReader aReader, BMPImage bmp)
+        {
+            int w = Mathf.Abs(bmp.info.width);
+            int h = Mathf.Abs(bmp.info.height);
+            int bitCount = bmp.info.nBitsPerPixel;
+            int rowLength = ((bitCount * w + 31) / 32) * 4;
+            int count = rowLength * h;
+            int pad = rowLength - (w * bitCount + 7) / 8;
+            Color32[] data = bmp.imageData = new Color32[w * h];
+            if (aReader.BaseStream.Position + count > aReader.BaseStream.Length)
+            {
+                Debug.LogError("Unexpected end of file. (Have " + (aReader.BaseStream.Position + count) + " bytes, expected " + aReader.BaseStream.Length + " bytes)");
+                return;
+            }
+            BitStreamReader bitReader = new BitStreamReader(aReader);
+            for (int y = 0; y < h; y++)
+            {
+                for (int x = 0; x < w; x++)
+                {
+                    int v = (int)bitReader.ReadBits(bitCount);
+                    if (v >= bmp.palette.Count)
+                    {
+                        Debug.LogError("Indexed bitmap has indices greater than it's color palette");
+                        return;
+                    }
+                    data[x + y * w] = bmp.palette[v];
+                }
+                bitReader.Flush();
+                for (int i = 0; i < pad; i++)
+                    aReader.ReadByte();
+            }
+        }
+        private static void ReadIndexedImageRLE4(BinaryReader aReader, BMPImage bmp)
+        {
+            int w = Mathf.Abs(bmp.info.width);
+            int h = Mathf.Abs(bmp.info.height);
+            Color32[] data = bmp.imageData = new Color32[w * h];
+            int x = 0;
+            int y = 0;
+            int yOffset = 0;
+            while (aReader.BaseStream.Position < aReader.BaseStream.Length - 1)
+            {
+                int count = (int)aReader.ReadByte();
+                byte d = aReader.ReadByte();
+                if (count > 0)
+                {
+                    for (int i = (count / 2); i > 0; i--)
+                    {
+                        data[x++ + yOffset] = bmp.palette[(d >> 4) & 0x0F];
+                        data[x++ + yOffset] = bmp.palette[d & 0x0F];
+                    }
+                    if ((count & 0x01) > 0)
+                    {
+                        data[x++ + yOffset] = bmp.palette[(d >> 4) & 0x0F];
+                    }
+                }
+                else
+                {
+                    if (d == 0)
+                    {
+                        x = 0;
+                        y += 1;
+                        yOffset = y * w;
+                    }
+                    else if (d == 1)
+                    {
+                        break;
+                    }
+                    else if (d == 2)
+                    {
+                        x += aReader.ReadByte();
+                        y += aReader.ReadByte();
+                        yOffset = y * w;
+                    }
+                    else
+                    {
+                        for (int i = (d / 2); i > 0; i--)
+                        {
+                            byte d2 = aReader.ReadByte();
+                            data[x++ + yOffset] = bmp.palette[(d2 >> 4) & 0x0F];
+                            data[x++ + yOffset] = bmp.palette[d2 & 0x0F];
+                        }
+                        if ((d & 0x01) > 0)
+                        {
+                            data[x++ + yOffset] = bmp.palette[(aReader.ReadByte() >> 4) & 0x0F];
+                        }
+                        if ((((d - 1) / 2) & 1) == 0)
+                        {
+                            aReader.ReadByte(); // padding (word alignment)
+                        }
+                    }
+                }
+            }
+        }
+        private static void ReadIndexedImageRLE8(BinaryReader aReader, BMPImage bmp)
+        {
+            int w = Mathf.Abs(bmp.info.width);
+            int h = Mathf.Abs(bmp.info.height);
+            Color32[] data = bmp.imageData = new Color32[w * h];
+            int x = 0;
+            int y = 0;
+            int yOffset = 0;
+            while (aReader.BaseStream.Position < aReader.BaseStream.Length - 1)
+            {
+                int count = (int)aReader.ReadByte();
+                byte d = aReader.ReadByte();
+                if (count > 0)
+                {
+                    for (int i = count; i > 0; i--)
+                    {
+                        data[x++ + yOffset] = bmp.palette[d];
+                    }
+                }
+                else
+                {
+                    if (d == 0)
+                    {
+                        x = 0;
+                        y += 1;
+                        yOffset = y * w;
+                    }
+                    else if (d == 1)
+                    {
+                        break;
+                    }
+                    else if (d == 2)
+                    {
+                        x += aReader.ReadByte();
+                        y += aReader.ReadByte();
+                        yOffset = y * w;
+                    }
+                    else
+                    {
+                        for (int i = d; i > 0; i--)
+                        {
+                            data[x++ + yOffset] = bmp.palette[aReader.ReadByte()];
+                        }
+                        if ((d & 0x01) > 0)
+                        {
+                            aReader.ReadByte(); // padding (word alignment)
+                        }
+                    }
+                }
+            }
+        }
+        private static int GetShiftCount(uint mask)
+        {
+            for (int i = 0; i < 32; i++)
+            {
+                if ((mask & 0x01) > 0)
+                    return i;
+                mask >>= 1;
+            }
+            return -1;
+        }
+        private static uint GetMask(int bitCount)
+        {
+            uint mask = 0;
+            for (int i = 0; i < bitCount; i++)
+            {
+                mask <<= 1;
+                mask |= 0x01;
+            }
+            return mask;
+        }
+        private static bool ReadFileHeader(BinaryReader aReader, ref BMPFileHeader aFileHeader)
+        {
+            aFileHeader.magic = aReader.ReadUInt16();
+            if (aFileHeader.magic != MAGIC)
+                return false;
+            aFileHeader.filesize = aReader.ReadUInt32();
+            aFileHeader.reserved = aReader.ReadUInt32();
+            aFileHeader.offset = aReader.ReadUInt32();
+            return true;
+        }
+        private static bool ReadInfoHeader(BinaryReader aReader, ref BitmapInfoHeader aHeader)
+        {
+            aHeader.size = aReader.ReadUInt32();
+            if (aHeader.size < 40)
+                return false;
+            aHeader.width = aReader.ReadInt32();
+            aHeader.height = aReader.ReadInt32();
+            aHeader.nColorPlanes = aReader.ReadUInt16();
+            aHeader.nBitsPerPixel = aReader.ReadUInt16();
+            aHeader.compressionMethod = (BMPComressionMode)aReader.ReadInt32();
+            aHeader.rawImageSize = aReader.ReadUInt32();
+            aHeader.xPPM = aReader.ReadInt32();
+            aHeader.yPPM = aReader.ReadInt32();
+            aHeader.nPaletteColors = aReader.ReadUInt32();
+            aHeader.nImportantColors = aReader.ReadUInt32();
+            int pad = (int)aHeader.size - 40;
+            if (pad > 0)
+                aReader.ReadBytes(pad);
+            return true;
+        }
+        public static List<Color32> ReadPalette(BinaryReader aReader, BMPImage aBmp, bool aReadAlpha)
+        {
+            uint count = aBmp.info.nPaletteColors;
+            if (count == 0u)
+                count = 1u << aBmp.info.nBitsPerPixel;
+            var palette = new List<Color32>((int)count);
+            for (int i = 0; i < count; i++)
+            {
+                byte b = aReader.ReadByte();
+                byte g = aReader.ReadByte();
+                byte r = aReader.ReadByte();
+                byte a = aReader.ReadByte();
+                if (!aReadAlpha)
+                    a = 255;
+                palette.Add(new Color32(r, g, b, a));
+            }
+            return palette;
+        }
+
+    }
+    public class BitStreamReader
+    {
+        BinaryReader m_Reader;
+        byte m_Data = 0;
+        int m_Bits = 0;
+
+        public BitStreamReader(BinaryReader aReader)
+        {
+            m_Reader = aReader;
+        }
+        public BitStreamReader(Stream aStream) : this(new BinaryReader(aStream)) { }
+
+        public byte ReadBit()
+        {
+            if (m_Bits <= 0)
+            {
+                m_Data = m_Reader.ReadByte();
+                m_Bits = 8;
+            }
+            return (byte)((m_Data >> --m_Bits) & 1);
+        }
+
+        public ulong ReadBits(int aCount)
+        {
+            ulong val = 0UL;
+            if (aCount <= 0 || aCount > 32)
+                throw new System.ArgumentOutOfRangeException("aCount", "aCount must be between 1 and 32 inclusive");
+            for (int i = aCount - 1; i >= 0; i--)
+                val |= ((ulong)ReadBit() << i);
+            return val;
+        }
+        public void Flush()
+        {
+            m_Data = 0;
+            m_Bits = 0;
+        }
+    }
+}
\ No newline at end of file

OBJImport/TextureLoader/BMPLoader.cs.meta

+fileFormatVersion: 2
+guid: 27d41db6b761bbb4a989566820e47137
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader/BinaryExtensions.cs

+using System.Collections;
+using System.Collections.Generic;
+using System.IO;
+using UnityEngine;
+
+namespace Dummiesman
+{
+    public static class BinaryExtensions
+    {
+        public static Color32 ReadColor32RGBR(this BinaryReader r)
+        {
+            var bytes = r.ReadBytes(4);
+            return new Color32(bytes[0], bytes[1], bytes[2], 255);
+        }
+
+        public static Color32 ReadColor32RGBA(this BinaryReader r)
+        {
+            var bytes = r.ReadBytes(4);
+            return new Color32(bytes[0], bytes[1], bytes[2], bytes[3]);
+        }
+
+        public static Color32 ReadColor32RGB(this BinaryReader r)
+        {
+            var bytes = r.ReadBytes(3);
+            return new Color32(bytes[0], bytes[1], bytes[2], 255);
+        }
+
+        public static Color32 ReadColor32BGR(this BinaryReader r)
+        {
+            var bytes = r.ReadBytes(3);
+            return new Color32(bytes[2], bytes[1], bytes[0], 255);
+        }
+    }
+}
\ No newline at end of file

OBJImport/TextureLoader/BinaryExtensions.cs.meta

+fileFormatVersion: 2
+guid: 5b61b2ce827ad8842a6812f45f25aaa1
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader/ColorExtensions.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Dummiesman.Extensions
+{
+    public static class ColorExtensions
+    {
+        public static Color FlipRB(this Color color)
+        {
+            return new Color(color.b, color.g, color.r, color.a);
+        }
+
+        public static Color32 FlipRB(this Color32 color)
+        {
+            return new Color32(color.b, color.g, color.r, color.a);
+        }
+    }
+}
\ No newline at end of file

OBJImport/TextureLoader/ColorExtensions.cs.meta

+fileFormatVersion: 2
+guid: 8034e4bf991d15a4b88970b534ce4a0f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader/DDSLoader.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.IO;
+using System.Linq;
+using UnityEngine;
+
+namespace Dummiesman
+{
+    public static class DDSLoader
+    {
+        public static Texture2D Load(Stream ddsStream)
+        {
+            byte[] buffer = new byte[ddsStream.Length];
+            ddsStream.Read(buffer, 0, (int)ddsStream.Length);
+            return Load(buffer);
+        }
+
+        public static Texture2D Load(string ddsPath)
+        {
+           return Load(File.ReadAllBytes(ddsPath));
+        }
+
+        public static Texture2D Load(byte[] ddsBytes)
+        {
+            try
+            {
+
+                //do size check
+                byte ddsSizeCheck = ddsBytes[4];
+                if (ddsSizeCheck != 124)
+                    throw new System.Exception("Invalid DDS header. Structure length is incrrrect."); //this header byte should be 124 for DDS image files
+
+                //verify we have a readable tex
+                byte DXTType = ddsBytes[87];
+                if (DXTType != 49 && DXTType != 53)
+                    throw new System.Exception("Cannot load DDS due to an unsupported pixel format. Needs to be DXT1 or DXT5.");
+
+                int height = ddsBytes[13] * 256 + ddsBytes[12];
+                int width = ddsBytes[17] * 256 + ddsBytes[16];
+                bool mipmaps = ddsBytes[28] > 0;
+                TextureFormat textureFormat = DXTType == 49 ? TextureFormat.DXT1 : TextureFormat.DXT5;
+
+                int DDS_HEADER_SIZE = 128;
+                byte[] dxtBytes = new byte[ddsBytes.Length - DDS_HEADER_SIZE];
+                Buffer.BlockCopy(ddsBytes, DDS_HEADER_SIZE, dxtBytes, 0, ddsBytes.Length - DDS_HEADER_SIZE);
+
+                Texture2D texture = new Texture2D(width, height, textureFormat, mipmaps);
+                texture.LoadRawTextureData(dxtBytes);
+                texture.Apply();
+
+                return texture;
+            }
+            catch (System.Exception ex)
+            {
+                throw new Exception("An error occured while loading DirectDraw Surface: " + ex.Message);
+            }
+        }
+    }
+}

OBJImport/TextureLoader/DDSLoader.cs.meta

+fileFormatVersion: 2
+guid: 1f9e65e68712e0f41aae633289ab739e
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader/ImageLoader.cs

+/*
+ * Created by Dummiesman 2013-2019
+ * Thanks to mikezila for improving the initial TGA loading code
+*/
+
+using System;
+using UnityEngine;
+using System.Collections;
+using System.IO;
+using B83.Image.BMP;
+
+namespace Dummiesman
+{
+    public class ImageLoader
+    {
+        /// <summary>
+        /// Converts a DirectX normal map to Unitys expected format
+        /// </summary>
+        /// <param name="tex">Texture to convert</param>
+        public static void SetNormalMap(ref Texture2D tex)
+        {
+            Color[] pixels = tex.GetPixels();
+            for (int i = 0; i < pixels.Length; i++)
+            {
+                Color temp = pixels[i];
+                temp.r = pixels[i].g;
+                temp.a = pixels[i].r;
+                pixels[i] = temp;
+            }
+            tex.SetPixels(pixels);
+            tex.Apply(true);
+        }
+
+        public enum TextureFormat
+        {
+            DDS,
+            TGA,
+            BMP,
+            PNG,
+            JPG,
+            CRN
+        }
+
+
+        /// <summary>
+        /// Loads a texture from a stream
+        /// </summary>
+        /// <param name="stream">The stream</param>
+        /// <param name="format">The format **NOT UNITYENGINE.TEXTUREFORMAT**</param>
+        /// <returns></returns>
+        public static Texture2D LoadTexture(Stream stream, TextureFormat format)
+        {
+            if (format == TextureFormat.BMP)
+            {
+                return new BMPLoader().LoadBMP(stream).ToTexture2D();
+            }
+            else if (format == TextureFormat.DDS)
+            {
+                return DDSLoader.Load(stream);
+            }
+            else if (format == TextureFormat.JPG || format == TextureFormat.PNG)
+            {
+                byte[] buffer = new byte[stream.Length];
+                stream.Read(buffer, 0, (int)stream.Length);
+
+                Texture2D texture = new Texture2D(1, 1);
+                texture.LoadImage(buffer);
+                return texture;
+            }
+            else if (format == TextureFormat.TGA)
+            {
+                return TGALoader.Load(stream);
+            }
+            else
+            {
+                return null;
+            }
+        }
+
+      
+        /// <summary>
+        /// Loads a texture from a file
+        /// </summary>
+        /// <param name="fn"></param>
+        /// <param name="normalMap"></param>
+        /// <returns></returns>
+        public static Texture2D LoadTexture(string fn)
+        {
+            if (!File.Exists(fn))
+                return null;
+
+            var textureBytes = File.ReadAllBytes(fn);
+            string ext = Path.GetExtension(fn).ToLower();
+            string name = Path.GetFileName(fn);
+            Texture2D returnTex = null;
+
+            switch (ext)
+            {
+                case ".png":
+                case ".jpg":
+                case ".jpeg":
+                    returnTex = new Texture2D(1, 1);
+                    returnTex.LoadImage(textureBytes);
+                    break;
+                case ".dds":
+                    returnTex = DDSLoader.Load(textureBytes);
+                    break;
+                case ".tga":
+                    returnTex = TGALoader.Load(textureBytes);
+                    break;
+                case ".bmp":
+                    returnTex = new BMPLoader().LoadBMP(textureBytes).ToTexture2D();
+                    break;
+                case ".crn":
+                    byte[] crnBytes = textureBytes;
+                    ushort crnWidth = System.BitConverter.ToUInt16(new byte[2] { crnBytes[13], crnBytes[12] }, 0);
+                    ushort crnHeight = System.BitConverter.ToUInt16(new byte[2] { crnBytes[15], crnBytes[14] }, 0);
+                    byte crnFormatByte = crnBytes[18];
+
+                    var crnTextureFormat = UnityEngine.TextureFormat.RGB24;
+                    if (crnFormatByte == 0)
+                    {
+                        crnTextureFormat = UnityEngine.TextureFormat.DXT1Crunched;
+                    }else if(crnFormatByte == 2)
+                    {
+                        crnTextureFormat = UnityEngine.TextureFormat.DXT5Crunched;
+                    }
+                    else if(crnFormatByte == 12)
+                    {
+                        crnTextureFormat = UnityEngine.TextureFormat.ETC2_RGBA8Crunched;
+                    }
+                    else
+                    {
+                        Debug.LogError("Could not load crunched texture " + name + " because its format is not supported (" + crnFormatByte + "): " + fn);
+                        break;
+                    }
+
+                    returnTex = new Texture2D(crnWidth, crnHeight, crnTextureFormat, true);
+                    returnTex.LoadRawTextureData(crnBytes);
+                    returnTex.Apply(true);
+
+                    break;
+                default:
+                    Debug.LogError("Could not load texture " + name + " because its format is not supported : " + fn);
+                    break;
+            }
+            
+            if (returnTex != null)
+            {
+                returnTex = ImageLoaderHelper.VerifyFormat(returnTex);
+                returnTex.name = Path.GetFileNameWithoutExtension(fn);
+            }                                           
+
+            return returnTex;
+        }
+
+    }
+}
\ No newline at end of file

OBJImport/TextureLoader/ImageLoader.cs.meta

+fileFormatVersion: 2
+guid: 075edea996ccdd6489ad5e1197dd3f67
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader/ImageLoaderHelper.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Runtime.InteropServices;
+using UnityEngine;
+
+namespace Dummiesman
+{
+    public class ImageLoaderHelper
+    {
+        /// <summary>
+        /// Verifies that a 32bpp texture is actuall 32bpp
+        /// </summary>
+        /// <returns>The verified texture</returns>
+        public static Texture2D VerifyFormat(Texture2D tex)
+        {
+            if (tex.format != UnityEngine.TextureFormat.ARGB32 && tex.format != UnityEngine.TextureFormat.RGBA32 && tex.format != UnityEngine.TextureFormat.DXT5)
+                return tex;
+
+            //get pixels
+            var pixels = tex.GetPixels32();
+            bool validFormat = false;
+
+            //check each pixel alpha
+            foreach(var px in pixels)
+            {
+                if(px.a < 255)
+                {
+                    validFormat = true;
+                    break;
+                }
+            }
+
+            //if it's not a valid format return a new 24bpp image
+            if (!validFormat)
+            {
+                var tex24 = new Texture2D(tex.width, tex.height, UnityEngine.TextureFormat.RGB24, tex.mipmapCount > 0);
+                tex24.SetPixels32(pixels);
+                tex24.Apply(true);
+                return tex24;
+            }
+
+            //return original if valid
+            return tex;
+        }
+
+        /// <summary>
+        /// A cluster for creating arrays of Unity Color 
+        /// </summary>
+        /// <param name="fillArray"></param>
+        /// <param name="pixelData"></param>
+        /// <param name="bytesPerPixel"></param>
+        /// <param name="bgra"></param>
+        public static void FillPixelArray(Color32[] fillArray, byte[] pixelData, int bytesPerPixel, bool bgra = false)
+        {
+            //special case for TGA :(
+            if (bgra)
+            {
+                if (bytesPerPixel == 4)
+                {
+                    for (int i = 0; i < fillArray.Length; i++)
+                    {
+                        int bi = i * bytesPerPixel;
+                        fillArray[i] = new Color32(pixelData[bi + 2], pixelData[bi + 1], pixelData[bi], pixelData[bi + 3]);
+                    }
+                }
+                else
+                {
+                    //24 bit BGR to Color32 (RGBA)
+                    //this is faster than safe code
+                    for (int i = 0; i < fillArray.Length; i++)
+                    {
+                        fillArray[i].r = pixelData[(i * 3) + 2];
+                        fillArray[i].g = pixelData[(i * 3) + 1];
+                        fillArray[i].b = pixelData[(i * 3) + 0];
+                    }
+                }
+            }
+            else
+            {
+                if (bytesPerPixel == 4)
+                {
+                    for (int i = 0; i < fillArray.Length; i++)
+                    {
+                        fillArray[i].r = pixelData[i * 4];
+                        fillArray[i].g = pixelData[(i * 4) + 1];
+                        fillArray[i].b = pixelData[(i * 4) + 2];
+                        fillArray[i].a = pixelData[(i * 4) + 3];
+                    }
+                }
+                else
+                {
+                    //with RGB we can't! :(
+                    int bi = 0;
+                    for (int i = 0; i < fillArray.Length; i++)
+                    {
+                        fillArray[i].r = pixelData[bi++];
+                        fillArray[i].g = pixelData[bi++];
+                        fillArray[i].b = pixelData[bi++];
+                        fillArray[i].a = 255;
+                    }
+                }
+            }
+        }
+    }
+}
\ No newline at end of file

OBJImport/TextureLoader/ImageLoaderHelper.cs.meta

+fileFormatVersion: 2
+guid: b8658e22fc9a3c94ab5d8c6460812b69
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader/ImageUtils.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Dummiesman
+{
+    public static class ImageUtils
+    {
+        public static Texture2D ConvertToNormalMap(Texture2D tex)
+        {
+            Texture2D returnTex = tex;
+            if(tex.format != TextureFormat.RGBA32 && tex.format != TextureFormat.ARGB32)
+            {
+                returnTex = new Texture2D(tex.width, tex.height, TextureFormat.RGBA32, true);
+            }
+
+            Color[] pixels = tex.GetPixels();
+            for (int i = 0; i < pixels.Length; i++)
+            {
+                Color temp = pixels[i];
+                temp.a = pixels[i].r;
+                temp.r = 0f;
+                temp.g = pixels[i].g;
+                temp.b = 0f;
+                pixels[i] = temp;
+            }
+            
+            returnTex.SetPixels(pixels);
+            returnTex.Apply(true);
+            return returnTex;
+        }
+        
+    }
+}
+

OBJImport/TextureLoader/ImageUtils.cs.meta

+fileFormatVersion: 2
+guid: 594f0c9c36902bc4aa1e6fd12ae28fd0
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

OBJImport/TextureLoader/TGALoader.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using System.IO;
+using System;
+using Dummiesman.Extensions;
+using System.Runtime.InteropServices;
+
+namespace Dummiesman
+{
+    public class TGALoader
+    {
+        private static int GetBits(byte b, int offset, int count)
+        {
+            return (b >> offset) & ((1 << count) - 1);
+        }
+
+        private static Color32[] LoadRawTGAData(BinaryReader r, int bitDepth, int width, int height)
+        {
+            Color32[] pulledColors = new Color32[width * height];
+
+            byte[] colorData = r.ReadBytes(width * height * (bitDepth / 8));
+            ImageLoaderHelper.FillPixelArray(pulledColors, colorData, (bitDepth / 8), true);
+
+            return pulledColors;
+        }
+
+        private static Color32[] LoadRLETGAData(BinaryReader r, int bitDepth, int width, int height)
+        {
+            Color32[] pulledColors = new Color32[width * height];
+            int pulledColorCount = 0;
+
+            while (pulledColorCount < pulledColors.Length)
+            {
+                byte rlePacket = r.ReadByte();
+                int RLEPacketType = GetBits(rlePacket, 7, 1);
+                int RLEPixelCount = GetBits(rlePacket, 0, 7) + 1;
+
+
+                if (RLEPacketType == 0)
+                {
+                    //raw packet
+                    for (int i = 0; i < RLEPixelCount; i++)
+                    {
+                        var color = (bitDepth == 32) ? r.ReadColor32RGBA().FlipRB() : r.ReadColor32RGB().FlipRB();
+                        pulledColors[i + pulledColorCount] = color;
+                    }
+
+                }
+                else
+                {
+                    //rle packet
+                    var color = (bitDepth == 32) ? r.ReadColor32RGBA().FlipRB() : r.ReadColor32RGB().FlipRB();
+
+                    for (int i = 0; i < RLEPixelCount; i++)
+                    {
+                        pulledColors[i + pulledColorCount] = color;
+                    }
+                }
+
+                pulledColorCount += RLEPixelCount;
+            }
+
+            return pulledColors;
+        }
+
+        public static Texture2D Load(string fileName)
+        {
+            using (var imageFile = File.OpenRead(fileName))
+            {
+                return Load(imageFile);
+            }
+        }
+
+        public static Texture2D Load(byte[] bytes)
+        {
+            using (var ms = new MemoryStream(bytes))
+            {
+                return Load(ms);
+            }
+        }
+
+        public static Texture2D Load(Stream TGAStream)
+        {
+
+            using (BinaryReader r = new BinaryReader(TGAStream))
+            {
+                // Skip some header info we don't care about.
+                r.BaseStream.Seek(2, SeekOrigin.Begin);
+
+                byte imageType = r.ReadByte();
+                if (imageType != 10 && imageType != 2)
+                {
+                    Debug.LogError($"Unsupported targa image type. ({imageType})");
+                    return null;
+                }
+
+                //Skip right to some more data we need
+                r.BaseStream.Seek(12, SeekOrigin.Begin);
+
+                short width = r.ReadInt16();
+                short height = r.ReadInt16();
+                int bitDepth = r.ReadByte();
+
+                if (bitDepth < 24)
+                    throw new Exception("Tried to load TGA with unsupported bit depth");
+
+                // Skip a byte of header information we don't care about.
+                r.BaseStream.Seek(1, SeekOrigin.Current);
+
+                Texture2D tex = new Texture2D(width, height, (bitDepth == 24) ? TextureFormat.RGB24 :  TextureFormat.ARGB32, true);
+                if (imageType == 2)
+                {
+                    tex.SetPixels32(LoadRawTGAData(r, bitDepth, width, height));
+                }
+                else
+                {
+                    tex.SetPixels32(LoadRLETGAData(r, bitDepth, width, height));
+                }
+
+                tex.Apply();
+                return tex;
+
+            }
+        }
+    }
+}

OBJImport/TextureLoader/TGALoader.cs.meta

+fileFormatVersion: 2
+guid: 1b682979498a095418396a690d23c0b0
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

ObjectSetter.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+using BSM = BulletSharp.Math;
+
+namespace GuidewireSim
+{
+/**
+ * This class is responsible for setting the transformation positions of the GameObjects in the scene to their respective simulation
+ * data like @p spherePositions.
+ */
+public class ObjectSetter : MonoBehaviour
+{
+    MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
+
+    private void Awake()
+    {
+        mathHelper = GetComponent<MathHelper>();
+        Assert.IsNotNull(mathHelper);
+    }
+
+    /**
+     * Sets the positions of the GameObjects @p spheres to their respective @p spherePositions.
+     * @param spheres All spheres that are part of the guidewire.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param spherePositions The position at the current frame of each sphere.
+     */
+    public void SetSpherePositions(GameObject[] spheres, int spheresCount, Vector3[] spherePositions)
+    {
+        for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
+        {
+            spheres[sphereIndex].transform.position = spherePositions[sphereIndex];
+        }
+    }
+
+    /**
+     * Sets the positions of the GameObjects @p cylinders to their respective @p cylinderPositions.
+     * @param cylinders All cylinders that are part of the guidewire.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param cylinderPositions The position/ center of mass of each cylinder.
+     */
+    public void SetCylinderPositions(GameObject[] cylinders, int cylinderCount, Vector3[] cylinderPositions)
+    {
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            cylinders[cylinderIndex].transform.position = cylinderPositions[cylinderIndex];
+        }
+    }
+
+    /**
+     * Rotates each cylinder GameObject such that its centerline is parallel with the line segment that is spanned by the two adjacent
+     * sphere's center of masses.
+     * @param cylinders All cylinders that are part of the guidewire.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param cylinderOrientations The orientation of each cylinder at its center of mass.
+     * @param directors The orthonormal basis of each orientation element / cylinder, also called directors.
+     * @note @p appliedTransformation is the rotation that aligns the y-axis of the cylinder with the z-axis of the orientations
+     * (the third director). This is needed, because the y-axis of the cylinder is parallel with its centerline, while the z-axis
+     * of the orientations (the third director) is also defined as being parallel with the cylinder's centerline. Thus @p appliedTransformation
+     * is necessary.
+     */
+    public void SetCylinderOrientations(GameObject[] cylinders, int cylinderCount, BSM.Quaternion[] cylinderOrientations, Vector3[][] directors)
+    {
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            Quaternion cylinderOrientation = Quaternion.LookRotation(directors[2][cylinderIndex]);
+
+            cylinders[cylinderIndex].transform.rotation = cylinderOrientation;
+        }
+    }
+}
+}
\ No newline at end of file

ObjectSetter.cs.meta

+fileFormatVersion: 2
+guid: 77a1cbfa612b03d4b859875eea4b7e3e
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

PredictionStep.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+using BSM = BulletSharp.Math;
+
+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 void Awake()
+    {
+        mathHelper = GetComponent<MathHelper>();
+        Assert.IsNotNull(mathHelper);
+    }
+
+    /**
+     * Calculates the predictions for the sphere velocities for the prediction step of the algorithm.
+     * @param sphereVelocities The velocity of the current frame of each sphere.
+     * @param sphereInverseMasses The constant inverse masses  of each sphere.
+     * @param sphereExternalForces The sum of all current external forces that are applied per particle/ sphere.
+     * @return The predictions of the positions of the spheres, i.e. @p spherePositionPredictions.
+     * @note The predictions are again stored in @p sphereVelocities.
+     */
+    public Vector3[] PredictSphereVelocities(Vector3[] sphereVelocities, float[] sphereInverseMasses, Vector3[] sphereExternalForces)
+    {
+        Vector3 calc = Time.deltaTime * sphereInverseMasses[1] * sphereExternalForces[1];
+
+        for (int sphereIndex = 0; sphereIndex < sphereVelocities.Length; sphereIndex++)
+        {
+            sphereVelocities[sphereIndex] += Time.deltaTime * sphereInverseMasses[sphereIndex] * sphereExternalForces[sphereIndex];
+        }
+
+        return sphereVelocities;
+    }
+
+    /**
+     * Calculates the predictions for the sphere positions for the prediction step of the algorithm.
+     * @param spherePositionPredictions The prediction of the position at the current frame of each sphere (in this case of the last frame).
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param spherePositions The position at the current frame of each sphere.
+     * @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++)
+        {
+            spherePositionPredictions[sphereIndex] = spherePositions[sphereIndex] + Time.deltaTime * sphereVelocities[sphereIndex];
+        }
+
+        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.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param inertiaTensor The inertia tensor. Entries are approximates as in the CoRdE paper.
+     * @param cylinderExternalTorques The sum of all current external torques that are applied per orientation element/ cylinder.
+     * @param inverseInertiaTensor The inverse of @p inertiaTensor.
+     * @return The angular velocity of the current frame of each orientation element/ cylinder, i.e. @p cylinderAngularVelocities.
+     * @note The predictions are again stored in @p cylinderAngularVelocities.
+     */
+    public Vector3[] PredictAngularVelocities(Vector3[] cylinderAngularVelocities, int cylinderCount, float[,] inertiaTensor,
+                                              Vector3[] cylinderExternalTorques, float[,] inverseInertiaTensor)
+    {
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            Vector3 Iw = mathHelper.MatrixVectorMultiplication(inertiaTensor, cylinderAngularVelocities[cylinderIndex]);
+            Vector3 wXIw = Vector3.Cross(cylinderAngularVelocities[cylinderIndex], Iw);
+            Vector3 TauwXIw = cylinderExternalTorques[cylinderIndex] - wXIw;
+            Vector3 ITauwXIw = mathHelper.MatrixVectorMultiplication(inverseInertiaTensor, TauwXIw);
+            Vector3 summand = Time.deltaTime * ITauwXIw;
+            cylinderAngularVelocities[cylinderIndex] += summand;
+        }
+
+        return cylinderAngularVelocities;
+    }
+
+    /**
+     * Calculates the predictions for the cylinder orientations for the prediction step of the algorithm.
+     * @param cylinderOrientationPredictions The prediction of the orientation of each cylinder at its center of mass.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param cylinderAngularVelocities The angular velocity of the current frame of each orientation element/ cylinder.
+     * @param cylinderOrientations The orientation of each cylinder at its center of mass.
+     * @return The prediction of the orientation of each cylinder at its center of mass, i.e. @p cylinderOrientationPredictions.
+     */
+    public BSM.Quaternion[] PredictCylinderOrientations(BSM.Quaternion[] cylinderOrientationPredictions, int cylinderCount,
+                                                        Vector3[] cylinderAngularVelocities, BSM.Quaternion[] cylinderOrientations)
+    {
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            BSM.Quaternion embeddedAngularVelocity = mathHelper.EmbeddedVector(cylinderAngularVelocities[cylinderIndex]);
+            BSM.Quaternion qw = BSM.Quaternion.Multiply(cylinderOrientations[cylinderIndex], embeddedAngularVelocity);
+            BSM.Quaternion summand = BSM.Quaternion.Multiply(qw, 0.5f * Time.deltaTime);
+            cylinderOrientationPredictions[cylinderIndex] = cylinderOrientations[cylinderIndex] + summand;
+            
+            cylinderOrientationPredictions[cylinderIndex].Normalize();
+        }
+
+        return cylinderOrientationPredictions;
+    }
+}
+}
\ No newline at end of file

PredictionStep.cs.meta

+fileFormatVersion: 2
+guid: 368ffebfbbc47cb4191bda557c7229f9
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

SimulationLoop.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+using BSM = BulletSharp.Math;
+using System.IO;
+
+namespace GuidewireSim
+{
+    public class SimulationLoop : MonoBehaviour
+    {
+    		public Camera followingCamera;  // Drag your camera here in the Unity Editor
+    		private Vector3 cameraOffset = new Vector3(0, 781, 0); // The offset of the camera in the y direction is 781
+    		private string logFilePath = "";
+
+    		InitializationStep initializationStep; //!< The component InitializationStep that is responsible for initializing the simulation.
+    		PredictionStep predictionStep; //!< The component PredictionStep that is responsible for executing the Prediction Step of the algorithm.
+    		ConstraintSolvingStep constraintSolvingStep; /**< The component ConstraintSolvingStep that is responsible for correcting the predictions
+                                                  *   with the collision and model constraints.
+                                                  */
+    		UpdateStep updateStep; //!< The component UpdateStep that is responsible for executing the Update Step of the algorithm.
+    		ObjectSetter objectSetter; //!< The component ObjectSetter that is responsible for setting all positions and rotations the the GameObjects.
+    		MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
+    		CollisionSolvingStep collisionSolvingStep; //!< The component CollisionSolvingStep that solves all collisions.
+    		CollisionHandler collisionHandler; //!< The component CollisionHandler that tracks all collisions.
+
+    		[SerializeField] public GameObject[] spheres; /**< All spheres that are part of the guidewire.
+                                        *   @attention The order in which the spheres are assigned matters. Assign them such that
+                                        *   two adjacent spheres are adjacent in the array as well.
+                                        */
+    		[SerializeField] GameObject[] cylinders; /**< All cylinders that are part of the guidewire.
+                                              *   @attention The order in which the cylinders are assigned matters. Assign them such that
+                                              *   two adjacent cylinders are adjacent in the array as well.
+                                              */
+
+    		[HideInInspector] public Vector3[] spherePositions; //!< The position at the current frame of each sphere.
+    		[HideInInspector] public Vector3[] sphereVelocities; //!< The velocity of the current frame of each sphere. Initalized with zero entries.
+    		public float[] sphereInverseMasses; /**< The constant inverse masses  of each sphere.
+                                                           *   @note Set to 1 for moving spheres and to 0 for fixed spheres.
+                                                           */
+    [HideInInspector] public Vector3[] sphereExternalForces; //!< The sum of all current external forces that are applied per particle/ sphere.
+    Vector3[] spherePositionPredictions; //!< The prediction of the position at the current frame of each sphere.
+    
+    [HideInInspector] public Vector3[] cylinderPositions; //!< The center of mass of each cylinder.
+    BSM.Quaternion[] cylinderOrientations; //!< The orientation of each cylinder at its center of mass.
+    BSM.Quaternion[] cylinderOrientationPredictions; //!< The prediction of the orientation of each cylinder at its center of mass.
+    Vector3[] discreteRestDarbouxVectors; /**< The discrete Darboux Vector at the rest configuration, i.e. at frame 0.
+                                        *   @note It is important to only take the imaginary part in the calculation
+                                        *   for the discrete Darboux Vector, thus we only save it as a Vector3.
+                                        *   To use it in a quaternion setting, embedd the Vector3 with scalar part 0, i.e.
+                                        *   with EmbeddedVector().
+                                        *   @attention There is only CylinderCount - 1 many darboux vectors. The i-th Darboux Vector
+                                        *   is between orientation i and orientation i+1.
+                                        */
+    Vector3[] cylinderAngularVelocities; /**< The angular velocity of the current frame of each orientation element/ cylinder.
+                                        *   Initalized with zero entries.
+                                        */
+    [HideInInspector] public float[] cylinderScalarWeights; /**< The constant scalar weights of each orientation/ quaternion similar to #sphereInverseMasses.
+                                    *   @note Set to 1 for moving orientations (so that angular motion can be applied)
+                                    *   and to 0 for fixed orientations.
+                                    */
+    float[,] inertiaTensor; //!< The inertia tensor. Entries are approximates as in the CoRdE paper.
+    float[,] inverseInertiaTensor; //!< The inverse of #inertiaTensor.
+    [HideInInspector] public Vector3[] cylinderExternalTorques; //!< The sum of all current external torques that are applied per orientation element/ cylinder.
+    public Vector3[][] directors; /**< The orthonormal basis of each orientation element / cylinder, also called directors.
+                                   *   @note In the 0th row are the first directors of each orientation element, not in the 1th row.
+                                   *   Example: The (i, j)th element holds the (i-1)th director of orientation element j.
+                                   */
+    BSM.Quaternion[] worldSpaceBasis; /**< The three basis vectors of the world coordinate system as embedded quaternions with scalar part 0.
+                                       *   E.g. the first basis vector is (1, 0, 0), the second (0, 1, 0) and the third (0, 0, 1).
+                                       */
+
+    public bool ExecuteSingleLoopTest { get; set; } = false; /**< Whether or not to execute the Single Loop Test, in which the outer simulation loop
+                                                              *   is exactly executed once. 
+                                                              */
+    public bool solveStretchConstraints = true; //!< Whether or not to perform the constraint solving of the stretch constraint.
+    public bool solveBendTwistConstraints = true; //!< Whether or not to perform the constraint solving of the bend twist constraint.
+    public bool solveCollisionConstraints = true; //!< Whether or not to perform the constraint solving of collision constraints.
+
+    public float GetRodElementLength(){
+    	return rodElementLength;
+    }   
+    float rodElementLength = 100f;
+
+
+
+
+//The distance between two spheres, also the distance between two orientations.
+
+     // The distance between two spheres, also the distance between two orientations.
+                                //  Also the length of one cylinder.
+                                //  @note This should be two times the radius of a sphere.
+                               //  @attention Make sure that the guidewire setup fulfilles that the distance between two adjacent
+                                //   spheres is #rodElementLength.
+                                
+
+    [SerializeField] [Range(1, 1000)] int constraintSolverSteps = 1000; /**< How often the constraint solver iterates over each constraint during
+                                                                                 *   the Constraint Solving Step.
+                                                                                 *   @attention This value must be positive.
+                                                                                 */
+    public int ConstraintSolverSteps { get { return constraintSolverSteps; }
+                                       set { constraintSolverSteps = value; } }
+    private float timeStep;   /**< The fixed time step in seconds at which the simulation runs.
+                                                                      *  @note A lower timestep than 0.002 can not be guaranteed by
+                                                                      *  the test hardware to be executed in time. Only choose a lower timestep if
+                                                                      *  you are certain your hardware can handle it.
+                                                                      */
+    public float TimeStep { get { return timeStep; }}
+
+    public int SpheresCount { get; private set; } //!< The count of all #spheres of the guidewire.
+    public int CylinderCount { get; private set; } //!< The count of all #cylinders of the guidewire.
+    
+    private void Awake()
+    {        
+    	string[] args = System.Environment.GetCommandLineArgs();
+    	for (int i = 0; i < args.Length; i++)
+    	{
+        	if (args[i] == "-logFilePath" && args.Length > i + 1)
+        	{
+            		logFilePath = args[i + 1];
+        	}
+        	else if (args[i] == "-timeStep" && args.Length > i + 1)
+        	{
+           		if (float.TryParse(args[i + 1], out float parsedTimeStep))
+            		{
+                		timeStep = parsedTimeStep;
+            		}
+            		else
+            		{
+                		Debug.LogError("Failed to parse timeStep from command-line arguments. Using default value.");
+                		timeStep = 0.02f;
+            		}
+        	}
+    	}
+    	
+    	for (int i = 0; i < args.Length; i++)
+    	{
+        
+        	if (args[i] == "-rodElementLength" && args.Length > i + 1)
+        	{
+            		if (float.TryParse(args[i + 1], out float parsedRodElementLength))
+            		{
+                		rodElementLength = parsedRodElementLength;
+            		}
+            		else
+            		{
+                		Debug.LogError("Failed to parse rodElementLength from command-line arguments. Using default value.");
+                		rodElementLength = 100f;
+            		}
+       	}
+    	}            
+            Assert.IsFalse(spheres.Length == 0);
+            Assert.IsFalse(cylinders.Length == 0);
+            Assert.IsTrue(spheres.Length == cylinders.Length + 1);
+
+            initializationStep = GetComponent<InitializationStep>();
+            Assert.IsNotNull(initializationStep);
+
+            predictionStep = GetComponent<PredictionStep>();
+            Assert.IsNotNull(predictionStep);
+
+            constraintSolvingStep = GetComponent<ConstraintSolvingStep>();
+            Assert.IsNotNull(constraintSolvingStep);
+
+            updateStep = GetComponent<UpdateStep>();
+            Assert.IsNotNull(updateStep);
+
+            objectSetter = GetComponent<ObjectSetter>();
+            Assert.IsNotNull(objectSetter);
+
+            mathHelper = GetComponent<MathHelper>();
+            Assert.IsNotNull(mathHelper);
+
+            collisionSolvingStep = GetComponent<CollisionSolvingStep>();
+            Assert.IsNotNull(collisionSolvingStep);
+
+            collisionHandler = GetComponent<CollisionHandler>();
+            Assert.IsNotNull(collisionHandler);
+
+            // Get command line arguments
+            //string[] args = System.Environment.GetCommandLineArgs();
+            for (int i = 0; i < args.Length; i++)
+            {
+                if (args[i] == "-logFilePath" && args.Length > i + 1)
+                {
+                    logFilePath = args[i + 1];
+                }
+            }
+        }
+
+        private void Start()
+        {
+            Time.fixedDeltaTime = timeStep;
+
+            PerformInitializationStep();
+            InvokeRepeating("SavePositionsToFile", 1f, 1f);
+
+            objectSetter.SetCylinderPositions(cylinders, CylinderCount, cylinderPositions);
+            objectSetter.SetCylinderOrientations(cylinders, CylinderCount, cylinderOrientations, directors);
+        }
+
+        private void FixedUpdate()
+        {
+            if (ExecuteSingleLoopTest) return;
+
+            PerformSimulationLoop();
+            UpdateCameraPosition();
+        }
+
+        public void SavePositionsToFile()
+        {
+            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);
+                }
+            }
+        }
+        private void UpdateCameraPosition()
+    	{
+        	if (spheres != null && spheres.Length > 0)
+        	{
+            		GameObject lastSphere = spheres[spheres.Length - 1];
+            		Vector3 newCameraPosition = lastSphere.transform.position + cameraOffset;
+            		followingCamera.transform.position = newCameraPosition;
+        	}
+    	}
+
+
+        private void PerformInitializationStep()
+        {
+            SpheresCount = spheres.Length;
+            CylinderCount = cylinders.Length;
+
+            Assert.IsTrue(SpheresCount == CylinderCount + 1);
+
+            initializationStep.InitSpherePositions(spheres, SpheresCount, out spherePositions);
+            initializationStep.InitSphereVelocities(SpheresCount, out sphereVelocities);
+            initializationStep.InitSphereInverseMasses(SpheresCount, out sphereInverseMasses);
+            initializationStep.InitCylinderPositions(CylinderCount, spherePositions, out cylinderPositions);
+            initializationStep.InitCylinderOrientations(CylinderCount, out cylinderOrientations);
+            initializationStep.InitDiscreteRestDarbouxVectors(CylinderCount, cylinderOrientations, out discreteRestDarbouxVectors, rodElementLength);
+            initializationStep.InitCylinderAngularVelocities(CylinderCount, out cylinderAngularVelocities);
+            initializationStep.InitCylinderScalarWeights(CylinderCount, out cylinderScalarWeights);
+            initializationStep.InitSphereExternalForces(SpheresCount, out sphereExternalForces);
+            initializationStep.InitSpherePositionPredictions(spheres, SpheresCount, out spherePositionPredictions);
+            initializationStep.InitInertiaTensor(out inertiaTensor);
+            initializationStep.InitInverseInertiaTensor(inertiaTensor, out inverseInertiaTensor);
+            initializationStep.InitCylinderExternalTorques(CylinderCount, out cylinderExternalTorques);
+            initializationStep.InitCylinderOrientationPredictions(CylinderCount, out cylinderOrientationPredictions);
+            initializationStep.InitWorldSpaceBasis(out worldSpaceBasis);
+            initializationStep.InitDirectors(CylinderCount, worldSpaceBasis, out directors);
+            initializationStep.InitSphereColliders(SpheresCount, spheres);
+        }
+
+        public void PerformSimulationLoop()
+        {
+            PerformConstraintSolvingStep();
+            PerformUpdateStep();
+            PerformPredictionStep();
+            AdaptCalculations();
+            SetCollidersStep();
+        }
+
+        private void PerformConstraintSolvingStep()
+        {
+            if (solveStretchConstraints)
+            {
+                Assert.IsTrue(SpheresCount >= 2);
+                Assert.IsTrue(CylinderCount >= 1);
+            }
+
+            if (solveBendTwistConstraints)
+            {
+                Assert.IsTrue(SpheresCount >= 3);
+                Assert.IsTrue(CylinderCount >= 2);
+            }
+
+            for (int solverStep = 0; solverStep < ConstraintSolverSteps; solverStep++)
+            {
+                if (solveStretchConstraints)
+                {
+                    constraintSolvingStep.SolveStretchConstraints(spherePositionPredictions, cylinderOrientationPredictions, SpheresCount, worldSpaceBasis, rodElementLength);
+                }
+
+                if (solveBendTwistConstraints)
+                {
+                    constraintSolvingStep.SolveBendTwistConstraints(cylinderOrientationPredictions, CylinderCount, discreteRestDarbouxVectors, rodElementLength);
+                }
+
+                if (solveCollisionConstraints)
+                {
+                    collisionSolvingStep.SolveCollisionConstraints(spherePositionPredictions, solverStep, ConstraintSolverSteps);
+                }
+            }
+
+            if (solveCollisionConstraints)
+            {
+                collisionHandler.ResetRegisteredCollisions();
+            }
+        }
+
+        private void PerformUpdateStep()
+        {
+            sphereVelocities = updateStep.UpdateSphereVelocities(sphereVelocities, SpheresCount, spherePositionPredictions, spherePositions);
+            spherePositions = updateStep.UpdateSpherePositions(spherePositions, SpheresCount, spherePositionPredictions);
+            cylinderAngularVelocities = updateStep.UpdateCylinderAngularVelocities(cylinderAngularVelocities, CylinderCount, cylinderOrientations, cylinderOrientationPredictions);
+            cylinderOrientations = updateStep.UpdateCylinderOrientations(cylinderOrientations, CylinderCount, cylinderOrientationPredictions);
+            directors = mathHelper.UpdateDirectors(CylinderCount, cylinderOrientations, directors, worldSpaceBasis);
+        }
+
+        private void PerformPredictionStep()
+        {
+            sphereVelocities = predictionStep.PredictSphereVelocities(sphereVelocities, sphereInverseMasses, sphereExternalForces);
+            spherePositionPredictions = predictionStep.PredictSpherePositions(spherePositionPredictions, SpheresCount, spherePositions, sphereVelocities);
+            cylinderAngularVelocities = predictionStep.PredictAngularVelocities(cylinderAngularVelocities, CylinderCount, inertiaTensor, cylinderExternalTorques, inverseInertiaTensor);
+            cylinderOrientationPredictions = predictionStep.PredictCylinderOrientations(cylinderOrientationPredictions, CylinderCount, cylinderAngularVelocities, cylinderOrientations);
+        }
+
+        private void AdaptCalculations()
+        {
+            objectSetter.SetSpherePositions(spheres, SpheresCount, spherePositions);
+            mathHelper.CalculateCylinderPositions(CylinderCount, spherePositions, cylinderPositions);
+            objectSetter.SetCylinderPositions(cylinders, CylinderCount, cylinderPositions);
+            objectSetter.SetCylinderOrientations(cylinders, CylinderCount, cylinderOrientations, directors);
+        }
+
+        private void SetCollidersStep()
+        {
+            collisionHandler.SetCollidersToPredictions(SpheresCount, spherePositionPredictions, spherePositions);
+        }
+
+        public void SetSpheres(GameObject[] spheresArray)
+        {
+            spheres = spheresArray;
+        }
+
+        public void SetCylinders(GameObject[] cylindersArray)
+        {
+            cylinders = cylindersArray;
+        }
+    }
+}
+

SimulationLoop.cs.meta

+fileFormatVersion: 2
+guid: 1efdb27511548744d82097948ce1b3df
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 800
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

StressTestPerformer.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+namespace GuidewireSim
+{
+/**
+ * This class enables the user to test the impact of mutliple external forces and external torques with one button within the Unity inspector.
+ * @attention In the current version, the user is not able to fix positions or orientations of the guidewire,
+ * which is necessary e.g. for stress test one.
+ */
+public class StressTestPerformer : MonoBehaviour
+{
+    SimulationLoop simulationLoop; //!< The SimulationLoop component that executes all steps of the simulation loop.
+
+    [Tooltip("Performs stress test one. This test fixes the position of one end of the guidewire, and applies @p pullForce at the other end for"
+     + "@p applyForceTime seconds, and then applies - @p pullForce for another @p applyForceTime seconds.")]
+    [SerializeField] bool doStressTestOne = false; /**< Whether to run Stress Test One. This test fixes the position of one end of the
+                                                    *   guidewire, and applies @p pullForce at the other end for @p applyForceTime seconds,
+                                                    *   and then applies - @p pullForce for another @p applyForceTime seconds.
+                                                    */
+
+    private void Awake()
+    {
+        simulationLoop = GetComponent<SimulationLoop>();
+        Assert.IsNotNull(simulationLoop);
+    }
+
+    private void Start()
+    {
+        PerformStressTests();
+    }
+
+    /**
+     * Performs each Stress Test whose respective serialized boolean is set to true in the Unity inspector.
+     */
+    private void PerformStressTests()
+    {
+        if (doStressTestOne) StartCoroutine(PerformStressTestOne());
+    }
+
+    /**
+     * Performs stress test one. This test fixes the position of one end of the guidewire, and applies @p pullForce at the other end for
+     * @p applyForceTime seconds, and then applies - @p pullForce for another @p applyForceTime seconds.
+     * @param applyForceTime For how many seconds to apply the force to the particles.
+     * @attention In the current version, the user is not able to fix positions or orientations of the guidewire,
+     * which is necessary e.g. for stress test one.
+     * @req Output a log message when no further forces are applied to the guidewire.
+     */
+    private IEnumerator PerformStressTestOne(float applyForceTime = 1f)
+    {
+        // fix the position of the first sphere. Does not fix the position, but nullifies forces acting on this particle.
+        simulationLoop.sphereInverseMasses[0] = 0f;
+        simulationLoop.sphereInverseMasses[1] = 0f;
+        simulationLoop.cylinderScalarWeights[0] = 0f;
+
+        Vector3 pullForce = new Vector3(0f, 2f, 0f);
+
+        for (int sphereIndex = 0; sphereIndex < (simulationLoop.SpheresCount - 1); sphereIndex++)
+        {
+            simulationLoop.sphereExternalForces[sphereIndex] = Vector3.zero;
+        }
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = pullForce;
+
+        yield return new WaitForSeconds(applyForceTime);
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = -pullForce;
+
+        yield return new WaitForSeconds(applyForceTime);
+
+        simulationLoop.sphereExternalForces[simulationLoop.SpheresCount - 1] = Vector3.zero;
+
+        Debug.Log("End of Stress Test One");
+    }
+}
+}
\ No newline at end of file

StressTestPerformer.cs.meta

+fileFormatVersion: 2
+guid: 6947fd09b329f8245abe8a5cef1532ed
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 1100
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

TorqueTestPerformer.cs

+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+namespace GuidewireSim
+{
+/**
+ * This class enables the user to test the impact of external torques with one button within the Unity inspector.
+ */
+public class TorqueTestPerformer : MonoBehaviour
+{
+    SimulationLoop simulationLoop; //!< The SimulationLoop component that executes all steps of the simulation loop.
+
+    [Tooltip("The external torque that is applied to the respective parts of the guidewire, depending on the test.")]
+    [SerializeField] Vector3 pullTorque = new Vector3(0f, 0.3f, 0f); /**< The external torque that is applied to the respective parts of the guidewire,
+                                                                      *   depending on the test.
+                                                                      */
+    [Tooltip("Applies an external torque to one end of the guidewire.")]
+    [SerializeField] bool doTorqueTestOne = false; //!< Whether to run Torque Test One. This test applies an external torque to one end of the guidewire.
+    [Tooltip("Applies an external torque to one end of the guidewire for a fixed amount of time and then the opposite torque"
+    + "at the same orientation for the same amount of time.")]
+    [SerializeField] bool doTorqueTestTwo = false; /**< Whether to run Torque Test Two. This test applies an external torque to one end of the guidewire for
+                                                    *   a fixed amount of time and then the opposite torque at the same orientation for the same amount of time.
+                                                    */
+    [Tooltip("Applies an external torque to one end of the guidewire and at the same time the opposite torque at the other end of the guidewire."
+    + "The applied torque starts at 0 and linearly interpolates until it reaches @p pullTorque at @p applyTorqueTime seconds.")]
+    [SerializeField] bool doTorqueTestThree = false; /**< Whether to run Torque Test Three. This test applies an external torque to one end of the
+                                                      *   guidewire and at the same time the opposite torque at the other end of the guidewire.
+                                                      *   The applied torque starts at 0 and linearly interpolates until it reaches
+                                                      *   @p pullTorque at @p applyTorqueTime seconds.
+                                                      */
+
+    private void Awake()
+    {
+        simulationLoop = GetComponent<SimulationLoop>();
+        Assert.IsNotNull(simulationLoop);
+    }
+
+    private void Start()
+    {
+        PerformTorqueTests();
+    }
+
+    /**
+     * Performs each Torque Test whose respective serialized boolean is set to true in the Unity inspector.
+     */
+    private void PerformTorqueTests()
+    {
+        if (doTorqueTestOne) PerformTorqueTestOne();
+        else if (doTorqueTestTwo) StartCoroutine(PerformTorqueTestTwo(pullTorque));
+        else if (doTorqueTestThree) StartCoroutine(PerformTorqueTestThree(pullTorque));
+    }
+
+    /**
+     * Performs torque test one. This test applies an external torque to one end of the guidewire.
+     */
+    private void PerformTorqueTestOne()
+    {
+        for (int cylinderIndex = 0; cylinderIndex < (simulationLoop.CylinderCount - 1); cylinderIndex++)
+        {
+            simulationLoop.cylinderExternalTorques[cylinderIndex] = Vector3.zero;
+        }
+
+        simulationLoop.cylinderExternalTorques[simulationLoop.CylinderCount - 1] = pullTorque;
+    }
+
+    /**
+     * Performs torque test two. This test applies an external torque to one end of the guidewire for a fixed amount of time
+     * and then the opposite torque at the same orientation for the same amount of time.
+     * @param pullTorque The external torque that is applied to one end of the guidewire.
+     * @param applyTorqueTime For how many seconds to apply the torque to the orientations.
+     * @req Output a log message when no further torques are applied to the guidewire.
+     */
+    private IEnumerator PerformTorqueTestTwo(Vector3 pullTorque, float applyTorqueTime = 1f)
+    {
+        for (int cylinderIndex = 0; cylinderIndex < (simulationLoop.CylinderCount - 1); cylinderIndex++)
+        {
+            simulationLoop.cylinderExternalTorques[cylinderIndex] = Vector3.zero;
+        }
+
+        simulationLoop.cylinderExternalTorques[simulationLoop.CylinderCount - 1] = pullTorque;
+
+        yield return new WaitForSeconds(applyTorqueTime);
+
+        simulationLoop.cylinderExternalTorques[simulationLoop.CylinderCount - 1] = -pullTorque;
+
+        yield return new WaitForSeconds(applyTorqueTime);
+
+        simulationLoop.cylinderExternalTorques[simulationLoop.CylinderCount - 1] = Vector3.zero;
+
+        Debug.Log("End of Torque Test Two");
+    }
+
+    /**
+     * Performs torque test three. This test applies an external torque to one end of the guidewire and at the same time the
+     * opposite torque at the other end of the guidewire. The applied torque starts at 0 and linearly interpolates until it reaches
+     * @p pullTorque at @p applyTorqueTime seconds.
+     * @param pullTorque The external torque that is applied to one end of the guidewire.
+     * @param applyTorqueTime For how many seconds to apply the torque to the orientations.
+     * @req Output a log message when no further torques are applied to the guidewire.
+     */
+    private IEnumerator PerformTorqueTestThree(Vector3 pullTorque, float applyTorqueTime = 10f)
+    {
+        yield return new WaitForSeconds(1f);
+
+        for (int cylinderIndex = 1; cylinderIndex < (simulationLoop.CylinderCount - 1); cylinderIndex++)
+        {
+            simulationLoop.cylinderExternalTorques[cylinderIndex] = Vector3.zero;
+        }
+
+        float elapsedTime = 0f;
+
+        while (elapsedTime < applyTorqueTime)
+        {
+            Vector3 effectiveTorque = Vector3.Lerp(Vector3.zero, pullTorque, elapsedTime / applyTorqueTime);
+
+            simulationLoop.cylinderExternalTorques[0] = - effectiveTorque;
+            simulationLoop.cylinderExternalTorques[simulationLoop.CylinderCount - 1] = effectiveTorque;
+
+            yield return null;
+
+            elapsedTime += Time.deltaTime;
+        }
+
+        simulationLoop.cylinderExternalTorques[0] = Vector3.zero;
+        simulationLoop.cylinderExternalTorques[simulationLoop.CylinderCount - 1] = Vector3.zero;
+
+        Debug.Log("End of Torque Test Three");
+    }
+}
+}
\ No newline at end of file

TorqueTestPerformer.cs.meta

+fileFormatVersion: 2
+guid: 704884f6d16c9c645add951843e0ec16
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 1000
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

UpdateStep.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.Assertions;
+using BSM = BulletSharp.Math;
+
+namespace GuidewireSim
+{
+/**
+ * This class implements the update step of the algorithm.
+ */
+public class UpdateStep : MonoBehaviour
+{
+    MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
+
+    private void Awake()
+    {
+        mathHelper = GetComponent<MathHelper>();
+        Assert.IsNotNull(mathHelper);
+    }
+
+    /**
+     * Updates the sphere velocities given the current prediction and the current position.
+     * @param sphereVelocities The velocity of the current frame of each sphere.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @param spherePositionPredictions The prediction of the position at the current frame of each sphere (in this case of the last frame).
+     * @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)
+    {
+        for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
+        {
+            sphereVelocities[sphereIndex] = (spherePositionPredictions[sphereIndex] - spherePositions[sphereIndex]) / Time.deltaTime;
+        }
+
+        return sphereVelocities;
+    }
+
+    /**
+     * Updates the sphere positions given the current position predictions.
+     * @param spherePositions The position at the current frame of each sphere.
+     * @param spheresCount The count of all spheres of the guidewire. Equals the length of @p spherePositionPredictions.
+     * @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)
+    {
+        for (int sphereIndex = 0; sphereIndex < spheresCount; sphereIndex++)
+        {
+            spherePositions[sphereIndex] = spherePositionPredictions[sphereIndex];
+        }
+
+        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.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param cylinderOrientations The orientation of each cylinder at its center of mass.
+     * @param cylinderOrientationPredictions The prediction of the orientation of each cylinder at its center of mass.
+     * @return The angular velocity of the current frame of each orientation element/ cylinder, i.e. @p cylinderAngularVelocities.
+     */
+    public Vector3[] UpdateCylinderAngularVelocities(Vector3[] cylinderAngularVelocities, int cylinderCount, BSM.Quaternion[] cylinderOrientations,
+                                                     BSM.Quaternion[] cylinderOrientationPredictions)
+    {
+        float factor = 2f / Time.deltaTime;
+
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            BSM.Quaternion qu = BSM.Quaternion.Conjugate(cylinderOrientations[cylinderIndex]) * cylinderOrientationPredictions[cylinderIndex];
+            BSM.Quaternion quaternionResult = factor * qu;
+            cylinderAngularVelocities[cylinderIndex] = mathHelper.ImaginaryPart(quaternionResult);
+        }
+
+        return cylinderAngularVelocities;
+    }
+
+    /**
+     * Updates the cylinder orientations given the current orientation predictions for the update step of the simulation.
+     * @param cylinderOrientations The orientation of each cylinder at its center of mass.
+     * @param cylinderCount The count of all cylinders of the guidewire. Equals the length of @p cylinderOrientationPredictions.
+     * @param cylinderOrientationPredictions The prediction of the orientation of each cylinder at its center of mass.
+     * @return The orientation of each cylinder at its center of mass, i.e. @p cylinderOrientations.
+     */
+    public BSM.Quaternion[] UpdateCylinderOrientations(BSM.Quaternion[] cylinderOrientations, int cylinderCount,
+                                                     BSM.Quaternion[] cylinderOrientationPredictions)
+    {
+        for (int cylinderIndex = 0; cylinderIndex < cylinderCount; cylinderIndex++)
+        {
+            cylinderOrientations[cylinderIndex] = cylinderOrientationPredictions[cylinderIndex];
+        }
+
+        return cylinderOrientations;
+    }
+}
+}
\ No newline at end of file

UpdateStep.cs.meta

+fileFormatVersion: 2
+guid: e02df0032f2020c44962b6ef4cb01098
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: