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