public override CmdResult ExecuteRequest(CmdRequest args)
{
bool asLocal = args.IsTrue("--local");
SimPosition pos, pos2;
if (!args.TryGetValue("start", out pos))
{
pos = TheSimAvatar;
}
if (!args.TryGetValue("end", out pos2))
{
pos = TheSimAvatar;
}
Vector3d from, to;
if (pos2 == null)
{
pos2 = pos;
pos = TheSimAvatar;
}
from = pos.GlobalPosition;
to = pos2.GlobalPosition;
Vector3d offset = Vector3d.Zero;
if (asLocal)
{
offset = pos.PathStore.GlobalStart;
}
bool onlyStart, faked;
var path = SimPathStore.GetPath(null, from, to, 1, out onlyStart, out faked);
WriteLine("onlyStart=" + onlyStart);
WriteLine("faked=" + faked);
WriteLine("start=" + VectorRoundString(from - offset));
WriteLine("end=" + VectorRoundString(to - offset));
WriteLine("pstart=" + pos);
WriteLine("pend=" + pos2);
WriteLine("len=" + path.Count);
int step = 0;
foreach (Vector3d vector3D in path)
{
WriteLine("p" + step + "=" + VectorRoundString(vector3D - offset));
step++;
}
return(Success("SUCCESS " + Name));
}
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);
}