/// <summary> /// Checks if the specified geom intersect the specified point. /// </summary> /// <param name="geom">The geom.</param> /// <param name="point">The point.</param> /// <returns></returns> public bool Intersect(Geom geom, ref Vector2 point) { //Lookup the geometry's distancegrid DistanceGridData gridData = _distanceGrids[geom.id]; Feature feature; return(gridData.Intersect(ref point, out feature)); }
/// <summary> /// Computes the grid. /// </summary> /// <param name="geom">The geometry.</param> /// <exception cref="ArgumentNullException"><c>geometry</c> is null.</exception> public void CreateDistanceGrid(Geom geom) { if (geom == null) { throw new ArgumentNullException("geom", "Geometry can't be null"); } //Don't create distancegrid for geometry that already have one. //NOTE: This should be used to -update- the geometry's distance grid (if grid cell size has changed). if (_distanceGrids.ContainsKey(geom.id)) { return; } //By default, calculate the gridcellsize from the AABB if (geom.GridCellSize <= 0) { geom.GridCellSize = CalculateGridCellSizeFromAABB(ref geom.AABB); } //Prepare the geometry. Reset the geometry matrix Matrix old = geom.Matrix; geom.Matrix = Matrix.Identity; //Create data needed for gridcalculations AABB aabb = new AABB(ref geom.AABB); float gridCellSizeInv = 1 / geom.GridCellSize; //Note: Physics2d have +2 int xSize = (int)Math.Ceiling((double)(aabb.Max.X - aabb.Min.X) * gridCellSizeInv) + 1; int ySize = (int)Math.Ceiling((double)(aabb.Max.Y - aabb.Min.Y) * gridCellSizeInv) + 1; float[,] nodes = new float[xSize, ySize]; Vector2 vector = aabb.Min; for (int x = 0; x < xSize; ++x, vector.X += geom.GridCellSize) { vector.Y = aabb.Min.Y; for (int y = 0; y < ySize; ++y, vector.Y += geom.GridCellSize) { nodes[x, y] = geom.GetNearestDistance(ref vector); // shape.GetDistance(vector); } } //restore the geometry geom.Matrix = old; DistanceGridData distanceGridData = new DistanceGridData(); distanceGridData.AABB = aabb; distanceGridData.GridCellSize = geom.GridCellSize; distanceGridData.GridCellSizeInv = gridCellSizeInv; distanceGridData.Nodes = nodes; _distanceGrids.Add(geom.id, distanceGridData); }
/// <summary> /// Computes the grid. /// </summary> /// <param name="geom">The geometry.</param> /// <exception cref="ArgumentNullException"><c>geometry</c> is null.</exception> public void CreateDistanceGrid(Geom geom) { if (geom == null) throw new ArgumentNullException("geom", "Geometry can't be null"); //Don't create distancegrid for geometry that already have one. //NOTE: This should be used to -update- the geometry's distance grid (if grid cell size has changed). if (_distanceGrids.ContainsKey(geom.id)) return; //By default, calculate the gridcellsize from the AABB if (geom.GridCellSize <= 0) geom.GridCellSize = CalculateGridCellSizeFromAABB(ref geom.AABB); //Prepare the geometry. Reset the geometry matrix Matrix old = geom.Matrix; geom.Matrix = Matrix.Identity; //Create data needed for gridcalculations AABB aabb = new AABB(ref geom.AABB); float gridCellSizeInv = 1 / geom.GridCellSize; //Note: Physics2d have +2 int xSize = (int)Math.Ceiling((double)(aabb.Max.X - aabb.Min.X) * gridCellSizeInv) + 1; int ySize = (int)Math.Ceiling((double)(aabb.Max.Y - aabb.Min.Y) * gridCellSizeInv) + 1; float[,] nodes = new float[xSize, ySize]; Vector2 vector = aabb.Min; for (int x = 0; x < xSize; ++x, vector.X += geom.GridCellSize) { vector.Y = aabb.Min.Y; for (int y = 0; y < ySize; ++y, vector.Y += geom.GridCellSize) { nodes[x, y] = geom.GetNearestDistance(ref vector); // shape.GetDistance(vector); } } //restore the geometry geom.Matrix = old; DistanceGridData distanceGridData = new DistanceGridData(); distanceGridData.AABB = aabb; distanceGridData.GridCellSize = geom.GridCellSize; distanceGridData.GridCellSizeInv = gridCellSizeInv; distanceGridData.Nodes = nodes; _distanceGrids.Add(geom.id, distanceGridData); }
/// <summary> /// Finds the contactpoints between the two geometries. /// </summary> /// <param name="geomA">The first geom.</param> /// <param name="geomB">The second geom.</param> /// <param name="contactList">The contact list.</param> public void Collide(Geom geomA, Geom geomB, ContactList contactList) { int vertexIndex = -1; //Lookup distancegrid A data from list DistanceGridData geomAGridData = _distanceGrids[geomA.id]; //Iterate the second geometry vertices for (int i = 0; i < geomB.worldVertices.Count; i++) { if (contactList.Count == PhysicsSimulator.MaxContactsToDetect) { break; } vertexIndex += 1; _vertRef = geomB.WorldVertices[i]; geomA.TransformToLocalCoordinates(ref _vertRef, out _localVertex); //The geometry intersects when distance <= 0 //Continue in the list if the current vector does not intersect if (!geomAGridData.Intersect(ref _localVertex, out _feature)) { continue; } //If the geometries collide, create a new contact and add it to the contact list. if (_feature.Distance < 0f) { geomA.TransformNormalToWorld(ref _feature.Normal, out _feature.Normal); Contact contact = new Contact(geomB.WorldVertices[i], _feature.Normal, _feature.Distance, new ContactId(geomB.id, vertexIndex, geomA.id)); contactList.Add(contact); } } //Lookup distancegrid B data from list DistanceGridData geomBGridData = _distanceGrids[geomB.id]; //Iterate the first geometry vertices for (int i = 0; i < geomA.WorldVertices.Count; i++) { if (contactList.Count == PhysicsSimulator.MaxContactsToDetect) { break; } vertexIndex += 1; _vertRef = geomA.WorldVertices[i]; geomB.TransformToLocalCoordinates(ref _vertRef, out _localVertex); if (!geomBGridData.Intersect(ref _localVertex, out _feature)) { continue; } if (_feature.Distance < 0f) { geomB.TransformNormalToWorld(ref _feature.Normal, out _feature.Normal); _feature.Normal = -_feature.Normal; Contact contact = new Contact(geomA.WorldVertices[i], _feature.Normal, _feature.Distance, new ContactId(geomA.id, vertexIndex, geomB.id)); contactList.Add(contact); } } }
/// <summary> /// Clones this instance. /// </summary> /// <returns></returns> public DistanceGridData Clone() { DistanceGridData grid = new DistanceGridData(); grid.GridCellSize = GridCellSize; grid.GridCellSizeInv = GridCellSizeInv; grid.AABB = AABB; grid.Nodes = (float[,])Nodes.Clone(); return grid; }