public void InnerPoint()
{
var aabb = _field.GetAabb(Pose.Identity);
Assert.IsTrue(GeometryHelper.HaveContact(aabb, _field.InnerPoint));
Assert.IsTrue(_field.InnerPoint.Y < _field.GetHeight(_field.InnerPoint.X, _field.InnerPoint.Z));
}
/// <summary>
/// Clamps a road path to the terrain height.
/// </summary>
/// <param name="road">The path that represents the road.</param>
/// <param name="terrain">The terrain represented by a <see cref="HeightField"/>.</param>
/// <remarks>
/// The y position of each path key is set to the terrain height at the xz position.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="terrain"/> is <see langword="null"/>.
/// </exception>
public static void ClampRoadToTerrain(Path3F road, HeightField terrain)
{
if (road == null)
{
return;
}
if (terrain == null)
{
throw new ArgumentNullException("terrain");
}
foreach (var key in road)
{
Vector3F position = key.Point;
float height = terrain.GetHeight(position.X, position.Z);
if (!Numeric.IsNaN(height))
{
position.Y = height;
key.Point = position;
}
}
}
public void GetHeight()
{
Assert.AreEqual(-5, _heightField.GetHeight(2.0f, 3.0f));
}
/// <summary>
/// Guesses the closest pair.
/// </summary>
/// <param name="contactSet">The contact set.</param>
/// <param name="swapped">
/// Object A in <paramref name="contactSet"/> should be the height field. This parameter
/// indicates whether object A and object B in the contact set are swapped.
/// </param>
/// <param name="isOverHole">
/// <see langword="true"/> if the guessed contact is over a hole and probably shouldn't be used.
/// </param>
/// <returns>Guess for closest pair.</returns>
/// <remarks>
/// For general shapes: Inner point of B to height field point "under" inner point of B.
/// For convex shapes: Support point of B in the "down" direction to height field point "under"
/// this support point.
/// </remarks>
private static Contact GuessClosestPair(ContactSet contactSet, bool swapped, out bool isOverHole)
{
// Object A should be the height field.
IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
// Swap if necessary.
if (swapped)
{
MathHelper.Swap(ref objectA, ref objectB);
}
HeightField heightFieldA = (HeightField)objectA.Shape;
Shape shapeB = objectB.Shape;
Vector3 scaleA = objectA.Scale;
Vector3 scaleB = objectB.Scale;
Pose poseA = objectA.Pose;
Pose poseB = objectB.Pose;
// Get the height field up-axis in world space.
Vector3 heightFieldUpAxis = poseA.ToWorldDirection(Vector3.UnitY);
// Get point on other object.
Vector3 positionBLocal;
ConvexShape shapeBAsConvex = shapeB as ConvexShape;
if (shapeBAsConvex != null)
{
// Use support point for convex shapes.
positionBLocal = shapeBAsConvex.GetSupportPoint(poseB.ToLocalDirection(-heightFieldUpAxis), scaleB);
}
else
{
// Use inner point for general shapes.
positionBLocal = shapeB.InnerPoint * scaleB;
}
// Convert point (object B) into height field space (object A).
Vector3 positionB = poseB.ToWorldPosition(positionBLocal);
Vector3 positionBInA = poseA.ToLocalPosition(positionB);
// Get point on the surface of the height field (object A):
// Clamp x and z coordinate to height field widths.
// For y coordinate get the height of the height field at the x-z position.
float originX = heightFieldA.OriginX;
float originZ = heightFieldA.OriginZ;
float x = MathHelper.Clamp(positionBInA.X, originX * scaleA.X, (originX + heightFieldA.WidthX) * scaleA.X);
float z = MathHelper.Clamp(positionBInA.Z, originZ * scaleA.Z, (originZ + heightFieldA.WidthZ) * scaleA.Z);
float y = heightFieldA.GetHeight(x / scaleA.X, z / scaleA.Z) * scaleA.Y; // Inverse scale applied in GetHeight() parameters.
Vector3 positionALocal = new Vector3(x, y, z);
// Special handling of holes.
isOverHole = Numeric.IsNaN(y);
if (isOverHole)
{
positionALocal = heightFieldA.InnerPoint * scaleA;
}
// Convert point on height field to world space.
Vector3 positionA = poseA.ToWorldPosition(positionALocal);
// Use the world positions (positionA, positionB) as our closest-pair/contact guess.
// Compute contact information.
Vector3 position = (positionA + positionB) / 2;
float penetrationDepth = (positionA - positionB).Length;
bool haveContact = (positionALocal.Y >= positionBInA.Y);
Vector3 normal = positionA - positionB;
if (!normal.TryNormalize())
{
normal = heightFieldUpAxis;
}
if (swapped)
{
normal = -normal;
}
bool isRayHit = haveContact && shapeB is RayShape;
if (!haveContact)
{
// For separation: Switch the normal and make the penetration depth negative to indicate
// separation.
normal = -normal;
penetrationDepth = -penetrationDepth;
}
return(ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, isRayHit));
}
