internal static FP Pow2(FP x)
{
if (x.RawValue == 0)
{
return(FP.One);
}
// Avoid negative arguments by exploiting that exp(-x) = 1/exp(x).
bool neg = x.RawValue < 0;
if (neg)
{
x = -x;
}
// static readonly FP Log2Max = new FP(LOG2MAX);
// static readonly FP Log2Min = new FP(LOG2MIN);
if (x == FP.One)
{
return(neg ? FP.Half : 2);
}
if (x >= LOG2MAX)
{
return(neg ? FP.One / FP.MaxValue : FP.MaxValue);
}
if (x <= LOG2MIN)
{
return(neg ? FP.MaxValue : FP.Zero);
}
/* The algorithm is based on the power series for exp(x):
* http://en.wikipedia.org/wiki/Exponential_function#Formal_definition
*
* From term n, we get term n+1 by multiplying with x/n.
* When the sum term drops to zero, we can stop summing.
*/
int integerPart = TSMath.Floor(x).AsInt();
// Take fractional part of exponent
x._serializedValue = x._serializedValue & 0x00000000FFFFFFFF;
var result = FP.One;
var term = FP.One;
int i = 1;
while (term._serializedValue != 0)
{
term = FastMul(FastMul(x, term), LN2) / i;
result += term;
i++;
}
result._serializedValue = (result._serializedValue << integerPart);
if (neg)
{
result = FP.One / result;
}
return(result);
}
public bool GetGridCoord(TSVector position, ref IInt2 coord)
{
TSVector vec = GetGridFloatCoord(position);
int ix = TSMath.Floor(vec.x).AsInt();
int iz = TSMath.Floor(vec.z).AsInt();
int x = CustomMath.Clamp(ix, 0, _mapSizeX - 1);
int z = CustomMath.Clamp(iz, 0, _mapSizeZ - 1);
//TSVector2 coord;
coord.x = x;
coord.y = z;
return(x == ix && z == iz);
}
internal TSVector steeringBehaviourFlowField(IAgentBehaviour agent)
{
//bilinear interpolation
TSVector vec = _gridMap.GetGridFloatCoord(agent.position);
IInt2 floor = IInt2.zero;// GetNearestGridCoordWithoutClamp(agent.position);
floor.x = TSMath.Floor(vec.x).AsInt();
floor.y = TSMath.Floor(vec.z).AsInt();
bool bIn1 = IsValid(floor.x, floor.y);
TSVector f00 = (bIn1) ? GetFlowField(_gridMap.ClampX(floor.x), _gridMap.ClampZ(floor.y)) : TSVector.zero;//(posOrigin - pos1) ;// TSVector.zero
int offsetX = f00.x > 0 ? 1 : -1;
int offsetY = f00.z > 0 ? 1 : -1;
bool bIn2 = IsValid(floor.x, floor.y + offsetY);
bool bIn3 = IsValid(floor.x + offsetX, floor.y);
bool bIn4 = IsValid(floor.x + offsetX, floor.y + offsetY);
// bool bAllIn = bIn1 && bIn2 && bIn3 && bIn4;//bAllIn ||!
//
//TSVector posOrigin = (TSVector)vec;
//TSVector pos1 = TSVector.zero;
//pos1.Set((float)floor.x, 0, floor.y);
//TSVector pos2 = TSVector.zero;
//pos2.Set((float)floor.x, 0, floor.y + 1);
//TSVector pos3 = TSVector.zero;
//pos3.Set((float)floor.x + 1, 0, floor.y);
//TSVector pos4 = TSVector.zero;
//pos3.Set((float)floor.x + 1, 0, floor.y + 1);
//TSVector f00 = (bAllIn) ? _flowField[ClampX(floor.x), ClampZ(floor.y)] : (bIn1?TSVector.zero: ((bIn2&& bIn3&& bIn4) ? TSVector._northEst:(bIn2?TSVector._forward:(bIn3?TSVector._right:TSVector.zero))));
//TSVector f01 = (bAllIn) ? _flowField[ClampX(floor.x), ClampZ(floor.y + 1)] : (bIn2 ? TSVector.zero : ((bIn1 && bIn3 && bIn4) ? TSVector._southEst : (bIn1 ? -TSVector._forward : (bIn4 ? TSVector._right : TSVector.zero))));
//TSVector f10 = (bAllIn) ? _flowField[ClampX(floor.x + 1), ClampZ(floor.y)] : (bIn3 ? TSVector.zero : ((bIn2 && bIn1 && bIn4) ? TSVector._northWest : (bIn4 ? TSVector._forward : (bIn1 ? -TSVector._right : TSVector.zero))));
//TSVector f11 = (bAllIn) ? _flowField[ClampX(floor.x + 1), ClampZ(floor.y + 1)] : (bIn4 ? TSVector.zero : ((bIn2 && bIn3 && bIn1) ? TSVector._southWest : (bIn3 ? -TSVector._forward : (bIn2 ? -TSVector._right : TSVector.zero))));
TSVector f01 = (bIn2) ? GetFlowField(_gridMap.ClampX(floor.x), _gridMap.ClampZ(floor.y + offsetY)) : TSVector.zero; //(posOrigin - pos2);
TSVector f10 = (bIn3) ? GetFlowField(_gridMap.ClampX(floor.x + offsetX), _gridMap.ClampZ(floor.y)) : TSVector.zero; //(posOrigin - pos3);
TSVector f11 = (bIn4) ? GetFlowField(_gridMap.ClampX(floor.x + offsetX), _gridMap.ClampZ(floor.y + offsetY)) : TSVector.zero; // (posOrigin - pos4);
//Do the x interpolations
FP fx = agent.position.x;
FP fy = agent.position.z;
FP w = FP.One * _gridWidth * GridMap.GetNodeSize();
FP h = FP.One * _gridHeight * GridMap.GetNodeSize();
FP dstX0 = TSMath.Abs(vec.x - (floor.x + CustomMath.FPHalf));
FP dstX1 = TSMath.Abs(vec.x - (floor.x + offsetX + CustomMath.FPHalf));
FP dstY0 = TSMath.Abs(vec.z - (floor.y + CustomMath.FPHalf));
FP dstY1 = TSMath.Abs(vec.z - (floor.y + offsetY + CustomMath.FPHalf));
FP xW = (dstX0) / (dstX0 + dstX1);
FP xWeight = (fx < w && fx >= FP.Zero) ? (xW) : FP.Zero;//vec.x - floor.x
TSVector top = f00 * (1 - xWeight) + f10 * (xWeight);
TSVector bottom = f01 * (1 - xWeight) + f11 * xWeight;
FP yW = (dstY0) / (dstY0 + dstY1);
//Do the y interpolation
FP yWeight = (fy < h && fy >= FP.Zero) ? (yW) : FP.Zero;//vec.z - floor.y
//
TSVector desiredDirection = (top * (1 - yWeight) + bottom * yWeight);
//}
// desiredDirection = desiredDirection + Boids.BoidsBehaviourTerrainSeparation(vec,IsValid);
//desiredDirection = desiredDirection.normalized;
// return Boids.SteerTowards(agent, desiredDirection);
return(desiredDirection);
}
