// calculate to next floor
public static double FromNextPeds(Ped ped, Floor nextFloor)
{
double res = 0.0;
var pedVector = ped.CurPos.NewSubtract(ped.GetAtFloor().AddX, ped.GetAtFloor().AddY);
foreach (var nextPed in nextFloor.Peds)
{
if (InWhichBlock(nextPed.CurPos, nextFloor) == Block.SecondStair)
{
var nextVector = nextPed.CurPos.NewSubtract(nextFloor.AddX, nextFloor.AddY);
res += CalculateFromPed(nextVector, pedVector);
}
}
return(res);
}
// enhance judge
public static bool CanMove(Ped ped, Vector target, Space space)
{
var atFloor = ped.GetAtFloor();
// ped's curBlock
var curBlock = InWhichBlock(ped.CurPos, atFloor);
foreach (var p in atFloor.Peds)
{
if (p.Equals(ped))
{
continue;
}
if (curBlock == Block.FirstInterval || curBlock == Block.ThirdCorner)
{
var pedBlock = InWhichBlock(p.CurPos, atFloor);
// the ped in second stair should not be calculated
if (pedBlock == Block.SecondStair)
{
continue;
}
}
if (target.DistanceTo(p.CurPos) < 2 * kR)
{
return(false);
}
}
foreach (var wall in atFloor.Walls)
{
if (wall.Green)
{
continue;
}
if (wall.InWall(target, kR / 2.0))
{
if (wall.DistanceTo(target) < kR)
{
return(false);
}
}
}
if ((curBlock == Block.ThirdCorner || curBlock == Block.FirstInterval) && atFloor.Number > 0)
{
var nextFloor = space.Floors[atFloor.Number - 1];
var vector = target.NewSubtract(atFloor.AddX, atFloor.AddY);
// var ped in next floor
foreach (var nextPed in nextFloor.Peds)
{
if (InWhichBlock(nextPed.CurPos, nextFloor) == Block.SecondStair)
{
var pedVector = nextPed.CurPos.NewSubtract(nextFloor.AddX, nextFloor.AddY);
if (pedVector.DistanceTo(vector) < 2 * kR)
{
return(false);
}
}
}
}
return(true);
}
// calculate the floor field
public static double GetSff(Ped ped, Vector target)
{
var atFloor = ped.GetAtFloor();
var curBlock = InWhichBlock(ped.CurPos, atFloor);
var targetBlock = InWhichBlock(target, atFloor);
if (targetBlock == Block.OutOfSize)
{
return(Double.MaxValue);
}
var vector = target.NewSubtract(atFloor.AddX, atFloor.AddY);
if (atFloor.Start)
{
switch (targetBlock)
{
case Block.StartBlock:
// by curBlock to judge
if (curBlock != Block.StartBlock)
{
return(Double.MaxValue);
}
return(-kCornerHeight * kSffScale + kCornerVal - vector.X * kSffScale);
case Block.FirstCorner:
if (curBlock != Block.StartBlock && curBlock != Block.FirstCorner)
{
return(Double.MaxValue);
}
// if in the first corner, max value and min value
// min value -kCornerHeight * kSffScale
// max value -kCornerHeight * kSffScale + kCornerVal
var tanX = (kCornerHeight - vector.Y) / (vector.X);
return(GetCornerSff(-kCornerHeight * kSffScale + kCornerVal, -kCornerHeight * kSffScale, tanX));
case Block.FirstStair:
if (curBlock != Block.FirstStair && curBlock != Block.FirstCorner)
{
return(Double.MaxValue);
}
return(-vector.Y * kSffScale);
case Block.SecondCorner:
if (curBlock != Block.SecondCorner && curBlock != Block.FirstStair)
{
return(Double.MaxValue);
}
// second corner in this target
tanX = (kCornerWidth - vector.X) / (double)(vector.Y - kCornerHeight - kStairNum * kStairLength);
return(GetCornerSff(-(kCornerHeight + kStairNum * kStairLength) * kSffScale,
-(kCornerHeight + kStairNum * kStairLength) * kSffScale - kCornerVal, tanX));
case Block.FirstInterval:
if (curBlock != Block.FirstInterval && curBlock != Block.SecondCorner)
{
return(Double.MaxValue);
}
return(-(kCornerHeight + kStairNum * kStairLength) * kSffScale - kCornerVal - (vector.X - kCornerWidth) * kSffScale);
case Block.ThirdCorner:
if (curBlock != Block.ThirdCorner && curBlock != Block.FirstInterval)
{
return(Double.MaxValue);
}
tanX = (vector.Y - kCornerHeight - kStairNum * kStairLength) / (double)(vector.X - kCornerWidth - kIntervalLength);
return(GetCornerSff(-(kCornerHeight + kStairNum * kStairLength) * kSffScale - kCornerVal - kIntervalLength * kSffScale,
-(kCornerHeight + kStairNum * kStairLength) * kSffScale - kCornerVal - kIntervalLength * kSffScale - kCornerVal, tanX));
case Block.SecondStair:
if (curBlock != Block.ThirdCorner && curBlock != Block.FirstInterval)
{
return(Double.MaxValue);
}
return(-(kCornerHeight + kStairNum * kStairLength) * kSffScale -
kCornerVal - kIntervalLength * kSffScale - kCornerVal + (vector.Y - kCornerHeight - kStairNum * kStairLength) * kSffScale);
}
}
else if (atFloor.End)
{
switch (targetBlock)
{
case Block.SecondStair:
if (curBlock != Block.SecondStair)
{
return(Double.MaxValue);
}
return(vector.Y * kSffScale);
case Block.FourthCorner:
if (curBlock != Block.FourthCorner && curBlock != Block.SecondStair)
{
return(Double.MaxValue);
}
var tanX = (2 * kCornerWidth + kIntervalLength - vector.X) / (double)(kCornerHeight - vector.Y);
return(GetCornerSff(kCornerHeight * kSffScale, kCornerHeight * kSffScale - kCornerVal, tanX));
case Block.ExitBlock:
if (curBlock != Block.ExitBlock && curBlock != Block.FourthCorner)
{
return(Double.MaxValue);
}
return(kCornerHeight * kSffScale - kCornerVal - (vector.X - kCornerWidth - kIntervalLength) * kSffScale);
}
}
else
{
switch (targetBlock)
{
// judge by cur's block
case Block.StartBlock:
if (ped.StartFloor != atFloor.Number)
{
return(Double.MaxValue);
}
if (curBlock != Block.StartBlock)
{
return(Double.MaxValue);
}
return(-kCornerHeight * kSffScale + kCornerVal - vector.X * kSffScale);
case Block.FirstCorner:
if (ped.StartFloor == atFloor.Number)
{
if (curBlock != Block.StartBlock && curBlock != Block.FirstCorner)
{
return(Double.MaxValue);
}
var tan = (kCornerHeight - vector.Y) / (vector.X);
return(GetCornerSff(-kCornerHeight * kSffScale + kCornerVal, -kCornerHeight * kSffScale, tan));
}
if (curBlock != Block.SecondInterval && curBlock != Block.FirstCorner)
{
return(Double.MaxValue);
}
var tanX = (kCornerHeight - vector.Y) / (double)(kCornerWidth - vector.X);
return(GetCornerSff(-kCornerHeight * kSffScale + kCornerVal, -kCornerHeight * kSffScale, tanX));
case Block.FirstStair:
if (curBlock != Block.FirstStair && curBlock != Block.FirstCorner && curBlock != Block.SecondInterval)
{
return(Double.MaxValue);
}
if (ped.StartFloor == atFloor.Number)
{
if (curBlock != Block.FirstStair && curBlock != Block.FirstCorner)
{
return(Double.MaxValue);
}
}
return(-vector.Y * kSffScale);
case Block.SecondCorner:
// second corner in this target
if (curBlock != Block.SecondCorner && curBlock != Block.FirstStair)
{
return(Double.MaxValue);
}
tanX = (kCornerWidth - vector.X) / (double)(vector.Y - kCornerHeight - kStairNum * kStairLength);
return(GetCornerSff(-(kCornerHeight + kStairNum * kStairLength) * kSffScale,
-(kCornerHeight + kStairNum * kStairLength) * kSffScale - kCornerVal, tanX));
case Block.FirstInterval:
if (curBlock != Block.SecondCorner && curBlock != Block.FirstInterval)
{
return(Double.MaxValue);
}
if (curBlock != Block.FirstInterval && curBlock != Block.SecondCorner && curBlock != Block.FirstStair)
{
return(Double.MaxValue);
}
return(-(kCornerHeight + kStairNum * kStairLength) * kSffScale - kCornerVal - (vector.X - kCornerWidth) * kSffScale);
case Block.ThirdCorner:
if (curBlock != Block.ThirdCorner && curBlock != Block.FirstInterval)
{
return(Double.MaxValue);
}
tanX = (vector.Y - kCornerHeight - kStairNum * kStairLength) / (double)(vector.X - kCornerWidth - kIntervalLength);
return(GetCornerSff(-(kCornerHeight + kStairNum * kStairLength) * kSffScale - kCornerVal - kIntervalLength * kSffScale,
-(kCornerHeight + kStairNum * kStairLength) * kSffScale - 2 * kCornerVal - kIntervalLength * kSffScale, tanX));
case Block.SecondStair:
if (curBlock == Block.ThirdCorner || curBlock == Block.FirstInterval)
{
return(-(kCornerHeight + kStairNum * kStairLength) * kSffScale -
kCornerVal - kIntervalLength * kSffScale - kCornerVal + (vector.Y - kCornerHeight - kStairNum * kStairLength) * kSffScale);
}
else
{
if (curBlock != Block.SecondStair)
{
return(Double.MaxValue);
}
return(-kCornerHeight * kSffScale + 2 * kCornerVal + kIntervalLength * kSffScale + (vector.Y - kCornerHeight) * kSffScale);
}
case Block.FourthCorner:
if (ped.StartFloor == atFloor.Number)
{
return(Double.MaxValue);
}
if (curBlock != Block.FourthCorner && curBlock != Block.SecondStair)
{
return(Double.MaxValue);
}
tanX = (vector.X - kCornerWidth - kIntervalLength) / (double)(kCornerHeight - vector.Y);
return(GetCornerSff(-kCornerHeight * kSffScale + 2 * kCornerVal + kIntervalLength * kSffScale, -kCornerHeight * kSffScale + kCornerVal + kIntervalLength * kSffScale, tanX));
case Block.SecondInterval:
if (ped.StartFloor == atFloor.Number)
{
return(Double.MaxValue);
}
if (curBlock != Block.SecondInterval && curBlock != Block.FourthCorner)
{
return(Double.MaxValue);
}
return(-kCornerHeight * kSffScale + kCornerVal + (vector.X - kCornerWidth) * kSffScale);
}
}
return(Double.MaxValue);
}