public static void GetPoints(ConvexShape shape, out float volume, RawList<Vector3> outputPointContributions)
{
RigidTransform transform = RigidTransform.Identity;
BoundingBox boundingBox;
shape.GetBoundingBox(ref transform, out boundingBox);
//Find the direction which maximizes the possible hits. Generally, this is the smallest area axis.
//Possible options are:
//YZ -> use X
//XZ -> use Y
//XY -> use Z
Ray ray;
float width = boundingBox.Max.X - boundingBox.Min.X;
float height = boundingBox.Max.Y - boundingBox.Min.Y;
float length = boundingBox.Max.Z - boundingBox.Min.Z;
float yzArea = height * length;
float xzArea = width * length;
float xyArea = width * height;
Vector3 increment1, increment2;
float incrementMultiplier = 1f / NumberOfSamplesPerDimension;
float maxLength;
float rayIncrement;
if (yzArea > xzArea && yzArea > xyArea)
{
//use the x axis as the direction.
ray.Direction = Vector3.Right;
ray.Position = new Vector3(boundingBox.Min.X, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z + .5f * incrementMultiplier * length);
increment1 = new Vector3(0, incrementMultiplier * height, 0);
increment2 = new Vector3(0, 0, incrementMultiplier * length);
rayIncrement = incrementMultiplier * width;
maxLength = width;
}
else if (xzArea > xyArea) //yz is not the max, given by the previous if. Is xz or xy the max?
{
//use the y axis as the direction.
ray.Direction = Vector3.Up;
ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y, boundingBox.Min.Z + .5f * incrementMultiplier * length);
increment1 = new Vector3(incrementMultiplier * width, 0, 0);
increment2 = new Vector3(0, 0, incrementMultiplier * height);
rayIncrement = incrementMultiplier * height;
maxLength = height;
}
else
{
//use the z axis as the direction.
ray.Direction = Vector3.Backward;
ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z);
increment1 = new Vector3(incrementMultiplier * width, 0, 0);
increment2 = new Vector3(0, incrementMultiplier * height, 0);
rayIncrement = incrementMultiplier * length;
maxLength = length;
}
Ray oppositeRay;
volume = 0;
for (int i = 0; i < NumberOfSamplesPerDimension; i++)
{
for (int j = 0; j < NumberOfSamplesPerDimension; j++)
{
//Ray cast from one direction. If it succeeds, try the other way. This forms an interval in which inertia tensor contributions are contained.
RayHit hit;
if (shape.RayTest(ref ray, ref transform, maxLength, out hit))
{
Vector3.Multiply(ref ray.Direction, maxLength, out oppositeRay.Position);
Vector3.Add(ref oppositeRay.Position, ref ray.Position, out oppositeRay.Position);
Vector3.Negate(ref ray.Direction, out oppositeRay.Direction);
RayHit oppositeHit;
if (shape.RayTest(ref oppositeRay, ref transform, maxLength, out oppositeHit))
{
//It should always get here if one direction casts, but there may be numerical issues.
float scanVolume;
ScanObject(rayIncrement, maxLength, ref increment1, ref increment2, ref ray, ref hit, ref oppositeHit, outputPointContributions, out scanVolume);
volume += scanVolume;
}
}
Vector3.Add(ref ray.Position, ref increment2, out ray.Position);
}
Vector3.Add(ref ray.Position, ref increment1, out ray.Position);
//Move the ray back to the starting position along the other axis.
Vector3 subtract;
Vector3.Multiply(ref increment2, NumberOfSamplesPerDimension, out subtract);
Vector3.Subtract(ref ray.Position, ref subtract, out ray.Position);
}
}
