/// <summary>
/// 获取指定方向移动的最佳距离
/// </summary>
/// <param name="sdf"></param>
/// <param name="pos">起始位置</param>
/// <param name="dir">方向</param>
/// <param name="speed">速度</param>
/// <param name="radius">碰撞半径</param>
/// <returns></returns>
public static TSVector2 GetVaildPositionBySDF(this SDFRawData sdf, TSVector2 pos, TSVector2 dir, FP speed, FP radius)
{
TSVector2 newPos = pos + dir * speed;
FP sd = Sample(sdf, newPos);
//距离障碍物太近，不可行走
if (sd < radius)
{
TSVector2 gradient = Gradient(sdf, newPos);
TSVector2 asjustDir = dir - gradient * TSVector2.Dot(gradient, dir);
newPos = pos + asjustDir.normalized * speed;
//多次迭代
for (int i = 0; i < 3; ++i)
{
sd = Sample(sdf, newPos);
if (sd >= radius)
{
break;
}
newPos += Gradient(sdf, newPos) * (radius - sd);
}
//避免往返
if (TSVector2.Dot(newPos - pos, dir) < FP.Zero)
{
newPos = pos;
}
}
return(newPos);
}
public static SDFRawData SceneToSDF(GameObject go, Vector3 validPos, int layerMask = -1)
{
float cellSize = 0.25f;
float sdfScale = 0.25f;
var bounds = GetBoundsFromGameObject(go);
bounds = AlignBounds(bounds);
var voxel = Creat(bounds, cellSize);
AddGameObject(voxel, go, layerMask);
var heightfield = VoxelToHeightfield(voxel);
var grid = HeightfieldToPlaneGrid(heightfield, 0, 2);
Vector2Int size = new Vector2Int(voxel.Size.x, voxel.Size.z);
var pos = Vector3Int.CeilToInt((validPos - voxel.Bounds.min) / voxel.CellSize);
grid.Mask = CalcClosePoint(grid.Mask, size, new Vector2Int(pos.x, pos.z));
grid = FilterEdge(grid);
//生成sdf信息
sbyte[] data = new sbyte[grid.Width * grid.Length];
SDFGenerate.Gen(grid.Mask, grid.Width, grid.Length, cellSize, sdfScale, data);
SDFRawData sdf = new SDFRawData();
TSVector2 orign = new TSVector2(grid.OriginPoint.x, grid.OriginPoint.y);
sdf.Init(grid.Width, grid.Length, cellSize, sdfScale, orign, data);
return(sdf);
}
public static RectInt ToRect(this SDFRawData sdf, TSVector2 start, TSVector2 end)
{
TSVector2 min = new TSVector2(TSMath.Min(start.x, end.x), TSMath.Min(start.y, end.y));
TSVector2 max = new TSVector2(TSMath.Max(start.x, end.x), TSMath.Max(start.y, end.y));
return(new RectInt(sdf.FloorToGrid(min), sdf.CeilingToGrid(max)));
}
//求位置的梯度方向
public static TSVector2 Gradient(this SDFRawData sdf, TSVector2 pos)
{
FP delat = FP.ONE;
FP x = Sample(sdf, pos.x + delat, pos.y) - Sample(sdf, pos.x - delat, pos.y);
FP y = Sample(sdf, pos.x, pos.y + delat) - Sample(sdf, pos.x, pos.y - delat);
return(new TSVector2(FP.Half * x, FP.Half * y));
}
public static Texture2D SDFToTexture(SDFRawData sdf)
{
Texture2D texture = new Texture2D(sdf.Width, sdf.Heigh);
for (int i = 0; i < texture.width; ++i)
{
for (int j = 0; j < texture.height; ++j)
{
texture.SetPixel(i, j, sdf[i, j] < 0 ? Color.black : Color.white);
}
}
return(texture);
}
public static void SDFRenderToTexture(SDFRawData sdf, Texture2D texture)
{
float xScale = (sdf.Width / (float)texture.width) * (float)sdf.Grain;
float yScale = (sdf.Heigh / (float)texture.height) * (float)sdf.Grain;
for (int i = 0; i < texture.width; ++i)
{
for (int j = 0; j < texture.height; ++j)
{
FP sd = sdf.Sample(new TSVector2(i * xScale, j * yScale));
texture.SetPixel(i, j, sd < 0 ? Color.black : Color.white);
}
}
}
public static Texture2D ToGrayTexture(SDFRawData sdf)
{
Texture2D texture = new Texture2D(sdf.Width, sdf.Height);
for (int i = 0; i < sdf.Width; ++i)
{
for (int j = 0; j < sdf.Height; ++j)
{
float val = ((float)sdf[i, j] - short.MinValue) / 65536;
Color color = new Color(val, val, val, 1);
texture.SetPixel(i, j, color);
}
}
return(texture);
}
/// <summary>
/// SDF 采样
/// </summary>
/// <param name="sdf">sdf数据</param>
/// <param name="pos">相对于sdf原点的位置</param>
/// <returns>采样后对应位置的SD值</returns>
public static FP Sample(this SDFRawData sdf, TSVector2 pos)
{
pos /= sdf.Grain;
int x = (int)FP.Floor(pos.x);
int y = (int)FP.Floor(pos.y);
int idx = x + y * sdf.Width;
FP rx = pos.x - x;
FP ry = pos.y - y;
//2 3
//0 1
FP v0 = sdf[idx];
FP v1 = sdf[idx + 1];
FP v2 = sdf[idx + sdf.Width];
FP v3 = sdf[idx + sdf.Width + 1];
return((v0 * (1 - rx) + v1 * rx) * (1 - ry) + (v2 * (1 - rx) + v3 * rx) * ry);
}
//圆盘投射，获取最远可移动的位置
public static TSVector2 DiskCast(this SDFRawData sdf, TSVector2 origin, TSVector2 dir, FP radius, FP maxDistance)
{
FP t = FP.Zero;
while (true)
{
TSVector2 p = origin + dir * t;
FP sd = Sample(sdf, p);
if (sd <= radius)
{
return(p);
}
t += (sd - radius);
if (t >= maxDistance)
{
return(origin + dir * maxDistance);
}
}
}
public SDFMap(SDFRawData sdf)
{
SDF = sdf;
}
public static TSVector2 WorldToLocal(this SDFRawData sdf, TSVector2 pos)
{
return(pos - sdf.Origin);
}
public static FP Sample(this SDFRawData sdf, FP posX, FP posY)
{
return(Sample(sdf, new TSVector2(posX, posY)));
}
public static Vector2Int CeilingToGrid(this SDFRawData sdf, TSVector2 positin)
{
positin /= sdf.Grain;
return(new Vector2Int((int)FP.Ceiling(positin.x), (int)FP.Ceiling(positin.y)));
}
