public static bool AvoideVessel(VesselWrapper vsl, Vector3d dir, float dist, Vector3d dV,
float r2, Bounds exhaust2, Transform refT2,
out Vector3d maneuver, float threshold = -1)
{
maneuver = Vector3d.zero;
//filter vessels on non-collision courses
var dVn = dV.normalized;
var cosA = Mathf.Clamp(Vector3.Dot(dir, dVn), -1, 1);
var collision_dist = threshold.Equals(0)?
2*dist-vsl.Geometry.DistToBounds(vsl.Physics.wCoM+dir*dist)-
Mathf.Sqrt(exhaust2.SqrDistance(refT2.InverseTransformPoint(vsl.Physics.wCoM))) :
Mathf.Max(vsl.Geometry.R, dist-vsl.Geometry.DistToBounds(vsl.Physics.wCoM+dir*dist)) +
Mathf.Max(r2, dist-Mathf.Sqrt(exhaust2.SqrDistance(refT2.InverseTransformPoint(vsl.Physics.wCoM))));
// vsl.Log("R {}, R2 {}; r {}, r2 {}", vsl.Geometry.R, r2,
// dist-vsl.DistToBounds(vsl.wCoM+dir*dist),
// dist-Mathf.Sqrt(exhaust2.SqrDistance(refT2.InverseTransformPoint(vsl.wCoM))));//debug
// Utils.Log("{}: cosA: {}, threshold {}", vsl.vessel.vesselName, cosA, threshold);//debug
if(cosA <= 0) goto check_distance;
if(threshold < 0) threshold = CPS.MinDistance;
var sinA = Mathf.Sqrt(1-cosA*cosA);
var min_separation = dist*sinA;
var sep_threshold = collision_dist+threshold;
// Utils.Log("{}: min_sep > sep_thresh: {} > {}, threshold {}",
// vsl.vessel.vesselName, min_separation, sep_threshold, threshold);//debug
if(min_separation > sep_threshold) goto check_distance;
//calculate time to collision
var vDist = 0f;
var alpha = Mathf.Acos(cosA);
var dVm = dV.magnitude;
if(sinA <= 0) vDist = dist-sep_threshold;
else if(dist > sep_threshold) vDist = sep_threshold*Mathf.Sin(Mathf.Asin(min_separation/sep_threshold)-alpha)/sinA;
var vTime = Utils.ClampL(vDist, 0.1f)/dVm;
// Utils.Log("{}: vTime > SafeTime: {} > {}",
// vsl.vessel.vesselName, vTime, CPS.SafeTime/Utils.Clamp(Mathf.Abs(Vector3.Dot(vsl.wMaxAngularA, dVn)), 0.01f, 1f));//debug
if(vTime > SafeTime(vsl, dVn)) goto check_distance;
//calculate maneuver
Vector3d side;
if(cosA < 0.9) side = (dVn*cosA-dir).normalized;
else if(Math.Abs(Vector3d.Dot(dVn, vsl.Physics.Up)) < 0.9)
side = Vector3d.Cross(dVn, vsl.Physics.Up).normalized;
else side = Vector3d.Cross(dVn, vsl.OnPlanetParams.Fwd).normalized;
// if(dist > sep_threshold)
// {
// var beta = Mathf.Asin(sep_threshold/dist)-alpha;
// maneuver = (Mathf.Sin(beta)*side + dVn*(Mathf.Cos(beta)-1)).normalized;
// }
// else
maneuver = side;
maneuver += vsl.Physics.Up*Mathf.Sign(Vector3.Dot(dir, vsl.Physics.Up))*(min_separation/sep_threshold-1);
maneuver *= (sep_threshold-min_separation) / Math.Sqrt(vTime);
// maneuver = (-vsl.Up*Mathf.Sign(Vector3.Dot(dir, vsl.Up))*(1-min_separation/sep_threshold) +
// (dVn*cosA-dir).normalized).normalized * (sep_threshold-min_separation) / vTime;
// vsl.Log("\ndist {}\ndV {}\nmaneuver: {}\n" +
// "vTime {}, vDist {}, min sep {}, sep_thresh {}",
// dir*dist, dV, maneuver, vTime, vDist, min_separation, sep_threshold);//debug
#if DEBUG
Collided |= dist < collision_dist;
#endif
//if distance is not safe, correct course anyway
check_distance:
var collision = !maneuver.IsZero();
dist -= collision_dist;
if(dist < threshold)
{
var dist_to_safe = Utils.ClampH(dist-threshold, -0.01f);
var dc = dir*dist_to_safe;
if(vsl.HorizontalSpeed.NeededVector.sqrMagnitude > CPS.LatAvoidMinVelSqr)
{
var lat_avoid = Vector3d.Cross(vsl.Physics.Up, vsl.HorizontalSpeed.NeededVector.normalized);
dc = Vector3d.Dot(dc, lat_avoid) >= 0?
lat_avoid*dist_to_safe :
lat_avoid*-dist_to_safe;
}
if(dc.sqrMagnitude > 0.25) maneuver += dc/CPS.SafeTime*2;
// vsl.Log("adding safe distance correction: {}", dc/CPS.SafeTime*2);//debug
}
// #if DEBUG
// Dir = dir*dist;
// DeltaV = dV;
// Maneuver = maneuver;
// #endif
return collision;
}
bool ComputeManeuver(Vessel v, out Vector3d maneuver)
{
maneuver = Vector3d.zero;
//calculate distance
var dir = (v.CurrentCoM-VSL.Physics.wCoM);
var dist = dir.magnitude;
dir /= dist;
//first try to get TCA from other vessel and get vessel's R and Exhaust
var vR = 0f;
var vB = new Bounds();
Transform vT = null;
var tca = ModuleTCA.EnabledTCA(v);
if(tca != null)
{
// if(tca.CPS != null &&
// tca.CPS.IsActive &&
// VSL.Physics.M > tca.VSL.Physics.M &&
// VSL.vessel.srfSpeed > v.srfSpeed) //test
// return false;
vR = tca.VSL.Geometry.R;
vB = tca.VSL.Geometry.B;
vT = tca.VSL.refT;
}
else //do a raycast
{
RaycastHit raycastHit;
if(Physics.SphereCast(VSL.Geometry.C+dir*(VSL.Geometry.R+0.1f), VSL.Geometry.R, dir,
out raycastHit, dist, RadarMask))
vR = (raycastHit.point-v.CurrentCoM).magnitude;
vT = v.ReferenceTransform;
vB = v.Bounds(vT);
vB.Encapsulate(v.EnginesExhaust());
}
//compute course correction
var dV = VSL.vessel.srf_velocity-v.srf_velocity+(VSL.vessel.acceleration-v.acceleration)*CPS.LookAheadTime;
var thrershold = -1f;
if(v.LandedOrSplashed) thrershold = 0;
else if(Dangerous.Contains(v.id)) thrershold = CPS.SafeDistance;
if(AvoideVessel(VSL, dir, dist, dV, vR, vB, vT, out maneuver, thrershold))
Dangerous.Add(v.id);
else Dangerous.Remove(v.id);
return !maneuver.IsZero();
}
