aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/Meshing/Simplex.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/Meshing/Simplex.cs198
1 files changed, 198 insertions, 0 deletions
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