public virtual Body GetBodyAtMouse(bool includeStatic) { // Make a small box mousePVec.X = MouseXWorldPhys; mousePVec.Y = MouseYWorldPhys; FVector2 lowerBound = new FVector2(MouseXWorldPhys - 0.001f, MouseYWorldPhys - 0.001f); FVector2 upperBound = new FVector2(MouseXWorldPhys + 0.001f, MouseYWorldPhys + 0.001f); AABB aabb = new AABB(lowerBound, upperBound); Body body = null; // Query the world for overlapping shapes System.Func<Fixture, bool> GetBodyCallback = delegate (Fixture fixture0) { Shape shape = fixture0.Shape; if(fixture0.Body.BodyType != BodyType.Static || includeStatic) { FarseerPhysics.Common.Transform transform0; fixture0.Body.GetTransform(out transform0); bool inside = shape.TestPoint(ref transform0, ref mousePVec); if(inside) { body = fixture0.Body; return false; } } return true; }; FSWorldComponent.PhysicsWorld.QueryAABB(GetBodyCallback, ref aabb); return body; }
public override void computeAABB(AABB aabb, Transform xf, int childIndex) { Debug.Assert(childIndex < m_count); Vec2 lower = aabb.lowerBound; Vec2 upper = aabb.upperBound; int i1 = childIndex; int i2 = childIndex + 1; if (i2 == m_count) { i2 = 0; } Vec2 vi1 = m_vertices[i1]; Vec2 vi2 = m_vertices[i2]; Rot xfq = xf.q; Vec2 xfp = xf.p; float v1x = (xfq.c*vi1.x - xfq.s*vi1.y) + xfp.x; float v1y = (xfq.s*vi1.x + xfq.c*vi1.y) + xfp.y; float v2x = (xfq.c*vi2.x - xfq.s*vi2.y) + xfp.x; float v2y = (xfq.s*vi2.x + xfq.c*vi2.y) + xfp.y; lower.x = v1x < v2x ? v1x : v2x; lower.y = v1y < v2y ? v1y : v2y; upper.x = v1x > v2x ? v1x : v2x; upper.y = v1y > v2y ? v1y : v2y; }
public NavCell(float minX, float minY, float minZ, float maxX, float maxY, float maxZ, int flags) { BoundingBox = new AABB( new Vector3f(minX, minY, minZ), new Vector3f(maxX, maxY, maxZ)); Flags = (NavCellFlags)flags; }
public AABB WorldBound(Point[] vertices) { var pts = vertices; var res = new AABB(pts[this.v0.VertexIndex], pts[this.v1.VertexIndex]); res.Union(pts[v2.VertexIndex]); return res; }
public void CalculateBounds() { Bounds = new AABB { Max = Zone.ZoneMax.ToVector3(), Min = Zone.ZoneMin.ToVector3() }; Max = Bounds.Max; Min = Bounds.Min; // Create a persistable way to uniquely identify a scene at a location SceneHash = string.Format("{0}_[{1},{2}][{3},{4}]", _scene.Name, Min.X, Min.Y, Max.X, Max.Y); EdgeLength = Bounds.Max.X - Bounds.Min.X / 2; HalfEdgeLength = EdgeLength / 2; SouthWest = new Vector3(Bounds.Max.X, Bounds.Min.Y, 0); SouthEast = new Vector3(Bounds.Max.X, Bounds.Max.Y, 0); NorthWest = new Vector3(Bounds.Min.X, Bounds.Min.Y, 0); NorthEast = new Vector3(Bounds.Min.X, Bounds.Max.Y, 0); CenterX = Bounds.Min.X + (Bounds.Max.X - Bounds.Min.X) / 2; CenterY = Bounds.Min.Y + (Bounds.Max.Y - Bounds.Min.Y) / 2; Center = new Vector3(CenterX, CenterY, 0); }
internal void InitInterior(BVHBuildNode left, BVHBuildNode right, int axis) { children[0] = left; children[1] = right; splitAxis = axis; bounds = left.bounds.Union(right.bounds); nPrims = 0; }
public AABB WorldBound(Point[] vertices) { var pts = vertices; var res = new AABB(pts[this.v0], pts[this.v1]); res.Union(pts[v2]); return res; }
public ExploreCell(AABB aabb, List<Cell> cells, Vec3 position, int id = -1) : base(aabb.Min.X, aabb.Min.Y, 0, aabb.Max.X, aabb.Max.Y, 0, MovementFlag.None, id) { InitExploreCell(); Position = position; Cells = cells; }
public KdTreePhotonMap(int max) { this.max_photons = max; this.prev_scale = 1; this.photonsCount = 0; this.bbox = new AABB(); this.photons = new Photon[max]; }
public int createProxy(AABB aabb, object userData) { int proxyId = m_tree.createProxy(aabb, userData); ++m_proxyCount; bufferMove(proxyId); return proxyId; }
/// <summary> /// Creates a new quad tree broadphase with the specified span. /// </summary> /// <param name="span">the maximum span of the tree (world size)</param> public QuadTreeBroadPhase(AABB span) { _quadTree = new QuadTree<FixtureProxy>(span, 5, 10); _idRegister = new Dictionary<int, Element<FixtureProxy>>(); _moveBuffer = new List<Element<FixtureProxy>>(); _pairBuffer = new List<Pair>(); }
public void GetFatAABB(int proxyID, out AABB aabb) { if (_idRegister.ContainsKey(proxyID)) aabb = _idRegister[proxyID].Span; else throw new KeyNotFoundException("proxyID not found in register"); }
// DensityRegion Public Methods public DensityRegion(Spectrum sa, Spectrum ss, float gg, Spectrum emit, AABB b) { sig_a = sa; sig_s = ss; le = emit; g = gg; worldBound = b; }
internal unsafe void MoveAndWallSlide() { if( entity.Velocity == Vector3.Zero ) return; int* minY = stackalloc int[4096]; int* maxY = stackalloc int[4096]; int* minX = stackalloc int[4096]; int* maxX = stackalloc int[4096]; for( int i = 0; i < 4096; i++ ) { minX[i] = int.MaxValue; minY[i] = int.MaxValue; maxX[i] = int.MinValue; maxY[i] = int.MinValue; } bb = entity.CollisionBounds; int count = 0; // First raytrace out from the origin to find approximate blocks CalcRayDirection(); while( true ) { UpdateCoords( tracerP1Y1, minX, maxX, minY, maxY ); UpdateCoords( tracerP1Y2, minX, maxX, minY, maxY ); UpdateCoords( tracerP2Y1, minX, maxX, minY, maxY ); UpdateCoords( tracerP2Y2, minX, maxX, minY, maxY ); } // Now precisely which blocks we really intersect with // .. then perform collision }
/// <summary> /// Handles what happens when a user clicks the mouse /// </summary> /// <param name="p"> /// The position of the mouse click in relative coordinates. /// </param> public void MouseDown(Vector point) { Vector p = RelativeToWorld(point); if (mouseJoint != null) throw new Exception("ASSERT: mouseJoint should be null"); // Make a small box. Vector d = new Vector(.001f, .001f); AABB aabb = new AABB(p - d, p + d); // Query the world for overlapping shapes. IList<Shape> shapes = world.Query(aabb); Body body = null; foreach (Shape shape in shapes) { if (shape.Body.Static == false && shape.TestPoint(shape.Body.GetXForm(), p)) { body = shape.Body; break; } } if (body != null) { MouseJointDef md = new MouseJointDef(); md.Body1 = world.GetGroundBody(); md.Body2 = body; md.Target = p; md.MaxForce = 1000 * body.Mass; mouseJoint = new MouseJoint(world.CreateJoint(md)); body.WakeUp(); } }
public void SetProxyAABB(BroadphaseProxy proxy, ref AABB aabb) { // maintain the uniform grid as the aabb moves. this works by removing // the stale aabb proxies and then adding the fresh aabb proxies. RemoveProxyFromCells(proxy); proxy.AABB = aabb; AddProxyToCells(proxy); }
public DungeonRoom(int id, AABB b) { boundary = b; this.id = id; this.color = new Color(Random.Range(0f, 1f), Random.Range(0f, 1f), Random.Range(0f, 1f)); this.tiles = new List<DungeonTile>(); }
private QuadTree<FixtureProxy>[] _quadTrees; // array indexed by log2(Fixture.Category) #endregion Fields #region Constructors /// <summary> /// Creates a new quad tree broadphase with the specified span. /// </summary> /// <param name="span">world size</param> public QuadTreeBroadPhase(AABB span) { var fatSpan = span.Fattened; _quadTrees = new QuadTree<FixtureProxy>[32]; for (int i=0;i<_quadTrees.Length;i++) _quadTrees[i] = new QuadTree<FixtureProxy>(fatSpan, 5, 10); _idRegister = new Dictionary<int, Element<FixtureProxy>>(); _moveBuffer = new List<Element<FixtureProxy>>(); _pairBuffer = new List<Pair>(); }
// Use this for initialization void Start () { Vector3 p = transform.position; FVector2 aa = new FVector2(p.x + StartPoint.x, p.y + StartPoint.y); FVector2 bb = new FVector2(p.x + EndPoint.x, p.y + EndPoint.y); aabb = new AABB(aa, bb); buoyancyController = new BuoyancyController(aabb, Density, LinearDragCoef, RotationalDragCoef, FSWorldComponent.PhysicsWorld.Gravity); FSWorldComponent.PhysicsWorld.AddController(buoyancyController); }
public ModernHashGridPhotonMap(int max) { this.max_photons = max; this.prev_scale = 1; this.photonsCount = 0; this.bbox = new AABB(); this.photons = new Photon[max]; this.counts = new int[max]; }
SingleScatteringIntegrator(AABB bbox, float step, float prob, RgbSpectrum inScattering, RgbSpectrum emission) { region = bbox; stepSize = step; rrProb = prob; sig_s = inScattering; lightEmission = emission; }
public AABB BoundingBox() { var bbox = new AABB(); foreach (var vertex in Vertices) { bbox.Union(vertex); } return bbox; }
public override EPlacement Placement(AABB bound) { EPlacement ap = a.Placement(bound); EPlacement bp = b.Placement(bound); if (ap == EPlacement.BInsideA || bp == EPlacement.BInsideA) return EPlacement.BInsideA; //this one isn't quite right, might be BInsideA also if there's an intersection if (ap == EPlacement.Outside && bp == EPlacement.Outside) return EPlacement.Outside; if (ap == EPlacement.AInsideB && bp == EPlacement.AInsideB) return EPlacement.AInsideB; return EPlacement.Intersect; }
public DungeonRoom(int id, AABB b, QuadTree q) { boundary = b; quadtree = q; quadtree.room = this; this.id = id; this.color = new Color(Random.Range(0f, 1f), Random.Range(0f, 1f), Random.Range(0f, 1f)); this.tiles = new List<DungeonTile>(); }
public void DrawAABB(ref AABB aabb, Color color) { FixedArray8<Vector2> verts = new FixedArray8<Vector2>(); verts[0] = new Vector2(aabb.lowerBound.X, aabb.lowerBound.Y); verts[1] = new Vector2(aabb.upperBound.X, aabb.lowerBound.Y); verts[2] = new Vector2(aabb.upperBound.X, aabb.upperBound.Y); verts[3] = new Vector2(aabb.lowerBound.X, aabb.upperBound.Y); DrawPolygon(ref verts, 4, color); }
protected override void Recycle(bool isReleasing) { AABB = AABB.Zero; ClientObject = null; CollisionFilterGroup = CollisionGroups.None; CollisionFilterMask = CollisionGroups.None; if (isReleasing) CollisionGlobals.TotalProxies--; base.Recycle(isReleasing); }
/* * METHODS */ // Clean QuadTree public void Clear() { seed = -1; boundary = new AABB(); northWest = null; northEast = null; southWest = null; southEast = null; room = null; }
public bool Intersects(AABB other) { bool _x = false; bool _y = false; XY distance = other.center - center; if (Mathf.Abs(distance.x) < (other.half.x + half.x)) _x = true; if (Mathf.Abs(distance.y) < (other.half.y + half.y)) _y = true; return _x&&_y; }
public override void CalculateAABB(ref Matrix3 transform, out AABB aabb) { Vector2 halfWidth, halfHeight; CalculateExtents(ref transform, out halfWidth, out halfHeight); Vector2 halfWidthOther, halfHeightOther; CalculateOtherExtents(ref transform, out halfWidthOther, out halfHeightOther); aabb.Min = Vector2.Min(halfWidth, halfWidthOther) + Vector2.Min(halfHeight, halfHeightOther) - transform.Origin; aabb.Max = Vector2.Max(halfWidth, halfWidthOther) + Vector2.Max(halfHeight, halfHeightOther) - transform.Origin; }
public override void computeAABB(AABB aabb, Transform transform, int childIndex) { Rot tq = transform.q; Vec2 tp = transform.p; float px = tq.c*m_p.x - tq.s*m_p.y + tp.x; float py = tq.s*m_p.x + tq.c*m_p.y + tp.y; aabb.lowerBound.x = px - m_radius; aabb.lowerBound.y = py - m_radius; aabb.upperBound.x = px + m_radius; aabb.upperBound.y = py + m_radius; }
/// <summary> /// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a Shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// </summary> /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(BroadPhaseRayCastCallback callback, ref RayCastInput input) { Vector2 p1 = input.Point1; Vector2 p2 = input.Point2; Vector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { Vector2 t = p1 + maxFraction * (p2 - p1); Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound); Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound); } _raycastStack.Clear(); _raycastStack.Push(_root); while (_raycastStack.Count > 0) { int nodeId = _raycastStack.Pop(); if (nodeId == NullNode) { continue; } //TreeNode<T>* node = &_nodes[nodeId]; if (AABB.TestOverlap(ref _nodes[nodeId].AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vector2 c = _nodes[nodeId].AABB.Center; Vector2 h = _nodes[nodeId].AABB.Extents; float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (_nodes[nodeId].IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; float value = callback(ref subInput, nodeId); if (value == 0.0f) { // the client has terminated the raycast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; Vector2 t = p1 + maxFraction * (p2 - p1); Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound); Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound); } } else { _raycastStack.Push(_nodes[nodeId].Child1); _raycastStack.Push(_nodes[nodeId].Child2); } } }
void RecalcDownExtent(ref AABB bb, int steps, int dx, int dz) { AABB downExtent = bb.Adjust(dx * steps, -32, dz * steps); downsCount = AABB.FindIntersectingSolids(downExtent, level, ref downs); }
void DoMove(int steps) { Position pos = Pos; AABB bb = ModelBB.OffsetPosition(pos); int dx = Math.Sign(TargetPos.X - pos.X); int dz = Math.Sign(TargetPos.Z - pos.Z); if (downsCount == -1) { RecalcDownExtent(ref bb, steps, dx, dz); RecalcUpExtent(ref bb, steps, dx, dz); } // Advance the AABB to the bot's next position bb = bb.Offset(dx, 0, dz); AABB bbCopy = bb; // Attempt to drop the bot down up to 1 block int hitY = -32; for (int dy = 0; dy >= -32; dy--) { bool intersectsAny = false; for (int i = 0; i < downsCount; i++) { if (AABB.Intersects(ref bb, ref downs[i])) { intersectsAny = true; break; } } if (intersectsAny) { hitY = dy + 1; break; } bb.Min.Y--; bb.Max.Y--; } // Does the bot fall down a block if (hitY < 0) { pos.X += dx; pos.Y += hitY; pos.Z += dz; Pos = pos; RecalcDownExtent(ref bb, steps, dx, dz); RecalcUpExtent(ref bb, steps, dx, dz); return; } // Attempt to move the bot up to 1 block bb = bbCopy; for (int dy = 0; dy <= 32; dy++) { bool intersectsAny = false; for (int i = 0; i < upsCount; i++) { if (AABB.Intersects(ref bb, ref ups[i])) { intersectsAny = true; break; } } if (!intersectsAny) { pos.X += dx; pos.Y += dy; pos.Z += dz; Pos = pos; if (dy != 0) { RecalcDownExtent(ref bb, steps, dx, dz); RecalcUpExtent(ref bb, steps, dx, dz); } return; } bb.Min.Y++; bb.Max.Y++; } }
private AABB Fatten(ref AABB aabb) { Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension); return(new AABB(aabb.LowerBound - r, aabb.UpperBound + r)); }
/// <summary> /// Query the world for all fixtures that potentially overlap the provided AABB. /// Inside the callback: /// Return true: Continues the query /// Return false: Terminate the query /// </summary> /// <param name="callback">A user implemented callback class.</param> /// <param name="aabb">The aabb query box.</param> public void QueryAABB(Func <Fixture, bool> callback, ref AABB aabb) { _queryAABBCallback = callback; ContactManager.BroadPhase.Query(_queryAABBCallbackWrapper, ref aabb); _queryAABBCallback = null; }
private void UpdateVisibility(int x, int y, int z, int rangeX, int rangeY, int rangeZ) { if (rangeX == 0 || rangeY == 0 || rangeZ == 0) { return; } bool isLast = rangeX == 1 && rangeY == 1 && rangeZ == 1; int wx = m_viewerPos.x + (x * Env.ChunkSize); int wy = m_viewerPos.y + (y * Env.ChunkSize); int wz = m_viewerPos.z + (z * Env.ChunkSize); int rx = rangeX * Env.ChunkSize; int ry = rangeY * Env.ChunkSize; int rz = rangeZ * Env.ChunkSize; // Stop if there is no further subdivision possible if (isLast) { // Update chunk's visibility information Vector3Int chunkPos = new Vector3Int(wx, wy, wz); Chunk chunk = world.chunks.Get(ref chunkPos); if (chunk == null) { return; } ChunkStateManagerClient stateManager = chunk.stateManager; int tx = m_clipmap.TransformX(x); int ty = m_clipmap.TransformY(y); int tz = m_clipmap.TransformZ(z); // Skip chunks which are too far away if (!m_clipmap.IsInsideBounds_Transformed(tx, ty, tz)) { return; } // Update visibility information ClipmapItem item = m_clipmap.Get_Transformed(tx, ty, tz); bool isVisible = Planes.TestPlanesAABB(m_cameraPlanes, chunk.WorldBounds); stateManager.Visible = isVisible && item.IsInVisibleRange; stateManager.PossiblyVisible = isVisible || FullLoadOnStartUp; return; } // Check whether the bouding box lies inside the camera's frustum AABB bounds2 = new AABB(wx, wy, wz, wx + rx, wy + ry, wz + rz); int inside = Planes.TestPlanesAABB2(m_cameraPlanes, bounds2); #region Full invisibility if (inside == 0) { // Full invisibility. All chunks in this area need to be made invisible for (int cy = wy; cy < wy + ry; cy += Env.ChunkSize) { for (int cz = wz; cz < wz + rz; cz += Env.ChunkSize) { for (int cx = wx; cx < wx + rx; cx += Env.ChunkSize) { // Update chunk's visibility information Vector3Int chunkPos = new Vector3Int(cx, cy, cz); Chunk chunk = world.chunks.Get(ref chunkPos); if (chunk == null) { continue; } ChunkStateManagerClient stateManager = chunk.stateManager; // Update visibility information stateManager.PossiblyVisible = FullLoadOnStartUp; stateManager.Visible = false; } } } return; } #endregion #region Full visibility if (inside == 6) { // Full visibility. All chunks in this area need to be made visible for (int cy = wy; cy < wy + ry; cy += Env.ChunkSize) { for (int cz = wz; cz < wz + rz; cz += Env.ChunkSize) { for (int cx = wx; cx < wx + rx; cx += Env.ChunkSize) { // Update chunk's visibility information Vector3Int chunkPos = new Vector3Int(cx, cy, cz); Chunk chunk = world.chunks.Get(ref chunkPos); if (chunk == null) { continue; } ChunkStateManagerClient stateManager = chunk.stateManager; int tx = m_clipmap.TransformX(x); int ty = m_clipmap.TransformY(y); int tz = m_clipmap.TransformZ(z); // Update visibility information ClipmapItem item = m_clipmap.Get_Transformed(tx, ty, tz); stateManager.Visible = item.IsInVisibleRange; stateManager.PossiblyVisible = true; } } } return; } #endregion #region Partial visibility int offX = rangeX; if (rangeX > 1) { offX = rangeX >> 1; rangeX = (rangeX + 1) >> 1; // ceil the number } int offY = rangeY; if (rangeY > 1) { offY = rangeY >> 1; rangeY = (rangeY + 1) >> 1; // ceil the number } int offZ = rangeZ; if (rangeZ > 1) { offZ = rangeZ >> 1; rangeZ = (rangeZ + 1) >> 1; // ceil the number } // Subdivide if possible // TODO: Avoid the recursion UpdateVisibility(x, y, z, offX, offY, offZ); UpdateVisibility(x + offX, y, z, rangeX, offY, offZ); UpdateVisibility(x, y, z + offZ, offX, offY, rangeZ); UpdateVisibility(x + offX, y, z + offZ, rangeX, offY, rangeZ); UpdateVisibility(x, y + offY, z, offX, rangeY, offZ); UpdateVisibility(x + offX, y + offY, z, rangeX, rangeY, offZ); UpdateVisibility(x, y + offY, z + offZ, offX, rangeY, rangeZ); UpdateVisibility(x + offX, y + offY, z + offZ, rangeX, rangeY, rangeZ); #endregion }
/// <summary> /// Get the fat AABB for a proxy. /// </summary> /// <param name="proxyId">The proxy id.</param> /// <param name="fatAABB">The fat AABB.</param> public void GetFatAABB(int proxyId, out AABB fatAABB) { Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); fatAABB = _nodes[proxyId].AABB; }
// Use this for initialization void Start() { return; func = callBack; //int count = 0; //for (int i = 0; i < 100; i++) //{ // var g = GameObject.CreatePrimitive(PrimitiveType.Cube); // g.transform.position = new Vector3(Random.Range(0, 100), 0, Random.Range(0, 100)); //} System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch(); //sw.Start(); //for (int i = 0; i < 100; i++) //{ // Ray r = new Ray(); // r.origin = new Vector3(Random.Range(0, 50),0, Random.Range(0, 50)); // r.direction = new Vector3(Random.Range(0, 1f), 0, Random.Range(0, 1f)); // r.direction = r.direction.normalized; // var hit = Physics.Raycast(r, 100, -1); // // Debug.DrawLine(r.origin, r.direction*100+r.origin, Color.red, 10); // if (hit) count++; //} // Debug.Log(sw.ElapsedMilliseconds); //return; Vector2d[] vs = new Vector2d[4]; for (int i = 0; i < 100; i++) { var center = new Vector2d(Random.Range(0, 100), Random.Range(0, 100)); var aabb = new AABB(center, FixedMath.One * 2, FixedMath.One * 2); FixtureProxy fp = new FixtureProxy(); long angle = FixedMath.One * (int)(Random.Range(0, 359)); Vector2d a = -aabb.Extents; Vector2d b = a + new Vector2d(0, aabb.Height); Vector2d c = aabb.Extents; Vector2d d = a + new Vector2d(aabb.Width, 0); var halfw = aabb.Width / 2; var halfh = aabb.Height / 2; var radius = FixedMath.Sqrt((halfw).Mul(halfw) + halfh.Mul(halfh)); a = RotatePosi(a, angle); b = RotatePosi(b, angle); c = RotatePosi(c, angle); d = RotatePosi(d, angle); vs[0] = a; vs[1] = b; vs[2] = c; vs[3] = d; Vector2d min = Vector2d.Min(vs) + center; Vector2d max = Vector2d.Max(vs) + center; DLog.Log(radius.ToFloat().ToString()); var outteraabb = new AABB(center, radius * 2, radius * 2); fp.AABB = aabb; fp.Fixture = new Transform2d(ref center, ref angle); int id = tree.AddProxy(ref outteraabb, fp); tree.MoveProxy(id, ref outteraabb, Vector2d.zero); DrawFixtureProxy(fp); DrawAABB(outteraabb); } //var bcs = GameObject.FindObjectsOfType<BoxCollider>(); //for (int i = 0; i < bcs.Length; i++) //{ // var bc = bcs[i]; // var aabb = new AABB(new Vector2d(bc.transform.position), FixedMath.One, FixedMath.One); // FixtureProxy fp = new FixtureProxy(); // fp.AABB = aabb; // long angle = FixedMath.One * (int)(bc.transform.eulerAngles.y); // Vector2d p = new Vector2d(bc.transform.position); // fp.Fixture = new Transform2d(ref p, ref angle); // tree.AddProxy(ref aabb, fp); // DrawFixtureProxy(fp); //} // sw.Reset(); sw.Start(); for (int i = 0; i < 1; i++) { RayCastInput input = new RayCastInput(); input.Point1 = new Vector2d(Random.Range(0, 50) * FixedMath.One, Random.Range(0, 50) * FixedMath.One); input.Point2 = new Vector2d(Random.Range(50, 100) * FixedMath.One, Random.Range(50, 100) * FixedMath.One); NewSphere(input.Point1.ToVector3(), "start"); NewSphere(input.Point2.ToVector3(), "end"); input.MaxFraction = FixedMath.One; DrawLine(input); tree.RayCast(callBack, ref input); } Debug.Log(sw.ElapsedMilliseconds); }
/// <summary> /// Update the contact manifold and touching status. /// Note: do not assume the fixture AABBs are overlapping or are valid. /// </summary> /// <param name="contactManager">The contact manager.</param> internal void Update(ContactManager contactManager) { Manifold oldManifold = Manifold; // Re-enable this contact. Flags |= ContactFlags.Enabled; bool touching; bool wasTouching = (Flags & ContactFlags.Touching) == ContactFlags.Touching; bool sensor = FixtureA.IsSensor || FixtureB.IsSensor; Body bodyA = FixtureA.Body; Body bodyB = FixtureB.Body; // Is this contact a sensor? if (sensor) { Shape shapeA = FixtureA.Shape; Shape shapeB = FixtureB.Shape; touching = AABB.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA.Xf, ref bodyB.Xf); // Sensors don't generate manifolds. Manifold.PointCount = 0; } else { Evaluate(ref Manifold, ref bodyA.Xf, ref bodyB.Xf); touching = Manifold.PointCount > 0; // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. for (int i = 0; i < Manifold.PointCount; ++i) { ManifoldPoint mp2 = Manifold.Points[i]; mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; ContactID id2 = mp2.Id; bool found = false; for (int j = 0; j < oldManifold.PointCount; ++j) { ManifoldPoint mp1 = oldManifold.Points[j]; if (mp1.Id.Key == id2.Key) { mp2.NormalImpulse = mp1.NormalImpulse; mp2.TangentImpulse = mp1.TangentImpulse; found = true; break; } } if (found == false) { mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; } Manifold.Points[i] = mp2; } if (touching != wasTouching) { bodyA.Awake = true; bodyB.Awake = true; } } if (touching) { Flags |= ContactFlags.Touching; } else { Flags &= ~ContactFlags.Touching; } if (wasTouching == false && touching) { //Report the collision to both participants: if (FixtureA != null) { if (FixtureA.OnCollision != null) { Enabled = FixtureA.OnCollision(FixtureA, FixtureB, this); } } //Reverse the order of the reported fixtures. The first fixture is always the one that the //user subscribed to. if (FixtureB != null) { if (FixtureB.OnCollision != null) { Enabled = FixtureB.OnCollision(FixtureB, FixtureA, this); } } //BeginContact can also return false and disable the contact if (contactManager.BeginContact != null) { Enabled = contactManager.BeginContact(this); } //if the user disabled the contact (needed to exclude it in TOI solver), we also need to mark //it as not touching. if (Enabled == false) { Flags &= ~ContactFlags.Touching; } } if (wasTouching && touching == false) { //Report the separation to both participants: if (FixtureA != null && FixtureA.OnSeparation != null) { FixtureA.OnSeparation(FixtureA, FixtureB); } //Reverse the order of the reported fixtures. The first fixture is always the one that the //user subscribed to. if (FixtureB != null && FixtureB.OnSeparation != null) { FixtureB.OnSeparation(FixtureB, FixtureA); } if (contactManager.EndContact != null) { contactManager.EndContact(this); } } if (sensor) { return; } if (contactManager.PreSolve != null) { contactManager.PreSolve(this, ref oldManifold); } }
bool FloodFrom(Region tregion, Location start, List <KeyValuePair <Location, BlockInternal> > blocks, double maxRad, AABB extent) { Queue <Location> locsToGo = new Queue <Location>(); locsToGo.Enqueue(start); while (locsToGo.Count > 0) { Location c = locsToGo.Dequeue(); if ((c - start).LengthSquared() > maxRad * maxRad) { SysConsole.Output(OutputType.INFO, "Escaped radius!"); return(false); } BlockInternal bi = tregion.GetBlockInternal(c); if ((Material)bi.BlockMaterial == Material.AIR) { continue; } if (!((BlockFlags)bi.BlockLocalData).HasFlag(BlockFlags.EDITED)) { SysConsole.Output(OutputType.INFO, "Found natural block!"); return(false); } if (((BlockFlags)bi.BlockLocalData).HasFlag(BlockFlags.PROTECTED)) { continue; } blocks.Add(new KeyValuePair <Location, BlockInternal>(c, bi)); tregion.SetBlockMaterial(c, (Material)bi.BlockMaterial, bi.BlockData, bi.BlockPaint, (byte)(bi.BlockLocalData | (byte)BlockFlags.PROTECTED), bi.Damage, false, false); extent.Include(c); foreach (Location dir in FloodDirs) { locsToGo.Enqueue(c + dir); } } return(true); }
public P2DSimpleWindForce(Transform xf, List <Rigidbody2D> bodies, AABB container) : base(xf, bodies, container) { }
/// <summary> Determines whether any of the blocks that intersect the /// given bounding box satisfy the given condition. </summary> public bool TouchesAny(AABB bounds, Predicate <byte> condition) { return(TouchesAny(bounds, delegate(BlockID bl) { return condition((byte)bl); })); }
/// <summary> Determines whether any of the blocks that intersect the /// bounding box of this entity are water or still water. </summary> public bool TouchesAnyWater() { AABB bounds = Bounds.Offset(liqExpand); return(TouchesAny(bounds, touchesAnyWater)); }
/// <summary> /// Test overlap of fat AABBs. /// </summary> /// <param name="proxyIdA">The proxy id A.</param> /// <param name="proxyIdB">The proxy id B.</param> public bool TestFatAABBOverlap(int proxyIdA, int proxyIdB) { Debug.Assert(0 <= proxyIdA && proxyIdA < _nodeCapacity); Debug.Assert(0 <= proxyIdB && proxyIdB < _nodeCapacity); return(AABB.TestOverlap(ref _nodes[proxyIdA].AABB, ref _nodes[proxyIdB].AABB)); }
/// <summary> /// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a Shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// </summary> /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input) { FVector2 p1 = input.Point1; FVector2 p2 = input.Point2; FVector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. FVector2 absV = MathUtils.Abs(new FVector2(-r.Y, r.X)); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { FVector2 t = p1 + maxFraction * (p2 - p1); FVector2.Min(ref p1, ref t, out segmentAABB.LowerBound); FVector2.Max(ref p1, ref t, out segmentAABB.UpperBound); } _stack.Clear(); _stack.Push(_root); while (_stack.Count > 0) { int nodeId = _stack.Pop(); if (nodeId == NullNode) { continue; } TreeNode<T> node = _nodes[nodeId]; if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) FVector2 c = node.AABB.Center; FVector2 h = node.AABB.Extents; float separation = Math.Abs(FVector2.Dot(new FVector2(-r.Y, r.X), p1 - c)) - FVector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; float value = callback(subInput, nodeId); if (value == 0.0f) { // the client has terminated the raycast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; FVector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = FVector2.Min(p1, t); segmentAABB.UpperBound = FVector2.Max(p1, t); } } else { _stack.Push(node.Child1); _stack.Push(node.Child2); } } }
/// <summary> /// Get all intersections between a line segment and an AABB. /// </summary> /// <param name="point1">The first point of the line segment to test</param> /// <param name="point2">The second point of the line segment to test.</param> /// <param name="aabb">The AABB that is used for testing intersection.</param> public static Vertices LineSegmentAABBIntersect(ref Vector2 point1, ref Vector2 point2, AABB aabb) { return(LineSegmentVerticesIntersect(ref point1, ref point2, aabb.Vertices)); }
private void OnDrawGizmos() { // // Generate the points we are going to find the convex hull from // //Random points HashSet <Vector3> points = TestAlgorithmsHelpMethods.GenerateRandomPoints(seed, mapSize, numberOfPoints); //Points from a plane mesh //HashSet<Vector3> points = TestAlgorithmsHelpMethods.GeneratePointsFromPlane(planeTrans); // // Prepare the points // //From 3d to 2d HashSet <MyVector2> points_2d = new HashSet <MyVector2>(); foreach (Vector3 v in points) { points_2d.Add(v.ToMyVector2()); } //Normalize to range 0-1 AABB normalizingBox = HelpMethods.GetAABB(new List <MyVector2>(points_2d)); float dMax = HelpMethods.CalculateDMax(normalizingBox); HashSet <MyVector2> points_2d_normalized = HelpMethods.Normalize(points_2d, normalizingBox, dMax); // // Generate the convex hull // //Algorithm 1. Jarvis March - slow but simple List <MyVector2> pointsOnConvexHull_2d_normalized = _ConvexHull.JarvisMarch(points_2d_normalized); if (pointsOnConvexHull_2d_normalized == null) { Debug.Log("Couldnt find a convex hull"); } // // Display // //Display points on the hull and lines between the points if (pointsOnConvexHull_2d_normalized != null) { //UnNormalize List <MyVector2> pointsOnConvexHull_2d = HelpMethods.UnNormalize(pointsOnConvexHull_2d_normalized, normalizingBox, dMax); //From 2d to 3d List <Vector3> pointsOnConvexHull = new List <Vector3>(); foreach (MyVector2 v in pointsOnConvexHull_2d) { pointsOnConvexHull.Add(v.ToVector3()); } //print(pointsOnConvexHull.Count); for (int i = 1; i < pointsOnConvexHull.Count; i++) { Gizmos.DrawLine(pointsOnConvexHull[i - 1], pointsOnConvexHull[i]); } float size = 0.1f; for (int i = 1; i < pointsOnConvexHull.Count; i++) { Gizmos.DrawWireSphere(pointsOnConvexHull[i], size); //So we can see in which order they were added size += 0.01f; } } //Display all the original points foreach (Vector3 p in points) { Gizmos.DrawSphere(p, 0.1f); } }
public World(Vector2 gravity, AABB span) : this() { this.Gravity = gravity; this.ContactManager = new ContactManager(new QuadTreeBroadPhase(span)); }
/// <summary> /// Given a transform, compute the associated axis aligned bounding box for a child shape. /// </summary> /// <param name="aabb">The aabb results.</param> /// <param name="transform">The world transform of the shape.</param> /// <param name="childIndex">The child shape index.</param> public abstract void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex);
public override bool BoundingBox(float t0, float t1, ref AABB box) { box = new AABB(pmin, pmax); return(true); }
public void UpdateVelocityState() { if (hacks.Flying || hacks.Noclip) { entity.Velocity.Y = 0; // eliminate the effect of gravity int dir = (hacks.FlyingUp || jumping) ? 1 : (hacks.FlyingDown ? -1 : 0); entity.Velocity.Y += 0.12f * dir; if (hacks.Speeding && hacks.CanSpeed) { entity.Velocity.Y += 0.12f * dir; } if (hacks.HalfSpeeding && hacks.CanSpeed) { entity.Velocity.Y += 0.06f * dir; } } else if (jumping && entity.TouchesAnyRope() && entity.Velocity.Y > 0.02f) { entity.Velocity.Y = 0.02f; } if (!jumping) { canLiquidJump = false; return; } bool touchWater = entity.TouchesAnyWater(); bool touchLava = entity.TouchesAnyLava(); if (touchWater || touchLava) { AABB bounds = entity.Bounds; int feetY = Utils.Floor(bounds.Min.Y), bodyY = feetY + 1; int headY = Utils.Floor(bounds.Max.Y); if (bodyY > headY) { bodyY = headY; } bounds.Max.Y = bounds.Min.Y = feetY; bool liquidFeet = entity.TouchesAny(bounds, StandardLiquid); bounds.Min.Y = Math.Min(bodyY, headY); bounds.Max.Y = Math.Max(bodyY, headY); bool liquidRest = entity.TouchesAny(bounds, StandardLiquid); bool pastJumpPoint = liquidFeet && !liquidRest && (entity.Position.Y % 1 >= 0.4); if (!pastJumpPoint) { canLiquidJump = true; entity.Velocity.Y += 0.04f; if (hacks.Speeding && hacks.CanSpeed) { entity.Velocity.Y += 0.04f; } if (hacks.HalfSpeeding && hacks.CanSpeed) { entity.Velocity.Y += 0.02f; } } else if (pastJumpPoint) { // either A) jump bob in water B) climb up solid on side if (collisions.HorizontalCollision) { entity.Velocity.Y += touchLava ? 0.30f : 0.13f; } else if (canLiquidJump) { entity.Velocity.Y += touchLava ? 0.20f : 0.10f; } canLiquidJump = false; } } else if (useLiquidGravity) { entity.Velocity.Y += 0.04f; if (hacks.Speeding && hacks.CanSpeed) { entity.Velocity.Y += 0.04f; } if (hacks.HalfSpeeding && hacks.CanSpeed) { entity.Velocity.Y += 0.02f; } canLiquidJump = false; } else if (entity.TouchesAnyRope()) { entity.Velocity.Y += (hacks.Speeding && hacks.CanSpeed) ? 0.15f : 0.10f; canLiquidJump = false; } else if (entity.onGround) { DoNormalJump(); } }
/// <summary> /// Query an AABB for overlapping proxies. The callback class /// is called for each proxy that overlaps the supplied AABB. /// </summary> /// <param name="callback">The callback.</param> /// <param name="aabb">The aabb.</param> public void Query(Func <int, bool> callback, ref AABB aabb) { _tree.Query(callback, ref aabb); }
public void Query(Func <int, bool> callback, ref AABB query) { _quadTree.QueryAABB(TransformPredicate(callback), ref query); }
/// <summary> /// Get the AABB for a proxy. /// </summary> /// <param name="proxyId">The proxy id.</param> /// <param name="aabb">The aabb.</param> public void GetFatAABB(int proxyId, out AABB aabb) { _tree.GetFatAABB(proxyId, out aabb); }
void RecalcUpExtent(ref AABB bb, int steps, int dx, int dz) { AABB upExtent = bb.Adjust(dx * steps, 32, dz * steps); upsCount = AABB.FindIntersectingSolids(upExtent, level, ref ups); }
/// Build an optimal tree. Very expensive. For testing. void RebuildBottomUp() { int[] nodes = new int[_nodeCount]; int count = 0; // Build array of leaves. Free the rest. for (int i = 0; i < _nodeCapacity; ++i) { if (_nodes[i].Height < 0) { // free node in pool continue; } if (_nodes[i].IsLeaf()) { _nodes[i].ParentOrNext = NullNode; nodes[count] = i; ++count; } else { FreeNode(i); } } while (count > 1) { float minCost = FSSettings.MaxFloat; int iMin = -1, jMin = -1; for (int i = 0; i < count; ++i) { AABB AABBi = _nodes[nodes[i]].AABB; for (int j = i + 1; j < count; ++j) { AABB AABBj = _nodes[nodes[j]].AABB; AABB b = new AABB(); b.Combine(ref AABBi, ref AABBj); float cost = b.Perimeter; if (cost < minCost) { iMin = i; jMin = j; minCost = cost; } } } int index1 = nodes[iMin]; int index2 = nodes[jMin]; TreeNode<T> child1 = _nodes[index1]; TreeNode<T> child2 = _nodes[index2]; int parentIndex = AllocateNode(); TreeNode<T> parent = _nodes[parentIndex]; parent.Child1 = index1; parent.Child2 = index2; parent.Height = 1 + Math.Max(child1.Height, child2.Height); parent.AABB.Combine(ref child1.AABB, ref child2.AABB); parent.ParentOrNext = NullNode; child1.ParentOrNext = parentIndex; child2.ParentOrNext = parentIndex; nodes[jMin] = nodes[count - 1]; nodes[iMin] = parentIndex; --count; } _root = nodes[0]; Validate(); }
public Element(AABB span) { Span = span; Value = default(T); Parent = null; }
private void InsertLeaf(int leaf) { ++_insertionCount; if (_root == NullNode) { _root = leaf; _nodes[_root].ParentOrNext = NullNode; return; } // Find the best sibling for this node AABB leafAABB = _nodes[leaf].AABB; int index = _root; while (_nodes[index].IsLeaf() == false) { int child1 = _nodes[index].Child1; int child2 = _nodes[index].Child2; float area = _nodes[index].AABB.Perimeter; AABB combinedAABB = new AABB(); combinedAABB.Combine(ref _nodes[index].AABB, ref leafAABB); float combinedArea = combinedAABB.Perimeter; // Cost of creating a new parent for this node and the new leaf float cost = 2.0f * combinedArea; // Minimum cost of pushing the leaf further down the tree float inheritanceCost = 2.0f * (combinedArea - area); // Cost of descending into child1 float cost1; if (_nodes[child1].IsLeaf()) { AABB aabb = new AABB(); aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); cost1 = aabb.Perimeter + inheritanceCost; } else { AABB aabb = new AABB(); aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); float oldArea = _nodes[child1].AABB.Perimeter; float newArea = aabb.Perimeter; cost1 = (newArea - oldArea) + inheritanceCost; } // Cost of descending into child2 float cost2; if (_nodes[child2].IsLeaf()) { AABB aabb = new AABB(); aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); cost2 = aabb.Perimeter + inheritanceCost; } else { AABB aabb = new AABB(); aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); float oldArea = _nodes[child2].AABB.Perimeter; float newArea = aabb.Perimeter; cost2 = newArea - oldArea + inheritanceCost; } // Descend according to the minimum cost. if (cost < cost1 && cost1 < cost2) { break; } // Descend if (cost1 < cost2) { index = child1; } else { index = child2; } } int sibling = index; // Create a new parent. int oldParent = _nodes[index].ParentOrNext; int newParent = AllocateNode(); _nodes[newParent].ParentOrNext = oldParent; _nodes[newParent].UserData = default(T); _nodes[newParent].AABB.Combine(ref leafAABB, ref _nodes[sibling].AABB); _nodes[newParent].Height = _nodes[sibling].Height + 1; if (oldParent != NullNode) { // The sibling was not the root. if (_nodes[oldParent].Child1 == sibling) { _nodes[oldParent].Child1 = newParent; } else { _nodes[oldParent].Child2 = newParent; } _nodes[newParent].Child1 = sibling; _nodes[newParent].Child2 = leaf; _nodes[index].ParentOrNext = newParent; _nodes[leaf].ParentOrNext = newParent; } else { // The sibling was the root. _nodes[newParent].Child1 = sibling; _nodes[newParent].Child2 = leaf; _nodes[index].ParentOrNext = newParent; _nodes[leaf].ParentOrNext = newParent; _root = newParent; } // Walk back up the tree fixing heights and AABBs index = _nodes[leaf].ParentOrNext; while (index != NullNode) { index = Balance(index); int child1 = _nodes[index].Child1; int child2 = _nodes[index].Child2; Debug.Assert(child1 != NullNode); Debug.Assert(child2 != NullNode); _nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height); _nodes[index].AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB); index = _nodes[index].ParentOrNext; } //Validate(); }
public AABBFluidContainer(Vector2 min, Vector2 max) { AABB = new AABB(ref min, ref max); }
public Map(Context ctx, Controller controller, GameObject o, Scene scene) : base(ctx, controller, o) { _scene = scene; _aabb = new AABB(new Vector3(0, 0, 0), new Vector3(100, 0, 100)); }