예제 #1
0
        protected override void Update()
        {
            NeededHorVelocity = HSC == null? Vector3d.zero : VSL.HorizontalSpeed.NeededVector;
            var zero_needed = NeededHorVelocity.sqrMagnitude <= 0.01;

            //check boundary conditions
            if (ViewAngle > RAD.DownViewAngle)
            {
                if (BestHit.Valid)
                {
                    DetectedHit.Copy(BestHit);
                    rewind();
                }
                else
                {
                    Reset();
                }
            }
            else if (AngleDelta < RAD.MinAngleDelta || LittleSteps > RAD.MaxLittleSteps)
            {
                if (BestHit.Valid)
                {
                    DetectedHit.Copy(BestHit);
                }
                rewind();
            }
            //calculate closing speed and initial ray direction
            Dir             = Vector3.zero;
            LookAheadTime   = Utils.ClampL(RAD.LookAheadTime / VSL.OnPlanetParams.MaxTWR * VSL.Engines.AccelerationTime90, 10);
            SurfaceVelocity = VSL.PredictedSrfVelocity(GLB.CPS.LookAheadTime);
            var SurfaceVelocityDir = SurfaceVelocity.normalized;
            var SurfaceSpeed       = (float)SurfaceVelocity.magnitude;
            var alt_threshold      = VSL.Altitude.Absolute -
                                     Mathf.Min(Utils.ClampL(CFG.DesiredAltitude - 1, 0), VSL.Geometry.H * RAD.MinAltitudeFactor * (CollisionSpeed < 0? 1 : 2));

            if ((DistanceAhead < 0 || DistanceAhead > RAD.MinDistanceAhead ||
                 Vector3.Dot(RelObstaclePosition, NeededHorVelocity) < 0) &&
                (VSL.HorizontalSpeed >= RAD.MaxClosingSpeed ||
                 zero_needed &&
                 VSL.HorizontalSpeed >= RAD.MinClosingSpeed))
            {
                Dir = VSL.HorizontalSpeed.normalized;
                if (VSL.IsStateSet(TCAState.LoosingAltitude))
                {
                    Dir = Vector3.Lerp(Dir, SurfaceVelocityDir,
                                       LookAheadTime / (VSL.Altitude / -VSL.VerticalSpeed.Relative) / VSL.OnPlanetParams.MaxDTWR);
                }
            }
            else
            {
                Dir = zero_needed?
                      Vector3d.Exclude(VSL.Physics.Up, VSL.OnPlanetParams.Fwd).normalized :
                      NeededHorVelocity.normalized;
            }
            Dir.Normalize();
            ClosingSpeed = Utils.ClampL(SurfaceSpeed > NeededHorVelocity.magnitude?
                                        Vector3.Dot(SurfaceVelocity, Dir) :
                                        Vector3.Dot(NeededHorVelocity, Dir), 0);
            //calculate ray length
            var ray_speed = CollisionSpeed < ClosingSpeed? ClosingSpeed : CollisionSpeed;

            if (VSL.Info.Destination.IsZero())
            {
                MaxDistance = ray_speed * LookAheadTime;
            }
            else
            {
                MaxDistance = VSL.Info.Destination.magnitude + ray_speed * GLB.CPS.LookAheadTime;
            }
            if (ViewAngle < 0)
            {
                MaxDistance /= Mathf.Cos(ViewAngle * Mathf.Deg2Rad);
                if (ClosingSpeed > RAD.MinClosingSpeed)
                {
                    MaxDistance *= Utils.ClampL((ClosingSpeed - RAD.MinClosingSpeed) / RAD.UpViewSlope * (-ViewAngle / RAD.UpViewAngle), 1);
                }
            }
            MaxDistance = Mathf.Max(MaxDistance, VSL.Geometry.D);
            //cast the sweep, the velocity and direct path rays
            if (ViewAngle < 0 && !zero_needed)
            {
                DirectPathRay.Cast(VSL.Physics.wCoM, Dir, MaxDistance, VSL.Geometry.R * 1.1f);
                if (!DirectPathRay.Valid || !DirectPathRay.BeforeDestination(VSL, NeededHorVelocity))
                {
                    ViewAngle = 0;
                }
            }
            CurHit.Cast(Dir, ViewAngle, MaxDistance);
            VelocityRay.Cast(VSL.Physics.wCoM, VSL.vessel.srf_vel_direction, (float)VSL.vessel.srfSpeed * GLB.CPS.LookAheadTime * 3, VSL.Geometry.R * 1.1f);
            //check the hit
            if (CurHit.BeforeDestination(SurfaceVelocity))
            {
                if (CurHit.Maneuver == Sweep.ManeuverType.Horizontal &&
                    (mode & Mode.Horizontal) == Mode.Horizontal)
                {
                    LastHitValid = false;
                    //check if it is indeed a collision
                    if (CurHit.Altitude > alt_threshold)
                    {
                        //queue avoiding maneuver with CPS
                        SideCollision = true;
                        var      collision_point = Vector3.ProjectOnPlane(CurHit.Obstacle.RelPosition(VSL.Physics.wCoM), VSL.Physics.Up);
                        var      dist            = collision_point.magnitude;
                        Vector3d maneuver;
                        if (CollisionPreventionSystem.AvoidStatic(VSL, collision_point / dist, dist,
                                                                  Vector3d.Exclude(VSL.Physics.Up, SurfaceVelocity), out maneuver))
                        {
                            if (Vector3d.Dot(SideManeuver, maneuver) > 0 ||
                                SideManeuver.sqrMagnitude < maneuver.sqrMagnitude)
                            {
                                SideManeuver = maneuver;
                            }
                        }
                    }
                }
                else
                {
                    if (CurHit > BestHit)
                    {
                        BestHit.Copy(CurHit);
                    }
                    if (BestHit.Valid && BestHit > DetectedHit)
                    {
                        DetectedHit.Copy(BestHit);
                    }
                    //rewind the ray one step and decrease the delta if direct collision detected
                    if (LastHitValid)
                    {
                        ViewAngle -= 2 * AngleDelta;
                    }
                    else
                    {
                        LastHitValid = true;
                        ViewAngle   -= AngleDelta;
                        AngleDelta  /= 2;
                    }
                }
            }
            else if (!CurHit.Valid)
            {
                if (LastHitValid)
                {
                    AngleDelta /= 2;
                }
                LastHitValid = false;
            }
            if (AngleDelta < RAD.AngleDelta)
            {
                LittleSteps++;
            }
            //probe for surface height
            Altimeter.ProbeHeightAhead(Dir);
            //update collision info if detected something
            TimeAhead           = -1;
            DistanceAhead       = -1;
            RelObstaclePosition = Vector3.zero;
            Obstacle            = DetectedHit.Obstacle;
            if (Altimeter.BeforeDestination(SurfaceVelocity) &&
                DetectedHit.Obstacle < Altimeter.Obstacle)
            {
                Obstacle = Altimeter.Obstacle;
            }
            if (VelocityRay.Valid)
            {
                VelocityRay.ClaculateAltitude(VSL.Physics.Up, VSL.Altitude.Absolute);
                VelocityHit = new TerrainPoint(VelocityRay.Altitude, VelocityRay.CollisionPoint);
                if (VelocityRay.Altitude > Obstacle.Altitude || VelocityRay.Altitude > alt_threshold)
                {
                    Obstacle = VelocityHit;
                }
            }
            if (Obstacle.Valid)
            {
                VSL.Altitude.Ahead  = (float)Obstacle.Altitude;
                RelObstaclePosition = Obstacle.HorPosition(VSL);
                DistanceAhead       = Utils.ClampL(RelObstaclePosition.magnitude - VSL.Geometry.R, 0.1f);
            }
//			if(Obstacle.Valid) VSL.Info.AddCustopWaypoint(Obstacle.Position, "Obstacle: "+Utils.formatBigValue((float)Obstacle.Altitude, "m"));//debug
            //if on side collision course, correct it
            if (HSC != null && !SideManeuver.IsZero())
            {
                HSC.AddWeightedCorrection(SideManeuver);
            }
//			Log("Obstacle! {}, SrfSpeed {}, NeedeSpeed {}, ClosingSpeed {}, MaxDistance {}\n" +
//				"CurHit {}\nBestHit {}\nDetectedHit {}\nRayObstacle {}\nAltObstacle {}\nForwardRay {}",
//			    VSL.Altitude.Ahead > alt_threshold, SurfaceSpeed, NeededHorVelocity.magnitude, ClosingSpeed, MaxDistance,
//			    CurHit, BestHit, DetectedHit, Obstacle, Altimeter.Obstacle, ForwardRay);//debug
            //check for possible stright collision
            if (VSL.Altitude.Ahead > alt_threshold)
            {
                if (CollisionSpeed < ClosingSpeed)
                {
                    CollisionSpeed = ClosingSpeed;
                }

                TimeAhead = DistanceAhead / Utils.ClampL(Vector3.Dot(SurfaceVelocity, RelObstaclePosition.normalized), 1e-5f);
//				Log("DistAhead {}, Speed {}, Time {}", DistanceAhead,
//				    Utils.ClampL(Vector3.Dot(SurfaceVelocity, RelObstaclePosition.normalized), 1e-5f),
//				    TimeAhead);//debug
                if (HSC != null)
                {
                    Vector3d dV;
                    if (DistanceAhead > RAD.MinDistanceAhead)
                    {
                        dV = Vector3d.Project(SurfaceVelocity, RelObstaclePosition) *
                             -Math.Sqrt(1 - Utils.ClampH(DistanceAhead / ClosingSpeed / LookAheadTime * VSL.OnPlanetParams.MaxTWR * RAD.NHVf, 1));
                    }
                    else if (DistanceAhead > RAD.MinDistanceAhead / 2)
                    {
                        dV = -NeededHorVelocity;
                    }
                    else if (Vector3d.Dot(SurfaceVelocity, RelObstaclePosition) > 0)
                    {
                        dV = Vector3d.Project(SurfaceVelocity, RelObstaclePosition) *
                             -RAD.MinDistanceAhead / DistanceAhead * RAD.PitchRollAAf / VSL.Torque.MaxPitchRoll.AA_rad;
                    }
                    else
                    {
                        dV = -NeededHorVelocity;
                    }
                    HSC.AddRawCorrection(dV);
                }
            }
            else
            {
                if (VelocityHit.Valid)
                {
//					VSL.Info.AddCustopWaypoint(DirectHit.Position, "DirectHit");//debug
                    VSL.Altitude.LowerThreshold = (float)VelocityHit.Altitude;
                    var rel_pos = VelocityHit.HorPosition(VSL);
                    if (HSC != null &&
                        !VSL.HorizontalSpeed.MoovingFast && !VSL.Info.Destination.IsZero() &&
                        VelocityHit.Altitude - VSL.Altitude.TerrainAltitude > 1 &&
                        Vector3.Dot(VSL.Info.Destination, rel_pos - VSL.Info.Destination) < 0)
                    {
                        var dV = rel_pos.normalized * GLB.HSC.TranslationMaxDeltaV;
                        if (Vector3.Dot(rel_pos, VSL.Info.Destination) > 0)
                        {
                            HSC.AddRawCorrection(Vector3.Project(dV, VSL.Info.Destination) * 2 - dV);
                        }
                        else
                        {
                            HSC.AddRawCorrection(-dV);
                        }
                    }
                    if (!VelocityRay.Valid &&
                        (VSL.HorizontalSpeed.MoovingFast || rel_pos.magnitude > VSL.Geometry.R))
                    {
                        VelocityHit.Reset();
                    }
                }
                CollisionSpeed = -1;
            }
            //update angle for the next ray
            ViewAngle += AngleDelta;
        }
        protected override void Update()
        {
            if (!IsActive)
            {
                return;
            }
            var NeededHorVelocity = HSC == null? Vector3d.zero : VSL.HorizontalSpeed.NeededVector;
            var zero_needed       = NeededHorVelocity.sqrMagnitude <= 0.01;

            //check boundary conditions
            if (ViewAngle > RAD.DownViewAngle)
            {
                if (BestHit.Valid)
                {
                    DetectedHit.Copy(BestHit);
                    rewind();
                }
                else
                {
                    reset();
                }
            }
            else if (AngleDelta < RAD.MinAngleDelta || LittleSteps > RAD.MaxLittleSteps)
            {
                if (BestHit.Valid)
                {
                    DetectedHit.Copy(BestHit);
                }
                rewind();
            }
            //calculate closing speed and initial ray direction
            Dir = Vector3.zero;
            var alt_threshold = VSL.Altitude.Absolute - Mathf.Min(CFG.DesiredAltitude, VSL.Geometry.H * RAD.MinAltitudeFactor * (CollisionSpeed < 0? 1 : 2));

            SurfaceVelocity = VSL.PredictedSrfVelocity(GLB.CPS.LookAheadTime);
            if ((DistanceAhead < 0 || DistanceAhead > RAD.MinDistanceAhead ||
                 Vector3.Dot(RelObstaclePosition, NeededHorVelocity) < 0) &&
                (VSL.HorizontalSpeed >= RAD.MaxClosingSpeed ||
                 zero_needed &&
                 VSL.HorizontalSpeed >= RAD.MinClosingSpeed))
            {
                Dir = VSL.HorizontalSpeed.normalized;
                if (VSL.IsStateSet(TCAState.LoosingAltitude))
                {
                    Dir = Vector3.Lerp(Dir, SurfaceVelocity.normalized,
                                       RAD.LookAheadTime / (VSL.Altitude / -VSL.VerticalSpeed.Relative) / VSL.OnPlanetParams.MaxDTWR);
                }
            }
            else
            {
                Dir = zero_needed?
                      Vector3d.Exclude(VSL.Physics.Up, VSL.OnPlanetParams.Fwd).normalized :
                      NeededHorVelocity.normalized;
            }
            Dir.Normalize();
            ClosingSpeed = Utils.ClampL(Vector3.Dot(SurfaceVelocity, Dir), RAD.MinClosingSpeed);
            //cast the sweep and the fwd ray
            MaxDistance = (CollisionSpeed < ClosingSpeed? ClosingSpeed : CollisionSpeed) * RAD.LookAheadTime;
            if (ViewAngle < 0)
            {
                MaxDistance = MaxDistance / Mathf.Cos(ViewAngle * Mathf.Deg2Rad) * (1 + ClosingSpeed / RAD.UpViewSlope * Utils.ClampL(-ViewAngle / RAD.UpViewAngle, 0));
            }
            CurHit.Cast(Dir, ViewAngle, MaxDistance);
            ForwardRay.Cast(VSL.Physics.wCoM, SurfaceVelocity.normalized, (float)SurfaceVelocity.magnitude * GLB.CPS.LookAheadTime * 3, VSL.Geometry.D);
            //check the hit
            if (CurHit.BeforeDestination(SurfaceVelocity))
            {
                if (CurHit.Maneuver == Sweep.ManeuverType.Horizontal &&
                    (mode & Mode.Horizontal) == Mode.Horizontal)
                {
                    LastHitValid = false;
                    //check if it is indeed a collision
                    if (CurHit.Altitude > alt_threshold)
                    {
                        //queue avoiding maneuver with CPS
                        SideCollision = true;
                        var      collision_point = Vector3.ProjectOnPlane(CurHit.Obstacle.RelPosition(VSL.Physics.wCoM), VSL.Physics.Up);
                        var      dist            = collision_point.magnitude;
                        Vector3d maneuver;
                        if (CollisionPreventionSystem.AvoidStatic(VSL, collision_point / dist, dist,
                                                                  Vector3d.Exclude(VSL.Physics.Up, SurfaceVelocity), out maneuver))
                        {
                            if (Vector3d.Dot(SideManeuver, maneuver) > 0 ||
                                SideManeuver.sqrMagnitude < maneuver.sqrMagnitude)
                            {
                                SideManeuver = maneuver;
                            }
                        }
                    }
                }
                else
                {
                    if (CurHit > BestHit)
                    {
                        BestHit.Copy(CurHit);
                    }
                    if (BestHit.Valid && BestHit > DetectedHit)
                    {
                        DetectedHit.Copy(BestHit);
                    }
                    //rewind the ray one step and decrease the delta if direct collision detected
                    if (LastHitValid)
                    {
                        ViewAngle -= 2 * AngleDelta;
                    }
                    else
                    {
                        LastHitValid = true;
                        ViewAngle   -= AngleDelta;
                        AngleDelta  /= 2;
                    }
                }
            }
            else if (!CurHit.Valid)
            {
                if (LastHitValid)
                {
                    AngleDelta /= 2;
                }
                LastHitValid = false;
            }
            if (AngleDelta < RAD.AngleDelta)
            {
                LittleSteps++;
            }
            //if on side collision course, correct it
            if (HSC != null && !SideManeuver.IsZero())
            {
                HSC.AddWeightedCorrection(SideManeuver);
            }
            //probe for surface height
            Altimeter.ProbeHeightAhead(Dir);
            //update collision info if detected something
            TimeAhead           = -1;
            DistanceAhead       = -1;
            RelObstaclePosition = Vector3.zero;
            Obstacle            = DetectedHit.Obstacle;
            if (Altimeter.BeforeDestination(SurfaceVelocity) &&
                DetectedHit.Obstacle < Altimeter.Obstacle)
            {
                Obstacle = Altimeter.Obstacle;
            }
            if (ForwardRay.Valid)
            {
                ForwardRay.ClaculateAltitude(VSL.Physics.Up, VSL.Altitude.Absolute);
                if (ForwardRay.Altitude > Obstacle.Altitude || ForwardRay.Altitude > alt_threshold)
                {
                    Obstacle = new TerrainPoint(ForwardRay.Altitude, ForwardRay.CollisionPoint);
                }
            }
            if (Obstacle.Valid)
            {
                VSL.Altitude.Ahead = (float)Obstacle.Altitude;
            }
//			Log("\nCurHit {}\nBestHit {}\nDetectedHit {}\nRObstacle {}\nAObstacle {}\nForwardRay {}",
//			    CurHit, BestHit, DetectedHit, Obstacle, Altimeter.Obstacle, ForwardRay);//debug
            //check for possible stright collision
            if (VSL.Altitude.Ahead > alt_threshold)            //deadzone of twice the detection height
            {
                if (CollisionSpeed < ClosingSpeed)
                {
                    CollisionSpeed = ClosingSpeed;
                }
                RelObstaclePosition = Vector3.ProjectOnPlane(Obstacle.RelPosition(VSL.Physics.wCoM), VSL.Physics.Up);
                DistanceAhead       = Utils.ClampL(RelObstaclePosition.magnitude - VSL.Geometry.R, 0.1f);
                TimeAhead           = DistanceAhead / Vector3.Dot(SurfaceVelocity, RelObstaclePosition.normalized);
                if (HSC != null)
                {
                    Vector3d dV;
                    if (DistanceAhead > RAD.MinDistanceAhead)
                    {
                        dV = Vector3d.Project(SurfaceVelocity, RelObstaclePosition) *
                             -Math.Sqrt(1 - Utils.ClampH(DistanceAhead / ClosingSpeed / RAD.LookAheadTime * VSL.OnPlanetParams.MaxTWR * RAD.NHVf, 1));
                    }
                    else if (DistanceAhead > RAD.MinDistanceAhead / 2)
                    {
                        dV = -NeededHorVelocity;
                    }
                    else if (Vector3d.Dot(SurfaceVelocity, RelObstaclePosition) > 0)
                    {
                        dV = Vector3d.Project(SurfaceVelocity, RelObstaclePosition) *
                             -RAD.MinDistanceAhead / DistanceAhead * RAD.PitchRollAAf / VSL.Torque.MaxPitchRoll.AA_rad;
                    }
                    else
                    {
                        dV = -NeededHorVelocity;
                    }
                    HSC.AddRawCorrection(dV);
                }
            }
            else
            {
                CollisionSpeed = -1;
            }
            //update angle for the next ray
            ViewAngle += AngleDelta;
        }