public void AppendMeshConvex(Vector3[] verts, int[] tris, Matrix4x4 matrix, byte area, bool FLIP_Y, Bounds bounds)
{
Vector3 chunkBoundsMin = template.chunkOffsetedBounds.min;
Vector3 meshBoundsLocalMin = bounds.min - chunkBoundsMin;
Vector3 meshBoundsLocalMax = bounds.max - chunkBoundsMin;
float voxelSize = template.voxelSize;
int gridMinX = (int)(meshBoundsLocalMin.x / voxelSize);
int gridMinZ = (int)(meshBoundsLocalMin.z / voxelSize);
int gridMaxX = (int)(meshBoundsLocalMax.x / voxelSize) + 2;
int gridMaxZ = (int)(meshBoundsLocalMax.z / voxelSize) + 2;
if (gridMinX < 0)
{
gridMinX = 0;
}
if (gridMinZ < 0)
{
gridMinZ = 0;
}
if (gridMaxX > sizeX)
{
gridMaxX = sizeX;
}
if (gridMaxZ > sizeZ)
{
gridMaxZ = sizeZ;
}
DataCompact[] compactData = TakeCompactData();
AppendMeshConvexPrivate(compactData, verts, tris, matrix, area, FLIP_Y);
AppendCompactData(compactData, area, gridMinX, gridMaxX, gridMinZ, gridMaxZ);
GenericPoolArray <DataCompact> .ReturnToPool(ref compactData);
}
public void Init(NavMeshTemplateCreation template)
{
this.template = template;
int length = flattenSize;
//!!!!!!!!!!!!!!!!//
freeIndexStack = GenericPoolArray <int> .Take(INITIAL_FREE_INDEX_POOL_SIZE);
arrayData = GenericPoolArray <Data> .Take(length *ARRAY_DATA_SIZE);
for (int i = 0; i < length; i++)
{
arrayData[i].next = -2;
}
for (int i = length; i < arrayData.Length; i++)
{
arrayData[i].next = -1;
}
filledIndexes = length;
freeIndexStackLength = 0;
voxelDistanceThreshold = template.voxelSize;
freeStackHS.Clear();
}
private void AppendCapsule(Vector3 sphereA, Vector3 sphereB, float capsileRadius, bool FLIP_Y, byte area)
{
DataCompact[] compactData = TakeCompactData();
AppendCapsulePrivate(compactData, sphereA, sphereB, capsileRadius, FLIP_Y, area);
AppendCompactData(compactData, area);
GenericPoolArray <DataCompact> .ReturnToPool(ref compactData);
}
private static Vector2[] MakeElipse(float radiusInner, float radiusOuter, int elipseQuadLength)
{
Vector2[] array = GenericPoolArray <Vector2> .Take((elipseQuadLength * 4) + 1);
float radiusDifference = radiusOuter - radiusInner;
float elipseQuadStep = Mathf.PI / elipseQuadLength * 0.5f;
for (int i = 0; i < elipseQuadLength; i++)
{
float xValue = Mathf.Cos(i * elipseQuadStep);
float yValue = Mathf.Sin(i * elipseQuadStep);
float xInner = xValue * radiusInner;
float yInner = yValue * radiusInner;
float xOuter = xValue * radiusOuter;
float yOuter = yValue * radiusOuter;
float lerp1 = xOuter / radiusOuter;
float lerp2 = yOuter / radiusOuter;
array[i] = new Vector2(xInner + (radiusDifference * lerp1), yInner);
array[elipseQuadLength + i] = new Vector2(-yInner - (radiusDifference * lerp2), xInner);
array[(elipseQuadLength * 2) + i] = new Vector2(-xInner - (radiusDifference * lerp1), -yInner);
array[(elipseQuadLength * 3) + i] = new Vector2(yInner + (radiusDifference * lerp2), -xInner);
}
array[array.Length - 1] = array[0];
return(array);
}
public void ChangeArea(List <ShapeDataAbstract> shapes, Area area)
{
byte areaValue = GetAreaValue(area);
for (int i = 0; i < shapes.Count; i++)
{
DataCompact[] compactData = TakeCompactData();
var shape = shapes[i];
if (shape is ShapeDataSphere)
{
ShapeDataSphere castedShape = shape as ShapeDataSphere;
//not walkable cause in this case we only intrested in min and max and this method ignores it
AppendSpherePrivate(compactData, castedShape.bounds.center, castedShape.bounds.extents.x, 0f, false, false);
}
else if (shape is ShapeDataCapsule)
{
ShapeDataCapsule castedShape = shape as ShapeDataCapsule;
AppendCapsulePrivate(compactData, castedShape.sphereA, castedShape.sphereB, castedShape.capsileRadius, false, areaValue);
}
else if (shape is ShapeDataBox)
{
ShapeDataBox castedShape = shape as ShapeDataBox;
AppendMeshConvexPrivate(compactData, ColliderCollector.cubeVerts, ColliderCollector.cubeTris, castedShape.boxMatrix, areaValue, false);
}
else
{
Debug.LogError("no support for current area modifyer in shape collector");
}
for (int x = 0; x < sizeX; x++)
{
for (int z = 0; z < sizeZ; z++)
{
int index = GetIndex(x, z);
DataCompact data = compactData[index];
if (data.pass == -1 || arrayData[index].next == -2)
{
continue;
}
for (; index != -1; index = arrayData[index].next)
{
var arrData = arrayData[index];
if (arrData.max >= data.min & arrData.max <= data.max)
{
arrayData[index].area = areaValue;
}
}
}
}
GenericPoolArray <DataCompact> .ReturnToPool(ref compactData);
}
}
public static void ReturnToPool(ref ShapeCollector shape)
{
GenericPoolArray <Data> .ReturnToPool(ref shape.arrayData);
GenericPoolArray <int> .ReturnToPool(ref shape.freeIndexStack);
shape.template = null;
lock (poolDictionary) {
poolDictionary[shape.size].Push(shape);
}
shape = null;
}
public LayerInfoHolder(int Length)
{
isValid = true;
mask = GenericPoolArray <bool> .Take(Length);
indexes = GenericPoolArray <IndexPair> .Take(Length);
indexesCount = 0;
banned = GenericPoolArray <int> .Take(Length);
bannedCount = 0;
}
public DataCompact[] TakeCompactData()
{
int size = flattenSize;
DataCompact[] result = GenericPoolArray <DataCompact> .Take(size);
sbyte initialPass = -1;
for (int i = 0; i < size; i++)
{
result[i].pass = initialPass;
}
return(result);
}
//IS EXAMPLE
private void ReturnFreeIndex(int index)
{
if (freeIndexStack.Length == freeIndexStackLength)
{
//Debug.LogFormat("stack doubled from {0} to {1}\n", freeIndexStack.Length, freeIndexStack.Length * 2);
int[] newFreeIndexStack = GenericPoolArray <int> .Take(freeIndexStack.Length * 2);
Array.Copy(freeIndexStack, newFreeIndexStack, freeIndexStack.Length);
GenericPoolArray <int> .ReturnToPool(ref freeIndexStack);
freeIndexStack = newFreeIndexStack;
}
freeIndexStack[freeIndexStackLength++] = index;
//Debug.LogFormat("ReturnFreeIndex: returned {0}, stack length {1}, Cur stack {2}", index, stackLength, DebugCurFreeStack());
}
public void AppendCharacterControler(ShapeDataCharacterControler character)
{
Bounds bounds = character.bounds;
DataCompact[] compactData = TakeCompactData();
AppendSpherePrivate(
compactData,
bounds.center,
bounds.extents.x,
Mathf.Abs(bounds.extents.y - bounds.extents.x) * 0.5f,
true,
character.area.id != 1);
AppendCompactData(compactData, GetAreaValue(character.area));
GenericPoolArray <DataCompact> .ReturnToPool(ref compactData);
}
public void Release()
{
if (isValid)
{
for (int i = 0; i < indexesCount; i++)
{
mask[indexes[i].maskIndex] = default(bool);
indexes[i] = default(IndexPair);
}
GenericPoolArray <bool> .ReturnToPool(ref mask, false);
GenericPoolArray <IndexPair> .ReturnToPool(ref indexes, false);
for (int i = 0; i < bannedCount; i++)
{
banned[i] = default(int);
}
GenericPoolArray <int> .ReturnToPool(ref banned, false);
isValid = false;
}
}
/// <summary>
/// if area is null then area is retained
/// </summary>
public void MakeHole(List <ShapeDataAbstract> shapes, Area area)
{
bool appyArea = area != null;
byte areaValue = 0;
if (appyArea)
{
areaValue = GetAreaValue(area);
}
for (int i = 0; i < shapes.Count; i++)
{
DataCompact[] compactData = TakeCompactData();
var shape = shapes[i];
if (shape is ShapeDataSphere)
{
ShapeDataSphere castedShape = shape as ShapeDataSphere;
AppendSpherePrivate(compactData, castedShape.bounds.center, castedShape.bounds.extents.x, 0f, false, true);
}
else if (shape is ShapeDataCapsule)
{
ShapeDataCapsule castedShape = shape as ShapeDataCapsule;
AppendCapsulePrivate(compactData, castedShape.sphereA, castedShape.sphereB, castedShape.capsileRadius, true, areaValue);
}
else if (shape is ShapeDataBox)
{
ShapeDataBox castedShape = shape as ShapeDataBox;
AppendMeshConvexPrivate(compactData, ColliderCollector.cubeVerts, ColliderCollector.cubeTris, castedShape.boxMatrix, areaValue, true);
//Debug.LogWarning("dont forget to flip Y in ShapeDataBox");
}
else
{
Debug.LogError("no support for current area modifyer in shape collector");
}
Vector3 realChunkPos = template.realOffsetedPosition;
Vector3 offset = template.halfVoxelOffset;
for (int x = 0; x < sizeX; x++)
{
for (int z = 0; z < sizeZ; z++)
{
int index = GetIndex(x, z);
DataCompact mask = compactData[index];
if (mask.pass != -1 && arrayData[index].next != -2)
{
int prevIndex = -1;
int curIndex = index;
while (true)
{
if (curIndex < 0)
{
break;
}
Data curNode = arrayData[curIndex];
if ((mask.min > curNode.max | mask.max < curNode.min) == false) //if current mask in not higher or lower than current node
{
if (SomeMath.InRangeExclusive(mask.min, curNode.min, curNode.max))
{
arrayData[curIndex].max = mask.min;
arrayData[curIndex].pass = mask.pass;
if (appyArea)
{
arrayData[curIndex].area = areaValue;
}
if (SomeMath.InRangeExclusive(mask.max, curNode.min, curNode.max))
{
int freeIndex = GetFreeIndex();
arrayData[freeIndex] = new Data(mask.max, curNode.max, curNode.pass, curNode.next, curNode.area);
arrayData[curIndex].next = freeIndex;
break;
}
prevIndex = curIndex;
curIndex = arrayData[curIndex].next;
}
else if (SomeMath.InRangeExclusive(mask.max, curNode.min, curNode.max))
{
//top of mask inside current shape
arrayData[curIndex].min = mask.max;
break;//this nothing can be intersected after that
}
else
{
if (curNode.next == -1) //if there no nodes after that
{
if (prevIndex == -1) //if it first node
{
arrayData[curIndex].next = -2; //make it invalid
}
else
{
arrayData[prevIndex].next = -1;
}
break;
}
//if some nodes after that
else
{
//shift next thata to current index
//NOTE: curIndex is not changed to check new data at this index
ReturnFreeIndex(curNode.next);
arrayData[curIndex] = arrayData[curNode.next];
continue;
}
}
}
prevIndex = curIndex;
curIndex = curNode.next;
}
}
}
}
GenericPoolArray <DataCompact> .ReturnToPool(ref compactData);
}
}
public void AppendCompactData(DataCompact[] value, byte compactDataArea, int startX, int endX, int startZ, int endZ)
{
for (int z = startZ; z < endZ; z++)
{
for (int x = startX; x < endX; x++)
{
int curIndex = (z * sizeX) + x;
var val = value[curIndex];
if (val.pass != -1)
{
//SetVoxel(x, z, val.min, val.max, val.pass, compactDataArea);
//copy pasted code from set voxel to reduce overhead
Data curNode = arrayData[curIndex];
byte area = compactDataArea;
if (curNode.next == -2)
{
arrayData[curIndex] = new Data(val.min, val.max, val.pass, -1, area);
}
else
{
bool isApplyed = false;
int prevIndex = -1;
while (true)
{
curNode = arrayData[curIndex];
if (curNode.min > val.max)
{
if (isApplyed)
{
break;
}
int freeIndex = freeIndexStackLength > 0 ? freeIndexStack[--freeIndexStackLength] : filledIndexes++;
if (freeIndex == arrayData.Length)
{
Data[] newArrayData = GenericPoolArray <Data> .Take(arrayData.Length * 2);
Array.Copy(arrayData, newArrayData, arrayData.Length);
GenericPoolArray <Data> .ReturnToPool(ref arrayData);
arrayData = newArrayData;
}
arrayData[freeIndex] = curNode;
arrayData[curIndex] = new Data(val.min, val.max, val.pass, freeIndex, area);
break;
}
if (curNode.max < val.min)
{
if (curNode.next == -1)
{
int freeIndex = freeIndexStackLength > 0 ? freeIndexStack[--freeIndexStackLength] : filledIndexes++;
if (freeIndex == arrayData.Length)
{
Data[] newArrayData = GenericPoolArray <Data> .Take(arrayData.Length * 2);
Array.Copy(arrayData, newArrayData, arrayData.Length);
GenericPoolArray <Data> .ReturnToPool(ref arrayData);
arrayData = newArrayData;
}
arrayData[curIndex].next = freeIndex;
arrayData[freeIndex] = new Data(val.min, val.max, val.pass, -1, area);
break;
}
else
{
curIndex = curNode.next;
continue;
}
}
if (curNode.min < val.min)
{
val.min = curNode.min;
}
if (curNode.max > val.max)
{
val.max = curNode.max;
}
if (Math.Abs(val.max - curNode.max) <= voxelDistanceThreshold)
{
val.pass = Math.Max(val.pass, curNode.pass);
area = curNode.area;
}
if (prevIndex != -1 && arrayData[prevIndex].Intersect(val.min, val.max))
{
if (freeIndexStack.Length == freeIndexStackLength)
{
int[] newFreeIndexStack = GenericPoolArray <int> .Take(freeIndexStack.Length * 2);
Array.Copy(freeIndexStack, newFreeIndexStack, freeIndexStack.Length);
GenericPoolArray <int> .ReturnToPool(ref freeIndexStack);
freeIndexStack = newFreeIndexStack;
}
freeIndexStack[freeIndexStackLength++] = curIndex;
arrayData[prevIndex] = new Data(val.min, val.max, val.pass, curNode.next, area);
}
else
{
arrayData[curIndex] = new Data(val.min, val.max, val.pass, curNode.next, area);
prevIndex = curIndex;
}
isApplyed = true;
curIndex = curNode.next;
if (curIndex == -1)
{
break;
}
}
}
}
}
}
}
//const int targetIndex = 9700;
//int debugAdditions = 0;
public void SetVoxel(int x, int z, float min, float max, sbyte pass, byte area)
{
//work++;
int curIndex = (z * sizeX) + x;
Data curNode = arrayData[curIndex];
//bool doDebug = curIndex == targetIndex;
//if (doDebug) {
// debugAdditions++;
// Debug.LogFormat("{3} Set Voxel {0} min {1} max {2}", curIndex, min, max, debugAdditions - 1);
// Vector3 vMin = GetPos(x + debugAdditions - 1, z, min);
// Vector3 vMax = GetPos(x + debugAdditions - 1, z, max);
// Debuger_K.AddDot(vMin, Color.black, 0.005f);
// Debuger_K.AddDot(vMax, Color.white, 0.005f);
// Debuger_K.AddLine(vMin, vMax, Color.white);
// Debuger_K.AddLabel(SomeMath.MidPoint(vMin, vMax), debugAdditions - 1);
//}
if (curNode.next == -2)
{
//if (doDebug) Debug.LogFormat("{0} next == -2", debugAdditions - 1);
arrayData[curIndex] = new Data(min, max, pass, -1, area);
}
else
{
bool isApplyed = false;
int prevIndex = -1;
while (true)
{
curNode = arrayData[curIndex];
//if (doDebug) Debug.LogFormat("{0} Iteration", debugAdditions - 1);
if (curNode.min > max)
{
//if (doDebug) Debug.LogFormat("{1} curNode.min > max. isApplyed: {0}", isApplyed, debugAdditions - 1);
if (isApplyed)
{
break;
}
//int freeIndex = GetFreeIndex();
int freeIndex = freeIndexStackLength > 0 ? freeIndexStack[--freeIndexStackLength] : filledIndexes++;
if (freeIndex == arrayData.Length)
{
Data[] newArrayData = GenericPoolArray <Data> .Take(arrayData.Length * 2);
Array.Copy(arrayData, newArrayData, arrayData.Length);
GenericPoolArray <Data> .ReturnToPool(ref arrayData);
arrayData = newArrayData;
}
arrayData[freeIndex] = curNode;
arrayData[curIndex] = new Data(min, max, pass, freeIndex, area);
//if (doDebug) Debug.LogFormat("{1} Setted up at {0}", freeIndex, debugAdditions - 1);
break;
}
if (curNode.max < min)
{
//if (doDebug) Debug.LogFormat("{0} curNode.max < min", debugAdditions - 1);
//current node are below current data
if (curNode.next == -1) //no data next just add node
//if (doDebug) Debug.LogFormat("{0} no data next just add node", debugAdditions - 1);
//int freeIndex = GetFreeIndex();
{
int freeIndex = freeIndexStackLength > 0 ? freeIndexStack[--freeIndexStackLength] : filledIndexes++;
if (freeIndex == arrayData.Length)
{
Data[] newArrayData = GenericPoolArray <Data> .Take(arrayData.Length * 2);
Array.Copy(arrayData, newArrayData, arrayData.Length);
GenericPoolArray <Data> .ReturnToPool(ref arrayData);
arrayData = newArrayData;
}
arrayData[curIndex].next = freeIndex;
arrayData[freeIndex] = new Data(min, max, pass, -1, area);
break;
}
else //there is some data next check it
//if (doDebug) Debug.LogFormat("{0} there is some data next check it", debugAdditions - 1);
{
curIndex = curNode.next;
continue;
}
}
if (curNode.min < min)
{
min = curNode.min;
}
if (curNode.max > max)
{
max = curNode.max;
}
//if (doDebug) Debug.LogFormat("{0} Overlaping. new min {1} max {2}", debugAdditions - 1, min, max);
if (Math.Abs(max - curNode.max) <= voxelDistanceThreshold)
{
pass = Math.Max(pass, curNode.pass);
area = curNode.area; //here actualy should be priority
}
if (prevIndex != -1 && arrayData[prevIndex].Intersect(min, max))
{
//set data to previous index
//if (doDebug) Debug.LogFormat("{0} set data to PREVIOUS ({1}) index", debugAdditions - 1, prevIndex);
//ReturnFreeIndex(curIndex);
if (freeIndexStack.Length == freeIndexStackLength)
{
int[] newFreeIndexStack = GenericPoolArray <int> .Take(freeIndexStack.Length * 2);
Array.Copy(freeIndexStack, newFreeIndexStack, freeIndexStack.Length);
GenericPoolArray <int> .ReturnToPool(ref freeIndexStack);
freeIndexStack = newFreeIndexStack;
}
freeIndexStack[freeIndexStackLength++] = curIndex;
arrayData[prevIndex] = new Data(min, max, pass, curNode.next, area);
}
else
{
//if (doDebug) Debug.LogFormat("{0} set data to CURRENT ({1}) index", debugAdditions - 1, curIndex);
arrayData[curIndex] = new Data(min, max, pass, curNode.next, area);
prevIndex = curIndex;
}
isApplyed = true;
curIndex = curNode.next;
if (curIndex == -1)
{
break;
}
}
}
}
public void AppendMeshConvexPrivate(DataCompact[] compactData, Vector3[] verts, int[] tris, Matrix4x4 matrix, byte area, bool FLIP_Y)
{
float voxelSize = template.voxelSize;
int startX_extra = template.startX_extra;
int endX_extra = template.endX_extra;
int startZ_extra = template.startZ_extra;
int endZ_extra = template.endZ_extra;
ShapeDataHelperTriangleRasterization triangleRasterizator = GenericPool <ShapeDataHelperTriangleRasterization> .Take();
Vector3[] tempVerts = GenericPoolArray <Vector3> .Take(verts.Length);
for (int i = 0; i < verts.Length; i++)
{
tempVerts[i] = matrix.MultiplyPoint3x4(verts[i]);
}
//rasterization
float maxSlopeCos = Mathf.Cos(template.maxSlope * Mathf.PI / 180f);
for (int t = 0; t < tris.Length; t += 3)
{
Vector3 A = tempVerts[tris[t]];
Vector3 B = tempVerts[tris[t + 1]];
Vector3 C = tempVerts[tris[t + 2]];
sbyte passability = -1;
if (area == 1)//id of clear Area all time
{
passability = (sbyte)Passability.Unwalkable;
}
else if (CalculateWalk(A, B, C, maxSlopeCos, FLIP_Y))
{
passability = (sbyte)Passability.Walkable;
}
else
{
passability = (sbyte)Passability.Slope;
}
//float crossY = Vector3.Cross(B - A, C - A).normalized.y;
//if (FLIP_Y)
// crossY *= -1;
// if (crossY > 0) {
// bool unwalkableBySlope = !(crossY >= maxSlopeCos);
// if (area == 1)//id of clear Area all time
// passability = (sbyte)Passability.Unwalkable;
// else if (unwalkableBySlope)
// passability = (sbyte)Passability.Slope;
// else
// passability = (sbyte)Passability.Walkable;
//#if UNITY_EDITOR
// if (!unwalkableBySlope && Debuger_K.doDebug && Debuger_K.debugOnlyNavMesh == false)
// Debuger_K.AddWalkablePolygon(template.gridPosX, template.gridPosZ, template.properties, A, B, C);
//#endif
// }
triangleRasterizator.RasterizeTriangle(
A, B, C,
voxelSize,
startX_extra, endX_extra,
startZ_extra, endZ_extra,
compactData,
passability,
sizeX);
}
GenericPool <ShapeDataHelperTriangleRasterization> .ReturnToPool(ref triangleRasterizator);
GenericPoolArray <Vector3> .ReturnToPool(ref tempVerts);
}
private void AppendCapsulePrivate(DataCompact[] compactData, Vector3 sphereA, Vector3 sphereB, float capsileRadius, bool FLIP_Y, byte area)
{
bool IS_WALKABLE = area != 1;
float voxelSize = template.voxelSize;
float voxelSizeHalf = voxelSize * 0.5f;
//if sphere is on top
if ((int)(sphereA.x / voxelSize) == (int)(sphereB.x / voxelSize) & (int)(sphereA.z / voxelSize) == (int)(sphereB.z / voxelSize))
{
AppendSpherePrivate(compactData,
SomeMath.MidPoint(sphereA, sphereB),
capsileRadius,
Mathf.Abs(sphereA.y - sphereB.y) * 0.5f,
true,
IS_WALKABLE);
return;
}
#region values setup
Vector3 AB = sphereB - sphereA;
Vector3 AB_normalized = AB.normalized;
Vector2 sphereA_v2 = new Vector2(sphereA.x, sphereA.z);
Vector2 sphereB_v2 = new Vector2(sphereB.x, sphereB.z);
Vector2 AB_v2 = sphereB_v2 - sphereA_v2;
float alighmentAxis = Vector2.Angle(AB_v2, new Vector2(0, 1));
Vector3 axisPlaneNormal;
C_Axis alighment;
if (alighmentAxis >= 45 & alighmentAxis <= 135)
{
axisPlaneNormal = new Vector3(1, 0, 0);
alighment = C_Axis.x;
}
else
{
axisPlaneNormal = new Vector3(0, 0, 1);
alighment = C_Axis.z;
}
Vector3 v3 = Math3d.ProjectVectorOnPlane(axisPlaneNormal, AB);
Vector3 v3normalized = v3.normalized;
float angle = Vector3.Angle(AB, v3);
float outerRadius = capsileRadius / Mathf.Sin(angle * Mathf.Deg2Rad);
//float radiusDifference = outerRadius - capsileRadius;
Quaternion q = new Quaternion();
switch (alighment)
{
case C_Axis.x:
q = Quaternion.Euler(0, 0, Mathf.Atan2(v3.z, v3.y) * Mathf.Rad2Deg);
break;
case C_Axis.z:
q = Quaternion.Euler(0, 0, Mathf.Atan2(v3.y, v3.x) * Mathf.Rad2Deg);
break;
}
Bounds2DInt volumeBoundsSphereA = GetVolumeBounds(sphereA, capsileRadius, template);
Bounds2DInt volumeBoundsSphereB = GetVolumeBounds(sphereB, capsileRadius, template);
Bounds2DInt volumeBoundsCombined = Bounds2DInt.GetIncluded(volumeBoundsSphereA, volumeBoundsSphereB);
int startX = volumeBoundsCombined.minX + template.startX_extra;
int startZ = volumeBoundsCombined.minY + template.startZ_extra;
int endX = volumeBoundsCombined.maxX + template.startX_extra;
int endZ = volumeBoundsCombined.maxY + template.startZ_extra;
#endregion
//generating elipse
#region elipse generation
Vector2[] generatedElipse = MakeElipse(capsileRadius, outerRadius, 6);
for (int i = 0; i < generatedElipse.Length; i++)
{
generatedElipse[i] = q * generatedElipse[i];
}
//generating ordered lines
List <ElipseLine> elipseLines = new List <ElipseLine>();
for (int i = 0; i < generatedElipse.Length - 1; i++)
{
Vector2 p1 = generatedElipse[i];
Vector2 p2 = generatedElipse[i + 1];
sbyte pass = -1;
if (IS_WALKABLE)
{
Vector3 p1valid = GetValidVector3(p1, alighment);
Vector3 p2valid = GetValidVector3(p2, alighment);
Vector3 mid = SomeMath.MidPoint(p1valid, p2valid);
Vector3 nearest = SomeMath.NearestPointOnLine(new Vector3(), AB, mid);
Vector3 normal = mid - nearest;
if (FLIP_Y)
{
normal *= -1;
}
float normalAngle = Vector3.Angle(Vector3.up, normal);
if (normal.y >= 0)
{
if (normalAngle <= template.maxSlope)
{
pass = (int)Passability.Walkable;
}
else
{
pass = (int)Passability.Slope;
}
}
}
else
{
pass = (int)Passability.Unwalkable;
}
//get line itself
switch (alighment)
{
case C_Axis.x:
if (p1.y < p2.y)
{
elipseLines.Add(new ElipseLine(p1, p2, pass));
}
else
if (p1.y > p2.y)
{
elipseLines.Add(new ElipseLine(p2, p1, pass));
}
break;
case C_Axis.z:
if (p1.x < p2.x)
{
elipseLines.Add(new ElipseLine(p1, p2, pass));
}
else
if (p1.x > p2.x)
{
elipseLines.Add(new ElipseLine(p2, p1, pass));
}
break;
}
}
GenericPoolArray <Vector2> .ReturnToPool(ref generatedElipse);
#endregion
if (alighment == C_Axis.z)
{
for (int currentZ = startZ; currentZ < endZ; currentZ++)
{
Vector3 intersection = SomeMath.ClipLineToPlaneZ(sphereA, AB_normalized, (currentZ * voxelSize) + voxelSizeHalf);
float targetZ = intersection.z;
for (int i = 0; i < elipseLines.Count; i++)
{
ElipseLine line = elipseLines[i];
float p1x = line.point1x + intersection.x;
float p1y = line.point1y + intersection.y;
float p2x = line.point2x + intersection.x;
sbyte pass = line.passability;
//if (pass != -1)
// pass += 10;
//Vector3 p1 = GetValidVector3(line.point1) + intersection;
//Vector3 p2 = GetValidVector3(line.point2) + intersection;
//Debuger_K.AddLine(p1, p2, Color.blue);
for (int currentX = (int)(p1x / voxelSize) - 1; currentX < (int)(p2x / voxelSize) + 1; currentX++)
{
if (currentX >= startX && currentX < endX)
{
int vx = currentX - template.startX_extra;
int vz = currentZ - template.startZ_extra;
//float actualX;
//switch (intMask[vx][vz]) {
// case 3: actualX = currentX * voxelSize + voxelSizeHalf; break;
// case 4: actualX = currentX * voxelSize; break;
// case 5: actualX = currentX * voxelSize + voxelSize; break;
// default: actualX = currentX * voxelSize; break;
//}
float actualX = currentX * voxelSize + voxelSizeHalf;
float dx = (actualX - p1x) / line.normalizedX;//determinant
//Vector3 px = new Vector3(actualX, p1y + (line.normalizedY * dx), targetZ);
//Debuger_K.AddDot(px, Color.magenta, 0.01f);
//Debuger_K.AddLabelFormat(px, "{0}", dx, line.length);
if (dx >= 0f && dx <= line.length)
{
float targetY = p1y + (line.normalizedY * dx);
//VolumeSetTime.Start();
if (Mathf.Sign(SomeMath.Dot(AB.x, AB.y, AB.z, actualX - sphereA.x, targetY - sphereA.y, targetZ - sphereA.z)) !=
Mathf.Sign(SomeMath.Dot(AB.x, AB.y, AB.z, actualX - sphereB.x, targetY - sphereB.y, targetZ - sphereB.z)))
{
if (pass == -1)
{
compactData[GetIndex(vx, vz)].Update(targetY);
}
else
{
compactData[GetIndex(vx, vz)].Update(targetY, pass);
}
}
//VolumeSetTime.Stop();
}
}
}
}
}
}
else
if (alighment == C_Axis.x)
{
for (int currentX = startX; currentX < endX; currentX++)
{
Vector3 intersection = SomeMath.ClipLineToPlaneX(sphereA, AB_normalized, (currentX * voxelSize) + voxelSizeHalf);
float targetX = intersection.x;
for (int i = 0; i < elipseLines.Count; i++)
{
ElipseLine line = elipseLines[i];
float p1y = line.point1x + intersection.y;
float p1z = line.point1y + intersection.z;
float p2z = line.point2y + intersection.z;
sbyte pass = line.passability;
//if (pass != -1)
// pass += 10;
//Vector3 p1 = GetValidVector3(line.point1) + intersection;
//Vector3 p2 = GetValidVector3(line.point2) + intersection;
//Debuger_K.AddLine(p1, p2, Color.blue);
for (int currentZ = (int)(p1z / voxelSize) - 1; currentZ < (int)(p2z / voxelSize) + 1; currentZ++)
{
if (currentZ >= startZ && currentZ < endZ)
{
int vx = currentX - template.startX_extra;
int vz = currentZ - template.startZ_extra;
//float actualZ;
//switch (intMask[vx][vz]) {
// case 3: actualZ = currentZ * voxelSize + voxelSizeHalf; break;
// case 4: actualZ = currentZ * voxelSize; break;
// case 5: actualZ = currentZ * voxelSize + voxelSize; break;
// default: actualZ = currentZ * voxelSize; break;
//}
float actualZ = currentZ * voxelSize + voxelSizeHalf;
float dz = (actualZ - p1z) / line.normalizedY;//determinant
//Vector3 px = new Vector3(targetX, p1y + (line.normalizedY * dz), actualZ);
//Debuger_K.AddDot(px, Color.magenta, 0.01f);
if (dz >= 0f && dz <= line.length)
{
float targetY = p1y + (line.normalizedX * dz);
//VolumeSetTime.Start();
if (Mathf.Sign(SomeMath.Dot(AB.x, AB.y, AB.z, targetX - sphereA.x, targetY - sphereA.y, actualZ - sphereA.z)) !=
Mathf.Sign(SomeMath.Dot(AB.x, AB.y, AB.z, targetX - sphereB.x, targetY - sphereB.y, actualZ - sphereB.z)))
{
if (pass == -1)
{
compactData[GetIndex(vx, vz)].Update(targetY);
}
else
{
compactData[GetIndex(vx, vz)].Update(targetY, pass);
}
}
//VolumeSetTime.Stop();
}
}
}
}
}
}
if (IS_WALKABLE == false)
{
AppendSpherePrivate(compactData, sphereA, capsileRadius, 0, false, false);
AppendSpherePrivate(compactData, sphereB, capsileRadius, 0, false, false);
}
else
{
if (sphereA.y == sphereB.y)
{
AppendSpherePrivate(compactData, sphereA, capsileRadius, 0, false, true);
AppendSpherePrivate(compactData, sphereB, capsileRadius, 0, false, true);
}
else if (FLIP_Y == false)
{
if (sphereA.y > sphereB.y)
{
AppendSpherePrivate(compactData, sphereA, capsileRadius, 0, false, true);
AppendSpherePrivate(compactData, sphereB, capsileRadius, 0, true, true);
}
else if (sphereA.y < sphereB.y)
{
AppendSpherePrivate(compactData, sphereA, capsileRadius, 0, true, true);
AppendSpherePrivate(compactData, sphereB, capsileRadius, 0, false, true);
}
}
else
{
if (sphereA.y < sphereB.y)
{
AppendSpherePrivate(compactData, sphereA, capsileRadius, 0, true, true);
AppendSpherePrivate(compactData, sphereB, capsileRadius, 0, false, true);
}
else if (sphereA.y > sphereB.y)
{
AppendSpherePrivate(compactData, sphereA, capsileRadius, 0, false, true);
AppendSpherePrivate(compactData, sphereB, capsileRadius, 0, true, true);
}
}
}
}
