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