public static int GetNextZ(Mobile m, Position3D loc, Direction d)
{
int newZ;
if (CheckMovement(m, loc, d, out newZ, true))
return newZ;
return loc.Z;
}
// Average position calculation
private void Filter()
{
double x = 0, y = 0, z = 0;
int count = this._samples.Count;
for (int i = 0; i < count; i++)
{
x += _samples[i].Longitude.DecimalDegrees;
y += _samples[i].Latitude.DecimalDegrees;
z += _samples[i].Altitude.ToMeters().Value;
}
this._filteredPositon =
new Position3D(
new Longitude(x / count),
new Latitude(y / count),
Distance.FromMeters(z / count));
}
public void Update(Map map, Position3D center, MousePicking mousePick)
{
int pixelScale = (Settings.UserInterface.PlayWindowPixelDoubling) ? 2 : 1;
if (m_RenderTargetSprites == null || m_RenderTargetSprites.Width != Settings.UserInterface.PlayWindowGumpResolution.Width / pixelScale || m_RenderTargetSprites.Height != Settings.UserInterface.PlayWindowGumpResolution.Height / pixelScale)
{
if (m_RenderTargetSprites != null)
m_RenderTargetSprites.Dispose();
m_RenderTargetSprites = new RenderTarget2D(
m_SpriteBatch.GraphicsDevice,
Settings.UserInterface.PlayWindowGumpResolution.Width / pixelScale,
Settings.UserInterface.PlayWindowGumpResolution.Height / pixelScale,
false,
SurfaceFormat.Color,
DepthFormat.Depth24Stencil8,
0,
RenderTargetUsage.DiscardContents);
}
DetermineIfClientIsUnderEntity(map, center);
DrawEntities(map, center, mousePick, out m_DrawOffset);
}
/// <summary>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition"> The new observation to add to the filter. </param>
/// <param name="deviceError"> Does not currently affect position averaging. </param>
/// <param name="horizontalDOP"> Does not currently affect position averaging. </param>
/// <param name="verticalDOP"> Does not currently affect position averaging. </param>
/// <param name="bearing"> Does not currently affect position averaging. </param>
/// <param name="speed"> Does not currently affect position averaging. </param>
/// <remarks>
/// This method updates the FilteredLocation property without consideration for SampleCount.
/// </remarks>
public override Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
this._samples.Add(gpsPosition);
this._sampleTimes.Add(DateTime.Now);
int count = this._samples.Count;
int timeCount = this._sampleTimes.Count;
int maxCount = 0;
// Only average the number of samples specified in the constructor
while (count > _sampleCount)
{
this._samples.RemoveAt(0);
count--;
maxCount++;
}
// Only 2 times are needed, oldest and most recent.
// Try to remove as many as were removed from the sample collection.
while (timeCount > 2 && maxCount > 0)
{
this._sampleTimes.RemoveAt(0);
timeCount--;
}
Filter();
return _filteredPositon;
}
public static bool CheckMovement(Mobile m, Position3D loc, Direction d, out int newZ, bool forceOK = false)
{
Map map = m.Map;
if (map == null)
{
newZ = 0;
return(true);
}
int xStart = (int)loc.X;
int yStart = (int)loc.Y;
int xForward = xStart, yForward = yStart;
int xRight = xStart, yRight = yStart;
int xLeft = xStart, yLeft = yStart;
bool checkDiagonals = ((int)d & 0x1) == 0x1;
offsetXY(d, ref xForward, ref yForward);
offsetXY((Direction)(((int)d - 1) & 0x7), ref xLeft, ref yLeft);
offsetXY((Direction)(((int)d + 1) & 0x7), ref xRight, ref yRight);
if (xForward < 0 || yForward < 0 || xForward >= map.Width || yForward >= map.Height)
{
newZ = 0;
return(false);
}
int startZ, startTop;
List <Item> itemsStart = m_Pools[0];
List <Item> itemsForward = m_Pools[1];
List <Item> itemsLeft = m_Pools[2];
List <Item> itemsRight = m_Pools[3];
TileFlag reqFlags = ImpassableSurface;
// if (m.CanSwim)
// reqFlags |= TileFlag.Wet;
if (checkDiagonals)
{
MapTile tileStart = map.GetMapTile(xStart, yStart);
MapTile tileForward = map.GetMapTile(xForward, yForward);
MapTile tileLeft = map.GetMapTile(xLeft, yLeft);
MapTile tileRight = map.GetMapTile(xRight, yRight);
if ((tileForward == null) || (tileStart == null) || (tileLeft == null) || (tileRight == null))
{
newZ = (int)loc.Z;
return(false);
}
List <MapTile> tiles = new List <MapTile>(); //m_Sectors;
tiles.Add(tileStart);
tiles.Add(tileForward);
tiles.Add(tileLeft);
tiles.Add(tileRight);
for (int i = 0; i < tiles.Count; ++i)
{
MapTile tile = tiles[i];
for (int j = 0; j < tile.Entities.Count; ++j)
{
AEntity entity = tile.Entities[j]; //Item item = sector.Items[j];
// if (ignoreMovableImpassables && item.Movable && item.ItemData.Impassable)
// continue;
if (entity is Item)
{
Item item = (Item)entity;
if ((item.ItemData.Flags & reqFlags) == 0)
{
continue;
}
if (tile == tileStart && item.AtWorldPoint(xStart, yStart) && item.ItemID < 0x4000)
{
itemsStart.Add(item);
}
else if (tile == tileForward && item.AtWorldPoint(xForward, yForward) && item.ItemID < 0x4000)
{
itemsForward.Add(item);
}
else if (tile == tileLeft && item.AtWorldPoint(xLeft, yLeft) && item.ItemID < 0x4000)
{
itemsLeft.Add(item);
}
else if (tile == tileRight && item.AtWorldPoint(xRight, yRight) && item.ItemID < 0x4000)
{
itemsRight.Add(item);
}
}
}
}
}
else
{
MapTile tileStart = map.GetMapTile(xStart, yStart);
MapTile tileForward = map.GetMapTile(xForward, yForward);
if ((tileForward == null) || (tileStart == null))
{
newZ = (int)loc.Z;
return(false);
}
if (tileStart == tileForward)
{
for (int i = 0; i < tileStart.Entities.Count; ++i)
{
AEntity entity = tileStart.Entities[i]; // Item item = sectorStart.Items[i];
if (entity is Item)
{
Item item = (Item)entity;
// if (ignoreMovableImpassables && item.Movable && item.ItemData.Impassable)
// continue;
if ((item.ItemData.Flags & reqFlags) == 0)
{
continue;
}
if (item.AtWorldPoint(xStart, yStart) && item.ItemID < 0x4000)
{
itemsStart.Add(item);
}
else if (item.AtWorldPoint(xForward, yForward) && item.ItemID < 0x4000)
{
itemsForward.Add(item);
}
}
}
}
else
{
for (int i = 0; i < tileForward.Entities.Count; ++i)
{
AEntity entity = tileForward.Entities[i]; // Item item = sectorForward.Items[i];
if (entity is Item)
{
Item item = (Item)entity;
// if (ignoreMovableImpassables && item.Movable && item.ItemData.Impassable)
// continue;
if ((item.ItemData.Flags & reqFlags) == 0)
{
continue;
}
if (item.AtWorldPoint(xForward, yForward) && item.ItemID < 0x4000)
{
itemsForward.Add(item);
}
}
}
for (int i = 0; i < tileStart.Entities.Count; ++i)
{
AEntity entity = tileStart.Entities[i]; // Item item = sectorStart.Items[i];
if (entity is Item)
{
Item item = (Item)entity;
// if (ignoreMovableImpassables && item.Movable && item.ItemData.Impassable)
// continue;
if ((item.ItemData.Flags & reqFlags) == 0)
{
continue;
}
if (item.AtWorldPoint(xStart, yStart) && item.ItemID < 0x4000)
{
itemsStart.Add(item);
}
}
}
}
}
getStartZ(m, map, loc, itemsStart, out startZ, out startTop);
bool moveIsOk = check(map, m, itemsForward, xForward, yForward, startTop, startZ, out newZ) | forceOK;
if (moveIsOk && checkDiagonals)
{
int hold;
if (!check(map, m, itemsLeft, xLeft, yLeft, startTop, startZ, out hold) && !check(map, m, itemsRight, xRight, yRight, startTop, startZ, out hold))
{
moveIsOk = false;
}
}
for (int i = 0; i < (checkDiagonals ? 4 : 2); ++i)
{
if (m_Pools[i].Count > 0)
{
m_Pools[i].Clear();
}
}
if (!moveIsOk)
{
newZ = startZ;
}
return(moveIsOk);
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</param>
/// <remarks>The results of a Kalman filter are cumulative. Each
/// position processed changes the state of the filter, thus making
/// the results more accurate with each subsequent position.</remarks>
public override void Initialize(Position3D gpsPosition)
{
Initialize(gpsPosition, DilutionOfPrecision.CurrentAverageDevicePrecision, DilutionOfPrecision.Good, Ellipsoid.Default);
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</param>
/// <param name="deviceError">A distance measure of device error</param>
/// <param name="horizontalDOP">The horizontal dilution of precision</param>
/// <param name="verticalDOP">The vertical dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Ellipsoid ellipsoid)
{
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_currentState = new KalmanSystemState(gpsPosition, deviceError, horizontalDOP, verticalDOP, ellipsoid);
}
/// <summary>
/// Returns the 3D position
/// </summary>
/// <param name="gpsPosition">The gps Position</param>
/// <param name="currentDOP">The current dilution of precision</param>
/// <param name="bearing">the directional azimuth</param>
/// <param name="speed">the magnitude of the velocity</param>
/// <returns>A Position3D sturcture</returns>
public Position3D Filter(Position3D gpsPosition, DilutionOfPrecision currentDOP, Azimuth bearing, Speed speed)
{
return Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, bearing, Speed.AtRest);
}
/// <summary>
/// Initializes the state to the supplied observation.
/// </summary>
/// <param name="z">The z.</param>
public void Initialize(Position3D z)
{
SquareMatrix3D hi = SquareMatrix3D.Invert(_h);
_z = z.ToCartesianPoint(_ellipsoid);
//s.x = inv(s.H)*s.z;
_x = hi * _z;
//s.P = inv(s.H)*s.R*inv(s.H');
_p = hi * _r * SquareMatrix3D.Invert(SquareMatrix3D.Transpose(_h));
_lastObservation = DateTime.Now;
_delay = TimeSpan.Zero;
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">A distance measure of device error</param>
public void Initialize(Position3D gpsPosition, Distance deviceError)
{
Initialize(gpsPosition, deviceError, DilutionOfPrecision.Good, Ellipsoid.Default);
}
private void DrawEntities(Map map, Position3D center, MousePicking mousePicking, out Vector2 renderOffset)
{
if (center == null)
{
renderOffset = new Vector2();
return;
}
// reset the spritebatch Z
m_SpriteBatch.Reset();
// set the lighting variables.
m_SpriteBatch.SetLightIntensity(Lighting.IsometricLightLevel);
m_SpriteBatch.SetLightDirection(Lighting.IsometricLightDirection);
// get variables that describe the tiles drawn in the viewport: the first tile to draw,
// the offset to that tile, and the number of tiles drawn in the x and y dimensions.
Point firstTile, renderDimensions;
int overDrawTilesOnSides = 3;
int overDrawTilesAtTopAndBottom = 6;
int overDrawAdditionalTilesOnBottom = 10;
CalculateViewport(center, overDrawTilesOnSides, overDrawTilesAtTopAndBottom, out firstTile, out renderOffset, out renderDimensions);
CountEntitiesRendered = 0; // Count of objects rendered for statistics and debug
MouseOverList overList = new MouseOverList(mousePicking); // List of entities mouse is over.
List<AEntity> deferredToRemove = new List<AEntity>();
for (int y = 0; y < renderDimensions.Y * 2 + 1 + overDrawAdditionalTilesOnBottom; y++)
{
Vector3 drawPosition = new Vector3();
drawPosition.X = (firstTile.X - firstTile.Y + (y % 2)) * TILE_SIZE_FLOAT_HALF + renderOffset.X;
drawPosition.Y = (firstTile.X + firstTile.Y + y) * TILE_SIZE_FLOAT_HALF + renderOffset.Y;
Point firstTileInRow = new Point(firstTile.X + ((y + 1) / 2), firstTile.Y + (y / 2));
for (int x = 0; x < renderDimensions.X + 1; x++)
{
MapTile tile = map.GetMapTile(firstTileInRow.X - x, firstTileInRow.Y + x);
if (tile == null)
{
drawPosition.X -= TILE_SIZE_FLOAT;
continue;
}
List<AEntity> entities = tile.Entities;
bool draw = true;
for (int i = 0; i < entities.Count; i++)
{
if (entities[i] is DeferredEntity)
deferredToRemove.Add(entities[i]);
if (!m_DrawTerrain)
{
if ((entities[i] is Ground) || (entities[i].Z > tile.Ground.Z))
draw = false;
}
if ((entities[i].Z >= m_DrawMaxItemAltitude || (m_DrawMaxItemAltitude != 255 && entities[i] is Item && (entities[i] as Item).ItemData.IsRoof)) && !(entities[i] is Ground))
{
continue;
}
if (draw)
{
AEntityView view = entities[i].GetView();
if (view != null)
{
if (view.Draw(m_SpriteBatch, drawPosition, overList, map, !m_UnderSurface))
CountEntitiesRendered++;
}
}
}
foreach (AEntity deferred in deferredToRemove)
tile.OnExit(deferred);
deferredToRemove.Clear();
drawPosition.X -= TILE_SIZE_FLOAT;
}
}
OverheadsView.Render(m_SpriteBatch, overList, map, m_UnderSurface);
// Update the MouseOver objects
mousePicking.UpdateOverEntities(overList, mousePicking.Position);
// Draw the objects we just send to the spritebatch.
m_SpriteBatch.GraphicsDevice.SetRenderTarget(m_RenderTargetSprites);
m_SpriteBatch.GraphicsDevice.Clear(Color.Black);
m_SpriteBatch.FlushSprites(true);
m_SpriteBatch.GraphicsDevice.SetRenderTarget(null);
}
public override void _Ready()
{
_markLocation = GetNode <Position3D>("MarkLocation");
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <remarks>The results of a Kalman filter are cumulative. Each
/// position processed changes the state of the filter, thus making
/// the results more accurate with each subsequent position.</remarks>
public override void Initialize(Position3D gpsPosition)
{
Initialize(gpsPosition, DilutionOfPrecision.CurrentAverageDevicePrecision, DilutionOfPrecision.Good, Ellipsoid.Default);
}
public override void _Ready()
{
RotationNode = (Spatial)GetNode(RotationNodePath);
EyeRelief = (RemoteTransform)GetNode(EyeReliefPath);
RemoteEyeRelief = (Position3D)EyeRelief.GetNode(EyeRelief.RemotePath);
}
internal virtual void Draw(MapTile tile, Position3D position)
{
}
// ============================================================
// Methods
// ============================================================
public AEntity(Serial serial, Map map)
{
Serial = serial;
Map = map;
m_Position = new Position3D(OnTileChanged);
}
internal static object GetValueFromTypeAndArray(byte type, byte[] array)
{
byte[] take;
int length, count;
var sb = new StringBuilder();
switch (type)
{
case 0x07:
return(array[0]);
case 0x08:
return(BitConverter.ToChar(array, 0));
case 0x09:
take = array.Take(4).Reverse().ToArray();
return(BitConverter.ToInt32(take, 0));
case 0x0A:
take = array.Take(4).Reverse().ToArray();
return(BitConverter.ToSingle(take, 0));
case 0x0B:
take = array.Take(8).Reverse().ToArray();
return(BitConverter.ToDouble(take, 0));
case 0x0C:
sb = new StringBuilder();
take = array.Take(4).Reverse().ToArray();
length = BitConverter.ToInt32(take, 0);
for (var i = 0; i < length; ++i)
{
sb.Append((char)array[i + 4]);
}
return(sb.ToString());
case 0x0E:
sb = new StringBuilder();
take = array.Take(4).Reverse().ToArray();
count = BitConverter.ToInt32(take, 0);
var list = new List <string>();
var k1 = 4;
for (var i1 = 0; i1 < count; ++i1)
{
sb.Clear();
take = array.Skip(k1).Take(4).Reverse().ToArray();
k1 += 4;
length = BitConverter.ToInt32(take, 0);
for (var j = 0; j < length; ++j)
{
sb.Append((char)array[k1]);
++k1;
}
list.Add(sb.ToString());
}
return(list);
case 0x0F:
take = array.Take(4).Reverse().ToArray();
count = BitConverter.ToInt32(take, 0);
var ctlist = new List <ComposedTypeBase>();
var k2 = 4;
for (var i = 0; i < count; ++i)
{
var ctype = array[k2];
++k2;
switch (ctype)
{
case 0x00:
var lonlat = new LonLatPosition();
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
lonlat.Longitude = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
lonlat.Latitude = BitConverter.ToDouble(take, 0);
ctlist.Add(lonlat);
break;
case 0x01:
var pos2d = new Position2D();
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
pos2d.X = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
pos2d.Y = BitConverter.ToDouble(take, 0);
ctlist.Add(pos2d);
break;
case 0x02:
var lonlatalt = new LonLatAltPosition();
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
lonlatalt.Longitude = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
lonlatalt.Latitude = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
lonlatalt.Altitude = BitConverter.ToDouble(take, 0);
ctlist.Add(lonlatalt);
break;
case 0x03:
var pos3d = new Position3D();
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
pos3d.X = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
pos3d.Y = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
pos3d.Z = BitConverter.ToDouble(take, 0);
ctlist.Add(pos3d);
break;
case 0x04:
var rmp = new RoadMapPosition();
sb = new StringBuilder();
take = array.Skip(k2).Take(4).Reverse().ToArray();
k2 += 4;
length = BitConverter.ToInt32(take, 0);
for (var j = 0; j < length; ++j)
{
sb.Append((char)array[j + 4]);
++k2;
}
rmp.RoadId = sb.ToString();
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
rmp.Pos = BitConverter.ToDouble(take, 0);
rmp.LaneId = array[k2];
ctlist.Add(rmp);
break;
case 0x05:
var bb = new BoundaryBox();
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
bb.LowerLeftX = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
bb.LowerLeftY = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
bb.UpperRightX = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
bb.UpperRightY = BitConverter.ToDouble(take, 0);
ctlist.Add(bb);
break;
case 0x06:
var pol = new Polygon();
var plen = array[k2];
++k2;
for (var j = 0; j < plen; j++)
{
var p = new Position2D();
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
p.X = BitConverter.ToDouble(take, 0);
take = array.Skip(k2).Take(8).Reverse().ToArray();
k2 += 8;
p.Y = BitConverter.ToDouble(take, 0);
pol.Points.Add(p);
}
ctlist.Add(pol);
break;
case 0x0D:
break;
case 0x11:
var c = new Color();
c.R = array[k2++];
c.G = array[k2++];
c.B = array[k2++];
c.A = array[k2++];
ctlist.Add(c);
break;
}
}
return(ctlist);
default:
throw new ArgumentOutOfRangeException();
}
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="meanDOP">The mean dilution of precision</param>
public void Initialize(Position3D gpsPosition, DilutionOfPrecision meanDOP)
{
Initialize(gpsPosition, DilutionOfPrecision.CurrentAverageDevicePrecision, meanDOP, meanDOP, Ellipsoid.Default);
}
private void CalculateViewport(Position3D center, int overDrawTilesOnSides, int overDrawTilesOnTopAndBottom, out Point firstTile, out Vector2 renderOffset, out Point renderDimensions)
{
int pixelScale = (Settings.UserInterface.PlayWindowPixelDoubling) ? 2 : 1;
renderDimensions.Y = Settings.UserInterface.PlayWindowGumpResolution.Height / pixelScale / TILE_SIZE_INTEGER + overDrawTilesOnTopAndBottom; // the number of tiles that are drawn for half the screen (doubled to fill the entire screen).
renderDimensions.X = Settings.UserInterface.PlayWindowGumpResolution.Width / pixelScale / TILE_SIZE_INTEGER + overDrawTilesOnSides; // the number of tiles that are drawn in the x-direction ( + renderExtraColumnsAtSides * 2 ).
int renderDimensionsDiff = Math.Abs(renderDimensions.X - renderDimensions.Y);
renderDimensionsDiff -= renderDimensionsDiff % 2; // make sure this is an even number...
// when the player entity is at a higher z altitude in the world, we must offset the first row drawn so that tiles at lower altitudes are drawn.
// The reverse is not true - at lower altitutdes, higher tiles are never on screen. This is an artifact of UO's isometric projection.
// Note: The value of this variable MUST be a multiple of 2 and MUST be positive.
int firstZOffset = (center.Z > 0) ? (int)Math.Abs(((center.Z + center.Z_offset) / 11)) : 0;
// this is used to draw tall objects that would otherwise not be visible until their ground tile was on screen. This may still skip VERY tall objects (those weird jungle trees?)
firstTile = new Point(center.X - firstZOffset, center.Y - renderDimensions.Y - firstZOffset);
if (renderDimensions.Y > renderDimensions.X)
{
firstTile.X -= renderDimensionsDiff / 2;
firstTile.Y -= renderDimensionsDiff / 2;
}
else
{
firstTile.X += renderDimensionsDiff / 2;
firstTile.Y -= renderDimensionsDiff / 2;
}
renderOffset.X = (((Settings.UserInterface.PlayWindowGumpResolution.Width / pixelScale) + ((renderDimensions.Y) * TILE_SIZE_INTEGER)) / 2) - TILE_SIZE_FLOAT_HALF;
renderOffset.X -= (int)((center.X_offset - center.Y_offset) * TILE_SIZE_FLOAT_HALF);
renderOffset.X -= (firstTile.X - firstTile.Y) * TILE_SIZE_FLOAT_HALF;
renderOffset.X += renderDimensionsDiff * TILE_SIZE_FLOAT_HALF;
renderOffset.Y = ((Settings.UserInterface.PlayWindowGumpResolution.Height / pixelScale) / 2 - (renderDimensions.Y * TILE_SIZE_INTEGER / 2));
renderOffset.Y += ((center.Z + center.Z_offset) * 4);
renderOffset.Y -= (int)((center.X_offset + center.Y_offset) * TILE_SIZE_FLOAT_HALF);
renderOffset.Y -= (firstTile.X + firstTile.Y) * TILE_SIZE_FLOAT_HALF;
renderOffset.Y -= TILE_SIZE_FLOAT_HALF;
renderOffset.Y -= firstZOffset * TILE_SIZE_FLOAT;
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">A distance measure of device error</param>
/// <param name="meanDOP">The mean dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision meanDOP, Ellipsoid ellipsoid)
{
Initialize(gpsPosition, deviceError, meanDOP, meanDOP, ellipsoid);
}
internal void InternalDrawOverheads(MapTile tile, Position3D position)
{
// base entities do not draw, but they can have overheads, so we draw those.
foreach (Overhead overhead in m_Overheads)
{
if (!overhead.IsDisposed)
overhead.Draw(tile, position);
}
}
/// <summary>
/// Return filtered position from specified parameters
/// </summary>
/// <param name="gpsPosition">The GPS position.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <returns></returns>
/// <inheritdocs/>
public override Position Filter(Position gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
Position3D pos3D = Filter(new Position3D(gpsPosition), deviceError, horizontalDOP, verticalDOP, bearing, speed);
return(new Position(pos3D.Latitude, pos3D.Longitude));
}
/// <summary>
/// Initializes a new instance of the <see cref="KalmanSystemState"/> struct.
/// </summary>
/// <param name="gpsPosition">The GPS position.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
internal KalmanSystemState(
Position3D gpsPosition, Distance deviceError,
DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid)
: this(
gpsPosition, deviceError, verticalDOP, horizontalDOP, ellipsoid,
CartesianPoint.Empty, CartesianPoint.Invalid, gpsPosition.ToCartesianPoint(),
null, null, null,
null, null, null)
{ }
/// <summary>
/// Returns the 3D position
/// </summary>
/// <param name="gpsPosition">The gps Position</param>
/// <returns>A Position3D structure</returns>
public override Position3D Filter(Position3D gpsPosition)
{
return(Filter(gpsPosition, _currentState.DeviceError, _currentState.HorizontalDilutionOfPrecision, _currentState.VerticalDilutionOfPrecision, Azimuth.Empty, Speed.AtRest));
}
/// <summary>
/// Updates the state.
/// </summary>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <param name="z">The z.</param>
public void UpdateState(Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed, Position3D z)
{
if (_x.IsInvalid)
{
Initialize(z);
return;
}
// More insanity
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Covariance values are invalid. Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_deviceError = deviceError.Value;
_horizontalDOP = horizontalDOP.Value;
_verticalDOP = verticalDOP.Value;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
// Setup the observation covariance (measurement error)
_r = new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
#region Process Noise Estimation
// Get the translation of the last correction
CartesianPoint subX = _x.ToPosition3D(_ellipsoid)
.TranslateTo(bearing, speed.ToDistance(_delay), _ellipsoid)
.ToCartesianPoint();
// Get the vector of the translation and the last observation
//CartesianPoint w = (subX - this.z);
CartesianPoint w =
new CartesianPoint(
Distance.FromMeters(subX.X.Value - _z.X.Value), // Values are in meters
Distance.FromMeters(subX.Y.Value - _z.Y.Value), // Values are in meters
Distance.FromMeters(subX.Z.Value - _z.Z.Value)); // Values are in meters
// Setup the noise covariance (process error)
_q = new SquareMatrix3D(
Math.Abs(w.X.Value), 0, 0,
0, Math.Abs(w.Y.Value), 0,
0, 0, Math.Abs(w.Z.Value));
#endregion Process Noise Estimation
// Update the observation state
_z = z.ToCartesianPoint(_ellipsoid);
#region State vector prediction and covariance
//s.x = s.A*s.x + s.B*s.u;
//this.x = this.A * this.x + this.B * this.u;
CartesianPoint ax = _a.TransformVector(_x);
CartesianPoint bu = _b.TransformVector(_u);
_x =
new CartesianPoint(
Distance.FromMeters(ax.X.Value + bu.X.Value),
Distance.FromMeters(ax.Y.Value + bu.Y.Value),
Distance.FromMeters(ax.Z.Value + bu.Z.Value));
//s.P = s.A * s.P * s.A' + s.Q;
_p = _a * _p * SquareMatrix3D.Transpose(_a) + _q;
#endregion State vector prediction and covariance
#region Kalman gain factor
//K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
SquareMatrix3D ht = SquareMatrix3D.Transpose(_h);
SquareMatrix3D k = _p * ht * SquareMatrix3D.Invert(_h * _p * ht + _r);
#endregion Kalman gain factor
#region Observational correction
//s.x = s.x + K*(s.z-s.H*s.x);
//this.x = this.x + K * (this.z - this.H * this.x);
CartesianPoint hx = _h.TransformVector(_x);
CartesianPoint zHx = new CartesianPoint(
Distance.FromMeters(_z.X.Value - hx.X.Value),
Distance.FromMeters(_z.Y.Value - hx.Y.Value),
Distance.FromMeters(_z.Z.Value - hx.Z.Value));
CartesianPoint kzHx = k.TransformVector(zHx);
_x =
new CartesianPoint(
Distance.FromMeters(_x.X.Value + kzHx.X.Value),
Distance.FromMeters(_x.Y.Value + kzHx.Y.Value),
Distance.FromMeters(_x.Z.Value + kzHx.Z.Value));
//s.P = s.P - K*s.H*s.P;
_p = _p - k * _h * _p;
#endregion Observational correction
// Bump the state count
_interval++;
// Calculate the average error for the system stste.
_errorState = (_errorState + Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2))) * .5f;
// Calculate the interval between samples
DateTime now = DateTime.Now;
_delay = now.Subtract(_lastObservation);
_lastObservation = now;
}
/// <summary>
/// Returns the 3D position
/// </summary>
/// <param name="gpsPosition">The gps Position</param>
/// <param name="currentDOP">The current dilution of precision</param>
/// <param name="bearing">the directional azimuth</param>
/// <param name="speed">the magnitude of the velocity</param>
/// <returns>A Position3D structure</returns>
public Position3D Filter(Position3D gpsPosition, DilutionOfPrecision currentDOP, Azimuth bearing, Speed speed)
{
return(Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, bearing, Speed.AtRest));
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</param>
/// <param name="meanDOP">The mean dilution of precision</param>
public void Initialize(Position3D gpsPosition, DilutionOfPrecision meanDOP)
{
Initialize(gpsPosition, DilutionOfPrecision.CurrentAverageDevicePrecision, meanDOP, meanDOP, Ellipsoid.Default);
}
private void InternalDetermineIfUnderEntity(Map map, Position3D center)
{
// Are we inside (under a roof)? Do not draw tiles above our head.
_maxItemAltitude = 255;
MapTile mapTile;
if ((mapTile = map.GetMapTile(center.X, center.Y)) == null)
{
return;
}
AEntity underObject, underTerrain;
mapTile.IsZUnderEntityOrGround(center.Z, out underObject, out underTerrain);
// if we are under terrain, then do not draw any terrain at all.
DrawTerrain = (underTerrain == null);
if (underObject == null)
{
return;
}
// Roofing and new floors ALWAYS begin at intervals of 20.
// if we are under a ROOF, then get rid of everything above me.Z + 20
// (this accounts for A-frame roofs). Otherwise, get rid of everything
// at the object above us.Z.
var underItem = underObject as Item;
if (underItem != null)
{
if (underItem.ItemData.IsRoof)
{
_maxItemAltitude = center.Z - (center.Z % 20) + 20;
}
else if (underItem.ItemData.IsSurface || underItem.ItemData.IsWall)
{
_maxItemAltitude = underItem.Z - (underItem.Z % 10);
}
else
{
var z = center.Z + ((underItem.ItemData.Height > 20) ? underItem.ItemData.Height : 20);
_maxItemAltitude = (int)(z);// - (z % 20));
}
}
// If we are under a roof tile, do not make roofs transparent if we are on an edge.
if (!(underObject is Item) || !((Item)underObject).ItemData.IsRoof)
{
return;
}
var isRoofSouthEast = true;
if ((mapTile = map.GetMapTile(center.X + 1, center.Y)) != null)
{
mapTile.IsZUnderEntityOrGround(center.Z, out underObject, out underTerrain);
isRoofSouthEast = underObject != null;
}
if (!isRoofSouthEast)
{
_maxItemAltitude = 255;
}
}
/// <summary>
/// Adds an initialization position.
/// </summary>
/// <param name="gpsPosition"> The initialization position to add. </param>
/// <remarks>
/// This method does not update the SampleCount or the FilteredLocation
/// properties.
/// </remarks>
public override void Initialize(Position3D gpsPosition)
{
this._samples = new List<Position3D>(_sampleCount + 1);
this._sampleTimes = new List<DateTime>(_sampleCount + 1);
this._samples.Add(gpsPosition);
this._sampleTimes.Add(DateTime.Now);
}
private void InternalDrawEntities(Map map, Position3D center, MousePicking mousePick, out Vector2 renderOffset)
{
if (center == null)
{
renderOffset = new Vector2();
return;
}
const int renderDimensionY = 16; // the number of tiles that are drawn for half the screen (doubled to fill the entire screen).
const int renderDimensionX = 18; // the number of tiles that are drawn in the x-direction ( + renderExtraColumnsAtSides * 2 ).
const int renderExtraColumnsAtSides = 2; // the client draws additional tiles at the edge to make wide objects that are mostly offscreen visible.
// when the player entity is higher (z) in the world, we must offset the first row drawn. This variable MUST be a multiple of 2.
int renderZOffset = (center.Z / 14) * 2 + 4;
// this is used to draw tall objects that would otherwise not be visible until their ground tile was on screen. This may still skip VERY tall objects (those weird jungle trees?)
int renderExtraRowsAtBottom = renderZOffset + 10;
Point firstTile = new Point(
center.X + renderExtraColumnsAtSides - ((renderZOffset + 1) / 2),
center.Y - renderDimensionY - renderExtraColumnsAtSides - (renderZOffset / 2));
renderOffset.X = ((Settings.Game.Resolution.Width + ((renderDimensionY) * 44)) / 2) - 22 + renderExtraColumnsAtSides * 44;
renderOffset.X -= (int)((center.X_offset - center.Y_offset) * 22);
renderOffset.X -= (firstTile.X - firstTile.Y) * 22;
renderOffset.Y = ((Settings.Game.Resolution.Height - (renderDimensionY * 44)) / 2);
renderOffset.Y += (center.Z * 4) + (int)(center.Z_offset * 4);
renderOffset.Y -= (int)((center.X_offset + center.Y_offset) * 22);
renderOffset.Y -= (firstTile.X + firstTile.Y) * 22;
renderOffset.Y -= (renderZOffset) * 22;
ObjectsRendered = 0; // Count of objects rendered for statistics and debug
MouseOverList overList = new MouseOverList(_input.MousePosition, mousePick.PickOnly); // List of entities mouse is over.
List <AEntity> deferredToRemove = new List <AEntity>();
for (int col = 0; col < renderDimensionY * 2 + renderExtraRowsAtBottom; col++)
{
Vector3 drawPosition = new Vector3();
drawPosition.X = (firstTile.X - firstTile.Y + (col % 2)) * 22 + renderOffset.X;
drawPosition.Y = (firstTile.X + firstTile.Y + col) * 22 + renderOffset.Y;
Point index = new Point(firstTile.X + ((col + 1) / 2), firstTile.Y + (col / 2));
for (int row = 0; row < renderDimensionX + renderExtraColumnsAtSides * 2; row++)
{
var maptile = map.GetMapTile(index.X - row, index.Y + row);
if (maptile == null)
{
continue;
}
foreach (var entity in maptile.Entities)
{
if (!DrawTerrain)
{
if (entity is Ground)
{
break;
}
if (entity.Z > maptile.Ground.Z)
{
break;
}
}
if (entity.Z >= _maxItemAltitude)
{
continue;
}
var view = entity.GetView();
if (view != null)
{
if (view.Draw(_spriteBatch, drawPosition, overList, map))
{
ObjectsRendered++;
}
}
if (entity is DeferredEntity)
{
deferredToRemove.Add(entity);
}
}
foreach (var deferred in deferredToRemove)
{
maptile.OnExit(deferred);
}
deferredToRemove.Clear();
drawPosition.X -= 44f;
}
}
OverheadRenderer.Render(_spriteBatch, overList, map);
// Update the MouseOver objects
mousePick.UpdateOverEntities(overList, _input.MousePosition);
// Draw the objects we just send to the spritebatch.
_spriteBatch.Prepare(true, true);
_spriteBatch.Flush();
}
public static Vector3D ToVector3D(this Position3D pos) => new Vector3D(pos.X, pos.Y, pos.Z);
public void Draw(Map map, Position3D center, MousePicking mousePick)
{
InternalDetermineIfUnderEntity(map, center);
InternalDrawEntities(map, center, mousePick, out _renderOffset);
}
private static void getStartZ(AEntity m, Map map, Position3D loc, List <Item> itemList, out int zLow, out int zTop)
{
int xCheck = (int)loc.X, yCheck = (int)loc.Y;
MapTile mapTile = map.GetMapTile(xCheck, yCheck);
if (mapTile == null)
{
zLow = int.MinValue;
zTop = int.MinValue;
}
bool landBlocks = mapTile.Ground.LandData.IsImpassible; //(TileData.LandTable[landTile.ID & 0x3FFF].Flags & TileFlag.Impassable) != 0;
// if (landBlocks && m.CanSwim && (TileData.LandTable[landTile.ID & 0x3FFF].Flags & TileFlag.Wet) != 0)
// landBlocks = false;
// else if (m.CantWalk && (TileData.LandTable[landTile.ID & 0x3FFF].Flags & TileFlag.Wet) == 0)
// landBlocks = true;
int landLow = 0, landCenter = 0, landTop = 0;
landCenter = map.GetAverageZ(xCheck, yCheck, ref landLow, ref landTop);
bool considerLand = !mapTile.Ground.IsIgnored;
int zCenter = zLow = zTop = 0;
bool isSet = false;
if (considerLand && !landBlocks && loc.Z >= landCenter)
{
zLow = landLow;
zCenter = landCenter;
if (!isSet || landTop > zTop)
{
zTop = landTop;
}
isSet = true;
}
StaticItem[] staticTiles = mapTile.GetStatics().ToArray();
for (int i = 0; i < staticTiles.Length; ++i)
{
StaticItem tile = staticTiles[i];
int calcTop = ((int)tile.Z + tile.ItemData.CalcHeight);
if ((!isSet || calcTop >= zCenter) && ((tile.ItemData.Flags & TileFlag.Surface) != 0) && loc.Z >= calcTop)
{
// || (m.CanSwim && (id.Flags & TileFlag.Wet) != 0)
// if (m.CantWalk && (id.Flags & TileFlag.Wet) == 0)
// continue;
zLow = (int)tile.Z;
zCenter = calcTop;
int top = (int)tile.Z + tile.ItemData.Height;
if (!isSet || top > zTop)
{
zTop = top;
}
isSet = true;
}
}
for (int i = 0; i < itemList.Count; ++i)
{
Item item = itemList[i];
IO.ItemData id = item.ItemData;
int calcTop = item.Z + id.CalcHeight;
if ((!isSet || calcTop >= zCenter) && ((id.Flags & TileFlag.Surface) != 0) && loc.Z >= calcTop)
{
// || (m.CanSwim && (id.Flags & TileFlag.Wet) != 0)
// if (m.CantWalk && (id.Flags & TileFlag.Wet) == 0)
// continue;
zLow = item.Z;
zCenter = calcTop;
int top = item.Z + id.Height;
if (!isSet || top > zTop)
{
zTop = top;
}
isSet = true;
}
}
if (!isSet)
{
zLow = zTop = (int)loc.Z;
}
else if (loc.Z > zTop)
{
zTop = (int)loc.Z;
}
}
protected override SubContainer3D CreateSubcontainer(Position3D position, Cuboid size)
{
return(new ShelfSubContainer3D(position.X, position.Y, position.Z, size.Width, size.Height, size.Depth));
}
public void Update(double frameMS)
{
if (!IsMoving)
{
if (m_entity.IsClientEntity && m_playerMobile_NextMoveInMS <= 0d)
{
PlayerMobile_CheckForMoveEvent();
}
MobileMoveEvent moveEvent;
int sequence;
if (m_entity.IsClientEntity)
{
while ((moveEvent = m_MoveEvents.GetNextMoveEvent(out sequence)) != null)
{
Facing = (Direction)moveEvent.Facing;
Mobile pl = (Mobile)m_entity;
if (pl.Stamina.Current == 0)
{
break;
}
if ((pl.Stamina.Current < m_MinimumStaminaToRun) && (pl.Stamina.Current > 0) && ((Facing & Direction.Running) == Direction.Running))
{
Facing &= Direction.FacingMask;
}
if (moveEvent.CreatedByPlayerInput)
{
SendMoveRequestPacket(new MoveRequestPacket((byte)Facing, (byte)sequence, moveEvent.Fastwalk));
}
Position3D p = new Position3D(moveEvent.X, moveEvent.Y, moveEvent.Z);
if (p != CurrentPosition)
{
GoalPosition = p;
break;
}
}
}
else
{
moveEvent = m_MoveEvents.GetAndForwardToFinalMoveEvent(out sequence);
if (moveEvent != null)
{
Facing = (Direction)moveEvent.Facing;
Position3D p = new Position3D(moveEvent.X, moveEvent.Y, moveEvent.Z);
if (p != CurrentPosition)
{
GoalPosition = p;
}
}
}
}
// Are we moving? (if our current location != our destination, then we are moving)
if (IsMoving)
{
double diff = (frameMS / MovementSpeed.TimeToCompleteMove(m_entity, Facing));
MoveSequence += diff;
if (m_entity.IsClientEntity)
{
m_playerMobile_NextMoveInMS -= frameMS;
}
if (Math.Abs(GoalPosition.X - CurrentPosition.X) > 1 || Math.Abs(GoalPosition.Y - CurrentPosition.Y) > 1)
{
int x, y;
if (CurrentPosition.X < GoalPosition.X)
{
x = GoalPosition.X - 1;
}
else if (CurrentPosition.X > GoalPosition.X)
{
x = GoalPosition.X + 1;
}
else
{
x = GoalPosition.X;
}
if (CurrentPosition.Y < GoalPosition.Y)
{
y = GoalPosition.Y - 1;
}
else if (CurrentPosition.Y > GoalPosition.Y)
{
y = GoalPosition.Y + 1;
}
else
{
y = GoalPosition.Y;
}
CurrentPosition.Set(x, y, CurrentPosition.Z);
}
if (MoveSequence < 1f)
{
CurrentPosition.Offset = new Vector3(
GoalPosition.X - CurrentPosition.X,
GoalPosition.Y - CurrentPosition.Y,
GoalPosition.Z - CurrentPosition.Z) * (float)MoveSequence;
}
else
{
CurrentPosition.Set(GoalPosition.X, GoalPosition.Y, GoalPosition.Z);
CurrentPosition.Offset = Vector3.Zero;
MoveSequence = 0f;
if (m_entity.IsClientEntity)
{
m_playerMobile_NextMoveInMS = 0;
}
}
}
else
{
MoveSequence = 0f;
if (m_entity.IsClientEntity)
{
m_playerMobile_NextMoveInMS = 0d;
}
}
}
public static void AddCube(List <Vector3> vertices, List <int> triangles, List <Vector2> uvs, Position3D tileMapIndex)
{
var tilePositionInChunk = MapPositionUtil.GetPositionInChunk(tileMapIndex);
var tilePositionRelativeToChunk =
new Vector3(tilePositionInChunk.x * Tile.TileSize.x, tilePositionInChunk.y * Tile.TileSize.y, tilePositionInChunk.z * Tile.TileSize.z);
var tile = Map.Instance.GetTile(tileMapIndex);
if (Map.Instance.IsTransparent(tileMapIndex.getPositionPlusY(1)))
{
CubeGenerator.AddSquareFaceYPositive(vertices, triangles, uvs, tilePositionRelativeToChunk, Tile.TileSize, tile.TileType.TextureTileOffset);
}
if (Map.Instance.IsTransparent(tileMapIndex.getPositionPlusY(-1)))
{
CubeGenerator.AddSquareFaceYNegative(vertices, triangles, uvs, tilePositionRelativeToChunk, Tile.TileSize, tile.TileType.TextureTileOffset);
}
if (Map.Instance.IsTransparent(tileMapIndex.getPositionPlusZ(-1)))
{
CubeGenerator.AddSquareFaceZNegative(vertices, triangles, uvs, tilePositionRelativeToChunk, Tile.TileSize, tile.TileType.TextureTileOffset);
}
if (Map.Instance.IsTransparent(tileMapIndex.getPositionPlusZ(1)))
{
CubeGenerator.AddSquareFaceZPositive(vertices, triangles, uvs, tilePositionRelativeToChunk, Tile.TileSize, tile.TileType.TextureTileOffset);
}
if (Map.Instance.IsTransparent(tileMapIndex.getPositionPlusX(-1)))
{
CubeGenerator.AddSquareFaceXNegative(vertices, triangles, uvs, tilePositionRelativeToChunk, Tile.TileSize, tile.TileType.TextureTileOffset);
}
if (Map.Instance.IsTransparent(tileMapIndex.getPositionPlusX(1)))
{
CubeGenerator.AddSquareFaceXPositive(vertices, triangles, uvs, tilePositionRelativeToChunk, Tile.TileSize, tile.TileType.TextureTileOffset);
}
}
private void DetermineIfClientIsUnderEntity(Map map, Position3D center)
{
// Are we inside (under a roof)? Do not draw tiles above our head.
m_DrawMaxItemAltitude = 255;
m_DrawTerrain = true;
m_UnderSurface = false;
MapTile tile;
AEntity underObject, underTerrain;
if ((tile = map.GetMapTile(center.X, center.Y)) != null)
{
if (tile.IsZUnderEntityOrGround(center.Z, out underObject, out underTerrain))
{
// if we are under terrain, then do not draw any terrain at all.
m_DrawTerrain = (underTerrain == null);
if (!(underObject == null))
{
// Roofing and new floors ALWAYS begin at intervals of 20.
// if we are under a ROOF, then get rid of everything above me.Z + 20
// (this accounts for A-frame roofs). Otherwise, get rid of everything
// at the object above us.Z.
if (underObject is Item)
{
Item item = (Item)underObject;
if (item.ItemData.IsRoof)
m_DrawMaxItemAltitude = center.Z - (center.Z % 20) + 20;
else if (item.ItemData.IsSurface || (item.ItemData.IsWall && !item.ItemData.IsDoor))
m_DrawMaxItemAltitude = item.Z;
else
{
int z = center.Z + ((item.ItemData.Height > 20) ? item.ItemData.Height : 20);
m_DrawMaxItemAltitude = z;
}
}
// If we are under a roof tile, do not make roofs transparent if we are on an edge.
if (underObject is Item && ((Item)underObject).ItemData.IsRoof)
{
bool isRoofSouthEast = true;
if ((tile = map.GetMapTile(center.X + 1, center.Y)) != null)
{
tile.IsZUnderEntityOrGround(center.Z, out underObject, out underTerrain);
isRoofSouthEast = !(underObject == null);
}
if (!isRoofSouthEast)
m_DrawMaxItemAltitude = 255;
}
m_UnderSurface = (m_DrawMaxItemAltitude != 255);
}
}
}
}
public void Render(Camera cam, bool orthographic, bool backfaceCulling, float scale, DepthTexture texture, Position3D modelPosition)
{
//multi threaded
Parallel.ForEach(frames[currentFrame], triangle =>
{
triangle.Render(cam, orthographic, backfaceCulling, scale, texture, modelPosition);
});
}
internal virtual void Draw(MapTile tile, Position3D position)
{
}
public void Update(Map map, Position3D center)
{
var centerCellX = (uint)center.X / 8;
var centerCellY = (uint)center.Y / 8;
var centerDiffX = (int)(centerCellX - _lastCenterCellX);
var centerDiffY = (int)(centerCellY - _lastCenterCellY);
_lastCenterCellX = centerCellX;
_lastCenterCellY = centerCellY;
if (centerDiffX < -1 || centerDiffX > 1 || centerDiffY < -1 || centerDiffY > 1)
{
_mustRedrawEntireTexture = true;
}
if (_mustRedrawEntireTexture)
{
var firstX = centerCellX - 15;
var firstY = centerCellY;
for (uint y = 0; y < 32; y++)
{
for (uint x = 0; x < 16; x++)
{
InternalQueueMapBlock(map, firstX + ((y + 1) / 2) + x, firstY + (y / 2) - x);
}
}
_mustRedrawEntireTexture = false;
}
else if (centerDiffX != 0 || centerDiffY != 0)
{
// draw just enough of the minimap to cover the newly exposed area.
if (centerDiffX < 0)
{
if (centerDiffY <= 0)
{
// traveling UP/WEST, draw new rows.
var firstX = centerCellX - 15;
var firstY = centerCellY;
for (uint y = 0; y < 2; y++)
{
for (uint x = 0; x < 16; x++)
{
InternalQueueMapBlock(map, firstX + x + ((y + 1) / 2), firstY - x + (y / 2));
}
}
}
if (centerDiffY >= 0)
{
// traveling LEFT/WEST, draw a new column.
var firstX = centerCellX - 15;
var firstY = centerCellY + 0;
for (uint y = 0; y < 32; y++)
{
InternalQueueMapBlock(map, firstX + ((y + 1) / 2), firstY + (y / 2));
}
}
}
else if (centerDiffX > 0)
{
if (centerDiffY <= 0)
{
// traveling RIGHT/EAST, draw a new column.
var firstX = centerCellX + 0;
var firstY = centerCellY - 15;
for (uint y = 0; y < 32; y++)
{
InternalQueueMapBlock(map, firstX + ((y + 1) / 2), firstY + (y / 2));
}
}
if (centerDiffY >= 0)
{
// travelling DOWN/EAST, draw new rows.
var firstX = centerCellX + 0;
var firstY = centerCellY + 15;
for (uint y = 0; y < 2; y++)
{
for (uint x = 0; x < 16; x++)
{
InternalQueueMapBlock(map, firstX + ((y + 1) / 2) + x, firstY - x + (y / 2));
}
}
}
}
else if (centerDiffY != 0)
{
if (centerDiffY < 0)
{
// traveling NORTH, draw a new row and column.
var firstX = centerCellX - 15;
var firstY = centerCellY + 0;
for (uint y = 0; y < 2; y++)
{
for (uint x = 0; x < 16; x++)
{
InternalQueueMapBlock(map, firstX + x + ((y + 1) / 2), firstY - x + (y / 2));
}
}
firstX = centerCellX + 0;
firstY = centerCellY - 15;
for (uint y = 0; y < 32; y++)
{
InternalQueueMapBlock(map, firstX + ((y + 1) / 2), firstY + (y / 2));
}
}
else if (centerDiffY > 0)
{
// traveling SOUTH, draw a new row and column.
var firstX = centerCellX - 15;
var firstY = centerCellY + 0;
for (uint y = 0; y < 32; y++)
{
InternalQueueMapBlock(map, firstX + ((y + 1) / 2), firstY + (y / 2));
}
firstX = centerCellX - 0;
firstY = centerCellY + 15;
for (uint y = 0; y < 2; y++)
{
for (uint x = 0; x < 16; x++)
{
InternalQueueMapBlock(map, firstX + ((y + 1) / 2) + x, firstY - x + (y / 2));
}
}
}
}
}
if (_queuedToDrawBlocks.Count > 0)
{
InternalDrawQueuedMapBlocks();
//_spriteBatch.GraphicsDevice.Textures[3] = null;
//Texture.SetData(_textureData);
//_spriteBatch.GraphicsDevice.Textures[3] = Texture;
}
}
// ============================================================
// Methods
// ============================================================
public AEntity(Serial serial, Map map)
{
Serial = serial;
Map = map;
m_Position = new Position3D(OnTileChanged);
}
private void DetermineIfClientIsUnderEntity(Map map, Position3D center)
{
// Are we inside (under a roof)? Do not draw tiles above our head.
m_DrawMaxItemAltitude = 255;
m_DrawTerrain = true;
m_UnderSurface = false;
MapTile tile;
AEntity underObject, underTerrain;
if ((tile = map.GetMapTile(center.X, center.Y)) != null)
{
if (tile.IsZUnderEntityOrGround(center.Z, out underObject, out underTerrain))
{
// if we are under terrain, then do not draw any terrain at all.
m_DrawTerrain = (underTerrain == null);
if (!(underObject == null))
{
// Roofing and new floors ALWAYS begin at intervals of 20.
// if we are under a ROOF, then get rid of everything above me.Z + 20
// (this accounts for A-frame roofs). Otherwise, get rid of everything
// at the object above us.Z.
if (underObject is Item)
{
Item item = (Item)underObject;
if (item.ItemData.IsRoof)
{
m_DrawMaxItemAltitude = center.Z - (center.Z % 20) + 20;
}
else if (item.ItemData.IsSurface || (item.ItemData.IsWall && !item.ItemData.IsDoor))
{
m_DrawMaxItemAltitude = item.Z;
}
else
{
int z = center.Z + ((item.ItemData.Height > 20) ? item.ItemData.Height : 20);
m_DrawMaxItemAltitude = z;
}
}
// If we are under a roof tile, do not make roofs transparent if we are on an edge.
if (underObject is Item && ((Item)underObject).ItemData.IsRoof)
{
bool isRoofSouthEast = true;
if ((tile = map.GetMapTile(center.X + 1, center.Y)) != null)
{
tile.IsZUnderEntityOrGround(center.Z, out underObject, out underTerrain);
isRoofSouthEast = !(underObject == null);
}
if (!isRoofSouthEast)
{
m_DrawMaxItemAltitude = 255;
}
}
m_UnderSurface = (m_DrawMaxItemAltitude != 255);
}
}
}
}
/// <summary>
/// Return a filtered Position3D from the specified parameters
/// </summary>
/// <param name="gpsPosition">The GPS position.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <returns></returns>
public override Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_currentState.UpdateState(deviceError, horizontalDOP, verticalDOP, bearing, speed, gpsPosition);
return _currentState.CorrectedLocation();
}
private void DrawEntities(Map map, Position3D center, MousePicking mousePicking, out Vector2 renderOffset)
{
if (center == null)
{
renderOffset = new Vector2();
return;
}
// reset the spritebatch Z
m_SpriteBatch.Reset();
// set the lighting variables.
m_SpriteBatch.SetLightIntensity(Lighting.IsometricLightLevel);
m_SpriteBatch.SetLightDirection(Lighting.IsometricLightDirection);
// get variables that describe the tiles drawn in the viewport: the first tile to draw,
// the offset to that tile, and the number of tiles drawn in the x and y dimensions.
Point firstTile, renderDimensions;
int overDrawTilesOnSides = 3;
int overDrawTilesAtTopAndBottom = 6;
int overDrawAdditionalTilesOnBottom = 10;
CalculateViewport(center, overDrawTilesOnSides, overDrawTilesAtTopAndBottom, out firstTile, out renderOffset, out renderDimensions);
CountEntitiesRendered = 0; // Count of objects rendered for statistics and debug
MouseOverList overList = new MouseOverList(mousePicking); // List of entities mouse is over.
List <AEntity> deferredToRemove = new List <AEntity>();
for (int y = 0; y < renderDimensions.Y * 2 + 1 + overDrawAdditionalTilesOnBottom; y++)
{
Vector3 drawPosition = new Vector3();
drawPosition.X = (firstTile.X - firstTile.Y + (y % 2)) * TILE_SIZE_FLOAT_HALF + renderOffset.X;
drawPosition.Y = (firstTile.X + firstTile.Y + y) * TILE_SIZE_FLOAT_HALF + renderOffset.Y;
Point firstTileInRow = new Point(firstTile.X + ((y + 1) / 2), firstTile.Y + (y / 2));
for (int x = 0; x < renderDimensions.X + 1; x++)
{
MapTile tile = map.GetMapTile(firstTileInRow.X - x, firstTileInRow.Y + x);
if (tile == null)
{
drawPosition.X -= TILE_SIZE_FLOAT;
continue;
}
List <AEntity> entities = tile.Entities;
bool draw = true;
for (int i = 0; i < entities.Count; i++)
{
if (entities[i] is DeferredEntity)
{
deferredToRemove.Add(entities[i]);
}
if (!m_DrawTerrain)
{
if ((entities[i] is Ground) || (entities[i].Z > tile.Ground.Z))
{
draw = false;
}
}
if ((entities[i].Z >= m_DrawMaxItemAltitude || (m_DrawMaxItemAltitude != 255 && entities[i] is Item && (entities[i] as Item).ItemData.IsRoof)) && !(entities[i] is Ground))
{
continue;
}
if (draw)
{
AEntityView view = entities[i].GetView();
if (view != null)
{
if (view.Draw(m_SpriteBatch, drawPosition, overList, map, !m_UnderSurface))
{
CountEntitiesRendered++;
}
}
}
}
foreach (AEntity deferred in deferredToRemove)
{
tile.OnExit(deferred);
}
deferredToRemove.Clear();
drawPosition.X -= TILE_SIZE_FLOAT;
}
}
OverheadsView.Render(m_SpriteBatch, overList, map, m_UnderSurface);
// Update the MouseOver objects
mousePicking.UpdateOverEntities(overList, mousePicking.Position);
// Draw the objects we just send to the spritebatch.
m_SpriteBatch.GraphicsDevice.SetRenderTarget(m_RenderTargetSprites);
m_SpriteBatch.GraphicsDevice.Clear(Color.Black);
m_SpriteBatch.FlushSprites(true);
m_SpriteBatch.GraphicsDevice.SetRenderTarget(null);
}
/// <summary>
/// Initializes a new instance of the <see cref="KalmanSystemState"/> struct.
/// </summary>
/// <param name="gpsPosition">The GPS position.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
/// <param name="u">The u.</param>
/// <param name="x">The x.</param>
/// <param name="z">The z.</param>
/// <param name="a">A.</param>
/// <param name="b">The b.</param>
/// <param name="h">The h.</param>
/// <param name="p">The p.</param>
/// <param name="q">The q.</param>
/// <param name="r">The r.</param>
internal KalmanSystemState(
Position3D gpsPosition, Distance deviceError,
DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid,
CartesianPoint u, CartesianPoint x, CartesianPoint z,
SquareMatrix3D a, SquareMatrix3D b, SquareMatrix3D h,
SquareMatrix3D p, SquareMatrix3D q, SquareMatrix3D r)
{
_deviceError = deviceError.IsEmpty ? DilutionOfPrecision.CurrentAverageDevicePrecision.Value : deviceError.Value;
_horizontalDOP = horizontalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : horizontalDOP.Value;
_verticalDOP = verticalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : verticalDOP.Value;
_ellipsoid = ellipsoid ?? Ellipsoid.Default;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
_u = u.IsInvalid ? CartesianPoint.Empty : u;
_x = x;
_z = z.IsInvalid ? CartesianPoint.Empty : z;
_a = a ?? new SquareMatrix3D();
_b = b ?? new SquareMatrix3D();
_h = h ?? new SquareMatrix3D();
_p = p ?? new SquareMatrix3D();
_q = q ?? SquareMatrix3D.Default(0);
_r = r ?? new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
_interval = 0;
_errorState = Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2));
_delay = TimeSpan.MaxValue;
_lastObservation = DateTime.MinValue;
if (!gpsPosition.IsEmpty)
Initialize(gpsPosition);
}
public void Unrender(Position3D chunkPosition)
{
Object.Destroy(_renderedChunks[chunkPosition]);
_renderedChunks.Remove(chunkPosition);
}
/// <summary>
/// Updates the state.
/// </summary>
/// <param name="currentDOP">The current DOP.</param>
/// <param name="z">The z.</param>
public void UpdateState(DilutionOfPrecision currentDOP, Position3D z)
{
UpdateState(Distance.FromMeters(_deviceError), currentDOP, currentDOP, Azimuth.Empty, Speed.AtRest, z);
}
private static void getStartZ(AEntity m, Map map, Position3D loc, List<Item> itemList, out int zLow, out int zTop)
{
int xCheck = (int)loc.X, yCheck = (int)loc.Y;
MapTile mapTile = map.GetMapTile(xCheck, yCheck);
if (mapTile == null)
{
zLow = int.MinValue;
zTop = int.MinValue;
}
bool landBlocks = mapTile.Ground.LandData.IsImpassible; //(TileData.LandTable[landTile.ID & 0x3FFF].Flags & TileFlag.Impassable) != 0;
// if (landBlocks && m.CanSwim && (TileData.LandTable[landTile.ID & 0x3FFF].Flags & TileFlag.Wet) != 0)
// landBlocks = false;
// else if (m.CantWalk && (TileData.LandTable[landTile.ID & 0x3FFF].Flags & TileFlag.Wet) == 0)
// landBlocks = true;
int landLow = 0, landCenter = 0, landTop = 0;
landCenter = map.GetAverageZ(xCheck, yCheck, ref landLow, ref landTop);
bool considerLand = !mapTile.Ground.IsIgnored;
int zCenter = zLow = zTop = 0;
bool isSet = false;
if (considerLand && !landBlocks && loc.Z >= landCenter)
{
zLow = landLow;
zCenter = landCenter;
if (!isSet || landTop > zTop)
zTop = landTop;
isSet = true;
}
StaticItem[] staticTiles = mapTile.GetStatics().ToArray();
for (int i = 0; i < staticTiles.Length; ++i)
{
StaticItem tile = staticTiles[i];
int calcTop = ((int)tile.Z + tile.ItemData.CalcHeight);
if ((!isSet || calcTop >= zCenter) && ((tile.ItemData.Flags & TileFlag.Surface) != 0) && loc.Z >= calcTop)
{
// || (m.CanSwim && (id.Flags & TileFlag.Wet) != 0)
// if (m.CantWalk && (id.Flags & TileFlag.Wet) == 0)
// continue;
zLow = (int)tile.Z;
zCenter = calcTop;
int top = (int)tile.Z + tile.ItemData.Height;
if (!isSet || top > zTop)
zTop = top;
isSet = true;
}
}
for (int i = 0; i < itemList.Count; ++i)
{
Item item = itemList[i];
ItemData id = item.ItemData;
int calcTop = item.Z + id.CalcHeight;
if ((!isSet || calcTop >= zCenter) && ((id.Flags & TileFlag.Surface) != 0) && loc.Z >= calcTop)
{
// || (m.CanSwim && (id.Flags & TileFlag.Wet) != 0)
// if (m.CantWalk && (id.Flags & TileFlag.Wet) == 0)
// continue;
zLow = item.Z;
zCenter = calcTop;
int top = item.Z + id.Height;
if (!isSet || top > zTop)
zTop = top;
isSet = true;
}
}
if (!isSet)
zLow = zTop = (int)loc.Z;
else if (loc.Z > zTop)
zTop = (int)loc.Z;
}
/// <summary>
/// Kalman Filter with parameters
/// </summary>
/// <param name="initialObservation">The initial observation.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
public KalmanFilter(
Position3D initialObservation,
Distance deviceError,
DilutionOfPrecision horizontalDOP,
DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid)
{
_currentState = new KalmanSystemState(
initialObservation,
deviceError,
horizontalDOP,
verticalDOP,
ellipsoid);
}
public static Body Create(int count, double size)
{
var creator = new GraphicsCreator();
var positions = CreatePositions(count, size);
for (var y = 0; y < count; y++)
{
for (var x = 0; x < count; x++)
{
var point1 = positions[x][y];
var point2 = positions[x][y + 1];
var point3 = positions[x + 1][y + 1];
var point4 = positions[x + 1][y];
creator.AddFace(true, false);
creator.AddTriangle(point1, point2, point4);
creator.AddTriangle(point3, point4, point2);
}
}
size = size * count;
size = size > 0 ? size / 2.0 : 0.5;
var hight = 2;
var p1 = new Position3D(-size, -size, -hight);
var p2 = new Position3D(size, -size, -hight);
var p3 = new Position3D(size, -size, 0);
var p4 = new Position3D(-size, -size, 0);
var p5 = new Position3D(-size, size, -hight);
var p6 = new Position3D(size, size, -hight);
var p7 = new Position3D(size, size, 0);
var p8 = new Position3D(-size, size, 0);
// South
creator.AddFace(true, false);
creator.AddTriangle(p1, p2, p3);
creator.AddTriangle(p3, p4, p1);
// East
creator.AddFace(true, false);
creator.AddTriangle(p2, p6, p7);
creator.AddTriangle(p7, p3, p2);
// North
creator.AddFace(true, false);
creator.AddTriangle(p6, p5, p8);
creator.AddTriangle(p8, p7, p6);
// West
creator.AddFace(true, false);
creator.AddTriangle(p5, p1, p4);
creator.AddTriangle(p4, p8, p5);
// Bottom
creator.AddFace(true, false);
creator.AddTriangle(p2, p1, p5);
creator.AddTriangle(p5, p6, p2);
var body = creator.CreateBody();
return(body);
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</param>
/// <param name="deviceError">A distance measure of device error</param>
public void Initialize(Position3D gpsPosition, Distance deviceError)
{
Initialize(gpsPosition, deviceError, DilutionOfPrecision.Good, Ellipsoid.Default);
}
/// <summary> /// Return a filtered Position3d /// </summary> /// <param name="gpsPosition">The GPS position.</param> /// <returns></returns> public abstract Position3D Filter(Position3D gpsPosition);
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</param>
/// <param name="deviceError">A distance measure of device error</param>
/// <param name="meanDOP">The mean dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision meanDOP, Ellipsoid ellipsoid)
{
Initialize(gpsPosition, deviceError, meanDOP, meanDOP, ellipsoid);
}
/// <summary> /// Return a filtered Position3D from the specified parameters /// </summary> /// <param name="gpsPosition">The GPS position.</param> /// <param name="deviceError">The device error.</param> /// <param name="horizontalDOP">The horizontal DOP.</param> /// <param name="verticalDOP">The vertical DOP.</param> /// <param name="bearing">The bearing.</param> /// <param name="speed">The speed.</param> /// <returns></returns> public abstract Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed);
/// <summary>
/// Returns the 3D position
/// </summary>
/// <param name="gpsPosition">The gps Position</param>
/// <returns>A Position3D sturcture</returns>
public override Position3D Filter(Position3D gpsPosition)
{
return Filter(gpsPosition, _currentState.DeviceError, _currentState.HorizontalDilutionOfPrecision, _currentState.VerticalDilutionOfPrecision, Azimuth.Empty, Speed.AtRest);
}
public virtual void PlaceObject(Object3D theObject, Position3D position)
{
var placedObject = new PlacedObject3D(position, theObject);
PlacedObjects.Add(placedObject);
}
/// <summary>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition"> The new observation to add to the filter. </param>
/// <returns> The filtered position. </returns>
/// <remarks>
/// This method updates the FilteredLocation property without consideration for SampleCount.
/// </remarks>
public override Position3D Filter(Position3D gpsPosition)
{
return Filter(gpsPosition);
}
public static bool CheckMovementForced(Mobile m, Position3D loc, Direction d, out int newZ)
{
return(CheckMovement(m, loc, d, out newZ, true));
}
/// <summary> /// Initialise the filter from a specified Position3D /// </summary> /// <param name="gpsPosition">The GPS position.</param> public abstract void Initialize(Position3D gpsPosition);
/// <summary>
/// Constructor
/// </summary>
/// <param name="parentState"></param>
/// <param name="pointX"></param>
/// <param name="pointY"></param>
/// <param name="pointZ"></param>
public MazeState(AState parentState, int pointX, int pointY, int pointZ)
: base(parentState)
{
m_state = "(" + pointX + "," + pointY + "," + pointZ + ")";
m_position = new Position3D(pointZ, pointY, pointX);
}
