コード例 #1
0
        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);
                    }
                }
            }
        }