private static void TraceMOBottom(MovingObject mo, string description)
{
if (MO.DoLog) {
CollisionShape obstacle = mo.parts[0].shape;
CollisionCapsule moCapsule = null;
if (obstacle is CollisionCapsule)
moCapsule = (CollisionCapsule)obstacle;
string rest = (moCapsule != null ?
string.Format("mo bottom is {0}",
moCapsule.bottomcenter - new Vector3(0f, moCapsule.capRadius, 0f)) :
string.Format("obstacle {0}", obstacle));
MO.Log("{0}, {1}", description, rest);
}
}
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);
}
// Decide, based on the normal to the collision object, if the
// moving object can slide across the obstacle, and if it can,
// return the updated displacement. This displacement may in fact
// run into _another_ obstacle, however, so the call must again
// run the collision test.
private static bool DecideToSlide(MovingObject mo, Vector3 mobNodePosition,
CollisionParms parms, ref Vector3 displacement)
{
Vector3 normDisplacement = displacement.ToNormalized();
Vector3 normObstacle = parms.normObstacle.ToNormalized();
if (MO.DoLog) {
MO.Log(" DecideToSlide: normObstacle {0}, normDisplacement {1}",
normObstacle.ToString(), normDisplacement.ToString());
MO.Log(" DecideToSlide: displacement {0}", displacement);
}
// First we find the angle between the normal and the
// direction of travel, and reject the displacement if
// it's too small
float slideAngle = (NormalizeAngle((float)Math.Acos((double)normDisplacement.Dot(normObstacle))) -
.5f * (float)Math.PI);
if (Math.Abs(slideAngle) > CollisionAPI.MinSlideAngle) {
if (MO.DoLog)
MO.Log(" After collision, displacement {0}, won't slide because slideAngle {1} > minSlideAngle {2}",
displacement.ToString(), slideAngle, CollisionAPI.MinSlideAngle);
displacement = Vector3.Zero;
return false;
}
// Then we find the angle with the y axis, and reject the
// displacement if it's too steep
float verticalAngle = NormalizeAngle((float)Math.Acos((double)normDisplacement[1]));
if (Math.Abs(verticalAngle) > CollisionAPI.MaxVerticalAngle) {
if (MO.DoLog)
MO.Log(" After collision, displacement {0}, won't slide because verticalAngle {1} <= maxVerticalAngle {2}",
displacement.ToString(), verticalAngle, CollisionAPI.MaxVerticalAngle);
displacement = Vector3.Zero;
return false;
}
// Else, we can slide, so return a displacement that
// points in the direction we're sliding, and has length
// equal to a constant times the displacement length
// Rotate displacement so that it's 90 degress from the
// obstacle normal
Vector3 cross = normObstacle.Cross(normDisplacement);
Quaternion q = Quaternion.FromAngleAxis(.5f * (float)Math.PI, cross);
Matrix4 transform = q.ToRotationMatrix();
Vector3 transformedNorm = transform * normObstacle.ToNormalized();
float len = displacement.Length;
displacement = transformedNorm * len;
// Vector3 alignedPart = normObstacle * (normObstacle.Dot(displacement));
// displacement -= alignedPart;
Vector3 p = mobNodePosition + displacement;
float h = worldManager.GetHeightAt(p);
// If sliding would put us below ground, limit the displacement
if (h > p.y) {
if (MO.DoLog)
MO.Log(" Sliding up because terrain height is {0} is higher than projected mobNode height {1}",
h, p.y);
displacement.y += h - p.y;
}
if (MO.DoLog) {
MO.Log(" Exiting DecideToSlide, sliding displacement {0}, slideAngle {1}, verticalAngle {2}",
displacement.ToString(), slideAngle, verticalAngle);
MO.Log(" Exiting DecideToSlide, cross product {0}, quaternion {1}, transformedNorm {2}",
cross, q, transformedNorm);
// MO.Log(" Exiting DecideToSlide, alignedPart {0}", alignedPart);
}
return true;
}
private static bool NowColliding(MovingObject mo, string description)
{
CollisionParms parms = new CollisionParms();
bool colliding = collisionManager.CollideAfterAddingDisplacement(mo, Vector3.Zero, parms);
CollisionShape obstacle = mo.parts[0].shape;
CollisionCapsule moCapsule = null;
if (obstacle is CollisionCapsule)
moCapsule = (CollisionCapsule)obstacle;
string rest = (moCapsule != null ?
string.Format("mo bottom is {0}",
moCapsule.bottomcenter - new Vector3(0f, moCapsule.capRadius, 0f)) :
string.Format("obstacle {0}", obstacle));
if (MO.DoLog)
MO.Log("{0}, now colliding = {1}; {2}", description, colliding, rest);
log.DebugFormat("{0}, now colliding = {1}; {2}", description, colliding, rest);
return colliding;
}
private bool StepTowardCollision(MovingObject mo, Vector3 displacement,
int nsteps, Vector3 step, CollisionParms parms)
{
Vector3 accumulatedSteps = Vector3.Zero;
for (int i=0; i<nsteps; i++) {
Vector3 nextStep;
if (i == nsteps - 1)
nextStep = displacement - accumulatedSteps;
else
nextStep = step;
if (CollideAfterAddingDisplacement(mo, nextStep, parms)) {
topLevelCollisions++;
return true;
}
accumulatedSteps += nextStep;
}
// No collision!
parms.obstacle = null;
return false;
}
private void MoveToObject(StreamWriter stream,
CollisionAPI API, CollisionShape collisionShape,
CollisionShape movingShape)
{
stream.Write("\n\n\nEntering MoveToObject\n");
// Create a MovingObject, and add movingShape to it
MovingObject mo = new MovingObject();
API.AddPartToMovingObject(mo, movingShape);
// Move movingObject 1 foot at a time toward the sphere
Vector3 toShape = collisionShape.center - movingShape.center;
stream.Write(string.Format("movingShape {0}\n", movingShape.ToString()));
stream.Write(string.Format("collisionShape {0}\nDisplacement Vector {1}\n",
collisionShape.ToString(), toShape.ToString()));
// We'll certainly get there before steps expires
int steps = (int)Math.Ceiling(toShape.Length);
// 1 foot step in the right direction
Vector3 stepVector = toShape.ToNormalized();
stream.Write(string.Format("Steps {0}, stepVector {1}\n", steps, stepVector.ToString()));
bool hitIt = false;
// Loop til we smack into something
CollisionParms parms = new CollisionParms();
for (int i=0; i<steps; i++) {
// Move 1 foot; if returns true, we hit something
hitIt = (API.TestCollision (mo, stepVector, parms));
stream.Write(string.Format("i = {0}, hitIt = {1}, movingShape.center = {2}\n",
i, hitIt, movingShape.center.ToString()));
if (hitIt) {
stream.Write(string.Format("collidingPart {0}\nblockingObstacle {1}\n, normPart {2}, normObstacle {3}\n",
parms.part.ToString(), parms.obstacle.ToString(),
parms.normPart.ToString(), parms.normObstacle.ToString()));
return;
}
stream.Write("\n");
}
Debug.Assert(hitIt, "Didn't hit the obstacle");
}
public bool TestCollision(MovingObject mo, ref Vector3 displacement, CollisionParms parms)
{
// Find the minimum step size for the assembly of parts
float stepSize = mo.StepSize(displacement);
return TestCollision(mo, stepSize, ref displacement, parms);
}
// The is the top-level collision detection function.
//
// The MovingObject doesn't intersect anything now; would it
// do so if the vector displacement is applied?
//
// If we do have a collision, we "creep up" on the object we collide
// with until we're within stepsize of the minimum dimension of the
// moving part
//
public bool TestCollision(MovingObject mo, float stepSize, ref Vector3 displacement, CollisionParms parms)
{
topLevelCalls++;
parms.Initialize();
if (mo.parts.Count == 0)
return false;
// Remember where the first part started
Vector3 start = mo.parts[0].shape.center;
Vector3 originalDisplacement = displacement;
float len = displacement.Length;
//MaybeDumpSphereTree();
int nsteps = (int)Math.Ceiling(len / stepSize);
Vector3 step = (len <= stepSize ? displacement : displacement * (stepSize/len));
// Try to step the whole way
if (!StepTowardCollision(mo, displacement, nsteps, step, parms)) {
displacement = Vector3.Zero;
return false;
}
// Otherwise, we hit something. Step toward it
// The minimum distance
float smallStepSize = Math.Min(MO.InchesToMeters(MinInchesToObstacle),
stepSize * MinFractionToObstacle);
len = step.Length;
nsteps = (int)Math.Ceiling(len/smallStepSize);
Vector3 smallStep = step * (smallStepSize/len);
bool hit = StepTowardCollision(mo, step, nsteps, smallStep, parms);
displacement = originalDisplacement - (mo.parts[0].shape.center - start);
return hit;
}
// Test all the parts of a moving object to see if they collide with
// any obstacle
public bool CollideAfterAddingDisplacement(MovingObject mo, Vector3 step, CollisionParms parms)
{
for (int i=0; i<mo.parts.Count; i++) {
MovingPart movingPart = mo.parts[i];
collisionTimeStamp++;
movingPart.AddDisplacement(step);
SphereTreeNode s = sphereTree.FindSmallestContainer(movingPart.shape, movingPart.sphere);
//MaybeDumpSphereTree();
movingPart.sphere = s;
partCalls++;
if (s.TestSphereCollision(movingPart.shape, collisionTimeStamp, ref collisionTestCount, parms)) {
// We hit something, so back out the displacements
// added to the parts tested so far
for (int j=0; j<=i; j++) {
MovingPart displacedPart = mo.parts[j];
displacedPart.AddDisplacement (-step);
//MaybeDumpSphereTree();
}
return true;
}
}
return false;
}
// The "global" method to add to the set of parts in a moving
// object
public void AddPartToMovingObject(MovingObject mo, CollisionShape part)
{
mo.AddPart(part);
}
/// <summary>
/// Add the collision data for an object. This involves looking
/// for a physics file that matches the mesh file's name, and
/// loading the information from that file to build collision
/// volumes.
/// </summary>
/// <param name="objNode">the object for which we are adding the collision data</param>
public void AddCollisionObject(ObjectNode objNode)
{
if (worldManager.CollisionHelper == null)
return;
if (!objNode.UseCollisionObject)
return;
// Create a set of collision shapes for the object
List<CollisionShape> shapes = new List<CollisionShape>();
string meshName = objNode.Entity.Mesh.Name;
PhysicsData pd = new PhysicsData();
PhysicsSerializer ps = new PhysicsSerializer();
bool static_object = true;
if ((objNode.ObjectType == ObjectNodeType.Npc) ||
(objNode.ObjectType == ObjectNodeType.User)) {
static_object = false;
}
if (meshName.EndsWith(".mesh")) {
string physicsName = meshName.Substring(0, meshName.Length - 5) + ".physics";
try {
Stream stream = ResourceManager.FindCommonResourceData(physicsName);
ps.ImportPhysics(pd, stream);
foreach (SubEntity subEntity in objNode.Entity.SubEntities) {
if (subEntity.IsVisible) {
string submeshName = subEntity.SubMesh.Name;
List<CollisionShape> subEntityShapes = pd.GetCollisionShapes(submeshName);
foreach (CollisionShape subShape in subEntityShapes) {
// static objects will be transformed here, but movable objects
// are transformed on the fly
if (static_object)
subShape.Transform(objNode.SceneNode.DerivedScale,
objNode.SceneNode.DerivedOrientation,
objNode.SceneNode.DerivedPosition);
shapes.Add(subShape);
log.DebugFormat("Added collision shape for oid {0}, subShape {1}, subMesh {2}",
objNode.Oid, subShape, submeshName);
}
}
}
// Now populate the region volumes
foreach (KeyValuePair<string, List<CollisionShape>> entry in pd.CollisionShapes) {
string regionName = RegionVolumes.ExtractRegionName(entry.Key);
if (regionName != "") {
// We must record this region - - must be
// a static object
Debug.Assert(static_object);
List<CollisionShape> subShapes = new List<CollisionShape>();
foreach (CollisionShape subShape in entry.Value) {
subShape.Transform(objNode.SceneNode.DerivedScale,
objNode.SceneNode.DerivedOrientation,
objNode.SceneNode.DerivedPosition);
subShapes.Add(subShape);
}
RegionVolumes.Instance.AddRegionShapes(objNode.Oid, regionName, subShapes);
}
}
} catch (Exception) {
// Unable to load physics data -- use a sphere or no collision data?
log.InfoFormat("Unable to load physics data: {0}", physicsName);
//// For now, I'm going to put in spheres. Later I should do something real.
//CollisionShape shape = new CollisionSphere(Vector3.Zero, Client.OneMeter);
//if (static_object)
// // static objects will be transformed here, but movable objects
// // are transformed on the fly
// shape.Transform(objNode.SceneNode.DerivedScale,
// objNode.SceneNode.DerivedOrientation,
// objNode.SceneNode.DerivedPosition);
//shapes.Add(shape);
}
}
if (static_object) {
foreach (CollisionShape shape in shapes)
worldManager.CollisionHelper.AddCollisionShape(shape, objNode.Oid);
objNode.CollisionShapes = shapes;
} else {
MovingObject mo = new MovingObject(worldManager.CollisionHelper);
foreach (CollisionShape shape in shapes)
worldManager.CollisionHelper.AddPartToMovingObject(mo, shape);
objNode.Collider = mo;
objNode.Collider.Transform(objNode.SceneNode.DerivedScale,
objNode.SceneNode.DerivedOrientation,
objNode.SceneNode.DerivedPosition);
}
}
