public override double GetZFor(Vector3d v3)
{
// return v3.Z;
Vector3 local = SimPathStore.GlobalToLocal(v3);
return(MoverPlaneZ.GetHeight(local));
}
public static SimWaypoint CreateGlobal(Vector3d v3d)
{
Vector3 v3 = SimPathStore.GlobalToLocal(v3d);
SimPathStore PathStore = SimPathStore.GetPathStore(v3d);
return(CreateLocal(v3, PathStore));
}
public string DistanceVectorString(Vector3d loc3d)
{
Vector3 loc = SimPathStore.GlobalToLocal(loc3d);
SimPathStore R = SimPathStore.GetPathStore(loc3d);
return(String.Format("{0:0.0#}m ", Vector3d.Distance(GlobalPosition, loc3d))
+ String.Format("{0}/{1:0.0#}/{2:0.0#}/{3:0.0#}", R.RegionName, loc.X, loc.Y, loc.Z));
}
public string DistanceVectorString(Vector3d loc3d)
{
Vector3 loc = SimPathStore.GlobalToLocal(loc3d);
SimPathStore R = SimPathStore.GetPathStore(loc3d);
return(String.Format("{0:0.00}m ", DistanceNoZ(GetWorldPosition(), loc3d))
+ String.Format("{0}/{1:0.00}/{2:0.00}/{3:0.00}", R.RegionName, loc.X, loc.Y, loc.Z));
}
/// <summary>
/// Make sure this the non-simplfied route
/// </summary>
/// <param name="route"></param>
private void DepricateRoute(IEnumerable <Vector3d> route, double finalDist)
{
List <Vector3d> reallyDepricate = new List <Vector3d>();
List <ThreadStart> listUndo = new List <ThreadStart>();
const int time = 120;
Vector3d first = default(Vector3d);
foreach (Vector3d list in route)
{
if (first == default(Vector3d))
{
first = list;
continue;
}
if (Vector3d.Distance(first, list) > finalDist)
{
break;
}
reallyDepricate.Add(list);
}
Mover.IndicateRoute(reallyDepricate, Color.Orchid);
foreach (Vector3d list in reallyDepricate)
{
PathStore.BlockPointTemp(SimPathStore.GlobalToLocal(list), listUndo, SimPathStore.BLOCKED);
}
if (listUndo.Count == 0)
{
return;
}
string tmp = string.Format("Blocking {0} points for {1} seconds", listUndo.Count, time);
Thread thr = new Thread(() =>
{
Debug(tmp);
Thread.Sleep(time * 1000);
Debug("Un-{0}", tmp);
foreach (ThreadStart undo in listUndo)
{
undo();
}
})
{
Name = tmp
};
thr.Start();
}
public void SetGlobalPos(Vector3d v3d)
{
SimPathStore R = SimPathStore.GetPathStore(v3d);
_LocalPos = SimPathStore.GlobalToLocal(v3d);
PathStore = R.GetPathStore3D(_LocalPos);
_GlobalPos = R.LocalToGlobal(_LocalPos);
//PX = (int)Math.Round(_LocalPos.X * PathStore.POINTS_PER_METER);
//PY = (int)Math.Round(_LocalPos.Y * PathStore.POINTS_PER_METER);
if (_IncomingArcs != null)
{
foreach (SimRoute A in _IncomingArcs)
{
A.LengthUpdated = false;
}
}
if (_OutgoingArcs != null)
{
foreach (SimRoute A in _OutgoingArcs)
{
A.LengthUpdated = false;
}
}
}
// just return v3.Z if unsure
public virtual double GetZFor(Vector3d v3)
{
return(SimPathStore.GlobalToLocal(v3).Z);
}
public SimMoverState FollowPathTo(IList <Vector3d> v3s, Vector3d finalTarget, double finalDistance)
{
completedAt = 0;
STATE = SimMoverState.MOVING;
Vector3d vstart = v3s[0];
// Vector3 vv3 = SimPathStore.GlobalToLocal(vstart);
v3s = GetSimplifedRoute(vstart, v3s);
Mover.IndicateRoute(v3s, Color.Red);
int maxReverse = 2;
Debug("FollowPath: {0} -> {1} for {2}", v3s.Count, DistanceVectorString(finalTarget), finalDistance);
int CanSkip = UseSkipping ? 0 : 2; //never right now
int Skipped = 0;
if (UseSkippingToggle)
{
UseSkipping = !UseSkipping;
}
int v3sLength = v3s.Count;
int at = 0;
int MoveToFailed = 0;
while (at < v3sLength)
{
Vector3d v3 = v3s[at];
// try to get there first w/in StepSize 0.2f
v3.Z = GetSimPosition().Z;
if (!MoveTo(v3, PathStore.StepSize, 2))
{
// didn't make it but are we close enough for govt work?
if (Mover.Distance(FinalPosition) < finalDistance)
{
return(SimMoverState.COMPLETE);
}
// See what point in the list we are closest to
int nbest = ClosestAt(v3s);
if (nbest > at)
{
Debug("Fast-forward {0} -> {1} ", at, nbest);
at = nbest + 1;
continue;
}
// Try again with distance of 0.6f
if (!MoveTo(v3, PathStore.StepSize * 3, 2))
{
MoveToFailed++;
// still did not make it
OpenNearbyClosedPassages();
if (Skipped++ <= CanSkip)
{
// move around
MoveToPassableArround(GetSimPosition());
Skipped++;
// move onto the next point
at++;
continue;
}
{
if (UseStarJumpBreakaway)
{
Mover.ThreadJump();
}
}
Vector3 l3 = SimPathStore.GlobalToLocal(v3);
BlockTowardsVector(l3);
Debug("!MoveTo: {0} ", DistanceVectorString(v3));
MoveToPassableArround(l3);
return(SimMoverState.TRYAGAIN);
}
}
else
{
Skipped = 0;
}
if (DistanceNoZ(GetWorldPosition(), finalTarget) < finalDistance)
{
return(SimMoverState.COMPLETE);
}
int best = ClosestAt(v3s);
if (best > at)
{
Debug("Fast forward {0} -> {1} ", at, best);
}
else if (best < at)
{
if (!UseReverse || maxReverse-- < 0)
{
// not using reverse
// Debug("Wont Reverse {0} -> {1} ", at, best);
best = at;
}
else
{
Debug("Reverse {0} -> {1} ", at, best);
}
if (MoveToFailed == 0)
{
best = at;
}
// for now never use (leave default false)
// UseReverse = !UseReverse;
}
completedAt = best;
at = best + 1;
}
double fd = DistanceNoZ(GetWorldPosition(), finalTarget);
STATE = fd <= finalDistance ? SimMoverState.COMPLETE : SimMoverState.BLOCKED;
Debug("{0}: {1:0.00}m", STATE, fd);
return(STATE);
}
