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);
}
private CollisionPlane CollisionPlaneAt(float z)
{
return(PathStore.GetCollisionPlane(z));
}