Polygons
+ {
+ get { return _polygons; }
+ }
+
+ public void Add(Polygon p)
+ {
+ _polygons.Add(p);
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs
new file mode 100644
index 0000000..f6294f9
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs
@@ -0,0 +1,114 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Sets
+{
+ /*
+ * Extends the PointSet by adding some Constraints on how it will be triangulated
+ * A constraint defines an edge between two points in the set, these edges can not
+ * be crossed. They will be enforced triangle edges after a triangulation.
+ *
+ *
+ *
+ * @author Thomas Åhlén, thahlen@gmail.com
+ */
+
+ internal class ConstrainedPointSet : PointSet
+ {
+ private List _constrainedPointList;
+
+ public ConstrainedPointSet(List points, int[] index)
+ : base(points)
+ {
+ EdgeIndex = index;
+ }
+
+ /**
+ *
+ * @param points - A list of all points in PointSet
+ * @param constraints - Pairs of two points defining a constraint, all points must be part of given PointSet!
+ */
+
+ public ConstrainedPointSet(List points, IEnumerable constraints)
+ : base(points)
+ {
+ _constrainedPointList = new List();
+ _constrainedPointList.AddRange(constraints);
+ }
+
+ public int[] EdgeIndex { get; private set; }
+
+ public override TriangulationMode TriangulationMode
+ {
+ get { return TriangulationMode.Constrained; }
+ }
+
+ public override void PrepareTriangulation(TriangulationContext tcx)
+ {
+ base.PrepareTriangulation(tcx);
+ if (_constrainedPointList != null)
+ {
+ TriangulationPoint p1, p2;
+ List.Enumerator iterator = _constrainedPointList.GetEnumerator();
+ while (iterator.MoveNext())
+ {
+ p1 = iterator.Current;
+ iterator.MoveNext();
+ p2 = iterator.Current;
+ tcx.NewConstraint(p1, p2);
+ }
+ }
+ else
+ {
+ for (int i = 0; i < EdgeIndex.Length; i += 2)
+ {
+ // XXX: must change!!
+ tcx.NewConstraint(Points[EdgeIndex[i]], Points[EdgeIndex[i + 1]]);
+ }
+ }
+ }
+
+ /**
+ * TODO: TO BE IMPLEMENTED!
+ * Peforms a validation on given input
+ * 1. Check's if there any constraint edges are crossing or collinear
+ * 2.
+ * @return
+ */
+
+ public bool isValid()
+ {
+ return true;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/PointSet.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/PointSet.cs
new file mode 100644
index 0000000..ea840ef
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/PointSet.cs
@@ -0,0 +1,84 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+using System.Collections.Generic;
+using FarseerPhysics.Common.Decomposition.CDT.Delaunay;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Sets
+{
+ internal class PointSet : Triangulatable
+ {
+ public PointSet(List points)
+ {
+ Points = new List(points);
+ }
+
+ #region Triangulatable Members
+
+ public IList Points { get; private set; }
+ public IList Triangles { get; private set; }
+
+ public virtual TriangulationMode TriangulationMode
+ {
+ get { return TriangulationMode.Unconstrained; }
+ }
+
+ public void AddTriangle(DelaunayTriangle t)
+ {
+ Triangles.Add(t);
+ }
+
+ public void AddTriangles(IEnumerable list)
+ {
+ foreach (DelaunayTriangle tri in list) Triangles.Add(tri);
+ }
+
+ public void ClearTriangles()
+ {
+ Triangles.Clear();
+ }
+
+ public virtual void PrepareTriangulation(TriangulationContext tcx)
+ {
+ if (Triangles == null)
+ {
+ Triangles = new List(Points.Count);
+ }
+ else
+ {
+ Triangles.Clear();
+ }
+ tcx.Points.AddRange(Points);
+ }
+
+ #endregion
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationConstraint.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationConstraint.cs
new file mode 100644
index 0000000..91af927
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationConstraint.cs
@@ -0,0 +1,46 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+/**
+ * Forces a triangle edge between two points p and q
+ * when triangulating. For example used to enforce
+ * Polygon Edges during a polygon triangulation.
+ *
+ * @author Thomas Åhlén, thahlen@gmail.com
+ */
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+ internal class TriangulationConstraint
+ {
+ public TriangulationPoint P;
+ public TriangulationPoint Q;
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationContext.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationContext.cs
new file mode 100644
index 0000000..c1b9f7c
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationContext.cs
@@ -0,0 +1,84 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+using System.Collections.Generic;
+using System.Runtime.CompilerServices;
+using FarseerPhysics.Common.Decomposition.CDT.Delaunay;
+
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+ internal abstract class TriangulationContext
+ {
+ public readonly List Points = new List(200);
+ public readonly List Triangles = new List();
+ private int _stepTime = -1;
+
+ public TriangulationContext()
+ {
+ Terminated = false;
+ }
+
+ public TriangulationMode TriangulationMode { get; protected set; }
+ public Triangulatable Triangulatable { get; private set; }
+
+ public bool WaitUntilNotified { get; private set; }
+ public bool Terminated { get; set; }
+
+ public int StepCount { get; private set; }
+ public virtual bool IsDebugEnabled { get; protected set; }
+
+ public void Done()
+ {
+ StepCount++;
+ }
+
+ public virtual void PrepareTriangulation(Triangulatable t)
+ {
+ Triangulatable = t;
+ TriangulationMode = t.TriangulationMode;
+ t.PrepareTriangulation(this);
+ }
+
+ public abstract TriangulationConstraint NewConstraint(TriangulationPoint a, TriangulationPoint b);
+
+ [MethodImpl(MethodImplOptions.Synchronized)]
+ public void Update(string message)
+ {
+ }
+
+ public virtual void Clear()
+ {
+ Points.Clear();
+ Terminated = false;
+ StepCount = 0;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationMode.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationMode.cs
new file mode 100644
index 0000000..af3b83f
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationMode.cs
@@ -0,0 +1,40 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+ internal enum TriangulationMode
+ {
+ Unconstrained,
+ Constrained,
+ Polygon
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationPoint.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationPoint.cs
new file mode 100644
index 0000000..cc4d033
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationPoint.cs
@@ -0,0 +1,82 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+using System.Collections.Generic;
+using FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep;
+
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+ internal class TriangulationPoint
+ {
+ // List of edges this point constitutes an upper ending point (CDT)
+
+ public double X, Y;
+
+ public TriangulationPoint(double x, double y)
+ {
+ X = x;
+ Y = y;
+ }
+
+ public List Edges { get; private set; }
+
+ public float Xf
+ {
+ get { return (float) X; }
+ set { X = value; }
+ }
+
+ public float Yf
+ {
+ get { return (float) Y; }
+ set { Y = value; }
+ }
+
+ public bool HasEdges
+ {
+ get { return Edges != null; }
+ }
+
+ public override string ToString()
+ {
+ return "[" + X + "," + Y + "]";
+ }
+
+ public void AddEdge(DTSweepConstraint e)
+ {
+ if (Edges == null)
+ {
+ Edges = new List();
+ }
+ Edges.Add(e);
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationUtil.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationUtil.cs
new file mode 100644
index 0000000..c346e8d
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationUtil.cs
@@ -0,0 +1,175 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+ /**
+ * @author Thomas Åhlén, thahlen@gmail.com
+ */
+
+ internal class TriangulationUtil
+ {
+ public static double EPSILON = 1e-12;
+
+ ///
+ /// Requirements:
+ /// 1. a,b and c form a triangle.
+ /// 2. a and d is know to be on opposite side of bc
+ ///
+ /// a
+ /// +
+ /// / \
+ /// / \
+ /// b/ \c
+ /// +-------+
+ /// / B \
+ /// / \
+ ///
+ /// Facts:
+ /// d has to be in area B to have a chance to be inside the circle formed by a,b and c
+ /// d is outside B if orient2d(a,b,d) or orient2d(c,a,d) is CW
+ /// This preknowledge gives us a way to optimize the incircle test
+ ///
+ /// triangle point, opposite d
+ /// triangle point
+ /// triangle point
+ /// point opposite a
+ /// true if d is inside circle, false if on circle edge
+ public static bool SmartIncircle(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc,
+ TriangulationPoint pd)
+ {
+ double pdx = pd.X;
+ double pdy = pd.Y;
+ double adx = pa.X - pdx;
+ double ady = pa.Y - pdy;
+ double bdx = pb.X - pdx;
+ double bdy = pb.Y - pdy;
+
+ double adxbdy = adx * bdy;
+ double bdxady = bdx * ady;
+ double oabd = adxbdy - bdxady;
+ // oabd = orient2d(pa,pb,pd);
+ if (oabd <= 0) return false;
+
+ double cdx = pc.X - pdx;
+ double cdy = pc.Y - pdy;
+
+ double cdxady = cdx * ady;
+ double adxcdy = adx * cdy;
+ double ocad = cdxady - adxcdy;
+ // ocad = orient2d(pc,pa,pd);
+ if (ocad <= 0) return false;
+
+ double bdxcdy = bdx * cdy;
+ double cdxbdy = cdx * bdy;
+
+ double alift = adx * adx + ady * ady;
+ double blift = bdx * bdx + bdy * bdy;
+ double clift = cdx * cdx + cdy * cdy;
+
+ double det = alift * (bdxcdy - cdxbdy) + blift * ocad + clift * oabd;
+
+ return det > 0;
+ }
+ /*
+ public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc,
+ TriangulationPoint pd)
+ {
+ double pdx = pd.X;
+ double pdy = pd.Y;
+ double adx = pa.X - pdx;
+ double ady = pa.Y - pdy;
+ double bdx = pb.X - pdx;
+ double bdy = pb.Y - pdy;
+
+ double adxbdy = adx*bdy;
+ double bdxady = bdx*ady;
+ double oabd = adxbdy - bdxady;
+ // oabd = orient2d(pa,pb,pd);
+ if (oabd <= 0)
+ {
+ return false;
+ }
+
+ double cdx = pc.X - pdx;
+ double cdy = pc.Y - pdy;
+
+ double cdxady = cdx*ady;
+ double adxcdy = adx*cdy;
+ double ocad = cdxady - adxcdy;
+ // ocad = orient2d(pc,pa,pd);
+ if (ocad <= 0)
+ {
+ return false;
+ }
+ return true;
+ }
+ */
+
+ public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, TriangulationPoint pd)
+ {
+ double oadb = (pa.X - pb.X) * (pd.Y - pb.Y) - (pd.X - pb.X) * (pa.Y - pb.Y);
+ if (oadb >= -EPSILON)
+ {
+ return false;
+ }
+
+ double oadc = (pa.X - pc.X) * (pd.Y - pc.Y) - (pd.X - pc.X) * (pa.Y - pc.Y);
+ if (oadc <= EPSILON)
+ {
+ return false;
+ }
+ return true;
+ }
+
+ /// Forumla to calculate signed area
+ /// Positive if CCW
+ /// Negative if CW
+ /// 0 if collinear
+ /// A[P1,P2,P3] = (x1*y2 - y1*x2) + (x2*y3 - y2*x3) + (x3*y1 - y3*x1)
+ /// = (x1-x3)*(y2-y3) - (y1-y3)*(x2-x3)
+ public static Orientation Orient2d(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc)
+ {
+ double detleft = (pa.X - pc.X) * (pb.Y - pc.Y);
+ double detright = (pa.Y - pc.Y) * (pb.X - pc.X);
+ double val = detleft - detright;
+ if (val > -EPSILON && val < EPSILON)
+ {
+ return Orientation.Collinear;
+ }
+ else if (val > 0)
+ {
+ return Orientation.CCW;
+ }
+ return Orientation.CW;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedArray3.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedArray3.cs
new file mode 100644
index 0000000..e4d5c75
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedArray3.cs
@@ -0,0 +1,118 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Util
+{
+ internal struct FixedArray3 : IEnumerable where T : class
+ {
+ public T _0, _1, _2;
+
+ public T this[int index]
+ {
+ get
+ {
+ switch (index)
+ {
+ case 0:
+ return _0;
+ case 1:
+ return _1;
+ case 2:
+ return _2;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ set
+ {
+ switch (index)
+ {
+ case 0:
+ _0 = value;
+ break;
+ case 1:
+ _1 = value;
+ break;
+ case 2:
+ _2 = value;
+ break;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ }
+
+ #region IEnumerable Members
+
+ public IEnumerator GetEnumerator()
+ {
+ return Enumerate().GetEnumerator();
+ }
+
+ IEnumerator IEnumerable.GetEnumerator()
+ {
+ return GetEnumerator();
+ }
+
+ #endregion
+
+ public bool Contains(T value)
+ {
+ for (int i = 0; i < 3; ++i) if (this[i] == value) return true;
+ return false;
+ }
+
+ public int IndexOf(T value)
+ {
+ for (int i = 0; i < 3; ++i) if (this[i] == value) return i;
+ return -1;
+ }
+
+ public void Clear()
+ {
+ _0 = _1 = _2 = null;
+ }
+
+ public void Clear(T value)
+ {
+ for (int i = 0; i < 3; ++i) if (this[i] == value) this[i] = null;
+ }
+
+ private IEnumerable Enumerate()
+ {
+ for (int i = 0; i < 3; ++i) yield return this[i];
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedBitArray3.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedBitArray3.cs
new file mode 100644
index 0000000..bb488ba
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedBitArray3.cs
@@ -0,0 +1,118 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Util
+{
+ internal struct FixedBitArray3 : IEnumerable
+ {
+ public bool _0, _1, _2;
+
+ public bool this[int index]
+ {
+ get
+ {
+ switch (index)
+ {
+ case 0:
+ return _0;
+ case 1:
+ return _1;
+ case 2:
+ return _2;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ set
+ {
+ switch (index)
+ {
+ case 0:
+ _0 = value;
+ break;
+ case 1:
+ _1 = value;
+ break;
+ case 2:
+ _2 = value;
+ break;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ }
+
+ #region IEnumerable Members
+
+ public IEnumerator GetEnumerator()
+ {
+ return Enumerate().GetEnumerator();
+ }
+
+ IEnumerator IEnumerable.GetEnumerator()
+ {
+ return GetEnumerator();
+ }
+
+ #endregion
+
+ public bool Contains(bool value)
+ {
+ for (int i = 0; i < 3; ++i) if (this[i] == value) return true;
+ return false;
+ }
+
+ public int IndexOf(bool value)
+ {
+ for (int i = 0; i < 3; ++i) if (this[i] == value) return i;
+ return -1;
+ }
+
+ public void Clear()
+ {
+ _0 = _1 = _2 = false;
+ }
+
+ public void Clear(bool value)
+ {
+ for (int i = 0; i < 3; ++i) if (this[i] == value) this[i] = false;
+ }
+
+ private IEnumerable Enumerate()
+ {
+ for (int i = 0; i < 3; ++i) yield return this[i];
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PointGenerator.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PointGenerator.cs
new file mode 100644
index 0000000..96f44ad
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PointGenerator.cs
@@ -0,0 +1,38 @@
+using System;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Util
+{
+ internal class PointGenerator
+ {
+ private static readonly Random RNG = new Random();
+
+ public static List UniformDistribution(int n, double scale)
+ {
+ List points = new List();
+ for (int i = 0; i < n; i++)
+ {
+ points.Add(new TriangulationPoint(scale*(0.5 - RNG.NextDouble()), scale*(0.5 - RNG.NextDouble())));
+ }
+ return points;
+ }
+
+ public static List UniformGrid(int n, double scale)
+ {
+ double x = 0;
+ double size = scale/n;
+ double halfScale = 0.5*scale;
+
+ List points = new List();
+ for (int i = 0; i < n + 1; i++)
+ {
+ x = halfScale - i*size;
+ for (int j = 0; j < n + 1; j++)
+ {
+ points.Add(new TriangulationPoint(x, halfScale - j*size));
+ }
+ }
+ return points;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PolygonGenerator.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PolygonGenerator.cs
new file mode 100644
index 0000000..e91bb03
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PolygonGenerator.cs
@@ -0,0 +1,98 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * 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 Poly2Tri 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 OWNER 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.
+ */
+
+using System;
+using FarseerPhysics.Common.Decomposition.CDT.Polygon;
+
+namespace FarseerPhysics.Common.Decomposition.CDT.Util
+{
+ internal class PolygonGenerator
+ {
+ private static readonly Random RNG = new Random();
+
+ private static double PI_2 = 2.0*Math.PI;
+
+ public static Polygon.Polygon RandomCircleSweep(double scale, int vertexCount)
+ {
+ PolygonPoint point;
+ PolygonPoint[] points;
+ double radius = scale/4;
+
+ points = new PolygonPoint[vertexCount];
+ for (int i = 0; i < vertexCount; i++)
+ {
+ do
+ {
+ if (i%250 == 0)
+ {
+ radius += scale/2*(0.5 - RNG.NextDouble());
+ }
+ else if (i%50 == 0)
+ {
+ radius += scale/5*(0.5 - RNG.NextDouble());
+ }
+ else
+ {
+ radius += 25*scale/vertexCount*(0.5 - RNG.NextDouble());
+ }
+ radius = radius > scale/2 ? scale/2 : radius;
+ radius = radius < scale/10 ? scale/10 : radius;
+ } while (radius < scale/10 || radius > scale/2);
+ point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount),
+ radius*Math.Sin((PI_2*i)/vertexCount));
+ points[i] = point;
+ }
+ return new Polygon.Polygon(points);
+ }
+
+ public static Polygon.Polygon RandomCircleSweep2(double scale, int vertexCount)
+ {
+ PolygonPoint point;
+ PolygonPoint[] points;
+ double radius = scale/4;
+
+ points = new PolygonPoint[vertexCount];
+ for (int i = 0; i < vertexCount; i++)
+ {
+ do
+ {
+ radius += scale/5*(0.5 - RNG.NextDouble());
+ radius = radius > scale/2 ? scale/2 : radius;
+ radius = radius < scale/10 ? scale/10 : radius;
+ } while (radius < scale/10 || radius > scale/2);
+ point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount),
+ radius*Math.Sin((PI_2*i)/vertexCount));
+ points[i] = point;
+ }
+ return new Polygon.Polygon(points);
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDTDecomposer.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDTDecomposer.cs
new file mode 100644
index 0000000..48618e3
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/CDTDecomposer.cs
@@ -0,0 +1,75 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*/
+
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common.Decomposition.CDT;
+using FarseerPhysics.Common.Decomposition.CDT.Delaunay;
+using FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep;
+using FarseerPhysics.Common.Decomposition.CDT.Polygon;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+ ///
+ /// 2D constrained Delaunay triangulation algorithm.
+ /// Based on the paper "Sweep-line algorithm for constrained Delaunay triangulation" by V. Domiter and and B. Zalik
+ ///
+ /// Properties:
+ /// - Creates triangles with a large interior angle.
+ /// - Supports holes
+ /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library.
+ /// - Running time is O(n^2), n = number of vertices.
+ /// - Does not care about winding order.
+ ///
+ /// Source: http://code.google.com/p/poly2tri/
+ ///
+ internal static class CDTDecomposer
+ {
+ ///
+ /// Decompose the polygon into several smaller non-concave polygon.
+ ///
+ public static List ConvexPartition(Vertices vertices)
+ {
+ Debug.Assert(vertices.Count > 3);
+
+ Polygon poly = new Polygon();
+
+ foreach (Vector2 vertex in vertices)
+ poly.Points.Add(new TriangulationPoint(vertex.X, vertex.Y));
+
+ if (vertices.Holes != null)
+ {
+ foreach (Vertices holeVertices in vertices.Holes)
+ {
+ Polygon hole = new Polygon();
+
+ foreach (Vector2 vertex in holeVertices)
+ hole.Points.Add(new TriangulationPoint(vertex.X, vertex.Y));
+
+ poly.AddHole(hole);
+ }
+ }
+
+ DTSweepContext tcx = new DTSweepContext();
+ tcx.PrepareTriangulation(poly);
+ DTSweep.Triangulate(tcx);
+
+ List results = new List();
+
+ foreach (DelaunayTriangle triangle in poly.Triangles)
+ {
+ Vertices v = new Vertices();
+ foreach (TriangulationPoint p in triangle.Points)
+ {
+ v.Add(new Vector2((float)p.X, (float)p.Y));
+ }
+ results.Add(v);
+ }
+
+ return results;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/EarclipDecomposer.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/EarclipDecomposer.cs
new file mode 100644
index 0000000..489e56f
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/EarclipDecomposer.cs
@@ -0,0 +1,403 @@
+/*
+* C# Version Ported by Matt Bettcher and Ian Qvist 2009-2010
+*
+* Original C++ Version Copyright (c) 2007 Eric Jordan
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+ ///
+ /// Convex decomposition algorithm using ear clipping
+ ///
+ /// Properties:
+ /// - Only works on simple polygons.
+ /// - Does not support holes.
+ /// - Running time is O(n^2), n = number of vertices.
+ ///
+ /// Source: http://www.ewjordan.com/earClip/
+ ///
+ internal static class EarclipDecomposer
+ {
+ //box2D rev 32 - for details, see http://www.box2d.org/forum/viewtopic.php?f=4&t=83&start=50
+
+ ///
+ /// Decompose the polygon into several smaller non-concave polygon.
+ /// Each resulting polygon will have no more than Settings.MaxPolygonVertices vertices.
+ ///
+ /// The vertices.
+ /// The tolerance.
+ public static List ConvexPartition(Vertices vertices, float tolerance = 0.001f)
+ {
+ Debug.Assert(vertices.Count > 3);
+ Debug.Assert(!vertices.IsCounterClockWise());
+
+ return TriangulatePolygon(vertices, tolerance);
+ }
+
+ ///
+ /// Triangulates a polygon using simple ear-clipping algorithm. Returns
+ /// size of Triangle array unless the polygon can't be triangulated.
+ /// This should only happen if the polygon self-intersects,
+ /// though it will not _always_ return null for a bad polygon - it is the
+ /// caller's responsibility to check for self-intersection, and if it
+ /// doesn't, it should at least check that the return value is non-null
+ /// before using. You're warned!
+ ///
+ /// Triangles may be degenerate, especially if you have identical points
+ /// in the input to the algorithm. Check this before you use them.
+ ///
+ /// This is totally unoptimized, so for large polygons it should not be part
+ /// of the simulation loop.
+ ///
+ ///
+ /// Only works on simple polygons.
+ ///
+ private static List TriangulatePolygon(Vertices vertices, float tolerance)
+ {
+ //FPE note: Check is needed as invalid triangles can be returned in recursive calls.
+ if (vertices.Count < 3)
+ return new List();
+
+ List results = new List();
+
+ //Recurse and split on pinch points
+ Vertices pA, pB;
+ Vertices pin = new Vertices(vertices);
+ if (ResolvePinchPoint(pin, out pA, out pB, tolerance))
+ {
+ List mergeA = TriangulatePolygon(pA, tolerance);
+ List mergeB = TriangulatePolygon(pB, tolerance);
+
+ if (mergeA.Count == -1 || mergeB.Count == -1)
+ throw new Exception("Can't triangulate your polygon.");
+
+ for (int i = 0; i < mergeA.Count; ++i)
+ {
+ results.Add(new Vertices(mergeA[i]));
+ }
+ for (int i = 0; i < mergeB.Count; ++i)
+ {
+ results.Add(new Vertices(mergeB[i]));
+ }
+
+ return results;
+ }
+
+ Vertices[] buffer = new Vertices[vertices.Count - 2];
+ int bufferSize = 0;
+ float[] xrem = new float[vertices.Count];
+ float[] yrem = new float[vertices.Count];
+ for (int i = 0; i < vertices.Count; ++i)
+ {
+ xrem[i] = vertices[i].X;
+ yrem[i] = vertices[i].Y;
+ }
+
+ int vNum = vertices.Count;
+
+ while (vNum > 3)
+ {
+ // Find an ear
+ int earIndex = -1;
+ float earMaxMinCross = -10.0f;
+ for (int i = 0; i < vNum; ++i)
+ {
+ if (IsEar(i, xrem, yrem, vNum))
+ {
+ int lower = Remainder(i - 1, vNum);
+ int upper = Remainder(i + 1, vNum);
+ Vector2 d1 = new Vector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]);
+ Vector2 d2 = new Vector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]);
+ Vector2 d3 = new Vector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]);
+
+ d1.Normalize();
+ d2.Normalize();
+ d3.Normalize();
+ float cross12;
+ MathUtils.Cross(ref d1, ref d2, out cross12);
+ cross12 = Math.Abs(cross12);
+
+ float cross23;
+ MathUtils.Cross(ref d2, ref d3, out cross23);
+ cross23 = Math.Abs(cross23);
+
+ float cross31;
+ MathUtils.Cross(ref d3, ref d1, out cross31);
+ cross31 = Math.Abs(cross31);
+
+ //Find the maximum minimum angle
+ float minCross = Math.Min(cross12, Math.Min(cross23, cross31));
+ if (minCross > earMaxMinCross)
+ {
+ earIndex = i;
+ earMaxMinCross = minCross;
+ }
+ }
+ }
+
+ // If we still haven't found an ear, we're screwed.
+ // Note: sometimes this is happening because the
+ // remaining points are collinear. Really these
+ // should just be thrown out without halting triangulation.
+ if (earIndex == -1)
+ {
+ for (int i = 0; i < bufferSize; i++)
+ {
+ results.Add(buffer[i]);
+ }
+
+ return results;
+ }
+
+ // Clip off the ear:
+ // - remove the ear tip from the list
+
+ --vNum;
+ float[] newx = new float[vNum];
+ float[] newy = new float[vNum];
+ int currDest = 0;
+ for (int i = 0; i < vNum; ++i)
+ {
+ if (currDest == earIndex) ++currDest;
+ newx[i] = xrem[currDest];
+ newy[i] = yrem[currDest];
+ ++currDest;
+ }
+
+ // - add the clipped triangle to the triangle list
+ int under = (earIndex == 0) ? (vNum) : (earIndex - 1);
+ int over = (earIndex == vNum) ? 0 : (earIndex + 1);
+ Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under],
+ yrem[under]);
+ buffer[bufferSize] = toAdd;
+ ++bufferSize;
+
+ // - replace the old list with the new one
+ xrem = newx;
+ yrem = newy;
+ }
+
+ Triangle tooAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2], xrem[0], yrem[0]);
+ buffer[bufferSize] = tooAdd;
+ ++bufferSize;
+
+ for (int i = 0; i < bufferSize; i++)
+ {
+ results.Add(new Vertices(buffer[i]));
+ }
+
+ return results;
+ }
+
+ ///
+ /// Finds and fixes "pinch points," points where two polygon
+ /// vertices are at the same point.
+ ///
+ /// If a pinch point is found, pin is broken up into poutA and poutB
+ /// and true is returned; otherwise, returns false.
+ ///
+ /// Mostly for internal use.
+ ///
+ /// O(N^2) time, which sucks...
+ ///
+ /// The pin.
+ /// The pout A.
+ /// The pout B.
+ ///
+ private static bool ResolvePinchPoint(Vertices pin, out Vertices poutA, out Vertices poutB, float tolerance)
+ {
+ poutA = new Vertices();
+ poutB = new Vertices();
+
+ if (pin.Count < 3)
+ return false;
+
+ bool hasPinchPoint = false;
+ int pinchIndexA = -1;
+ int pinchIndexB = -1;
+ for (int i = 0; i < pin.Count; ++i)
+ {
+ for (int j = i + 1; j < pin.Count; ++j)
+ {
+ //Don't worry about pinch points where the points
+ //are actually just dupe neighbors
+ if (Math.Abs(pin[i].X - pin[j].X) < tolerance && Math.Abs(pin[i].Y - pin[j].Y) < tolerance && j != i + 1)
+ {
+ pinchIndexA = i;
+ pinchIndexB = j;
+ hasPinchPoint = true;
+ break;
+ }
+ }
+ if (hasPinchPoint) break;
+ }
+ if (hasPinchPoint)
+ {
+ int sizeA = pinchIndexB - pinchIndexA;
+ if (sizeA == pin.Count) return false; //has dupe points at wraparound, not a problem here
+ for (int i = 0; i < sizeA; ++i)
+ {
+ int ind = Remainder(pinchIndexA + i, pin.Count); // is this right
+ poutA.Add(pin[ind]);
+ }
+
+ int sizeB = pin.Count - sizeA;
+ for (int i = 0; i < sizeB; ++i)
+ {
+ int ind = Remainder(pinchIndexB + i, pin.Count); // is this right
+ poutB.Add(pin[ind]);
+ }
+ }
+ return hasPinchPoint;
+ }
+
+ ///
+ /// Fix for obnoxious behavior for the % operator for negative numbers...
+ ///
+ /// The x.
+ /// The modulus.
+ ///
+ private static int Remainder(int x, int modulus)
+ {
+ int rem = x % modulus;
+ while (rem < 0)
+ {
+ rem += modulus;
+ }
+ return rem;
+ }
+
+ ///
+ /// Checks if vertex i is the tip of an ear in polygon defined by xv[] and yv[].
+ ///
+ /// The i.
+ /// The xv.
+ /// The yv.
+ /// Length of the xv.
+ ///
+ /// Assumes clockwise orientation of polygon.
+ ///
+ ///
+ /// true if the specified i is ear; otherwise, false.
+ ///
+ private static bool IsEar(int i, float[] xv, float[] yv, int xvLength)
+ {
+ float dx0, dy0, dx1, dy1;
+ if (i >= xvLength || i < 0 || xvLength < 3)
+ {
+ return false;
+ }
+ int upper = i + 1;
+ int lower = i - 1;
+ if (i == 0)
+ {
+ dx0 = xv[0] - xv[xvLength - 1];
+ dy0 = yv[0] - yv[xvLength - 1];
+ dx1 = xv[1] - xv[0];
+ dy1 = yv[1] - yv[0];
+ lower = xvLength - 1;
+ }
+ else if (i == xvLength - 1)
+ {
+ dx0 = xv[i] - xv[i - 1];
+ dy0 = yv[i] - yv[i - 1];
+ dx1 = xv[0] - xv[i];
+ dy1 = yv[0] - yv[i];
+ upper = 0;
+ }
+ else
+ {
+ dx0 = xv[i] - xv[i - 1];
+ dy0 = yv[i] - yv[i - 1];
+ dx1 = xv[i + 1] - xv[i];
+ dy1 = yv[i + 1] - yv[i];
+ }
+
+ float cross = dx0 * dy1 - dx1 * dy0;
+
+ if (cross > 0)
+ return false;
+
+ Triangle myTri = new Triangle(xv[i], yv[i], xv[upper], yv[upper], xv[lower], yv[lower]);
+
+ for (int j = 0; j < xvLength; ++j)
+ {
+ if (j == i || j == lower || j == upper)
+ continue;
+ if (myTri.IsInside(xv[j], yv[j]))
+ return false;
+ }
+ return true;
+ }
+
+ private class Triangle : Vertices
+ {
+ //Constructor automatically fixes orientation to ccw
+ public Triangle(float x1, float y1, float x2, float y2, float x3, float y3)
+ {
+ float cross = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
+ if (cross > 0)
+ {
+ Add(new Vector2(x1, y1));
+ Add(new Vector2(x2, y2));
+ Add(new Vector2(x3, y3));
+ }
+ else
+ {
+ Add(new Vector2(x1, y1));
+ Add(new Vector2(x3, y3));
+ Add(new Vector2(x2, y2));
+ }
+ }
+
+ public bool IsInside(float x, float y)
+ {
+ Vector2 a = this[0];
+ Vector2 b = this[1];
+ Vector2 c = this[2];
+
+ if (x < a.X && x < b.X && x < c.X) return false;
+ if (x > a.X && x > b.X && x > c.X) return false;
+ if (y < a.Y && y < b.Y && y < c.Y) return false;
+ if (y > a.Y && y > b.Y && y > c.Y) return false;
+
+ float vx2 = x - a.X;
+ float vy2 = y - a.Y;
+ float vx1 = b.X - a.X;
+ float vy1 = b.Y - a.Y;
+ float vx0 = c.X - a.X;
+ float vy0 = c.Y - a.Y;
+
+ float dot00 = vx0 * vx0 + vy0 * vy0;
+ float dot01 = vx0 * vx1 + vy0 * vy1;
+ float dot02 = vx0 * vx2 + vy0 * vy2;
+ float dot11 = vx1 * vx1 + vy1 * vy1;
+ float dot12 = vx1 * vx2 + vy1 * vy2;
+ float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
+ float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+ float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+ return ((u > 0) && (v > 0) && (u + v < 1));
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/FlipcodeDecomposer.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/FlipcodeDecomposer.cs
new file mode 100644
index 0000000..895cd3f
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/FlipcodeDecomposer.cs
@@ -0,0 +1,152 @@
+using System.Collections.Generic;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+ ///
+ /// Convex decomposition algorithm created by unknown
+ ///
+ /// Properties:
+ /// - No support for holes
+ /// - Very fast
+ /// - Only works on simple polygons
+ /// - Only works on counter clockwise polygons
+ ///
+ /// More information: http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml
+ ///
+ internal static class FlipcodeDecomposer
+ {
+ private static Vector2 _tmpA;
+ private static Vector2 _tmpB;
+ private static Vector2 _tmpC;
+
+ ///
+ /// Decompose the polygon into triangles.
+ ///
+ /// Properties:
+ /// - Only works on counter clockwise polygons
+ ///
+ ///
+ /// The list of points describing the polygon
+ public static List ConvexPartition(Vertices vertices)
+ {
+ Debug.Assert(vertices.Count > 3);
+ Debug.Assert(vertices.IsCounterClockWise());
+
+ int[] polygon = new int[vertices.Count];
+
+ for (int v = 0; v < vertices.Count; v++)
+ polygon[v] = v;
+
+ int nv = vertices.Count;
+
+ // Remove nv-2 Vertices, creating 1 triangle every time
+ int count = 2 * nv; /* error detection */
+
+ List result = new List();
+
+ for (int v = nv - 1; nv > 2; )
+ {
+ // If we loop, it is probably a non-simple polygon
+ if (0 >= (count--))
+ {
+ // Triangulate: ERROR - probable bad polygon!
+ return new List();
+ }
+
+ // Three consecutive vertices in current polygon,
+ int u = v;
+ if (nv <= u)
+ u = 0; // Previous
+ v = u + 1;
+ if (nv <= v)
+ v = 0; // New v
+ int w = v + 1;
+ if (nv <= w)
+ w = 0; // Next
+
+ _tmpA = vertices[polygon[u]];
+ _tmpB = vertices[polygon[v]];
+ _tmpC = vertices[polygon[w]];
+
+ if (Snip(vertices, u, v, w, nv, polygon))
+ {
+ int s, t;
+
+ // Output Triangle
+ Vertices triangle = new Vertices(3);
+ triangle.Add(_tmpA);
+ triangle.Add(_tmpB);
+ triangle.Add(_tmpC);
+ result.Add(triangle);
+
+ // Remove v from remaining polygon
+ for (s = v, t = v + 1; t < nv; s++, t++)
+ {
+ polygon[s] = polygon[t];
+ }
+ nv--;
+
+ // Reset error detection counter
+ count = 2 * nv;
+ }
+ }
+
+ return result;
+ }
+
+ ///
+ /// Check if the point P is inside the triangle defined by
+ /// the points A, B, C
+ ///
+ /// The A point.
+ /// The B point.
+ /// The C point.
+ /// The point to be tested.
+ /// True if the point is inside the triangle
+ private static bool InsideTriangle(ref Vector2 a, ref Vector2 b, ref Vector2 c, ref Vector2 p)
+ {
+ //A cross bp
+ float abp = (c.X - b.X) * (p.Y - b.Y) - (c.Y - b.Y) * (p.X - b.X);
+
+ //A cross ap
+ float aap = (b.X - a.X) * (p.Y - a.Y) - (b.Y - a.Y) * (p.X - a.X);
+
+ //b cross cp
+ float bcp = (a.X - c.X) * (p.Y - c.Y) - (a.Y - c.Y) * (p.X - c.X);
+
+ return ((abp >= 0.0f) && (bcp >= 0.0f) && (aap >= 0.0f));
+ }
+
+ ///
+ /// Cut a the contour and add a triangle into V to describe the
+ /// location of the cut
+ ///
+ /// The list of points defining the polygon
+ /// The index of the first point
+ /// The index of the second point
+ /// The index of the third point
+ /// The number of elements in the array.
+ /// The array to populate with indicies of triangles.
+ /// True if a triangle was found
+ private static bool Snip(Vertices contour, int u, int v, int w, int n, int[] V)
+ {
+ if (Settings.Epsilon > MathUtils.Area(ref _tmpA, ref _tmpB, ref _tmpC))
+ return false;
+
+ for (int p = 0; p < n; p++)
+ {
+ if ((p == u) || (p == v) || (p == w))
+ continue;
+
+ Vector2 point = contour[V[p]];
+
+ if (InsideTriangle(ref _tmpA, ref _tmpB, ref _tmpC, ref point))
+ return false;
+ }
+
+ return true;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Edge.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Edge.cs
new file mode 100644
index 0000000..116158d
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Edge.cs
@@ -0,0 +1,60 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class Edge
+ {
+ // Pointers used for building trapezoidal map
+ public Trapezoid Above;
+ public float B;
+ public Trapezoid Below;
+
+ // Montone mountain points
+ public HashSet MPoints;
+ public Point P;
+ public Point Q;
+
+ // Slope of the line (m)
+ public float Slope;
+
+
+ public Edge(Point p, Point q)
+ {
+ P = p;
+ Q = q;
+
+ if (q.X - p.X != 0)
+ Slope = (q.Y - p.Y) / (q.X - p.X);
+ else
+ Slope = 0;
+
+ B = p.Y - (p.X * Slope);
+ Above = null;
+ Below = null;
+ MPoints = new HashSet();
+ MPoints.Add(p);
+ MPoints.Add(q);
+ }
+
+ public bool IsAbove(Point point)
+ {
+ return P.Orient2D(Q, point) < 0;
+ }
+
+ public bool IsBelow(Point point)
+ {
+ return P.Orient2D(Q, point) > 0;
+ }
+
+ public void AddMpoint(Point point)
+ {
+ foreach (Point mp in MPoints)
+ {
+ if (!mp.Neq(point))
+ return;
+ }
+
+ MPoints.Add(point);
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/MonotoneMountain.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/MonotoneMountain.cs
new file mode 100644
index 0000000..862f528
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/MonotoneMountain.cs
@@ -0,0 +1,166 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class MonotoneMountain
+ {
+ // Almost Pi!
+ private const float PiSlop = 3.1f;
+
+ // Triangles that constitute the mountain
+ public List> Triangles;
+ private HashSet _convexPoints;
+ private Point _head;
+
+ // Monotone mountain points
+ private List _monoPoly;
+
+ // Used to track which side of the line we are on
+ private bool _positive;
+ private int _size;
+ private Point _tail;
+
+ public MonotoneMountain()
+ {
+ _size = 0;
+ _tail = null;
+ _head = null;
+ _positive = false;
+ _convexPoints = new HashSet();
+ _monoPoly = new List();
+ Triangles = new List>();
+ }
+
+ // Append a point to the list
+ public void Add(Point point)
+ {
+ if (_size == 0)
+ {
+ _head = point;
+ _size = 1;
+ }
+ else if (_size == 1)
+ {
+ // Keep repeat points out of the list
+ _tail = point;
+ _tail.Prev = _head;
+ _head.Next = _tail;
+ _size = 2;
+ }
+ else
+ {
+ // Keep repeat points out of the list
+ _tail.Next = point;
+ point.Prev = _tail;
+ _tail = point;
+ _size += 1;
+ }
+ }
+
+ // Remove a point from the list
+ public void Remove(Point point)
+ {
+ Point next = point.Next;
+ Point prev = point.Prev;
+ point.Prev.Next = next;
+ point.Next.Prev = prev;
+ _size -= 1;
+ }
+
+ // Partition a x-monotone mountain into triangles O(n)
+ // See "Computational Geometry in C", 2nd edition, by Joseph O'Rourke, page 52
+ public void Process()
+ {
+ // Establish the proper sign
+ _positive = AngleSign();
+ // create monotone polygon - for dubug purposes
+ GenMonoPoly();
+
+ // Initialize internal angles at each nonbase vertex
+ // Link strictly convex vertices into a list, ignore reflex vertices
+ Point p = _head.Next;
+ while (p.Neq(_tail))
+ {
+ float a = Angle(p);
+ // If the point is almost colinear with it's neighbor, remove it!
+ if (a >= PiSlop || a <= -PiSlop || a == 0.0f)
+ Remove(p);
+ else if (IsConvex(p))
+ _convexPoints.Add(p);
+ p = p.Next;
+ }
+
+ Triangulate();
+ }
+
+ private void Triangulate()
+ {
+ while (_convexPoints.Count != 0)
+ {
+ IEnumerator e = _convexPoints.GetEnumerator();
+ e.MoveNext();
+ Point ear = e.Current;
+
+ _convexPoints.Remove(ear);
+ Point a = ear.Prev;
+ Point b = ear;
+ Point c = ear.Next;
+ List triangle = new List(3);
+ triangle.Add(a);
+ triangle.Add(b);
+ triangle.Add(c);
+
+ Triangles.Add(triangle);
+
+ // Remove ear, update angles and convex list
+ Remove(ear);
+ if (Valid(a))
+ _convexPoints.Add(a);
+ if (Valid(c))
+ _convexPoints.Add(c);
+ }
+
+ Debug.Assert(_size <= 3, "Triangulation bug, please report");
+ }
+
+ private bool Valid(Point p)
+ {
+ return p.Neq(_head) && p.Neq(_tail) && IsConvex(p);
+ }
+
+ // Create the monotone polygon
+ private void GenMonoPoly()
+ {
+ Point p = _head;
+ while (p != null)
+ {
+ _monoPoly.Add(p);
+ p = p.Next;
+ }
+ }
+
+ private float Angle(Point p)
+ {
+ Point a = (p.Next - p);
+ Point b = (p.Prev - p);
+ return (float)Math.Atan2(a.Cross(b), a.Dot(b));
+ }
+
+ private bool AngleSign()
+ {
+ Point a = (_head.Next - _head);
+ Point b = (_tail - _head);
+ return Math.Atan2(a.Cross(b), a.Dot(b)) >= 0;
+ }
+
+ // Determines if the inslide angle is convex or reflex
+ private bool IsConvex(Point p)
+ {
+ if (_positive != (Angle(p) >= 0))
+ return false;
+ return true;
+ }
+ }
+}
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Node.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Node.cs
new file mode 100644
index 0000000..0ef99d0
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Node.cs
@@ -0,0 +1,41 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ // Node for a Directed Acyclic graph (DAG)
+ internal abstract class Node
+ {
+ protected Node LeftChild;
+ public List ParentList;
+ protected Node RightChild;
+
+ protected Node(Node left, Node right)
+ {
+ ParentList = new List();
+ LeftChild = left;
+ RightChild = right;
+
+ if (left != null)
+ left.ParentList.Add(this);
+ if (right != null)
+ right.ParentList.Add(this);
+ }
+
+ public abstract Sink Locate(Edge s);
+
+ // Replace a node in the graph with this node
+ // Make sure parent pointers are updated
+ public void Replace(Node node)
+ {
+ foreach (Node parent in node.ParentList)
+ {
+ // Select the correct node to replace (left or right child)
+ if (parent.LeftChild == node)
+ parent.LeftChild = this;
+ else
+ parent.RightChild = this;
+ }
+ ParentList.AddRange(node.ParentList);
+ }
+ }
+}
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Point.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Point.cs
new file mode 100644
index 0000000..21167d4
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Point.cs
@@ -0,0 +1,61 @@
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class Point
+ {
+ // Pointers to next and previous points in Monontone Mountain
+ public Point Next, Prev;
+ public float X, Y;
+
+ public Point(float x, float y)
+ {
+ X = x;
+ Y = y;
+ Next = null;
+ Prev = null;
+ }
+
+ public static Point operator -(Point p1, Point p2)
+ {
+ return new Point(p1.X - p2.X, p1.Y - p2.Y);
+ }
+
+ public static Point operator +(Point p1, Point p2)
+ {
+ return new Point(p1.X + p2.X, p1.Y + p2.Y);
+ }
+
+ public static Point operator -(Point p1, float f)
+ {
+ return new Point(p1.X - f, p1.Y - f);
+ }
+
+ public static Point operator +(Point p1, float f)
+ {
+ return new Point(p1.X + f, p1.Y + f);
+ }
+
+ public float Cross(Point p)
+ {
+ return X * p.Y - Y * p.X;
+ }
+
+ public float Dot(Point p)
+ {
+ return X * p.X + Y * p.Y;
+ }
+
+ public bool Neq(Point p)
+ {
+ return p.X != X || p.Y != Y;
+ }
+
+ public float Orient2D(Point pb, Point pc)
+ {
+ float acx = X - pc.X;
+ float bcx = pb.X - pc.X;
+ float acy = Y - pc.Y;
+ float bcy = pb.Y - pc.Y;
+ return acx * bcy - acy * bcx;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/QueryGraph.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/QueryGraph.cs
new file mode 100644
index 0000000..7fb5001
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/QueryGraph.cs
@@ -0,0 +1,78 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ // Directed Acyclic graph (DAG)
+ // See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
+ internal class QueryGraph
+ {
+ private Node _head;
+
+ public QueryGraph(Node head)
+ {
+ _head = head;
+ }
+
+ private Trapezoid Locate(Edge edge)
+ {
+ return _head.Locate(edge).Trapezoid;
+ }
+
+ public List FollowEdge(Edge edge)
+ {
+ List trapezoids = new List();
+ trapezoids.Add(Locate(edge));
+ int j = 0;
+
+ while (edge.Q.X > trapezoids[j].RightPoint.X)
+ {
+ if (edge.IsAbove(trapezoids[j].RightPoint))
+ {
+ trapezoids.Add(trapezoids[j].UpperRight);
+ }
+ else
+ {
+ trapezoids.Add(trapezoids[j].LowerRight);
+ }
+ j += 1;
+ }
+ return trapezoids;
+ }
+
+ private void Replace(Sink sink, Node node)
+ {
+ if (sink.ParentList.Count == 0)
+ _head = node;
+ else
+ node.Replace(sink);
+ }
+
+ public void Case1(Sink sink, Edge edge, Trapezoid[] tList)
+ {
+ YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2]));
+ XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[3]));
+ XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), qNode);
+ Replace(sink, pNode);
+ }
+
+ public void Case2(Sink sink, Edge edge, Trapezoid[] tList)
+ {
+ YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2]));
+ XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), yNode);
+ Replace(sink, pNode);
+ }
+
+ public void Case3(Sink sink, Edge edge, Trapezoid[] tList)
+ {
+ YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1]));
+ Replace(sink, yNode);
+ }
+
+ public void Case4(Sink sink, Edge edge, Trapezoid[] tList)
+ {
+ YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1]));
+ XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[2]));
+ Replace(sink, qNode);
+ }
+ }
+}
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Sink.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Sink.cs
new file mode 100644
index 0000000..2672f4b
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Sink.cs
@@ -0,0 +1,27 @@
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class Sink : Node
+ {
+ public Trapezoid Trapezoid;
+
+ private Sink(Trapezoid trapezoid)
+ : base(null, null)
+ {
+ Trapezoid = trapezoid;
+ trapezoid.Sink = this;
+ }
+
+ public static Sink Isink(Trapezoid trapezoid)
+ {
+ if (trapezoid.Sink == null)
+ return new Sink(trapezoid);
+
+ return trapezoid.Sink;
+ }
+
+ public override Sink Locate(Edge edge)
+ {
+ return this;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Trapezoid.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Trapezoid.cs
new file mode 100644
index 0000000..92fa994
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Trapezoid.cs
@@ -0,0 +1,123 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class Trapezoid
+ {
+ public Edge Bottom;
+ public bool Inside;
+ public Point LeftPoint;
+
+ // Neighbor pointers
+ public Trapezoid LowerLeft;
+ public Trapezoid LowerRight;
+
+ public Point RightPoint;
+ public Sink Sink;
+
+ public Edge Top;
+ public Trapezoid UpperLeft;
+ public Trapezoid UpperRight;
+
+ public Trapezoid(Point leftPoint, Point rightPoint, Edge top, Edge bottom)
+ {
+ LeftPoint = leftPoint;
+ RightPoint = rightPoint;
+ Top = top;
+ Bottom = bottom;
+ UpperLeft = null;
+ UpperRight = null;
+ LowerLeft = null;
+ LowerRight = null;
+ Inside = true;
+ Sink = null;
+ }
+
+ // Update neighbors to the left
+ public void UpdateLeft(Trapezoid ul, Trapezoid ll)
+ {
+ UpperLeft = ul;
+ if (ul != null) ul.UpperRight = this;
+ LowerLeft = ll;
+ if (ll != null) ll.LowerRight = this;
+ }
+
+ // Update neighbors to the right
+ public void UpdateRight(Trapezoid ur, Trapezoid lr)
+ {
+ UpperRight = ur;
+ if (ur != null) ur.UpperLeft = this;
+ LowerRight = lr;
+ if (lr != null) lr.LowerLeft = this;
+ }
+
+ // Update neighbors on both sides
+ public void UpdateLeftRight(Trapezoid ul, Trapezoid ll, Trapezoid ur, Trapezoid lr)
+ {
+ UpperLeft = ul;
+ if (ul != null) ul.UpperRight = this;
+ LowerLeft = ll;
+ if (ll != null) ll.LowerRight = this;
+ UpperRight = ur;
+ if (ur != null) ur.UpperLeft = this;
+ LowerRight = lr;
+ if (lr != null) lr.LowerLeft = this;
+ }
+
+ // Recursively trim outside neighbors
+ public void TrimNeighbors()
+ {
+ if (Inside)
+ {
+ Inside = false;
+ if (UpperLeft != null) UpperLeft.TrimNeighbors();
+ if (LowerLeft != null) LowerLeft.TrimNeighbors();
+ if (UpperRight != null) UpperRight.TrimNeighbors();
+ if (LowerRight != null) LowerRight.TrimNeighbors();
+ }
+ }
+
+ // Determines if this point lies inside the trapezoid
+ public bool Contains(Point point)
+ {
+ return (point.X > LeftPoint.X && point.X < RightPoint.X && Top.IsAbove(point) && Bottom.IsBelow(point));
+ }
+
+ public List GetVertices()
+ {
+ List verts = new List(4);
+ verts.Add(LineIntersect(Top, LeftPoint.X));
+ verts.Add(LineIntersect(Bottom, LeftPoint.X));
+ verts.Add(LineIntersect(Bottom, RightPoint.X));
+ verts.Add(LineIntersect(Top, RightPoint.X));
+ return verts;
+ }
+
+ private Point LineIntersect(Edge edge, float x)
+ {
+ float y = edge.Slope * x + edge.B;
+ return new Point(x, y);
+ }
+
+ // Add points to monotone mountain
+ public void AddPoints()
+ {
+ if (LeftPoint != Bottom.P)
+ {
+ Bottom.AddMpoint(LeftPoint);
+ }
+ if (RightPoint != Bottom.Q)
+ {
+ Bottom.AddMpoint(RightPoint);
+ }
+ if (LeftPoint != Top.P)
+ {
+ Top.AddMpoint(LeftPoint);
+ }
+ if (RightPoint != Top.Q)
+ {
+ Top.AddMpoint(RightPoint);
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/TrapezoidalMap.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/TrapezoidalMap.cs
new file mode 100644
index 0000000..7a89976
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/TrapezoidalMap.cs
@@ -0,0 +1,195 @@
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class TrapezoidalMap
+ {
+ // Trapezoid container
+ public HashSet Map;
+
+ // Bottom segment that spans multiple trapezoids
+ private Edge _bCross;
+
+ // Top segment that spans multiple trapezoids
+ private Edge _cross;
+
+ // AABB margin
+ private float _margin;
+
+ public TrapezoidalMap()
+ {
+ Map = new HashSet();
+ _margin = 50.0f;
+ _bCross = null;
+ _cross = null;
+ }
+
+ public void Clear()
+ {
+ _bCross = null;
+ _cross = null;
+ }
+
+ // Case 1: segment completely enclosed by trapezoid
+ // break trapezoid into 4 smaller trapezoids
+ public Trapezoid[] Case1(Trapezoid t, Edge e)
+ {
+ Trapezoid[] trapezoids = new Trapezoid[4];
+ trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom);
+ trapezoids[1] = new Trapezoid(e.P, e.Q, t.Top, e);
+ trapezoids[2] = new Trapezoid(e.P, e.Q, e, t.Bottom);
+ trapezoids[3] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom);
+
+ trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft);
+ trapezoids[1].UpdateLeftRight(trapezoids[0], null, trapezoids[3], null);
+ trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, trapezoids[3]);
+ trapezoids[3].UpdateRight(t.UpperRight, t.LowerRight);
+
+ return trapezoids;
+ }
+
+ // Case 2: Trapezoid contains point p, q lies outside
+ // break trapezoid into 3 smaller trapezoids
+ public Trapezoid[] Case2(Trapezoid t, Edge e)
+ {
+ Point rp;
+ if (e.Q.X == t.RightPoint.X)
+ rp = e.Q;
+ else
+ rp = t.RightPoint;
+
+ Trapezoid[] trapezoids = new Trapezoid[3];
+ trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom);
+ trapezoids[1] = new Trapezoid(e.P, rp, t.Top, e);
+ trapezoids[2] = new Trapezoid(e.P, rp, e, t.Bottom);
+
+ trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft);
+ trapezoids[1].UpdateLeftRight(trapezoids[0], null, t.UpperRight, null);
+ trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, t.LowerRight);
+
+ _bCross = t.Bottom;
+ _cross = t.Top;
+
+ e.Above = trapezoids[1];
+ e.Below = trapezoids[2];
+
+ return trapezoids;
+ }
+
+ // Case 3: Trapezoid is bisected
+ public Trapezoid[] Case3(Trapezoid t, Edge e)
+ {
+ Point lp;
+ if (e.P.X == t.LeftPoint.X)
+ lp = e.P;
+ else
+ lp = t.LeftPoint;
+
+ Point rp;
+ if (e.Q.X == t.RightPoint.X)
+ rp = e.Q;
+ else
+ rp = t.RightPoint;
+
+ Trapezoid[] trapezoids = new Trapezoid[2];
+
+ if (_cross == t.Top)
+ {
+ trapezoids[0] = t.UpperLeft;
+ trapezoids[0].UpdateRight(t.UpperRight, null);
+ trapezoids[0].RightPoint = rp;
+ }
+ else
+ {
+ trapezoids[0] = new Trapezoid(lp, rp, t.Top, e);
+ trapezoids[0].UpdateLeftRight(t.UpperLeft, e.Above, t.UpperRight, null);
+ }
+
+ if (_bCross == t.Bottom)
+ {
+ trapezoids[1] = t.LowerLeft;
+ trapezoids[1].UpdateRight(null, t.LowerRight);
+ trapezoids[1].RightPoint = rp;
+ }
+ else
+ {
+ trapezoids[1] = new Trapezoid(lp, rp, e, t.Bottom);
+ trapezoids[1].UpdateLeftRight(e.Below, t.LowerLeft, null, t.LowerRight);
+ }
+
+ _bCross = t.Bottom;
+ _cross = t.Top;
+
+ e.Above = trapezoids[0];
+ e.Below = trapezoids[1];
+
+ return trapezoids;
+ }
+
+ // Case 4: Trapezoid contains point q, p lies outside
+ // break trapezoid into 3 smaller trapezoids
+ public Trapezoid[] Case4(Trapezoid t, Edge e)
+ {
+ Point lp;
+ if (e.P.X == t.LeftPoint.X)
+ lp = e.P;
+ else
+ lp = t.LeftPoint;
+
+ Trapezoid[] trapezoids = new Trapezoid[3];
+
+ if (_cross == t.Top)
+ {
+ trapezoids[0] = t.UpperLeft;
+ trapezoids[0].RightPoint = e.Q;
+ }
+ else
+ {
+ trapezoids[0] = new Trapezoid(lp, e.Q, t.Top, e);
+ trapezoids[0].UpdateLeft(t.UpperLeft, e.Above);
+ }
+
+ if (_bCross == t.Bottom)
+ {
+ trapezoids[1] = t.LowerLeft;
+ trapezoids[1].RightPoint = e.Q;
+ }
+ else
+ {
+ trapezoids[1] = new Trapezoid(lp, e.Q, e, t.Bottom);
+ trapezoids[1].UpdateLeft(e.Below, t.LowerLeft);
+ }
+
+ trapezoids[2] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom);
+ trapezoids[2].UpdateLeftRight(trapezoids[0], trapezoids[1], t.UpperRight, t.LowerRight);
+
+ return trapezoids;
+ }
+
+ // Create an AABB around segments
+ public Trapezoid BoundingBox(List edges)
+ {
+ Point max = edges[0].P + _margin;
+ Point min = edges[0].Q - _margin;
+
+ foreach (Edge e in edges)
+ {
+ if (e.P.X > max.X) max = new Point(e.P.X + _margin, max.Y);
+ if (e.P.Y > max.Y) max = new Point(max.X, e.P.Y + _margin);
+ if (e.Q.X > max.X) max = new Point(e.Q.X + _margin, max.Y);
+ if (e.Q.Y > max.Y) max = new Point(max.X, e.Q.Y + _margin);
+ if (e.P.X < min.X) min = new Point(e.P.X - _margin, min.Y);
+ if (e.P.Y < min.Y) min = new Point(min.X, e.P.Y - _margin);
+ if (e.Q.X < min.X) min = new Point(e.Q.X - _margin, min.Y);
+ if (e.Q.Y < min.Y) min = new Point(min.X, e.Q.Y - _margin);
+ }
+
+ Edge top = new Edge(new Point(min.X, max.Y), new Point(max.X, max.Y));
+ Edge bottom = new Edge(new Point(min.X, min.Y), new Point(max.X, min.Y));
+ Point left = bottom.P;
+ Point right = top.Q;
+
+ return new Trapezoid(left, right, top, bottom);
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Triangulator.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Triangulator.cs
new file mode 100644
index 0000000..cf44812
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Triangulator.cs
@@ -0,0 +1,203 @@
+using System;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class Triangulator
+ {
+ // Trapezoid decomposition list
+ public List Trapezoids;
+ public List> Triangles;
+
+ // Initialize trapezoidal map and query structure
+ private Trapezoid _boundingBox;
+ private List _edgeList;
+ private QueryGraph _queryGraph;
+ private float _sheer = 0.001f;
+ private TrapezoidalMap _trapezoidalMap;
+ private List _xMonoPoly;
+
+ public Triangulator(List polyLine, float sheer)
+ {
+ _sheer = sheer;
+ Triangles = new List>();
+ Trapezoids = new List();
+ _xMonoPoly = new List();
+ _edgeList = InitEdges(polyLine);
+ _trapezoidalMap = new TrapezoidalMap();
+ _boundingBox = _trapezoidalMap.BoundingBox(_edgeList);
+ _queryGraph = new QueryGraph(Sink.Isink(_boundingBox));
+
+ Process();
+ }
+
+ // Build the trapezoidal map and query graph
+ private void Process()
+ {
+ foreach (Edge edge in _edgeList)
+ {
+ List traps = _queryGraph.FollowEdge(edge);
+
+ // Remove trapezoids from trapezoidal Map
+ foreach (Trapezoid t in traps)
+ {
+ _trapezoidalMap.Map.Remove(t);
+
+ bool cp = t.Contains(edge.P);
+ bool cq = t.Contains(edge.Q);
+ Trapezoid[] tList;
+
+ if (cp && cq)
+ {
+ tList = _trapezoidalMap.Case1(t, edge);
+ _queryGraph.Case1(t.Sink, edge, tList);
+ }
+ else if (cp && !cq)
+ {
+ tList = _trapezoidalMap.Case2(t, edge);
+ _queryGraph.Case2(t.Sink, edge, tList);
+ }
+ else if (!cp && !cq)
+ {
+ tList = _trapezoidalMap.Case3(t, edge);
+ _queryGraph.Case3(t.Sink, edge, tList);
+ }
+ else
+ {
+ tList = _trapezoidalMap.Case4(t, edge);
+ _queryGraph.Case4(t.Sink, edge, tList);
+ }
+ // Add new trapezoids to map
+ foreach (Trapezoid y in tList)
+ {
+ _trapezoidalMap.Map.Add(y);
+ }
+ }
+ _trapezoidalMap.Clear();
+ }
+
+ // Mark outside trapezoids
+ foreach (Trapezoid t in _trapezoidalMap.Map)
+ {
+ MarkOutside(t);
+ }
+
+ // Collect interior trapezoids
+ foreach (Trapezoid t in _trapezoidalMap.Map)
+ {
+ if (t.Inside)
+ {
+ Trapezoids.Add(t);
+ t.AddPoints();
+ }
+ }
+
+ // Generate the triangles
+ CreateMountains();
+ }
+
+ // Build a list of x-monotone mountains
+ private void CreateMountains()
+ {
+ foreach (Edge edge in _edgeList)
+ {
+ if (edge.MPoints.Count > 2)
+ {
+ MonotoneMountain mountain = new MonotoneMountain();
+
+ // Sorting is a perfromance hit. Literature says this can be accomplised in
+ // linear time, although I don't see a way around using traditional methods
+ // when using a randomized incremental algorithm
+
+ // Insertion sort is one of the fastest algorithms for sorting arrays containing
+ // fewer than ten elements, or for lists that are already mostly sorted.
+
+ List points = new List(edge.MPoints);
+ points.Sort((p1, p2) => p1.X.CompareTo(p2.X));
+
+ foreach (Point p in points)
+ mountain.Add(p);
+
+ // Triangulate monotone mountain
+ mountain.Process();
+
+ // Extract the triangles into a single list
+ foreach (List t in mountain.Triangles)
+ {
+ Triangles.Add(t);
+ }
+
+ _xMonoPoly.Add(mountain);
+ }
+ }
+ }
+
+ // Mark the outside trapezoids surrounding the polygon
+ private void MarkOutside(Trapezoid t)
+ {
+ if (t.Top == _boundingBox.Top || t.Bottom == _boundingBox.Bottom)
+ t.TrimNeighbors();
+ }
+
+ // Create segments and connect end points; update edge event pointer
+ private List InitEdges(List points)
+ {
+ List edges = new List();
+
+ for (int i = 0; i < points.Count - 1; i++)
+ {
+ edges.Add(new Edge(points[i], points[i + 1]));
+ }
+ edges.Add(new Edge(points[0], points[points.Count - 1]));
+ return OrderSegments(edges);
+ }
+
+ private List OrderSegments(List edgeInput)
+ {
+ // Ignore vertical segments!
+ List edges = new List();
+
+ foreach (Edge e in edgeInput)
+ {
+ Point p = ShearTransform(e.P);
+ Point q = ShearTransform(e.Q);
+
+ // Point p must be to the left of point q
+ if (p.X > q.X)
+ {
+ edges.Add(new Edge(q, p));
+ }
+ else if (p.X < q.X)
+ {
+ edges.Add(new Edge(p, q));
+ }
+ }
+
+ // Randomized triangulation improves performance
+ // See Seidel's paper, or O'Rourke's book, p. 57
+ Shuffle(edges);
+ return edges;
+ }
+
+ private static void Shuffle(IList list)
+ {
+ Random rng = new Random();
+ int n = list.Count;
+ while (n > 1)
+ {
+ n--;
+ int k = rng.Next(n + 1);
+ T value = list[k];
+ list[k] = list[n];
+ list[n] = value;
+ }
+ }
+
+ // Prevents any two distinct endpoints from lying on a common vertical line, and avoiding
+ // the degenerate case. See Mark de Berg et al, Chapter 6.3
+ private Point ShearTransform(Point point)
+ {
+ return new Point(point.X + _sheer * point.Y, point.Y);
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/XNode.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/XNode.cs
new file mode 100644
index 0000000..b623415
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/XNode.cs
@@ -0,0 +1,21 @@
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class XNode : Node
+ {
+ private Point _point;
+
+ public XNode(Point point, Node lChild, Node rChild)
+ : base(lChild, rChild)
+ {
+ _point = point;
+ }
+
+ public override Sink Locate(Edge edge)
+ {
+ if (edge.P.X >= _point.X)
+ return RightChild.Locate(edge); // Move to the right in the graph
+
+ return LeftChild.Locate(edge); // Move to the left in the graph
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/YNode.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/YNode.cs
new file mode 100644
index 0000000..b534ab2
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/YNode.cs
@@ -0,0 +1,29 @@
+namespace FarseerPhysics.Common.Decomposition.Seidel
+{
+ internal class YNode : Node
+ {
+ private Edge _edge;
+
+ public YNode(Edge edge, Node lChild, Node rChild)
+ : base(lChild, rChild)
+ {
+ _edge = edge;
+ }
+
+ public override Sink Locate(Edge edge)
+ {
+ if (_edge.IsAbove(edge.P))
+ return RightChild.Locate(edge); // Move down the graph
+
+ if (_edge.IsBelow(edge.P))
+ return LeftChild.Locate(edge); // Move up the graph
+
+ // s and segment share the same endpoint, p
+ if (edge.Slope < _edge.Slope)
+ return RightChild.Locate(edge); // Move down the graph
+
+ // Move up the graph
+ return LeftChild.Locate(edge);
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/SeidelDecomposer.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/SeidelDecomposer.cs
new file mode 100644
index 0000000..d4c0b9d
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/SeidelDecomposer.cs
@@ -0,0 +1,109 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*/
+
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common.Decomposition.Seidel;
+using Microsoft.Xna.Framework;
+using Point = FarseerPhysics.Common.Decomposition.Seidel.Point;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+ ///
+ /// Convex decomposition algorithm created by Raimund Seidel
+ ///
+ /// Properties:
+ /// - Decompose the polygon into trapezoids, then triangulate.
+ /// - To use the trapezoid data, use ConvexPartitionTrapezoid()
+ /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library.
+ /// - Running time is O(n log n), n = number of vertices.
+ /// - Running time is almost linear for most simple polygons.
+ /// - Does not care about winding order.
+ ///
+ /// For more information, see Raimund Seidel's paper "A simple and fast incremental randomized
+ /// algorithm for computing trapezoidal decompositions and for triangulating polygons"
+ ///
+ /// See also: "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
+ /// "Computational Geometry in C", 2nd edition, by Joseph O'Rourke
+ ///
+ /// Original code from the Poly2Tri project by Mason Green.
+ /// http://code.google.com/p/poly2tri/source/browse?repo=archive#hg/scala/src/org/poly2tri/seidel
+ ///
+ /// This implementation is from Dec 14, 2010
+ ///
+ internal static class SeidelDecomposer
+ {
+ ///
+ /// Decompose the polygon into several smaller non-concave polygons.
+ ///
+ /// The polygon to decompose.
+ /// The sheer to use if you get bad results, try using a higher value.
+ /// A list of triangles
+ public static List ConvexPartition(Vertices vertices, float sheer = 0.001f)
+ {
+ Debug.Assert(vertices.Count > 3);
+
+ List compatList = new List(vertices.Count);
+
+ foreach (Vector2 vertex in vertices)
+ {
+ compatList.Add(new Point(vertex.X, vertex.Y));
+ }
+
+ Triangulator t = new Triangulator(compatList, sheer);
+
+ List list = new List();
+
+ foreach (List triangle in t.Triangles)
+ {
+ Vertices outTriangles = new Vertices(triangle.Count);
+
+ foreach (Point outTriangle in triangle)
+ {
+ outTriangles.Add(new Vector2(outTriangle.X, outTriangle.Y));
+ }
+
+ list.Add(outTriangles);
+ }
+
+ return list;
+ }
+
+ ///
+ /// Decompose the polygon into several smaller non-concave polygons.
+ ///
+ /// The polygon to decompose.
+ /// The sheer to use if you get bad results, try using a higher value.
+ /// A list of trapezoids
+ public static List ConvexPartitionTrapezoid(Vertices vertices, float sheer = 0.001f)
+ {
+ List compatList = new List(vertices.Count);
+
+ foreach (Vector2 vertex in vertices)
+ {
+ compatList.Add(new Point(vertex.X, vertex.Y));
+ }
+
+ Triangulator t = new Triangulator(compatList, sheer);
+
+ List list = new List();
+
+ foreach (Trapezoid trapezoid in t.Trapezoids)
+ {
+ Vertices verts = new Vertices();
+
+ List points = trapezoid.GetVertices();
+ foreach (Point point in points)
+ {
+ verts.Add(new Vector2(point.X, point.Y));
+ }
+
+ list.Add(verts);
+ }
+
+ return list;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Triangulate.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Triangulate.cs
new file mode 100644
index 0000000..884180f
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Decomposition/Triangulate.cs
@@ -0,0 +1,172 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common.ConvexHull;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+ public enum TriangulationAlgorithm
+ {
+ ///
+ /// Convex decomposition algorithm using ear clipping
+ ///
+ /// Properties:
+ /// - Only works on simple polygons.
+ /// - Does not support holes.
+ /// - Running time is O(n^2), n = number of vertices.
+ ///
+ Earclip,
+
+ ///
+ /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/)
+ ///
+ /// Properties:
+ /// - Tries to decompose using polygons instead of triangles.
+ /// - Tends to produce optimal results with low processing time.
+ /// - Running time is O(nr), n = number of vertices, r = reflex vertices.
+ /// - Does not support holes.
+ ///
+ Bayazit,
+
+ ///
+ /// Convex decomposition algorithm created by unknown
+ ///
+ /// Properties:
+ /// - No support for holes
+ /// - Very fast
+ /// - Only works on simple polygons
+ /// - Only works on counter clockwise polygons
+ ///
+ Flipcode,
+
+ ///
+ /// Convex decomposition algorithm created by Raimund Seidel
+ ///
+ /// Properties:
+ /// - Decompose the polygon into trapezoids, then triangulate.
+ /// - To use the trapezoid data, use ConvexPartitionTrapezoid()
+ /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library.
+ /// - Running time is O(n log n), n = number of vertices.
+ /// - Running time is almost linear for most simple polygons.
+ /// - Does not care about winding order.
+ ///
+ Seidel,
+ SeidelTrapezoids,
+
+ ///
+ /// 2D constrained Delaunay triangulation algorithm.
+ /// Based on the paper "Sweep-line algorithm for constrained Delaunay triangulation" by V. Domiter and and B. Zalik
+ ///
+ /// Properties:
+ /// - Creates triangles with a large interior angle.
+ /// - Supports holes
+ /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library.
+ /// - Running time is O(n^2), n = number of vertices.
+ /// - Does not care about winding order.
+ ///
+ Delauny
+ }
+
+ public static class Triangulate
+ {
+ public static List ConvexPartition(Vertices vertices, TriangulationAlgorithm algorithm, bool discardAndFixInvalid = true, float tolerance = 0.001f)
+ {
+ if (vertices.Count <= 3)
+ return new List { vertices };
+
+ List results;
+
+ switch (algorithm)
+ {
+ case TriangulationAlgorithm.Earclip:
+ if (Settings.SkipSanityChecks)
+ Debug.Assert(!vertices.IsCounterClockWise(), "The Earclip algorithm expects the polygon to be clockwise.");
+ else
+ {
+ if (vertices.IsCounterClockWise())
+ {
+ Vertices temp = new Vertices(vertices);
+ temp.Reverse();
+ results = EarclipDecomposer.ConvexPartition(temp, tolerance);
+ }
+ else
+ results = EarclipDecomposer.ConvexPartition(vertices, tolerance);
+ }
+ break;
+ case TriangulationAlgorithm.Bayazit:
+ if (Settings.SkipSanityChecks)
+ Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly.");
+ else
+ {
+ if (!vertices.IsCounterClockWise())
+ {
+ Vertices temp = new Vertices(vertices);
+ temp.Reverse();
+ results = BayazitDecomposer.ConvexPartition(temp);
+ }
+ else
+ results = BayazitDecomposer.ConvexPartition(vertices);
+ }
+ break;
+ case TriangulationAlgorithm.Flipcode:
+ if (Settings.SkipSanityChecks)
+ Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly.");
+ else
+ {
+ if (!vertices.IsCounterClockWise())
+ {
+ Vertices temp = new Vertices(vertices);
+ temp.Reverse();
+ results = FlipcodeDecomposer.ConvexPartition(temp);
+ }
+ else
+ results = FlipcodeDecomposer.ConvexPartition(vertices);
+ }
+ break;
+ case TriangulationAlgorithm.Seidel:
+ results = SeidelDecomposer.ConvexPartition(vertices, tolerance);
+ break;
+ case TriangulationAlgorithm.SeidelTrapezoids:
+ results = SeidelDecomposer.ConvexPartitionTrapezoid(vertices, tolerance);
+ break;
+ case TriangulationAlgorithm.Delauny:
+ results = CDTDecomposer.ConvexPartition(vertices);
+ break;
+ default:
+ throw new ArgumentOutOfRangeException("algorithm");
+ }
+
+ if (discardAndFixInvalid)
+ {
+ for (int i = results.Count - 1; i >= 0; i--)
+ {
+ Vertices polygon = results[i];
+
+ if (!ValidatePolygon(polygon))
+ results.RemoveAt(i);
+ }
+ }
+
+ return results;
+ }
+
+ private static bool ValidatePolygon(Vertices polygon)
+ {
+ PolygonError errorCode = polygon.CheckPolygon();
+
+ if (errorCode == PolygonError.InvalidAmountOfVertices || errorCode == PolygonError.AreaTooSmall || errorCode == PolygonError.SideTooSmall || errorCode == PolygonError.NotSimple)
+ return false;
+
+ if (errorCode == PolygonError.NotCounterClockWise) //NotCounterCloseWise is the last check in CheckPolygon(), thus we don't need to call ValidatePolygon again.
+ polygon.Reverse();
+
+ if (errorCode == PolygonError.NotConvex)
+ {
+ polygon = GiftWrap.GetConvexHull(polygon);
+ return ValidatePolygon(polygon);
+ }
+
+ return true;
+ }
+ }
+}
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/FixedArray.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/FixedArray.cs
new file mode 100644
index 0000000..c628470
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/FixedArray.cs
@@ -0,0 +1,224 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+
+namespace FarseerPhysics.Common
+{
+ public struct FixedArray2
+ {
+ private T _value0;
+ private T _value1;
+
+ public T this[int index]
+ {
+ get
+ {
+ switch (index)
+ {
+ case 0:
+ return _value0;
+ case 1:
+ return _value1;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ set
+ {
+ switch (index)
+ {
+ case 0:
+ _value0 = value;
+ break;
+ case 1:
+ _value1 = value;
+ break;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ }
+ }
+
+ public struct FixedArray3
+ {
+ private T _value0;
+ private T _value1;
+ private T _value2;
+
+ public T this[int index]
+ {
+ get
+ {
+ switch (index)
+ {
+ case 0:
+ return _value0;
+ case 1:
+ return _value1;
+ case 2:
+ return _value2;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ set
+ {
+ switch (index)
+ {
+ case 0:
+ _value0 = value;
+ break;
+ case 1:
+ _value1 = value;
+ break;
+ case 2:
+ _value2 = value;
+ break;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ }
+ }
+
+ public struct FixedArray4
+ {
+ private T _value0;
+ private T _value1;
+ private T _value2;
+ private T _value3;
+
+ public T this[int index]
+ {
+ get
+ {
+ switch (index)
+ {
+ case 0:
+ return _value0;
+ case 1:
+ return _value1;
+ case 2:
+ return _value2;
+ case 3:
+ return _value3;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ set
+ {
+ switch (index)
+ {
+ case 0:
+ _value0 = value;
+ break;
+ case 1:
+ _value1 = value;
+ break;
+ case 2:
+ _value2 = value;
+ break;
+ case 3:
+ _value3 = value;
+ break;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ }
+ }
+
+ public struct FixedArray8
+ {
+ private T _value0;
+ private T _value1;
+ private T _value2;
+ private T _value3;
+ private T _value4;
+ private T _value5;
+ private T _value6;
+ private T _value7;
+
+ public T this[int index]
+ {
+ get
+ {
+ switch (index)
+ {
+ case 0:
+ return _value0;
+ case 1:
+ return _value1;
+ case 2:
+ return _value2;
+ case 3:
+ return _value3;
+ case 4:
+ return _value4;
+ case 5:
+ return _value5;
+ case 6:
+ return _value6;
+ case 7:
+ return _value7;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ set
+ {
+ switch (index)
+ {
+ case 0:
+ _value0 = value;
+ break;
+ case 1:
+ _value1 = value;
+ break;
+ case 2:
+ _value2 = value;
+ break;
+ case 3:
+ _value3 = value;
+ break;
+ case 4:
+ _value4 = value;
+ break;
+ case 5:
+ _value5 = value;
+ break;
+ case 6:
+ _value6 = value;
+ break;
+ case 7:
+ _value7 = value;
+ break;
+ default:
+ throw new IndexOutOfRangeException();
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/HashSet.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/HashSet.cs
new file mode 100644
index 0000000..ce3e26f
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/HashSet.cs
@@ -0,0 +1,78 @@
+#if WINDOWS_PHONE || XBOX
+
+using System.Collections;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common
+{
+ public class HashSet : ICollection
+ {
+ private Dictionary _dict;
+
+ public HashSet(int capacity)
+ {
+ _dict = new Dictionary(capacity);
+ }
+
+ public HashSet()
+ {
+ _dict = new Dictionary();
+ }
+
+ #region ICollection Members
+
+ public void Add(T item)
+ {
+ // We don't care for the value in dictionary, only keys matter.
+ if (!_dict.ContainsKey(item))
+ _dict.Add(item, 0);
+ }
+
+ public void Clear()
+ {
+ _dict.Clear();
+ }
+
+ public bool Contains(T item)
+ {
+ return _dict.ContainsKey(item);
+ }
+
+ public void CopyTo(T[] array, int arrayIndex)
+ {
+ foreach (var item in _dict.Keys)
+ {
+ array[arrayIndex++] = item;
+ }
+ }
+
+ public bool Remove(T item)
+ {
+ return _dict.Remove(item);
+ }
+
+ public IEnumerator GetEnumerator()
+ {
+ return _dict.Keys.GetEnumerator();
+ }
+
+ IEnumerator IEnumerable.GetEnumerator()
+ {
+ return _dict.Keys.GetEnumerator();
+ }
+
+ // Properties
+ public int Count
+ {
+ get { return _dict.Keys.Count; }
+ }
+
+ public bool IsReadOnly
+ {
+ get { return false; }
+ }
+
+ #endregion
+ }
+}
+#endif
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/LineTools.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/LineTools.cs
new file mode 100644
index 0000000..8b6b8e4
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/LineTools.cs
@@ -0,0 +1,287 @@
+using System;
+using FarseerPhysics.Collision;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common
+{
+ ///
+ /// Collection of helper methods for misc collisions.
+ /// Does float tolerance and line collisions with lines and AABBs.
+ ///
+ public static class LineTools
+ {
+ public static float DistanceBetweenPointAndLineSegment(ref Vector2 point, ref Vector2 start, ref Vector2 end)
+ {
+ if (start == end)
+ return Vector2.Distance(point, start);
+
+ Vector2 v = Vector2.Subtract(end, start);
+ Vector2 w = Vector2.Subtract(point, start);
+
+ float c1 = Vector2.Dot(w, v);
+ if (c1 <= 0) return Vector2.Distance(point, start);
+
+ float c2 = Vector2.Dot(v, v);
+ if (c2 <= c1) return Vector2.Distance(point, end);
+
+ float b = c1 / c2;
+ Vector2 pointOnLine = Vector2.Add(start, Vector2.Multiply(v, b));
+ return Vector2.Distance(point, pointOnLine);
+ }
+
+ // From Eric Jordan's convex decomposition library
+ ///
+ ///Check if the lines a0->a1 and b0->b1 cross.
+ ///If they do, intersectionPoint will be filled
+ ///with the point of crossing.
+ ///
+ ///Grazing lines should not return true.
+ ///
+ ///
+ public static bool LineIntersect2(ref Vector2 a0, ref Vector2 a1, ref Vector2 b0, ref Vector2 b1, out Vector2 intersectionPoint)
+ {
+ intersectionPoint = Vector2.Zero;
+
+ if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1)
+ return false;
+
+ float x1 = a0.X;
+ float y1 = a0.Y;
+ float x2 = a1.X;
+ float y2 = a1.Y;
+ float x3 = b0.X;
+ float y3 = b0.Y;
+ float x4 = b1.X;
+ float y4 = b1.Y;
+
+ //AABB early exit
+ if (Math.Max(x1, x2) < Math.Min(x3, x4) || Math.Max(x3, x4) < Math.Min(x1, x2))
+ return false;
+
+ if (Math.Max(y1, y2) < Math.Min(y3, y4) || Math.Max(y3, y4) < Math.Min(y1, y2))
+ return false;
+
+ float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
+ float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
+ float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
+ if (Math.Abs(denom) < Settings.Epsilon)
+ {
+ //Lines are too close to parallel to call
+ return false;
+ }
+ ua /= denom;
+ ub /= denom;
+
+ if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1))
+ {
+ intersectionPoint.X = (x1 + ua * (x2 - x1));
+ intersectionPoint.Y = (y1 + ua * (y2 - y1));
+ return true;
+ }
+
+ return false;
+ }
+
+ //From Mark Bayazit's convex decomposition algorithm
+ public static Vector2 LineIntersect(Vector2 p1, Vector2 p2, Vector2 q1, Vector2 q2)
+ {
+ Vector2 i = Vector2.Zero;
+ float a1 = p2.Y - p1.Y;
+ float b1 = p1.X - p2.X;
+ float c1 = a1 * p1.X + b1 * p1.Y;
+ float a2 = q2.Y - q1.Y;
+ float b2 = q1.X - q2.X;
+ float c2 = a2 * q1.X + b2 * q1.Y;
+ float det = a1 * b2 - a2 * b1;
+
+ if (!MathUtils.FloatEquals(det, 0))
+ {
+ // lines are not parallel
+ i.X = (b2 * c1 - b1 * c2) / det;
+ i.Y = (a1 * c2 - a2 * c1) / det;
+ }
+ return i;
+ }
+
+ ///
+ /// This method detects if two line segments (or lines) intersect,
+ /// and, if so, the point of intersection. Use the and
+ /// parameters to set whether the intersection point
+ /// must be on the first and second line segments. Setting these
+ /// both to true means you are doing a line-segment to line-segment
+ /// intersection. Setting one of them to true means you are doing a
+ /// line to line-segment intersection test, and so on.
+ /// Note: If two line segments are coincident, then
+ /// no intersection is detected (there are actually
+ /// infinite intersection points).
+ /// Author: Jeremy Bell
+ ///
+ /// The first point of the first line segment.
+ /// The second point of the first line segment.
+ /// The first point of the second line segment.
+ /// The second point of the second line segment.
+ /// This is set to the intersection
+ /// point if an intersection is detected.
+ /// Set this to true to require that the
+ /// intersection point be on the first line segment.
+ /// Set this to true to require that the
+ /// intersection point be on the second line segment.
+ /// True if an intersection is detected, false otherwise.
+ public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4, bool firstIsSegment, bool secondIsSegment, out Vector2 point)
+ {
+ point = new Vector2();
+
+ // these are reused later.
+ // each lettered sub-calculation is used twice, except
+ // for b and d, which are used 3 times
+ float a = point4.Y - point3.Y;
+ float b = point2.X - point1.X;
+ float c = point4.X - point3.X;
+ float d = point2.Y - point1.Y;
+
+ // denominator to solution of linear system
+ float denom = (a * b) - (c * d);
+
+ // if denominator is 0, then lines are parallel
+ if (!(denom >= -Settings.Epsilon && denom <= Settings.Epsilon))
+ {
+ float e = point1.Y - point3.Y;
+ float f = point1.X - point3.X;
+ float oneOverDenom = 1.0f / denom;
+
+ // numerator of first equation
+ float ua = (c * e) - (a * f);
+ ua *= oneOverDenom;
+
+ // check if intersection point of the two lines is on line segment 1
+ if (!firstIsSegment || ua >= 0.0f && ua <= 1.0f)
+ {
+ // numerator of second equation
+ float ub = (b * e) - (d * f);
+ ub *= oneOverDenom;
+
+ // check if intersection point of the two lines is on line segment 2
+ // means the line segments intersect, since we know it is on
+ // segment 1 as well.
+ if (!secondIsSegment || ub >= 0.0f && ub <= 1.0f)
+ {
+ // check if they are coincident (no collision in this case)
+ if (ua != 0f || ub != 0f)
+ {
+ //There is an intersection
+ point.X = point1.X + ua * b;
+ point.Y = point1.Y + ua * d;
+ return true;
+ }
+ }
+ }
+ }
+
+ return false;
+ }
+
+ ///
+ /// This method detects if two line segments (or lines) intersect,
+ /// and, if so, the point of intersection. Use the and
+ /// parameters to set whether the intersection point
+ /// must be on the first and second line segments. Setting these
+ /// both to true means you are doing a line-segment to line-segment
+ /// intersection. Setting one of them to true means you are doing a
+ /// line to line-segment intersection test, and so on.
+ /// Note: If two line segments are coincident, then
+ /// no intersection is detected (there are actually
+ /// infinite intersection points).
+ /// Author: Jeremy Bell
+ ///
+ /// The first point of the first line segment.
+ /// The second point of the first line segment.
+ /// The first point of the second line segment.
+ /// The second point of the second line segment.
+ /// This is set to the intersection
+ /// point if an intersection is detected.
+ /// Set this to true to require that the
+ /// intersection point be on the first line segment.
+ /// Set this to true to require that the
+ /// intersection point be on the second line segment.
+ /// True if an intersection is detected, false otherwise.
+ public static bool LineIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4, bool firstIsSegment, bool secondIsSegment, out Vector2 intersectionPoint)
+ {
+ return LineIntersect(ref point1, ref point2, ref point3, ref point4, firstIsSegment, secondIsSegment, out intersectionPoint);
+ }
+
+ ///
+ /// This method detects if two line segments intersect,
+ /// and, if so, the point of intersection.
+ /// Note: If two line segments are coincident, then
+ /// no intersection is detected (there are actually
+ /// infinite intersection points).
+ ///
+ /// The first point of the first line segment.
+ /// The second point of the first line segment.
+ /// The first point of the second line segment.
+ /// The second point of the second line segment.
+ /// This is set to the intersection
+ /// point if an intersection is detected.
+ /// True if an intersection is detected, false otherwise.
+ public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4, out Vector2 intersectionPoint)
+ {
+ return LineIntersect(ref point1, ref point2, ref point3, ref point4, true, true, out intersectionPoint);
+ }
+
+ ///
+ /// This method detects if two line segments intersect,
+ /// and, if so, the point of intersection.
+ /// Note: If two line segments are coincident, then
+ /// no intersection is detected (there are actually
+ /// infinite intersection points).
+ ///
+ /// The first point of the first line segment.
+ /// The second point of the first line segment.
+ /// The first point of the second line segment.
+ /// The second point of the second line segment.
+ /// This is set to the intersection
+ /// point if an intersection is detected.
+ /// True if an intersection is detected, false otherwise.
+ public static bool LineIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4, out Vector2 intersectionPoint)
+ {
+ return LineIntersect(ref point1, ref point2, ref point3, ref point4, true, true, out intersectionPoint);
+ }
+
+ ///
+ /// Get all intersections between a line segment and a list of vertices
+ /// representing a polygon. The vertices reuse adjacent points, so for example
+ /// edges one and two are between the first and second vertices and between the
+ /// second and third vertices. The last edge is between vertex vertices.Count - 1
+ /// and verts0. (ie, vertices from a Geometry or AABB)
+ ///
+ /// The first point of the line segment to test
+ /// The second point of the line segment to test.
+ /// The vertices, as described above
+ public static Vertices LineSegmentVerticesIntersect(ref Vector2 point1, ref Vector2 point2, Vertices vertices)
+ {
+ Vertices intersectionPoints = new Vertices();
+
+ for (int i = 0; i < vertices.Count; i++)
+ {
+ Vector2 point;
+ if (LineIntersect(vertices[i], vertices[vertices.NextIndex(i)], point1, point2, true, true, out point))
+ {
+ intersectionPoints.Add(point);
+ }
+ }
+
+ return intersectionPoints;
+ }
+
+ ///
+ /// Get all intersections between a line segment and an AABB.
+ ///
+ /// The first point of the line segment to test
+ /// The second point of the line segment to test.
+ /// The AABB that is used for testing intersection.
+ public static Vertices LineSegmentAABBIntersect(ref Vector2 point1, ref Vector2 point2, AABB aabb)
+ {
+ return LineSegmentVerticesIntersect(ref point1, ref point2, aabb.Vertices);
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Math.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Math.cs
new file mode 100644
index 0000000..766212f
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Math.cs
@@ -0,0 +1,806 @@
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+*
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Diagnostics;
+using System.Runtime.InteropServices;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common
+{
+ public static class MathUtils
+ {
+ public static float Cross(ref Vector2 a, ref Vector2 b)
+ {
+ return a.X * b.Y - a.Y * b.X;
+ }
+
+ public static float Cross(Vector2 a, Vector2 b)
+ {
+ return Cross(ref a, ref b);
+ }
+
+ /// Perform the cross product on two vectors.
+ public static Vector3 Cross(Vector3 a, Vector3 b)
+ {
+ return new Vector3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X);
+ }
+
+ public static Vector2 Cross(Vector2 a, float s)
+ {
+ return new Vector2(s * a.Y, -s * a.X);
+ }
+
+ public static Vector2 Cross(float s, Vector2 a)
+ {
+ return new Vector2(-s * a.Y, s * a.X);
+ }
+
+ public static Vector2 Abs(Vector2 v)
+ {
+ return new Vector2(Math.Abs(v.X), Math.Abs(v.Y));
+ }
+
+ public static Vector2 Mul(ref Mat22 A, Vector2 v)
+ {
+ return Mul(ref A, ref v);
+ }
+
+ public static Vector2 Mul(ref Mat22 A, ref Vector2 v)
+ {
+ return new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y);
+ }
+
+ public static Vector2 Mul(ref Transform T, Vector2 v)
+ {
+ return Mul(ref T, ref v);
+ }
+
+ public static Vector2 Mul(ref Transform T, ref Vector2 v)
+ {
+ float x = (T.q.c * v.X - T.q.s * v.Y) + T.p.X;
+ float y = (T.q.s * v.X + T.q.c * v.Y) + T.p.Y;
+
+ return new Vector2(x, y);
+ }
+
+ public static Vector2 MulT(ref Mat22 A, Vector2 v)
+ {
+ return MulT(ref A, ref v);
+ }
+
+ public static Vector2 MulT(ref Mat22 A, ref Vector2 v)
+ {
+ return new Vector2(v.X * A.ex.X + v.Y * A.ex.Y, v.X * A.ey.X + v.Y * A.ey.Y);
+ }
+
+ public static Vector2 MulT(ref Transform T, Vector2 v)
+ {
+ return MulT(ref T, ref v);
+ }
+
+ public static Vector2 MulT(ref Transform T, ref Vector2 v)
+ {
+ float px = v.X - T.p.X;
+ float py = v.Y - T.p.Y;
+ float x = (T.q.c * px + T.q.s * py);
+ float y = (-T.q.s * px + T.q.c * py);
+
+ return new Vector2(x, y);
+ }
+
+ // A^T * B
+ public static void MulT(ref Mat22 A, ref Mat22 B, out Mat22 C)
+ {
+ C = new Mat22();
+ C.ex.X = A.ex.X * B.ex.X + A.ex.Y * B.ex.Y;
+ C.ex.Y = A.ey.X * B.ex.X + A.ey.Y * B.ex.Y;
+ C.ey.X = A.ex.X * B.ey.X + A.ex.Y * B.ey.Y;
+ C.ey.Y = A.ey.X * B.ey.X + A.ey.Y * B.ey.Y;
+ }
+
+ /// Multiply a matrix times a vector.
+ public static Vector3 Mul(Mat33 A, Vector3 v)
+ {
+ return v.X * A.ex + v.Y * A.ey + v.Z * A.ez;
+ }
+
+ // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
+ // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
+ public static Transform Mul(Transform A, Transform B)
+ {
+ Transform C = new Transform();
+ C.q = Mul(A.q, B.q);
+ C.p = Mul(A.q, B.p) + A.p;
+ return C;
+ }
+
+ // v2 = A.q' * (B.q * v1 + B.p - A.p)
+ // = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+ public static void MulT(ref Transform A, ref Transform B, out Transform C)
+ {
+ C = new Transform();
+ C.q = MulT(A.q, B.q);
+ C.p = MulT(A.q, B.p - A.p);
+ }
+
+ public static void Swap(ref T a, ref T b)
+ {
+ T tmp = a;
+ a = b;
+ b = tmp;
+ }
+
+ /// Multiply a matrix times a vector.
+ public static Vector2 Mul22(Mat33 A, Vector2 v)
+ {
+ return new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y);
+ }
+
+ /// Multiply two rotations: q * r
+ public static Rot Mul(Rot q, Rot r)
+ {
+ // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
+ // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
+ // s = qs * rc + qc * rs
+ // c = qc * rc - qs * rs
+ Rot qr;
+ qr.s = q.s * r.c + q.c * r.s;
+ qr.c = q.c * r.c - q.s * r.s;
+ return qr;
+ }
+
+ public static Vector2 MulT(Transform T, Vector2 v)
+ {
+ float px = v.X - T.p.X;
+ float py = v.Y - T.p.Y;
+ float x = (T.q.c * px + T.q.s * py);
+ float y = (-T.q.s * px + T.q.c * py);
+
+ return new Vector2(x, y);
+ }
+
+ /// Transpose multiply two rotations: qT * r
+ public static Rot MulT(Rot q, Rot r)
+ {
+ // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
+ // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
+ // s = qc * rs - qs * rc
+ // c = qc * rc + qs * rs
+ Rot qr;
+ qr.s = q.c * r.s - q.s * r.c;
+ qr.c = q.c * r.c + q.s * r.s;
+ return qr;
+ }
+
+ // v2 = A.q' * (B.q * v1 + B.p - A.p)
+ // = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+ public static Transform MulT(Transform A, Transform B)
+ {
+ Transform C = new Transform();
+ C.q = MulT(A.q, B.q);
+ C.p = MulT(A.q, B.p - A.p);
+ return C;
+ }
+
+ /// Rotate a vector
+ public static Vector2 Mul(Rot q, Vector2 v)
+ {
+ return new Vector2(q.c * v.X - q.s * v.Y, q.s * v.X + q.c * v.Y);
+ }
+
+ /// Inverse rotate a vector
+ public static Vector2 MulT(Rot q, Vector2 v)
+ {
+ return new Vector2(q.c * v.X + q.s * v.Y, -q.s * v.X + q.c * v.Y);
+ }
+
+ /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
+ public static Vector2 Skew(Vector2 input)
+ {
+ return new Vector2(-input.Y, input.X);
+ }
+
+ ///
+ /// This function is used to ensure that a floating point number is
+ /// not a NaN or infinity.
+ ///
+ /// The x.
+ ///
+ /// true if the specified x is valid; otherwise, false.
+ ///
+ public static bool IsValid(float x)
+ {
+ if (float.IsNaN(x))
+ {
+ // NaN.
+ return false;
+ }
+
+ return !float.IsInfinity(x);
+ }
+
+ public static bool IsValid(this Vector2 x)
+ {
+ return IsValid(x.X) && IsValid(x.Y);
+ }
+
+ ///
+ /// This is a approximate yet fast inverse square-root.
+ ///
+ /// The x.
+ ///
+ public static float InvSqrt(float x)
+ {
+ FloatConverter convert = new FloatConverter();
+ convert.x = x;
+ float xhalf = 0.5f * x;
+ convert.i = 0x5f3759df - (convert.i >> 1);
+ x = convert.x;
+ x = x * (1.5f - xhalf * x * x);
+ return x;
+ }
+
+ public static int Clamp(int a, int low, int high)
+ {
+ return Math.Max(low, Math.Min(a, high));
+ }
+
+ public static float Clamp(float a, float low, float high)
+ {
+ return Math.Max(low, Math.Min(a, high));
+ }
+
+ public static Vector2 Clamp(Vector2 a, Vector2 low, Vector2 high)
+ {
+ return Vector2.Max(low, Vector2.Min(a, high));
+ }
+
+ public static void Cross(ref Vector2 a, ref Vector2 b, out float c)
+ {
+ c = a.X * b.Y - a.Y * b.X;
+ }
+
+ ///
+ /// Return the angle between two vectors on a plane
+ /// The angle is from vector 1 to vector 2, positive anticlockwise
+ /// The result is between -pi -> pi
+ ///
+ public static double VectorAngle(ref Vector2 p1, ref Vector2 p2)
+ {
+ double theta1 = Math.Atan2(p1.Y, p1.X);
+ double theta2 = Math.Atan2(p2.Y, p2.X);
+ double dtheta = theta2 - theta1;
+ while (dtheta > Math.PI)
+ dtheta -= (2 * Math.PI);
+ while (dtheta < -Math.PI)
+ dtheta += (2 * Math.PI);
+
+ return (dtheta);
+ }
+
+ /// Perform the dot product on two vectors.
+ public static float Dot(Vector3 a, Vector3 b)
+ {
+ return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
+ }
+
+ public static double VectorAngle(Vector2 p1, Vector2 p2)
+ {
+ return VectorAngle(ref p1, ref p2);
+ }
+
+ ///
+ /// Returns a positive number if c is to the left of the line going from a to b.
+ ///
+ /// Positive number if point is left, negative if point is right,
+ /// and 0 if points are collinear.
+ public static float Area(Vector2 a, Vector2 b, Vector2 c)
+ {
+ return Area(ref a, ref b, ref c);
+ }
+
+ ///
+ /// Returns a positive number if c is to the left of the line going from a to b.
+ ///
+ /// Positive number if point is left, negative if point is right,
+ /// and 0 if points are collinear.
+ public static float Area(ref Vector2 a, ref Vector2 b, ref Vector2 c)
+ {
+ return a.X * (b.Y - c.Y) + b.X * (c.Y - a.Y) + c.X * (a.Y - b.Y);
+ }
+
+ ///
+ /// Determines if three vertices are collinear (ie. on a straight line)
+ ///
+ /// First vertex
+ /// Second vertex
+ /// Third vertex
+ /// The tolerance
+ ///
+ public static bool IsCollinear(ref Vector2 a, ref Vector2 b, ref Vector2 c, float tolerance = 0)
+ {
+ return FloatInRange(Area(ref a, ref b, ref c), -tolerance, tolerance);
+ }
+
+ public static void Cross(float s, ref Vector2 a, out Vector2 b)
+ {
+ b = new Vector2(-s * a.Y, s * a.X);
+ }
+
+ public static bool FloatEquals(float value1, float value2)
+ {
+ return Math.Abs(value1 - value2) <= Settings.Epsilon;
+ }
+
+ ///
+ /// Checks if a floating point Value is equal to another,
+ /// within a certain tolerance.
+ ///
+ /// The first floating point Value.
+ /// The second floating point Value.
+ /// The floating point tolerance.
+ /// True if the values are "equal", false otherwise.
+ public static bool FloatEquals(float value1, float value2, float delta)
+ {
+ return FloatInRange(value1, value2 - delta, value2 + delta);
+ }
+
+ ///
+ /// Checks if a floating point Value is within a specified
+ /// range of values (inclusive).
+ ///
+ /// The Value to check.
+ /// The minimum Value.
+ /// The maximum Value.
+ /// True if the Value is within the range specified,
+ /// false otherwise.
+ public static bool FloatInRange(float value, float min, float max)
+ {
+ return (value >= min && value <= max);
+ }
+
+ #region Nested type: FloatConverter
+
+ [StructLayout(LayoutKind.Explicit)]
+ private struct FloatConverter
+ {
+ [FieldOffset(0)]
+ public float x;
+ [FieldOffset(0)]
+ public int i;
+ }
+
+ #endregion
+
+ public static Vector2 Mul(ref Rot rot, Vector2 axis)
+ {
+ return Mul(rot, axis);
+ }
+
+ public static Vector2 MulT(ref Rot rot, Vector2 axis)
+ {
+ return MulT(rot, axis);
+ }
+ }
+
+ ///
+ /// A 2-by-2 matrix. Stored in column-major order.
+ ///
+ public struct Mat22
+ {
+ public Vector2 ex, ey;
+
+ ///
+ /// Construct this matrix using columns.
+ ///
+ /// The c1.
+ /// The c2.
+ public Mat22(Vector2 c1, Vector2 c2)
+ {
+ ex = c1;
+ ey = c2;
+ }
+
+ ///
+ /// Construct this matrix using scalars.
+ ///
+ /// The a11.
+ /// The a12.
+ /// The a21.
+ /// The a22.
+ public Mat22(float a11, float a12, float a21, float a22)
+ {
+ ex = new Vector2(a11, a21);
+ ey = new Vector2(a12, a22);
+ }
+
+ public Mat22 Inverse
+ {
+ get
+ {
+ float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y;
+ float det = a * d - b * c;
+ if (det != 0.0f)
+ {
+ det = 1.0f / det;
+ }
+
+ Mat22 result = new Mat22();
+ result.ex.X = det * d;
+ result.ex.Y = -det * c;
+
+ result.ey.X = -det * b;
+ result.ey.Y = det * a;
+
+ return result;
+ }
+ }
+
+ ///
+ /// Initialize this matrix using columns.
+ ///
+ /// The c1.
+ /// The c2.
+ public void Set(Vector2 c1, Vector2 c2)
+ {
+ ex = c1;
+ ey = c2;
+ }
+
+ ///
+ /// Set this to the identity matrix.
+ ///
+ public void SetIdentity()
+ {
+ ex.X = 1.0f;
+ ey.X = 0.0f;
+ ex.Y = 0.0f;
+ ey.Y = 1.0f;
+ }
+
+ ///
+ /// Set this matrix to all zeros.
+ ///
+ public void SetZero()
+ {
+ ex.X = 0.0f;
+ ey.X = 0.0f;
+ ex.Y = 0.0f;
+ ey.Y = 0.0f;
+ }
+
+ ///
+ /// Solve A * x = b, where b is a column vector. This is more efficient
+ /// than computing the inverse in one-shot cases.
+ ///
+ /// The b.
+ ///
+ public Vector2 Solve(Vector2 b)
+ {
+ float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y;
+ float det = a11 * a22 - a12 * a21;
+ if (det != 0.0f)
+ {
+ det = 1.0f / det;
+ }
+
+ return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X));
+ }
+
+ public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R)
+ {
+ R.ex = A.ex + B.ex;
+ R.ey = A.ey + B.ey;
+ }
+ }
+
+ ///
+ /// A 3-by-3 matrix. Stored in column-major order.
+ ///
+ public struct Mat33
+ {
+ public Vector3 ex, ey, ez;
+
+ ///
+ /// Construct this matrix using columns.
+ ///
+ /// The c1.
+ /// The c2.
+ /// The c3.
+ public Mat33(Vector3 c1, Vector3 c2, Vector3 c3)
+ {
+ ex = c1;
+ ey = c2;
+ ez = c3;
+ }
+
+ ///
+ /// Set this matrix to all zeros.
+ ///
+ public void SetZero()
+ {
+ ex = Vector3.Zero;
+ ey = Vector3.Zero;
+ ez = Vector3.Zero;
+ }
+
+ ///
+ /// Solve A * x = b, where b is a column vector. This is more efficient
+ /// than computing the inverse in one-shot cases.
+ ///
+ /// The b.
+ ///
+ public Vector3 Solve33(Vector3 b)
+ {
+ float det = Vector3.Dot(ex, Vector3.Cross(ey, ez));
+ if (det != 0.0f)
+ {
+ det = 1.0f / det;
+ }
+
+ return new Vector3(det * Vector3.Dot(b, Vector3.Cross(ey, ez)), det * Vector3.Dot(ex, Vector3.Cross(b, ez)), det * Vector3.Dot(ex, Vector3.Cross(ey, b)));
+ }
+
+ ///
+ /// Solve A * x = b, where b is a column vector. This is more efficient
+ /// than computing the inverse in one-shot cases. Solve only the upper
+ /// 2-by-2 matrix equation.
+ ///
+ /// The b.
+ ///
+ public Vector2 Solve22(Vector2 b)
+ {
+ float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y;
+ float det = a11 * a22 - a12 * a21;
+
+ if (det != 0.0f)
+ {
+ det = 1.0f / det;
+ }
+
+ return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X));
+ }
+
+ /// Get the inverse of this matrix as a 2-by-2.
+ /// Returns the zero matrix if singular.
+ public void GetInverse22(ref Mat33 M)
+ {
+ float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y;
+ float det = a * d - b * c;
+ if (det != 0.0f)
+ {
+ det = 1.0f / det;
+ }
+
+ M.ex.X = det * d; M.ey.X = -det * b; M.ex.Z = 0.0f;
+ M.ex.Y = -det * c; M.ey.Y = det * a; M.ey.Z = 0.0f;
+ M.ez.X = 0.0f; M.ez.Y = 0.0f; M.ez.Z = 0.0f;
+ }
+
+ /// Get the symmetric inverse of this matrix as a 3-by-3.
+ /// Returns the zero matrix if singular.
+ public void GetSymInverse33(ref Mat33 M)
+ {
+ float det = MathUtils.Dot(ex, MathUtils.Cross(ey, ez));
+ if (det != 0.0f)
+ {
+ det = 1.0f / det;
+ }
+
+ float a11 = ex.X, a12 = ey.X, a13 = ez.X;
+ float a22 = ey.Y, a23 = ez.Y;
+ float a33 = ez.Z;
+
+ M.ex.X = det * (a22 * a33 - a23 * a23);
+ M.ex.Y = det * (a13 * a23 - a12 * a33);
+ M.ex.Z = det * (a12 * a23 - a13 * a22);
+
+ M.ey.X = M.ex.Y;
+ M.ey.Y = det * (a11 * a33 - a13 * a13);
+ M.ey.Z = det * (a13 * a12 - a11 * a23);
+
+ M.ez.X = M.ex.Z;
+ M.ez.Y = M.ey.Z;
+ M.ez.Z = det * (a11 * a22 - a12 * a12);
+ }
+ }
+
+ ///
+ /// Rotation
+ ///
+ public struct Rot
+ {
+ /// Sine and cosine
+ public float s, c;
+
+ ///
+ /// Initialize from an angle in radians
+ ///
+ /// Angle in radians
+ public Rot(float angle)
+ {
+ // TODO_ERIN optimize
+ s = (float)Math.Sin(angle);
+ c = (float)Math.Cos(angle);
+ }
+
+ ///
+ /// Set using an angle in radians.
+ ///
+ ///
+ public void Set(float angle)
+ {
+ // TODO_ERIN optimize
+ s = (float)Math.Sin(angle);
+ c = (float)Math.Cos(angle);
+ }
+
+ ///
+ /// Set to the identity rotation
+ ///
+ public void SetIdentity()
+ {
+ s = 0.0f;
+ c = 1.0f;
+ }
+
+ ///
+ /// Get the angle in radians
+ ///
+ public float GetAngle()
+ {
+ return (float)Math.Atan2(s, c);
+ }
+
+ ///
+ /// Get the x-axis
+ ///
+ public Vector2 GetXAxis()
+ {
+ return new Vector2(c, s);
+ }
+
+ ///
+ /// Get the y-axis
+ ///
+ public Vector2 GetYAxis()
+ {
+ return new Vector2(-s, c);
+ }
+ }
+
+ ///
+ /// A transform contains translation and rotation. It is used to represent
+ /// the position and orientation of rigid frames.
+ ///
+ public struct Transform
+ {
+ public Vector2 p;
+ public Rot q;
+
+ ///
+ /// Initialize using a position vector and a rotation matrix.
+ ///
+ /// The position.
+ /// The r.
+ public Transform(ref Vector2 position, ref Rot rotation)
+ {
+ p = position;
+ q = rotation;
+ }
+
+ ///
+ /// Set this to the identity transform.
+ ///
+ public void SetIdentity()
+ {
+ p = Vector2.Zero;
+ q.SetIdentity();
+ }
+
+ ///
+ /// Set this based on the position and angle.
+ ///
+ /// The position.
+ /// The angle.
+ public void Set(Vector2 position, float angle)
+ {
+ p = position;
+ q.Set(angle);
+ }
+ }
+
+ ///
+ /// This describes the motion of a body/shape for TOI computation.
+ /// Shapes are defined with respect to the body origin, which may
+ /// no coincide with the center of mass. However, to support dynamics
+ /// we must interpolate the center of mass position.
+ ///
+ public struct Sweep
+ {
+ ///
+ /// World angles
+ ///
+ public float A;
+
+ public float A0;
+
+ ///
+ /// Fraction of the current time step in the range [0,1]
+ /// c0 and a0 are the positions at alpha0.
+ ///
+ public float Alpha0;
+
+ ///
+ /// Center world positions
+ ///
+ public Vector2 C;
+
+ public Vector2 C0;
+
+ ///
+ /// Local center of mass position
+ ///
+ public Vector2 LocalCenter;
+
+ ///
+ /// Get the interpolated transform at a specific time.
+ ///
+ /// The transform.
+ /// beta is a factor in [0,1], where 0 indicates alpha0.
+ public void GetTransform(out Transform xfb, float beta)
+ {
+ xfb = new Transform();
+ xfb.p.X = (1.0f - beta) * C0.X + beta * C.X;
+ xfb.p.Y = (1.0f - beta) * C0.Y + beta * C.Y;
+ float angle = (1.0f - beta) * A0 + beta * A;
+ xfb.q.Set(angle);
+
+ // Shift to origin
+ xfb.p -= MathUtils.Mul(xfb.q, LocalCenter);
+ }
+
+ ///
+ /// Advance the sweep forward, yielding a new initial state.
+ ///
+ /// new initial time..
+ public void Advance(float alpha)
+ {
+ Debug.Assert(Alpha0 < 1.0f);
+ float beta = (alpha - Alpha0) / (1.0f - Alpha0);
+ C0 += beta * (C - C0);
+ A0 += beta * (A - A0);
+ Alpha0 = alpha;
+ }
+
+ ///
+ /// Normalize the angles.
+ ///
+ public void Normalize()
+ {
+ float d = MathHelper.TwoPi * (float)Math.Floor(A0 / MathHelper.TwoPi);
+ A0 -= d;
+ A -= d;
+ }
+ }
+}
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/MathHelper.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/MathHelper.cs
new file mode 100644
index 0000000..3f48a8b
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/MathHelper.cs
@@ -0,0 +1,158 @@
+#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID
+
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+
+namespace Microsoft.Xna.Framework
+{
+ public static class MathHelper
+ {
+ public const float E = (float) Math.E;
+ public const float Log10E = 0.4342945f;
+ public const float Log2E = 1.442695f;
+ public const float Pi = (float) Math.PI;
+ public const float PiOver2 = (float) (Math.PI/2.0);
+ public const float PiOver4 = (float) (Math.PI/4.0);
+ public const float TwoPi = (float) (Math.PI*2.0);
+
+ public static float Barycentric(float value1, float value2, float value3, float amount1, float amount2)
+ {
+ return value1 + (value2 - value1)*amount1 + (value3 - value1)*amount2;
+ }
+
+ public static float CatmullRom(float value1, float value2, float value3, float value4, float amount)
+ {
+ // Using formula from http://www.mvps.org/directx/articles/catmull/
+ // Internally using doubles not to lose precission
+ double amountSquared = amount*amount;
+ double amountCubed = amountSquared*amount;
+ return (float) (0.5*(2.0*value2 +
+ (value3 - value1)*amount +
+ (2.0*value1 - 5.0*value2 + 4.0*value3 - value4)*amountSquared +
+ (3.0*value2 - value1 - 3.0*value3 + value4)*amountCubed));
+ }
+
+ public static float Clamp(float value, float min, float max)
+ {
+ // First we check to see if we're greater than the max
+ value = (value > max) ? max : value;
+
+ // Then we check to see if we're less than the min.
+ value = (value < min) ? min : value;
+
+ // There's no check to see if min > max.
+ return value;
+ }
+
+ public static float Distance(float value1, float value2)
+ {
+ return Math.Abs(value1 - value2);
+ }
+
+ public static float Hermite(float value1, float tangent1, float value2, float tangent2, float amount)
+ {
+ // All transformed to double not to lose precission
+ // Otherwise, for high numbers of param:amount the result is NaN instead of Infinity
+ double v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount, result;
+ double sCubed = s*s*s;
+ double sSquared = s*s;
+
+ if (amount == 0f)
+ result = value1;
+ else if (amount == 1f)
+ result = value2;
+ else
+ result = (2*v1 - 2*v2 + t2 + t1)*sCubed +
+ (3*v2 - 3*v1 - 2*t1 - t2)*sSquared +
+ t1*s +
+ v1;
+ return (float) result;
+ }
+
+ public static float Lerp(float value1, float value2, float amount)
+ {
+ return value1 + (value2 - value1)*amount;
+ }
+
+ public static float Max(float value1, float value2)
+ {
+ return Math.Max(value1, value2);
+ }
+
+ public static float Min(float value1, float value2)
+ {
+ return Math.Min(value1, value2);
+ }
+
+ public static float SmoothStep(float value1, float value2, float amount)
+ {
+ // It is expected that 0 < amount < 1
+ // If amount < 0, return value1
+ // If amount > 1, return value2
+ float result = Clamp(amount, 0f, 1f);
+ result = Hermite(value1, 0f, value2, 0f, result);
+ return result;
+ }
+
+ public static float ToDegrees(float radians)
+ {
+ // This method uses double precission internally,
+ // though it returns single float
+ // Factor = 180 / pi
+ return (float) (radians*57.295779513082320876798154814105);
+ }
+
+ public static float ToRadians(float degrees)
+ {
+ // This method uses double precission internally,
+ // though it returns single float
+ // Factor = pi / 180
+ return (float) (degrees*0.017453292519943295769236907684886);
+ }
+
+ public static float WrapAngle(float angle)
+ {
+ angle = (float) Math.IEEERemainder((double) angle, 6.2831854820251465); //2xPi precission is double
+ if (angle <= -3.141593f)
+ {
+ angle += 6.283185f;
+ return angle;
+ }
+ if (angle > 3.141593f)
+ {
+ angle -= 6.283185f;
+ }
+ return angle;
+ }
+ }
+}
+
+#endif
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Matrix.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Matrix.cs
new file mode 100644
index 0000000..1f98507
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Matrix.cs
@@ -0,0 +1,1130 @@
+#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID
+
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+using System.Runtime.InteropServices;
+
+namespace Microsoft.Xna.Framework
+{
+ [StructLayout(LayoutKind.Sequential)]
+ public struct Matrix : IEquatable
+ {
+#region Public Fields
+
+ public float M11;
+ public float M12;
+ public float M13;
+ public float M14;
+ public float M21;
+ public float M22;
+ public float M23;
+ public float M24;
+ public float M31;
+ public float M32;
+ public float M33;
+ public float M34;
+ public float M41;
+ public float M42;
+ public float M43;
+ public float M44;
+
+#endregion Public Fields
+
+#region Static Properties
+
+ private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f);
+
+ public static Matrix Identity
+ {
+ get { return identity; }
+ }
+
+#endregion Static Properties
+
+#region Public Properties
+
+ public Vector3 Backward
+ {
+ get { return new Vector3(M31, M32, M33); }
+ set
+ {
+ M31 = value.X;
+ M32 = value.Y;
+ M33 = value.Z;
+ }
+ }
+
+ public Vector3 Down
+ {
+ get { return new Vector3(-M21, -M22, -M23); }
+ set
+ {
+ M21 = -value.X;
+ M22 = -value.Y;
+ M23 = -value.Z;
+ }
+ }
+
+ public Vector3 Forward
+ {
+ get { return new Vector3(-M31, -M32, -M33); }
+ set
+ {
+ M31 = -value.X;
+ M32 = -value.Y;
+ M33 = -value.Z;
+ }
+ }
+
+ public Vector3 Left
+ {
+ get { return new Vector3(-M11, -M12, -M13); }
+ set
+ {
+ M11 = -value.X;
+ M12 = -value.Y;
+ M13 = -value.Z;
+ }
+ }
+
+ public Vector3 Right
+ {
+ get { return new Vector3(M11, M12, M13); }
+ set
+ {
+ M11 = value.X;
+ M12 = value.Y;
+ M13 = value.Z;
+ }
+ }
+
+ public Vector3 Translation
+ {
+ get { return new Vector3(M41, M42, M43); }
+ set
+ {
+ M41 = value.X;
+ M42 = value.Y;
+ M43 = value.Z;
+ }
+ }
+
+ public Vector3 Up
+ {
+ get { return new Vector3(M21, M22, M23); }
+ set
+ {
+ M21 = value.X;
+ M22 = value.Y;
+ M23 = value.Z;
+ }
+ }
+
+#endregion Public Properties
+
+#region Constructors
+
+ ///
+ /// Constructor for 4x4 Matrix
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ public Matrix(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24,
+ float m31, float m32, float m33, float m34, float m41, float m42, float m43, float m44)
+ {
+ M11 = m11;
+ M12 = m12;
+ M13 = m13;
+ M14 = m14;
+ M21 = m21;
+ M22 = m22;
+ M23 = m23;
+ M24 = m24;
+ M31 = m31;
+ M32 = m32;
+ M33 = m33;
+ M34 = m34;
+ M41 = m41;
+ M42 = m42;
+ M43 = m43;
+ M44 = m44;
+ }
+
+#endregion Constructors
+
+#region Public Static Methods
+
+ public static Matrix CreateWorld(Vector3 position, Vector3 forward, Vector3 up)
+ {
+ Matrix ret;
+ CreateWorld(ref position, ref forward, ref up, out ret);
+ return ret;
+ }
+
+ public static void CreateWorld(ref Vector3 position, ref Vector3 forward, ref Vector3 up, out Matrix result)
+ {
+ Vector3 x, y, z;
+ Vector3.Normalize(ref forward, out z);
+ Vector3.Cross(ref forward, ref up, out x);
+ Vector3.Cross(ref x, ref forward, out y);
+ x.Normalize();
+ y.Normalize();
+
+ result = new Matrix();
+ result.Right = x;
+ result.Up = y;
+ result.Forward = z;
+ result.Translation = position;
+ result.M44 = 1f;
+ }
+
+ ///
+ /// Adds second matrix to the first.
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ public static Matrix Add(Matrix matrix1, Matrix matrix2)
+ {
+ matrix1.M11 += matrix2.M11;
+ matrix1.M12 += matrix2.M12;
+ matrix1.M13 += matrix2.M13;
+ matrix1.M14 += matrix2.M14;
+ matrix1.M21 += matrix2.M21;
+ matrix1.M22 += matrix2.M22;
+ matrix1.M23 += matrix2.M23;
+ matrix1.M24 += matrix2.M24;
+ matrix1.M31 += matrix2.M31;
+ matrix1.M32 += matrix2.M32;
+ matrix1.M33 += matrix2.M33;
+ matrix1.M34 += matrix2.M34;
+ matrix1.M41 += matrix2.M41;
+ matrix1.M42 += matrix2.M42;
+ matrix1.M43 += matrix2.M43;
+ matrix1.M44 += matrix2.M44;
+ return matrix1;
+ }
+
+
+ ///
+ /// Adds two Matrix and save to the result Matrix
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ ///
+ /// A
+ ///
+ public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
+ {
+ result.M11 = matrix1.M11 + matrix2.M11;
+ result.M12 = matrix1.M12 + matrix2.M12;
+ result.M13 = matrix1.M13 + matrix2.M13;
+ result.M14 = matrix1.M14 + matrix2.M14;
+ result.M21 = matrix1.M21 + matrix2.M21;
+ result.M22 = matrix1.M22 + matrix2.M22;
+ result.M23 = matrix1.M23 + matrix2.M23;
+ result.M24 = matrix1.M24 + matrix2.M24;
+ result.M31 = matrix1.M31 + matrix2.M31;
+ result.M32 = matrix1.M32 + matrix2.M32;
+ result.M33 = matrix1.M33 + matrix2.M33;
+ result.M34 = matrix1.M34 + matrix2.M34;
+ result.M41 = matrix1.M41 + matrix2.M41;
+ result.M42 = matrix1.M42 + matrix2.M42;
+ result.M43 = matrix1.M43 + matrix2.M43;
+ result.M44 = matrix1.M44 + matrix2.M44;
+ }
+
+
+ public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition,
+ Vector3 cameraUpVector, Nullable cameraForwardVector)
+ {
+ Matrix ret;
+ CreateBillboard(ref objectPosition, ref cameraPosition, ref cameraUpVector, cameraForwardVector, out ret);
+ return ret;
+ }
+
+ public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
+ ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result)
+ {
+ Vector3 translation = objectPosition - cameraPosition;
+ Vector3 backwards, right, up;
+ Vector3.Normalize(ref translation, out backwards);
+ Vector3.Normalize(ref cameraUpVector, out up);
+ Vector3.Cross(ref backwards, ref up, out right);
+ Vector3.Cross(ref backwards, ref right, out up);
+ result = Identity;
+ result.Backward = backwards;
+ result.Right = right;
+ result.Up = up;
+ result.Translation = translation;
+ }
+
+ public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition,
+ Vector3 rotateAxis, Nullable cameraForwardVector,
+ Nullable objectForwardVector)
+ {
+ throw new NotImplementedException();
+ }
+
+
+ public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
+ ref Vector3 rotateAxis, Vector3? cameraForwardVector,
+ Vector3? objectForwardVector, out Matrix result)
+ {
+ throw new NotImplementedException();
+ }
+
+
+ public static Matrix CreateFromAxisAngle(Vector3 axis, float angle)
+ {
+ throw new NotImplementedException();
+ }
+
+
+ public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result)
+ {
+ throw new NotImplementedException();
+ }
+
+ public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)
+ {
+ Matrix ret;
+ CreateLookAt(ref cameraPosition, ref cameraTarget, ref cameraUpVector, out ret);
+ return ret;
+ }
+
+
+ public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector,
+ out Matrix result)
+ {
+ // http://msdn.microsoft.com/en-us/library/bb205343(v=VS.85).aspx
+
+ Vector3 vz = Vector3.Normalize(cameraPosition - cameraTarget);
+ Vector3 vx = Vector3.Normalize(Vector3.Cross(cameraUpVector, vz));
+ Vector3 vy = Vector3.Cross(vz, vx);
+ result = Identity;
+ result.M11 = vx.X;
+ result.M12 = vy.X;
+ result.M13 = vz.X;
+ result.M21 = vx.Y;
+ result.M22 = vy.Y;
+ result.M23 = vz.Y;
+ result.M31 = vx.Z;
+ result.M32 = vy.Z;
+ result.M33 = vz.Z;
+ result.M41 = -Vector3.Dot(vx, cameraPosition);
+ result.M42 = -Vector3.Dot(vy, cameraPosition);
+ result.M43 = -Vector3.Dot(vz, cameraPosition);
+ }
+
+ public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane)
+ {
+ Matrix ret;
+ CreateOrthographic(width, height, zNearPlane, zFarPlane, out ret);
+ return ret;
+ }
+
+
+ public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane,
+ out Matrix result)
+ {
+ result.M11 = 2/width;
+ result.M12 = 0;
+ result.M13 = 0;
+ result.M14 = 0;
+ result.M21 = 0;
+ result.M22 = 2/height;
+ result.M23 = 0;
+ result.M24 = 0;
+ result.M31 = 0;
+ result.M32 = 0;
+ result.M33 = 1/(zNearPlane - zFarPlane);
+ result.M34 = 0;
+ result.M41 = 0;
+ result.M42 = 0;
+ result.M43 = zNearPlane/(zNearPlane - zFarPlane);
+ result.M44 = 1;
+ }
+
+
+ public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top,
+ float zNearPlane, float zFarPlane)
+ {
+ Matrix ret;
+ CreateOrthographicOffCenter(left, right, bottom, top, zNearPlane, zFarPlane, out ret);
+ return ret;
+ }
+
+
+ public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top,
+ float zNearPlane, float zFarPlane, out Matrix result)
+ {
+ result.M11 = 2/(right - left);
+ result.M12 = 0;
+ result.M13 = 0;
+ result.M14 = 0;
+ result.M21 = 0;
+ result.M22 = 2/(top - bottom);
+ result.M23 = 0;
+ result.M24 = 0;
+ result.M31 = 0;
+ result.M32 = 0;
+ result.M33 = 1/(zNearPlane - zFarPlane);
+ result.M34 = 0;
+ result.M41 = (left + right)/(left - right);
+ result.M42 = (bottom + top)/(bottom - top);
+ result.M43 = zNearPlane/(zNearPlane - zFarPlane);
+ result.M44 = 1;
+ }
+
+
+ public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane)
+ {
+ throw new NotImplementedException();
+ }
+
+
+ public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane,
+ out Matrix result)
+ {
+ throw new NotImplementedException();
+ }
+
+
+ public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance,
+ float farPlaneDistance)
+ {
+ Matrix ret;
+ CreatePerspectiveFieldOfView(fieldOfView, aspectRatio, nearPlaneDistance, farPlaneDistance, out ret);
+ return ret;
+ }
+
+
+ public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance,
+ float farPlaneDistance, out Matrix result)
+ {
+ // http://msdn.microsoft.com/en-us/library/bb205351(v=VS.85).aspx
+ // http://msdn.microsoft.com/en-us/library/bb195665.aspx
+
+ result = new Matrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+
+ if (fieldOfView < 0 || fieldOfView > 3.14159262f)
+ throw new ArgumentOutOfRangeException("fieldOfView",
+ "fieldOfView takes a value between 0 and Pi (180 degrees) in radians.");
+
+ if (nearPlaneDistance <= 0.0f)
+ throw new ArgumentOutOfRangeException("nearPlaneDistance",
+ "You should specify positive value for nearPlaneDistance.");
+
+ if (farPlaneDistance <= 0.0f)
+ throw new ArgumentOutOfRangeException("farPlaneDistance",
+ "You should specify positive value for farPlaneDistance.");
+
+ if (farPlaneDistance <= nearPlaneDistance)
+ throw new ArgumentOutOfRangeException("nearPlaneDistance",
+ "Near plane distance is larger than Far plane distance. Near plane distance must be smaller than Far plane distance.");
+
+ float yscale = (float) 1/(float) Math.Tan(fieldOfView/2);
+ float xscale = yscale/aspectRatio;
+
+ result.M11 = xscale;
+ result.M22 = yscale;
+ result.M33 = farPlaneDistance/(nearPlaneDistance - farPlaneDistance);
+ result.M34 = -1;
+ result.M43 = nearPlaneDistance*farPlaneDistance/(nearPlaneDistance - farPlaneDistance);
+ }
+
+
+ public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top,
+ float zNearPlane, float zFarPlane)
+ {
+ throw new NotImplementedException();
+ }
+
+
+ public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top,
+ float nearPlaneDistance, float farPlaneDistance, out Matrix result)
+ {
+ throw new NotImplementedException();
+ }
+
+
+ public static Matrix CreateRotationX(float radians)
+ {
+ Matrix returnMatrix = Identity;
+
+ returnMatrix.M22 = (float) Math.Cos(radians);
+ returnMatrix.M23 = (float) Math.Sin(radians);
+ returnMatrix.M32 = -returnMatrix.M23;
+ returnMatrix.M33 = returnMatrix.M22;
+
+ return returnMatrix;
+ }
+
+
+ public static void CreateRotationX(float radians, out Matrix result)
+ {
+ result = Identity;
+
+ result.M22 = (float) Math.Cos(radians);
+ result.M23 = (float) Math.Sin(radians);
+ result.M32 = -result.M23;
+ result.M33 = result.M22;
+ }
+
+
+ public static Matrix CreateRotationY(float radians)
+ {
+ Matrix returnMatrix = Identity;
+
+ returnMatrix.M11 = (float) Math.Cos(radians);
+ returnMatrix.M13 = (float) Math.Sin(radians);
+ returnMatrix.M31 = -returnMatrix.M13;
+ returnMatrix.M33 = returnMatrix.M11;
+
+ return returnMatrix;
+ }
+
+
+ public static void CreateRotationY(float radians, out Matrix result)
+ {
+ result = Identity;
+
+ result.M11 = (float) Math.Cos(radians);
+ result.M13 = (float) Math.Sin(radians);
+ result.M31 = -result.M13;
+ result.M33 = result.M11;
+ }
+
+
+ public static Matrix CreateRotationZ(float radians)
+ {
+ Matrix returnMatrix = Identity;
+
+ returnMatrix.M11 = (float) Math.Cos(radians);
+ returnMatrix.M12 = (float) Math.Sin(radians);
+ returnMatrix.M21 = -returnMatrix.M12;
+ returnMatrix.M22 = returnMatrix.M11;
+
+ return returnMatrix;
+ }
+
+
+ public static void CreateRotationZ(float radians, out Matrix result)
+ {
+ result = Identity;
+
+ result.M11 = (float) Math.Cos(radians);
+ result.M12 = (float) Math.Sin(radians);
+ result.M21 = -result.M12;
+ result.M22 = result.M11;
+ }
+
+
+ public static Matrix CreateScale(float scale)
+ {
+ Matrix returnMatrix = Identity;
+
+ returnMatrix.M11 = scale;
+ returnMatrix.M22 = scale;
+ returnMatrix.M33 = scale;
+
+ return returnMatrix;
+ }
+
+
+ public static void CreateScale(float scale, out Matrix result)
+ {
+ result = Identity;
+
+ result.M11 = scale;
+ result.M22 = scale;
+ result.M33 = scale;
+ }
+
+
+ public static Matrix CreateScale(float xScale, float yScale, float zScale)
+ {
+ Matrix returnMatrix = Identity;
+
+ returnMatrix.M11 = xScale;
+ returnMatrix.M22 = yScale;
+ returnMatrix.M33 = zScale;
+
+ return returnMatrix;
+ }
+
+
+ public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result)
+ {
+ result = Identity;
+
+ result.M11 = xScale;
+ result.M22 = yScale;
+ result.M33 = zScale;
+ }
+
+
+ public static Matrix CreateScale(Vector3 scales)
+ {
+ Matrix returnMatrix = Identity;
+
+ returnMatrix.M11 = scales.X;
+ returnMatrix.M22 = scales.Y;
+ returnMatrix.M33 = scales.Z;
+
+ return returnMatrix;
+ }
+
+
+ public static void CreateScale(ref Vector3 scales, out Matrix result)
+ {
+ result = Identity;
+
+ result.M11 = scales.X;
+ result.M22 = scales.Y;
+ result.M33 = scales.Z;
+ }
+
+
+ public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition)
+ {
+ Matrix returnMatrix = Identity;
+
+ returnMatrix.M41 = xPosition;
+ returnMatrix.M42 = yPosition;
+ returnMatrix.M43 = zPosition;
+
+ return returnMatrix;
+ }
+
+
+ public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result)
+ {
+ result = Identity;
+
+ result.M41 = xPosition;
+ result.M42 = yPosition;
+ result.M43 = zPosition;
+ }
+
+
+ public static Matrix CreateTranslation(Vector3 position)
+ {
+ Matrix returnMatrix = Identity;
+
+ returnMatrix.M41 = position.X;
+ returnMatrix.M42 = position.Y;
+ returnMatrix.M43 = position.Z;
+
+ return returnMatrix;
+ }
+
+
+ public static void CreateTranslation(ref Vector3 position, out Matrix result)
+ {
+ result = Identity;
+
+ result.M41 = position.X;
+ result.M42 = position.Y;
+ result.M43 = position.Z;
+ }
+
+ public static Matrix Divide(Matrix matrix1, Matrix matrix2)
+ {
+ Matrix ret;
+ Divide(ref matrix1, ref matrix2, out ret);
+ return ret;
+ }
+
+
+ public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
+ {
+ Matrix inverse = Invert(matrix2);
+ Multiply(ref matrix1, ref inverse, out result);
+ }
+
+
+ public static Matrix Divide(Matrix matrix1, float divider)
+ {
+ Matrix ret;
+ Divide(ref matrix1, divider, out ret);
+ return ret;
+ }
+
+
+ public static void Divide(ref Matrix matrix1, float divider, out Matrix result)
+ {
+ float inverseDivider = 1f/divider;
+ Multiply(ref matrix1, inverseDivider, out result);
+ }
+
+ public static Matrix Invert(Matrix matrix)
+ {
+ Invert(ref matrix, out matrix);
+ return matrix;
+ }
+
+
+ public static void Invert(ref Matrix matrix, out Matrix result)
+ {
+ //
+ // Use Laplace expansion theorem to calculate the inverse of a 4x4 matrix
+ //
+ // 1. Calculate the 2x2 determinants needed and the 4x4 determinant based on the 2x2 determinants
+ // 2. Create the adjugate matrix, which satisfies: A * adj(A) = det(A) * I
+ // 3. Divide adjugate matrix with the determinant to find the inverse
+
+ float det1 = matrix.M11*matrix.M22 - matrix.M12*matrix.M21;
+ float det2 = matrix.M11*matrix.M23 - matrix.M13*matrix.M21;
+ float det3 = matrix.M11*matrix.M24 - matrix.M14*matrix.M21;
+ float det4 = matrix.M12*matrix.M23 - matrix.M13*matrix.M22;
+ float det5 = matrix.M12*matrix.M24 - matrix.M14*matrix.M22;
+ float det6 = matrix.M13*matrix.M24 - matrix.M14*matrix.M23;
+ float det7 = matrix.M31*matrix.M42 - matrix.M32*matrix.M41;
+ float det8 = matrix.M31*matrix.M43 - matrix.M33*matrix.M41;
+ float det9 = matrix.M31*matrix.M44 - matrix.M34*matrix.M41;
+ float det10 = matrix.M32*matrix.M43 - matrix.M33*matrix.M42;
+ float det11 = matrix.M32*matrix.M44 - matrix.M34*matrix.M42;
+ float det12 = matrix.M33*matrix.M44 - matrix.M34*matrix.M43;
+
+ float detMatrix = (float) (det1*det12 - det2*det11 + det3*det10 + det4*det9 - det5*det8 + det6*det7);
+
+ float invDetMatrix = 1f/detMatrix;
+
+ Matrix ret; // Allow for matrix and result to point to the same structure
+
+ ret.M11 = (matrix.M22*det12 - matrix.M23*det11 + matrix.M24*det10)*invDetMatrix;
+ ret.M12 = (-matrix.M12*det12 + matrix.M13*det11 - matrix.M14*det10)*invDetMatrix;
+ ret.M13 = (matrix.M42*det6 - matrix.M43*det5 + matrix.M44*det4)*invDetMatrix;
+ ret.M14 = (-matrix.M32*det6 + matrix.M33*det5 - matrix.M34*det4)*invDetMatrix;
+ ret.M21 = (-matrix.M21*det12 + matrix.M23*det9 - matrix.M24*det8)*invDetMatrix;
+ ret.M22 = (matrix.M11*det12 - matrix.M13*det9 + matrix.M14*det8)*invDetMatrix;
+ ret.M23 = (-matrix.M41*det6 + matrix.M43*det3 - matrix.M44*det2)*invDetMatrix;
+ ret.M24 = (matrix.M31*det6 - matrix.M33*det3 + matrix.M34*det2)*invDetMatrix;
+ ret.M31 = (matrix.M21*det11 - matrix.M22*det9 + matrix.M24*det7)*invDetMatrix;
+ ret.M32 = (-matrix.M11*det11 + matrix.M12*det9 - matrix.M14*det7)*invDetMatrix;
+ ret.M33 = (matrix.M41*det5 - matrix.M42*det3 + matrix.M44*det1)*invDetMatrix;
+ ret.M34 = (-matrix.M31*det5 + matrix.M32*det3 - matrix.M34*det1)*invDetMatrix;
+ ret.M41 = (-matrix.M21*det10 + matrix.M22*det8 - matrix.M23*det7)*invDetMatrix;
+ ret.M42 = (matrix.M11*det10 - matrix.M12*det8 + matrix.M13*det7)*invDetMatrix;
+ ret.M43 = (-matrix.M41*det4 + matrix.M42*det2 - matrix.M43*det1)*invDetMatrix;
+ ret.M44 = (matrix.M31*det4 - matrix.M32*det2 + matrix.M33*det1)*invDetMatrix;
+
+ result = ret;
+ }
+
+
+ public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount)
+ {
+ throw new NotImplementedException();
+ }
+
+
+ public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result)
+ {
+ throw new NotImplementedException();
+ }
+
+ public static Matrix Multiply(Matrix matrix1, Matrix matrix2)
+ {
+ Matrix ret;
+ Multiply(ref matrix1, ref matrix2, out ret);
+ return ret;
+ }
+
+
+ public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
+ {
+ result.M11 = matrix1.M11*matrix2.M11 + matrix1.M12*matrix2.M21 + matrix1.M13*matrix2.M31 +
+ matrix1.M14*matrix2.M41;
+ result.M12 = matrix1.M11*matrix2.M12 + matrix1.M12*matrix2.M22 + matrix1.M13*matrix2.M32 +
+ matrix1.M14*matrix2.M42;
+ result.M13 = matrix1.M11*matrix2.M13 + matrix1.M12*matrix2.M23 + matrix1.M13*matrix2.M33 +
+ matrix1.M14*matrix2.M43;
+ result.M14 = matrix1.M11*matrix2.M14 + matrix1.M12*matrix2.M24 + matrix1.M13*matrix2.M34 +
+ matrix1.M14*matrix2.M44;
+
+ result.M21 = matrix1.M21*matrix2.M11 + matrix1.M22*matrix2.M21 + matrix1.M23*matrix2.M31 +
+ matrix1.M24*matrix2.M41;
+ result.M22 = matrix1.M21*matrix2.M12 + matrix1.M22*matrix2.M22 + matrix1.M23*matrix2.M32 +
+ matrix1.M24*matrix2.M42;
+ result.M23 = matrix1.M21*matrix2.M13 + matrix1.M22*matrix2.M23 + matrix1.M23*matrix2.M33 +
+ matrix1.M24*matrix2.M43;
+ result.M24 = matrix1.M21*matrix2.M14 + matrix1.M22*matrix2.M24 + matrix1.M23*matrix2.M34 +
+ matrix1.M24*matrix2.M44;
+
+ result.M31 = matrix1.M31*matrix2.M11 + matrix1.M32*matrix2.M21 + matrix1.M33*matrix2.M31 +
+ matrix1.M34*matrix2.M41;
+ result.M32 = matrix1.M31*matrix2.M12 + matrix1.M32*matrix2.M22 + matrix1.M33*matrix2.M32 +
+ matrix1.M34*matrix2.M42;
+ result.M33 = matrix1.M31*matrix2.M13 + matrix1.M32*matrix2.M23 + matrix1.M33*matrix2.M33 +
+ matrix1.M34*matrix2.M43;
+ result.M34 = matrix1.M31*matrix2.M14 + matrix1.M32*matrix2.M24 + matrix1.M33*matrix2.M34 +
+ matrix1.M34*matrix2.M44;
+
+ result.M41 = matrix1.M41*matrix2.M11 + matrix1.M42*matrix2.M21 + matrix1.M43*matrix2.M31 +
+ matrix1.M44*matrix2.M41;
+ result.M42 = matrix1.M41*matrix2.M12 + matrix1.M42*matrix2.M22 + matrix1.M43*matrix2.M32 +
+ matrix1.M44*matrix2.M42;
+ result.M43 = matrix1.M41*matrix2.M13 + matrix1.M42*matrix2.M23 + matrix1.M43*matrix2.M33 +
+ matrix1.M44*matrix2.M43;
+ result.M44 = matrix1.M41*matrix2.M14 + matrix1.M42*matrix2.M24 + matrix1.M43*matrix2.M34 +
+ matrix1.M44*matrix2.M44;
+ }
+
+
+ public static Matrix Multiply(Matrix matrix1, float factor)
+ {
+ matrix1.M11 *= factor;
+ matrix1.M12 *= factor;
+ matrix1.M13 *= factor;
+ matrix1.M14 *= factor;
+ matrix1.M21 *= factor;
+ matrix1.M22 *= factor;
+ matrix1.M23 *= factor;
+ matrix1.M24 *= factor;
+ matrix1.M31 *= factor;
+ matrix1.M32 *= factor;
+ matrix1.M33 *= factor;
+ matrix1.M34 *= factor;
+ matrix1.M41 *= factor;
+ matrix1.M42 *= factor;
+ matrix1.M43 *= factor;
+ matrix1.M44 *= factor;
+ return matrix1;
+ }
+
+
+ public static void Multiply(ref Matrix matrix1, float factor, out Matrix result)
+ {
+ result.M11 = matrix1.M11*factor;
+ result.M12 = matrix1.M12*factor;
+ result.M13 = matrix1.M13*factor;
+ result.M14 = matrix1.M14*factor;
+ result.M21 = matrix1.M21*factor;
+ result.M22 = matrix1.M22*factor;
+ result.M23 = matrix1.M23*factor;
+ result.M24 = matrix1.M24*factor;
+ result.M31 = matrix1.M31*factor;
+ result.M32 = matrix1.M32*factor;
+ result.M33 = matrix1.M33*factor;
+ result.M34 = matrix1.M34*factor;
+ result.M41 = matrix1.M41*factor;
+ result.M42 = matrix1.M42*factor;
+ result.M43 = matrix1.M43*factor;
+ result.M44 = matrix1.M44*factor;
+ }
+
+
+ public static Matrix Negate(Matrix matrix)
+ {
+ Multiply(ref matrix, -1.0f, out matrix);
+ return matrix;
+ }
+
+
+ public static void Negate(ref Matrix matrix, out Matrix result)
+ {
+ Multiply(ref matrix, -1.0f, out result);
+ }
+
+ public static Matrix Subtract(Matrix matrix1, Matrix matrix2)
+ {
+ matrix1.M11 -= matrix2.M11;
+ matrix1.M12 -= matrix2.M12;
+ matrix1.M13 -= matrix2.M13;
+ matrix1.M14 -= matrix2.M14;
+ matrix1.M21 -= matrix2.M21;
+ matrix1.M22 -= matrix2.M22;
+ matrix1.M23 -= matrix2.M23;
+ matrix1.M24 -= matrix2.M24;
+ matrix1.M31 -= matrix2.M31;
+ matrix1.M32 -= matrix2.M32;
+ matrix1.M33 -= matrix2.M33;
+ matrix1.M34 -= matrix2.M34;
+ matrix1.M41 -= matrix2.M41;
+ matrix1.M42 -= matrix2.M42;
+ matrix1.M43 -= matrix2.M43;
+ matrix1.M44 -= matrix2.M44;
+ return matrix1;
+ }
+
+ public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
+ {
+ result.M11 = matrix1.M11 - matrix2.M11;
+ result.M12 = matrix1.M12 - matrix2.M12;
+ result.M13 = matrix1.M13 - matrix2.M13;
+ result.M14 = matrix1.M14 - matrix2.M14;
+ result.M21 = matrix1.M21 - matrix2.M21;
+ result.M22 = matrix1.M22 - matrix2.M22;
+ result.M23 = matrix1.M23 - matrix2.M23;
+ result.M24 = matrix1.M24 - matrix2.M24;
+ result.M31 = matrix1.M31 - matrix2.M31;
+ result.M32 = matrix1.M32 - matrix2.M32;
+ result.M33 = matrix1.M33 - matrix2.M33;
+ result.M34 = matrix1.M34 - matrix2.M34;
+ result.M41 = matrix1.M41 - matrix2.M41;
+ result.M42 = matrix1.M42 - matrix2.M42;
+ result.M43 = matrix1.M43 - matrix2.M43;
+ result.M44 = matrix1.M44 - matrix2.M44;
+ }
+
+ public static Matrix Transpose(Matrix matrix)
+ {
+ Matrix ret;
+ Transpose(ref matrix, out ret);
+ return ret;
+ }
+
+
+ public static void Transpose(ref Matrix matrix, out Matrix result)
+ {
+ result.M11 = matrix.M11;
+ result.M12 = matrix.M21;
+ result.M13 = matrix.M31;
+ result.M14 = matrix.M41;
+
+ result.M21 = matrix.M12;
+ result.M22 = matrix.M22;
+ result.M23 = matrix.M32;
+ result.M24 = matrix.M42;
+
+ result.M31 = matrix.M13;
+ result.M32 = matrix.M23;
+ result.M33 = matrix.M33;
+ result.M34 = matrix.M43;
+
+ result.M41 = matrix.M14;
+ result.M42 = matrix.M24;
+ result.M43 = matrix.M34;
+ result.M44 = matrix.M44;
+ }
+
+#endregion Public Static Methods
+
+#region Public Methods
+
+ public float Determinant()
+ {
+ float minor1, minor2, minor3, minor4, minor5, minor6;
+
+ minor1 = M31*M42 - M32*M41;
+ minor2 = M31*M43 - M33*M41;
+ minor3 = M31*M44 - M34*M41;
+ minor4 = M32*M43 - M33*M42;
+ minor5 = M32*M44 - M34*M42;
+ minor6 = M33*M44 - M34*M43;
+
+ return M11*(M22*minor6 - M23*minor5 + M24*minor4) -
+ M12*(M21*minor6 - M23*minor3 + M24*minor2) +
+ M13*(M21*minor5 - M22*minor3 + M24*minor1) -
+ M14*(M21*minor4 - M22*minor2 + M23*minor1);
+ }
+
+ public bool Equals(Matrix other)
+ {
+ return this == other;
+ }
+
+#endregion Public Methods
+
+#region Operators
+
+ public static Matrix operator +(Matrix matrix1, Matrix matrix2)
+ {
+ Add(ref matrix1, ref matrix2, out matrix1);
+ return matrix1;
+ }
+
+ public static Matrix operator /(Matrix matrix1, Matrix matrix2)
+ {
+ Matrix ret;
+ Divide(ref matrix1, ref matrix2, out ret);
+ return ret;
+ }
+
+ public static Matrix operator /(Matrix matrix1, float divider)
+ {
+ Matrix ret;
+ Divide(ref matrix1, divider, out ret);
+ return ret;
+ }
+
+ public static bool operator ==(Matrix matrix1, Matrix matrix2)
+ {
+ return (matrix1.M11 == matrix2.M11) && (matrix1.M12 == matrix2.M12) &&
+ (matrix1.M13 == matrix2.M13) && (matrix1.M14 == matrix2.M14) &&
+ (matrix1.M21 == matrix2.M21) && (matrix1.M22 == matrix2.M22) &&
+ (matrix1.M23 == matrix2.M23) && (matrix1.M24 == matrix2.M24) &&
+ (matrix1.M31 == matrix2.M31) && (matrix1.M32 == matrix2.M32) &&
+ (matrix1.M33 == matrix2.M33) && (matrix1.M34 == matrix2.M34) &&
+ (matrix1.M41 == matrix2.M41) && (matrix1.M42 == matrix2.M42) &&
+ (matrix1.M43 == matrix2.M43) && (matrix1.M44 == matrix2.M44);
+ }
+
+ public static bool operator !=(Matrix matrix1, Matrix matrix2)
+ {
+ return !(matrix1 == matrix2);
+ }
+
+ public static Matrix operator *(Matrix matrix1, Matrix matrix2)
+ {
+ Matrix returnMatrix = new Matrix();
+ Multiply(ref matrix1, ref matrix2, out returnMatrix);
+ return returnMatrix;
+ }
+
+ public static Matrix operator *(Matrix matrix, float scaleFactor)
+ {
+ Multiply(ref matrix, scaleFactor, out matrix);
+ return matrix;
+ }
+
+ public static Matrix operator *(float scaleFactor, Matrix matrix)
+ {
+ Matrix target;
+ target.M11 = matrix.M11*scaleFactor;
+ target.M12 = matrix.M12*scaleFactor;
+ target.M13 = matrix.M13*scaleFactor;
+ target.M14 = matrix.M14*scaleFactor;
+ target.M21 = matrix.M21*scaleFactor;
+ target.M22 = matrix.M22*scaleFactor;
+ target.M23 = matrix.M23*scaleFactor;
+ target.M24 = matrix.M24*scaleFactor;
+ target.M31 = matrix.M31*scaleFactor;
+ target.M32 = matrix.M32*scaleFactor;
+ target.M33 = matrix.M33*scaleFactor;
+ target.M34 = matrix.M34*scaleFactor;
+ target.M41 = matrix.M41*scaleFactor;
+ target.M42 = matrix.M42*scaleFactor;
+ target.M43 = matrix.M43*scaleFactor;
+ target.M44 = matrix.M44*scaleFactor;
+ return target;
+ }
+
+ public static Matrix operator -(Matrix matrix1, Matrix matrix2)
+ {
+ Matrix returnMatrix = new Matrix();
+ Subtract(ref matrix1, ref matrix2, out returnMatrix);
+ return returnMatrix;
+ }
+
+
+ public static Matrix operator -(Matrix matrix1)
+ {
+ Negate(ref matrix1, out matrix1);
+ return matrix1;
+ }
+
+#endregion
+
+#region Object Overrides
+
+ public override bool Equals(object obj)
+ {
+ return this == (Matrix) obj;
+ }
+
+ public override int GetHashCode()
+ {
+ throw new NotImplementedException();
+ }
+
+ public override string ToString()
+ {
+ return "{ {M11:" + M11 + " M12:" + M12 + " M13:" + M13 + " M14:" + M14 + "}" +
+ " {M21:" + M21 + " M22:" + M22 + " M23:" + M23 + " M24:" + M24 + "}" +
+ " {M31:" + M31 + " M32:" + M32 + " M33:" + M33 + " M34:" + M34 + "}" +
+ " {M41:" + M41 + " M42:" + M42 + " M43:" + M43 + " M44:" + M44 + "} }";
+ }
+
+#endregion
+ }
+}
+
+#endif
\ No newline at end of file
diff --git a/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Path.cs b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Path.cs
new file mode 100644
index 0000000..f8a8bf5
--- /dev/null
+++ b/FarseerPhysics Source/Farseer Physics Engine 3.5/Common/Path.cs
@@ -0,0 +1,337 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common
+{
+ //Contributed by Matthew Bettcher
+
+ ///
+ /// Path:
+ /// Very similar to Vertices, but this
+ /// class contains vectors describing
+ /// control points on a Catmull-Rom
+ /// curve.
+ ///
+ public class Path
+ {
+ ///
+ /// All the points that makes up the curve
+ ///
+ public List ControlPoints;
+
+ private float _deltaT;
+
+ ///
+ /// Initializes a new instance of the class.
+ ///
+ public Path()
+ {
+ ControlPoints = new List();
+ }
+
+ ///
+ /// Initializes a new instance of the class.
+ ///
+ /// The vertices to created the path from.
+ public Path(Vector2[] vertices)
+ {
+ ControlPoints = new List(vertices.Length);
+
+ for (int i = 0; i < vertices.Length; i++)
+ {
+ Add(vertices[i]);
+ }
+ }
+
+ ///
+ /// Initializes a new instance of the