public override bool OpenNearbyClosedPassages()
{
Vector3 v3 = GetSimPosition();
byte b = PathStore.GetNodeQuality(v3, MoverPlaneZ);
if (b < 200)
{
b += 100;
}
if (b > 254)
{
b = 254;
}
CollisionPlane MoverPlane = MoverPlaneZ;
PathStore.SetNodeQuality(v3, b, MoverPlane);
bool changed = base.OpenNearbyClosedPassages();
if (changed)
{
MoverPlane.HeightMapNeedsUpdate = true;
MoverPlane.MatrixNeedsUpdate = true;
}
return(changed);
}
internal bool IsPortal(CollisionPlane collisionPlane)
{
IEnumerable <CollisionObject> mis = GetOccupiedObjects(collisionPlane.MinZ, collisionPlane.MaxZ);
lock (mis)
foreach (object o in mis)
{
string s = o.ToString().ToLower();
{
if (s.Contains("stair"))
{
return(true);
}
if (s.Contains("ramp"))
{
return(true);
}
// if (s.Contains("brigde")) return true;
if (s.Contains(" path "))
{
return(true);
}
}
}
return(false);
}
public static SimWaypoint CreateLocal(Vector3 from, CollisionPlane CP)
{
SimPathStore PathStore = CP.PathStore;
float POINTS_PER_METER = PathStore.POINTS_PER_METER;
int PX = PathStore.ARRAY_X(from.X);
int PY = PathStore.ARRAY_Y(from.Y);
SimWaypoint WP;
CollisionIndex CI = CollisionIndex.CreateCollisionIndex(from, PathStore);
lock (CI)
{
WP = CI.FindWayPoint(from.Z);
if (WP != null)
{
return(WP);
}
from.X = PX / POINTS_PER_METER;
from.Y = PY / POINTS_PER_METER;
Vector3d GlobalPos = PathStore.LocalToGlobal(from);
if (GlobalPos.X < 256 || GlobalPos.Y < 256)
{
CollisionPlane.Debug("bad global " + GlobalPos);
}
WP = new SimWaypointImpl(from, GlobalPos, CI, CP, PathStore);
WP.IsPassable = true;
}
// wp._IncomingArcs = new ArrayList();
// wp._OutgoingArcs = new ArrayList();
// PathStore.EnsureKnown(wp);
return(WP);
}
// public bool IsTimerTicking = false;
public void SetNodeQualityTimer(CollisionPlane CP, int value, List <ThreadStart> undo)
{
byte oldValue = GetMatrix(CP);
if (oldValue == value) // already set
{
return;
}
Debug("SetNodeQualityTimer of {0} form {1} to {2}", this, oldValue, value);
SetMatrixForced(CP, value);
//if (IsTimerTicking) return;
//IsTimerTicking = true;
float StepSize = PathStore.StepSize;
undo.Add(() =>
{
byte newValue = GetMatrix(CP);
if (newValue != value)
// its been changed by something else since we set to Zero
{
Debug("SetNodeQualityTimer Thread out of date {0} value changed to {1}", this, newValue);
// SetMatrixForced(CP, oldValue);
}
else
{
SetMatrixForced(CP, oldValue);
Debug("ResetNodeQualityTimer {0} value reset to {1}", this, oldValue);
}
//IsTimerTicking = false;
});
}
public SimWaypoint FindWayPoint(float z)
{
CollisionPlane CP = CollisionPlaneAt(z);
SimWaypoint v;
if (WaypointsHash.TryGetValue(CP, out v))
{
return(v);
}
return(null);// SimWaypointImpl.CreateLocal(from, PathStore);
}
public SimWaypoint GetWayPoint(float z)
{
CollisionPlane CP = CollisionPlaneAt(z);
SimWaypoint v;
if (!WaypointsHash.TryGetValue(CP, out v))
{
v = SimWaypointImpl.CreateLocal(_LocalPos.X, _LocalPos.Y, z, PathStore);
v.Plane = CP;
}
return(v);
}
public string OccupiedString(CollisionPlane cp)
{
IEnumerable<CollisionObject> OccupiedListObject = GetOccupiedObjects();
string S = "";
lock (OccupiedListObject)
{
foreach (CollisionObject O in OccupiedListObject)
{
S += O.ToString();
S += "\r\n";
}
}
return S + this.ToString() + " " + ExtraInfoString(cp);
}
public SimCollisionPlaneMover(SimMover mover, SimPosition finalGoal, double finalDistance) :
base(mover, finalGoal, finalDistance)
{
CollisionPlane MoverPlane = MoverPlaneZ;
float startZ = (float)SimPathStore.CalcStartZ(mover.SimPosition.Z, finalGoal.SimPosition.Z);
double diff = MoverPlane.MinZ - startZ;
MoverPlane.MinZ = startZ;
MoverPlane.MaxZ = startZ + 3;
if (diff > .5 || diff < -0.5f)
{
MoverPlane.HeightMapNeedsUpdate = true;
}
}
public string OccupiedString(CollisionPlane cp)
{
IEnumerable <CollisionObject> OccupiedListObject = GetOccupiedObjects();
string S = "";
lock (OccupiedListObject)
{
foreach (CollisionObject O in OccupiedListObject)
{
S += O.ToString();
S += "\r\n";
}
}
return(S + this.ToString() + " " + ExtraInfoString(cp));
}
internal System.Drawing.Color DebugColor(CollisionPlane collisionPlane)
{
IEnumerable <CollisionObject> mis = GetOccupiedObjects(collisionPlane.MinZ, collisionPlane.MaxZ);
lock (mis)
foreach (object O in mis)
{
if (O is IMeshedObject)
{
Color c = ((IMeshedObject)O).DebugColor();
if (c != Color.Empty)
{
return(c);
}
}
}
return(Color.Empty);
}
private SimWaypointImpl(Vector3 local, Vector3d global, CollisionIndex Ci, CollisionPlane Cp, SimPathStore pathStore)
{
_MinZ = local.Z;
_MaxZ = _MinZ + 1.5f;
CIndex = Ci;
_Plane = Cp;
_PathStore = pathStore;
_GlobalPos = global;// RoundPoint(firstP, PathStore);
_LocalPos = local;
//PX = PX0;
//PY = PY0;
//PathStore.SimWaypoint[PX, PY] = this;
//TaintMatrix();
//UpdateMatrix(pathStore.GL);
_Passable = true;
_IncomingArcs = null;
_OutgoingArcs = null;
}
public string ExtraInfoString(CollisionPlane cp)
{
float low = cp.MinZ;
float high = cp.MaxZ;
string S = String.Format("{0}/{1} GLevel={2}", PX, PY, GetGroundLevel());
S += String.Format(" HLevel={0}", cp.HeightMap[PX, PY]);
if (IsUnderWater(low, high))
{
S += String.Format(" UnderWater={0}", PathStore.WaterHeight);
}
if (IsFlyZone(low, high))
{
S += String.Format(" FlyZone={0}", low);
}
S += String.Format(" LastGL={0}-{1}", low, high);
S += String.Format(" ZLevel={0}", GetZLevel(low, high));
return(S);
}
//string OcString = null;
//public IList<Vector2> OccupiedListMinMaxZ = new List<Vector2>();
public string OccupiedString(CollisionPlane cp)
{
string S = "";
float low = cp.MinZ - 10f;
float high = cp.MaxZ + 10f;
if (OccupiedCount > 0)
{
IEnumerable <CollisionObject> objs = GetOccupiedObjects(low, high);
lock (objs)
{
foreach (CollisionObject O in objs)
{
S += "" + O;//.ToBoxString(_LocalPos.X, _LocalPos.Y, low, high);
S += Environment.NewLine;
}
}
}
return(String.Format("{0}{1} {2}", S, this.ToString(), ExtraInfoString(cp)));
}
//public class FallingPrim
//{
//public bool DoneMoving = false;
//public uint localId;
public void FallingPrim(OdeScene ps, CollisionPlane Plane, PhysicsVector position, int x, int y, float offsetZ)
{
uint localId = Plane.fallingPrims;
PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
OdePrim oprim = (OdePrim)ps.AddPrimShape("FallingPrim_" + localId, newcube, position, new PhysicsVector(0.1f, 0.1f, 2f), Quaternion.Identity, true);
oprim.LocalID = localId + 100000;
oprim.OnCollisionUpdate += delegate(EventArgs args)
{
if (!oprim.m_taintremove)
{
// CollisionEventUpdate arg = (CollisionEventUpdate)args;
//simhinfo 58 58 30
// DoneMoving = true;
LandingHieghts[x, y] = oprim.Position.Z + offsetZ;
fallingPrims--;
ps.remCollisionEventReporting(oprim);
ps.RemovePrim(oprim);
}
};
oprim.SubscribeEvents(30000);
}
public void CreateDefaultRoutes()
{
CollisionPlane.Debug("CreateDefaultRoutes <{0},{1}>-<{2},{3}> StepSize={4}", StartX, StartY, EndX, EndY, StepSize);
return;
double W = 0.75f;
int made = 0;
for (int x = StartX; x < EndX; x += StepSize)
{
for (int y = StartY; y < EndY; y += StepSize)
{
made++;
SimWaypoint sw00 = CreateXYZ(x, y);
SimWaypoint sw01 = CreateXYZ(x, y + StepSize);
SimWaypoint sw10 = CreateXYZ(x + StepSize, y);
SimWaypoint sw11 = CreateXYZ(x + StepSize, y + StepSize);
/*
*
* Draws two-way Routes StepSize meters appart
*
* - *
| X
* *
*
*
*/
AddNewArcs(sw00, sw01, W); //dirrection |
AddNewArcs(sw00, sw10, W); //dirrection -
AddNewArcs(sw00, sw11, W); //dirrection \
AddNewArcs(sw10, sw01, W); //dirrection /
}
}
CollisionPlane.Debug("CreateDefaultRoutes Made {0} waypoints", made);
}
public override SimMoverState Goto()
{
if (Goto(FinalPosition, FinalDistance))
{
return(SimMoverState.COMPLETE);
}
return(STATE);
BumpConstraint -= 0.05f; // tighten constraints locally
if (BumpConstraint < 0.15)
{
BumpConstraint = 0.9f; // recycle
}
CollisionPlane MoverPlane = MoverPlaneZ;
MoverPlane.ChangeConstraints(GetSimPosition(), BumpConstraint);
if (Goto(FinalPosition, FinalDistance))
{
// use new tighter constraint semipermanently
MoverPlane.GlobalBumpConstraint = BumpConstraint;
return(SimMoverState.COMPLETE);
}
return(STATE);
}
// public bool IsTimerTicking = false;
public void SetNodeQualityTimer(CollisionPlane CP, int value, List<ThreadStart> undo)
{
byte oldValue = GetMatrix(CP);
if (oldValue == value) // already set
return;
Debug("SetNodeQualityTimer of {0} form {1} to {2}", this, oldValue, value);
SetMatrixForced(CP, value);
//if (IsTimerTicking) return;
//IsTimerTicking = true;
float StepSize = PathStore.StepSize;
undo.Add(() =>
{
byte newValue = GetMatrix(CP);
if (newValue != value)
// its been changed by something else since we set to Zero
{
Debug("SetNodeQualityTimer Thread out of date {0} value changed to {1}", this, newValue);
// SetMatrixForced(CP, oldValue);
}
else
{
SetMatrixForced(CP, oldValue);
Debug("ResetNodeQualityTimer {0} value reset to {1}", this, oldValue);
}
//IsTimerTicking = false;
});
}
public string ExtraInfoString(CollisionPlane cp)
{
float low = cp.MinZ;
float high = cp.MaxZ;
string S = String.Format("{0}/{1} GLevel={2}", PX, PY, GetGroundLevel());
S += String.Format(" HLevel={0}", cp.HeightMap[PX, PY]);
if (IsUnderWater(low, high)) S += String.Format(" UnderWater={0}", PathStore.WaterHeight);
if (IsFlyZone(low, high)) S += String.Format(" FlyZone={0}", low);
S += String.Format(" LastGL={0}-{1}", low, high);
S += String.Format(" ZLevel={0}", GetZLevel(low, high));
return S;
}
//string OcString = null;
//public IList<Vector2> OccupiedListMinMaxZ = new List<Vector2>();
public string OccupiedString(CollisionPlane cp)
{
string S = "";
float low = cp.MinZ - 10f;
float high = cp.MaxZ + 10f;
if (OccupiedCount > 0)
{
IEnumerable<CollisionObject> objs = GetOccupiedObjects(low, high);
lock (objs)
{
foreach (CollisionObject O in objs)
{
S += "" + O;//.ToBoxString(_LocalPos.X, _LocalPos.Y, low, high);
S += Environment.NewLine;
}
}
}
return String.Format("{0}{1} {2}", S, this.ToString(), ExtraInfoString(cp));
}
public SimRoute Intern2Arc(SimWaypoint s, SimWaypoint e, double W)
{
InternArc(e, s, W);
CollisionPlane.Debug("Intern2Arc: " + s + " <-> " + e);
return(InternArc(s, e, W));
}
private void SetMatrix(CollisionPlane CP, int v)
{
CIndex.SetMatrixForced(CP, v);
}
public IList<Vector3d> GetLocalPath(CollisionPlane CP, Vector3 start, Vector3 end, out bool partialOnly, out bool faked)
{
CP.EnsureUpdated();
float Z = start.Z;
if (!IsPassable(start, CP))
{
Vector3 newStart = start;
Debug("start is not passable: " + start);
for (int i = 0; i < 360; i += 45)
{
newStart = start + GetXYDiffOfMovement(i, Quaternion.Identity, 4);
if (IsPassable(newStart))
{
break;
}
}
if (newStart == start)
{
start = GetUsableLocalPositionOf(CP, start, 2);
}
}
if (!IsPassable(end, CP))
{
Debug("end is not passable: " + end);
}
return (IList<Vector3d>)GetLocalPath0(start, GetUsableLocalPositionOf(CP,end, 2), CP, Z,out partialOnly, out faked);
}
/// <summary>
/// The closet usable space to the v3 TODO
/// </summary>
/// <param name="v3"></param>
/// <returns></returns>
///
public Vector3 GetUsableLocalPositionOf(CollisionPlane CP, Vector3 v3, float useDist)
{
SimPathStore PathStore = GetPathStore3D(v3);
int ix = ARRAY_X(v3.X);
int iy = ARRAY_Y(v3.Y);
byte[,] ByteMatrix = CP.ByteMatrix;
byte b = ByteMatrix[ix,iy];
if (b != BLOCKED) return v3;
float[,] GP = CP.HeightMap;
float zl = GP[ix,iy];
List<CollisionIndex> RestoreBlocked = new List<CollisionIndex>();
RestoreBlocked.Add(GetCollisionIndex(ix, iy));
int count = 1+CP.NeighborPredicate(ix, iy,(int)( useDist*POINTS_PER_METER), delegate(int NX, int NY)
{
byte NB = ByteMatrix[NX, NY];
if (NB == BLOCKED)
{
float NZ = GP[NX, NY];
if (Math.Abs(NZ - zl) < 0.2)
{
CollisionIndex CI = GetCollisionIndex(NX, NY);
// if (CI.IsTimerTicking) return 0;
RestoreBlocked.Add(CI);
return 1;
}
}
return 0;
});
if (count > 3)
{
Debug("Clearing small area " + v3);
foreach (CollisionIndex CI in RestoreBlocked)
{
// CI.IsTimerTicking = true;
CI.SetMatrixForced(CP,MAYBE_BLOCKED);
}
new Thread(() =>
{
Thread.Sleep(30000);
foreach (CollisionIndex CI in RestoreBlocked)
{
// CI.IsTimerTicking = false;
CI.SetMatrixForced(CP, BLOCKED);
}
});
if (PanelGUI != null) PanelGUI.Invalidate();
return v3;
}
return v3;
SimWaypoint swp = GetWaypointOf(v3);
for (float distance = PathStore.StepSize; distance < useDist * 2; distance += PathStore.StepSize)
{
for (int dir = 0; dir < 360; dir += 15)
{
v3 = GetLocalLeftPos(swp, dir, distance);
b = PathStore.GetNodeQuality(v3, CP);
if (b < BLOCKED) return v3;
}
}
Debug("Clearing area " + swp);
SetNodeQualityTimer(v3, MAYBE_BLOCKED, 30);
for (float distance = PathStore.StepSize; distance < useDist * 2; distance += PathStore.StepSize)
{
for (int dir = 0; dir < 360; dir += 15)
{
v3 = GetLocalLeftPos(swp, dir, distance);
b = PathStore.GetNodeQuality(v3, CP);
if (b == BLOCKED)
{
SetNodeQualityTimer(v3, MAYBE_BLOCKED, 30);
}
}
}
if (PanelGUI != null) PanelGUI.Invalidate();
return GetWaypointOf(v3).SimPosition;
}
internal IList<Vector3d> GetAtLeastPartial(CollisionPlane CP, Vector3 localStart, Vector3 localEnd, float endFudge, out bool OnlyStart, out bool faked)
{
Vector3 newEnd = localEnd;
bool partialOnly;
IList<Vector3d> route = GetLocalPath(CP, localStart, newEnd, out partialOnly, out faked);
if (route.Count > 1)
{
OnlyStart = partialOnly;
return route;
}
OnlyStart = false;
Vector3 diff = localEnd - localStart;
while (diff.Length() > 10)
{
diff = diff * 0.8f;
newEnd = localStart + diff;
route = GetLocalPath(CP, localStart, newEnd, out partialOnly, out faked);
if (partialOnly) OnlyStart = true;
if (route.Count > 1) return route;
}
OnlyStart = false; // Since this will be the best
// try to move to nearby
float step = 45 * RAD2DEG;
for (double angle = 0; angle < PI2; angle += step)
{
newEnd = localEnd + ZAngleVector(angle) * endFudge;
route = GetLocalPath(CP, localStart, newEnd, out partialOnly, out faked);
if (partialOnly) OnlyStart = true;
if (route.Count > 1) return route;
}
route = new List<Vector3d>();
route.Add(LocalToGlobal(localStart));
SimPathStore PathStore = GetPathStore3D(localStart);
faked = true;
Debug("very bad fake route for " + CP);
return route;
}
public static IList<Vector3d> GetPath(CollisionPlane CP, Vector3d globalStart, Vector3d globalEnd, double endFudge, out bool OnlyStart, out bool faked)
{
if (CP == null)
{
var PS = GetPathStore(globalStart);
CP = PS.GetCollisionPlane(globalEnd.Z);
var ret = GetPath(CP, globalStart, globalEnd, endFudge, out OnlyStart, out faked);
if (!OnlyStart && !faked)
{
if (IsRealPath(ret)) return ret;
}
var CP2 = PS.GetCollisionPlane(globalStart.Z);
if (CP2 == CP)
{
return ret;
}
CP = CP2;
//ret = GetPath(CP, globalEnd, globalStart, endFudge, out OnlyStart, out faked);
//if (IsRealPath(ret)) return ret;
}
CP.LastUsed = DateTime.Now;
SimPathStore regStart = SimPathStore.GetPathStore(globalStart);// posStart.GetPathStore();
SimPathStore regEnd = GetPathStore(globalEnd);
Vector3 localStart = GlobalToLocal(globalStart);
Vector3 localEnd = GlobalToLocal(globalEnd);
// Same region?
if (regStart == regEnd)
{
return regStart.GetAtLeastPartial(CP,localStart, localEnd, (float)endFudge, out OnlyStart, out faked);
}
OnlyStart = true; // will be only a partial path
SimPathStore nextRegion;
Vector3 localLast = regStart.LocalOuterEdge(localStart, globalEnd, out nextRegion);
// needs to go to edge
bool partialOnly;
IList<Vector3d> route = regStart.GetLocalPath(CP, localStart, localLast, out partialOnly, out faked);
if (partialOnly) OnlyStart = true;
// at edge so make a crossing
Vector3 enterEdge = EnterEdge(localLast, nextRegion.GetGridLocation() - regStart.GetGridLocation());
route.Add(nextRegion.LocalToGlobal(enterEdge));
return route;
}
internal CollisionPlane CreateMoverPlane(float Z)
{
CollisionPlane found = new CollisionPlane(MAPSPACE, MAPSPACE, Z, this);
CollisionPlane.Debug("Created matrix[{0}] {1} for {2}", Z, found, this);
DropOldMatrixes(6);
lock (Matrixes) Matrixes.Add(found);
if (PanelGUI != null) (new Thread(()=>
PanelGUI.OnNewCollisionPlane(found))).Start();
return found;
}
public bool IsPassable(Vector3 end, CollisionPlane CP)
{
double Dist;
if (GetNodeQuality(end,CP) == BLOCKED) return false;
if (true) return true;
SimWaypoint W = ClosestRegionNode(end.X, end.Y, end.Z, out Dist, true);
return W.IsPassable;
}
private IList<Vector3d> GetLocalPath0(Vector3 start, Vector3 end, CollisionPlane CP, float Z,out bool partialOnly, out bool faked)
{
PathFinderDemo panel = PanelGUI;
PunishChangeDirection = !PunishChangeDirection; //toggle each time
if (!PunishChangeDirection)
{
}
if (panel != null)
{
panel.PnlGUI.CurrentPlane = CP;
}
Point S = ToPoint(start);
Point E = ToPoint(end);
IList<PathFinderNode> pfn = null;
partialOnly = false;
faked = false;
try
{
var bm = CP.ByteMatrix;
CP.SetSurroundings(S.X, S.Y, 2, bm, SimPathStore.PASSABLE);
CP.SetSurroundings(E.X, E.Y, 2, bm, SimPathStore.PASSABLE);
PathFinderFasting pff = new PathFinderFasting(bm);
if (panel != null) panel.SetStartEnd(S, E);
// pff.Diagonals = false;
//pff.ReopenCloseNodes = true;
pff.SearchLimit = 100000000; // took off 2 0s
pff.PunishChangeDirection = PunishChangeDirection;
pfn = pff.FindPath(S, E);
if (pfn == null)
{
Point NE = PartWay(S, E, 0.3f);
{
if (NE != E)
{
pfn = pff.FindPath(S, NE);
if (pfn != null) partialOnly = true;
}
}
if (pfn == null)
{
faked = true;
pfn = pff.FindPathFallback(S, E);
}
}
pff = null;
}
catch (Exception e)
{
Debug("Cant do route! " + e + " on " + CP);
}
if (pfn == null || pfn.Count == 0)
{
Debug("Cant do pfn on " + CP);
IList<Vector3d> temp = new List<Vector3d>();
temp.Add(GetPathStore().LocalToGlobal(end));
faked = true;
return temp;
}
if (panel != null) panel.ShowPath(pfn);
List<Vector3d> r = (List<Vector3d>)PathfinderNodesToV3s(pfn, Z);
r.Reverse();
// faked = false;
return r;
}
public void SetNodeQuality(Vector3 v3, byte v, CollisionPlane CP)
{
CP.ByteMatrix[ARRAY_X((v3.X)), ARRAY_Y((v3.Y))] = v;
}
private float GetZLevel(CollisionPlane CP)
{
return CIndex.GetZLevel(MinZ, MaxZ);
}
public byte GetMatrix(CollisionPlane CP)
{
return(CP.ByteMatrix[PX, PY]);
}
//public class FallingPrim
//{
//public bool DoneMoving = false;
//public uint localId;
public void FallingPrim(OdeScene ps, CollisionPlane Plane, PhysicsVector position, int x, int y, float offsetZ)
{
uint localId = Plane.fallingPrims;
PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
OdePrim oprim = (OdePrim)ps.AddPrimShape("FallingPrim_" + localId, newcube, position, new PhysicsVector(0.1f, 0.1f, 2f), Quaternion.Identity, true);
oprim.LocalID = localId + 100000;
oprim.OnCollisionUpdate += delegate(EventArgs args)
{
if (!oprim.m_taintremove)
{
// CollisionEventUpdate arg = (CollisionEventUpdate)args;
//simhinfo 58 58 30
// DoneMoving = true;
LandingHieghts[x, y] = oprim.Position.Z + offsetZ;
fallingPrims--;
ps.remCollisionEventReporting(oprim);
ps.RemovePrim(oprim);
}
};
oprim.SubscribeEvents(30000);
}
private static Color OccupiedColor(CollisionPlane CP, Color c, CollisionIndex cIndex)
{
//return c;
if (cIndex != null)
{
Color cAdd = cIndex.DebugColor(CP);
int dense = cIndex.OccupiedCount;
int A = 240 - 10 * dense;
if (A < 0) A = 20;
if (cAdd!=Color.Empty)
{
c = cAdd;
}
return Color.FromArgb(A, c.R, c.G, c.B);
}
return c;
}
public string ExtraInfoString(CollisionPlane cp)
{
return CIndex.ExtraInfoString(cp);
}
readonly Color[] lastColour = new Color[256];//(Color.Black);
public Color GetColor(CollisionPlane CP, int x, int y , byte[,] matrix)
{
byte p = matrix[x, y];
switch (p)
{
case STICKY_PASSABLE:
return OccupiedColor(CP, Color.Blue, MeshIndex[x, y]);
case PASSABLE:
return OccupiedColor(CP, Color.Green, MeshIndex[x, y]);
case BLOCKED:
return OccupiedColor(CP, Color.Olive, MeshIndex[x, y]);
case MAYBE_BLOCKED:
return OccupiedColor(CP, Color.Pink, MeshIndex[x, y]);
case BLOCKED_YELLOW:
return OccupiedColor(CP, Color.Yellow, MeshIndex[x, y]);
case BLOCK_ORANGE:
return OccupiedColor(CP, Color.Orange, MeshIndex[x, y]);
case BLOCK_PURPLE:
return OccupiedColor(CP, Color.Orchid, MeshIndex[x, y]);
case WATER_G:
return OccupiedColor(CP, Color.Blue, MeshIndex[x, y]);
case WATER_Z:
return OccupiedColor(CP, Color.CornflowerBlue, MeshIndex[x, y]);
case TOO_LOW:
return OccupiedColor(CP, Color.Tomato, MeshIndex[x, y]);
case TOO_HIGH:
return OccupiedColor(CP, Color.Firebrick, MeshIndex[x, y]);
case BRIDGY:
return OccupiedColor(CP, Color.DarkTurquoise, MeshIndex[x, y]);
}
Color sb = lastColour[p];
if (sb == Color.Empty)
{
int colorIndex = 240 - ((int)(Math.Log10(p) * 127));
colorIndex = colorIndex < Byte.MinValue ? Byte.MinValue : colorIndex > Byte.MaxValue ? Byte.MaxValue : colorIndex;
sb = Color.FromArgb(Byte.MaxValue, colorIndex, colorIndex, colorIndex);
lastColour[p] = sb;
}
return sb;
}
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 void SetMatrix(CollisionPlane CP, int v)
//{
// if (GetMatrix(CP) == SimPathStore.STICKY_PASSABLE) return;
// //if (PathStore.mMatrix[PX, PY] == SimRegion.MAYBE_BLOCKED) return;
// SetMatrixForced(CP, v);
//}
public void SetMatrixForced(CollisionPlane CP, int v)
{
CP.ByteMatrix[PX, PY] = (byte)v;
}
private void SetCurrentPlane(CollisionPlane value)
{
if (InvokeRequired)
{
this.Invoke(new SetCurrentPlaneDelegate(SetCurrentPlane),new object[]{value});
return;
}
if (value != null && _CurrentPlane != value)
{
_CurrentPlane = value;
if (_CurrentPlane != null)
{
PathStore = _CurrentPlane.PathStore;
PathFinderDemo pfd = PathStore.PanelGUI;
if (pfd != null)
{
pfd.SetPlane(_CurrentPlane);
pfd.Text = String.Format("{0} {1}", PathStore.RegionName, _CurrentPlane);
}
_Matrix = _CurrentPlane.ByteMatrix;
}
Invalidate();
}
}
internal System.Drawing.Color DebugColor(CollisionPlane collisionPlane)
{
IEnumerable<CollisionObject> mis = GetOccupiedObjects(collisionPlane.MinZ, collisionPlane.MaxZ);
lock (mis)
foreach (object O in mis)
{
if (O is IMeshedObject)
{
Color c = ((IMeshedObject)O).DebugColor();
if (c != Color.Empty) return c;
}
}
return Color.Empty;
}
public byte GetMatrix(CollisionPlane CP)
{
return CP.ByteMatrix[PX, PY];
}
private float GetZLevel(CollisionPlane CP)
{
return(CIndex.GetZLevel(MinZ, MaxZ));
}
//public void SetMatrix(CollisionPlane CP, int v)
//{
// if (GetMatrix(CP) == SimPathStore.STICKY_PASSABLE) return;
// //if (PathStore.mMatrix[PX, PY] == SimRegion.MAYBE_BLOCKED) return;
// SetMatrixForced(CP, v);
//}
public void SetMatrixForced(CollisionPlane CP, int v)
{
CP.ByteMatrix[PX, PY] = (byte)v;
}
public Vector3 GetUsableLocalPositionOfOLD(CollisionPlane CP, Vector3 v3, float useDist)
{
SimPathStore PathStore = GetPathStore3D(v3);
byte b = PathStore.GetNodeQuality(v3, CP);
// float useDist = GetSizeDistance();
if (b != BLOCKED) return v3;
SimWaypoint swp = GetWaypointOf(v3);
for (float distance = PathStore.StepSize; distance < useDist * 1.5; distance += PathStore.StepSize)
{
for (int dir = 0; dir < 360; dir += 15)
{
v3 = GetLocalLeftPos(swp, dir, distance);
b = PathStore.GetNodeQuality(v3, CP);
if (b < BLOCKED) return v3;
}
}
Debug("Clearing area " + swp);
SetNodeQualityTimer(v3, MAYBE_BLOCKED, 30);
for (float distance = PathStore.StepSize; distance < useDist * 1.5; distance += PathStore.StepSize)
{
for (int dir = 0; dir < 360; dir += 15)
{
v3 = GetLocalLeftPos(swp, dir, distance);
b = PathStore.GetNodeQuality(v3, CP);
if (b == BLOCKED)
{
SetNodeQualityTimer(v3, MAYBE_BLOCKED, 30);
}
}
}
return GetWaypointOf(v3).SimPosition;
}
internal bool IsPortal(CollisionPlane collisionPlane)
{
IEnumerable<CollisionObject> mis = GetOccupiedObjects(collisionPlane.MinZ, collisionPlane.MaxZ);
lock (mis)
foreach (object o in mis)
{
string s = o.ToString().ToLower();
{
if (s.Contains("stair")) return true;
if (s.Contains("ramp")) return true;
// if (s.Contains("brigde")) return true;
if (s.Contains(" path ")) return true;
}
}
return false;
}
public byte GetNodeQuality(Vector3 v3, CollisionPlane CP)
{
return CP.ByteMatrix[ARRAY_X((v3.X)), ARRAY_Y((v3.Y))];
}
private void SetMatrix(CollisionPlane CP, int v)
{
CIndex.SetMatrixForced(CP, v);
}
public string ExtraInfoString(CollisionPlane cp)
{
return(CIndex.ExtraInfoString(cp));
}
public static SimWaypoint CreateLocal(Vector3 from, CollisionPlane CP)
{
SimPathStore PathStore = CP.PathStore;
float POINTS_PER_METER = PathStore.POINTS_PER_METER;
int PX = PathStore.ARRAY_X(from.X);
int PY = PathStore.ARRAY_Y(from.Y);
SimWaypoint WP;
CollisionIndex CI = CollisionIndex.CreateCollisionIndex(from, PathStore);
lock (CI)
{
WP = CI.FindWayPoint(from.Z);
if (WP != null) return WP;
from.X = PX / POINTS_PER_METER;
from.Y = PY / POINTS_PER_METER;
Vector3d GlobalPos = PathStore.LocalToGlobal(from);
if (GlobalPos.X < 256 || GlobalPos.Y < 256)
{
CollisionPlane.Debug("bad global " + GlobalPos);
}
WP = new SimWaypointImpl(from, GlobalPos, CI, CP, PathStore);
WP.IsPassable = true;
}
// wp._IncomingArcs = new ArrayList();
// wp._OutgoingArcs = new ArrayList();
// PathStore.EnsureKnown(wp);
return WP;
}
private float GetMatrix(CollisionPlane CP)
{
return CIndex.GetMatrix(CP);
}
internal void SetPlane(CollisionPlane _CurrentPlane)
{
if (_CurrentPlane == null) return;
if (this.InvokeRequired)
{
Invoke(new SetPlaneDelegate(SetPlane), new object[] { _CurrentPlane});
return;
}
Text = _CurrentPlane.ToString();
MinZevel.Text = ""+_CurrentPlane.MinZ;
PnlGUI.CurrentPlane = _CurrentPlane;
}
public void SetWayPoint(float z, SimWaypoint v)
{
CollisionPlane CP = CollisionPlaneAt(z);
WaypointsHash[CP] = v;
}
private float GetMatrix(CollisionPlane CP)
{
return(CIndex.GetMatrix(CP));
}
private SimWaypointImpl(Vector3 local, Vector3d global, CollisionIndex Ci, CollisionPlane Cp, SimPathStore pathStore)
{
_MinZ = local.Z;
_MaxZ = _MinZ + 1.5f;
CIndex = Ci;
_Plane = Cp;
_PathStore = pathStore;
_GlobalPos = global;// RoundPoint(firstP, PathStore);
_LocalPos = local;
//PX = PX0;
//PY = PY0;
//PathStore.SimWaypoint[PX, PY] = this;
//TaintMatrix();
//UpdateMatrix(pathStore.GL);
_Passable = true;
_IncomingArcs = null;
_OutgoingArcs = null;
}
internal void OnNewCollisionPlane(CollisionPlane found)
{
if (CurrentPlane == null)
{
CurrentPlane = found;
}
}
void ComputeLandingHeightsOld()
{
float fromZ = MaxZ + 10;
float StepSize = PathStore.StepSize; //0.2f
int MAPSPACE = PathStore.MAPSPACE;
int MAPSPACE1 = MAPSPACE - 1; // 1279
bool needsInit = false;
float[,] _HeightMap = HeightMap;
if (LandingHieghts == null)
{
if (_HeightMap != null)
{
LandingHieghts = (float[, ])_HeightMap.Clone();
}
else
{
LandingHieghts = new float[MAPSPACE, MAPSPACE];
needsInit = true;
}
}
CollisionIndex[,] MeshIndex = PathStore.MeshIndex;
// FallingPrims = new FallingPrim[MAPSPACE, MAPSPACE];
float fy = 256.1f;
OdeScene ps = PathStore.odeScene;
fallingPrims = 0;
for (int y = MAPSPACE1; y >= 0; y--)
{
fy = fy - StepSize;
float fx = 256.1f;
for (int x = MAPSPACE1; x >= 0; x--)
{
fx = fx - StepSize;
if (needsInit)
{
LandingHieghts[x, y] = float.MinValue;
}
if (MeshIndex[x, y] == null)
{
continue;
}
//FallingPrims[x, y] = new
FallingPrim(ps, this, new PhysicsVector(fx, fy, fromZ), x, y, 0f);
fallingPrims++;
}
int MaxTries = 100;
while (fallingPrims > 0 && MaxTries-- > 0)
{
// CollisionPlane.Debug("fallingPrims=" + fallingPrims);
ps.Simulate(0.133f);
}
//CollisionPlane.Debug("fallingPrims left over {0} MaxTries Left over = {1}", fallingPrims, MaxTries);
ps.Simulate(0.133f); // for removal of remainders or not needed?
if (fallingPrims < 10)
{
_HeightMap = LandingHieghts;
}
if (fallingPrims != 0)
{
CollisionPlane.Debug("fallingPrims left over {0} MaxTries Left over = {1}", fallingPrims, MaxTries);
}
else if (y % 100 == 0)
{
CollisionPlane.Debug("Y={0} MaxTries Left over = {1}", y, MaxTries);
}
}
}
