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>Adds or adjusts a primitive collider on the mesh.</summary>
/// <remarks>
/// Type of the primitive collider is chosen basing on the primitive type.
/// </remarks>
/// <param name="primitive">Primitive game object to adjust.</param>
/// <param name="meshSize">Size of the collider in local units.</param>
/// <param name="colliderType">Determines how a collider type should be selected.</param>
/// <param name="shapeType">
/// Type of the primitive when <paramref name="colliderType"/> is
/// <see cref="PrimitiveCollider.Shape"/>. It will determine the type of the collider. Only
/// <see cref="PrimitiveType.Cylinder"/>, <see cref="PrimitiveType.Sphere"/>, and
/// <see cref="PrimitiveType.Cube"/> are supported.
/// </param>
public static void AdjustCollider(
GameObject primitive, Vector3 meshSize, PrimitiveCollider colliderType,
PrimitiveType?shapeType = null)
{
UnityEngine.Object.Destroy(primitive.GetComponent <Collider>());
if (colliderType == PrimitiveCollider.Mesh)
{
var collider = primitive.AddComponent <MeshCollider>();
collider.convex = true;
}
else if (colliderType == PrimitiveCollider.Shape)
{
// FIXME: non tirival scales does't fit simple colliders. Fix it.
if (shapeType.Value == PrimitiveType.Cylinder)
{
// TODO(ihsoft): Choose direction so what the volume is minimized.
var collider = primitive.AddComponent <CapsuleCollider>();
collider.direction = 2; // Z axis
collider.height = meshSize.z; // It's now length.
collider.radius = meshSize.x;
}
else if (shapeType.Value == PrimitiveType.Sphere)
{
var collider = primitive.AddComponent <SphereCollider>();
collider.radius = meshSize.x;
}
else if (shapeType.Value == PrimitiveType.Cube)
{
var collider = primitive.AddComponent <BoxCollider>();
collider.size = meshSize;
}
else
{
DebugEx.Warning("Unknown primitive type {0}. Droppping collider.", shapeType.Value);
}
}
else if (colliderType == PrimitiveCollider.Bounds)
{
SetSimpleCollider(primitive, PrimitiveType.Cube, inscribeBoundaryIntoCollider: true);
}
else if (colliderType != PrimitiveCollider.None)
{
DebugEx.Warning(
"Unsupported collider type {0}. Droppping whatever collider part had", colliderType);
}
}
/// <summary>Adds or adjusts a primitive collider on the mesh.</summary>
/// <remarks>
/// Type of the primitive collider is chosen basing on the primitive type.
/// </remarks>
/// <param name="primitive">The primitive game object to adjust.</param>
/// <param name="meshSize">
/// The size of the collider in local units. Depending on <paramref name="shapeType"/> the meaning
/// of the components is different. If the shape has a "round" component, then it's a "diameter"
/// in this vector.
/// </param>
/// <param name="colliderType">Determines how a collider type should be selected.</param>
/// <param name="shapeType">
/// The type of the primitive when <paramref name="colliderType"/> is
/// <see cref="PrimitiveCollider.Shape"/>. It will determine the type of the collider. Only
/// <see cref="PrimitiveType.Cylinder"/>, <see cref="PrimitiveType.Sphere"/>, and
/// <see cref="PrimitiveType.Cube"/> are supported.
/// </param>
/// <returns>The adjusted collider.</returns>
/// FIXME: it's not working with asymmetric meshes
/// FIXME: use GameObject.GetRendererBounds?
public static Collider AdjustCollider(
GameObject primitive, Vector3 meshSize, PrimitiveCollider colliderType,
PrimitiveType?shapeType = null)
{
if (colliderType == PrimitiveCollider.None)
{
SafeDestroy(GetActiveCollider <Collider>(primitive));
return(null);
}
if (colliderType == PrimitiveCollider.Mesh)
{
var collider = GetColliderOfType <MeshCollider>(primitive);
collider.convex = true;
return(collider);
}
if (colliderType == PrimitiveCollider.Shape)
{
switch (shapeType)
{
// FIXME: non trivial scales doesn't fit simple colliders. Fix it.
case null:
SafeDestroy(GetActiveCollider <Collider>(primitive));
DebugEx.Warning("Primitive type not set. Dropping collider.");
return(null);
case PrimitiveType.Cylinder: {
// TODO(ihsoft): Choose direction so what the volume is minimized.
var collider = GetColliderOfType <CapsuleCollider>(primitive);
collider.direction = 2; // Z axis
collider.height = meshSize.z; // It's now length.
collider.radius = meshSize.x / 2.0f;
return(collider);
}
case PrimitiveType.Sphere: {
var collider = GetColliderOfType <SphereCollider>(primitive);
collider.radius = meshSize.x / 2.0f;
return(collider);
}
case PrimitiveType.Cube: {
var collider = GetColliderOfType <BoxCollider>(primitive);
collider.size = meshSize;
return(collider);
}
default:
SafeDestroy(GetActiveCollider <Collider>(primitive));
DebugEx.Warning("Unknown primitive type {0}. Dropping collider.", shapeType.Value);
return(null);
}
}
if (colliderType == PrimitiveCollider.Bounds)
{
return(SetSimpleCollider(primitive, PrimitiveType.Cube, inscribeBoundaryIntoCollider: true));
}
SafeDestroy(GetActiveCollider <Collider>(primitive));
DebugEx.Warning(
"Unsupported collider type {0}. Dropping whatever collider part had", colliderType);
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));
}
}
}
