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); } }