Matrix4d LorentzBoost(Vector3d v)
    {
        //Computes the Lorentz Boost for a given 3-velocity
        double βSqr = v.SqrMagnitude();

        if (βSqr == 0f)
        {
            return(Matrix4d.Identity());
        }
        double   βx    = v.x;
        double   βy    = v.y;
        double   βz    = v.z;
        double   gamma = Lorentz(v);
        Matrix4d boost = new Matrix4d(
            new Vector4d(gamma, -gamma * βx, -gamma * βy, -gamma * βz),
            new Vector4d(-βx * gamma, (gamma - 1) * (βx * βx) / (βSqr) + 1, (gamma - 1) * (βx * βy) / (βSqr), (gamma - 1) * (βx * βz) / (βSqr)),
            new Vector4d(-βy * gamma, (gamma - 1) * (βy * βx) / (βSqr), (gamma - 1) * (βy * βy) / (βSqr) + 1, (gamma - 1) * (βy * βz) / (βSqr)),
            new Vector4d(-βz * gamma, (gamma - 1) * (βz * βx) / (βSqr), (gamma - 1) * (βz * βy) / (βSqr), (gamma - 1) * (βz * βz) / (βSqr) + 1)
            );

        return(boost);
    }
    void DrawWorldline()
    {
        double maxT = 10;

        /*
         * for (double properTime = 0; properTime < 20.0; properTime += dT) {
         *      Matrix5d objectToCoordinateBoost = new Matrix5d(LorentzBoost(InitialVelocity), new Vector4d(0.0, -new Vector3d(transform.position))).Inverse();
         *      double calculatedTime = 0.0;
         *      for (int i=0; i<ProperAccelerations.Count; i++) {
         *              Vector3d A = ProperAccelerations[i];
         *              double T = AccelerationDurations[i];
         *              bool lastAccel = false;
         *              if (properTime - calculatedTime <= AccelerationDurations[i]) {
         *                      T = properTime - calculatedTime;
         *                      lastAccel = true;
         *              }
         *              double aSqr = A.SqrMagnitude();
         *              double a = A.Magnitude();
         *              double t = Math.Sinh(T * a) / a;
         *              Vector3d velocity = A * t / Math.Sqrt(1 + t * t * aSqr);
         *              Vector3d displacement = A * (Math.Sqrt(1 + t * t * aSqr) - 1) / aSqr;
         *              Matrix5d M = new Matrix5d(LorentzBoost(velocity), new Vector4d(-t, displacement));
         *              Matrix5d MInv = M.Inverse();
         *              objectToCoordinateBoost *= MInv;
         *              calculatedTime += T;
         *              if (lastAccel) break;
         *      }
         *      Vector4d localObjectEvent = new Vector4d(properTime - calculatedTime, new Vector3d());
         *
         *      Matrix5d coordinateToObserverStartBoost = new Matrix5d(LorentzBoost(-new Vector3d(observerScript.velocity)), new Vector4d(0, new Vector3d(Observer.transform.position))).Inverse();
         *      Matrix5d objectToObserverBoost = coordinateToObserverStartBoost * objectToCoordinateBoost;
         *      Vector4d currentObjectEvent = objectToObserverBoost * localObjectEvent;
         *      if (currentObjectEvent.t > 0) {
         *              for (int i = 0; i < observerScript.accelerations.Count; i++) {
         *                      Vector3d A = new Vector3d(observerScript.accelerations[i]);
         *                      Vector3d X = currentObjectEvent.Space();
         *                      double t = currentObjectEvent.t;
         *                      double aSqr = A.SqrMagnitude();
         *                      double a = Math.Sqrt(aSqr);
         *                      double t2 = 0;
         *                      if (A*A < Math.Pow(1 + A*X, 2)/(t*t)) {
         *                              if (t != 0) {
         *                                      t2 = Math.Abs(t) / Math.Sqrt(-t * t * aSqr + Math.Pow(1 + A * X, 2));
         *                                      if ((A * X > -1 && t < 0) || (A * X < -1 && t > 0)) {
         *                                              t2 = -t2;
         *                                      }
         *                              }
         *                              Vector3d velocity = A * t2 / Math.Sqrt(1 + t2 * t2 * aSqr);
         *                              Vector3d displacement = A * (Math.Sqrt(1 + t2 * t2 * aSqr) - 1) / aSqr;
         *                              double T = ASinh(t2 * a) / a;
         *                              Matrix5d M = new Matrix5d(LorentzBoost(velocity), new Vector4d(-t2+T, displacement));
         *                              //Matrix5d MInv = M.Inverse();
         *                              objectToObserverBoost = M * objectToObserverBoost;
         *                      } else {
         *                              // Rindler Horizon
         *                              // Debug.Log("Horizon " + A.x + " " + A.y + " " + A.z + " | " + X.x + " " + X.y + " " + X.z + " | " + t);
         *                      }
         *              }
         *      }
         *      currentObjectEvent = objectToObserverBoost * localObjectEvent;
         *      Vector3d drawInObserverFrame = currentObjectEvent.Space();
         *      drawInObserverFrame.z += currentObjectEvent.t;
         *      Debug.DrawRay(drawInObserverFrame.Vector3(), Vector3.up, Color.green);
         * }
         */
        bool first = true;

        Matrix5d initialBoost = new Matrix5d(LorentzBoost(InitialVelocity), new Vector4d(0, new Vector3d(-transform.position))) * new Matrix5d(LorentzBoost(new Vector3d(observerScript.velocity)), new Vector4d(0, new Vector3d(-Observer.transform.position))).Inverse();
        Vector4d back         = initialBoost.Inverse().MultiplyDirection(new Vector4d {
            t = -1
        });
        Vector4d center = initialBoost * new Vector4d();

        Debug.DrawRay(center.Draw(), back.Draw() * 1000);

        for (double observerTime = 0; observerTime < maxT; observerTime += deltaT)
        {
            Vector3d observerVelocity          = new Vector3d(observerScript.velocity);
            Matrix5d observerToCoordinateBoost = new Matrix5d(LorentzBoost(observerVelocity), new Vector4d(0, new Vector3d(-Observer.transform.position))).Inverse();
            Vector4d localObserver             = new Vector4d {
                t = observerTime
            };
            Vector4d coordObserver = observerToCoordinateBoost * localObserver;
            //Debug.DrawRay(coordObserverDraw.Vector3(), Vector3.up, Color.cyan);
            double   calculatedTime = 0;
            Vector4d prevTr         = new Vector4d();
            int      i     = 0;
            double   a     = 1.0;
            double   prevT = 0;
            for (; i < observerScript.accelerations.Count; i++)
            {
                Vector3d A         = new Vector3d(observerScript.accelerations[i]);
                double   T         = observerScript.durations[i];
                bool     lastAccel = false;
                calculatedTime += prevT;
                if (observerTime < calculatedTime + T)
                {
                    T         = observerTime - calculatedTime;
                    lastAccel = true;
                }

                double aSqr = A.SqrMagnitude();
                a = A.Magnitude();
                double   t            = Math.Sinh(T * a) / a;
                Vector3d velocity     = A * t / Math.Sqrt(1 + t * t * aSqr);
                Vector3d displacement = A * (Math.Sqrt(1 + t * t * aSqr) - 1) / aSqr;
                //Vector4d newTr = new Vector4d(, new Vector3d());
                //Matrix5d Tr = new Matrix5d(Matrix4d.Identity(), prevTr);
                Matrix5d M = new Matrix5d(LorentzBoost(velocity), new Vector4d(-t + T, displacement));
                observerToCoordinateBoost *= new Matrix5d(Matrix4d.Identity(), prevTr) * M.Inverse() * new Matrix5d(Matrix4d.Identity(), new Vector4d(-calculatedTime, new Vector3d()));
                prevTr = new Vector4d(T + prevT, new Vector3d());
                //observerToCoordinateBoost *= MInv * Tr;// * observerToCoordinateBoost;
                //Debug.Log(simulPlane2.Normal.Space() * displacement + simulPlane2.Normal.t * t - simulPlane2.Distance);
                //observerToCoordinateBoost *= new Matrix5d(Matrix4d.Identity(), new Vector4d(-prevObserverTime, new Vector3d { x = observerTime }));
                localObserver = new Vector4d {
                    t = observerTime
                };
                prevT = T;
                if (lastAccel)
                {
                    break;
                }
            }
            //localObserverEvent = new Vector4d(observerTime - calculatedTime, new Vector3d());
            Matrix5d coordinateToObjectBoost = new Matrix5d(LorentzBoost(InitialVelocity), new Vector4d(0, new Vector3d(-transform.position)));
            Matrix5d observerToObjectBoost   = coordinateToObjectBoost * observerToCoordinateBoost;

            /*
             * Vector4d right = observerToObjectBoost.MultiplyDirection(new Vector4d { x = 1 });
             * Vector4d up = observerToObjectBoost.MultiplyDirection(new Vector4d { y = 1 });
             * Vector4d forward = observerToObjectBoost.MultiplyDirection(new Vector4d { t = 1 });
             */


            Plane simulPlane = observerToObjectBoost.InvTransposePlaneMultiplication(new Plane {
                Distance = observerTime
            });
            double   simulPlaneT = simulPlane.Normal.t;
            Vector4d localObject = new Vector4d {
                t = simulPlane.Distance / simulPlaneT
            };
            calculatedTime = 0;
            Matrix5d prevBoost = new Matrix5d();
            Vector3  offset    = Vector3.zero;
            for (i = 0; i < ProperAccelerations.Count; i++)
            {
                Vector3d A     = ProperAccelerations[i];
                Vector4d N     = simulPlane.Normal.Normalized();
                Vector3d nX    = N.Space();
                double   nXSqr = nX.SqrMagnitude();
                double   nT    = N.t;
                double   nT2   = nT * nT;
                double   d     = simulPlane.Distance;
                double   d2    = d * d;
                double   aSqr  = A.SqrMagnitude();

                double AnX  = A * nX;
                double AnX2 = AnX * AnX;
                double t2;
                //Find intersection time t2 between simulPlane and worldline of particle under proper acceleration A:
                if (d == 0 && nT == 0 && AnX != 0 && nXSqr >= 0 && aSqr > 0)
                {
                    t2 = 0;
                }
                else if (AnX == 0 && d >= 0 && nT != 0 && nXSqr >= 0 && aSqr > 0)
                {
                    t2 = d / nT;
                }
                else if (aSqr == AnX2 / nT2 && nXSqr >= 0 && nT != 0 && ((d >= 0 && ((nT2 + 2 * d * AnX >= 0 && AnX < 0) || AnX > 0)) || (nT2 + d * AnX > 0 && AnX < 0 && nT2 + 2 * d * AnX < 0)))
                {
                    t2 = 1 / (nT * (1 / d + 1 / (d + (2 * AnX) / aSqr)));
                }
                else if (nXSqr >= 0 && AnX > 0 && nT == 0 && d > 0 && aSqr > 0)
                {
                    t2 = -(Math.Sqrt(d * (d * aSqr + 2 * AnX)) / AnX);
                }
                else if (nXSqr >= 0 && AnX > 0 && nT == 0 && d > 0 && aSqr > 0)
                {
                    t2 = Math.Sqrt((d * (d * aSqr + 2 * AnX)) / AnX2);
                }
                else if (nXSqr >= 0 && ((AnX < 0 && ((nT2 + 2 * d * AnX < 0 && ((nT2 + d * AnX > 0 && nT != 0 && (nT2 + d2 * aSqr + 2 * d * AnX == 0 || (nT2 + d2 * aSqr + 2 * d * AnX > 0 && nT2 * aSqr < AnX2))) || (nT2 * aSqr > AnX2 && nT < 0))) || (d >= 0 && ((nT < 0 && ((aSqr > 0 && nT2 + 2 * d * AnX >= 0 && nT2 * aSqr < AnX2) || nT2 * aSqr > AnX2)) || (nT > 0 && aSqr > 0 && nT2 + 2 * d * AnX >= 0 && nT2 * aSqr < AnX2))) || (nT < 0 && nT2 + d * AnX <= 0 && nT2 * aSqr > AnX2))) || (d >= 0 && AnX > 0 && ((nT != 0 && aSqr > 0 && nT2 * aSqr < AnX2) || (nT > 0 && nT2 * aSqr > AnX2)))))
                {
                    t2 = -((Math.Abs(AnX) * Math.Sqrt(nT2 + d2 * aSqr + 2 * d * AnX)) / Math.Abs((-nT2) * aSqr + AnX2)) + (nT * (d * aSqr + AnX)) / (nT2 * aSqr - AnX2);
                }
                else if (nXSqr >= 0 && ((nT != 0 && nT2 * aSqr < AnX2 && ((aSqr > 0 && d >= 0 && ((nT2 + 2 * d * AnX >= 0 && AnX < 0) || AnX > 0)) || (AnX < 0 && nT2 + 2 * d * AnX < 0 && nT2 + d * AnX > 0 && nT2 + d2 * aSqr + 2 * d * AnX > 0))) || (nT2 * aSqr > AnX2 && ((d >= 0 && ((nT < 0 && AnX > 0) || (nT > 0 && AnX < 0))) || (nT > 0 && AnX < 0 && (nT2 + 2 * d * AnX < 0 || nT2 + d * AnX <= 0))))))
                {
                    t2 = (Math.Abs(AnX) * Math.Sqrt(nT2 + d2 * aSqr + 2 * d * AnX)) / Math.Abs((-nT2) * aSqr + AnX2) + (nT * (d * aSqr + AnX)) / (nT2 * aSqr - AnX2);
                }
                else
                {
                    break;
                }
                double accel      = Math.Sqrt(aSqr);
                double properTime = ASinh(t2 * accel) / accel;
                if (properTime < 0)
                {
                    break;
                }
                bool lastAccel = properTime <= AccelerationDurations[i];
                properTime = Math.Min(properTime, AccelerationDurations[i]);
                t2         = Math.Sinh(properTime * accel) / accel;
                Vector3d velocity     = A * t2 / Math.Sqrt(1 + t2 * t2 * aSqr);
                Vector3d displacement = A * (Math.Sqrt(1 + t2 * t2 * aSqr) - 1) / aSqr;
                localObject = new Vector4d(t2, displacement);
                //Matrix5d M = new Matrix5d(LorentzBoost(velocity), new Vector4d(-t2 + properTime, displacement));
                observerToObjectBoost = prevBoost * observerToObjectBoost;
                simulPlane            = observerToObjectBoost.InvTransposePlaneMultiplication(new Plane {
                    Distance = observerTime
                });
                //prevBoost = new Matrix5d(LorentzBoost(velocity), new Vector4d(-t2, -displacement));
                prevBoost       = new Matrix5d(LorentzBoost(velocity), new Vector4d(-t2, displacement));          // -t2, -displacement));
                calculatedTime += properTime;
                if (lastAccel)
                {
                    break;
                }
                simulPlane = prevBoost.InvTransposePlaneMultiplication(simulPlane);
                offset    += Vector3.up * 2;
            }
            //Debug.DrawRay(Vector3.zero, simulPlane.Normal.Draw().normalized * (float)simulPlane.Distance);
            Vector4d objObserver = observerToObjectBoost * localObserver;
            //Debug.DrawRay(objObserver.Draw(), Vector3.up, Color.HSVToRGB((float)(observerTime / maxT + 0 / 3.0) % 1f, 1, 1));

            Vector4d right = observerToObjectBoost.Inverse().MultiplyDirection(new Vector4d {
                x = 1
            });
            Vector4d up = observerToObjectBoost.Inverse().MultiplyDirection(new Vector4d {
                y = 1
            });
            Vector4d forward = observerToObjectBoost.Inverse().MultiplyDirection(new Vector4d {
                t = 1
            });
            Vector4d origin = observerToObjectBoost * localObserver;

            /*
             * Debug.DrawRay(offset + origin.Draw(), right.Draw() / (float)a, Color.red);
             * Debug.DrawRay(offset + origin.Draw(), up.Draw() * (float)deltaT, Color.green);
             * Debug.DrawRay(offset + origin.Draw(), forward.Draw() * (float)deltaT, Color.blue);
             * Debug.DrawRay(offset + origin.Draw(), -right.Draw()/ (float)a, Color.red);
             * Debug.DrawRay(offset + origin.Draw(), -up.Draw() * (float)deltaT, Color.green);
             * Debug.DrawRay(offset + origin.Draw(), -forward.Draw() * (float)deltaT, Color.blue);
             */

            //Debug.DrawRay(offset + localObject.Draw(), Vector3.up*0.3f, Color.HSVToRGB((float)(observerTime / maxT + i / 3.0) % 1f, 1, 1));

            Vector4d objInObserverFrame = observerToObjectBoost.Inverse() * localObject;
            Debug.DrawRay(objInObserverFrame.Draw(), Vector3.up * 2, Color.HSVToRGB((float)(observerTime / maxT + i / 3.0) % 1f, 1, 1));
        }
    }
 public Matrix5d()
 {
     Transformation = Matrix4d.Identity();
     Translation    = new Vector4d();
 }