PersistentTrails.Track.evaluateAtTime C# (CSharp) Method

        public void evaluateAtTime(double ut, out Vector3 position, out Quaternion orientation, out Vector3 velocity)
        {
            position = new Vector3();
            orientation = new Quaternion();
            velocity = new Vector3();

            if (waypoints.Count == 0)
                return;

            closeLoop(); //only does anything at all if EndAction == LOOP

            //Debug.Log("Track.evaluateAt ut=" + ut + ", track starts at " + waypoints.First().recordTime + " and ends at " + waypoints.Last().recordTime);

            if (ut <= waypoints.First().recordTime)
            {
                position = referenceBody.GetWorldSurfacePosition(waypoints.First().latitude, waypoints.First().longitude, waypoints.First().altitude);
                orientation = waypoints.First().orientation;
                velocity = waypoints.First().velocity;
            }
            else if (ut >= waypoints.Last().recordTime)
            {
                position = referenceBody.GetWorldSurfacePosition(waypoints.Last().latitude, waypoints.Last().longitude, waypoints.Last().altitude);
                orientation = waypoints.Last().orientation;
                velocity = waypoints.Last().velocity;
            }
            else
            { //intermediate point

                for (int i = 0; i < waypoints.Count - 1; ++i)
                {

                    //find out how much relPos is covered by this segment
                    //Vector3 segment = end - start;

                    double timeThis = waypoints[i].recordTime;
                    double timeNext = waypoints[i + 1].recordTime;

                    if (timeNext > ut)
                    {
                        //evaluate on this segment
                        Vector3 start = referenceBody.GetWorldSurfacePosition(waypoints[i].latitude, waypoints[i].longitude, waypoints[i].altitude);
                        Vector3 end = referenceBody.GetWorldSurfacePosition(waypoints[i + 1].latitude, waypoints[i + 1].longitude, waypoints[i + 1].altitude);

                        double timeOnSegment = timeNext - timeThis;

                        //Debug.Log(string.Format("found segment at i={0}  from {1} to {2}, evaluating at relvalue {3}", i, start.ToString(), end.ToString(), (float)(ut - timeThis) / (float) timeOnSegment));
                        float progress = (float)(ut - timeThis) / (float)timeOnSegment;
                        position = Vector3.Lerp(start, end, progress);
                        orientation = Quaternion.Lerp(waypoints[i].orientation, waypoints[i + 1].orientation, progress); //Lerp is faster, maybe use the more accurate Slerp for better results?
                        velocity = Vector3.Lerp(waypoints[i].velocity, waypoints[i + 1].velocity, progress); //Lerp is faster, maybe use the more accurate Slerp for better results?

                        if (float.IsNaN(orientation.x))
                            orientation.x = 0;

                        if (float.IsNaN(orientation.y))
                            orientation.y = 0;

                        if (float.IsNaN(orientation.z))
                            orientation.z = 0;

                        if (float.IsNaN(orientation.w))
                            orientation.w = 0;

                        break;
                    }
                }
            }

            openLoop();
        }

        public void initialize(Track track, GameObject ghost)
        {
            this.track   = track;
            this.ghost   = ghost;
            trackStartUT = track.GetStartTime();


            totalReplayTime = track.GetEndTime() - track.GetStartTime();
            if (track.EndAction == Track.EndActions.LOOP)
            {
                totalReplayTime += track.LoopClosureTime;
            }

            currentReplayTime = 0;


            replayStartUT = Planetarium.GetUniversalTime();
            lastUpdateUT  = replayStartUT;

            setGhostToPlaybackAt(trackStartUT + currentReplayTime);
            Vector3    trackPos;
            Quaternion orientation;
            Vector3    velocity;

            track.evaluateAtTime(trackStartUT + currentReplayTime, out trackPos, out orientation, out velocity);
            ghost.transform.position = trackPos;
            ghost.transform.rotation = orientation;

            playbackFactor = 0;
            Debug.Log("initialized replayBehaviour, ghost at trackPos =" + trackPos.ToString());
        }