/// <summary>
    /// Compute the axis of the orbit by taking r x v and normalizing.
    /// </summary>
    /// <returns></returns>
    public Vector3d GetAxis()
    {
        GravityEngine ge = GravityEngine.Instance();
        Vector3d      r  = ge.GetPositionDoubleV3(nbody) - ge.GetPositionDoubleV3(centralMass);
        Vector3d      v  = GravityEngine.Instance().GetVelocityDoubleV3(nbody) - ge.GetVelocityDoubleV3(centralMass);

        return(Vector3d.Normalize(Vector3d.Cross(r, v)));
    }
    public LambertBattin(OrbitData _fromOrbit, OrbitData _toOrbit) : base(_fromOrbit, _toOrbit)
    {
        name = "LambertBattin";
        // Fundamentals of Astrodynamics and Applications, Vallado, 4th Ed., Algorithm 56 p475
        // Take r0, r => a_min, e_min, t_min, v0

        GravityEngine ge = GravityEngine.Instance();

        // If Nbody is available get position directly. If not (target marker) then
        // use orbit phase to compute position

        center3d = ge.GetPositionDoubleV3(_fromOrbit.centralMass);

        if (fromOrbit.nbody != null)
        {
            r1        = ge.GetPositionDoubleV3(fromOrbit.nbody);
            r1crossv1 = _fromOrbit.GetAxis();
        }
        else
        {
            r1 = new Vector3d(toOrbit.GetPhysicsPositionforEllipse(fromOrbit.phase));
            Debug.LogError("Code incomplete need to get v1");
        }

        if (toOrbit.nbody != null)
        {
            r2 = ge.GetPositionDoubleV3(toOrbit.nbody);
        }
        else
        {
            r2 = new Vector3d(toOrbit.GetPhysicsPositionforEllipse(toOrbit.phase));
        }

        r1 = r1 - center3d;
        r2 = r2 - center3d;

        if (fromOrbit.a < toOrbit.a)
        {
            innerOrbit   = fromOrbit;
            outerOrbit   = toOrbit;
            innerToOuter = true;
        }
        else
        {
            innerOrbit   = toOrbit;
            outerOrbit   = fromOrbit;
            innerToOuter = false;
        }
        fromNBody = fromOrbit.nbody;
        mu        = ge.GetPhysicsMass(fromOrbit.centralMass);
    }
    public CircularizeXfer(OrbitData fromOrbit) : base(fromOrbit)
    {
        name = "Circularize";
        GravityEngine ge = GravityEngine.Instance();

        // find velocity vector perpendicular to r for circular orbit
        Vector3d r_ship   = ge.GetPositionDoubleV3(fromOrbit.nbody);
        Vector3d v_ship   = ge.GetVelocityDoubleV3(fromOrbit.nbody);
        Vector3d r_center = ge.GetPositionDoubleV3(fromOrbit.centralMass);
        Vector3d v_center = ge.GetVelocityDoubleV3(fromOrbit.centralMass);

        Vector3d r = r_ship - r_center;

        // want velocity relative to central mass (it could be moving)
        Vector3d v = v_ship - v_center;

        // to get axis of orbit, can take r x v
        Vector3d axis = Vector3d.Cross(r, v).normalized;
        // vis visa for circular orbit
        double mu    = GravityEngine.Instance().GetMass(centerBody);
        double v_mag = Mathd.Sqrt(mu / r.magnitude);
        // positive v is counter-clockwise
        Vector3d v_dir      = Vector3d.Cross(axis, r).normalized;
        Vector3d v_circular = v_mag * v_dir;

        Maneuver m1;

        m1           = new Maneuver();
        m1.nbody     = fromOrbit.nbody;
        m1.mtype     = Maneuver.Mtype.vector;
        m1.velChange = (v_circular - v).ToVector3();
        m1.dV        = Vector3.Magnitude(m1.velChange);

        // maneuver positions and info for KeplerSeq conversion and velocity directions
        Vector3d h_unit = fromOrbit.GetAxis();

        m1.physPosition = r_ship;
        m1.relativePos  = r_ship - r_center;
        m1.relativeVel  = v_circular.magnitude * Vector3d.Cross(h_unit, m1.relativePos).normalized;
        m1.relativeTo   = fromOrbit.centralMass;

        //Debug.LogFormat("v_ship={0} v_center={1} v_c.x={2}", vel_ship, vel_center, v_center[0]);
        //Debug.Log(string.Format("v_ship={0} v_circular={1} axis={2} v_dir={3} velChange={4}",
        //    vel_ship, v_circular, axis, v_dir, m1.velChange));
        m1.worldTime = ge.GetPhysicalTime();
        maneuvers.Add(m1);
    }
Beispiel #4
0
    // Update is called once per frame
    void Update()
    {
        // on a deferred add (while running) may get an OP update before actually added to GE. This would be bad.
        if (nbody.engineRef == null)
        {
            return;
        }

        // TODO: optimize/accuracy: if on rails could use orbitU (or KS) directly
        // Now there is MapToRender MUST use physics and not transform position
        Vector3 centerPos = GravityEngine.Instance().GetPhysicsPosition(aroundNBody);

        // Is the resulting orbit and ellipse or hyperbola?
        bool     mapToScene = true;
        Vector3d pos        = ge.GetPositionDoubleV3(nbody);
        Vector3d vel;

        if (velocityFromScript)
        {
            vel = new Vector3d(velocity);
        }
        else
        {
            vel = ge.GetVelocityDoubleV3(nbody);
        }

        orbitU.InitFromRVT(pos, vel, ge.GetPhysicalTimeDouble(), aroundNBody, false);

        // Add lines to the inclination=0 plane of the orbit
        Vector3[] points      = orbitU.OrbitPositions(numPoints, centerPos, mapToScene, hyperDisplayRadius);
        int       totalPoints = numPoints + 2 * numPlaneProjections;

        if (numPlaneProjections > 0)
        {
            Vector3[] pointsWithProj = new Vector3[totalPoints];
            int       projEvery      = numPoints / numPlaneProjections;
            int       p      = 0;
            int       orbitP = 0;
            while (p < totalPoints)
            {
                pointsWithProj[p++] = points[orbitP];
                if ((orbitP % projEvery) == 0)
                {
                    // add a line to plane and back
                    pointsWithProj[p++] = Vector3.ProjectOnPlane(points[orbitP], planeNormal);
                    pointsWithProj[p++] = points[orbitP];
                }
                orbitP++;
            }
            lineR.SetPositions(pointsWithProj);
        }
        else
        {
            lineR.SetPositions(points);
        }
    }
    public LambertBattin(NBody fromBody, NBody centerNBody, Vector3d r_from, Vector3d r_to, Vector3d fromAxis) : base()
    {
        name = "LambertBattin";

        fromNBody = fromBody;
        GravityEngine ge = GravityEngine.Instance();

        centerBody = centerNBody;

        center3d = ge.GetPositionDoubleV3(centerNBody);
        mu       = ge.GetPhysicsMass(centerNBody);

        r1        = r_from - center3d;
        r1crossv1 = fromAxis;
        r2        = r_to - center3d;

        innerOrbit   = fromOrbit;
        innerToOuter = true;
    }
    /// <summary>
    /// Create a Lambert transfer from a given NBody orbit to a position in physics space.
    ///
    /// The constructor will compute the minimum energy transfer to the point taking the shortest
    /// path setting into account. The time for the transfer can then be retreived.
    /// </summary>
    /// <param name="_fromOrbit"></param>
    /// <param name="r_from"></param>
    /// <param name="r_to"></param>
    /// <param name="shortPath"></param>
    public LambertUniversal(OrbitData _fromOrbit, Vector3d r_from, Vector3d r_to, bool shortPath) : base(_fromOrbit)
    {
        name = "LambertUniversal";

        GravityEngine ge = GravityEngine.Instance();

        center3d = ge.GetPositionDoubleV3(centerBody);
        r1       = r_from - center3d;
        r2       = r_to - center3d;

        fromOrbit    = _fromOrbit;
        innerOrbit   = fromOrbit;
        innerToOuter = true;
        mu           = ge.GetPhysicsMass(fromOrbit.centralMass);
        // determine t_min.
        double[] r0 = new double[] { r1.x, r1.y, r1.z };
        double[] r  = new double[] { r2.x, r2.y, r2.z };
        ComputeMinTime(ref r0, ref r, _fromOrbit.centralMass, shortPath);
    }
 // Update is called once per frame
 void Update()
 {
     if (Input.GetKeyDown(KeyCode.Space))
     {
         // toggle the engine
         if (rocketEngine.engineOn)
         {
             rocketEngine.SetEngine(false);
             ge.BodyOnRails(shipNbody, centerNbody);
         }
         else
         {
             rocketEngine.SetEngine(true);
             Vector3d vel = ge.GetVelocityDoubleV3(shipNbody);
             rocketEngine.SetThrustAxis(vel.ToVector3().normalized);
             ge.BodyOffRails(shipNbody, ge.GetPositionDoubleV3(shipNbody), vel);
         }
     }
 }
    private void Deactivate()
    {
        // Get CM object pos & vel from GE
        GravityEngine ge    = GravityEngine.Instance();
        Vector3d      cmPos = ge.GetPositionDoubleV3(cmNbody);
        Vector3d      cmVel = ge.GetVelocityDoubleV3(cmNbody);

        int i = 0;

        foreach (NBody nbody in nbodies)
        {
            Rigidbody rb = nbody.GetComponentInChildren <Rigidbody>();
            if (rb == null)
            {
                Debug.LogWarning("could not find rigidbody for  " + nbody.gameObject.name);
                continue;
            }
            // rb.isKinematic = true;
            // set position and velocity
            Vector3d nbodyVel = new Vector3d(GravityScaler.ScaleVelSceneToPhys(rb.velocity));
            ge.SetVelocityDoubleV3(nbody, cmVel + nbodyVel);
            Vector3d nbodyPos = new Vector3d(GravityScaler.ScalePositionSceneToPhys(nbody.transform.localPosition));
            ge.SetPositionDoubleV3(nbody, cmPos + nbodyPos);
            ge.ActivateBody(nbody.gameObject);
            // restore parent
            nbody.transform.parent = priorParents[i];
            i++;
        }
        ge.RemoveBody(cmObject);
        Destroy(cmObject);
        // de-activate any RCS elements
        foreach (ReactionControlSystem r in rcs)
        {
            if (r != null)
            {
                r.SetRigidBodyEnabled(false);
            }
        }
    }
    // Update is called once per frame
    void Update()
    {
        // Now there is MapToRender MUST use physics and not transform position
        Vector3 centerPos = GravityEngine.Instance().GetPhysicsPosition(aroundNBody);

        // Is the resulting orbit and ellipse or hyperbola?
        bool     mapToScene = true;
        Vector3d pos        = ge.GetPositionDoubleV3(nbody);
        Vector3d vel;

        if (velocityFromScript)
        {
            vel = new Vector3d(velocity);
        }
        else
        {
            vel = ge.GetVelocityDoubleV3(nbody);
        }

        if (destination != null)
        {
            // since the segment code just uses this for the angle, the scale does not matter
            destPoint = destination.transform.position;
        }

        orbitU.InitFromRVT(pos, vel, ge.GetPhysicalTimeDouble(), aroundNBody, false);
        // TODO: Decide on best segment approach. Common for hyperbola vs ellipse ??
        Vector3[] positions;
        if (orbitU.eccentricity < 1.0)
        {
            positions = orbitU.EllipseSegment(numPoints, centerPos, pos.ToVector3(), destPoint, shortPath);
        }
        else
        {
            float radius = (pos.ToVector3() - centerPos).magnitude;
            positions = orbitU.HyperSegmentSymmetric(numPoints, centerPos, radius, mapToScene);
        }
        lineR.SetPositions(positions);
    }
    /// <summary>
    /// Computes the transfer and updates all the ghost bodies.
    /// </summary>
    /// <returns></returns>
    private void ComputeTransfer()
    {
        double timeNow = ge.GetPhysicalTimeDouble();

        // First using the transfer time, move the ghost Moon to position at SOI arrival.
        // Call evolve via LockAtTime on the ghostMoon to move it. Set position based on this.
        double t_flight  = tflightFactor * timeHohmann;
        double timeatSoi = timeNow + t_flight;

        ghostMoonOrbit[MOON_SOI_ENTER].LockAtTime(timeatSoi);
        // Determine the moon phase angle
        double moonPhase = ghostMoonSoiEnterOrbitPredictor.GetOrbitUniversal().phase;

        // Place the TLI ship at the user-requested angle wrt planet-moon line
        // Put ghost ship in same orbit geometry as the moon, assuming it is circular. Then
        // can use same phase value.
        // (Ship needs to reach this departure point, it may not even be on the ship orbit
        //  in general).
        ghostShipOrbit[TLI].phase       = shipTLIAngleDeg + (moonPhase + 180f);
        ghostShipOrbit[TLI].inclination = ghostMoonOrbit[MOON_SOI_ENTER].inclination;
        ghostShipOrbit[TLI].omega_lc    = ghostMoonOrbit[MOON_SOI_ENTER].omega_lc;
        ghostShipOrbit[TLI].omega_uc    = ghostMoonOrbit[MOON_SOI_ENTER].omega_uc;
        ghostShipOrbit[TLI].p_inspector = shipOrbit.p;
        ghostShipOrbit[TLI].Init();
        ghostShipOrbit[TLI].LockAtTime(0);

        // Place the SOI enter ship at the user-requested angle in an SOI orbit. Lock at time 0 so the phase
        // is held per the user input.
        ghostShipOrbit[ENTER_SOI].phase       = soiAngleDeg + moonPhase;
        ghostShipOrbit[ENTER_SOI].inclination = soiInclination + shipOrbit.inclination;
        ghostShipOrbit[ENTER_SOI].omega_lc    = ghostMoonOrbit[MOON_SOI_ENTER].omega_lc;
        ghostShipOrbit[ENTER_SOI].omega_uc    = ghostMoonOrbit[MOON_SOI_ENTER].omega_uc;
        ghostShipOrbit[ENTER_SOI].Init();
        ghostShipOrbit[ENTER_SOI].LockAtTime(0);

        // Find the line to the ENTER_SOI position. Ship departs from that line continued through planet
        // at the shipRadius distance (assumes circular ship orbit)
        // TODO: Handle planet not at (0,0,0)
        Vector3d soiEntryPos    = ge.GetPositionDoubleV3(ghostShip[ENTER_SOI]);
        Vector3d planetPos      = ge.GetPositionDoubleV3(planet);
        Vector3d departurePoint = ge.GetPositionDoubleV3(ghostShip[TLI]);

        // Use Lambert to find the departure velocity to get from departure to soiEntry
        // Since we need 180 degrees from departure to arrival, use LambertBattin
        lambertB = new LambertBattin(ghostShip[TO_MOON], planet, departurePoint, soiEntryPos, shipOrbit.GetAxis());

        // apply any time of flight change
        bool reverse = !shortPath;

        const bool df    = false;
        const int  nrev  = 0;
        int        error = lambertB.ComputeXfer(reverse, df, nrev, t_flight);

        if (error != 0)
        {
            Debug.LogWarning("Lambert failed to find solution. error=" + error);
            return;
        }
        // Check Lambert is going in the correct direction
        //Vector3 shipOrbitAxis = Vector3.Cross(ge.GetPhysicsPosition(spaceship), ge.GetVelocity(spaceship) ).normalized;
        //Vector3 tliOrbitAxis = Vector3.Cross(departurePoint.ToVector3(), lambertB.GetTransferVelocity().ToVector3()).normalized;
        Vector3 shipOrbitAxis = Vector3.Cross(ge.GetVelocity(spaceship), ge.GetPhysicsPosition(spaceship)).normalized;
        Vector3 tliOrbitAxis  = Vector3.Cross(lambertB.GetTransferVelocity().ToVector3(), departurePoint.ToVector3()).normalized;

        if (Vector3.Dot(shipOrbitAxis, tliOrbitAxis) < 0)
        {
            error = lambertB.ComputeXfer(!reverse, df, nrev, t_flight);
            if (error != 0)
            {
                Debug.LogWarning("Lambert failed to find solution for reverse path. error=" + error);
                return;
            }
        }
        Debug.LogFormat("tli_vel={0}", lambertB.GetTransferVelocity());

        ghostShipOrbit[TO_MOON].InitFromRVT(departurePoint, lambertB.GetTransferVelocity(), timeNow, planet, false);

        // Set velocity for orbit around moon. Will be updated every frame
        ghostShipOrbit[SOI_HYPER].InitFromRVT(soiEntryPos, lambertB.GetFinalVelocity(), timeNow, ghostMoon[MOON_SOI_ENTER], false);

        // Find the exit point of the hyperbola in the SOI
        OrbitUtils.OrbitElements oe = OrbitUtils.RVtoCOE(soiEntryPos, lambertB.GetFinalVelocity(), ghostMoon[MOON_SOI_ENTER], false);
        Vector3d soiExitR           = new Vector3d();
        Vector3d soiExitV           = new Vector3d();

        OrbitUtils.COEtoRVMirror(oe, ghostMoon[MOON_SOI_ENTER], ref soiExitR, ref soiExitV, false);

        // Find time to go around the moon. TOF requires relative positions!!
        Vector3d ghostSoiEnterPos   = ge.GetPositionDoubleV3(ghostMoon[MOON_SOI_ENTER]);
        Vector3d soiEnterRelative   = soiEntryPos - ghostSoiEnterPos;
        Vector3d soiExitRelative    = soiExitR - ghostSoiEnterPos;
        Vector3d soiExitVelRelative = soiExitV - ge.GetVelocityDoubleV3(ghostMoon[MOON_SOI_ENTER]);
        double   hyperTOF           = ghostShipOrbit[SOI_HYPER].TimeOfFlight(soiEnterRelative, soiExitRelative);

        // Position the ghost moon for SOI exit (timeAtSoi includes timeNow)
        t_soiExit = timeatSoi + hyperTOF;
        ghostMoonOrbit[MOON_SOI_EXIT].LockAtTime(t_soiExit);

        // Set position and vel for exit ship, so exit orbit predictor can run.
        Vector3d ghostMoonSoiExitPos = ge.GetPositionDoubleV3(ghostMoon[MOON_SOI_EXIT]);
        Vector3d ghostMoonSoiExitVel = ge.GetVelocityDoubleV3(ghostMoon[MOON_SOI_EXIT]);

        ghostShipOrbit[EXIT_SOI].InitFromRVT(soiExitRelative + ghostMoonSoiExitPos,
                                             soiExitVelRelative + ghostMoonSoiExitVel,
                                             timeNow, planet, false);
    }
    /// <summary>
    /// Take the Nbody objects in the nbodies list set them inactive and make them children of a new
    /// NBody object moving as the CM of the nbodies. This allows RigidBody mechanics during close
    /// encounters.
    /// </summary>
    private void Activate()
    {
        if (nbodies.Length < 2)
        {
            Debug.LogError("Need two or more nbodies");
            return;
        }

        GravityEngine ge = GravityEngine.Instance();

        // Step 1: calculate CM position and velocity
        Vector3d cmPos = new Vector3d(0, 0, 0);
        Vector3d cmVel = new Vector3d(0, 0, 0);

        float mass = 0f;

        rigidBodies = new Rigidbody[nbodies.Length];

        // RigidBody is assumed to be attached to one of the children (to keep model scale independent)
        int i = 0;

        foreach (NBody nbody in nbodies)
        {
            rigidBodies[i] = nbody.GetComponentInChildren <Rigidbody>();
            //rigidBodies[i] = nbody.GetComponent<Rigidbody>();
            if (rigidBodies[i] == null)
            {
                Debug.LogError("Abort - No rigidbody detected on " + nbody.gameObject.name);
                return;
            }
            mass  += rigidBodies[i].mass;
            cmPos += rigidBodies[i].mass * ge.GetPositionDoubleV3(nbody);
            cmVel += rigidBodies[i].mass * ge.GetVelocityDoubleV3(nbody);
            i++;
        }
        cmPos /= mass;
        cmVel /= mass;
        Debug.LogFormat("CM p={0} v={1} mass={2}", cmPos.ToVector3(), cmVel.ToVector3(), mass);

        // Step2: Inactivate the NBodies and make children of a new NBody object
        priorParents = new Transform[nbodies.Length];
        cmObject     = new GameObject("DockingGroupCM");
        cmNbody      = cmObject.AddComponent <NBody>();

        // Set cm pos/vel
        // NBody InitPosition will use transform or initialPos base on units. Set both.
        cmNbody.initialPos         = cmPos.ToVector3() * ge.physToWorldFactor;
        cmNbody.transform.position = cmNbody.initialPos;
        ge.AddBody(cmObject);
        ge.SetVelocity(cmNbody, cmVel.ToVector3());
        Debug.LogFormat("set pos={0} actual={1}", cmPos.ToVector3(), ge.GetPhysicsPosition(cmNbody));
        i = 0;
        foreach (NBody nbody in nbodies)
        {
            Vector3d pos = ge.GetPositionDoubleV3(nbody);
            Vector3d vel = ge.GetVelocityDoubleV3(nbody);
            priorParents[i] = nbody.gameObject.transform.parent;
            ge.InactivateBody(nbody.gameObject);
            nbody.gameObject.transform.parent = cmObject.transform;
            // position wrt to CM. Need to convert to Unity scene units from GE Internal
            pos = (pos - cmPos) * ge.physToWorldFactor;
            vel = GravityScaler.ScaleVelPhysToScene(vel - cmVel);
            nbody.transform.localPosition = pos.ToVector3();
            rigidBodies[i].velocity       = vel.ToVector3();
            // rigidBodies[i].isKinematic = false;
            i++;
            Debug.LogFormat("body {0} p={1} v={2}", nbody.gameObject.name, pos.ToVector3(), vel.ToVector3());
        }
        // activate any RCS elements
        foreach (ReactionControlSystem r in rcs)
        {
            if (r != null)
            {
                r.SetRigidBodyEnabled(true);
            }
        }
    }