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