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); }