aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin
diff options
context:
space:
mode:
authordan miller2007-11-05 18:05:21 +0000
committerdan miller2007-11-05 18:05:21 +0000
commitc1d05740e5f36bea303db7c810a81382663b7e13 (patch)
tree4894fee91c3d8069866f3878dbf75e0171fd6b48 /OpenSim/Region/Physics/OdePlugin
parent* Reverted local xsd copy (diff)
downloadopensim-SC-c1d05740e5f36bea303db7c810a81382663b7e13.zip
opensim-SC-c1d05740e5f36bea303db7c810a81382663b7e13.tar.gz
opensim-SC-c1d05740e5f36bea303db7c810a81382663b7e13.tar.bz2
opensim-SC-c1d05740e5f36bea303db7c810a81382663b7e13.tar.xz
adding missing ODE files for mesh
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin')
-rwxr-xr-xOpenSim/Region/Physics/OdePlugin/Meshing/Extruder.cs83
-rwxr-xr-xOpenSim/Region/Physics/OdePlugin/Meshing/Mesh.cs197
-rwxr-xr-xOpenSim/Region/Physics/OdePlugin/Meshing/SimpleHull.cs363
-rwxr-xr-xOpenSim/Region/Physics/OdePlugin/Meshing/Simplex.cs198
4 files changed, 841 insertions, 0 deletions
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 @@
1using System;
2using System.Collections.Generic;
3using System.Text;
4
5namespace OpenSim.Region.Physics.OdePlugin.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/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 @@
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.OdePlugin.Meshing
12{
13 public class Mesh
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 float[] getVertexListAsFloat()
124 {
125 float[] result = new float[vertices.Count * 3];
126 for (int i = 0; i < vertices.Count; i++)
127 {
128 Vertex v = vertices[i];
129 if (v == null)
130 continue;
131 result[3 * i + 0] = v.X;
132 result[3 * i + 1] = v.Y;
133 result[3 * i + 2] = v.Z;
134 }
135 GCHandle.Alloc(result, GCHandleType.Pinned);
136 return result;
137 }
138
139 public int[] getIndexListAsInt()
140 {
141 int[] result = new int[triangles.Count * 3];
142 for (int i = 0; i < triangles.Count; i++)
143 {
144 Triangle t = triangles[i];
145 result[3 * i + 0] = vertices.IndexOf(t.v1);
146 result[3 * i + 1] = vertices.IndexOf(t.v2);
147 result[3 * i + 2] = vertices.IndexOf(t.v3);
148 }
149 GCHandle.Alloc(result, GCHandleType.Pinned);
150 return result;
151 }
152
153
154 public void Append(Mesh newMesh)
155 {
156 foreach (Vertex v in newMesh.vertices)
157 vertices.Add(v);
158
159 foreach (Triangle t in newMesh.triangles)
160 Add(t);
161
162 }
163
164 // Do a linear transformation of mesh.
165 public void TransformLinear(float[,] matrix, float[] offset)
166 {
167 foreach (Vertex v in vertices)
168 {
169 if (v == null)
170 continue;
171 float x, y, z;
172 x = v.X * matrix[0, 0] + v.Y * matrix[1, 0] + v.Z * matrix[2, 0];
173 y = v.X * matrix[0, 1] + v.Y * matrix[1, 1] + v.Z * matrix[2, 1];
174 z = v.X * matrix[0, 2] + v.Y * matrix[1, 2] + v.Z * matrix[2, 2];
175 v.X = x + offset[0];
176 v.Y = y + offset[1];
177 v.Z = z + offset[2];
178 }
179 }
180
181 public void DumpRaw(String path, String name, String title)
182 {
183 if (path == null)
184 return;
185 String fileName = name + "_" + title + ".raw";
186 String completePath = Path.Combine(path, fileName);
187 StreamWriter sw = new StreamWriter(completePath);
188 foreach (Triangle t in triangles)
189 {
190 String s = t.ToStringRaw();
191 sw.WriteLine(s);
192 }
193 sw.Close();
194 }
195 }
196
197}
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 @@
1using System;
2using System.Collections.Generic;
3using System.Text;
4
5using OpenSim.Framework.Console;
6
7namespace OpenSim.Region.Physics.OdePlugin.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/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 @@
1using System;
2using System.Collections.Generic;
3using System.Text;
4using OpenSim.Region.Physics.Manager;
5
6namespace OpenSim.Region.Physics.OdePlugin.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}
generated by cgit v1.1 at 2024-12-17 11:46:10 +0000