using System.Collections.Generic;
using FarseerPhysics.Collision;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Common
{
// Ported by Matthew Bettcher - Feb 2011
/*
Copyright (c) 2010, Luca Deltodesco
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted
provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this list of conditions
and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice, this list of
conditions and the following disclaimer in the documentation and/or other materials provided
with the distribution.
* Neither the name of the nape project nor the names of its contributors may be used to endorse
or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
public static class MarchingSquares
{
/// <summary>
/// Marching squares over the given domain using the mesh defined via the dimensions
/// (wid,hei) to build a set of polygons such that f(x,y) less than 0, using the given number
/// 'bin' for recursive linear inteprolation along cell boundaries.
///
/// if 'comb' is true, then the polygons will also be composited into larger possible concave
/// polygons.
/// </summary>
/// <param name="domain"></param>
/// <param name="cellWidth"></param>
/// <param name="cellHeight"></param>
/// <param name="f"></param>
/// <param name="lerpCount"></param>
/// <param name="combine"></param>
/// <returns></returns>
public static List<Vertices> DetectSquares(AABB domain, float cellWidth, float cellHeight, sbyte[,] f,
int lerpCount, bool combine)
{
CxFastList<GeomPoly> ret = new CxFastList<GeomPoly>();
List<Vertices> verticesList = new List<Vertices>();
//NOTE: removed assignments as they were not used.
List<GeomPoly> polyList;
GeomPoly gp;
int xn = (int)(domain.Extents.X * 2 / cellWidth);
bool xp = xn == (domain.Extents.X * 2 / cellWidth);
int yn = (int)(domain.Extents.Y * 2 / cellHeight);
bool yp = yn == (domain.Extents.Y * 2 / cellHeight);
if (!xp) xn++;
if (!yp) yn++;
sbyte[,] fs = new sbyte[xn + 1, yn + 1];
GeomPolyVal[,] ps = new GeomPolyVal[xn + 1, yn + 1];
//populate shared function lookups.
for (int x = 0; x < xn + 1; x++)
{
int x0;
if (x == xn) x0 = (int)domain.UpperBound.X;
else x0 = (int)(x * cellWidth + domain.LowerBound.X);
for (int y = 0; y < yn + 1; y++)
{
int y0;
if (y == yn) y0 = (int)domain.UpperBound.Y;
else y0 = (int)(y * cellHeight + domain.LowerBound.Y);
fs[x, y] = f[x0, y0];
}
}
//generate sub-polys and combine to scan lines
for (int y = 0; y < yn; y++)
{
float y0 = y * cellHeight + domain.LowerBound.Y;
float y1;
if (y == yn - 1) y1 = domain.UpperBound.Y;
else y1 = y0 + cellHeight;
GeomPoly pre = null;
for (int x = 0; x < xn; x++)
{
float x0 = x * cellWidth + domain.LowerBound.X;
float x1;
if (x == xn - 1) x1 = domain.UpperBound.X;
else x1 = x0 + cellWidth;
gp = new GeomPoly();
int key = MarchSquare(f, fs, ref gp, x, y, x0, y0, x1, y1, lerpCount);
if (gp.Length != 0)
{
if (combine && pre != null && (key & 9) != 0)
{
combLeft(ref pre, ref gp);
gp = pre;
}
else
ret.Add(gp);
ps[x, y] = new GeomPolyVal(gp, key);
}
else
gp = null;
pre = gp;
}
}
if (!combine)
{
polyList = ret.GetListOfElements();
foreach (GeomPoly poly in polyList)
{
verticesList.Add(new Vertices(poly.Points.GetListOfElements()));
}
return verticesList;
}
//combine scan lines together
for (int y = 1; y < yn; y++)
{
int x = 0;
while (x < xn)
{
GeomPolyVal p = ps[x, y];
//skip along scan line if no polygon exists at this point
if (p == null)
{
x++;
continue;
}
//skip along if current polygon cannot be combined above.
if ((p.Key & 12) == 0)
{
x++;
continue;
}
//skip along if no polygon exists above.
GeomPolyVal u = ps[x, y - 1];
if (u == null)
{
x++;
continue;
}
//skip along if polygon above cannot be combined with.
if ((u.Key & 3) == 0)
{
x++;
continue;
}
float ax = x * cellWidth + domain.LowerBound.X;
float ay = y * cellHeight + domain.LowerBound.Y;
CxFastList<Vector2> bp = p.GeomP.Points;
CxFastList<Vector2> ap = u.GeomP.Points;
//skip if it's already been combined with above polygon
if (u.GeomP == p.GeomP)
{
x++;
continue;
}
//combine above (but disallow the hole thingies
CxFastListNode<Vector2> bi = bp.Begin();
while (Square(bi.Elem().Y - ay) > Settings.Epsilon || bi.Elem().X < ax) bi = bi.Next();
//NOTE: Unused
//Vector2 b0 = bi.elem();
Vector2 b1 = bi.Next().Elem();
if (Square(b1.Y - ay) > Settings.Epsilon)
{
x++;
continue;
}
bool brk = true;
CxFastListNode<Vector2> ai = ap.Begin();
while (ai != ap.End())
{
if (VecDsq(ai.Elem(), b1) < Settings.Epsilon)
{
brk = false;
break;
}
ai = ai.Next();
}
if (brk)
{
x++;
continue;
}
CxFastListNode<Vector2> bj = bi.Next().Next();
if (bj == bp.End()) bj = bp.Begin();
while (bj != bi)
{
ai = ap.Insert(ai, bj.Elem()); // .clone()
bj = bj.Next();
if (bj == bp.End()) bj = bp.Begin();
u.GeomP.Length++;
}
//u.p.simplify(float.Epsilon,float.Epsilon);
//
ax = x + 1;
while (ax < xn)
{
GeomPolyVal p2 = ps[(int)ax, y];
if (p2 == null || p2.GeomP != p.GeomP)
{
ax++;
continue;
}
p2.GeomP = u.GeomP;
ax++;
}
ax = x - 1;
while (ax >= 0)
{
GeomPolyVal p2 = ps[(int)ax, y];
if (p2 == null || p2.GeomP != p.GeomP)
{
ax--;
continue;
}
p2.GeomP = u.GeomP;
ax--;
}
ret.Remove(p.GeomP);
p.GeomP = u.GeomP;
x = (int)((bi.Next().Elem().X - domain.LowerBound.X) / cellWidth) + 1;
//x++; this was already commented out!
}
}
polyList = ret.GetListOfElements();
foreach (GeomPoly poly in polyList)
{
verticesList.Add(new Vertices(poly.Points.GetListOfElements()));
}
return verticesList;
}
#region Private Methods
//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
/** Linearly interpolate between (x0 to x1) given a value at these coordinates (v0 and v1)
such as to approximate value(return) = 0
**/
private static int[] _lookMarch = {
0x00, 0xE0, 0x38, 0xD8, 0x0E, 0xEE, 0x36, 0xD6, 0x83, 0x63, 0xBB, 0x5B, 0x8D,
0x6D, 0xB5, 0x55
};
private static float Lerp(float x0, float x1, float v0, float v1)
{
float dv = v0 - v1;
float t;
if (dv * dv < Settings.Epsilon)
t = 0.5f;
else t = v0 / dv;
return x0 + t * (x1 - x0);
}
//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
/** Recursive linear interpolation for use in marching squares **/
private static float Xlerp(float x0, float x1, float y, float v0, float v1, sbyte[,] f, int c)
{
float xm = Lerp(x0, x1, v0, v1);
if (c == 0)
return xm;
sbyte vm = f[(int)xm, (int)y];
if (v0 * vm < 0)
return Xlerp(x0, xm, y, v0, vm, f, c - 1);
return Xlerp(xm, x1, y, vm, v1, f, c - 1);
}
/** Recursive linear interpolation for use in marching squares **/
private static float Ylerp(float y0, float y1, float x, float v0, float v1, sbyte[,] f, int c)
{
float ym = Lerp(y0, y1, v0, v1);
if (c == 0)
return ym;
sbyte vm = f[(int)x, (int)ym];
if (v0 * vm < 0)
return Ylerp(y0, ym, x, v0, vm, f, c - 1);
return Ylerp(ym, y1, x, vm, v1, f, c - 1);
}
/** Square value for use in marching squares **/
private static float Square(float x)
{
return x * x;
}
private static float VecDsq(Vector2 a, Vector2 b)
{
Vector2 d = a - b;
return d.X * d.X + d.Y * d.Y;
}
private static float VecCross(Vector2 a, Vector2 b)
{
return a.X * b.Y - a.Y * b.X;
}
/** Look-up table to relate polygon key with the vertices that should be used for
the sub polygon in marching squares
**/
/** Perform a single celled marching square for for the given cell defined by (x0,y0) (x1,y1)
using the function f for recursive interpolation, given the look-up table 'fs' of
the values of 'f' at cell vertices with the result to be stored in 'poly' given the actual
coordinates of 'ax' 'ay' in the marching squares mesh.
**/
private static int MarchSquare(sbyte[,] f, sbyte[,] fs, ref GeomPoly poly, int ax, int ay, float x0, float y0,
float x1, float y1, int bin)
{
//key lookup
int key = 0;
sbyte v0 = fs[ax, ay];
if (v0 < 0) key |= 8;
sbyte v1 = fs[ax + 1, ay];
if (v1 < 0) key |= 4;
sbyte v2 = fs[ax + 1, ay + 1];
if (v2 < 0) key |= 2;
sbyte v3 = fs[ax, ay + 1];
if (v3 < 0) key |= 1;
int val = _lookMarch[key];
if (val != 0)
{
CxFastListNode<Vector2> pi = null;
for (int i = 0; i < 8; i++)
{
Vector2 p;
if ((val & (1 << i)) != 0)
{
if (i == 7 && (val & 1) == 0)
poly.Points.Add(p = new Vector2(x0, Ylerp(y0, y1, x0, v0, v3, f, bin)));
else
{
if (i == 0) p = new Vector2(x0, y0);
else if (i == 2) p = new Vector2(x1, y0);
else if (i == 4) p = new Vector2(x1, y1);
else if (i == 6) p = new Vector2(x0, y1);
else if (i == 1) p = new Vector2(Xlerp(x0, x1, y0, v0, v1, f, bin), y0);
else if (i == 5) p = new Vector2(Xlerp(x0, x1, y1, v3, v2, f, bin), y1);
else if (i == 3) p = new Vector2(x1, Ylerp(y0, y1, x1, v1, v2, f, bin));
else p = new Vector2(x0, Ylerp(y0, y1, x0, v0, v3, f, bin));
pi = poly.Points.Insert(pi, p);
}
poly.Length++;
}
}
//poly.simplify(float.Epsilon,float.Epsilon);
}
return key;
}
/** Used in polygon composition to composit polygons into scan lines
Combining polya and polyb into one super-polygon stored in polya.
**/
private static void combLeft(ref GeomPoly polya, ref GeomPoly polyb)
{
CxFastList<Vector2> ap = polya.Points;
CxFastList<Vector2> bp = polyb.Points;
CxFastListNode<Vector2> ai = ap.Begin();
CxFastListNode<Vector2> bi = bp.Begin();
Vector2 b = bi.Elem();
CxFastListNode<Vector2> prea = null;
while (ai != ap.End())
{
Vector2 a = ai.Elem();
if (VecDsq(a, b) < Settings.Epsilon)
{
//ignore shared vertex if parallel
if (prea != null)
{
Vector2 a0 = prea.Elem();
b = bi.Next().Elem();
Vector2 u = a - a0;
//vec_new(u); vec_sub(a.p.p, a0.p.p, u);
Vector2 v = b - a;
//vec_new(v); vec_sub(b.p.p, a.p.p, v);
float dot = VecCross(u, v);
if (dot * dot < Settings.Epsilon)
{
ap.Erase(prea, ai);
polya.Length--;
ai = prea;
}
}
//insert polyb into polya
bool fst = true;
CxFastListNode<Vector2> preb = null;
while (!bp.Empty())
{
Vector2 bb = bp.Front();
bp.Pop();
if (!fst && !bp.Empty())
{
ai = ap.Insert(ai, bb);
polya.Length++;
preb = ai;
}
fst = false;
}
//ignore shared vertex if parallel
ai = ai.Next();
Vector2 a1 = ai.Elem();
ai = ai.Next();
if (ai == ap.End()) ai = ap.Begin();
Vector2 a2 = ai.Elem();
Vector2 a00 = preb.Elem();
Vector2 uu = a1 - a00;
//vec_new(u); vec_sub(a1.p, a0.p, u);
Vector2 vv = a2 - a1;
//vec_new(v); vec_sub(a2.p, a1.p, v);
float dot1 = VecCross(uu, vv);
if (dot1 * dot1 < Settings.Epsilon)
{
ap.Erase(preb, preb.Next());
polya.Length--;
}
return;
}
prea = ai;
ai = ai.Next();
}
}
#endregion
#region CxFastList from nape physics
#region Nested type: CxFastList
/// <summary>
/// Designed as a complete port of CxFastList from CxStd.
/// </summary>
internal class CxFastList<T>
{
// first node in the list
private CxFastListNode<T> _head;
private int _count;
/// <summary>
/// Iterator to start of list (O(1))
/// </summary>
public CxFastListNode<T> Begin()
{
return _head;
}
/// <summary>
/// Iterator to end of list (O(1))
/// </summary>
public CxFastListNode<T> End()
{
return null;
}
/// <summary>
/// Returns first element of list (O(1))
/// </summary>
public T Front()
{
return _head.Elem();
}
/// <summary>
/// add object to list (O(1))
/// </summary>
public CxFastListNode<T> Add(T value)
{
CxFastListNode<T> newNode = new CxFastListNode<T>(value);
if (_head == null)
{
newNode._next = null;
_head = newNode;
_count++;
return newNode;
}
newNode._next = _head;
_head = newNode;
_count++;
return newNode;
}
/// <summary>
/// remove object from list, returns true if an element was removed (O(n))
/// </summary>
public bool Remove(T value)
{
CxFastListNode<T> head = _head;
CxFastListNode<T> prev = _head;
EqualityComparer<T> comparer = EqualityComparer<T>.Default;
if (head != null)
{
if (value != null)
{
do
{
// if we are on the value to be removed
if (comparer.Equals(head._elt, value))
{
// then we need to patch the list
// check to see if we are removing the _head
if (head == _head)
{
_head = head._next;
_count--;
return true;
}
else
{
// were not at the head
prev._next = head._next;
_count--;
return true;
}
}
// cache the current as the previous for the next go around
prev = head;
head = head._next;
} while (head != null);
}
}
return false;
}
/// <summary>
/// pop element from head of list (O(1)) Note: this does not return the object popped!
/// There is good reason to this, and it regards the Alloc list variants which guarantee
/// objects are released to the object pool. You do not want to retrieve an element
/// through pop or else that object may suddenly be used by another piece of code which
/// retrieves it from the object pool.
/// </summary>
public CxFastListNode<T> Pop()
{
return Erase(null, _head);
}
/// <summary>
/// insert object after 'node' returning an iterator to the inserted object.
/// </summary>
public CxFastListNode<T> Insert(CxFastListNode<T> node, T value)
{
if (node == null)
{
return Add(value);
}
CxFastListNode<T> newNode = new CxFastListNode<T>(value);
CxFastListNode<T> nextNode = node._next;
newNode._next = nextNode;
node._next = newNode;
_count++;
return newNode;
}
/// <summary>
/// removes the element pointed to by 'node' with 'prev' being the previous iterator,
/// returning an iterator to the element following that of 'node' (O(1))
/// </summary>
public CxFastListNode<T> Erase(CxFastListNode<T> prev, CxFastListNode<T> node)
{
// cache the node after the node to be removed
CxFastListNode<T> nextNode = node._next;
if (prev != null)
prev._next = nextNode;
else if (_head != null)
_head = _head._next;
else
return null;
_count--;
return nextNode;
}
/// <summary>
/// whether the list is empty (O(1))
/// </summary>
public bool Empty()
{
if (_head == null)
return true;
return false;
}
/// <summary>
/// computes size of list (O(n))
/// </summary>
public int Size()
{
CxFastListNode<T> i = Begin();
int count = 0;
do
{
count++;
} while (i.Next() != null);
return count;
}
/// <summary>
/// empty the list (O(1) if CxMixList, O(n) otherwise)
/// </summary>
public void Clear()
{
CxFastListNode<T> head = _head;
while (head != null)
{
CxFastListNode<T> node2 = head;
head = head._next;
node2._next = null;
}
_head = null;
_count = 0;
}
/// <summary>
/// returns true if 'value' is an element of the list (O(n))
/// </summary>
public bool Has(T value)
{
return (Find(value) != null);
}
// Non CxFastList Methods
public CxFastListNode<T> Find(T value)
{
// start at head
CxFastListNode<T> head = _head;
EqualityComparer<T> comparer = EqualityComparer<T>.Default;
if (head != null)
{
if (value != null)
{
do
{
if (comparer.Equals(head._elt, value))
{
return head;
}
head = head._next;
} while (head != _head);
}
else
{
do
{
if (head._elt == null)
{
return head;
}
head = head._next;
} while (head != _head);
}
}
return null;
}
public List<T> GetListOfElements()
{
List<T> list = new List<T>();
CxFastListNode<T> iter = Begin();
if (iter != null)
{
do
{
list.Add(iter._elt);
iter = iter._next;
} while (iter != null);
}
return list;
}
}
#endregion
#region Nested type: CxFastListNode
internal class CxFastListNode<T>
{
internal T _elt;
internal CxFastListNode<T> _next;
public CxFastListNode(T obj)
{
_elt = obj;
}
public T Elem()
{
return _elt;
}
public CxFastListNode<T> Next()
{
return _next;
}
}
#endregion
#endregion
#region Internal Stuff
#region Nested type: GeomPoly
internal class GeomPoly
{
public int Length;
public CxFastList<Vector2> Points;
public GeomPoly()
{
Points = new CxFastList<Vector2>();
Length = 0;
}
}
#endregion
#region Nested type: GeomPolyVal
private class GeomPolyVal
{
/** Associated polygon at coordinate **/
/** Key of original sub-polygon **/
public int Key;
public GeomPoly GeomP;
public GeomPolyVal(GeomPoly geomP, int K)
{
GeomP = geomP;
Key = K;
}
}
#endregion
#endregion
}
}
Submit an article or tip