public override void Transform(Vector3 scale, Quaternion rotate, Vector3 translate)
{
if (!IsUniformScale(scale))
{
throw new Exception("Unsupported non-uniform scale on Capsule");
}
Matrix4 transform = Matrix4.Identity;
transform.Scale = scale;
transform = rotate.ToRotationMatrix() * transform;
transform.Translation = translate;
bottomcenter = transform * bottomcenter;
topcenter = transform * topcenter;
capRadius *= scale.x;
this.height = (topcenter - bottomcenter).Length;
this.center = (topcenter + bottomcenter) * 0.5f;
this.radius = height * 0.5f + capRadius;
}
public override void Transform(Vector3 scale, Quaternion rotate, Vector3 translate)
{
if (!IsUniformScale(scale))
{
throw new Exception("Unsupported non-uniform scale on OBB");
}
Matrix4 transform = Matrix4.Identity;
transform.Scale = scale;
transform = rotate.ToRotationMatrix() * transform;
transform.Translation = translate;
this.center = transform * center;
for (int i = 0; i < this.axes.Length; ++i)
{
this.axes[i] = rotate * axes[i];
}
this.extents = scale.x * extents;
this.radius = extents.Length;
}
public override void Transform(Vector3 scale, Quaternion rotate, Vector3 translate)
{
if (rotate != Quaternion.Identity)
{
throw new Exception("Unsupported non-identity rotation on AABB");
}
Matrix4 transform = Matrix4.Identity;
transform.Scale = scale;
transform = rotate.ToRotationMatrix() * transform;
transform.Translation = translate;
float s = 0.0f;
this.min = transform * min;
this.max = transform * max;
for (int i = 0; i < 3; i++)
{
this.center[i] = (min[i] + max[i]) / 2;
s += ((max[i] - min[i]) * (max[i] - min[i]));
}
this.radius = (float)Math.Sqrt((double)(s / 4));
}
public PathGenerator(bool logPathGeneration, string modelName, PathObjectType poType, float terrainLevel,
Matrix4 modelTransform, List<CollisionShape> collisionVolumes)
{
this.dumpGrid = logPathGeneration;
this.modelName = modelName;
this.poType = poType;
this.modelWidth = poType.width * oneMeter;
this.cellWidth = poType.gridResolution * modelWidth;
this.boxHeight = poType.height * oneMeter;
this.maxClimbDistance = poType.maxClimbSlope * cellWidth;
this.maxDisjointDistance = poType.maxDisjointDistance * oneMeter;
this.minimumFeatureSize = poType.minimumFeatureSize;
this.terrainLevel = terrainLevel;
this.modelTransform = modelTransform;
this.collisionVolumes = collisionVolumes;
stopwatch = new Stopwatch();
stopwatch.Start();
// Create the collisionAPI object
collisionAPI = new CollisionAPI(false);
// Ugly workaround for a modularity problem do to
// unadvised use of static variables: remember the
// existing state of rendering collision volumes
RenderedNode.RenderState oldRenderState = RenderedNode.renderState;
RenderedNode.renderState = RenderedNode.RenderState.None;
// Form the union of the collision volumes; we don't care
// about Y coordinate, only the X and Z coordinates
Vector3 min = Vector3.Zero;
Vector3 max = Vector3.Zero;
for (int i=0; i<collisionVolumes.Count; i++) {
CollisionShape shape = collisionVolumes[i];
// Add the shape to the sphere tree
collisionAPI.SphereTree.AddCollisionShape(shape);
AxisAlignedBox vol = shape.BoundingBox();
// If this is the first iteration, set up the min and
// max
if (i == 0) {
min = vol.Minimum;
max = vol.Maximum;
min.y = terrainLevel;
max.y = terrainLevel;
continue;
}
// Enlarge the box by the dimensions of the shape
min.x = Math.Min(min.x, vol.Minimum.x);
min.z = Math.Min(min.z, vol.Minimum.z);
max.x = Math.Max(max.x, vol.Maximum.x);
max.z = Math.Max(max.z, vol.Maximum.z);
}
// Restore RenderState
RenderedNode.renderState = oldRenderState;
// Round out the max and min by 4 times the grid
// resolution, so we can just divide to find the
// horizontal and vertical numbers are cells
Vector3 margin = Vector3.UnitScale * 4 * cellWidth;
min -= margin;
max += margin;
// Now adjust the max the min coords so that they are
// exactly a multiple of the grid resolution
min.x = MultipleOfResolution(min.x);
min.z = MultipleOfResolution(min.z);
max.x = MultipleOfResolution(max.x);
max.z = MultipleOfResolution(max.z);
// Set the lower left and upper right corners
lowerLeftCorner = min;
upperRightCorner = max;
// Set the horizontal and vertical counts
xCount = (int)((max.x - min.x) / cellWidth);
zCount = (int)((max.z - min.z) / cellWidth);
// Initial the gridBox
gridBox = new AxisAlignedBox(min, max);
// Allocate the grid
grid = new List<GridCell>[xCount, zCount];
for (int i=0; i<xCount; i++)
for (int j=0; j<zCount; j++)
grid[i,j] = new List<GridCell>();
// Initialize the work list, adding the cell at 0,0 and
// the terrain height as the first member
workList = new List<CellLocation>();
workList.Add(new CellLocation(0, 0, terrainLevel, null));
// Create the moving box at the center of the 0, 0 cell,
// and the MovingObject object that contains the moving box
movingBox = new CollisionAABB(
new Vector3(min.x, terrainLevel, min.z),
new Vector3(min.x + cellWidth, terrainLevel + boxHeight, min.z + terrainLevel));
movingObject = new MovingObject(collisionAPI);
movingObject.AddPart(movingBox);
}
