// single step length's move(in stairs)
private void SingleStep(Block curBlock)
{
double resSff = Double.MaxValue;
Vector resTarget = null;
// around find min, 20;
var rotate = 2 * Math.PI / Util.kCircleSp;
var dir = new Vector(1, 0).Rotate(rotate * Util.GetRandom());
for (int i = 0; i < Util.kCircleSp; i++)
{
var target = CurPos.NewAdd(dir.NewMultiply(Util.kStepLen));
// calculate target's
double sff = Util.GetSff(this, target);
var atFloor = space.Floors[AtFloor];
// all sff from ped
foreach (var ped in atFloor.Peds)
{
if (ped.Equals(this))
{
continue;
}
if (curBlock == Block.ThirdCorner || curBlock == Block.FirstInterval)
{
// should not calculate second stair
var pedBlock = Util.InWhichBlock(ped.CurPos, ped.GetAtFloor());
if (pedBlock == Block.SecondStair)
{
continue;
}
}
sff += Util.CalculateFromPed(target, ped);
}
if (curBlock == Block.ThirdCorner || curBlock == Block.FirstInterval)
{
if (AtFloor > 0)
{
var nextFloor = space.Floors[AtFloor - 1];
sff += Util.FromNextPeds(this, nextFloor);
}
}
// calculate all wall's
// Green wall not(sign)
foreach (var wall in atFloor.Walls)
{
if (wall.Green)
{
continue;
}
if (wall.InWall(target, Util.kR / 2.0))
{
sff += Util.CalculateFromWall(target, wall);
}
}
// else set for dir
if (sff < resSff)
{
resSff = sff;
resTarget = target;
}
dir.Rotate(rotate);
}
// bool can move or can't move
if (resTarget != null)
{
if (Util.CanMove(this, resTarget, space))
{
Can = true;
CurPos = resTarget;
}
else
{
Can = false;
}
}
else
{
Can = false;
}
// if can't move
if (!Can && GetAtFloor().Peds.Count < 5)
{
// write to reporter this block and last block
// move, don't calculate for this
if (resTarget != null && curBlock == Block.SecondStair)
{
CurPos = resTarget;
}
}
}
// multi step length's move
private void MultiStep(Block curBlock)
{
// should be split to 3
double resSff = Double.MaxValue;
Vector resTarget = null;
var rotate = 2 * Math.PI / Util.kCircleSp;
var dir = new Vector(1, 0).Rotate(rotate * Util.GetRandom());
for (int i = 0; i < Util.kCircleSp; i++)
{
for (int j = 0; j < Util.kStepSp; j++)
{
var target = CurPos.NewAdd(dir.NewMultiply((j + 1) * Util.kStepLen / Util.kStepSp));
double sff = Util.GetSff(this, target);
var atFloor = GetAtFloor();
// all sff from ped
foreach (var ped in atFloor.Peds)
{
if (ped.Equals(this))
{
continue;
}
// if in coner or interval, don't calculate from the last floor's peds
if (curBlock == Block.ThirdCorner || curBlock == Block.FirstInterval)
{
// should not calculate second stair
var pedBlock = Util.InWhichBlock(ped.CurPos, ped.GetAtFloor());
if (pedBlock == Block.SecondStair)
{
continue;
}
}
sff += Util.CalculateFromPed(target, ped);
}
// if in corner or interval, should calculate next floor's peds
if (curBlock == Block.ThirdCorner || curBlock == Block.FirstInterval)
{
if (AtFloor > 0)
{
var nextFloor = space.Floors[AtFloor - 1];
sff += Util.FromNextPeds(this, nextFloor);
}
}
// calculate all wall's
// Green wall not(sign)
foreach (var wall in atFloor.Walls)
{
if (wall.Green)
{
continue;
}
if (wall.InWall(target, Util.kR / 2.0))
{
sff += Util.CalculateFromWall(target, wall);
}
}
// update result target
if (sff < resSff)
{
resSff = sff;
resTarget = target;
}
}
// dir rotate in i loop, not in j
dir.Rotate(rotate);
}
// the same to move
if (resTarget != null)
{
if (Util.CanMove(this, resTarget, space))
{
CurPos = resTarget;
Can = true;
}
else
{
Can = false;
}
}
else
{
Can = false;
}
if (!Can && GetAtFloor().Peds.Count < 5)
{
// write to reporter this block and last block
// move, don't calculate for this
if (resTarget != null && curBlock == Block.SecondStair)
{
CurPos = resTarget;
}
}
}