public void ProbeHeightAhead(Vector3 Dir)
{
if (VSL.Body == null || VSL.Body.pqsController == null)
{
return;
}
if (LookAheadTime > RAD.LookAheadTime)
{
Obstacle = BestPoint;
rewind();
}
else if (LookAheadTimeDelta < 0.1)
{
if (BestPoint.Valid)
{
Obstacle = BestPoint;
}
rewind();
}
CurPoint.Update(VSL.Physics.wCoM + Dir * (VSL.Geometry.R + Utils.ClampL(VSL.HorizontalSpeed, 0.1f) * LookAheadTime), VSL.Body);
if (CurPoint > BestPoint)
{
BestPoint = CurPoint;
}
if (BestPoint > Obstacle)
{
Obstacle = BestPoint;
}
if (VSL.Altitude.Absolute - CurPoint.Altitude <= VSL.Geometry.H)
{
LookAheadTime -= LookAheadTimeDelta;
LookAheadTimeDelta /= 2;
}
LookAheadTime += LookAheadTimeDelta;
}
public TerrainPoint WalkToMaxima(TerrainPoint start) {
bool atMaxima = false;
TerrainPoint last = new TerrainPoint(start.x, start.y);
while (!atMaxima) {
TerrainPoint max = new TerrainPoint(last.x, last.y);
float maxValue = this.data[last.x, last.y];
for (int x = last.x - 1; x <= last.x + 1; x++) {
for (int y = last.y - 1; y <= last.y + 1; y++) {
float v = this.data[x, y];
if (v > maxValue) {
maxValue = v;
max.x = x;
max.y = y;
}
}
}
if (max.x == last.x && max.y == last.y) {
atMaxima = true;
}
last.x = max.x;
last.y = max.y;
}
return last;
}
private double GetSlope(RasterReader reader, TerrainPoint tp)
{
double zx = 0, zy = 0, ex = 0;
if (tp.col - 1 > 0 && tp.row - 1 > 0 && tp.row + 1 < _terrainTile.GetLength(1) && tp.col + 1 < _terrainTile.GetLength(0))
{
iTelluro.GlobeEngine.DataSource.Geometry.Point3d d1 = null, d2 = null;
if (reader.IsProjected)
{
d1 = new Point3d(_terrainTile[tp.col - 1, tp.row].Longitude, tp.Latitude, tp.Altitude);
d2 = new Point3d(_terrainTile[tp.col + 1, tp.row].Longitude, tp.Latitude, tp.Altitude);
}
else
{
d1 = MathEngine.SphericalToCartesianD(
Angle.FromDegrees(tp.Latitude),
Angle.FromDegrees(_terrainTile[tp.col - 1, tp.row].Longitude), //lon - demSpan
GlobeTools.CurrentWorld.EquatorialRadius + tp.Altitude
);
d2 = MathEngine.SphericalToCartesianD(
Angle.FromDegrees(tp.Latitude),
Angle.FromDegrees(_terrainTile[tp.col + 1, tp.row].Longitude), //lon + demSpan),
GlobeTools.CurrentWorld.EquatorialRadius + tp.Altitude
);
}
iTelluro.GlobeEngine.DataSource.Geometry.Point3d segment = d2 - d1;
ex = segment.Length;
//zx = (elv[1, 2] - elv[1, 0]) / ex;
//zy = (elv[2, 1] - elv[0, 1]) / ex;
zx = (_terrainTile[tp.col + 1, tp.row + 0].Altitude - _terrainTile[tp.col - 1, tp.row + 0].Altitude) / ex;
zy = (_terrainTile[tp.col + 0, tp.row + 1].Altitude - _terrainTile[tp.col + 0, tp.row - 1].Altitude) / ex;
}
return(Math.Atan(Math.Sqrt(zx * zx + zy * zy)));
}
private void InitializeTerrainPoints(Mesh mesh)
{
terrainPoints = new TerrainPoint[planetResolution][];
for (int i = 0; i < terrainPoints.Length; i++)
{
terrainPoints[i] = new TerrainPoint[planetResolution];
for (int j = 0; j < terrainPoints.Length; j++)
{
terrainPoints[i][j] = new TerrainPoint();
}
}
for (int i = 0; i < terrainPoints.Length; i++)
{
for (int j = 0; j < terrainPoints.Length; j++)
{
//World Position
terrainPoints[i][j].worldPosition = mesh.vertices[i * planetResolution + j];
//Face Position
terrainPoints[i][j].facePosition = new Vector2Int(i, j);
//Terrain Kind
float distance = Vector3.Distance(terrainPoints[i][j].worldPosition, Vector3.zero);
if (distance > planetSeaLevel + 0.005f * planetSeaLevel)
{
terrainPoints[i][j].kind = 0;
}
else
{
terrainPoints[i][j].kind = -1;
}
}
}
}
private void WatershedArea(string shppath, string rasterpath, ref double _resultProjectArea, ref double?_resultSurfaceArea)
{
//获取流域面积形状以及它的外接矩形
GeoRect _analysisRect = new GeoRect(-90, 90, 180, -180); //外接矩形
iTelluroLib.GeoTools.Geometries.Polygon _analysisPolygon = _hydrology.CalLimitAreaPolygon(shppath, ref _analysisRect); //流域面形状
//在存在流域面积界定的情况下要进行裁剪筛选，不存在默认全部计算
RasterReader raster = new RasterReader(rasterpath);
double _widthStep = raster.CellSizeX;
double _heightStep = raster.CellSizeY;
// 网格大小：行、列数
int _widthNum = (int)((_analysisRect.East - _analysisRect.West) / _widthStep);
int _heightNum = (int)((_analysisRect.North - _analysisRect.South) / _heightStep);
_terrainTile = new TerrainPoint[_widthNum, _heightNum];
//构建高程矩阵
for (int i = 0; i < _widthNum; i++)
{
for (int j = 0; j < _heightNum; j++)
{
TerrainPoint tp = new TerrainPoint();
tp.Longitude = _analysisRect.West + i * _widthStep;
tp.Latitude = _analysisRect.South + j * _heightStep;
tp.Altitude = ReadBand(raster, i, j);
tp.col = i;
tp.row = j;
_terrainTile[i, j] = tp;
}
}
double _cellArea = CaculateCellAera(raster, _widthNum, _heightNum);
//坡度计算
if (_resultSurfaceArea == null)
{
_resultSurfaceArea = 0;
}
for (int i = 0; i < _widthNum; i++)
{
for (int j = 0; j < _heightNum; j++)
{
TerrainPoint tp = _terrainTile[i, j];
iTelluroLib.GeoTools.Geometries.Point currentPoint = new iTelluroLib.GeoTools.Geometries.Point(tp.Longitude, tp.Latitude);
if ((_analysisPolygon != null && _analysisPolygon.Contains(currentPoint)) || _analysisPolygon == null)
{
_resultProjectArea += _cellArea;
//计算坡度
double slope = GetSlope(raster, tp);
double currentCellArea = _cellArea / Math.Cos(slope);
_resultSurfaceArea += currentCellArea;
}
}
FormOutput.AppendProress(((i + 1) * 100) / _widthNum);
}
}
public void Copy(Sweep s)
{
VSL = s.VSL;
L = s.L; C = s.C; R = s.R;
Obstacle = s.Obstacle;
Altitude = s.Altitude;
Maneuver = s.Maneuver;
Valid = s.Valid;
}
public TerrainPoint[] RandomlyWithin(int nPoints) {
TerrainPoint[] points = new TerrainPoint[nPoints];
for (int i = 0; i < nPoints; i++) {
int x = (int)(Random.value * this.GetWidth());
int y = (int)(Random.value * this.GetHeight());
points[i] = new TerrainPoint(x, y);
}
return points;
}
/// <summary>
/// Gets the density at the given chunk position.
/// </summary>
/// <param name="pos">The chunk position.</param>
/// <returns>The density.</returns>
public byte GetDensity(Vector3I pos)
{
TerrainPoint point = this.Points[pos.X, pos.Y];
if (point != null)
{
return(point.GetDensity(pos.Z));
}
else
{
return(TerrainPoint.DensityMax);
}
}
private void setYByDifficulty(ref TerrainPoint point, int difficulty)
{
pointsCounter++;
smoothStepTime = (Time.time - startTime) / duration;
if (isUp)
{
// Narrow cave up towards the last points avg
float nextFloorYGoal = Mathf.Clamp(prevAvg + difficulty / diffToCaveRatio, minFloor, maxCeiling - minGap);
point.floorY = Mathf.SmoothStep(prevFloorY, nextFloorYGoal, smoothStepTime);
float floorDiff = Mathf.Abs(prevFloorY - point.floorY);
point.ceilingY = Random.Range(point.floorY + minGap, maxCeiling);
}
else
{
// Narrow cave down towards the last points avg
float nextCeilingYGoal = Mathf.Clamp(prevAvg - difficulty / diffToCaveRatio, minFloor + minGap, maxCeiling);
point.ceilingY = Mathf.SmoothStep(prevAvg, nextCeilingYGoal, smoothStepTime);
point.floorY = Random.Range(minFloor, point.ceilingY - minGap);
}
// Calc sliding avg of cave center
float avg = (point.floorY + point.ceilingY) / 2f;
midPoints.Enqueue(avg);
if (midPoints.Count() > midpointsNum)
{
midPoints.Dequeue();
}
float slidingAvg = midPoints.Average();
// Toggle up / down direction, make level harder and faster-changing
if (pointsCounter % caveDirectionToggle == 0)
{
isUp = !isUp;
caveDirectionToggle = (int)Mathf.Clamp((int)caveDirectionToggle - 1, 1, Mathf.Infinity);
pointsDistance = Mathf.Max(pointsDistance * disanceDecayRate, minPointsDistance);
diffToCaveRatio = Mathf.Clamp(diffToCaveRatio - 4f, 5f, Mathf.Infinity);
startTime = Time.time;
if (player != null)
{
player.incrementSpeed(batIncreaseSpeed);
}
}
// Save curr as previous
prevAvg = slidingAvg;
prevCeilingY = point.ceilingY;
prevFloorY = point.floorY;
}
void PopulateTerrainMap()
{
for (int x = 0; x < width + 1; x++)
{
for (int z = 0; z < width + 1; z++)
{
for (int y = 0; y < height + 1; y++)
{
float thisHeight;
thisHeight = GameData.GetTerrainHeight(x + chunkPosition.x, z + chunkPosition.z);
terrainMap[x, y, z] = new TerrainPoint((float)y - thisHeight, Random.Range(0, 2));
}
}
}
}
private void CheckNeighbors(TerrainPoint terrainPoint)
{
for (int i = 0; i < TRANSFORMS.Length; i++)
{
Vector2Int neighborPosition = terrainPoint.facePosition + TRANSFORMS[i];
if (neighborPosition.x >= 0 && neighborPosition.x < planetResolution &&
neighborPosition.y >= 0 && neighborPosition.y < planetResolution)
{
TerrainPoint neighbor = terrainPoints[neighborPosition.x][neighborPosition.y];
if (neighbor.kind != -1 && neighbor.kind == 0)
{
neighbor.kind = terrainPoint.kind;
listToCheck.Add(neighbor);
}
}
}
}
public void Cast(Vector3 dir, float angle, float dist)
{
Reset();
if (VSL.refT == null)
{
return;
}
//cast the rays
var side = Vector3.Cross(VSL.Physics.Up, dir) * VSL.Geometry.R * 1.5f;
var cast_dir = Quaternion.AngleAxis(angle, side) * dir;
Valid |= L.Cast(VSL.Physics.wCoM - side, cast_dir, dist, VSL.Geometry.R);
Valid |= C.Cast(VSL.Physics.wCoM, cast_dir, dist, VSL.Geometry.R);
Valid |= R.Cast(VSL.Physics.wCoM + side, cast_dir, dist, VSL.Geometry.R);
if (Valid)
{
L.ClaculateAltitude(dir, angle);
C.ClaculateAltitude(dir, angle);
R.ClaculateAltitude(dir, angle);
Altitude = VSL.Altitude.Absolute - Mathf.Min(L.Altitude, C.Altitude, R.Altitude);
Ray ray;
if (L.Valid && !R.Valid)
{
ray = L; Maneuver = ManeuverType.Horizontal;
}
else if (!L.Valid && R.Valid)
{
ray = R; Maneuver = ManeuverType.Horizontal;
}
else if (C.Valid)
{
ray = C; Maneuver = ManeuverType.Vertical;
}
else if (L.Valid && R.Valid)
{
ray = R.Distance < L.Distance? R : L; Maneuver = ManeuverType.Vertical;
}
else
{
VSL.Log("Unknown ManevuerType of a Valid Sweep. This should never happen."); return;
}
Obstacle = new TerrainPoint(Altitude, ray.CollisionPoint);
}
}
void PopulateTerrainMap()
{
// The data points for terrain are stored at the corners of our "cubes", so the terrainMap needs to be 1 larger
// than the width/height of our mesh.
for (int x = 0; x < width + 1; x++)
{
for (int z = 0; z < width + 1; z++)
{
for (int y = 0; y < height + 1; y++)
{
float thisHeight = GameData.GetTerrainHeight(x + chunkPosition.x, z + chunkPosition.z);
if (x > 5 && x < 10 && z > 5 && z < 10)
{
// Get a terrain height using regular old Perlin noise.
thisHeight = 1f;
}
// Set the value of this point in the terrainMap.
terrainMap[x, y, z] = new TerrainPoint((float)y - thisHeight, Random.Range(0, World.Instance.terrainTextures.Length));
}
}
}
}
private void Count()
{
for (int i = 0; i < terrainPoints.Length; i++)
{
for (int j = 0; j < terrainPoints.Length; j++)
{
TerrainPoint point = terrainPoints[i][j];
while (listToCheck.Count > 0)
{
// check points from list
CheckNeighbors(terrainPoints[listToCheck[0].facePosition.x][listToCheck[0].facePosition.y]);
listToCheck.Remove(listToCheck[0]);
}
if (point.kind == 0)
{
counter++;
point.kind = counter;
listToCheck.Add(point);
}
}
}
}
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;
}
public void ProbeHeightAhead(Vector3 Dir)
{
if(VSL.Body == null || VSL.Body.pqsController == null) return;
if(LookAheadTime > RAD.LookAheadTime)
{
Obstacle = BestPoint;
rewind();
}
else if(LookAheadTimeDelta < 0.1)
{
if(BestPoint.Valid)
Obstacle = BestPoint;
rewind();
}
CurPoint.Update(VSL.Physics.wCoM+Dir*(VSL.Geometry.R+Utils.ClampL(VSL.HorizontalSpeed, 0.1f)*LookAheadTime), VSL.Body);
if(CurPoint > BestPoint) BestPoint = CurPoint;
if(BestPoint > Obstacle) Obstacle = BestPoint;
if(VSL.Altitude.Absolute-CurPoint.Altitude <= VSL.Geometry.H)
{
LookAheadTime -= LookAheadTimeDelta;
LookAheadTimeDelta /= 2;
}
LookAheadTime += LookAheadTimeDelta;
}
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;
}
public void Cast(Vector3 dir, float angle, float dist)
{
Reset();
if(VSL.refT == null) return;
//cast the rays
var side = Vector3.Cross(VSL.Physics.Up, dir)*VSL.Geometry.R*1.5f;
var cast_dir = Quaternion.AngleAxis(angle, side)*dir;
Valid |= L.Cast(VSL.Physics.wCoM-side, cast_dir, dist, VSL.Geometry.R);
Valid |= C.Cast(VSL.Physics.wCoM, cast_dir, dist, VSL.Geometry.R);
Valid |= R.Cast(VSL.Physics.wCoM+side, cast_dir, dist, VSL.Geometry.R);
if(Valid)
{
L.ClaculateAltitude(dir, angle);
C.ClaculateAltitude(dir, angle);
R.ClaculateAltitude(dir, angle);
Altitude = VSL.Altitude.Absolute-Mathf.Min(L.Altitude, C.Altitude, R.Altitude);
Ray ray;
if(L.Valid && !R.Valid) { ray = L; Maneuver = ManeuverType.Horizontal; }
else if(!L.Valid && R.Valid) { ray = R; Maneuver = ManeuverType.Horizontal; }
else if(C.Valid) { ray = C; Maneuver = ManeuverType.Vertical; }
else if(L.Valid && R.Valid) { ray = R.Distance < L.Distance? R : L; Maneuver = ManeuverType.Vertical; }
else { VSL.Log("Unknown ManevuerType of a Valid Sweep. This should never happen."); return; }
Obstacle = new TerrainPoint(Altitude, ray.CollisionPoint);
}
}
public void Copy(Sweep s)
{
VSL = s.VSL;
L = s.L; C = s.C; R = s.R;
Obstacle = s.Obstacle;
Altitude = s.Altitude;
Maneuver = s.Maneuver;
Valid = s.Valid;
}
public void AddChunkAtPoints(TerrainPoint[] points, TerrainChunk chunk, AddMode mode = AddMode.Add) {
for (int i = 0; i < points.Length; i++) {
int x = points[i].x;
int y = points[i].y;
this.AddChunk(chunk, x - chunk.GetWidth() / 2, y - chunk.GetHeight() / 2, mode);
}
}
public bool BeforeDestination(VesselWrapper VSL, Vector3d vel)
{
var point = new TerrainPoint(0, CollisionPoint);
return(point.BeforeDestination(VSL, vel));
}
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;
}
