public bool GotoSimRoute(SimPosition pos)
{
bool IsFake;
for (int i = 0; i < 19; i++)
{
Debug("PLAN GotoTarget: " + pos);
// StopMoving();
if (TryGotoTarget(pos, out IsFake))
{
Mover.StopMoving();
Mover.TurnToward(pos.GlobalPosition);
Debug("SUCCESS GotoTarget: " + pos);
return(true);
}
//TurnToward(pos);
double posDist = Mover.Distance(pos);
if (posDist <= pos.GetSizeDistance() + 0.5)
{
Debug("OK GotoTarget: " + pos);
return(true);
}
TurnAvoid += 115;
while (TurnAvoid > 360)
{
TurnAvoid -= 360;
}
//Vector3d newPost = GetLeftPos(TurnAvoid, 4f);
//StopMoving();
//Debug("MOVELEFT GotoTarget: " + pos);
//MoveTo(newPost, 2f, 4);
if (IsFake)
{
break;
}
}
Debug("FAILED GotoTarget: " + pos);
return(false);
}
private bool Goto(SimPosition globalEnd, double distance)
{
bool OnlyStart = true;
bool MadeIt = true;
int maxTryAgains = 0;
IList <Vector3d> route = null;
CollisionPlane MoverPlane = MoverPlaneZ;
bool faked;
bool changePlanes = false;
while (OnlyStart && MadeIt)
{
float InitZ = Mover.SimPosition.Z;
Vector3d v3d = GetWorldPosition();
if (Mover.Distance(FinalPosition) < distance)
{
return(true);
}
float G = MoverPlane.GlobalBumpConstraint;
if (G < 1f)
{
Orig = G;
}
SimMoverState prev = STATE;
STATE = SimMoverState.THINKING;
route = SimPathStore.GetPath(MoverPlane, v3d, globalEnd.UsePosition.GlobalPosition, distance, out OnlyStart, out faked);
// todo need to look at OnlyStart?
PreXp = route.Count < 3 && G < MaxBigZ;
while (route.Count < 3 && G < MaxBigZ && faked)
{
if (G < 0.5)
{
G += 0.1f;
}
else
{
G += 0.5f;
if (Works > G)
{
G = Works;
}
}
MoverPlane.GlobalBumpConstraint = G;
route = SimPathStore.GetPath(MoverPlane, v3d, globalEnd.UsePosition.GlobalPosition, distance, out OnlyStart, out faked);
}
if (PreXp)
{
Works = G;
UsedBigZ++;
Debug("BigG used {0} instead of {1} !!", G, Orig);
OnlyStart = true;
}
else
{ // not PreXP
if (UsedBigZ > 0)
{
if (Works > 0.7f)
{
Works -= 0.5f; // todo not as liberal the next time?
}
UsedBigZ = 0;
Debug("BigG RESTORE {0} instead of {1} !!", Orig, MoverPlane.GlobalBumpConstraint);
MoverPlane.GlobalBumpConstraint = Orig;
}
}
STATE = prev;
STATE = FollowPathTo(route, globalEnd.UsePosition.GlobalPosition, distance);
Vector3 newPos = GetSimPosition();
switch (STATE)
{
case SimMoverState.TRYAGAIN:
{
if (maxTryAgains-- > 0)
{
OnlyStart = true;
continue;
}
MadeIt = false;
continue;
}
case SimMoverState.COMPLETE:
{
MadeIt = true;
continue;
}
case SimMoverState.BLOCKED:
{
float globalEndUsePositionGlobalPositionZ = (float)globalEnd.UsePosition.GlobalPosition.Z;
if (faked)
{
if (Math.Abs(Mover.SimPosition.Z - InitZ) > 1)
{
MoverPlane = MoverPlanes[Mover] = PathStore.GetCollisionPlane(Mover.SimPosition.Z);
}
else
{
MoverPlane = MoverPlanes[Mover] = PathStore.GetCollisionPlane(globalEndUsePositionGlobalPositionZ);
}
}
OnlyStart = true;
MadeIt = false;
continue;
}
case SimMoverState.PAUSED:
case SimMoverState.MOVING:
case SimMoverState.THINKING:
default:
{
//MadeIt = true;
OnlyStart = true;
continue;
}
}
}
if (!MadeIt && route != null)
{
SimMoverState prev = STATE;
STATE = SimMoverState.THINKING;
CollisionPlane CP = MoverPlane;
if (CP.HeightMapNeedsUpdate)
{
CP.MatrixNeedsUpdate = true;
}
if (!CP.MatrixNeedsUpdate || !CP.HeightMapNeedsUpdate)
{
if (false)
{
CP.MatrixNeedsUpdate = true;
CP.HeightMapNeedsUpdate = true;
Debug("Faking matrix needs update?");
}
}
else
{
Debug("Matrix really needed update");
}
Vector3d v3d = GetWorldPosition();
double fd = DistanceNoZ(v3d, globalEnd.GlobalPosition);
if (fd < distance)
{
STATE = SimMoverState.COMPLETE;
return(true);
}
CP.EnsureUpdated();
int index = ClosetPointOnRoute(route, v3d);
if (index > 0)
{
var blockMe = new List <Vector3d>();
if (index + 2 < route.Count)
{
Vector3d firstBad = route[index + 1];
var np = v3d + ((firstBad - v3d) / 2);
blockMe.Add(np);
blockMe.Add(firstBad);
DepricateRoute(blockMe, fd);
STATE = prev;
return(MadeIt);
}
}
if (fd > distance && fd > 2)
{
if (false)
{
DepricateRoute(route, fd);
}
}
Debug("Too far away " + fd + " wanted " + distance);
STATE = prev;
}
return(MadeIt);
}
public override SimMoverState Goto()
{
STATE = SimMoverState.MOVING;
int CanSkip = 0;
int Skipped = 0;
int tried = 0;
SimRoute prev = null;
for (int cI = CurrentRouteIndex; cI < Routes.Count; cI++)
{
CurrentRouteIndex = cI;
if (cI > 0)
{
prev = Routes[cI - 1];
}
SimRoute route = Routes[cI];
if (route.IsBlocked)
{
STATE = SimMoverState.BLOCKED;
continue;
}
tried++;
// TRY
STATE = FollowRoute(route);
double distance = Mover.Distance(FinalPosition);
if (STATE == SimMoverState.BLOCKED)
{
Mover.StopMoving();
// SetBlocked(route);
if (distance < FinalDistance)
{
return(SimMoverState.COMPLETE);
}
//CreateSurroundWaypoints();
route.ReWeight(1.1f);
route.BumpyCount++;
if (CanSkip > 0)
{
CanSkip--;
Skipped++;
continue;
}
if (route.BumpyCount > 0 && Skipped == 0)
{
SetBlocked(route);
// if (prev!=null) if (FollowRoute(prev.Reverse) == SimMoverState.COMPLETE)
// {
// return SimMoverState.TRYAGAIN;
// }
}
return(STATE);
}
if (STATE == SimMoverState.PAUSED)
{
if (distance < FinalDistance)
{
return(SimMoverState.COMPLETE);
}
return(SimMoverState.TRYAGAIN);
}
if (STATE == SimMoverState.COMPLETE)
{
// if made it here then the prev was very good
if (prev != null)
{
prev.ReWeight(0.8f);
}
}
if (distance < FinalDistance)
{
return(SimMoverState.COMPLETE);
}
}
if (STATE != SimMoverState.COMPLETE)
{
if (tried == 0)
{
return(SimMoverState.TRYAGAIN);
}
return(STATE);
}
OuterRoute.ReWeight(0.7f); // Reward
//TODO Mover.PathStore.AddArc(OuterRoute);
STATE = SimMoverState.COMPLETE;
return(STATE);
}