aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/Meshing
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/Meshing')
-rw-r--r--OpenSim/Region/Physics/Meshing/Extruder.cs83
-rw-r--r--OpenSim/Region/Physics/Meshing/HelperTypes.cs306
-rw-r--r--OpenSim/Region/Physics/Meshing/Mesh.cs213
-rw-r--r--OpenSim/Region/Physics/Meshing/Meshmerizer.cs393
-rw-r--r--OpenSim/Region/Physics/Meshing/SimpleHull.cs363
-rw-r--r--OpenSim/Region/Physics/Meshing/Simplex.cs198
6 files changed, 1556 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/Meshing/Extruder.cs b/OpenSim/Region/Physics/Meshing/Extruder.cs
new file mode 100644
index 0000000..63d727f
--- /dev/null
+++ b/OpenSim/Region/Physics/Meshing/Extruder.cs
@@ -0,0 +1,83 @@
1using System;
2using System.Collections.Generic;
3using System.Text;
4
5namespace OpenSim.Region.Physics.Meshing
6{
7 class Extruder
8 {
9 public float startParameter;
10 public float stopParameter;
11 public Manager.PhysicsVector size;
12
13 public Mesh Extrude(Mesh m)
14 {
15 // Currently only works for iSteps=1;
16 Mesh result = new Mesh();
17
18 Mesh workingPlus = m.Clone();
19 Mesh workingMinus = m.Clone();
20
21 foreach (Vertex v in workingPlus.vertices)
22 {
23 if (v == null)
24 continue;
25
26 v.Z = +.5f;
27 v.X *= size.X;
28 v.Y *= size.Y;
29 v.Z *= size.Z;
30 }
31
32 foreach (Vertex v in workingMinus.vertices)
33 {
34 if (v == null)
35 continue;
36
37 v.Z = -.5f;
38 v.X *= size.X;
39 v.Y *= size.Y;
40 v.Z *= size.Z;
41 }
42
43 foreach (Triangle t in workingMinus.triangles)
44 {
45 t.invertNormal();
46 }
47
48 result.Append(workingMinus);
49 result.Append(workingPlus);
50
51 int iLastNull = 0;
52 for (int i = 0; i < workingPlus.vertices.Count; i++)
53 {
54 int iNext = (i + 1);
55
56 if (workingPlus.vertices[i] == null) // Can't make a simplex here
57 {
58 iLastNull = i+1;
59 continue;
60 }
61
62 if (i == workingPlus.vertices.Count-1) // End of list
63 {
64 iNext = iLastNull;
65 }
66
67 if (workingPlus.vertices[iNext] == null) // Null means wrap to begin of last segment
68 {
69 iNext = iLastNull;
70 }
71
72 Triangle tSide;
73 tSide = new Triangle(workingPlus.vertices[i], workingMinus.vertices[i], workingPlus.vertices[iNext]);
74 result.Add(tSide);
75
76 tSide = new Triangle(workingPlus.vertices[iNext], workingMinus.vertices[i], workingMinus.vertices[iNext]);
77 result.Add(tSide);
78 }
79
80 return result;
81 }
82 }
83}
diff --git a/OpenSim/Region/Physics/Meshing/HelperTypes.cs b/OpenSim/Region/Physics/Meshing/HelperTypes.cs
new file mode 100644
index 0000000..9e75826
--- /dev/null
+++ b/OpenSim/Region/Physics/Meshing/HelperTypes.cs
@@ -0,0 +1,306 @@
1/*
2* Copyright (c) Contributors, http://opensimulator.org/
3* See CONTRIBUTORS.TXT for a full list of copyright holders.
4*
5* Redistribution and use in source and binary forms, with or without
6* modification, are permitted provided that the following conditions are met:
7* * Redistributions of source code must retain the above copyright
8* notice, this list of conditions and the following disclaimer.
9* * Redistributions in binary form must reproduce the above copyright
10* notice, this list of conditions and the following disclaimer in the
11* documentation and/or other materials provided with the distribution.
12* * Neither the name of the OpenSim Project nor the
13* names of its contributors may be used to endorse or promote products
14* derived from this software without specific prior written permission.
15*
16* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS AS IS AND ANY
17* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26*
27*/
28
29using System;
30using System.Collections.Generic;
31using System.Diagnostics;
32using System.Globalization;
33using OpenSim.Framework.Console;
34using OpenSim.Region.Physics.Manager;
35
36using OpenSim.Region.Physics.Meshing;
37
38public class Vertex : PhysicsVector, IComparable<Vertex>
39{
40 public Vertex(float x, float y, float z)
41 : base(x, y, z)
42 {
43 }
44
45 public Vertex(PhysicsVector v)
46 : base(v.X, v.Y, v.Z)
47 {
48 }
49
50 public Vertex Clone()
51 {
52 return new Vertex(X, Y, Z);
53 }
54
55 public static Vertex FromAngle(double angle)
56 {
57 return new Vertex((float)Math.Cos(angle), (float)Math.Sin(angle), 0.0f);
58 }
59
60
61 public virtual bool Equals(Vertex v, float tolerance)
62 {
63 PhysicsVector diff = this - v;
64 float d = diff.length();
65 if (d < tolerance)
66 return true;
67
68 return false;
69 }
70
71
72 public int CompareTo(Vertex other)
73 {
74 if (X < other.X)
75 return -1;
76
77 if (X > other.X)
78 return 1;
79
80 if (Y < other.Y)
81 return -1;
82
83 if (Y > other.Y)
84 return 1;
85
86 if (Z < other.Z)
87 return -1;
88
89 if (Z > other.Z)
90 return 1;
91
92 return 0;
93 }
94
95 public static bool operator >(Vertex me, Vertex other)
96 {
97 return me.CompareTo(other) > 0;
98 }
99
100 public static bool operator <(Vertex me, Vertex other)
101 {
102 return me.CompareTo(other) < 0;
103 }
104 public String ToRaw()
105 {
106 // Why this stuff with the number formatter?
107 // Well, the raw format uses the english/US notation of numbers
108 // where the "," separates groups of 1000 while the "." marks the border between 1 and 10E-1.
109 // The german notation uses these characters exactly vice versa!
110 // The Float.ToString() routine is a localized one, giving different results depending on the country
111 // settings your machine works with. Unusable for a machine readable file format :-(
112 NumberFormatInfo nfi = new NumberFormatInfo();
113 nfi.NumberDecimalSeparator = ".";
114 nfi.NumberDecimalDigits = 3;
115
116 String s1 = X.ToString("N2", nfi) + " " + Y.ToString("N2", nfi) + " " + Z.ToString("N2", nfi);
117
118 return s1;
119 }
120
121}
122
123public class Triangle
124{
125 public Vertex v1;
126 public Vertex v2;
127 public Vertex v3;
128
129 private float radius_square;
130 private float cx;
131 private float cy;
132
133 public Triangle(Vertex _v1, Vertex _v2, Vertex _v3)
134 {
135 v1 = _v1;
136 v2 = _v2;
137 v3 = _v3;
138
139 CalcCircle();
140 }
141
142 public bool isInCircle(float x, float y)
143 {
144 float dx, dy;
145 float dd;
146
147 dx = x - cx;
148 dy = y - cy;
149
150 dd = dx*dx + dy*dy;
151 if (dd < radius_square)
152 return true;
153 else
154 return false;
155 }
156
157 public bool isDegraded()
158 {
159 // This means, the vertices of this triangle are somewhat strange.
160 // They either line up or at least two of them are identical
161 return (radius_square == 0.0);
162 }
163
164 private void CalcCircle()
165 {
166 // Calculate the center and the radius of a circle given by three points p1, p2, p3
167 // It is assumed, that the triangles vertices are already set correctly
168 double p1x, p2x, p1y, p2y, p3x, p3y;
169
170 // Deviation of this routine:
171 // A circle has the general equation (M-p)^2=r^2, where M and p are vectors
172 // this gives us three equations f(p)=r^2, each for one point p1, p2, p3
173 // putting respectively two equations together gives two equations
174 // f(p1)=f(p2) and f(p1)=f(p3)
175 // bringing all constant terms to one side brings them to the form
176 // M*v1=c1 resp.M*v2=c2 where v1=(p1-p2) and v2=(p1-p3) (still vectors)
177 // and c1, c2 are scalars (Naming conventions like the variables below)
178 // Now using the equations that are formed by the components of the vectors
179 // and isolate Mx lets you make one equation that only holds My
180 // The rest is straight forward and eaasy :-)
181 //
182
183 /* helping variables for temporary results */
184 double c1, c2;
185 double v1x, v1y, v2x, v2y;
186
187 double z, n;
188
189 double rx, ry;
190
191 // Readout the three points, the triangle consists of
192 p1x = v1.X;
193 p1y = v1.Y;
194
195 p2x = v2.X;
196 p2y = v2.Y;
197
198 p3x = v3.X;
199 p3y = v3.Y;
200
201 /* calc helping values first */
202 c1 = (p1x*p1x + p1y*p1y - p2x*p2x - p2y*p2y)/2;
203 c2 = (p1x*p1x + p1y*p1y - p3x*p3x - p3y*p3y)/2;
204
205 v1x = p1x - p2x;
206 v1y = p1y - p2y;
207
208 v2x = p1x - p3x;
209 v2y = p1y - p3y;
210
211 z = (c1*v2x - c2*v1x);
212 n = (v1y*v2x - v2y*v1x);
213
214 if (n == 0.0) // This is no triangle, i.e there are (at least) two points at the same location
215 {
216 radius_square = 0.0f;
217 return;
218 }
219
220 cy = (float) (z/n);
221
222 if (v2x != 0.0)
223 {
224 cx = (float) ((c2 - v2y*cy)/v2x);
225 }
226 else if (v1x != 0.0)
227 {
228 cx = (float) ((c1 - v1y*cy)/v1x);
229 }
230 else
231 {
232 Debug.Assert(false, "Malformed triangle"); /* Both terms zero means nothing good */
233 }
234
235 rx = (p1x - cx);
236 ry = (p1y - cy);
237
238 radius_square = (float) (rx*rx + ry*ry);
239 }
240
241 public List<Simplex> GetSimplices()
242 {
243 List<Simplex> result = new List<Simplex>();
244 Simplex s1 = new Simplex(v1, v2);
245 Simplex s2 = new Simplex(v2, v3);
246 Simplex s3 = new Simplex(v3, v1);
247
248 result.Add(s1);
249 result.Add(s2);
250 result.Add(s3);
251
252 return result;
253 }
254
255 public override String ToString()
256 {
257 NumberFormatInfo nfi = new NumberFormatInfo();
258 nfi.CurrencyDecimalDigits = 2;
259 nfi.CurrencyDecimalSeparator = ".";
260
261 String s1 = "<" + v1.X.ToString(nfi) + "," + v1.Y.ToString(nfi) + "," + v1.Z.ToString(nfi) + ">";
262 String s2 = "<" + v2.X.ToString(nfi) + "," + v2.Y.ToString(nfi) + "," + v2.Z.ToString(nfi) + ">";
263 String s3 = "<" + v3.X.ToString(nfi) + "," + v3.Y.ToString(nfi) + "," + v3.Z.ToString(nfi) + ">";
264
265 return s1 + ";" + s2 + ";" + s3;
266 }
267
268 public PhysicsVector getNormal()
269 {
270 // Vertices
271
272 // Vectors for edges
273 PhysicsVector e1;
274 PhysicsVector e2;
275
276 e1 = new PhysicsVector(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
277 e2 = new PhysicsVector(v1.X - v3.X, v1.Y - v3.Y, v1.Z - v3.Z);
278
279 // Cross product for normal
280 PhysicsVector n = PhysicsVector.cross(e1, e2);
281
282 // Length
283 float l = n.length();
284
285 // Normalized "normal"
286 n = n / l;
287
288 return n;
289 }
290
291 public void invertNormal()
292 {
293 Vertex vt;
294 vt = v1;
295 v1 = v2;
296 v2 = vt;
297 }
298
299 // Dumps a triangle in the "raw faces" format, blender can import. This is for visualisation and
300 // debugging purposes
301 public String ToStringRaw()
302 {
303 String output = v1.ToRaw() + " " + v2.ToRaw() + " " +v3.ToRaw();
304 return output;
305 }
306} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs
new file mode 100644
index 0000000..fd4ce4e
--- /dev/null
+++ b/OpenSim/Region/Physics/Meshing/Mesh.cs
@@ -0,0 +1,213 @@
1using System;
2using System.IO;
3using System.Collections.Generic;
4using System.Text;
5
6using System.Runtime.InteropServices;
7
8
9using OpenSim.Region.Physics.Manager;
10
11namespace OpenSim.Region.Physics.Meshing
12{
13 public class Mesh : IMesh
14 {
15 public List<Vertex> vertices;
16 public List<Triangle> triangles;
17
18 public float[] normals;
19
20 public Mesh()
21 {
22 vertices = new List<Vertex>();
23 triangles = new List<Triangle>();
24 }
25
26 public Mesh Clone()
27 {
28 Mesh result = new Mesh();
29
30 foreach (Vertex v in vertices)
31 {
32 if (v == null)
33 result.vertices.Add(null);
34 else
35 result.vertices.Add(v.Clone());
36 }
37
38 foreach (Triangle t in triangles)
39 {
40 int iV1, iV2, iV3;
41 iV1 = this.vertices.IndexOf(t.v1);
42 iV2 = this.vertices.IndexOf(t.v2);
43 iV3 = this.vertices.IndexOf(t.v3);
44
45 Triangle newT = new Triangle(result.vertices[iV1], result.vertices[iV2], result.vertices[iV3]);
46 result.Add(newT);
47 }
48
49 return result;
50 }
51
52
53
54 public void Add(Triangle triangle)
55 {
56 int i;
57 i = vertices.IndexOf(triangle.v1);
58 if (i < 0)
59 throw new ArgumentException("Vertex v1 not known to mesh");
60 i = vertices.IndexOf(triangle.v2);
61 if (i < 0)
62 throw new ArgumentException("Vertex v2 not known to mesh");
63 i = vertices.IndexOf(triangle.v3);
64 if (i < 0)
65 throw new ArgumentException("Vertex v3 not known to mesh");
66
67 triangles.Add(triangle);
68 }
69
70 public void Add(Vertex v)
71 {
72 vertices.Add(v);
73 }
74
75 public void Remove(Vertex v)
76 {
77 int i;
78
79 // First, remove all triangles that are build on v
80 for (i = 0; i < triangles.Count; i++)
81 {
82 Triangle t = triangles[i];
83 if (t.v1 == v || t.v2 == v || t.v3 == v)
84 {
85 triangles.RemoveAt(i);
86 i--;
87 }
88 }
89
90 // Second remove v itself
91 vertices.Remove(v);
92 }
93
94 public void RemoveTrianglesOutside(SimpleHull hull)
95 {
96 int i;
97
98 for (i = 0; i < triangles.Count; i++)
99 {
100 Triangle t = triangles[i];
101 Vertex v1 = t.v1;
102 Vertex v2 = t.v2;
103 Vertex v3 = t.v3;
104 PhysicsVector m = v1 + v2 + v3;
105 m /= 3.0f;
106 if (!hull.IsPointIn(new Vertex(m)))
107 {
108 triangles.RemoveAt(i);
109 i--;
110 }
111 }
112 }
113
114
115 public void Add(List<Vertex> lv)
116 {
117 foreach (Vertex v in lv)
118 {
119 vertices.Add(v);
120 }
121 }
122
123 public List<PhysicsVector> getVertexList()
124 {
125 List<PhysicsVector> result = new List<PhysicsVector>();
126 foreach (Vertex v in vertices)
127 {
128 result.Add(v);
129 }
130 return result;
131 }
132
133 public float[] getVertexListAsFloatLocked()
134 {
135 float[] result = new float[vertices.Count * 3];
136 for (int i = 0; i < vertices.Count; i++)
137 {
138 Vertex v = vertices[i];
139 if (v == null)
140 continue;
141 result[3 * i + 0] = v.X;
142 result[3 * i + 1] = v.Y;
143 result[3 * i + 2] = v.Z;
144 }
145 GCHandle.Alloc(result, GCHandleType.Pinned);
146 return result;
147 }
148
149 public int[] getIndexListAsInt()
150 {
151 int[] result = new int[triangles.Count * 3];
152 for (int i = 0; i < triangles.Count; i++)
153 {
154 Triangle t = triangles[i];
155 result[3 * i + 0] = vertices.IndexOf(t.v1);
156 result[3 * i + 1] = vertices.IndexOf(t.v2);
157 result[3 * i + 2] = vertices.IndexOf(t.v3);
158 }
159 return result;
160 }
161
162 public int[] getIndexListAsIntLocked()
163 {
164 int[] result = getIndexListAsInt();
165 GCHandle.Alloc(result, GCHandleType.Pinned);
166 return result;
167 }
168
169
170 public void Append(Mesh newMesh)
171 {
172 foreach (Vertex v in newMesh.vertices)
173 vertices.Add(v);
174
175 foreach (Triangle t in newMesh.triangles)
176 Add(t);
177
178 }
179
180 // Do a linear transformation of mesh.
181 public void TransformLinear(float[,] matrix, float[] offset)
182 {
183 foreach (Vertex v in vertices)
184 {
185 if (v == null)
186 continue;
187 float x, y, z;
188 x = v.X * matrix[0, 0] + v.Y * matrix[1, 0] + v.Z * matrix[2, 0];
189 y = v.X * matrix[0, 1] + v.Y * matrix[1, 1] + v.Z * matrix[2, 1];
190 z = v.X * matrix[0, 2] + v.Y * matrix[1, 2] + v.Z * matrix[2, 2];
191 v.X = x + offset[0];
192 v.Y = y + offset[1];
193 v.Z = z + offset[2];
194 }
195 }
196
197 public void DumpRaw(String path, String name, String title)
198 {
199 if (path == null)
200 return;
201 String fileName = name + "_" + title + ".raw";
202 String completePath = Path.Combine(path, fileName);
203 StreamWriter sw = new StreamWriter(completePath);
204 foreach (Triangle t in triangles)
205 {
206 String s = t.ToStringRaw();
207 sw.WriteLine(s);
208 }
209 sw.Close();
210 }
211 }
212
213}
diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
new file mode 100644
index 0000000..da4ee58
--- /dev/null
+++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
@@ -0,0 +1,393 @@
1/*
2* Copyright (c) Contributors, http://opensimulator.org/
3* See CONTRIBUTORS.TXT for a full list of copyright holders.
4*
5* Redistribution and use in source and binary forms, with or without
6* modification, are permitted provided that the following conditions are met:
7* * Redistributions of source code must retain the above copyright
8* notice, this list of conditions and the following disclaimer.
9* * Redistributions in binary form must reproduce the above copyright
10* notice, this list of conditions and the following disclaimer in the
11* documentation and/or other materials provided with the distribution.
12* * Neither the name of the OpenSim Project nor the
13* names of its contributors may be used to endorse or promote products
14* derived from this software without specific prior written permission.
15*
16* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS AS IS AND ANY
17* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26*
27*/
28
29using System;
30using System.IO;
31using System.Globalization;
32using System.Diagnostics;
33using System.Collections.Generic;
34using System.Runtime.InteropServices;
35using OpenSim.Framework;
36using OpenSim.Framework.Console;
37using OpenSim.Region.Physics.Manager;
38
39namespace OpenSim.Region.Physics.Meshing
40{
41
42 public class MeshmerizerPlugin : IMeshingPlugin
43 {
44 public MeshmerizerPlugin()
45 {
46 }
47
48 public string GetName()
49 {
50 return "Meshmerizer";
51 }
52
53 public IMesher GetMesher()
54 {
55 return new Meshmerizer();
56 }
57 }
58
59 public class Meshmerizer : IMesher
60 {
61 // Setting baseDir to a path will enable the dumping of raw files
62 // raw files can be imported by blender so a visual inspection of the results can be done
63 // const string baseDir = "rawFiles";
64 const string baseDir = null;
65
66 static void IntersectionParameterPD(PhysicsVector p1, PhysicsVector r1, PhysicsVector p2, PhysicsVector r2, ref float lambda, ref float mu)
67 {
68 // p1, p2, points on the straight
69 // r1, r2, directional vectors of the straight. Not necessarily of length 1!
70 // note, that l, m can be scaled such, that the range 0..1 is mapped to the area between two points,
71 // thus allowing to decide whether an intersection is between two points
72
73 float r1x = r1.X;
74 float r1y = r1.Y;
75 float r2x = r2.X;
76 float r2y = r2.Y;
77
78 float denom = r1y*r2x - r1x*r2y;
79
80 if (denom == 0.0)
81 {
82 lambda = Single.NaN;
83 mu = Single.NaN;
84 return;
85 }
86
87 float p1x = p1.X;
88 float p1y = p1.Y;
89 float p2x = p2.X;
90 float p2y = p2.Y;
91 lambda = (-p2x * r2y + p1x * r2y + (p2y - p1y) * r2x) / denom;
92 mu = (-p2x * r1y + p1x * r1y + (p2y - p1y) * r1x) / denom;
93
94 }
95
96 private static List<Triangle> FindInfluencedTriangles(List<Triangle> triangles, Vertex v)
97 {
98 List<Triangle> influenced = new List<Triangle>();
99 foreach (Triangle t in triangles)
100 {
101 if (t.isInCircle(v.X, v.Y))
102 {
103 influenced.Add(t);
104 }
105 }
106 return influenced;
107 }
108
109
110 private static void InsertVertices(List<Vertex> vertices, int usedForSeed, List<Triangle> triangles)
111 {
112 // This is a variant of the delaunay algorithm
113 // each time a new vertex is inserted, all triangles that are influenced by it are deleted
114 // and replaced by new ones including the new vertex
115 // It is not very time efficient but easy to implement.
116
117 int iCurrentVertex;
118 int iMaxVertex = vertices.Count;
119 for (iCurrentVertex = usedForSeed; iCurrentVertex < iMaxVertex; iCurrentVertex++)
120 {
121 // Background: A triangle mesh fulfills the delaunay condition if (iff!)
122 // each circumlocutory circle (i.e. the circle that touches all three corners)
123 // of each triangle is empty of other vertices.
124 // Obviously a single (seeding) triangle fulfills this condition.
125 // If we now add one vertex, we need to reconstruct all triangles, that
126 // do not fulfill this condition with respect to the new triangle
127
128 // Find the triangles that are influenced by the new vertex
129 Vertex v=vertices[iCurrentVertex];
130 if (v == null)
131 continue; // Null is polygon stop marker. Ignore it
132 List<Triangle> influencedTriangles=FindInfluencedTriangles(triangles, v);
133
134 List<Simplex> simplices = new List<Simplex>();
135
136 // Reconstruction phase. First step, dissolve each triangle into it's simplices,
137 // i.e. it's "border lines"
138 // Goal is to find "inner" borders and delete them, while the hull gets conserved.
139 // Inner borders are special in the way that they always come twice, which is how we detect them
140 foreach (Triangle t in influencedTriangles)
141 {
142 List<Simplex> newSimplices = t.GetSimplices();
143 simplices.AddRange(newSimplices);
144 triangles.Remove(t);
145 }
146 // Now sort the simplices. That will make identical ones reside side by side in the list
147 simplices.Sort();
148
149 // Look for duplicate simplices here.
150 // Remember, they are directly side by side in the list right now,
151 // So we only check directly neighbours
152 int iSimplex;
153 List<Simplex> innerSimplices = new List<Simplex>();
154 for (iSimplex = 1; iSimplex < simplices.Count; iSimplex++) // Startindex=1, so we can refer backwards
155 {
156 if (simplices[iSimplex - 1].CompareTo(simplices[iSimplex]) == 0)
157 {
158 innerSimplices.Add(simplices[iSimplex - 1]);
159 innerSimplices.Add(simplices[iSimplex]);
160 }
161 }
162
163 foreach (Simplex s in innerSimplices)
164 {
165 simplices.Remove(s);
166 }
167
168 // each simplex still in the list belongs to the hull of the region in question
169 // The new vertex (yes, we still deal with verices here :-) ) forms a triangle
170 // with each of these simplices. Build the new triangles and add them to the list
171 foreach (Simplex s in simplices)
172 {
173 Triangle t = new Triangle(s.v1, s.v2, vertices[iCurrentVertex]);
174 if (!t.isDegraded())
175 {
176 triangles.Add(t);
177 }
178 }
179 }
180
181 }
182
183
184 static Mesh CreateBoxMesh(String primName, PrimitiveBaseShape primShape, PhysicsVector size)
185 // Builds the z (+ and -) surfaces of a box shaped prim
186 {
187 UInt16 hollowFactor = primShape.ProfileHollow;
188 UInt16 profileBegin = primShape.ProfileBegin;
189 UInt16 profileEnd = primShape.ProfileEnd;
190
191 // Procedure: This is based on the fact that the upper (plus) and lower (minus) Z-surface
192 // of a block are basically the same
193 // They may be warped differently but the shape is identical
194 // So we only create one surface as a model and derive both plus and minus surface of the block from it
195 // This is done in a model space where the block spans from -.5 to +.5 in X and Y
196 // The mapping to Scene space is done later during the "extrusion" phase
197
198 // Base
199 Vertex MM = new Vertex(-0.5f, -0.5f, 0.0f);
200 Vertex PM = new Vertex(+0.5f, -0.5f, 0.0f);
201 Vertex MP = new Vertex(-0.5f, +0.5f, 0.0f);
202 Vertex PP = new Vertex(+0.5f, +0.5f, 0.0f);
203
204 Meshing.SimpleHull outerHull = new SimpleHull();
205 outerHull.AddVertex(MM);
206 outerHull.AddVertex(PM);
207 outerHull.AddVertex(PP);
208 outerHull.AddVertex(MP);
209
210 // Deal with cuts now
211 if ((profileBegin != 0) || (profileEnd != 0))
212 {
213 double fProfileBeginAngle = profileBegin / 50000.0 * 360.0; // In degree, for easier debugging and understanding
214 fProfileBeginAngle -= (90.0 + 45.0); // for some reasons, the SL client counts from the corner -X/-Y
215 double fProfileEndAngle = 360.0 - profileEnd / 50000.0 * 360.0; // Pathend comes as complement to 1.0
216 fProfileEndAngle -= (90.0 + 45.0);
217 if (fProfileBeginAngle < fProfileEndAngle)
218 fProfileEndAngle -= 360.0;
219
220 // Note, that we don't want to cut out a triangle, even if this is a
221 // good approximation for small cuts. Indeed we want to cut out an arc
222 // and we approximate this arc by a polygon chain
223 // Also note, that these vectors are of length 1.0 and thus their endpoints lay outside the model space
224 // So it can easily be subtracted from the outer hull
225 int iSteps = (int)(((fProfileBeginAngle - fProfileEndAngle) / 45.0) + .5); // how many steps do we need with approximately 45 degree
226 double dStepWidth=(fProfileBeginAngle-fProfileEndAngle)/iSteps;
227
228 Vertex origin = new Vertex(0.0f, 0.0f, 0.0f);
229
230 // Note the sequence of vertices here. It's important to have the other rotational sense than in outerHull
231 SimpleHull cutHull = new SimpleHull();
232 cutHull.AddVertex(origin);
233 for (int i=0; i<iSteps; i++) {
234 double angle=fProfileBeginAngle-i*dStepWidth; // we count against the angle orientation!!!!
235 Vertex v = Vertex.FromAngle(angle * Math.PI / 180.0);
236 cutHull.AddVertex(v);
237 }
238 Vertex legEnd = Vertex.FromAngle(fProfileEndAngle * Math.PI / 180.0); // Calculated separately to avoid errors
239 cutHull.AddVertex(legEnd);
240
241 MainLog.Instance.Debug("Starting cutting of the hollow shape from the prim {1}", 0, primName);
242 SimpleHull cuttedHull = SimpleHull.SubtractHull(outerHull, cutHull);
243
244 outerHull = cuttedHull;
245 }
246
247 // Deal with the hole here
248 if (hollowFactor > 0)
249 {
250 float hollowFactorF = (float) hollowFactor/(float) 50000;
251 Vertex IMM = new Vertex(-0.5f * hollowFactorF, -0.5f * hollowFactorF, 0.0f);
252 Vertex IPM = new Vertex(+0.5f * hollowFactorF, -0.5f * hollowFactorF, 0.0f);
253 Vertex IMP = new Vertex(-0.5f * hollowFactorF, +0.5f * hollowFactorF, 0.0f);
254 Vertex IPP = new Vertex(+0.5f * hollowFactorF, +0.5f * hollowFactorF, 0.0f);
255
256 SimpleHull holeHull = new SimpleHull();
257
258 holeHull.AddVertex(IMM);
259 holeHull.AddVertex(IMP);
260 holeHull.AddVertex(IPP);
261 holeHull.AddVertex(IPM);
262
263 SimpleHull hollowedHull = SimpleHull.SubtractHull(outerHull, holeHull);
264
265 outerHull = hollowedHull;
266
267 }
268
269 Mesh m = new Mesh();
270
271 Vertex Seed1 = new Vertex(0.0f, -10.0f, 0.0f);
272 Vertex Seed2 = new Vertex(-10.0f, 10.0f, 0.0f);
273 Vertex Seed3 = new Vertex(10.0f, 10.0f, 0.0f);
274
275 m.Add(Seed1);
276 m.Add(Seed2);
277 m.Add(Seed3);
278
279 m.Add(new Triangle(Seed1, Seed2, Seed3));
280 m.Add(outerHull.getVertices());
281
282 InsertVertices(m.vertices, 3, m.triangles);
283 m.DumpRaw(baseDir, primName, "Proto first Mesh");
284
285 m.Remove(Seed1);
286 m.Remove(Seed2);
287 m.Remove(Seed3);
288 m.DumpRaw(baseDir, primName, "Proto seeds removed");
289
290 m.RemoveTrianglesOutside(outerHull);
291 m.DumpRaw(baseDir, primName, "Proto outsides removed");
292
293 foreach (Triangle t in m.triangles)
294 {
295 PhysicsVector n = t.getNormal();
296 if (n.Z < 0.0)
297 t.invertNormal();
298 }
299
300 Extruder extr = new Extruder();
301
302 extr.size = size;
303
304 Mesh result = extr.Extrude(m);
305 result.DumpRaw(baseDir, primName, "Z extruded");
306 return result;
307 }
308
309 public static void CalcNormals(Mesh mesh)
310 {
311 int iTriangles = mesh.triangles.Count;
312
313 mesh.normals = new float[iTriangles*3];
314
315 int i = 0;
316 foreach (Triangle t in mesh.triangles)
317 {
318 float ux, uy, uz;
319 float vx, vy, vz;
320 float wx, wy, wz;
321
322 ux = t.v1.X;
323 uy = t.v1.Y;
324 uz = t.v1.Z;
325
326 vx = t.v2.X;
327 vy = t.v2.Y;
328 vz = t.v2.Z;
329
330 wx = t.v3.X;
331 wy = t.v3.Y;
332 wz = t.v3.Z;
333
334
335 // Vectors for edges
336 float e1x, e1y, e1z;
337 float e2x, e2y, e2z;
338
339 e1x = ux - vx;
340 e1y = uy - vy;
341 e1z = uz - vz;
342
343 e2x = ux - wx;
344 e2y = uy - wy;
345 e2z = uz - wz;
346
347
348 // Cross product for normal
349 float nx, ny, nz;
350 nx = e1y*e2z - e1z*e2y;
351 ny = e1z*e2x - e1x*e2z;
352 nz = e1x*e2y - e1y*e2x;
353
354 // Length
355 float l = (float) Math.Sqrt(nx*nx + ny*ny + nz*nz);
356
357 // Normalized "normal"
358 nx /= l;
359 ny /= l;
360 nz /= l;
361
362 mesh.normals[i] = nx;
363 mesh.normals[i + 1] = ny;
364 mesh.normals[i + 2] = nz;
365
366 i += 3;
367 }
368 }
369
370 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, PhysicsVector size)
371 {
372 Mesh mesh = null;
373
374 switch (primShape.ProfileShape)
375 {
376 case ProfileShape.Square:
377 mesh=CreateBoxMesh(primName, primShape, size);
378 CalcNormals(mesh);
379 break;
380 default:
381 mesh = CreateBoxMesh(primName, primShape, size);
382 CalcNormals(mesh);
383 //Set default mesh to cube otherwise it'll return
384 // null and crash on the 'setMesh' method in the physics plugins.
385 //mesh = null;
386 break;
387 }
388
389 return mesh;
390 }
391 }
392
393}
diff --git a/OpenSim/Region/Physics/Meshing/SimpleHull.cs b/OpenSim/Region/Physics/Meshing/SimpleHull.cs
new file mode 100644
index 0000000..c199949
--- /dev/null
+++ b/OpenSim/Region/Physics/Meshing/SimpleHull.cs
@@ -0,0 +1,363 @@
1using System;
2using System.Collections.Generic;
3using System.Text;
4
5using OpenSim.Framework.Console;
6
7namespace OpenSim.Region.Physics.Meshing
8{
9 // A simple hull is a set of vertices building up to simplices that border a region
10 // The word simple referes to the fact, that this class assumes, that all simplices
11 // do not intersect
12 // Simple hulls can be added and subtracted.
13 // Vertices can be checked to lie inside a hull
14 // Also note, that the sequence of the vertices is important and defines if the region that
15 // is defined by the hull lies inside or outside the simplex chain
16 public class SimpleHull
17 {
18 List<Vertex> vertices = new List<Vertex>();
19 List<Vertex> holeVertices = new List<Vertex>(); // Only used, when the hull is hollow
20
21 // Adds a vertex to the end of the list
22 public void AddVertex(Vertex v) {
23 vertices.Add(v);
24 }
25
26 override public String ToString()
27 {
28 String result="";
29 foreach (Vertex v in vertices)
30 {
31 result += "b:" + v.ToString() + "\n";
32 }
33
34 return result;
35 }
36
37
38 public List<Vertex> getVertices() {
39 List<Vertex> newVertices = new List<Vertex>();
40
41 newVertices.AddRange(vertices);
42 newVertices.Add(null);
43 newVertices.AddRange(holeVertices);
44
45 return newVertices;
46 }
47
48 public SimpleHull Clone()
49 {
50 SimpleHull result = new SimpleHull();
51 foreach (Vertex v in vertices)
52 {
53 result.AddVertex(v.Clone());
54 }
55
56 foreach (Vertex v in this.holeVertices)
57 {
58 result.holeVertices.Add(v.Clone());
59 }
60
61 return result;
62 }
63
64 public bool IsPointIn(Vertex v1)
65 {
66 int iCounter=0;
67 List<Simplex> simplices=buildSimplexList();
68 foreach (Simplex s in simplices)
69 {
70 // Send a ray along the positive X-Direction
71 // Note, that this direction must correlate with the "below" interpretation
72 // of handling for the special cases below
73 Manager.PhysicsVector intersection = s.RayIntersect(v1, new Manager.PhysicsVector(1.0f, 0.0f, 0.0f), true);
74
75 if (intersection == null)
76 continue; // No intersection. Done. More tests to follow otherwise
77
78 // Did we hit the end of a simplex?
79 // Then this can be one of two special cases:
80 // 1. we go through a border exactly at a joint
81 // 2. we have just marginally touched a corner
82 // 3. we can slide along a border
83 // Solution: If the other vertex is "below" the ray, we don't count it
84 // Thus corners pointing down are counted twice, corners pointing up are not counted
85 // borders are counted once
86 if (intersection.IsIdentical(s.v1, 0.001f)) {
87 if (s.v2.Y < v1.Y)
88 continue;
89 }
90 // Do this for the other vertex two
91 if (intersection.IsIdentical(s.v2, 0.001f)) {
92 if (s.v1.Y<v1.Y)
93 continue;
94 }
95 iCounter++;
96 }
97
98 return iCounter % 2 == 1; // Point is inside if the number of intersections is odd
99 }
100
101 public bool containsPointsFrom(SimpleHull otherHull)
102 {
103 foreach (Vertex v in otherHull.vertices)
104 {
105 if (IsPointIn(v))
106 return true;
107 }
108
109 return false;
110 }
111
112
113 List<Simplex> buildSimplexList() {
114
115 List<Simplex> result = new List<Simplex>();
116
117 // Not asserted but assumed: at least three vertices
118 for (int i=0; i<vertices.Count-1; i++) {
119 Simplex s=new Simplex(vertices[i], vertices[i+1]);
120 result.Add(s);
121 }
122 Simplex s1=new Simplex(vertices[vertices.Count-1], vertices[0]);
123 result.Add(s1);
124
125 if (holeVertices.Count==0)
126 return result;
127
128 // Same here. At least three vertices in hole assumed
129 for (int i = 0; i < holeVertices.Count - 1; i++)
130 {
131 Simplex s = new Simplex(holeVertices[i], holeVertices[i + 1]);
132 result.Add(s);
133 }
134
135 s1 = new Simplex(holeVertices[holeVertices.Count - 1], holeVertices[0]);
136 result.Add(s1);
137 return result;
138 }
139
140 bool InsertVertex(Vertex v, int iAfter)
141 {
142 vertices.Insert(iAfter + 1, v);
143 return true;
144 }
145
146 Vertex getNextVertex(Vertex currentVertex)
147 {
148 int iCurrentIndex;
149 iCurrentIndex = vertices.IndexOf(currentVertex);
150
151 // Error handling for iCurrentIndex==-1 should go here (and probably never will)
152
153 iCurrentIndex++;
154 if (iCurrentIndex == vertices.Count)
155 iCurrentIndex = 0;
156
157 return vertices[iCurrentIndex];
158 }
159
160 public Vertex FindVertex(Vertex vBase, float tolerance) {
161 foreach (Vertex v in vertices) {
162 if (v.IsIdentical(vBase, tolerance))
163 return v;
164 }
165
166 return null;
167 }
168
169 public void FindIntersection(Simplex s, ref Vertex Intersection, ref Vertex nextVertex)
170 {
171 Vertex bestIntersection=null;
172 float distToV1=Single.PositiveInfinity;
173 Simplex bestIntersectingSimplex=null;
174
175 List<Simplex> simple = buildSimplexList();
176 foreach (Simplex sTest in simple)
177 {
178 Manager.PhysicsVector vvTemp = Simplex.Intersect(sTest, s, -.001f, -.001f, 0.999f, .999f);
179
180 Vertex vTemp=null;
181 if (vvTemp != null)
182 vTemp = new Vertex(vvTemp);
183
184 if (vTemp!=null) {
185
186 Manager.PhysicsVector diff=(s.v1-vTemp);
187 float distTemp=diff.length();
188
189 if (bestIntersection==null || distTemp<distToV1) {
190 bestIntersection=vTemp;
191 distToV1=distTemp;
192 bestIntersectingSimplex = sTest;
193 }
194
195 } // end if vTemp
196
197 } // end foreach
198
199 Intersection = bestIntersection;
200 if (bestIntersectingSimplex != null)
201 nextVertex = bestIntersectingSimplex.v2;
202 else
203 nextVertex = null;
204 }
205
206
207 public static SimpleHull SubtractHull(SimpleHull baseHull, SimpleHull otherHull)
208 {
209
210 SimpleHull baseHullClone = baseHull.Clone();
211 SimpleHull otherHullClone = otherHull.Clone();
212 bool intersects = false;
213
214 MainLog.Instance.Debug("State before intersection detection");
215 MainLog.Instance.Debug("The baseHull is:\n{1}", 0, baseHullClone.ToString());
216 MainLog.Instance.Debug("The otherHull is:\n{1}", 0, otherHullClone.ToString());
217
218 {
219 int iBase, iOther;
220
221 // Insert into baseHull
222 for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
223 {
224 int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
225 Simplex sBase = new Simplex(baseHullClone.vertices[iBase], baseHullClone.vertices[iBaseNext]);
226
227 for (iOther = 0; iOther < otherHullClone.vertices.Count; iOther++)
228 {
229 int iOtherNext = (iOther + 1) % otherHullClone.vertices.Count;
230 Simplex sOther = new Simplex(otherHullClone.vertices[iOther], otherHullClone.vertices[iOtherNext]);
231
232 Manager.PhysicsVector intersect = Simplex.Intersect(sBase, sOther, 0.001f, -.001f, 0.999f, 1.001f);
233 if (intersect != null)
234 {
235 Vertex vIntersect = new Vertex(intersect);
236 baseHullClone.vertices.Insert(iBase + 1, vIntersect);
237 sBase.v2 = vIntersect;
238 intersects = true;
239 }
240 }
241 }
242 }
243
244 MainLog.Instance.Debug("State after intersection detection for the base hull");
245 MainLog.Instance.Debug("The baseHull is:\n{1}", 0, baseHullClone.ToString());
246
247 {
248 int iOther, iBase;
249
250 // Insert into otherHull
251 for (iOther = 0; iOther < otherHullClone.vertices.Count; iOther++)
252 {
253 int iOtherNext = (iOther + 1) % otherHullClone.vertices.Count;
254 Simplex sOther = new Simplex(otherHullClone.vertices[iOther], otherHullClone.vertices[iOtherNext]);
255
256 for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
257 {
258 int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
259 Simplex sBase = new Simplex(baseHullClone.vertices[iBase], baseHullClone.vertices[iBaseNext]);
260
261 Manager.PhysicsVector intersect = Simplex.Intersect(sBase, sOther, -.001f, 0.001f, 1.001f, 0.999f);
262 if (intersect != null)
263 {
264 Vertex vIntersect = new Vertex(intersect);
265 otherHullClone.vertices.Insert(iOther + 1, vIntersect);
266 sOther.v2 = vIntersect;
267 intersects = true;
268 }
269 }
270 }
271 }
272
273 MainLog.Instance.Debug("State after intersection detection for the base hull");
274 MainLog.Instance.Debug("The otherHull is:\n{1}", 0, otherHullClone.ToString());
275
276
277 bool otherIsInBase = baseHullClone.containsPointsFrom(otherHullClone);
278 if (!intersects && otherIsInBase)
279 {
280 // We have a hole here
281 baseHullClone.holeVertices = otherHullClone.vertices;
282 return baseHullClone;
283 }
284
285
286 SimpleHull result = new SimpleHull();
287
288 // Find a good starting Simplex from baseHull
289 // A good starting simplex is one that is outside otherHull
290 // Such a simplex must exist, otherwise the result will be empty
291 Vertex baseStartVertex = null;
292 {
293 int iBase;
294 for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
295 {
296 int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
297 Vertex center = new Vertex((baseHullClone.vertices[iBase] + baseHullClone.vertices[iBaseNext]) / 2.0f);
298 bool isOutside = !otherHullClone.IsPointIn(center);
299 if (isOutside)
300 {
301 baseStartVertex = baseHullClone.vertices[iBaseNext];
302 break;
303 }
304 }
305 }
306
307
308 if (baseStartVertex == null) // i.e. no simplex fulfilled the "outside" condition.
309 // In otherwords, subtractHull completely embraces baseHull
310 {
311 return result;
312 }
313
314 // The simplex that *starts* with baseStartVertex is outside the cutting hull,
315 // so we can start our walk with the next vertex without loosing a branch
316 Vertex V1 = baseStartVertex;
317 bool onBase = true;
318
319 // And here is how we do the magic :-)
320 // Start on the base hull.
321 // Walk the vertices in the positive direction
322 // For each vertex check, whether it is a vertex shared with the other hull
323 // if this is the case, switch over to walking the other vertex list.
324 // Note: The other hull *must* go backwards to our starting point (via several orther vertices)
325 // Thus it is important that the cutting hull has the inverse directional sense than the
326 // base hull!!!!!!!!! (means if base goes CW around it's center cutting hull must go CCW)
327
328 bool done = false;
329 while (!done)
330 {
331 result.AddVertex(V1);
332 Vertex nextVertex = null;
333 if (onBase)
334 {
335 nextVertex = otherHullClone.FindVertex(V1, 0.001f);
336 }
337 else
338 {
339 nextVertex = baseHullClone.FindVertex(V1, 0.001f);
340 }
341
342 if (nextVertex != null) // A node that represents an intersection
343 {
344 V1 = nextVertex; // Needed to find the next vertex on the other hull
345 onBase = !onBase;
346 }
347
348 if (onBase)
349 V1 = baseHullClone.getNextVertex(V1);
350 else
351 V1 = otherHullClone.getNextVertex(V1);
352
353 if (V1 == baseStartVertex)
354 done = true;
355 }
356
357 MainLog.Instance.Debug("The resulting Hull is:\n{1}", 0, result.ToString());
358
359 return result;
360
361 }
362 }
363}
diff --git a/OpenSim/Region/Physics/Meshing/Simplex.cs b/OpenSim/Region/Physics/Meshing/Simplex.cs
new file mode 100644
index 0000000..4944f22
--- /dev/null
+++ b/OpenSim/Region/Physics/Meshing/Simplex.cs
@@ -0,0 +1,198 @@
1using System;
2using System.Collections.Generic;
3using System.Text;
4using OpenSim.Region.Physics.Manager;
5
6namespace OpenSim.Region.Physics.Meshing
7{
8 // A simplex is a section of a straight line.
9 // It is defined by its endpoints, i.e. by two vertices
10 // Operation on vertices are
11 public class Simplex : IComparable<Simplex>
12 {
13 public Vertex v1;
14 public Vertex v2;
15
16 public Simplex(Vertex _v1, Vertex _v2)
17 {
18 v1 = _v1;
19 v2 = _v2;
20 }
21
22 public int CompareTo(Simplex other)
23 {
24
25 Vertex lv1, lv2, ov1, ov2, temp;
26
27 lv1 = v1;
28 lv2 = v2;
29 ov1 = other.v1;
30 ov2 = other.v2;
31
32 if (lv1 > lv2)
33 {
34 temp = lv1;
35 lv1 = lv2;
36 lv2 = temp;
37 }
38
39 if (ov1 > ov2)
40 {
41 temp = ov1;
42 ov1 = ov2;
43 ov2 = temp;
44 }
45
46 if (lv1 > ov1)
47 {
48 return 1;
49 }
50 if (lv1 < ov1)
51 {
52 return -1;
53 }
54
55 if (lv2 > ov2)
56 {
57 return 1;
58 }
59 if (lv2 < ov2)
60 {
61 return -1;
62 }
63
64 return 0;
65 }
66
67 private static void intersectParameter(PhysicsVector p1, PhysicsVector r1, PhysicsVector p2, PhysicsVector r2, ref float lambda, ref float mu)
68 {
69 // Intersects two straights
70 // p1, p2, points on the straight
71 // r1, r2, directional vectors of the straight. Not necessarily of length 1!
72 // note, that l, m can be scaled such, that the range 0..1 is mapped to the area between two points,
73 // thus allowing to decide whether an intersection is between two points
74
75 float r1x = r1.X;
76 float r1y = r1.Y;
77 float r2x = r2.X;
78 float r2y = r2.Y;
79
80 float denom = r1y*r2x - r1x*r2y;
81
82 float p1x = p1.X;
83 float p1y = p1.Y;
84 float p2x = p2.X;
85 float p2y = p2.Y;
86
87 float z1=-p2x * r2y + p1x * r2y + (p2y - p1y) * r2x;
88 float z2=-p2x * r1y + p1x * r1y + (p2y - p1y) * r1x;
89
90 if (denom == 0.0f) // Means the straights are parallel. Either no intersection or an infinite number of them
91 {
92 if (z1==0.0f) {// Means they are identical -> many, many intersections
93 lambda = Single.NaN;
94 mu = Single.NaN;
95 } else {
96 lambda = Single.PositiveInfinity;
97 mu = Single.PositiveInfinity;
98 }
99 return;
100
101 }
102
103
104
105 lambda = z1 / denom;
106 mu = z2 / denom;
107
108 }
109
110
111 // Intersects the simplex with another one.
112 // the borders are used to deal with float inaccuracies
113 // As a rule of thumb, the borders are
114 // lowerBorder1 : 0.0
115 // lowerBorder2 : 0.0
116 // upperBorder1 : 1.0
117 // upperBorder2 : 1.0
118 // 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)
119 public static PhysicsVector Intersect(
120 Simplex s1,
121 Simplex s2,
122 float lowerBorder1,
123 float lowerBorder2,
124 float upperBorder1,
125 float upperBorder2)
126 {
127 PhysicsVector firstSimplexDirection = s1.v2 - s1.v1;
128 PhysicsVector secondSimplexDirection = s2.v2 - s2.v1;
129
130 float lambda = 0.0f;
131 float mu = 0.0f;
132
133 // Give us the parameters of an intersection. This subroutine does *not* take the constraints
134 // (intersection must be between v1 and v2 and it must be in the positive direction of the ray)
135 // into account. We do that afterwards.
136 intersectParameter(s1.v1, firstSimplexDirection, s2.v1, secondSimplexDirection, ref lambda, ref mu);
137
138 if (Single.IsInfinity(lambda)) // Special case. No intersection at all. directions parallel.
139 return null;
140
141 if (Single.IsNaN(lambda)) // Special case. many, many intersections.
142 return null;
143
144 if (lambda > upperBorder1) // We're behind v2
145 return null;
146
147 if (lambda < lowerBorder1)
148 return null;
149
150 if (mu < lowerBorder2) // outside simplex 2
151 return null;
152
153 if (mu > upperBorder2) // outside simplex 2
154 return null;
155
156 return s1.v1 + lambda * firstSimplexDirection;
157
158 }
159
160 // Intersects the simplex with a ray. The ray is defined as all p=origin + lambda*direction
161 // where lambda >= 0
162 public PhysicsVector RayIntersect(Vertex origin, PhysicsVector direction, bool bEndsIncluded)
163 {
164 PhysicsVector simplexDirection = v2 - v1;
165
166 float lambda = 0.0f;
167 float mu = 0.0f;
168
169 // Give us the parameters of an intersection. This subroutine does *not* take the constraints
170 // (intersection must be between v1 and v2 and it must be in the positive direction of the ray)
171 // into account. We do that afterwards.
172 intersectParameter(v1, simplexDirection, origin, direction, ref lambda, ref mu);
173
174 if (Single.IsInfinity(lambda)) // Special case. No intersection at all. directions parallel.
175 return null;
176
177 if (Single.IsNaN(lambda)) // Special case. many, many intersections.
178 return null;
179
180 if (mu < 0.0) // We're on the wrong side of the ray
181 return null;
182
183 if (lambda > 1.0) // We're behind v2
184 return null;
185
186 if (lambda == 1.0 && !bEndsIncluded)
187 return null; // The end of the simplices are not included
188
189 if (lambda < 0.0f) // we're before v1;
190 return null;
191
192 return this.v1 + lambda * simplexDirection;
193
194 }
195
196
197 }
198} \ No newline at end of file