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; }
public bool IsStateSet(TCAState state) { return(Available && VSL.IsStateSet(state)); }
public bool IsStateSet(TCAState state) { return(VSL.IsStateSet(state)); }
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; }