Exemplo n.º 1
0
    /// <summary>
    /// Method that takes the IK positions for each feet and apply displacement to ground.
    /// </summary>
    /// <param name="xLeft"></param>
    /// <param name="zLeft"></param>
    /// <param name="xRight"></param>
    /// <param name="zRight"></param>
    public override void DrawFootprint(int xLeft, int zLeft, int xRight, int zRight)
    {
        //       Initial Declarations      //
        // =============================== //

        // Reset counter hits
        counterHitsLeft  = 0;
        counterHitsRight = 0;

        // Heightmaps per foot
        float[,] heightMapLeft    = new float[2 * gridSize + 1, 2 * gridSize + 1];
        float[,] heightMapRight   = new float[2 * gridSize + 1, 2 * gridSize + 1];
        int[,] heightMapLeftBool  = new int[2 * gridSize + 1, 2 * gridSize + 1];
        int[,] heightMapRightBool = new int[2 * gridSize + 1, 2 * gridSize + 1];

        // Warning: Supossing that terrain is squared!
        if (printTerrainInformation)
        {
            Debug.Log("[INFO] Length Terrain - X: " + terrain.TerrainSize().x);
            Debug.Log("[INFO] Length Terrain - Z: " + terrain.TerrainSize().z);
            Debug.Log("[INFO] Number of heightmap cells: " + (terrain.GridSize().x - 1));
            Debug.Log("[INFO] Lenght of one cell - X: " + (terrain.TerrainSize().x / (terrain.GridSize().x - 1)));
            Debug.Log("[INFO] Lenght of one cell - Z: " + (terrain.TerrainSize().z / (terrain.GridSize().z - 1)));
            Debug.Log("[INFO] Area of one cell: " + (terrain.TerrainSize().x / (terrain.GridSize().x - 1)) * (terrain.TerrainSize().z / (terrain.GridSize().z - 1)));
        }

        // Calculate area per cell outside the loop
        lenghtCellX = terrain.TerrainSize().x / (terrain.GridSize().x - 1);
        lenghtCellZ = terrain.TerrainSize().z / (terrain.GridSize().z - 1);
        areaCell    = lenghtCellX * lenghtCellZ;

        //    Contact Area Calculation     //
        // =============================== //

        // 2D iteration for both feet
        // It counts the number of hits, save the classified cell in a list and debug ray-casting
        for (int zi = -gridSize; zi <= gridSize; zi++)
        {
            for (int xi = -gridSize; xi <= gridSize; xi++)
            {
                // Calculate each cell position wrt World and Heightmap - Left Foot
                // The sensors that counts the number of hits always remain on the surface
                Vector3 rayGridLeft      = new Vector3(xLeft + xi, terrain.Get(xLeft + xi, zLeft + zi) - offsetRay, zLeft + zi);
                Vector3 rayGridWorldLeft = terrain.Grid2World(rayGridLeft);

                // Calculate each cell position wrt World and Heightmap - Right Foot
                // The sensors that counts the number of hits always remain on the surface
                Vector3 rayGridRight      = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi) - offsetRay, zRight + zi);
                Vector3 rayGridWorldRight = terrain.Grid2World(rayGridRight);

                //------//

                // Create each ray for the grid (wrt World) - Left
                RaycastHit leftFootHit;
                Ray        upRayLeftFoot = new Ray(rayGridWorldLeft, Vector3.up);

                // Create each ray for the grid (wrt World) - Right
                RaycastHit rightFootHit;
                Ray        upRayRightFoot = new Ray(rayGridWorldRight, Vector3.up);

                //------//

                // If hits the Left Foot, increase counter and add cell to be affected
                if (LeftFootCollider.Raycast(upRayLeftFoot, out leftFootHit, rayDistance))
                {
                    counterHitsLeft++;
                    if (showGridDebugLeft)
                    {
                        Debug.DrawRay(rayGridWorldLeft, Vector3.up * rayDistance, Color.blue);
                    }
                }
                else
                {
                    if (showGridDebugLeft)
                    {
                        Debug.DrawRay(rayGridWorldLeft, Vector3.up * rayDistance, Color.red);
                    }
                }

                // If hits the Right Foot, increase counter and add cell to be affected
                if (RightFootCollider.Raycast(upRayRightFoot, out rightFootHit, rayDistance))
                {
                    counterHitsRight++;

                    if (showGridDebugRigth)
                    {
                        Debug.DrawRay(rayGridWorldRight, Vector3.up * rayDistance, Color.blue);
                    }
                }
                else
                {
                    if (showGridDebugRigth)
                    {
                        Debug.DrawRay(rayGridWorldRight, Vector3.up * rayDistance, Color.red);
                    }
                }
            }
        }

        // Terrain Deformation is affected by an increasing value of the contact area, therefore the deformation
        // will be defined by the maximum contact area in each frame.
        oldAreaTotalLeft = ((counterHitsLeft) * areaCell);
        if (oldAreaTotalLeft >= areaTotalLeft)
        {
            areaTotalLeft = ((counterHitsLeft) * areaCell);
        }

        oldAreaTotalRight = ((counterHitsRight) * areaCell);
        if (oldAreaTotalRight >= areaTotalRight)
        {
            areaTotalRight = ((counterHitsRight) * areaCell);
        }

        // Total Area for both feet
        areaTotal = areaTotalLeft + areaTotalRight;

        //       Physics Calculation       //
        // =============================== //

        // Calculate Pressure applicable per frame - if no contact, there is no pressure
        // The three values should be similar? - TODO

        // Pressure by left feet
        if (counterHitsLeft == 0)
        {
            pressureLeft = 0f;
        }
        else
        {
            pressureLeft = (TotalForceLeftY) / areaTotalLeft;
        }

        // Pressure by right feet
        if (counterHitsRight == 0)
        {
            pressureRight = 0f;
        }
        else
        {
            pressureRight = (TotalForceRightY) / areaTotalRight;
        }

        // Total pressure
        if (counterHitsLeft == 0 || counterHitsRight == 0)
        {
            pressure = 0f;
        }
        else
        {
            pressure = (TotalForceY) / areaTotal;
        }

        //     Deformation Calculation     //
        // =============================== //

        // Given area, pressure and terrain parameters, we calculate the displacement on the terrain
        // The decrement will depend also on the ContactTime used to calculate the corresponding Force
        // As for the area, we keep the maximum value

        oldHeightCellDisplacementYoungLeft = pressureLeft * (originaLength / (youngModulusPa));
        if (oldHeightCellDisplacementYoungLeft >= heightCellDisplacementYoungLeft)
        {
            heightCellDisplacementYoungLeft = pressureLeft * (originaLength / youngModulusPa);
        }

        oldHeightCellDisplacementYoungRight = pressureRight * (originaLength / (youngModulusPa));
        if (oldHeightCellDisplacementYoungRight >= heightCellDisplacementYoungRight)
        {
            heightCellDisplacementYoungRight = pressureRight * (originaLength / youngModulusPa);
        }

        // Given the entire deformation in Y, we calculate the corresponding frame-based deformation based on the frame-time.
        displacementLeft  = (Time.deltaTime * (float)heightCellDisplacementYoungLeft) / ContactTime;
        displacementRight = (Time.deltaTime * (float)heightCellDisplacementYoungRight) / ContactTime;

        //        Apply Deformation        //
        // =============================== //

        // 2D iteration Deformation
        // Once we have the displacement, we saved the actual result of applying it to the terrain (only when the foot is grounded)
        if (IsLeftFootGrounded)
        {
            StartCoroutine(DecreaseTerrainLeft(heightMapLeft, heightMapLeftBool, xLeft, zLeft));
        }
        else if (!IsLeftFootGrounded)
        {
            // Every time we lift the foot, we reset the variables and stop the coroutines.
            heightCellDisplacementYoungLeft = 0;
            StopAllCoroutines();
        }

        if (IsRightFootGrounded)
        {
            StartCoroutine(DecreaseTerrainRight(heightMapRight, heightMapRightBool, xRight, zRight));
        }
        else if (!IsRightFootGrounded)
        {
            heightCellDisplacementYoungRight = 0;
            StopAllCoroutines();
        }
    }
Exemplo n.º 2
0
    IEnumerator DecreaseTerrainRight(float[,] heightMapRight, int[,] heightMapRightBool, int xRight, int zRight)
    {
        // 1. Apply frame-per-frame deformation ("displacement")
        for (int zi = -gridSize; zi <= gridSize; zi++)
        {
            for (int xi = -gridSize; xi <= gridSize; xi++)
            {
                // Calculate each cell position wrt World and Heightmap - Right Foot
                Vector3 rayGridRight      = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi), zRight + zi);
                Vector3 rayGridWorldRight = terrain.Grid2World(rayGridRight);

                // Create each ray for the grid (wrt World) - Right
                RaycastHit rightFootHit;
                Ray        upRayRightFoot = new Ray(rayGridWorldRight, Vector3.up);

                // If hits the Right Foot, increase counter and add cell to be affected
                if (RightFootCollider.Raycast(upRayRightFoot, out rightFootHit, rayDistance))
                {
                    // Cell contacting directly
                    heightMapRightBool[zi + gridSize, xi + gridSize] = 2;

                    // TEST //

                    //Debug.Log("NOW IN RIGHT: " + terrain.Get(rayGridRight.x, rayGridRight.z));
                    //Debug.Log("HOW MUCH SHOULD IT BE RIGHT: " + (terrain.GetConstant(rayGridRight.x, rayGridRight.z) - heightCellDisplacementYoungRight));

                    if (terrain.Get(rayGridRight.x, rayGridRight.z) >= terrain.GetConstant(rayGridRight.x, rayGridRight.z) - heightCellDisplacementYoungRight)
                    {
                        heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z) - (displacementRight);
                        //Debug.Log("AFTER DEFORMATION: " + (heightMapRight[zi + gridSize, xi + gridSize]));
                    }
                    else
                    {
                        heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z);
                        //Debug.Log("STAYS THE SAME: " + (heightMapRight[zi + gridSize, xi + gridSize]));
                    }

                    //Before:
                    //heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z) - (displacementRight);
                }
                else
                {
                    // No contact
                    heightMapRightBool[zi + gridSize, xi + gridSize] = 0;
                    heightMapRight[zi + gridSize, xi + gridSize]     = terrain.Get(rayGridRight.x, rayGridRight.z);
                }
            }
        }

        // Gaussian pre-filter for Right Foot (To improve)
        if (applyPreFilterRight)
        {
            heightMapRight = FilterBufferRight(heightMapRight, heightMapRightBool);
        }

        // 3. Save terrain
        if (applyNewTerrainModification)
        {
            for (int zi = -gridSize; zi <= gridSize; zi++)
            {
                for (int xi = -gridSize; xi <= gridSize; xi++)
                {
                    Vector3 rayGridRight = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi), zRight + zi);
                    terrain.Set(rayGridRight.x, rayGridRight.z, heightMapRight[zi + gridSize, xi + gridSize]);
                }
            }
        }

        yield return(null);
    }
Exemplo n.º 3
0
    IEnumerator DecreaseTerrainRight(float[,] heightMapRight, int[,] heightMapRightBool, int xRight, int zRight)
    {
        // 1. Apply frame-per-frame deformation ("displacement")
        for (int zi = -gridSize; zi <= gridSize; zi++)
        {
            for (int xi = -gridSize; xi <= gridSize; xi++)
            {
                // Calculate each cell position wrt World and Heightmap - Right Foot
                Vector3 rayGridRight      = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi), zRight + zi);
                Vector3 rayGridWorldRight = terrain.Grid2World(rayGridRight);

                // Create each ray for the grid (wrt World) - Right
                RaycastHit rightFootHit;
                Ray        upRayRightFoot = new Ray(rayGridWorldRight, Vector3.up);

                // If hits the Right Foot and the cell was classified with 2 (direct contact):
                if (RightFootCollider.Raycast(upRayRightFoot, out rightFootHit, rayDistance) && (heightMapRightBool[zi + gridSize, xi + gridSize] == 2))
                {
                    // Cell contacting directly - Decrease until limit reached
                    if (terrain.Get(rayGridRight.x, rayGridRight.z) >= terrain.GetConstant(rayGridRight.x, rayGridRight.z) - heightCellDisplacementYoungRight)
                    {
                        heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z) - (displacementRight);
                    }
                    else
                    {
                        heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z);
                    }
                } // 3: Front 4: Back
                else if (!RightFootCollider.Raycast(upRayRightFoot, out rightFootHit, rayDistance) && (heightMapRightBool[zi + gridSize, xi + gridSize] == 4) && applyBumps)
                {
                    // If ray does not hit and is classified as BACK neightbour, we create a bump.
                    if (terrain.Get(rayGridRight.x, rayGridRight.z) <= terrain.GetConstant(rayGridRight.x, rayGridRight.z) - newBumpHeightDeformationRight)
                    {
                        //heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z) - (bumpDisplacementRightBack * MaxTotalForceRightFootZNorm);
                        heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z) - (bumpDisplacementRightBack);
                    }
                    else
                    {
                        heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z);
                    }
                }
                else if (!RightFootCollider.Raycast(upRayRightFoot, out rightFootHit, rayDistance) && (heightMapRightBool[zi + gridSize, xi + gridSize] == 3) && applyBumps)
                {
                    if (terrain.Get(rayGridRight.x, rayGridRight.z) <= terrain.GetConstant(rayGridRight.x, rayGridRight.z) - newBumpHeightDeformationRight)
                    {
                        heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z) - (bumpDisplacementRightBack);
                    }
                    else
                    {
                        heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z);
                    }
                }
                else
                {
                    // If is out of reach
                    heightMapRight[zi + gridSize, xi + gridSize] = terrain.Get(rayGridRight.x, rayGridRight.z);
                }
            }
        }

        // This version of smoothing, does not set the terrain, only filters (return) the new heightmap
        // TODO - NOT WORKING YET - See function
        if (applyFilterRight2)
        {
            // Provisional: When do we smooth?
            if (IsRightFootGrounded && !IsLeftFootGrounded)
            {
                if (!isFilteredRight)
                {
                    heightMapRight = NewFilterHeightMapReturn(xRight, zRight, heightMapRight);
                    filterIterationsRightCounter++;
                }

                if (filterIterationsRightCounter >= filterIterationsRightFoot)
                {
                    isFilteredRight = true;
                }
            }
            else
            {
                isFilteredRight = false;
                filterIterationsRightCounter = 0;
            }
        }

        // 2. Save terrain
        if (applyFootprints)
        {
            for (int zi = -gridSize; zi <= gridSize; zi++)
            {
                for (int xi = -gridSize; xi <= gridSize; xi++)
                {
                    Vector3 rayGridRight = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi), zRight + zi);
                    terrain.Set(rayGridRight.x, rayGridRight.z, heightMapRight[zi + gridSize, xi + gridSize]);
                }
            }
        }

        yield return(null);
    }
Exemplo n.º 4
0
    /// <summary>
    /// Method that takes the IK positions for each feet and apply displacement to ground.
    /// </summary>
    /// <param name="xLeft"></param>
    /// <param name="zLeft"></param>
    /// <param name="xRight"></param>
    /// <param name="zRight"></param>
    public override void DrawFootprint(int xLeft, int zLeft, int xRight, int zRight)
    {
        // Reset counter hits
        counterHitsLeft  = 0;
        counterHitsRight = 0;

        // Clear cells to be affected
        //cellsAffected.Clear();

        // New
        float[,] heightMapLeft    = new float[2 * gridSize + 1, 2 * gridSize + 1];
        float[,] heightMapRight   = new float[2 * gridSize + 1, 2 * gridSize + 1];
        int[,] heightMapLeftBool  = new int[2 * gridSize + 1, 2 * gridSize + 1];
        int[,] heightMapRightBool = new int[2 * gridSize + 1, 2 * gridSize + 1];

        // Warning: Supossing that terrain is squared!
        if (printFootprintsInformation)
        {
            Debug.Log("[INFO] Length Terrain - X: " + terrain.TerrainSize().x);
            Debug.Log("[INFO] Length Terrain - Z: " + terrain.TerrainSize().z);
            Debug.Log("[INFO] Number of heightmap cells: " + (terrain.GridSize().x - 1));
            Debug.Log("[INFO] Lenght of one cell - X: " + (terrain.TerrainSize().x / (terrain.GridSize().x - 1)));
            Debug.Log("[INFO] Lenght of one cell - Z: " + (terrain.TerrainSize().z / (terrain.GridSize().z - 1)));
            Debug.Log("[INFO] Area of one cell: " + (terrain.TerrainSize().x / (terrain.GridSize().x - 1)) * (terrain.TerrainSize().z / (terrain.GridSize().z - 1)));
        }

        // Calculate area per cell outside the loop
        lenghtCellX = terrain.TerrainSize().x / (terrain.GridSize().x - 1);
        lenghtCellZ = terrain.TerrainSize().z / (terrain.GridSize().z - 1);
        areaCell    = lenghtCellX * lenghtCellZ;

        // Max. applicable pressure
        maxPressure = Force / areaCell;

        // TODO: Calculate automatically
        // Min. applicable pressure (IDLE position, gets 40 hits)
        minPressure = Force / (areaCell * 40);

        // 2D iteration for Both Foot
        // It counts the number of hits, save the classified cell in a list and debug ray-casting
        for (int zi = -gridSize; zi <= gridSize; zi++)
        {
            for (int xi = -gridSize; xi <= gridSize; xi++)
            {
                // Calculate each cell position wrt World and Heightmap - Left Foot

                // The sensors that counts the number of hits always remain on the surface
                Vector3 rayGridLeft = new Vector3(xLeft + xi, terrain.Get(xLeft + xi, zLeft + zi), zLeft + zi);
                //Vector3 rayGridLeft = new Vector3(xLeft + xi, HeightIKLeft, zLeft + zi);

                Vector3 rayGridWorldLeft = terrain.Grid2World(rayGridLeft);

                // Calculate each cell position wrt World and Heightmap - Right Foot

                // The sensors that counts the number of hits always remain on the surface
                Vector3 rayGridRight = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi), zRight + zi);
                //Vector3 rayGridRight = new Vector3(xRight + xi, HeightIKRight, zRight + zi);

                Vector3 rayGridWorldRight = terrain.Grid2World(rayGridRight);

                //------//

                // Create each ray for the grid (wrt World) - Left
                RaycastHit leftFootHit;

                // The sensors measure going up
                Ray upRayLeftFoot = new Ray(rayGridWorldLeft, Vector3.up);
                //Ray upRayLeftFoot = new Ray(rayGridWorldLeft, terrain.GetNormal(xLeft + xi, zLeft + zi));

                // Create each ray for the grid (wrt World) - Right
                RaycastHit rightFootHit;

                // The sensors measure going up
                Ray upRayRightFoot = new Ray(rayGridWorldRight, Vector3.up);
                //Ray upRayRightFoot = new Ray(rayGridWorldRight, terrain.GetNormal(xRight + xi, zRight + zi));

                //------//

                // If hits the Left Foot, increase counter and add cell to be affected
                if (LeftFootCollider.Raycast(upRayLeftFoot, out leftFootHit, rayGridDistance))
                {
                    counterHitsLeft++;
                    //cellsAffected.Add(rayGridLeft);

                    if (showGridDebug)
                    {
                        Debug.DrawRay(rayGridWorldLeft, Vector3.up * rayGridDistance, Color.blue);
                        //Debug.DrawRay(rayGridWorldLeft, terrain.GetNormal(xLeft + xi, zLeft + zi) * rayDistance, Color.blue);
                    }
                }
                else
                {
                    //cellsNotAffected.Add(rayGridLeft);

                    if (showGridDebug)
                    {
                        Debug.DrawRay(rayGridWorldLeft, Vector3.up * rayGridDistance, Color.red);
                        //Debug.DrawRay(rayGridWorldLeft, terrain.GetNormal(xLeft + xi, zLeft + zi) * rayDistance, Color.red);
                    }
                }

                // If hits the Right Foot, increase counter and add cell to be affected
                if (RightFootCollider.Raycast(upRayRightFoot, out rightFootHit, rayGridDistance))
                {
                    counterHitsRight++;
                    //cellsAffected.Add(rayGridRight);

                    if (showGridDebug)
                    {
                        Debug.DrawRay(rayGridWorldRight, Vector3.up * rayGridDistance, Color.blue);
                        //Debug.DrawRay(rayGridWorldRight, terrain.GetNormal(xRight + xi, zRight + zi) * rayDistance, Color.blue);
                    }
                }
                else
                {
                    //cellsNotAffected.Add(rayGridRight);

                    if (showGridDebug)
                    {
                        Debug.DrawRay(rayGridWorldRight, Vector3.up * rayGridDistance, Color.red);
                        //Debug.DrawRay(rayGridWorldRight, terrain.GetNormal(xRight + xi, zRight + zi) * rayDistance, Color.red);
                    }
                }
            }
        }

        //------//

        // Calculate the pressure per cell that we need to apply given the force and contact area
        areaTotal = ((counterHitsLeft + counterHitsRight) * areaCell);
        pressure  = Force / areaTotal;

        // TODO: See how pressure should affect realistically based on material
        // PROVISIONAL -> NEED TO TACKLE MATERIAL: COMPRESIBILITY AND OTHERS
        float normalizedValuePressure = Mathf.InverseLerp(0, 75000, pressure);

        heightCellDisplacement = Mathf.Lerp(0f, 0.15f, normalizedValuePressure);

        if (printFootprintsInformation)
        {
            Debug.Log("[INFO] Counter Hits Left: " + counterHitsLeft);
            Debug.Log("[INFO] Counter Hits Right: " + counterHitsRight);
            Debug.Log("[INFO] Total Contact Area: " + areaTotal);
            Debug.Log("[INFO] Current Force: " + Force);
            Debug.Log("[INFO] Pressure/Cell NOW: " + pressure);
            Debug.Log("[INFO] Min Pressure: " + minPressure);
            Debug.Log("[INFO] Max Pressure: " + maxPressure);
        }

        if (printDeformationInformation)
        {
            Debug.Log("normalizedValuePressure: " + normalizedValuePressure);
            Debug.Log("heightCellDisplacement: " + heightCellDisplacement);
        }

        //------//

        // 2D iteration Deformation
        // Once we have the displacement based on the weight, we saved the actual result of applying it to the terrain
        for (int zi = -gridSize; zi <= gridSize; zi++)
        {
            for (int xi = -gridSize; xi <= gridSize; xi++)
            {
                // Calculate each cell position wrt World and Heightmap - Left Foot
                Vector3 rayGridLeft      = new Vector3(xLeft + xi, terrain.Get(xLeft + xi, zLeft + zi), zLeft + zi);
                Vector3 rayGridWorldLeft = terrain.Grid2World(rayGridLeft);

                // Calculate each cell position wrt World and Heightmap - Right Foot
                Vector3 rayGridRight = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi), zRight + zi);
                //Vector3 rayGridRight = new Vector3(xRight + xi, HeightIKRight, zRight + zi);
                Vector3 rayGridWorldRight = terrain.Grid2World(rayGridRight);

                // Create each ray for the grid (wrt World) - Left
                RaycastHit leftFootHit;
                Ray        upRayLeftFoot = new Ray(rayGridWorldLeft, Vector3.up);

                // Create each ray for the grid (wrt World) - Right
                RaycastHit rightFootHit;
                Ray        upRayRightFoot = new Ray(rayGridWorldRight, Vector3.up);

                // If hits the Left Foot, increase counter and add cell to be affected
                if (LeftFootCollider.Raycast(upRayLeftFoot, out leftFootHit, rayGridDistance))
                {
                    // Cell contacting directly
                    heightMapLeftBool[zi + gridSize, xi + gridSize] = 2;
                    heightMapLeft[zi + gridSize, xi + gridSize]     = terrain.Get(rayGridLeft.x, rayGridLeft.z) - heightCellDisplacement;
                }
                else
                {
                    // No contact
                    heightMapLeftBool[zi + gridSize, xi + gridSize] = 0;
                    heightMapLeft[zi + gridSize, xi + gridSize]     = terrain.Get(rayGridLeft.x, rayGridLeft.z);
                }

                // If hits the Right Foot, increase counter and add cell to be affected
                if (RightFootCollider.Raycast(upRayRightFoot, out rightFootHit, rayGridDistance))
                {
                    // Cell contacting directly
                    heightMapRightBool[zi + gridSize, xi + gridSize] = 2;
                    heightMapRight[zi + gridSize, xi + gridSize]     = terrain.Get(rayGridRight.x, rayGridRight.z) - heightCellDisplacement;
                }
                else
                {
                    // No contact
                    heightMapRightBool[zi + gridSize, xi + gridSize] = 0;
                    heightMapRight[zi + gridSize, xi + gridSize]     = terrain.Get(rayGridRight.x, rayGridRight.z);
                }
            }
        }

        // Pre-filter - TEST //
        ///////////////////////

        // Gaussian filter
        if (applyPreFilterLeft)
        {
            heightMapLeft = FilterBufferLeft(heightMapLeft, heightMapLeftBool);
        }

        // Gaussian filter
        if (applyPreFilterRight)
        {
            heightMapRight = FilterBufferRight(heightMapRight, heightMapRightBool);
        }

        // Deformation - Method using array //
        //////////////////////////////////////

        if (applyNewTerrainModification)
        {
            for (int zi = -gridSize; zi <= gridSize; zi++)
            {
                for (int xi = -gridSize; xi <= gridSize; xi++)
                {
                    //Vector3 rayGridLeft = new Vector3(xLeft + xi, terrain.GetConstant(xLeft + xi, zLeft + zi), zLeft + zi);
                    Vector3 rayGridLeft = new Vector3(xLeft + xi, terrain.Get(xLeft + xi, zLeft + zi), zLeft + zi);

                    //Vector3 rayGridRight = new Vector3(xRight + xi, terrain.GetConstant(xRight + xi, zRight + zi), zRight + zi);
                    Vector3 rayGridRight = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi), zRight + zi);

                    if (terrain.Get(rayGridLeft.x, rayGridLeft.z) >= terrain.GetConstant(rayGridLeft.x, rayGridLeft.z) - heightCellDisplacement)
                    {
                        terrain.Set(rayGridLeft.x, rayGridLeft.z, heightMapLeft[zi + gridSize, xi + gridSize]);
                    }

                    if (terrain.Get(rayGridRight.x, rayGridRight.z) >= terrain.GetConstant(rayGridRight.x, rayGridRight.z) - heightCellDisplacement)
                    {
                        terrain.Set(rayGridRight.x, rayGridRight.z, heightMapRight[zi + gridSize, xi + gridSize]);
                    }
                }
            }
        }

        // Post-filter - TEST //
        ////////////////////////
        /// Can't apply here - need to save the terrain with the deformations first

        if (applyPostFilter)
        {
            // Current height-map, including the footprints deformations
            // If refer to a cell, remember to multiply by * TerrainData.heightmapScale.y
            float[,] currentHeightMap = TerrainData.GetHeights(0, 0, (int)terrain.GridSize().x, (int)terrain.GridSize().z);

            // I provide the initial//current height-map to be filtered (for each foot? maybe more efficient way doing only once?)
            FilterHeightmap(zLeft, xLeft, currentHeightMap);
            FilterHeightmap(zRight, xRight, currentHeightMap);

            // Recursive filtering n times
            for (int i = 0; i < 5; i++)
            {
                FilterHeightmap(zLeft, xLeft, HeightMapFiltered);
                FilterHeightmap(zRight, xRight, HeightMapFiltered);
            }

            // Saves the height-map
            Debug.Log("Applying filter");
            TerrainData.SetHeights(0, 0, HeightMapFiltered);
        }
    }
Exemplo n.º 5
0
    /// <summary>
    /// Method that takes the IK positions for each feet and apply displacement to ground.
    /// </summary>
    /// <param name="xLeft"></param>
    /// <param name="zLeft"></param>
    /// <param name="xRight"></param>
    /// <param name="zRight"></param>
    public override void DrawFootprint(int xLeft, int zLeft, int xRight, int zRight)
    {
        //       Initial Declarations      //
        // =============================== //

        //Test
        if (UseTerrainPrefabs)
        {
            youngModulus = YoungM;
            poissonRatio = PoissonRatio;
            applyBumps   = ActivateBump;

            if (FilterIte != 0)
            {
                applyFilterLeft           = true;
                applyFilterRight          = true;
                filterIterationsLeftFoot  = FilterIte;
                filterIterationsRightFoot = FilterIte;
            }
            else if (FilterIte == 0)
            {
                applyFilterLeft  = false;
                applyFilterRight = false;
            }
        }

        // Reset counter hits
        counterHitsLeft     = 0;
        counterHitsRight    = 0;
        neighbourCellsLeft  = 0;
        neighbourCellsRight = 0;

        // TEST
        neighboursPositionsRightFront.Clear();
        neighboursPositionsLeftFront.Clear();
        neighboursPositionsRightBack.Clear();
        neighboursPositionsLeftBack.Clear();

        // Heightmaps for each foot
        float[,] heightMapLeft    = new float[2 * gridSize + 1, 2 * gridSize + 1];
        float[,] heightMapRight   = new float[2 * gridSize + 1, 2 * gridSize + 1];
        int[,] heightMapLeftBool  = new int[2 * gridSize + 1, 2 * gridSize + 1];
        int[,] heightMapRightBool = new int[2 * gridSize + 1, 2 * gridSize + 1];

        // Warning: Supossing that terrain is squared!
        if (printTerrainInformation)
        {
            Debug.Log("[INFO] Length Terrain - X: " + terrain.TerrainSize().x);
            Debug.Log("[INFO] Length Terrain - Z: " + terrain.TerrainSize().z);
            Debug.Log("[INFO] Number of heightmap cells: " + (terrain.GridSize().x - 1));
            Debug.Log("[INFO] Lenght of one cell - X: " + (terrain.TerrainSize().x / (terrain.GridSize().x - 1)));
            Debug.Log("[INFO] Lenght of one cell - Z: " + (terrain.TerrainSize().z / (terrain.GridSize().z - 1)));
            Debug.Log("[INFO] Area of one cell: " + (terrain.TerrainSize().x / (terrain.GridSize().x - 1)) * (terrain.TerrainSize().z / (terrain.GridSize().z - 1)));
        }

        // Calculate area per cell outside the loop
        lenghtCellX = terrain.TerrainSize().x / (terrain.GridSize().x - 1);
        lenghtCellZ = terrain.TerrainSize().z / (terrain.GridSize().z - 1);
        areaCell    = lenghtCellX * lenghtCellZ;

        //    Contact Area Calculation     //
        // =============================== //

        // 2D iteration for both feet
        // It counts the number of hits, save the classified cell in a list and debug ray-casting
        for (int zi = -gridSize; zi <= gridSize; zi++)
        {
            for (int xi = -gridSize; xi <= gridSize; xi++)
            {
                // Calculate each cell position wrt World and Heightmap - Left Foot
                // The sensors that counts the number of hits always remain on the surface
                Vector3 rayGridLeft      = new Vector3(xLeft + xi, terrain.Get(xLeft + xi, zLeft + zi) - offsetRay, zLeft + zi);
                Vector3 rayGridWorldLeft = terrain.Grid2World(rayGridLeft);

                // Calculate each cell position wrt World and Heightmap - Right Foot
                // The sensors that counts the number of hits always remain on the surface
                Vector3 rayGridRight      = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi) - offsetRay, zRight + zi);
                Vector3 rayGridWorldRight = terrain.Grid2World(rayGridRight);

                //------//

                // Create each ray for the grid (wrt World) - Left
                RaycastHit leftFootHit;
                Ray        upRayLeftFoot = new Ray(rayGridWorldLeft, Vector3.up);

                // Create each ray for the grid (wrt World) - Right
                RaycastHit rightFootHit;
                Ray        upRayRightFoot = new Ray(rayGridWorldRight, Vector3.up);

                //------//

                // If hits the Left Foot, increase counter and add cell to be affected
                if (LeftFootCollider.Raycast(upRayLeftFoot, out leftFootHit, rayDistance))
                {
                    // Cell contacting directly
                    heightMapLeftBool[zi + gridSize, xi + gridSize] = 2;
                    counterHitsLeft++;

                    if (showGridDebugLeft)
                    {
                        Debug.DrawRay(rayGridWorldLeft, Vector3.up * rayDistance, Color.blue);
                    }
                }
                else
                {
                    // No contact
                    heightMapLeftBool[zi + gridSize, xi + gridSize] = 0;

                    if (showGridDebugLeft)
                    {
                        Debug.DrawRay(rayGridWorldLeft, Vector3.up * rayDistance, Color.red);
                    }
                }

                // If hits the Right Foot, increase counter and add cell to be affected
                if (RightFootCollider.Raycast(upRayRightFoot, out rightFootHit, rayDistance))
                {
                    // Cell contacting directly
                    heightMapRightBool[zi + gridSize, xi + gridSize] = 2;
                    counterHitsRight++;

                    if (showGridDebugRight)
                    {
                        Debug.DrawRay(rayGridWorldRight, Vector3.up * rayDistance, Color.blue);
                    }
                }
                else
                {
                    // No contact
                    heightMapRightBool[zi + gridSize, xi + gridSize] = 0;

                    if (showGridDebugRight)
                    {
                        Debug.DrawRay(rayGridWorldRight, Vector3.up * rayDistance, Color.red);
                    }
                }
            }
        }

        // Terrain Deformation is affected by an increasing value of the contact area, therefore the deformation
        // will be defined by the maximum contact area in each frame
        oldAreaTotalLeft = ((counterHitsLeft) * areaCell);
        if (oldAreaTotalLeft >= areaTotalLeft)
        {
            // Area of contact
            areaTotalLeft = ((counterHitsLeft) * areaCell);

            // Volume under the foot for that recent calculated area
            volumeOriginalLeft = areaTotalLeft * (originalLength);
        }

        oldAreaTotalRight = ((counterHitsRight) * areaCell);
        if (oldAreaTotalRight >= areaTotalRight)
        {
            // Area of contact
            areaTotalRight = ((counterHitsRight) * areaCell);

            // Volume under the foot for that recent calculated area
            volumeOriginalRight = areaTotalRight * (originalLength);
        }

        // Total Area and Volume for both feet
        areaTotal = areaTotalLeft + areaTotalRight;

        //        Detecting Contour        //
        // =============================== //

        if (IsRightFootGrounded)
        {
            // We don't need to check the whole grid - just in the 5x5 inner grid is enough
            for (int zi = -gridSize + offsetBumpGrid; zi <= gridSize - offsetBumpGrid; zi++)
            {
                for (int xi = -gridSize + offsetBumpGrid; xi <= gridSize - offsetBumpGrid; xi++)
                {
                    // If the cell was not in contact, it's a potential neighbour (countour) cell
                    if (heightMapRightBool[zi + gridSize, xi + gridSize] == 0)
                    {
                        // Only checking adjacent cells - increasing this would allow increasing the area of the bump
                        for (int zi_sub = -neighbourSearchArea; zi_sub <= neighbourSearchArea; zi_sub++)
                        {
                            for (int xi_sub = -neighbourSearchArea; xi_sub <= neighbourSearchArea; xi_sub++)
                            {
                                if (heightMapRightBool[zi + zi_sub + gridSize, xi + xi_sub + gridSize] == 2)
                                {
                                    Vector3 rayGridRight      = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi) - offsetRay, zRight + zi);
                                    Vector3 rayGridWorldRight = terrain.Grid2World(rayGridRight);

                                    if (showGridBumpDebug)
                                    {
                                        Debug.DrawRay(rayGridWorldRight, Vector3.up * 0.2f, Color.yellow);
                                    }

                                    // Mark that cell as a countour point
                                    heightMapRightBool[zi + gridSize, xi + gridSize] = 1;
                                    break;
                                }
                            }
                        }
                    }
                }
            }
        }

        if (IsLeftFootGrounded)
        {
            // We don't need to check the whole grid - just in the 5x5 inner grid is enough
            for (int zi = -gridSize + offsetBumpGrid; zi <= gridSize - offsetBumpGrid; zi++)
            {
                for (int xi = -gridSize + offsetBumpGrid; xi <= gridSize - offsetBumpGrid; xi++)
                {
                    // If the cell was not in contact, it's a potential neighbour (countour) cell
                    if (heightMapLeftBool[zi + gridSize, xi + gridSize] == 0)
                    {
                        // Only checking adjacent cells - increasing this would allow increasing the area of the bump
                        for (int zi_sub = -neighbourSearchArea; zi_sub <= neighbourSearchArea; zi_sub++)
                        {
                            for (int xi_sub = -neighbourSearchArea; xi_sub <= neighbourSearchArea; xi_sub++)
                            {
                                if (heightMapLeftBool[zi + zi_sub + gridSize, xi + xi_sub + gridSize] == 2)
                                {
                                    Vector3 rayGridLeft      = new Vector3(xLeft + xi, terrain.Get(xLeft + xi, zLeft + zi) - offsetRay, zLeft + zi);
                                    Vector3 rayGridWorldLeft = terrain.Grid2World(rayGridLeft);

                                    if (showGridBumpDebug)
                                    {
                                        Debug.DrawRay(rayGridWorldLeft, Vector3.up * 0.2f, Color.yellow);
                                    }

                                    // Mark that cell as a countour point
                                    heightMapLeftBool[zi + gridSize, xi + gridSize] = 1;
                                    break;
                                }
                            }
                        }
                    }
                }
            }
        }

        // Calculating number of neightbouring hits to later get the area
        for (int zi = -gridSize + offsetBumpGrid; zi <= gridSize - offsetBumpGrid; zi++)
        {
            for (int xi = -gridSize + offsetBumpGrid; xi <= gridSize - offsetBumpGrid; xi++)
            {
                if (heightMapLeftBool[zi + gridSize, xi + gridSize] == 1)
                {
                    // Each neightbour cell in world space
                    Vector3 rayGridLeft      = new Vector3(xLeft + xi, terrain.Get(xLeft + xi, zLeft + zi) - offsetRay, zLeft + zi);
                    Vector3 rayGridWorldLeft = terrain.Grid2World(rayGridLeft);

                    // Position of the neighbour relative to the foot
                    Vector3 relativePos = rayGridWorldLeft - LeftFootCollider.transform.position;

                    // Check if is in front/back of the foot
                    if (Vector3.Dot(LeftFootCollider.transform.forward, relativePos) > 0.0f)
                    {
                        // Store the Vector3 positions in a dynamic array
                        neighboursPositionsLeftFront.Add(rayGridWorldLeft);

                        // TEST - 3 is contour in FRONT
                        heightMapLeftBool[zi + gridSize, xi + gridSize] = 3;

                        if (showGridBumpFrontBack)
                        {
                            Debug.DrawRay(LeftFootCollider.transform.position, relativePos, Color.red);
                        }
                    }
                    else
                    {
                        // Store the Vector3 positions in a dynamic array
                        neighboursPositionsLeftBack.Add(rayGridWorldLeft);

                        // TEST - 4 is contour in BACK
                        heightMapLeftBool[zi + gridSize, xi + gridSize] = 4;

                        if (showGridBumpFrontBack)
                        {
                            Debug.DrawRay(LeftFootCollider.transform.position, relativePos, Color.blue);
                        }
                    }

                    neighbourCellsLeft++;
                }

                if (heightMapRightBool[zi + gridSize, xi + gridSize] == 1)
                {
                    // Each neightbour cell in world space
                    Vector3 rayGridRight      = new Vector3(xRight + xi, terrain.Get(xRight + xi, zRight + zi) - offsetRay, zRight + zi);
                    Vector3 rayGridWorldRight = terrain.Grid2World(rayGridRight);

                    // Position of the neighbour relative to the foot
                    Vector3 relativePos = rayGridWorldRight - RightFootCollider.transform.position;

                    // Check if is in front/back of the foot
                    if (Vector3.Dot(RightFootCollider.transform.forward, relativePos) > 0.0f)
                    {
                        // Store the Vector3 positions in a dynamic array
                        neighboursPositionsRightFront.Add(rayGridWorldRight);

                        // TEST - 3 is contour in FRONT
                        heightMapRightBool[zi + gridSize, xi + gridSize] = 3;

                        if (showGridBumpFrontBack)
                        {
                            Debug.DrawRay(RightFootCollider.transform.position, relativePos, Color.red);
                        }
                    }
                    else
                    {
                        // Store the Vector3 positions in a dynamic array
                        neighboursPositionsRightBack.Add(rayGridWorldRight);

                        // TEST - 4 is contour in BACK
                        heightMapRightBool[zi + gridSize, xi + gridSize] = 4;

                        if (showGridBumpFrontBack)
                        {
                            Debug.DrawRay(RightFootCollider.transform.position, relativePos, Color.blue);
                        }
                    }

                    neighbourCellsRight++;
                }
            }
        }

        //

        // Calculate the neightbour area for each foot
        oldNeighbourAreaTotalLeft = ((neighbourCellsLeft) * areaCell);
        if (oldNeighbourAreaTotalLeft >= neighbourAreaTotalLeft)
        {
            // Area of bump - Not used yet - TODO
            neighbourAreaTotalLeft = ((neighbourCellsLeft) * areaCell);
        }

        oldNeighbourAreaTotalRight = ((neighbourCellsRight) * areaCell);
        if (oldNeighbourAreaTotalRight >= neighbourAreaTotalRight)
        {
            // Area of bump - Not used yet - TODO
            neighbourAreaTotalRight = ((neighbourCellsRight) * areaCell);
        }

        //       Physics Calculation       //
        // =============================== //

        // Calculate Pressure applicable per frame - if no contact, there is no pressure
        // The three values should be similar, since pressure is based on the contact area

        // Pressure by left feet
        if (counterHitsLeft == 0)
        {
            pressureStressLeft = 0f;
        }
        else
        {
            pressureStressLeft = (TotalForceLeftY) / areaTotalLeft;
        }

        // Pressure by right feet
        if (counterHitsRight == 0)
        {
            pressureStressRight = 0f;
        }
        else
        {
            pressureStressRight = (TotalForceRightY) / areaTotalRight;
        }

        // Total pressure
        if (counterHitsLeft == 0 || counterHitsRight == 0)
        {
            pressureStress = 0f;
        }
        else
        {
            pressureStress = (TotalForceY) / areaTotal;
        }


        //     Deformation Calculation     //
        // =============================== //

        // Given area, pressure and terrain parameters, we calculate the displacement on the terrain
        // The decrement will depend also on the ContactTime used to calculate the corresponding force
        // As for the area, we keep the maximum value

        oldHeightCellDisplacementYoungLeft = pressureStressLeft * (originalLength / (youngModulus));
        if (oldHeightCellDisplacementYoungLeft >= heightCellDisplacementYoungLeft)
        {
            // We use abs. value but for compression, the change in length is negative
            heightCellDisplacementYoungLeft = pressureStressLeft * (originalLength / youngModulus);

            // Resulting volume under the left foot after displacement
            volumeTotalLeft = areaTotalLeft * (originalLength - heightCellDisplacementYoungLeft);

            // TEST - Calculate the difference in volume, takes into account the compressibility and estimate volume up per neighbour cell
            volumeDifferenceLeft    = volumeTotalLeft - volumeOriginalLeft;
            volumeNetDifferenceLeft = volumeDifferenceLeft - volumeVariationPoissonLeft;
            volumeCellLeft          = volumeNetDifferenceLeft / neighbourCellsLeft;

            // 1. Calculate positive deformation for the contour based on the downward deformation and Poisson
            //newBumpHeightDeformationLeft = ((volumeTotalLeft - volumeVariationPoissonLeft) / areaTotalLeft) - originalLength;

            // 2. In this case, we do it with volume. Remember: must be negative for later.
            newBumpHeightDeformationLeft = volumeCellLeft / areaCell;
        }

        oldHeightCellDisplacementYoungRight = pressureStressRight * (originalLength / (youngModulus));
        if (oldHeightCellDisplacementYoungRight >= heightCellDisplacementYoungRight)
        {
            // We use abs. value but for compression, the change in length is negative
            heightCellDisplacementYoungRight = pressureStressRight * (originalLength / youngModulus);

            // Resulting volume under the right foot after displacement
            volumeTotalRight = areaTotalRight * (originalLength - heightCellDisplacementYoungRight);

            // TEST - Calculate the difference in volume, takes into account the compressibility and estimate volume up per neighbour cell
            volumeDifferenceRight    = volumeTotalRight - volumeOriginalRight;
            volumeNetDifferenceRight = volumeDifferenceRight - volumeVariationPoissonRight;
            volumeCellRight          = volumeNetDifferenceRight / neighbourCellsRight;

            // 1. Calculate positive deformation for the contour based on the downward deformation and Poisson
            //newBumpHeightDeformationRight = ((volumeTotalRight - volumeVariationPoissonRight) / areaTotalRight) - originalLength;

            // 2. In this case, we do it with volume. Remember: must be negative for later.
            newBumpHeightDeformationRight = volumeCellRight / areaCell;
        }

        // Given the entire deformation in Y, we calculate the corresponding frame-based deformation based on the frame-time.
        displacementLeft  = (Time.deltaTime * (float)heightCellDisplacementYoungLeft) / ContactTime;
        displacementRight = (Time.deltaTime * (float)heightCellDisplacementYoungRight) / ContactTime;

        if (useManualBumpDeformation)
        {
            newBumpHeightDeformationLeft  = -bumpHeightDeformation;
            newBumpHeightDeformationRight = -bumpHeightDeformation;
        }

        // Given the  deformation in Y for the bump, we calculate the corresponding frame-based deformation based on the frame-time.
        bumpDisplacementLeftBack   = (Time.deltaTime * (float)newBumpHeightDeformationLeft) / ContactTime;
        bumpDisplacementRightBack  = (Time.deltaTime * (float)newBumpHeightDeformationRight) / ContactTime;
        bumpDisplacementLeftFront  = (Time.deltaTime * (float)newBumpHeightDeformationLeft) / ContactTime;
        bumpDisplacementRightFront = (Time.deltaTime * (float)newBumpHeightDeformationRight) / ContactTime;

        //     Physics+ Calculation     //
        // =============================== //

        // Strains (compression) - Info
        strainLong  = -(heightCellDisplacementYoungRight) / originalLength;
        strainTrans = poissonRatio * strainLong;

        // TEST - If Poisson is 0.5 : ideal imcompressible material (no change in volume) - Compression : -/delta_L
        volumeVariationPoissonLeft  = (1 - 2 * poissonRatio) * (-heightCellDisplacementYoungLeft / originalLength) * volumeOriginalLeft;
        volumeVariationPoissonRight = (1 - 2 * poissonRatio) * (-heightCellDisplacementYoungRight / originalLength) * volumeOriginalRight;

        //        Apply Deformation        //
        // =============================== //

        // 2D iteration Deformation
        // Once we have the displacement, we saved the actual result of applying it to the terrain (only when the foot is grounded)
        if (IsLeftFootGrounded)
        {
            StartCoroutine(DecreaseTerrainLeft(heightMapLeft, heightMapLeftBool, xLeft, zLeft));
        }
        else if (!IsLeftFootGrounded)
        {
            // Every time we lift the foot, we reset the variables and stop the coroutines.
            heightCellDisplacementYoungLeft = 0;
            StopAllCoroutines();
        }

        if (IsRightFootGrounded)
        {
            StartCoroutine(DecreaseTerrainRight(heightMapRight, heightMapRightBool, xRight, zRight));
        }
        else if (!IsRightFootGrounded)
        {
            heightCellDisplacementYoungRight = 0;
            StopAllCoroutines();
        }

        //         Apply Smoothing         //
        // =============================== //

        // First smoothing version
        if (applyFilterLeft)
        {
            // Provisional: When do we smooth?
            if (IsLeftFootGrounded && !IsRightFootGrounded)
            {
                if (!isFilteredLeft)
                {
                    NewFilterHeightMap(xLeft, zLeft, heightMapLeft);
                    filterIterationsLeftCounter++;
                }

                if (filterIterationsLeftCounter >= filterIterationsLeftFoot)
                {
                    isFilteredLeft = true;
                }
            }
            else
            {
                isFilteredLeft = false;
                filterIterationsLeftCounter = 0;
            }
        }

        if (applyFilterRight)
        {
            // Provisional: When do we smooth?
            if (IsRightFootGrounded && !IsLeftFootGrounded)
            {
                if (!isFilteredRight)
                {
                    NewFilterHeightMap(xRight, zRight, heightMapRight);
                    filterIterationsRightCounter++;
                }

                if (filterIterationsRightCounter >= filterIterationsRightFoot)
                {
                    isFilteredRight = true;
                }
            }
            else
            {
                isFilteredRight = false;
                filterIterationsRightCounter = 0;
            }
        }
    }