public ICoordinate GetResult(int objectId, params IDistance[] distances)
{
double minH = double.MaxValue;
double maxH = 0;
double maxW = 0;
double minW = double.MaxValue;
foreach (IDistance distance in distances)
{
IAnchor anchor = _anchors[distance.FromAnchorId];
if (maxH < anchor.Z + distance.Distance)
{
maxH = anchor.Z + distance.Distance;
}
if (minH > anchor.Z - distance.Distance)
{
minH = anchor.Z - distance.Distance;
}
if (maxW < anchor.X + distance.Distance)
{
maxW = anchor.X + distance.Distance;
}
if (minW > anchor.X - distance.Distance)
{
minW = anchor.X - distance.Distance;
}
}
maxH += 50;
minH -= 50;
maxW += 50;
minW -= 50;
var worldSizeInfo = new WorldSizeInfo();
worldSizeInfo.WorldSizeWidthMin = minW;
worldSizeInfo.WorldSizeWidthMax = maxW;
worldSizeInfo.WorldSizeHeightMin = minH;
worldSizeInfo.WorldSizeHeightMax = maxH;
List <PJayParticle> particles;
bool doesExist = false;
if (_particles.ContainsKey(objectId))
{
particles = _particles[objectId].Particles;
doesExist = true;
}
else
{
particles = new List <PJayParticle>(_numberOfParticles);
for (int i = 0; i < _numberOfParticles; i++)
{
double x = Random.Next((int)worldSizeInfo.WorldSizeWidthMin,
(int)worldSizeInfo.WorldSizeWidthMax);
double z = Random.Next((int)worldSizeInfo.WorldSizeHeightMin,
(int)worldSizeInfo.WorldSizeHeightMax);
var particle = new PJayParticle(x, z);
particle.SetNoise(_numberOfCircleDistances);
particles.Add(particle);
}
_particles.Add(objectId, new ParticleHistory(10)
{
Particles = particles,
});
}
// particles = new List<PJayParticle>(_numberOfParticles);
ICoordinate result = GenerateCoordinates(distances, particles, out IList <PJayParticle> trainedParticles, doesExist);
double averageX = trainedParticles.Average(x => x.X);
double averageZ = trainedParticles.Average(x => x.Z);
for (int i = trainedParticles.Count; i < _numberOfParticles; i++)
{
double x = averageX + Random.Next(-_numberOfCircleDistances, _numberOfCircleDistances);
double z = averageZ + Random.Next(-_numberOfCircleDistances, _numberOfCircleDistances);
var particle = new PJayParticle(x, z);
particle.SetNoise(_numberOfCircleDistances);
trainedParticles.Add(particle);
}
_particles[objectId].Particles = trainedParticles as List <PJayParticle> ?? trainedParticles.ToList();
DateTime now = DateTime.Now;
if (_particles[objectId].GetAcceleration(result, now, out double acceleration) && !double.IsNaN(acceleration))
{
double std = _particles[objectId].AccelerationStd;
if (!double.IsNaN(std))
{
var kalmanFilter = new KalmanFilter2D(3, acceleration, _particles[objectId].AccelerationStd);
foreach (CoordinateHistory coordinateHistory in _particles[objectId].GeneratedCoordinate)
{
kalmanFilter.Push(coordinateHistory.Coordinate.X, coordinateHistory.Coordinate.Z);
}
result.X = (result.X + kalmanFilter.X) * .5;
result.Z = (result.Z + kalmanFilter.Y) * .5;
}
}
_particles[objectId].AddHistoric(result, now);
return(result);
}
private List <Particle3D> CreateParticles(int objectId, IDistance[] distances, out bool doesExist)
{
double minH = double.MaxValue;
double maxH = 0;
double maxW = 0;
double minW = double.MaxValue;
double minL = double.MaxValue;
double maxL = 0;
foreach (IDistance distance in distances)
{
IAnchor anchor = _anchors[distance.FromAnchorId];
if (maxH < anchor.Y + distance.Distance)
{
maxH = anchor.Y + distance.Distance;
}
if (minH > anchor.Y - distance.Distance)
{
minH = anchor.Y - distance.Distance;
}
if (maxW < anchor.X + distance.Distance)
{
maxW = anchor.X + distance.Distance;
}
if (minW > anchor.X - distance.Distance)
{
minW = anchor.X - distance.Distance;
}
if (maxL < anchor.Z + distance.Distance)
{
maxL = anchor.Z + distance.Distance;
}
if (minL > anchor.Z - distance.Distance)
{
minL = anchor.Z - distance.Distance;
}
}
maxH += 50;
minH -= 50;
maxW += 50;
minW -= 50;
maxL += 50;
minL -= 50;
var worldSizeInfo = new WorldSizeInfo();
worldSizeInfo.WorldSizeWidthMin = minW;
worldSizeInfo.WorldSizeWidthMax = maxW;
worldSizeInfo.WorldSizeHeightMin = 50;
worldSizeInfo.WorldSizeHeightMax = maxH;
worldSizeInfo.WorldSizeLengthMax = maxL;
worldSizeInfo.WorldSizeLengthMin = minL;
List <Particle3D> particles;
doesExist = false;
lock (_particles)
{
if (_particles.ContainsKey(objectId))
{
particles = _particles[objectId].Particles;
doesExist = true;
}
else
{
particles = new List <Particle3D>(_numberOfParticles);
for (int i = 0; i < _numberOfParticles; i++)
{
double x = Random.Next((int)worldSizeInfo.WorldSizeWidthMin,
(int)worldSizeInfo.WorldSizeWidthMax);
double y = Random.Next((int)worldSizeInfo.WorldSizeHeightMin,
(int)worldSizeInfo.WorldSizeHeightMax);
double z = Random.Next((int)worldSizeInfo.WorldSizeLengthMin,
(int)worldSizeInfo.WorldSizeLengthMax);
var particle = new Particle3D(x, y, z);
particle.SetNoise(_numberOfCircleDistances);
particles.Add(particle);
}
_particles.TryAdd(objectId, new ParticleHistory3D(10)
{
Particles = particles,
});
}
}
return(particles);
}