From c1d05740e5f36bea303db7c810a81382663b7e13 Mon Sep 17 00:00:00 2001
From: dan miller
Date: Mon, 5 Nov 2007 18:05:21 +0000
Subject: adding missing ODE files for mesh

---
 .../Region/Physics/OdePlugin/Meshing/Extruder.cs   |  83 +++++
 OpenSim/Region/Physics/OdePlugin/Meshing/Mesh.cs   | 197 +++++++++++
 .../Region/Physics/OdePlugin/Meshing/SimpleHull.cs | 363 +++++++++++++++++++++
 .../Region/Physics/OdePlugin/Meshing/Simplex.cs    | 198 +++++++++++
 4 files changed, 841 insertions(+)
 create mode 100755 OpenSim/Region/Physics/OdePlugin/Meshing/Extruder.cs
 create mode 100755 OpenSim/Region/Physics/OdePlugin/Meshing/Mesh.cs
 create mode 100755 OpenSim/Region/Physics/OdePlugin/Meshing/SimpleHull.cs
 create mode 100755 OpenSim/Region/Physics/OdePlugin/Meshing/Simplex.cs

(limited to 'OpenSim/Region')

diff --git a/OpenSim/Region/Physics/OdePlugin/Meshing/Extruder.cs b/OpenSim/Region/Physics/OdePlugin/Meshing/Extruder.cs
new file mode 100755
index 0000000..497e039
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/Meshing/Extruder.cs
@@ -0,0 +1,83 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace OpenSim.Region.Physics.OdePlugin.Meshing
+{
+    class Extruder
+    {
+        public float startParameter;
+        public float stopParameter;
+        public Manager.PhysicsVector size;
+
+        public Mesh Extrude(Mesh m)
+        {
+            // Currently only works for iSteps=1;
+            Mesh result = new Mesh();
+
+            Mesh workingPlus  = m.Clone();
+            Mesh workingMinus = m.Clone();
+
+            foreach (Vertex v in workingPlus.vertices)
+            {
+                if (v == null)
+                    continue;
+
+                v.Z = +.5f;
+                v.X *= size.X;
+                v.Y *= size.Y;
+                v.Z *= size.Z;
+            }
+
+            foreach (Vertex v in workingMinus.vertices)
+            {
+                if (v == null)
+                    continue;
+
+                v.Z = -.5f;
+                v.X *= size.X;
+                v.Y *= size.Y;
+                v.Z *= size.Z;
+            }
+
+            foreach (Triangle t in workingMinus.triangles)
+            {
+                t.invertNormal();
+            }
+
+            result.Append(workingMinus);
+            result.Append(workingPlus);
+
+            int iLastNull = 0;
+            for (int i = 0; i < workingPlus.vertices.Count; i++)
+            {
+                int iNext = (i + 1);
+                
+                if (workingPlus.vertices[i] == null) // Can't make a simplex here
+                {
+                    iLastNull = i+1;
+                    continue;
+                }
+
+                if (i == workingPlus.vertices.Count-1) // End of list
+                {
+                    iNext = iLastNull;
+                }
+
+                if (workingPlus.vertices[iNext] == null) // Null means wrap to begin of last segment
+                {
+                    iNext = iLastNull;
+                }
+
+                Triangle tSide;
+                tSide = new Triangle(workingPlus.vertices[i], workingMinus.vertices[i], workingPlus.vertices[iNext]);
+                result.Add(tSide);
+
+                tSide = new Triangle(workingPlus.vertices[iNext], workingMinus.vertices[i], workingMinus.vertices[iNext]);
+                result.Add(tSide);
+            }
+
+            return result;
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/OdePlugin/Meshing/Mesh.cs b/OpenSim/Region/Physics/OdePlugin/Meshing/Mesh.cs
new file mode 100755
index 0000000..5a51703
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/Meshing/Mesh.cs
@@ -0,0 +1,197 @@
+using System;
+using System.IO;
+using System.Collections.Generic;
+using System.Text;
+
+using System.Runtime.InteropServices;
+
+
+using OpenSim.Region.Physics.Manager;
+
+namespace OpenSim.Region.Physics.OdePlugin.Meshing
+{
+    public class Mesh
+    {
+        public List<Vertex> vertices;
+        public List<Triangle> triangles;
+
+        public float[] normals;
+
+        public Mesh()
+        {
+            vertices = new List<Vertex>();
+            triangles = new List<Triangle>();
+        }
+
+        public Mesh Clone()
+        {
+            Mesh result = new Mesh();
+
+            foreach (Vertex v in vertices)
+            {
+                if (v == null)
+                    result.vertices.Add(null);
+                else
+                    result.vertices.Add(v.Clone());
+            }
+
+            foreach (Triangle t in triangles)
+            {
+                int iV1, iV2, iV3;
+                iV1 = this.vertices.IndexOf(t.v1);
+                iV2 = this.vertices.IndexOf(t.v2);
+                iV3 = this.vertices.IndexOf(t.v3);
+
+                Triangle newT = new Triangle(result.vertices[iV1], result.vertices[iV2], result.vertices[iV3]);
+                result.Add(newT);
+            }
+
+            return result;
+        }
+
+        
+        
+        public void Add(Triangle triangle)
+        {
+            int i;
+            i = vertices.IndexOf(triangle.v1);
+            if (i < 0)
+                throw new ArgumentException("Vertex v1 not known to mesh");
+            i = vertices.IndexOf(triangle.v2);
+            if (i < 0)
+                throw new ArgumentException("Vertex v2 not known to mesh");
+            i = vertices.IndexOf(triangle.v3);
+            if (i < 0)
+                throw new ArgumentException("Vertex v3 not known to mesh");
+
+            triangles.Add(triangle);
+        }
+
+        public void Add(Vertex v)
+        {
+            vertices.Add(v);
+        }
+
+        public void Remove(Vertex v)
+        {
+            int i;
+
+            // First, remove all triangles that are build on v
+            for (i = 0; i < triangles.Count; i++)
+            {
+                Triangle t = triangles[i];
+                if (t.v1 == v || t.v2 == v || t.v3 == v)
+                {
+                    triangles.RemoveAt(i);
+                    i--;
+                }
+            }
+
+            // Second remove v itself
+            vertices.Remove(v);
+        }
+
+        public void RemoveTrianglesOutside(SimpleHull hull)
+        {
+            int i;
+
+            for (i = 0; i < triangles.Count; i++)
+            {
+                Triangle t = triangles[i];
+                Vertex v1 = t.v1;
+                Vertex v2 = t.v2;
+                Vertex v3 = t.v3;
+                PhysicsVector m = v1 + v2 + v3;
+                m /= 3.0f;
+                if (!hull.IsPointIn(new Vertex(m)))
+                {
+                    triangles.RemoveAt(i);
+                    i--;
+                }
+            }
+        }
+
+
+        public void Add(List<Vertex> lv)
+        {
+            foreach (Vertex v in lv)
+            {
+                vertices.Add(v);
+            }
+        }
+
+        public float[] getVertexListAsFloat()
+        {
+            float[] result = new float[vertices.Count * 3];
+            for (int i = 0; i < vertices.Count; i++)
+            {
+                Vertex v = vertices[i];
+                if (v == null)
+                    continue;
+                result[3 * i + 0] = v.X;
+                result[3 * i + 1] = v.Y;
+                result[3 * i + 2] = v.Z;
+            }
+            GCHandle.Alloc(result, GCHandleType.Pinned);
+            return result;
+        }
+
+        public int[] getIndexListAsInt()
+        {
+            int[] result = new int[triangles.Count * 3];
+            for (int i = 0; i < triangles.Count; i++)
+            {
+                Triangle t = triangles[i];
+                result[3 * i + 0] = vertices.IndexOf(t.v1);
+                result[3 * i + 1] = vertices.IndexOf(t.v2);
+                result[3 * i + 2] = vertices.IndexOf(t.v3);
+            }
+            GCHandle.Alloc(result, GCHandleType.Pinned);
+            return result;
+        }
+
+
+        public void Append(Mesh newMesh)
+        {
+            foreach (Vertex v in newMesh.vertices)
+                vertices.Add(v);
+
+            foreach (Triangle t in newMesh.triangles)
+                Add(t);
+
+        }
+
+        // Do a linear transformation of  mesh.
+        public void TransformLinear(float[,] matrix, float[] offset)
+        {
+            foreach (Vertex v in vertices)
+            {
+                if (v == null)
+                    continue;
+                float x, y, z;
+                x = v.X * matrix[0, 0] + v.Y * matrix[1, 0] + v.Z * matrix[2, 0];
+                y = v.X * matrix[0, 1] + v.Y * matrix[1, 1] + v.Z * matrix[2, 1];
+                z = v.X * matrix[0, 2] + v.Y * matrix[1, 2] + v.Z * matrix[2, 2];
+                v.X = x + offset[0];
+                v.Y = y + offset[1];
+                v.Z = z + offset[2];
+            }
+        }
+
+        public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+            foreach (Triangle t in triangles)
+            {
+                String s = t.ToStringRaw();
+                sw.WriteLine(s);
+            }
+            sw.Close();
+        }
+    }
+
+}
diff --git a/OpenSim/Region/Physics/OdePlugin/Meshing/SimpleHull.cs b/OpenSim/Region/Physics/OdePlugin/Meshing/SimpleHull.cs
new file mode 100755
index 0000000..2caa818
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/Meshing/SimpleHull.cs
@@ -0,0 +1,363 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using OpenSim.Framework.Console;
+
+namespace OpenSim.Region.Physics.OdePlugin.Meshing
+{
+    // A simple hull is a set of vertices building up to simplices that border a region
+    // The word simple referes to the fact, that this class assumes, that all simplices
+    // do not intersect
+    // Simple hulls can be added and subtracted.
+    // Vertices can be checked to lie inside a hull
+    // Also note, that the sequence of the vertices is important and defines if the region that
+    // is defined by the hull lies inside or outside the simplex chain
+    public class SimpleHull
+    {
+        List<Vertex> vertices = new List<Vertex>();
+        List<Vertex> holeVertices = new List<Vertex>(); // Only used, when the hull is hollow
+
+        // Adds a vertex to the end of the list
+        public void AddVertex(Vertex v) {
+            vertices.Add(v);
+        }
+
+        override public String ToString()
+        {
+            String result="";
+            foreach (Vertex v in vertices)
+            {
+                result += "b:" + v.ToString() + "\n";
+            }
+
+            return result;
+        }
+
+
+        public List<Vertex> getVertices() {
+            List<Vertex> newVertices = new List<Vertex>();
+
+            newVertices.AddRange(vertices);
+            newVertices.Add(null);
+            newVertices.AddRange(holeVertices);
+
+            return newVertices;
+        }
+
+        public SimpleHull Clone()
+        {
+            SimpleHull result = new SimpleHull();
+            foreach (Vertex v in vertices)
+            {
+                result.AddVertex(v.Clone());
+            }
+
+            foreach (Vertex v in this.holeVertices)
+            {
+                result.holeVertices.Add(v.Clone());
+            }
+            
+            return result;
+        }
+
+        public bool IsPointIn(Vertex v1)
+        {
+            int iCounter=0;
+            List<Simplex> simplices=buildSimplexList();
+            foreach (Simplex s in simplices)
+            {
+                // Send a ray along the positive X-Direction
+                // Note, that this direction must correlate with the "below" interpretation
+                // of handling for the special cases below
+                Manager.PhysicsVector intersection = s.RayIntersect(v1, new Manager.PhysicsVector(1.0f, 0.0f, 0.0f), true);
+
+                if (intersection == null)
+                    continue;   // No intersection. Done. More tests to follow otherwise
+
+                // Did we hit the end of a simplex?
+                // Then this can be one of two special cases:
+                // 1. we go through a border exactly at a joint
+                // 2. we have just marginally touched a corner
+                // 3. we can slide along a border
+                // Solution: If the other vertex is "below" the ray, we don't count it
+                // Thus corners pointing down are counted twice, corners pointing up are not counted
+                // borders are counted once
+                if (intersection.IsIdentical(s.v1, 0.001f)) {
+                    if (s.v2.Y < v1.Y)
+                        continue;
+                }
+                // Do this for the other vertex two
+                if (intersection.IsIdentical(s.v2, 0.001f)) {
+                    if (s.v1.Y<v1.Y)
+                        continue;
+                }
+                iCounter++;
+            }
+
+            return iCounter % 2 == 1; // Point is inside if the number of intersections is odd
+        }
+
+        public bool containsPointsFrom(SimpleHull otherHull)
+        {
+            foreach (Vertex v in otherHull.vertices)
+            {
+                if (IsPointIn(v))
+                    return true;
+            }
+
+            return false;
+        }
+
+
+        List<Simplex> buildSimplexList() {
+
+            List<Simplex> result = new List<Simplex>();
+
+            // Not asserted but assumed: at least three vertices
+            for (int i=0; i<vertices.Count-1; i++) {
+                Simplex s=new Simplex(vertices[i], vertices[i+1]);
+                result.Add(s);
+            }
+            Simplex s1=new Simplex(vertices[vertices.Count-1], vertices[0]);
+            result.Add(s1);
+
+            if (holeVertices.Count==0)
+                return result;
+
+            // Same here. At least three vertices in hole assumed
+            for (int i = 0; i < holeVertices.Count - 1; i++)
+            {
+                Simplex s = new Simplex(holeVertices[i], holeVertices[i + 1]);
+                result.Add(s);
+            }
+            
+            s1 = new Simplex(holeVertices[holeVertices.Count - 1], holeVertices[0]);
+            result.Add(s1);
+            return result;
+        }
+
+        bool InsertVertex(Vertex v, int iAfter)
+        {
+            vertices.Insert(iAfter + 1, v);
+            return true;
+        }
+
+        Vertex getNextVertex(Vertex currentVertex)
+        {
+            int iCurrentIndex;
+            iCurrentIndex = vertices.IndexOf(currentVertex);
+
+            // Error handling for iCurrentIndex==-1 should go here (and probably never will)
+
+            iCurrentIndex++;
+            if (iCurrentIndex == vertices.Count)
+                iCurrentIndex = 0;
+
+            return vertices[iCurrentIndex];
+        }
+
+        public Vertex FindVertex(Vertex vBase, float tolerance) {
+            foreach (Vertex v in vertices) {
+                if (v.IsIdentical(vBase, tolerance))
+                    return v;
+            }
+
+            return null;
+        }
+
+        public void FindIntersection(Simplex s, ref Vertex Intersection, ref Vertex nextVertex)
+        {
+            Vertex bestIntersection=null;
+            float distToV1=Single.PositiveInfinity;
+            Simplex bestIntersectingSimplex=null;
+
+            List<Simplex> simple = buildSimplexList();
+            foreach (Simplex sTest in simple)
+            {
+                Manager.PhysicsVector vvTemp = Simplex.Intersect(sTest, s, -.001f, -.001f, 0.999f, .999f);
+                
+                Vertex vTemp=null;
+                if (vvTemp != null)
+                    vTemp = new Vertex(vvTemp);
+
+                if (vTemp!=null) {
+
+                    Manager.PhysicsVector diff=(s.v1-vTemp);
+                    float distTemp=diff.length();
+
+                    if (bestIntersection==null || distTemp<distToV1) {
+                        bestIntersection=vTemp;
+                        distToV1=distTemp;
+                        bestIntersectingSimplex = sTest;
+                    }
+
+                } // end if vTemp
+
+            } // end foreach
+
+            Intersection = bestIntersection;
+            if (bestIntersectingSimplex != null)
+                nextVertex = bestIntersectingSimplex.v2;
+            else
+                nextVertex = null;
+        }
+
+
+        public static SimpleHull SubtractHull(SimpleHull baseHull, SimpleHull otherHull)
+        {
+
+            SimpleHull baseHullClone = baseHull.Clone();
+            SimpleHull otherHullClone = otherHull.Clone();
+            bool intersects = false;
+
+            MainLog.Instance.Debug("State before intersection detection");
+            MainLog.Instance.Debug("The baseHull is:\n{1}", 0, baseHullClone.ToString());
+            MainLog.Instance.Debug("The otherHull is:\n{1}", 0, otherHullClone.ToString());
+
+            {
+                int iBase, iOther;
+
+                // Insert into baseHull
+                for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
+                {
+                    int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
+                    Simplex sBase = new Simplex(baseHullClone.vertices[iBase], baseHullClone.vertices[iBaseNext]);
+
+                    for (iOther = 0; iOther < otherHullClone.vertices.Count; iOther++)
+                    {
+                        int iOtherNext = (iOther + 1) % otherHullClone.vertices.Count;
+                        Simplex sOther = new Simplex(otherHullClone.vertices[iOther], otherHullClone.vertices[iOtherNext]);
+
+                        Manager.PhysicsVector intersect = Simplex.Intersect(sBase, sOther, 0.001f, -.001f, 0.999f, 1.001f);
+                        if (intersect != null)
+                        {
+                            Vertex vIntersect = new Vertex(intersect);
+                            baseHullClone.vertices.Insert(iBase + 1, vIntersect);
+                            sBase.v2 = vIntersect;
+                            intersects = true;
+                        }
+                    }
+                }
+            }
+
+            MainLog.Instance.Debug("State after intersection detection for the base hull");
+            MainLog.Instance.Debug("The baseHull is:\n{1}", 0, baseHullClone.ToString());
+
+            {
+                int iOther, iBase;
+
+                // Insert into otherHull
+                for (iOther = 0; iOther < otherHullClone.vertices.Count; iOther++)
+                {
+                    int iOtherNext = (iOther + 1) % otherHullClone.vertices.Count;
+                    Simplex sOther = new Simplex(otherHullClone.vertices[iOther], otherHullClone.vertices[iOtherNext]);
+
+                    for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
+                    {
+                        int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
+                        Simplex sBase = new Simplex(baseHullClone.vertices[iBase], baseHullClone.vertices[iBaseNext]);
+
+                        Manager.PhysicsVector intersect = Simplex.Intersect(sBase, sOther, -.001f, 0.001f, 1.001f, 0.999f);
+                        if (intersect != null)
+                        {
+                            Vertex vIntersect = new Vertex(intersect);
+                            otherHullClone.vertices.Insert(iOther + 1, vIntersect);
+                            sOther.v2 = vIntersect;
+                            intersects = true;
+                        }
+                    }
+                }
+            }
+
+            MainLog.Instance.Debug("State after intersection detection for the base hull");
+            MainLog.Instance.Debug("The otherHull is:\n{1}", 0, otherHullClone.ToString());
+
+
+            bool otherIsInBase = baseHullClone.containsPointsFrom(otherHullClone);
+            if (!intersects && otherIsInBase)
+            {
+                // We have a hole here
+                baseHullClone.holeVertices = otherHullClone.vertices;
+                return baseHullClone;
+            }
+
+
+            SimpleHull result = new SimpleHull();
+
+            // Find a good starting Simplex from baseHull
+            // A good starting simplex is one that is outside otherHull
+            // Such a simplex must exist, otherwise the result will be empty
+            Vertex baseStartVertex = null;
+            {
+                int iBase;
+                for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
+                {
+                    int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
+                    Vertex center = new Vertex((baseHullClone.vertices[iBase] + baseHullClone.vertices[iBaseNext]) / 2.0f);
+                    bool isOutside = !otherHullClone.IsPointIn(center);
+                    if (isOutside)
+                    {
+                        baseStartVertex = baseHullClone.vertices[iBaseNext];
+                        break;
+                    }
+                }
+            }
+
+
+            if (baseStartVertex == null) // i.e. no simplex fulfilled the "outside" condition. 
+            // In otherwords, subtractHull completely embraces baseHull
+            {
+                return result;
+            }
+
+            // The simplex that *starts* with baseStartVertex is outside the cutting hull,
+            // so we can start our walk with the next vertex without loosing a branch
+            Vertex V1 = baseStartVertex;
+            bool onBase = true;
+
+            // And here is how we do the magic :-)
+            // Start on the base hull.
+            // Walk the vertices in the positive direction
+            // For each vertex check, whether it is a vertex shared with the other hull
+            // if this is the case, switch over to walking the other vertex list.
+            // Note: The other hull *must* go backwards to our starting point (via several orther vertices)
+            // Thus it is important that the cutting hull has the inverse directional sense than the
+            // base hull!!!!!!!!! (means if base goes CW around it's center cutting hull must go CCW)
+
+            bool done = false;
+            while (!done)
+            {
+                result.AddVertex(V1);
+                Vertex nextVertex = null;
+                if (onBase)
+                {
+                    nextVertex = otherHullClone.FindVertex(V1, 0.001f);
+                }
+                else
+                {
+                    nextVertex = baseHullClone.FindVertex(V1, 0.001f);
+                }
+
+                if (nextVertex != null) // A node that represents an intersection
+                {
+                    V1 = nextVertex;    // Needed to find the next vertex on the other hull
+                    onBase = !onBase;
+                }
+
+                if (onBase)
+                    V1 = baseHullClone.getNextVertex(V1);
+                else
+                    V1 = otherHullClone.getNextVertex(V1);
+
+                if (V1 == baseStartVertex)
+                    done = true;
+            }
+
+            MainLog.Instance.Debug("The resulting Hull is:\n{1}", 0, result.ToString());
+
+            return result;
+
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/OdePlugin/Meshing/Simplex.cs b/OpenSim/Region/Physics/OdePlugin/Meshing/Simplex.cs
new file mode 100755
index 0000000..caebb3c
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/Meshing/Simplex.cs
@@ -0,0 +1,198 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using OpenSim.Region.Physics.Manager;
+
+namespace OpenSim.Region.Physics.OdePlugin.Meshing
+{
+    // A simplex is a section of a straight line.
+    // It is defined by its endpoints, i.e. by two vertices
+    // Operation on vertices are
+    public class Simplex : IComparable<Simplex>
+    {
+        public Vertex v1;
+        public Vertex v2;
+
+        public Simplex(Vertex _v1, Vertex _v2)
+        {
+            v1 = _v1;
+            v2 = _v2;
+        }
+
+        public int CompareTo(Simplex other)
+        {
+
+            Vertex lv1, lv2, ov1, ov2, temp;
+
+            lv1 = v1;
+            lv2 = v2;
+            ov1 = other.v1;
+            ov2 = other.v2;
+
+            if (lv1 > lv2)
+            {
+                temp = lv1;
+                lv1 = lv2;
+                lv2 = temp;
+            }
+
+            if (ov1 > ov2)
+            {
+                temp = ov1;
+                ov1 = ov2;
+                ov2 = temp;
+            }
+
+            if (lv1 > ov1)
+            {
+                return 1;
+            }
+            if (lv1 < ov1)
+            {
+                return -1;
+            }
+
+            if (lv2 > ov2)
+            {
+                return 1;
+            }
+            if (lv2 < ov2)
+            {
+                return -1;
+            }
+
+            return 0;
+        }
+
+        private static void intersectParameter(PhysicsVector p1, PhysicsVector r1, PhysicsVector p2, PhysicsVector r2, ref float lambda, ref float mu)
+        { 
+            // Intersects two straights
+            // p1, p2, points on the straight
+            // r1, r2, directional vectors of the straight. Not necessarily of length 1!
+            // note, that l, m can be scaled such, that the range 0..1 is mapped to the area between two points,
+            // thus allowing to decide whether an intersection is between two points
+
+            float r1x = r1.X;
+            float r1y = r1.Y;
+            float r2x = r2.X;
+            float r2y = r2.Y;
+
+            float denom = r1y*r2x - r1x*r2y;
+
+            float p1x = p1.X;
+            float p1y = p1.Y;
+            float p2x = p2.X;
+            float p2y = p2.Y;
+
+            float z1=-p2x * r2y + p1x * r2y + (p2y - p1y) * r2x;
+            float z2=-p2x * r1y + p1x * r1y + (p2y - p1y) * r1x;
+
+            if (denom == 0.0f) // Means the straights are parallel. Either no intersection or an infinite number of them
+            {
+                if (z1==0.0f) {// Means they are identical -> many, many intersections
+                    lambda = Single.NaN;
+                    mu = Single.NaN;
+                } else {
+                    lambda = Single.PositiveInfinity;
+                    mu = Single.PositiveInfinity;
+                }
+                return;
+
+            }
+
+
+
+            lambda = z1 / denom;
+            mu     = z2 / denom;
+        
+        }
+
+
+        // Intersects the simplex with another one.
+        // the borders are used to deal with float inaccuracies
+        // As a rule of thumb, the borders are
+        // lowerBorder1 : 0.0
+        // lowerBorder2 : 0.0
+        // upperBorder1 : 1.0
+        // upperBorder2 : 1.0
+        // Set these to values near the given parameters (e.g. 0.001 instead of 1 to exclude simplex starts safely, or to -0.001 to include them safely)
+        public static PhysicsVector Intersect(
+                                        Simplex s1, 
+                                        Simplex s2, 
+                                        float lowerBorder1, 
+                                        float lowerBorder2, 
+                                        float upperBorder1, 
+                                        float upperBorder2)
+        {
+            PhysicsVector firstSimplexDirection = s1.v2 - s1.v1;
+            PhysicsVector secondSimplexDirection = s2.v2 - s2.v1;
+
+            float lambda = 0.0f;
+            float mu = 0.0f;
+
+            // Give us the parameters of an intersection. This subroutine does *not* take the constraints
+            // (intersection must be between v1 and v2 and it must be in the positive direction of the ray)
+            // into account. We do that afterwards.
+            intersectParameter(s1.v1, firstSimplexDirection, s2.v1, secondSimplexDirection, ref lambda, ref mu);
+
+            if (Single.IsInfinity(lambda)) // Special case. No intersection at all. directions parallel.
+                return null;
+
+            if (Single.IsNaN(lambda)) // Special case. many, many intersections.
+                return null;
+
+            if (lambda > upperBorder1) // We're behind v2
+                return null;
+
+            if (lambda < lowerBorder1)
+                return null;
+
+            if (mu < lowerBorder2) // outside simplex 2
+                return null;
+
+            if (mu > upperBorder2) // outside simplex 2
+                return null;
+
+            return s1.v1 + lambda * firstSimplexDirection;
+
+        }
+
+        // Intersects the simplex with a ray. The ray is defined as all p=origin + lambda*direction
+        // where lambda >= 0
+        public PhysicsVector RayIntersect(Vertex origin, PhysicsVector direction, bool bEndsIncluded)
+        {
+            PhysicsVector simplexDirection = v2 - v1;
+
+            float lambda = 0.0f;
+            float mu = 0.0f;
+
+            // Give us the parameters of an intersection. This subroutine does *not* take the constraints
+            // (intersection must be between v1 and v2 and it must be in the positive direction of the ray)
+            // into account. We do that afterwards.
+            intersectParameter(v1, simplexDirection, origin, direction, ref lambda, ref mu);
+
+            if (Single.IsInfinity(lambda)) // Special case. No intersection at all. directions parallel.
+                return null;
+
+            if (Single.IsNaN(lambda)) // Special case. many, many intersections.
+                return null;
+
+            if (mu < 0.0) // We're on the wrong side of the ray
+                return null;
+
+            if (lambda > 1.0) // We're behind v2
+                return null;
+
+            if (lambda == 1.0 && !bEndsIncluded)
+                return null;    // The end of the simplices are not included
+
+            if (lambda < 0.0f) // we're before v1;
+                return null;
+
+            return this.v1 + lambda * simplexDirection;
+
+        }
+
+
+    }
+}
-- 
cgit v1.1