/// <summary>
/// Initializes a new instance of the <see cref="ScaledConvexShape"/> class from two geometric
/// objects.
/// </summary>
/// <param name="shape">The convex shape that should be scaled.</param>
/// <param name="scale">The scale of the convex shape.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="shape"/> is <see langword="null"/>.
/// </exception>
public ScaledConvexShape(ConvexShape shape, Vector3F scale)
{
if (shape == null)
throw new ArgumentNullException("shape");
// Note: Virtual OnChanged() must not be called in constructor.
_shape = shape;
_shape.Changed += OnChildShapeChanged;
_scale = scale;
}
internal static IList<Vector3F> SampleConvexShape(ConvexShape shape, float distanceThreshold, int iterationLimit)
{
if (shape == null)
throw new ArgumentNullException("shape");
if (distanceThreshold < 0)
throw new ArgumentOutOfRangeException("distanceThreshold", "distanceThreshold must be greater than or equal to 0.");
if (iterationLimit < 1)
throw new ArgumentOutOfRangeException("iterationLimit", "iterationLimit must be greater than or equal to 1.");
distanceThreshold = Math.Max(distanceThreshold, Numeric.EpsilonF);
PointCollector points = new PointCollector { DistanceThreshold = distanceThreshold };
// Start with 6 sensors and 8 sensor triangles (like a double pyramid).
List<Sensor> sensors = new List<Sensor>
{
new Sensor(new Vector3F(-1, 0, 0)),
new Sensor(new Vector3F(1, 0, 0)),
new Sensor(new Vector3F(0, -1, 0)),
new Sensor(new Vector3F(0, 1, 0)),
new Sensor(new Vector3F(0, 0, -1)),
new Sensor(new Vector3F(0, 0, 1))
};
List<SensorTriangle> triangles = new List<SensorTriangle>
{
new SensorTriangle(5, 1, 3, 1), // SensorTriangle.Iteration is set to 1.
new SensorTriangle(5, 2, 1, 1),
new SensorTriangle(1, 2, 4, 1),
new SensorTriangle(4, 2, 0, 1),
new SensorTriangle(0, 2, 5, 1),
new SensorTriangle(1, 4, 3, 1),
new SensorTriangle(3, 4, 0, 1),
new SensorTriangle(0, 5, 3, 1)
};
// Sample the first 6 sensors.
int numberOfSensors = sensors.Count;
for (int i = 0; i < numberOfSensors; i++)
{
Sensor sensor = sensors[i];
Vector3F sample = shape.GetSupportPoint(sensor.Direction);
int index = points.GetIndex(sample);
if (index == -1)
{
// Add new point.
index = points.Points.Count;
points.Points.Add(sample);
}
sensor.IndexOfPoint = index;
}
// Collector for new points.
PointCollector newPoints = new PointCollector();
bool finished = false;
while (!finished)
{
// Assume that we cannot refine. If we can refine, finished must be set to false.
finished = true;
// Reset collector for new points.
newPoints.Points.Clear();
int numberOfPoints = points.Points.Count;
int numberOfTriangles = triangles.Count;
for (int currentTriangle = 0; currentTriangle < numberOfTriangles; currentTriangle++)
{
// In each iteration refine the existing triangles by breaking up edges.
SensorTriangle triangle = triangles[currentTriangle];
int iteration = triangle.Iteration;
if (iteration >= 0 && iteration < iterationLimit)
{
// Sensor indices.
int indexOfSensor0 = triangle.IndexOfSensor0;
int indexOfSensor1 = triangle.IndexOfSensor1;
int indexOfSensor2 = triangle.IndexOfSensor2;
// Sensors.
Sensor sensor0 = sensors[indexOfSensor0];
Sensor sensor1 = sensors[indexOfSensor1];
Sensor sensor2 = sensors[indexOfSensor2];
// New sensors.
Sensor sensor01 = new Sensor((sensor0.Direction + sensor1.Direction).Normalized);
Sensor sensor12 = new Sensor((sensor1.Direction + sensor2.Direction).Normalized);
Sensor sensor20 = new Sensor((sensor2.Direction + sensor0.Direction).Normalized);
// New Support Points.
Vector3F v01 = shape.GetSupportPoint(sensor01.Direction);
Vector3F v12 = shape.GetSupportPoint(sensor12.Direction);
Vector3F v20 = shape.GetSupportPoint(sensor20.Direction);
// See if the points are new.
sensor01.IndexOfPoint = points.GetIndex(v01);
sensor12.IndexOfPoint = points.GetIndex(v12);
sensor20.IndexOfPoint = points.GetIndex(v20);
// Check if at least one point is new, and if we have improved more than the distance threshold.
// (The PointCollector.GetIndex() automatically takes care of the distance threshold.)
if (sensor01.IndexOfPoint == -1 || sensor12.IndexOfPoint == -1 || sensor20.IndexOfPoint == -1)
{
// We refine the sensor triangle into 4 triangles.
finished = false;
// Indices of the new sensors.
int indexOfSensor01 = sensors.Count;
int indexOfSensor12 = indexOfSensor01 + 1;
int indexOfSensor20 = indexOfSensor12 + 1;
// Add sensors.
sensors.Add(sensor01);
sensors.Add(sensor12);
sensors.Add(sensor20);
// Use the instance of the old triangle for the first new triangle.
triangle.IndexOfSensor0 = indexOfSensor01;
triangle.IndexOfSensor1 = indexOfSensor12;
triangle.IndexOfSensor2 = indexOfSensor20;
iteration++;
triangle.Iteration = iteration;
// Add other triangles, but only if they contain a new point.
if (sensor01.IndexOfPoint == -1 || sensor12.IndexOfPoint == -1)
triangles.Add(new SensorTriangle(indexOfSensor01, indexOfSensor1, indexOfSensor12, iteration));
if (sensor12.IndexOfPoint == -1 || sensor20.IndexOfPoint == -1)
triangles.Add(new SensorTriangle(indexOfSensor12, indexOfSensor2, indexOfSensor20, iteration));
if (sensor01.IndexOfPoint == -1 || sensor20.IndexOfPoint == -1)
triangles.Add(new SensorTriangle(indexOfSensor0, indexOfSensor01, indexOfSensor20, iteration));
// Add new points
if (sensor01.IndexOfPoint == -1)
{
int newPointsIndex = newPoints.GetIndex(v01);
if (newPointsIndex == -1)
{
newPointsIndex = newPoints.Points.Count;
newPoints.Points.Add(v01);
}
sensor01.IndexOfPoint = numberOfPoints + newPointsIndex;
}
if (sensor12.IndexOfPoint == -1)
{
int newPointsIndex = newPoints.GetIndex(v12);
if (newPointsIndex == -1)
{
newPointsIndex = newPoints.Points.Count;
newPoints.Points.Add(v12);
}
sensor12.IndexOfPoint = numberOfPoints + newPointsIndex;
}
if (sensor20.IndexOfPoint == -1)
{
int newPointsIndex = newPoints.GetIndex(v20);
if (newPointsIndex == -1)
{
newPointsIndex = newPoints.Points.Count;
newPoints.Points.Add(v20);
}
sensor20.IndexOfPoint = numberOfPoints + newPointsIndex;
}
}
else
{
// Fine enough.
triangle.Iteration = -1;
}
}
}
// Copy new points to point collector.
int numberOfNewPoints = newPoints.Points.Count;
for (int i = 0; i < numberOfNewPoints; i++)
points.Points.Add(newPoints.Points[i]);
}
return points.Points;
}
//--------------------------------------------------------------
#region Creation and Cleanup
//--------------------------------------------------------------
/// <overloads>
/// <summary>
/// Initializes a new instance of the <see cref="ScaledConvexShape"/> class.
/// </summary>
/// </overloads>
///
/// <summary>
/// Initializes a new instance of the <see cref="ScaledConvexShape"/> class.
/// </summary>
public ScaledConvexShape()
{
// Note: Virtual OnChanged() must not be called in constructor.
_shape = new PointShape();
_shape.Changed += OnChildShapeChanged;
_scale = Vector3F.One;
}
private void ComputeCollisionFast(ContactSet contactSet, CollisionQueryType type,
IGeometricObject heightFieldGeometricObject, IGeometricObject convexGeometricObject,
HeightField heightField, ConvexShape convex, bool swapped)
{
Debug.Assert(type != CollisionQueryType.ClosestPoints);
// Assume no contact.
contactSet.HaveContact = false;
// Get scales and poses
Pose heightFieldPose = heightFieldGeometricObject.Pose;
Vector3F heightFieldScale = heightFieldGeometricObject.Scale;
if (heightFieldScale.X < 0 || heightFieldScale.Y < 0 || heightFieldScale.Z < 0)
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
Pose convexPose = convexGeometricObject.Pose;
Vector3F convexScale = convexGeometricObject.Scale;
// Get a point in the convex. (Could also use center of AABB.)
var convexPoint = convexPose.ToWorldPosition(convex.InnerPoint * convexScale);
// Get height field coordinates.
convexPoint = heightFieldPose.ToLocalPosition(convexPoint);
float xUnscaled = convexPoint.X / heightFieldScale.X;
float zUnscaled = convexPoint.Z / heightFieldScale.Z;
// If convex point is outside height field, abort.
var originX = heightField.OriginX;
var originZ = heightField.OriginZ;
if (xUnscaled < originX || xUnscaled > originX + heightField.WidthX
|| zUnscaled < originZ || zUnscaled > originZ + heightField.WidthZ)
{
return;
}
// Get height and normal.
float height;
Vector3F normal;
int featureIndex;
GetHeight(heightField, xUnscaled, zUnscaled, out height, out normal, out featureIndex);
// Check for holes.
if (Numeric.IsNaN(height))
return;
// Apply scaling.
height *= heightFieldScale.Y;
// Normals are transformed with the inverse transposed matrix --> 1 / scale.
normal = normal / heightFieldScale;
normal.Normalize();
// ----- Now we test convex vs. plane.
// Convert normal to convex space.
normal = heightFieldPose.ToWorldDirection(normal);
var normalInConvex = convexPose.ToLocalDirection(normal);
// Convert plane point to convex space.
Vector3F planePoint = new Vector3F(convexPoint.X, height, convexPoint.Z);
planePoint = heightFieldPose.ToWorldPosition(planePoint);
planePoint = convexPose.ToLocalPosition(planePoint);
// Get convex support point in plane normal direction.
Vector3F supportPoint = convex.GetSupportPoint(-normalInConvex, convexScale);
// Get penetration depth.
float penetrationDepth = Vector3F.Dot((planePoint - supportPoint), normalInConvex);
// Abort if there is no contact.
if (penetrationDepth < 0)
return;
// Abort if object is too deep under the height field.
// This is important for height fields with holes/caves. Without this check
// no objects could enter the cave.
if (penetrationDepth > heightField.Depth)
return;
// We have contact.
contactSet.HaveContact = true;
// Return for boolean queries.
if (type == CollisionQueryType.Boolean)
return;
// Contact position is in the "middle of the penetration".
Vector3F position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2));
if (swapped)
normal = -normal;
// Add contact
var contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
if (swapped)
contact.FeatureB = featureIndex;
else
contact.FeatureA = featureIndex;
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
if (CollisionDetection.FullContactSetPerFrame
&& contactSet.Count < 3)
{
// Trying to create a full contact set.
// We use arbitrary orthonormal values to perturb the normal direction.
var ortho1 = normalInConvex.Orthonormal1;
var ortho2 = normalInConvex.Orthonormal2;
// Test 4 perturbed support directions.
for (int i = 0; i < 4; i++)
{
Vector3F direction;
switch (i)
{
case 0:
direction = -normalInConvex + ortho1;
break;
case 1:
direction = -normalInConvex - ortho1;
break;
case 2:
direction = -normalInConvex + ortho2;
break;
default:
direction = -normalInConvex - ortho2;
break;
}
// Support point vs. plane test as above:
supportPoint = convex.GetSupportPoint(direction, convexScale);
penetrationDepth = Vector3F.Dot((planePoint - supportPoint), normalInConvex);
if (penetrationDepth >= 0)
{
position = convexPose.ToWorldPosition(supportPoint + normalInConvex * (penetrationDepth / 2));
contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
}
}
