adding missing ODE files for mesh
dan miller
parent
44f3e4fc50
commit
c1d05740e5
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue