/// <summary>
        /// Rebuilds outdated triangle data for colliders and recalculates hashes storing everything in StaticColliderData
        /// </summary>
        private void BuildInput(StaticColliderData[] collidersLocal, CollisionFilterGroupFlags includedCollisionGroups)
        {
            NavigationMeshCache lastCache = oldNavigationMesh?.Cache;

            bool clearCache = false;

            Dispatcher.ForEach(collidersLocal, colliderData =>
            {
                var entity = colliderData.Component.Entity;
                TransformComponent entityTransform = entity.Transform;
                Matrix entityWorldMatrix           = entityTransform.WorldMatrix;

                NavigationMeshInputBuilder entityNavigationMeshInputBuilder = colliderData.InputBuilder = new NavigationMeshInputBuilder();

                // Compute hash of collider and compare it with the previous build if there is one
                colliderData.ParameterHash = NavigationMeshBuildUtils.HashEntityCollider(colliderData.Component, includedCollisionGroups);
                colliderData.Previous      = null;
                if (lastCache?.Objects.TryGetValue(colliderData.Component.Id, out colliderData.Previous) ?? false)
                {
                    if ((!colliderData.Component.AlwaysUpdateNaviMeshCache) &&
                        (colliderData.Previous.ParameterHash == colliderData.ParameterHash))
                    {
                        // In this case, we don't need to recalculate the geometry for this shape, since it wasn't changed
                        // here we take the triangle mesh from the previous build as the current
                        colliderData.InputBuilder = colliderData.Previous.InputBuilder;
                        colliderData.Planes.Clear();
                        colliderData.Planes.AddRange(colliderData.Previous.Planes);
                        colliderData.Processed = false;
                        return;
                    }
                }

                // Clear cache on removal of infinite planes
                if (colliderData.Planes.Count > 0)
                {
                    clearCache = true;
                }

                // Clear planes
                colliderData.Planes.Clear();

                // Return empty data for disabled colliders, filtered out colliders or trigger colliders
                if (!colliderData.Component.Enabled || colliderData.Component.IsTrigger ||
                    !NavigationMeshBuildUtils.CheckColliderFilter(colliderData.Component, includedCollisionGroups))
                {
                    colliderData.Processed = true;
                    return;
                }

                // Make sure shape is up to date
                if (!NavigationMeshBuildUtils.HasLatestColliderShape(colliderData.Component))
                {
                    colliderData.Component.ComposeShape();
                }

                // Interate through all the colliders shapes while queueing all shapes in compound shapes to process those as well
                Queue <ColliderShape> shapesToProcess = new Queue <ColliderShape>();
                if (colliderData.Component.ColliderShape != null)
                {
                    shapesToProcess.Enqueue(colliderData.Component.ColliderShape);
                    while (shapesToProcess.Count > 0)
                    {
                        var shape     = shapesToProcess.Dequeue();
                        var shapeType = shape.GetType();
                        if (shapeType == typeof(BoxColliderShape))
                        {
                            var box          = (BoxColliderShape)shape;
                            var boxDesc      = GetColliderShapeDesc <BoxColliderShapeDesc>(box.Description);
                            Matrix transform = box.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Cube.New(boxDesc.Size, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(SphereColliderShape))
                        {
                            var sphere       = (SphereColliderShape)shape;
                            var sphereDesc   = GetColliderShapeDesc <SphereColliderShapeDesc>(sphere.Description);
                            Matrix transform = sphere.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Sphere.New(sphereDesc.Radius, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(CylinderColliderShape))
                        {
                            var cylinder     = (CylinderColliderShape)shape;
                            var cylinderDesc = GetColliderShapeDesc <CylinderColliderShapeDesc>(cylinder.Description);
                            Matrix transform = cylinder.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Cylinder.New(cylinderDesc.Height, cylinderDesc.Radius, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(CapsuleColliderShape))
                        {
                            var capsule      = (CapsuleColliderShape)shape;
                            var capsuleDesc  = GetColliderShapeDesc <CapsuleColliderShapeDesc>(capsule.Description);
                            Matrix transform = capsule.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Capsule.New(capsuleDesc.Length, capsuleDesc.Radius, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(ConeColliderShape))
                        {
                            var cone         = (ConeColliderShape)shape;
                            var coneDesc     = GetColliderShapeDesc <ConeColliderShapeDesc>(cone.Description);
                            Matrix transform = cone.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Cone.New(coneDesc.Radius, coneDesc.Height, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(StaticPlaneColliderShape))
                        {
                            var planeShape   = (StaticPlaneColliderShape)shape;
                            var planeDesc    = GetColliderShapeDesc <StaticPlaneColliderShapeDesc>(planeShape.Description);
                            Matrix transform = entityWorldMatrix;

                            Plane plane = new Plane(planeDesc.Normal, planeDesc.Offset);

                            // Pre-Transform plane parameters
                            plane.Normal = Vector3.TransformNormal(planeDesc.Normal, transform);
                            plane.Normal.Normalize();
                            plane.D += Vector3.Dot(transform.TranslationVector, plane.Normal);

                            colliderData.Planes.Add(plane);
                        }
                        else if (shapeType == typeof(ConvexHullColliderShape))
                        {
                            var hull         = (ConvexHullColliderShape)shape;
                            Matrix transform = hull.PositiveCenterMatrix * entityWorldMatrix;

                            // Convert hull indices to int
                            int[] indices = new int[hull.Indices.Count];
                            if (hull.Indices.Count % 3 != 0)
                            {
                                throw new InvalidOperationException($"{shapeType} does not consist of triangles");
                            }
                            for (int i = 0; i < hull.Indices.Count; i += 3)
                            {
                                indices[i]     = (int)hull.Indices[i];
                                indices[i + 2] = (int)hull.Indices[i + 1]; // NOTE: Reversed winding to create left handed input
                                indices[i + 1] = (int)hull.Indices[i + 2];
                            }

                            entityNavigationMeshInputBuilder.AppendArrays(hull.Points.ToArray(), indices, transform);
                        }
                        else if (shapeType == typeof(StaticMeshColliderShape))
                        {
                            var mesh         = (StaticMeshColliderShape)shape;
                            Matrix transform = mesh.PositiveCenterMatrix * entityWorldMatrix;

                            // Convert hull indices to int
                            int[] indices = new int[mesh.Indices.Count];
                            if (mesh.Indices.Count % 3 != 0)
                            {
                                throw new InvalidOperationException($"{shapeType} does not consist of triangles");
                            }
                            for (int i = 0; i < mesh.Indices.Count; i += 3)
                            {
                                indices[i]     = (int)mesh.Indices[i];
                                indices[i + 2] = (int)mesh.Indices[i + 1]; // NOTE: Reversed winding to create left handed input
                                indices[i + 1] = (int)mesh.Indices[i + 2];
                            }

                            entityNavigationMeshInputBuilder.AppendArrays(mesh.Vertices.ToArray(), indices, transform);
                        }
                        else if (shapeType == typeof(HeightfieldColliderShape))
                        {
                            var heightfield = (HeightfieldColliderShape)shape;

                            var halfRange    = (heightfield.MaxHeight - heightfield.MinHeight) * 0.5f;
                            var offset       = -(heightfield.MinHeight + halfRange);
                            Matrix transform = Matrix.Translation(new Vector3(0, offset, 0)) * heightfield.PositiveCenterMatrix * entityWorldMatrix;

                            var width  = heightfield.HeightStickWidth - 1;
                            var length = heightfield.HeightStickLength - 1;
                            var mesh   = GeometricPrimitive.Plane.New(width, length, width, length, normalDirection: NormalDirection.UpY, toLeftHanded: true);

                            var arrayLength = heightfield.HeightStickWidth * heightfield.HeightStickLength;

                            using (heightfield.LockToReadHeights())
                            {
                                switch (heightfield.HeightType)
                                {
                                case HeightfieldTypes.Short:
                                    if (heightfield.ShortArray == null)
                                    {
                                        continue;
                                    }
                                    for (int i = 0; i < arrayLength; ++i)
                                    {
                                        mesh.Vertices[i].Position.Y = heightfield.ShortArray[i] * heightfield.HeightScale;
                                    }
                                    break;

                                case HeightfieldTypes.Byte:
                                    if (heightfield.ByteArray == null)
                                    {
                                        continue;
                                    }
                                    for (int i = 0; i < arrayLength; ++i)
                                    {
                                        mesh.Vertices[i].Position.Y = heightfield.ByteArray[i] * heightfield.HeightScale;
                                    }
                                    break;

                                case HeightfieldTypes.Float:
                                    if (heightfield.FloatArray == null)
                                    {
                                        continue;
                                    }
                                    for (int i = 0; i < arrayLength; ++i)
                                    {
                                        mesh.Vertices[i].Position.Y = heightfield.FloatArray[i];
                                    }
                                    break;
                                }
                            }

                            entityNavigationMeshInputBuilder.AppendMeshData(mesh, transform);
                        }
                        else if (shapeType == typeof(CompoundColliderShape))
                        {
                            // Unroll compound collider shapes
                            var compound = (CompoundColliderShape)shape;
                            for (int i = 0; i < compound.Count; i++)
                            {
                                shapesToProcess.Enqueue(compound[i]);
                            }
                        }
                    }
                }

                // Clear cache on addition of infinite planes
                if (colliderData.Planes.Count > 0)
                {
                    clearCache = true;
                }

                // Mark collider as processed
                colliderData.Processed = true;
            });

            if (clearCache && oldNavigationMesh != null)
            {
                oldNavigationMesh = null;
            }
        }
        /// <summary>
        /// Rebuilds outdated triangle data for colliders and recalculates hashes storing everything in StaticColliderData
        /// </summary>
        private void BuildInput(StaticColliderData[] collidersLocal, CollisionFilterGroupFlags includedCollisionGroups)
        {
            NavigationMeshCache lastCache = oldNavigationMesh?.Cache;

            bool clearCache = false;

            Dispatcher.ForEach(collidersLocal, colliderData =>
            {
                var entity = colliderData.Component.Entity;
                TransformComponent entityTransform = entity.Transform;
                Matrix entityWorldMatrix           = entityTransform.WorldMatrix;

                NavigationMeshInputBuilder entityNavigationMeshInputBuilder = colliderData.InputBuilder = new NavigationMeshInputBuilder();

                // Compute hash of collider and compare it with the previous build if there is one
                colliderData.ParameterHash = NavigationMeshBuildUtils.HashEntityCollider(colliderData.Component, includedCollisionGroups);
                colliderData.Previous      = null;
                if (lastCache?.Objects.TryGetValue(colliderData.Component.Id, out colliderData.Previous) ?? false)
                {
                    if ((!colliderData.Component.AlwaysUpdateNaviMeshCache) &&
                        (colliderData.Previous.ParameterHash == colliderData.ParameterHash))
                    {
                        // In this case, we don't need to recalculate the geometry for this shape, since it wasn't changed
                        // here we take the triangle mesh from the previous build as the current
                        colliderData.InputBuilder = colliderData.Previous.InputBuilder;
                        colliderData.Planes.Clear();
                        colliderData.Planes.AddRange(colliderData.Previous.Planes);
                        colliderData.Processed = false;
                        return;
                    }
                }

                // Clear cache on removal of infinite planes
                if (colliderData.Planes.Count > 0)
                {
                    clearCache = true;
                }

                // Clear planes
                colliderData.Planes.Clear();

                // Return empty data for disabled colliders, filtered out colliders or trigger colliders
                if (!colliderData.Component.Enabled || colliderData.Component.GenerateOverlapEvents ||
                    !NavigationMeshBuildUtils.CheckColliderFilter(colliderData.Component, includedCollisionGroups))
                {
                    colliderData.Processed = true;
                    return;
                }

                // Make sure shape is up to date
                if (!NavigationMeshBuildUtils.HasLatestColliderShape(colliderData.Component))
                {
                    colliderData.Component.ComposeShape();
                }

                // Interate through all the colliders shapes while queueing all shapes in compound shapes to process those as well
                Queue <ColliderShape> shapesToProcess = new Queue <ColliderShape>();
                if (colliderData.Component.ColliderShape != null)
                {
                    shapesToProcess.Enqueue(colliderData.Component.ColliderShape);
                    while (shapesToProcess.Count > 0)
                    {
                        var shape     = shapesToProcess.Dequeue();
                        var shapeType = shape.GetType();
                        if (shapeType == typeof(BoxColliderShape))
                        {
                            var box          = (BoxColliderShape)shape;
                            var boxDesc      = GetColliderShapeDesc <BoxColliderShapeDesc>(box.Description);
                            Matrix transform = box.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Cube.New(boxDesc.Size, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(SphereColliderShape))
                        {
                            var sphere       = (SphereColliderShape)shape;
                            var sphereDesc   = GetColliderShapeDesc <SphereColliderShapeDesc>(sphere.Description);
                            Matrix transform = sphere.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Sphere.New(sphereDesc.Radius, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(CylinderColliderShape))
                        {
                            var cylinder     = (CylinderColliderShape)shape;
                            var cylinderDesc = GetColliderShapeDesc <CylinderColliderShapeDesc>(cylinder.Description);
                            Matrix transform = cylinder.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Cylinder.New(cylinderDesc.Length, cylinderDesc.Radius, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(CapsuleColliderShape))
                        {
                            var capsule      = (CapsuleColliderShape)shape;
                            var capsuleDesc  = GetColliderShapeDesc <CapsuleColliderShapeDesc>(capsule.Description);
                            Matrix transform = capsule.PositiveCenterMatrix * entityWorldMatrix;

                            var meshData = GeometricPrimitive.Capsule.New(capsuleDesc.Length, capsuleDesc.Radius, toLeftHanded: true);
                            entityNavigationMeshInputBuilder.AppendMeshData(meshData, transform);
                        }
                        else if (shapeType == typeof(ConvexHullColliderShape))
                        {
                            var hull         = (ConvexHullColliderShape)shape;
                            Matrix transform = hull.PositiveCenterMatrix * entityWorldMatrix;

                            // Convert hull indices to int
                            int[] indices = new int[hull.Indices.Count];
                            if (hull.Indices.Count % 3 != 0)
                            {
                                throw new InvalidOperationException($"{shapeType} does not consist of triangles");
                            }
                            for (int i = 0; i < hull.Indices.Count; i += 3)
                            {
                                indices[i]     = (int)hull.Indices[i];
                                indices[i + 2] = (int)hull.Indices[i + 1]; // NOTE: Reversed winding to create left handed input
                                indices[i + 1] = (int)hull.Indices[i + 2];
                            }

                            entityNavigationMeshInputBuilder.AppendArrays(hull.Points.ToArray(), indices, transform);
                        }
                        else if (shapeType == typeof(MeshColliderShape))
                        {
                            var mesh         = (MeshColliderShape)shape;
                            Matrix transform = mesh.PositiveCenterMatrix * entityWorldMatrix;

                            // Convert hull indices to int
                            int[] indices = new int[mesh.Indices.Length];
                            if (mesh.Indices.Length % 3 != 0)
                            {
                                throw new InvalidOperationException($"{shapeType} does not consist of triangles");
                            }
                            for (int i = 0; i < mesh.Indices.Length; i += 3)
                            {
                                indices[i]     = (int)mesh.Indices[i];
                                indices[i + 2] = (int)mesh.Indices[i + 1]; // NOTE: Reversed winding to create left handed input
                                indices[i + 1] = (int)mesh.Indices[i + 2];
                            }

                            entityNavigationMeshInputBuilder.AppendArrays(mesh.Vertices.ToArray(), indices, transform);
                        }
                    }
                }

                // Clear cache on addition of infinite planes
                if (colliderData.Planes.Count > 0)
                {
                    clearCache = true;
                }

                // Mark collider as processed
                colliderData.Processed = true;
            });

            if (clearCache && oldNavigationMesh != null)
            {
                oldNavigationMesh = null;
            }
        }