IEnumerator DecreaseTerrain(float contactTime, float[,] heightMapPrimitive, int[,] heightMapPrimitiveBool, int x, int z)
{
// 1. Apply frame-per-frame deformation ("displacement")
for (int zi = -gridSensorSize; zi <= gridSensorSize; zi++)
{
for (int xi = -gridSensorSize; xi <= gridSensorSize; xi++)
{
// Calculate each cell position wrt World and Heightmap - Left Foot
Vector3 rayGridPrimitive = new Vector3(x + xi, terrain.Get(x + xi, z + zi), z + zi);
Vector3 rayGridWorldPrimitive = terrain.Grid2World(rayGridPrimitive);
// Create each ray for the grid (wrt World) - Left
RaycastHit primitiveHit;
Ray upRayPrimitive = new Ray(rayGridWorldPrimitive, Vector3.up);
// If hits the Left Foot, increase counter and add cell to be affected
if (PrimitiveCollider.Raycast(upRayPrimitive, out primitiveHit, rayPrimitiveDistanceUp))
{
// Cell contacting directly
heightMapPrimitiveBool[zi + gridSensorSize, xi + gridSensorSize] = 2;
heightMapPrimitive[zi + gridSensorSize, xi + gridSensorSize] = terrain.Get(rayGridPrimitive.x, rayGridPrimitive.z) - (displacement);
}
else
{
// No contact
heightMapPrimitiveBool[zi + gridSensorSize, xi + gridSensorSize] = 0;
heightMapPrimitive[zi + gridSensorSize, xi + gridSensorSize] = terrain.Get(rayGridPrimitive.x, rayGridPrimitive.z);
}
}
}
// 2. Pre-filter Gaussian filter
if (applyPreFilterPrimitive)
{
heightMapPrimitive = FilterBufferPrimitive(heightMapPrimitive, heightMapPrimitiveBool);
}
// 3. Save terrain
if (applyNewTerrainModification)
{
for (int zi = -gridSensorSize; zi <= gridSensorSize; zi++)
{
for (int xi = -gridSensorSize; xi <= gridSensorSize; xi++)
{
Vector3 rayGridPrimitive = new Vector3(x + xi, terrain.Get(x + xi, z + zi), z + zi);
terrain.Set(rayGridPrimitive.x, rayGridPrimitive.z, heightMapPrimitive[zi + gridSensorSize, xi + gridSensorSize]);
}
}
}
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 x, int z)
{
//=============//
// Reset counter hits
counterHitsPrimitive = 0;
// Clear cells to be affected
//cellsAffected.Clear();
// Primitive
float[,] heightMapPrimitive = new float[2 * gridSensorSize + 1, 2 * gridSensorSize + 1];
int[,] heightMapPrimitiveBool = new int[2 * gridSensorSize + 1, 2 * gridSensorSize + 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;
//=============//
// 2D iteration for primitive
// It counts the number of hits, save the classified cell in a list and debug ray-casting
for (int zi = -gridSensorSize; zi <= gridSensorSize; zi++)
{
for (int xi = -gridSensorSize; xi <= gridSensorSize; xi++)
{
// Calculate each cell position wrt World and Heightmap - Primitive
Vector3 rayGridPrimitive = new Vector3(x + xi, terrain.Get(x + xi, z + zi) - offset, z + zi);
Vector3 rayGridWorldPrimitive = terrain.Grid2World(rayGridPrimitive);
// Create each ray for the grid (wrt World) - Primitive
RaycastHit primitiveHit;
Ray upRayPrimitive = new Ray(rayGridWorldPrimitive, Vector3.up);
// If hits the Left Foot, increase counter and add cell to be affected
if (PrimitiveCollider.Raycast(upRayPrimitive, out primitiveHit, rayPrimitiveDistanceUp))
{
counterHitsPrimitive++;
if (showGridDebug)
{
Debug.DrawRay(rayGridWorldPrimitive, Vector3.up * rayPrimitiveDistanceUp, Color.blue);
}
}
else
{
//cellsNotAffected.Add(rayGridLeft);
if (showGridDebug)
{
Debug.DrawRay(rayGridWorldPrimitive, Vector3.up * rayPrimitiveDistanceUp, Color.red);
}
}
}
}
//=============//
// 1. TotalForce gives the total force applicable (gravity + reaction impulse)
//Debug.Log("Total Force now: " + TotalForce);
// 2. Total Contact Area in Frame.
// TODO: Keep always larger area - (?)
oldAreaTotal = ((counterHitsPrimitive) * areaCell);
if (oldAreaTotal >= areaTotal)
{
areaTotal = ((counterHitsPrimitive) * areaCell);
}
// 3. Calculate Pressure applicable per Frame - no contact, no pressure.
if (counterHitsPrimitive == 0)
{
pressure = 0f;
}
else
{
pressure = (TotalForce) / areaTotal;
}
// Plot
//pressurePlot = pressure;
//=============//
// 4. Given those cells, and pressure, which should be the decrement in the terrain?
// The decrement will depend entirely on the pressure and time.
// A. Test - Interpolated Value
//float normalizedValuePressure = Mathf.InverseLerp(0, 2000, pressure);
//heightCellDisplacement = Mathf.Lerp(0f, 0.1f, normalizedValuePressure);
// B. Test - Constant Value
//heightCellDisplacement = 0.015f;
// C. Young Modulus to calculate maximum displacement for a equivalent rod of cross-section "areaTotal" and length "originalLength"
// We keep the maximum displacement caused - (?)
oldHeightCellDisplacementYoung = pressure * (originalLength / (youngModulus));
if (oldHeightCellDisplacementYoung >= heightCellDisplacementYoung)
{
heightCellDisplacementYoung = pressure * (originalLength / youngModulus);
}
// Then, if the previous displacement is the total deformation, we need to calculate the small step
// that we do per frame until we reach the given "ContactTime".
displacement = (Time.deltaTime * (float)heightCellDisplacementYoung) / ContactTime;
//Debug.Log("For the pressure " + pressure + " we have a displacement/cell of " + displacement);
// Plot
//displacementPlot = (float)heightCellDisplacementYoung;
//=============//
//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
if (counterHitsPrimitive != 0)
{
// Once we touch the ground, clock starts. It will deform the terrain during delta_t.
// TODO: We need to reset times for the steps example later on.
timePassed += Time.deltaTime;
if (timePassed <= ContactTime)
{
StartCoroutine(DecreaseTerrain(ContactTime, heightMapPrimitive, heightMapPrimitiveBool, x, z));
}
}
}