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