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;
}
