aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ConvexDecompositionDotNet
diff options
context:
space:
mode:
authorRobert Adams2011-06-20 17:14:59 -0700
committerDan Lake2011-06-20 17:24:42 -0700
commit302d72701da35b6d481a538c4cb953976d7e9044 (patch)
treefe11403b00e640da659c48dddbc9646860380ccd /OpenSim/Region/Physics/ConvexDecompositionDotNet
parentMerge branch 'master' of ssh://opensimulator.org/var/git/opensim (diff)
downloadopensim-SC_OLD-302d72701da35b6d481a538c4cb953976d7e9044.zip
opensim-SC_OLD-302d72701da35b6d481a538c4cb953976d7e9044.tar.gz
opensim-SC_OLD-302d72701da35b6d481a538c4cb953976d7e9044.tar.bz2
opensim-SC_OLD-302d72701da35b6d481a538c4cb953976d7e9044.tar.xz
BulletSim initial checkin
Diffstat (limited to 'OpenSim/Region/Physics/ConvexDecompositionDotNet')
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs341
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs233
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs411
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs200
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs74
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs171
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs99
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs1868
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt28
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs99
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs211
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs36
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs209
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt7
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs265
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs70
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs70
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs444
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs195
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs170
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs284
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs128
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs66
23 files changed, 5679 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs
new file mode 100644
index 0000000..4d84c44
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs
@@ -0,0 +1,341 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30
31namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
32{
33 public class Wpoint
34 {
35 public float3 mPoint;
36 public float mWeight;
37
38 public Wpoint(float3 p, float w)
39 {
40 mPoint = p;
41 mWeight = w;
42 }
43 }
44
45 public class CTri
46 {
47 private const int WSCALE = 4;
48
49 public float3 mP1;
50 public float3 mP2;
51 public float3 mP3;
52 public float3 mNear1;
53 public float3 mNear2;
54 public float3 mNear3;
55 public float3 mNormal;
56 public float mPlaneD;
57 public float mConcavity;
58 public float mC1;
59 public float mC2;
60 public float mC3;
61 public int mI1;
62 public int mI2;
63 public int mI3;
64 public int mProcessed; // already been added...
65
66 public CTri(float3 p1, float3 p2, float3 p3, int i1, int i2, int i3)
67 {
68 mProcessed = 0;
69 mI1 = i1;
70 mI2 = i2;
71 mI3 = i3;
72
73 mP1 = new float3(p1);
74 mP2 = new float3(p2);
75 mP3 = new float3(p3);
76
77 mNear1 = new float3();
78 mNear2 = new float3();
79 mNear3 = new float3();
80
81 mNormal = new float3();
82 mPlaneD = mNormal.ComputePlane(mP1, mP2, mP3);
83 }
84
85 public float Facing(CTri t)
86 {
87 return float3.dot(mNormal, t.mNormal);
88 }
89
90 public bool clip(float3 start, ref float3 end)
91 {
92 float3 sect = new float3();
93 bool hit = lineIntersectsTriangle(start, end, mP1, mP2, mP3, ref sect);
94
95 if (hit)
96 end = sect;
97 return hit;
98 }
99
100 public bool Concave(float3 p, ref float distance, ref float3 n)
101 {
102 n.NearestPointInTriangle(p, mP1, mP2, mP3);
103 distance = p.Distance(n);
104 return true;
105 }
106
107 public void addTri(int[] indices, int i1, int i2, int i3, ref int tcount)
108 {
109 indices[tcount * 3 + 0] = i1;
110 indices[tcount * 3 + 1] = i2;
111 indices[tcount * 3 + 2] = i3;
112 tcount++;
113 }
114
115 public float getVolume()
116 {
117 int[] indices = new int[8 * 3];
118
119 int tcount = 0;
120
121 addTri(indices, 0, 1, 2, ref tcount);
122 addTri(indices, 3, 4, 5, ref tcount);
123
124 addTri(indices, 0, 3, 4, ref tcount);
125 addTri(indices, 0, 4, 1, ref tcount);
126
127 addTri(indices, 1, 4, 5, ref tcount);
128 addTri(indices, 1, 5, 2, ref tcount);
129
130 addTri(indices, 0, 3, 5, ref tcount);
131 addTri(indices, 0, 5, 2, ref tcount);
132
133 List<float3> vertices = new List<float3> { mP1, mP2, mP3, mNear1, mNear2, mNear3 };
134 List<int> indexList = new List<int>(indices);
135
136 float v = Concavity.computeMeshVolume(vertices, indexList);
137 return v;
138 }
139
140 public float raySect(float3 p, float3 dir, ref float3 sect)
141 {
142 float4 plane = new float4();
143
144 plane.x = mNormal.x;
145 plane.y = mNormal.y;
146 plane.z = mNormal.z;
147 plane.w = mPlaneD;
148
149 float3 dest = p + dir * 100000f;
150
151 intersect(p, dest, ref sect, plane);
152
153 return sect.Distance(p); // return the intersection distance
154 }
155
156 public float planeDistance(float3 p)
157 {
158 float4 plane = new float4();
159
160 plane.x = mNormal.x;
161 plane.y = mNormal.y;
162 plane.z = mNormal.z;
163 plane.w = mPlaneD;
164
165 return DistToPt(p, plane);
166 }
167
168 public bool samePlane(CTri t)
169 {
170 const float THRESH = 0.001f;
171 float dd = Math.Abs(t.mPlaneD - mPlaneD);
172 if (dd > THRESH)
173 return false;
174 dd = Math.Abs(t.mNormal.x - mNormal.x);
175 if (dd > THRESH)
176 return false;
177 dd = Math.Abs(t.mNormal.y - mNormal.y);
178 if (dd > THRESH)
179 return false;
180 dd = Math.Abs(t.mNormal.z - mNormal.z);
181 if (dd > THRESH)
182 return false;
183 return true;
184 }
185
186 public bool hasIndex(int i)
187 {
188 if (i == mI1 || i == mI2 || i == mI3)
189 return true;
190 return false;
191 }
192
193 public bool sharesEdge(CTri t)
194 {
195 bool ret = false;
196 uint count = 0;
197
198 if (t.hasIndex(mI1))
199 count++;
200 if (t.hasIndex(mI2))
201 count++;
202 if (t.hasIndex(mI3))
203 count++;
204
205 if (count >= 2)
206 ret = true;
207
208 return ret;
209 }
210
211 public float area()
212 {
213 float a = mConcavity * mP1.Area(mP2, mP3);
214 return a;
215 }
216
217 public void addWeighted(List<Wpoint> list)
218 {
219 Wpoint p1 = new Wpoint(mP1, mC1);
220 Wpoint p2 = new Wpoint(mP2, mC2);
221 Wpoint p3 = new Wpoint(mP3, mC3);
222
223 float3 d1 = mNear1 - mP1;
224 float3 d2 = mNear2 - mP2;
225 float3 d3 = mNear3 - mP3;
226
227 d1 *= WSCALE;
228 d2 *= WSCALE;
229 d3 *= WSCALE;
230
231 d1 = d1 + mP1;
232 d2 = d2 + mP2;
233 d3 = d3 + mP3;
234
235 Wpoint p4 = new Wpoint(d1, mC1);
236 Wpoint p5 = new Wpoint(d2, mC2);
237 Wpoint p6 = new Wpoint(d3, mC3);
238
239 list.Add(p1);
240 list.Add(p2);
241 list.Add(p3);
242
243 list.Add(p4);
244 list.Add(p5);
245 list.Add(p6);
246 }
247
248 private static float DistToPt(float3 p, float4 plane)
249 {
250 float x = p.x;
251 float y = p.y;
252 float z = p.z;
253 float d = x*plane.x + y*plane.y + z*plane.z + plane.w;
254 return d;
255 }
256
257 private static void intersect(float3 p1, float3 p2, ref float3 split, float4 plane)
258 {
259 float dp1 = DistToPt(p1, plane);
260
261 float3 dir = new float3();
262 dir.x = p2[0] - p1[0];
263 dir.y = p2[1] - p1[1];
264 dir.z = p2[2] - p1[2];
265
266 float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2];
267 float dot2 = dp1 - plane[3];
268
269 float t = -(plane[3] + dot2) / dot1;
270
271 split.x = (dir[0] * t) + p1[0];
272 split.y = (dir[1] * t) + p1[1];
273 split.z = (dir[2] * t) + p1[2];
274 }
275
276 private static bool rayIntersectsTriangle(float3 p, float3 d, float3 v0, float3 v1, float3 v2, out float t)
277 {
278 t = 0f;
279
280 float3 e1, e2, h, s, q;
281 float a, f, u, v;
282
283 e1 = v1 - v0;
284 e2 = v2 - v0;
285 h = float3.cross(d, e2);
286 a = float3.dot(e1, h);
287
288 if (a > -0.00001f && a < 0.00001f)
289 return false;
290
291 f = 1f / a;
292 s = p - v0;
293 u = f * float3.dot(s, h);
294
295 if (u < 0.0f || u > 1.0f)
296 return false;
297
298 q = float3.cross(s, e1);
299 v = f * float3.dot(d, q);
300 if (v < 0.0f || u + v > 1.0f)
301 return false;
302
303 // at this stage we can compute t to find out where
304 // the intersection point is on the line
305 t = f * float3.dot(e2, q);
306 if (t > 0f) // ray intersection
307 return true;
308 else // this means that there is a line intersection but not a ray intersection
309 return false;
310 }
311
312 private static bool lineIntersectsTriangle(float3 rayStart, float3 rayEnd, float3 p1, float3 p2, float3 p3, ref float3 sect)
313 {
314 float3 dir = rayEnd - rayStart;
315
316 float d = (float)Math.Sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
317 float r = 1.0f / d;
318
319 dir *= r;
320
321 float t;
322 bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, out t);
323
324 if (ret)
325 {
326 if (t > d)
327 {
328 sect.x = rayStart.x + dir.x * t;
329 sect.y = rayStart.y + dir.y * t;
330 sect.z = rayStart.z + dir.z * t;
331 }
332 else
333 {
334 ret = false;
335 }
336 }
337
338 return ret;
339 }
340 }
341}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs
new file mode 100644
index 0000000..cc6383a
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs
@@ -0,0 +1,233 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Text;
31
32namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
33{
34 public static class Concavity
35 {
36 // compute's how 'concave' this object is and returns the total volume of the
37 // convex hull as well as the volume of the 'concavity' which was found.
38 public static float computeConcavity(List<float3> vertices, List<int> indices, ref float4 plane, ref float volume)
39 {
40 float cret = 0f;
41 volume = 1f;
42
43 HullResult result = new HullResult();
44 HullDesc desc = new HullDesc();
45
46 desc.MaxFaces = 256;
47 desc.MaxVertices = 256;
48 desc.SetHullFlag(HullFlag.QF_TRIANGLES);
49 desc.Vertices = vertices;
50
51 HullError ret = HullUtils.CreateConvexHull(desc, ref result);
52
53 if (ret == HullError.QE_OK)
54 {
55 volume = computeMeshVolume2(result.OutputVertices, result.Indices);
56
57 // ok..now..for each triangle on the original mesh..
58 // we extrude the points to the nearest point on the hull.
59 List<CTri> tris = new List<CTri>();
60
61 for (int i = 0; i < result.Indices.Count / 3; i++)
62 {
63 int i1 = result.Indices[i * 3 + 0];
64 int i2 = result.Indices[i * 3 + 1];
65 int i3 = result.Indices[i * 3 + 2];
66
67 float3 p1 = result.OutputVertices[i1];
68 float3 p2 = result.OutputVertices[i2];
69 float3 p3 = result.OutputVertices[i3];
70
71 CTri t = new CTri(p1, p2, p3, i1, i2, i3);
72 tris.Add(t);
73 }
74
75 // we have not pre-computed the plane equation for each triangle in the convex hull..
76 float totalVolume = 0;
77
78 List<CTri> ftris = new List<CTri>(); // 'feature' triangles.
79 List<CTri> input_mesh = new List<CTri>();
80
81 for (int i = 0; i < indices.Count / 3; i++)
82 {
83 int i1 = indices[i * 3 + 0];
84 int i2 = indices[i * 3 + 1];
85 int i3 = indices[i * 3 + 2];
86
87 float3 p1 = vertices[i1];
88 float3 p2 = vertices[i2];
89 float3 p3 = vertices[i3];
90
91 CTri t = new CTri(p1, p2, p3, i1, i2, i3);
92 input_mesh.Add(t);
93 }
94
95 for (int i = 0; i < indices.Count / 3; i++)
96 {
97 int i1 = indices[i * 3 + 0];
98 int i2 = indices[i * 3 + 1];
99 int i3 = indices[i * 3 + 2];
100
101 float3 p1 = vertices[i1];
102 float3 p2 = vertices[i2];
103 float3 p3 = vertices[i3];
104
105 CTri t = new CTri(p1, p2, p3, i1, i2, i3);
106
107 featureMatch(t, tris, input_mesh);
108
109 if (t.mConcavity > 0.05f)
110 {
111 float v = t.getVolume();
112 totalVolume += v;
113 ftris.Add(t);
114 }
115 }
116
117 SplitPlane.computeSplitPlane(vertices, indices, ref plane);
118 cret = totalVolume;
119 }
120
121 return cret;
122 }
123
124 public static bool featureMatch(CTri m, List<CTri> tris, List<CTri> input_mesh)
125 {
126 bool ret = false;
127 float neardot = 0.707f;
128 m.mConcavity = 0;
129
130 for (int i = 0; i < tris.Count; i++)
131 {
132 CTri t = tris[i];
133
134 if (t.samePlane(m))
135 {
136 ret = false;
137 break;
138 }
139
140 float dot = float3.dot(t.mNormal, m.mNormal);
141
142 if (dot > neardot)
143 {
144 float d1 = t.planeDistance(m.mP1);
145 float d2 = t.planeDistance(m.mP2);
146 float d3 = t.planeDistance(m.mP3);
147
148 if (d1 > 0.001f || d2 > 0.001f || d3 > 0.001f) // can't be near coplaner!
149 {
150 neardot = dot;
151
152 t.raySect(m.mP1, m.mNormal, ref m.mNear1);
153 t.raySect(m.mP2, m.mNormal, ref m.mNear2);
154 t.raySect(m.mP3, m.mNormal, ref m.mNear3);
155
156 ret = true;
157 }
158 }
159 }
160
161 if (ret)
162 {
163 m.mC1 = m.mP1.Distance(m.mNear1);
164 m.mC2 = m.mP2.Distance(m.mNear2);
165 m.mC3 = m.mP3.Distance(m.mNear3);
166
167 m.mConcavity = m.mC1;
168
169 if (m.mC2 > m.mConcavity)
170 m.mConcavity = m.mC2;
171 if (m.mC3 > m.mConcavity)
172 m.mConcavity = m.mC3;
173 }
174
175 return ret;
176 }
177
178 private static float det(float3 p1, float3 p2, float3 p3)
179 {
180 return p1.x * p2.y * p3.z + p2.x * p3.y * p1.z + p3.x * p1.y * p2.z - p1.x * p3.y * p2.z - p2.x * p1.y * p3.z - p3.x * p2.y * p1.z;
181 }
182
183 public static float computeMeshVolume(List<float3> vertices, List<int> indices)
184 {
185 float volume = 0f;
186
187 for (int i = 0; i < indices.Count / 3; i++)
188 {
189 float3 p1 = vertices[indices[i * 3 + 0]];
190 float3 p2 = vertices[indices[i * 3 + 1]];
191 float3 p3 = vertices[indices[i * 3 + 2]];
192
193 volume += det(p1, p2, p3); // compute the volume of the tetrahedran relative to the origin.
194 }
195
196 volume *= (1.0f / 6.0f);
197 if (volume < 0f)
198 return -volume;
199 return volume;
200 }
201
202 public static float computeMeshVolume2(List<float3> vertices, List<int> indices)
203 {
204 float volume = 0f;
205
206 float3 p0 = vertices[0];
207 for (int i = 0; i < indices.Count / 3; i++)
208 {
209 float3 p1 = vertices[indices[i * 3 + 0]];
210 float3 p2 = vertices[indices[i * 3 + 1]];
211 float3 p3 = vertices[indices[i * 3 + 2]];
212
213 volume += tetVolume(p0, p1, p2, p3); // compute the volume of the tetrahedron relative to the root vertice
214 }
215
216 return volume * (1.0f / 6.0f);
217 }
218
219 private static float tetVolume(float3 p0, float3 p1, float3 p2, float3 p3)
220 {
221 float3 a = p1 - p0;
222 float3 b = p2 - p0;
223 float3 c = p3 - p0;
224
225 float3 cross = float3.cross(b, c);
226 float volume = float3.dot(a, cross);
227
228 if (volume < 0f)
229 return -volume;
230 return volume;
231 }
232 }
233}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs
new file mode 100644
index 0000000..dfaede1
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs
@@ -0,0 +1,411 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Diagnostics;
31
32namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
33{
34 public class DecompDesc
35 {
36 public List<float3> mVertices;
37 public List<int> mIndices;
38
39 // options
40 public uint mDepth; // depth to split, a maximum of 10, generally not over 7.
41 public float mCpercent; // the concavity threshold percentage. 0=20 is reasonable.
42 public float mPpercent; // the percentage volume conservation threshold to collapse hulls. 0-30 is reasonable.
43
44 // hull output limits.
45 public uint mMaxVertices; // maximum number of vertices in the output hull. Recommended 32 or less.
46 public float mSkinWidth; // a skin width to apply to the output hulls.
47
48 public ConvexDecompositionCallback mCallback; // the interface to receive back the results.
49
50 public DecompDesc()
51 {
52 mDepth = 5;
53 mCpercent = 5;
54 mPpercent = 5;
55 mMaxVertices = 32;
56 }
57 }
58
59 public class CHull
60 {
61 public float[] mMin = new float[3];
62 public float[] mMax = new float[3];
63 public float mVolume;
64 public float mDiagonal;
65 public ConvexResult mResult;
66
67 public CHull(ConvexResult result)
68 {
69 mResult = new ConvexResult(result);
70 mVolume = Concavity.computeMeshVolume(result.HullVertices, result.HullIndices);
71
72 mDiagonal = getBoundingRegion(result.HullVertices, mMin, mMax);
73
74 float dx = mMax[0] - mMin[0];
75 float dy = mMax[1] - mMin[1];
76 float dz = mMax[2] - mMin[2];
77
78 dx *= 0.1f; // inflate 1/10th on each edge
79 dy *= 0.1f; // inflate 1/10th on each edge
80 dz *= 0.1f; // inflate 1/10th on each edge
81
82 mMin[0] -= dx;
83 mMin[1] -= dy;
84 mMin[2] -= dz;
85
86 mMax[0] += dx;
87 mMax[1] += dy;
88 mMax[2] += dz;
89 }
90
91 public void Dispose()
92 {
93 mResult = null;
94 }
95
96 public bool overlap(CHull h)
97 {
98 return overlapAABB(mMin, mMax, h.mMin, h.mMax);
99 }
100
101 // returns the d1Giagonal distance
102 private static float getBoundingRegion(List<float3> points, float[] bmin, float[] bmax)
103 {
104 float3 first = points[0];
105
106 bmin[0] = first.x;
107 bmin[1] = first.y;
108 bmin[2] = first.z;
109
110 bmax[0] = first.x;
111 bmax[1] = first.y;
112 bmax[2] = first.z;
113
114 for (int i = 1; i < points.Count; i++)
115 {
116 float3 p = points[i];
117
118 if (p[0] < bmin[0]) bmin[0] = p[0];
119 if (p[1] < bmin[1]) bmin[1] = p[1];
120 if (p[2] < bmin[2]) bmin[2] = p[2];
121
122 if (p[0] > bmax[0]) bmax[0] = p[0];
123 if (p[1] > bmax[1]) bmax[1] = p[1];
124 if (p[2] > bmax[2]) bmax[2] = p[2];
125 }
126
127 float dx = bmax[0] - bmin[0];
128 float dy = bmax[1] - bmin[1];
129 float dz = bmax[2] - bmin[2];
130
131 return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
132 }
133
134 // return true if the two AABB's overlap.
135 private static bool overlapAABB(float[] bmin1, float[] bmax1, float[] bmin2, float[] bmax2)
136 {
137 if (bmax2[0] < bmin1[0]) return false; // if the maximum is less than our minimum on any axis
138 if (bmax2[1] < bmin1[1]) return false;
139 if (bmax2[2] < bmin1[2]) return false;
140
141 if (bmin2[0] > bmax1[0]) return false; // if the minimum is greater than our maximum on any axis
142 if (bmin2[1] > bmax1[1]) return false; // if the minimum is greater than our maximum on any axis
143 if (bmin2[2] > bmax1[2]) return false; // if the minimum is greater than our maximum on any axis
144
145 return true; // the extents overlap
146 }
147 }
148
149 public class ConvexBuilder
150 {
151 public List<CHull> mChulls = new List<CHull>();
152 private ConvexDecompositionCallback mCallback;
153
154 private int MAXDEPTH = 8;
155 private float CONCAVE_PERCENT = 1f;
156 private float MERGE_PERCENT = 2f;
157
158 public ConvexBuilder(ConvexDecompositionCallback callback)
159 {
160 mCallback = callback;
161 }
162
163 public void Dispose()
164 {
165 int i;
166 for (i = 0; i < mChulls.Count; i++)
167 {
168 CHull cr = mChulls[i];
169 cr.Dispose();
170 }
171 }
172
173 public bool isDuplicate(uint i1, uint i2, uint i3, uint ci1, uint ci2, uint ci3)
174 {
175 uint dcount = 0;
176
177 Debug.Assert(i1 != i2 && i1 != i3 && i2 != i3);
178 Debug.Assert(ci1 != ci2 && ci1 != ci3 && ci2 != ci3);
179
180 if (i1 == ci1 || i1 == ci2 || i1 == ci3)
181 dcount++;
182 if (i2 == ci1 || i2 == ci2 || i2 == ci3)
183 dcount++;
184 if (i3 == ci1 || i3 == ci2 || i3 == ci3)
185 dcount++;
186
187 return dcount == 3;
188 }
189
190 public void getMesh(ConvexResult cr, VertexPool vc, List<int> indices)
191 {
192 List<int> src = cr.HullIndices;
193
194 for (int i = 0; i < src.Count / 3; i++)
195 {
196 int i1 = src[i * 3 + 0];
197 int i2 = src[i * 3 + 1];
198 int i3 = src[i * 3 + 2];
199
200 float3 p1 = cr.HullVertices[i1];
201 float3 p2 = cr.HullVertices[i2];
202 float3 p3 = cr.HullVertices[i3];
203
204 i1 = vc.getIndex(p1);
205 i2 = vc.getIndex(p2);
206 i3 = vc.getIndex(p3);
207 }
208 }
209
210 public CHull canMerge(CHull a, CHull b)
211 {
212 if (!a.overlap(b)) // if their AABB's (with a little slop) don't overlap, then return.
213 return null;
214
215 CHull ret = null;
216
217 // ok..we are going to combine both meshes into a single mesh
218 // and then we are going to compute the concavity...
219
220 VertexPool vc = new VertexPool();
221
222 List<int> indices = new List<int>();
223
224 getMesh(a.mResult, vc, indices);
225 getMesh(b.mResult, vc, indices);
226
227 int vcount = vc.GetSize();
228 List<float3> vertices = vc.GetVertices();
229 int tcount = indices.Count / 3;
230
231 //don't do anything if hull is empty
232 if (tcount == 0)
233 {
234 vc.Clear();
235 return null;
236 }
237
238 HullResult hresult = new HullResult();
239 HullDesc desc = new HullDesc();
240
241 desc.SetHullFlag(HullFlag.QF_TRIANGLES);
242 desc.Vertices = vertices;
243
244 HullError hret = HullUtils.CreateConvexHull(desc, ref hresult);
245
246 if (hret == HullError.QE_OK)
247 {
248 float combineVolume = Concavity.computeMeshVolume(hresult.OutputVertices, hresult.Indices);
249 float sumVolume = a.mVolume + b.mVolume;
250
251 float percent = (sumVolume * 100) / combineVolume;
252 if (percent >= (100.0f - MERGE_PERCENT))
253 {
254 ConvexResult cr = new ConvexResult(hresult.OutputVertices, hresult.Indices);
255 ret = new CHull(cr);
256 }
257 }
258
259 vc.Clear();
260 return ret;
261 }
262
263 public bool combineHulls()
264 {
265 bool combine = false;
266
267 sortChulls(mChulls); // sort the convex hulls, largest volume to least...
268
269 List<CHull> output = new List<CHull>(); // the output hulls...
270
271 int i;
272 for (i = 0; i < mChulls.Count && !combine; ++i)
273 {
274 CHull cr = mChulls[i];
275
276 int j;
277 for (j = 0; j < mChulls.Count; j++)
278 {
279 CHull match = mChulls[j];
280
281 if (cr != match) // don't try to merge a hull with itself, that be stoopid
282 {
283
284 CHull merge = canMerge(cr, match); // if we can merge these two....
285
286 if (merge != null)
287 {
288 output.Add(merge);
289
290 ++i;
291 while (i != mChulls.Count)
292 {
293 CHull cr2 = mChulls[i];
294 if (cr2 != match)
295 {
296 output.Add(cr2);
297 }
298 i++;
299 }
300
301 cr.Dispose();
302 match.Dispose();
303 combine = true;
304 break;
305 }
306 }
307 }
308
309 if (combine)
310 {
311 break;
312 }
313 else
314 {
315 output.Add(cr);
316 }
317 }
318
319 if (combine)
320 {
321 mChulls.Clear();
322 mChulls = output;
323 output.Clear();
324 }
325
326 return combine;
327 }
328
329 public int process(DecompDesc desc)
330 {
331 int ret = 0;
332
333 MAXDEPTH = (int)desc.mDepth;
334 CONCAVE_PERCENT = desc.mCpercent;
335 MERGE_PERCENT = desc.mPpercent;
336
337 ConvexDecomposition.calcConvexDecomposition(desc.mVertices, desc.mIndices, ConvexDecompResult, 0f, 0, MAXDEPTH, CONCAVE_PERCENT, MERGE_PERCENT);
338
339 while (combineHulls()) // keep combinging hulls until I can't combine any more...
340 ;
341
342 int i;
343 for (i = 0; i < mChulls.Count; i++)
344 {
345 CHull cr = mChulls[i];
346
347 // before we hand it back to the application, we need to regenerate the hull based on the
348 // limits given by the user.
349
350 ConvexResult c = cr.mResult; // the high resolution hull...
351
352 HullResult result = new HullResult();
353 HullDesc hdesc = new HullDesc();
354
355 hdesc.SetHullFlag(HullFlag.QF_TRIANGLES);
356
357 hdesc.Vertices = c.HullVertices;
358 hdesc.MaxVertices = desc.mMaxVertices; // maximum number of vertices allowed in the output
359
360 if (desc.mSkinWidth != 0f)
361 {
362 hdesc.SkinWidth = desc.mSkinWidth;
363 hdesc.SetHullFlag(HullFlag.QF_SKIN_WIDTH); // do skin width computation.
364 }
365
366 HullError ret2 = HullUtils.CreateConvexHull(hdesc, ref result);
367
368 if (ret2 == HullError.QE_OK)
369 {
370 ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices);
371
372 r.mHullVolume = Concavity.computeMeshVolume(result.OutputVertices, result.Indices); // the volume of the hull.
373
374 // compute the best fit OBB
375 //computeBestFitOBB(result.mNumOutputVertices, result.mOutputVertices, sizeof(float) * 3, r.mOBBSides, r.mOBBTransform);
376
377 //r.mOBBVolume = r.mOBBSides[0] * r.mOBBSides[1] * r.mOBBSides[2]; // compute the OBB volume.
378
379 //fm_getTranslation(r.mOBBTransform, r.mOBBCenter); // get the translation component of the 4x4 matrix.
380
381 //fm_matrixToQuat(r.mOBBTransform, r.mOBBOrientation); // extract the orientation as a quaternion.
382
383 //r.mSphereRadius = computeBoundingSphere(result.mNumOutputVertices, result.mOutputVertices, r.mSphereCenter);
384 //r.mSphereVolume = fm_sphereVolume(r.mSphereRadius);
385
386 mCallback(r);
387 }
388
389 result = null;
390 cr.Dispose();
391 }
392
393 ret = mChulls.Count;
394
395 mChulls.Clear();
396
397 return ret;
398 }
399
400 public void ConvexDecompResult(ConvexResult result)
401 {
402 CHull ch = new CHull(result);
403 mChulls.Add(ch);
404 }
405
406 public void sortChulls(List<CHull> hulls)
407 {
408 hulls.Sort(delegate(CHull a, CHull b) { return a.mVolume.CompareTo(b.mVolume); });
409 }
410 }
411}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs
new file mode 100644
index 0000000..2e2bb70
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs
@@ -0,0 +1,200 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Diagnostics;
31
32namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
33{
34 public delegate void ConvexDecompositionCallback(ConvexResult result);
35
36 public class FaceTri
37 {
38 public float3 P1;
39 public float3 P2;
40 public float3 P3;
41
42 public FaceTri() { }
43
44 public FaceTri(List<float3> vertices, int i1, int i2, int i3)
45 {
46 P1 = new float3(vertices[i1]);
47 P2 = new float3(vertices[i2]);
48 P3 = new float3(vertices[i3]);
49 }
50 }
51
52 public static class ConvexDecomposition
53 {
54 private static void addTri(VertexPool vl, List<int> list, float3 p1, float3 p2, float3 p3)
55 {
56 int i1 = vl.getIndex(p1);
57 int i2 = vl.getIndex(p2);
58 int i3 = vl.getIndex(p3);
59
60 // do *not* process degenerate triangles!
61 if ( i1 != i2 && i1 != i3 && i2 != i3 )
62 {
63 list.Add(i1);
64 list.Add(i2);
65 list.Add(i3);
66 }
67 }
68
69 public static void calcConvexDecomposition(List<float3> vertices, List<int> indices, ConvexDecompositionCallback callback, float masterVolume, int depth,
70 int maxDepth, float concavePercent, float mergePercent)
71 {
72 float4 plane = new float4();
73 bool split = false;
74
75 if (depth < maxDepth)
76 {
77 float volume = 0f;
78 float c = Concavity.computeConcavity(vertices, indices, ref plane, ref volume);
79
80 if (depth == 0)
81 {
82 masterVolume = volume;
83 }
84
85 float percent = (c * 100.0f) / masterVolume;
86
87 if (percent > concavePercent) // if great than 5% of the total volume is concave, go ahead and keep splitting.
88 {
89 split = true;
90 }
91 }
92
93 if (depth >= maxDepth || !split)
94 {
95 HullResult result = new HullResult();
96 HullDesc desc = new HullDesc();
97
98 desc.SetHullFlag(HullFlag.QF_TRIANGLES);
99
100 desc.Vertices = vertices;
101
102 HullError ret = HullUtils.CreateConvexHull(desc, ref result);
103
104 if (ret == HullError.QE_OK)
105 {
106 ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices);
107 callback(r);
108 }
109
110 return;
111 }
112
113 List<int> ifront = new List<int>();
114 List<int> iback = new List<int>();
115
116 VertexPool vfront = new VertexPool();
117 VertexPool vback = new VertexPool();
118
119 // ok..now we are going to 'split' all of the input triangles against this plane!
120 for (int i = 0; i < indices.Count / 3; i++)
121 {
122 int i1 = indices[i * 3 + 0];
123 int i2 = indices[i * 3 + 1];
124 int i3 = indices[i * 3 + 2];
125
126 FaceTri t = new FaceTri(vertices, i1, i2, i3);
127
128 float3[] front = new float3[4];
129 float3[] back = new float3[4];
130
131 int fcount = 0;
132 int bcount = 0;
133
134 PlaneTriResult result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount);
135
136 if (fcount > 4 || bcount > 4)
137 {
138 result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount);
139 }
140
141 switch (result)
142 {
143 case PlaneTriResult.PTR_FRONT:
144 Debug.Assert(fcount == 3);
145 addTri(vfront, ifront, front[0], front[1], front[2]);
146 break;
147 case PlaneTriResult.PTR_BACK:
148 Debug.Assert(bcount == 3);
149 addTri(vback, iback, back[0], back[1], back[2]);
150 break;
151 case PlaneTriResult.PTR_SPLIT:
152 Debug.Assert(fcount >= 3 && fcount <= 4);
153 Debug.Assert(bcount >= 3 && bcount <= 4);
154
155 addTri(vfront, ifront, front[0], front[1], front[2]);
156 addTri(vback, iback, back[0], back[1], back[2]);
157
158 if (fcount == 4)
159 {
160 addTri(vfront, ifront, front[0], front[2], front[3]);
161 }
162
163 if (bcount == 4)
164 {
165 addTri(vback, iback, back[0], back[2], back[3]);
166 }
167
168 break;
169 }
170 }
171
172 // ok... here we recursively call
173 if (ifront.Count > 0)
174 {
175 int vcount = vfront.GetSize();
176 List<float3> vertices2 = vfront.GetVertices();
177 for (int i = 0; i < vertices2.Count; i++)
178 vertices2[i] = new float3(vertices2[i]);
179 int tcount = ifront.Count / 3;
180
181 calcConvexDecomposition(vertices2, ifront, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent);
182 }
183
184 ifront.Clear();
185 vfront.Clear();
186
187 if (iback.Count > 0)
188 {
189 int vcount = vback.GetSize();
190 List<float3> vertices2 = vback.GetVertices();
191 int tcount = iback.Count / 3;
192
193 calcConvexDecomposition(vertices2, iback, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent);
194 }
195
196 iback.Clear();
197 vback.Clear();
198 }
199 }
200}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs
new file mode 100644
index 0000000..87758b5
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs
@@ -0,0 +1,74 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30
31namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
32{
33 public class ConvexResult
34 {
35 public List<float3> HullVertices;
36 public List<int> HullIndices;
37
38 public float mHullVolume; // the volume of the convex hull.
39
40 //public float[] OBBSides = new float[3]; // the width, height and breadth of the best fit OBB
41 //public float[] OBBCenter = new float[3]; // the center of the OBB
42 //public float[] OBBOrientation = new float[4]; // the quaternion rotation of the OBB.
43 //public float[] OBBTransform = new float[16]; // the 4x4 transform of the OBB.
44 //public float OBBVolume; // the volume of the OBB
45
46 //public float SphereRadius; // radius and center of best fit sphere
47 //public float[] SphereCenter = new float[3];
48 //public float SphereVolume; // volume of the best fit sphere
49
50 public ConvexResult()
51 {
52 HullVertices = new List<float3>();
53 HullIndices = new List<int>();
54 }
55
56 public ConvexResult(List<float3> hvertices, List<int> hindices)
57 {
58 HullVertices = hvertices;
59 HullIndices = hindices;
60 }
61
62 public ConvexResult(ConvexResult r)
63 {
64 HullVertices = new List<float3>(r.HullVertices);
65 HullIndices = new List<int>(r.HullIndices);
66 }
67
68 public void Dispose()
69 {
70 HullVertices = null;
71 HullIndices = null;
72 }
73 }
74}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs
new file mode 100644
index 0000000..d81df26
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs
@@ -0,0 +1,171 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30
31namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
32{
33 public class HullResult
34 {
35 public bool Polygons = true; // true if indices represents polygons, false indices are triangles
36 public List<float3> OutputVertices = new List<float3>();
37 public List<int> Indices;
38
39 // If triangles, then indices are array indexes into the vertex list.
40 // If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
41 }
42
43 public class PHullResult
44 {
45 public List<float3> Vertices = new List<float3>();
46 public List<int> Indices = new List<int>();
47 }
48
49 [Flags]
50 public enum HullFlag : int
51 {
52 QF_DEFAULT = 0,
53 QF_TRIANGLES = (1 << 0), // report results as triangles, not polygons.
54 QF_SKIN_WIDTH = (1 << 2) // extrude hull based on this skin width
55 }
56
57 public enum HullError : int
58 {
59 QE_OK, // success!
60 QE_FAIL // failed.
61 }
62
63 public class HullDesc
64 {
65 public HullFlag Flags; // flags to use when generating the convex hull.
66 public List<float3> Vertices;
67 public float NormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
68 public float SkinWidth;
69 public uint MaxVertices; // maximum number of vertices to be considered for the hull!
70 public uint MaxFaces;
71
72 public HullDesc()
73 {
74 Flags = HullFlag.QF_DEFAULT;
75 Vertices = new List<float3>();
76 NormalEpsilon = 0.001f;
77 MaxVertices = 4096;
78 MaxFaces = 4096;
79 SkinWidth = 0.01f;
80 }
81
82 public HullDesc(HullFlag flags, List<float3> vertices)
83 {
84 Flags = flags;
85 Vertices = new List<float3>(vertices);
86 NormalEpsilon = 0.001f;
87 MaxVertices = 4096;
88 MaxFaces = 4096;
89 SkinWidth = 0.01f;
90 }
91
92 public bool HasHullFlag(HullFlag flag)
93 {
94 return (Flags & flag) != 0;
95 }
96
97 public void SetHullFlag(HullFlag flag)
98 {
99 Flags |= flag;
100 }
101
102 public void ClearHullFlag(HullFlag flag)
103 {
104 Flags &= ~flag;
105 }
106 }
107
108 public class ConvexH
109 {
110 public struct HalfEdge
111 {
112 public short ea; // the other half of the edge (index into edges list)
113 public byte v; // the vertex at the start of this edge (index into vertices list)
114 public byte p; // the facet on which this edge lies (index into facets list)
115
116 public HalfEdge(short _ea, byte _v, byte _p)
117 {
118 ea = _ea;
119 v = _v;
120 p = _p;
121 }
122
123 public HalfEdge(HalfEdge e)
124 {
125 ea = e.ea;
126 v = e.v;
127 p = e.p;
128 }
129 }
130
131 public List<float3> vertices = new List<float3>();
132 public List<HalfEdge> edges = new List<HalfEdge>();
133 public List<Plane> facets = new List<Plane>();
134
135 public ConvexH(int vertices_size, int edges_size, int facets_size)
136 {
137 vertices = new List<float3>(vertices_size);
138 edges = new List<HalfEdge>(edges_size);
139 facets = new List<Plane>(facets_size);
140 }
141 }
142
143 public class VertFlag
144 {
145 public byte planetest;
146 public byte junk;
147 public byte undermap;
148 public byte overmap;
149 }
150
151 public class EdgeFlag
152 {
153 public byte planetest;
154 public byte fixes;
155 public short undermap;
156 public short overmap;
157 }
158
159 public class PlaneFlag
160 {
161 public byte undermap;
162 public byte overmap;
163 }
164
165 public class Coplanar
166 {
167 public ushort ea;
168 public byte v0;
169 public byte v1;
170 }
171}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs
new file mode 100644
index 0000000..1119a75
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs
@@ -0,0 +1,99 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Diagnostics;
31
32namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
33{
34 public class HullTriangle : int3
35 {
36 public int3 n = new int3();
37 public int id;
38 public int vmax;
39 public float rise;
40 private List<HullTriangle> tris;
41
42 public HullTriangle(int a, int b, int c, List<HullTriangle> tris)
43 : base(a, b, c)
44 {
45 this.tris = tris;
46
47 n = new int3(-1, -1, -1);
48 id = tris.Count;
49 tris.Add(this);
50 vmax = -1;
51 rise = 0.0f;
52 }
53
54 public void Dispose()
55 {
56 Debug.Assert(tris[id] == this);
57 tris[id] = null;
58 }
59
60 public int neib(int a, int b)
61 {
62 int i;
63
64 for (i = 0; i < 3; i++)
65 {
66 int i1 = (i + 1) % 3;
67 int i2 = (i + 2) % 3;
68 if ((this)[i] == a && (this)[i1] == b)
69 return n[i2];
70 if ((this)[i] == b && (this)[i1] == a)
71 return n[i2];
72 }
73
74 Debug.Assert(false);
75 return -1;
76 }
77
78 public void setneib(int a, int b, int value)
79 {
80 int i;
81
82 for (i = 0; i < 3; i++)
83 {
84 int i1 = (i + 1) % 3;
85 int i2 = (i + 2) % 3;
86 if ((this)[i] == a && (this)[i1] == b)
87 {
88 n[i2] = value;
89 return;
90 }
91 if ((this)[i] == b && (this)[i1] == a)
92 {
93 n[i2] = value;
94 return;
95 }
96 }
97 }
98 }
99}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs
new file mode 100644
index 0000000..c9ccfe2
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs
@@ -0,0 +1,1868 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Diagnostics;
31
32namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
33{
34 public static class HullUtils
35 {
36 public static int argmin(float[] a, int n)
37 {
38 int r = 0;
39 for (int i = 1; i < n; i++)
40 {
41 if (a[i] < a[r])
42 {
43 r = i;
44 }
45 }
46 return r;
47 }
48
49 public static float clampf(float a)
50 {
51 return Math.Min(1.0f, Math.Max(0.0f, a));
52 }
53
54 public static float Round(float a, float precision)
55 {
56 return (float)Math.Floor(0.5f + a / precision) * precision;
57 }
58
59 public static float Interpolate(float f0, float f1, float alpha)
60 {
61 return f0 * (1 - alpha) + f1 * alpha;
62 }
63
64 public static void Swap<T>(ref T a, ref T b)
65 {
66 T tmp = a;
67 a = b;
68 b = tmp;
69 }
70
71 public static bool above(List<float3> vertices, int3 t, float3 p, float epsilon)
72 {
73 float3 vtx = vertices[t.x];
74 float3 n = TriNormal(vtx, vertices[t.y], vertices[t.z]);
75 return (float3.dot(n, p - vtx) > epsilon); // EPSILON???
76 }
77
78 public static int hasedge(int3 t, int a, int b)
79 {
80 for (int i = 0; i < 3; i++)
81 {
82 int i1 = (i + 1) % 3;
83 if (t[i] == a && t[i1] == b)
84 return 1;
85 }
86 return 0;
87 }
88
89 public static bool hasvert(int3 t, int v)
90 {
91 return (t[0] == v || t[1] == v || t[2] == v);
92 }
93
94 public static int shareedge(int3 a, int3 b)
95 {
96 int i;
97 for (i = 0; i < 3; i++)
98 {
99 int i1 = (i + 1) % 3;
100 if (hasedge(a, b[i1], b[i]) != 0)
101 return 1;
102 }
103 return 0;
104 }
105
106 public static void b2bfix(HullTriangle s, HullTriangle t, List<HullTriangle> tris)
107 {
108 int i;
109 for (i = 0; i < 3; i++)
110 {
111 int i1 = (i + 1) % 3;
112 int i2 = (i + 2) % 3;
113 int a = (s)[i1];
114 int b = (s)[i2];
115 Debug.Assert(tris[s.neib(a, b)].neib(b, a) == s.id);
116 Debug.Assert(tris[t.neib(a, b)].neib(b, a) == t.id);
117 tris[s.neib(a, b)].setneib(b, a, t.neib(b, a));
118 tris[t.neib(b, a)].setneib(a, b, s.neib(a, b));
119 }
120 }
121
122 public static void removeb2b(HullTriangle s, HullTriangle t, List<HullTriangle> tris)
123 {
124 b2bfix(s, t, tris);
125 s.Dispose();
126 t.Dispose();
127 }
128
129 public static void checkit(HullTriangle t, List<HullTriangle> tris)
130 {
131 int i;
132 Debug.Assert(tris[t.id] == t);
133 for (i = 0; i < 3; i++)
134 {
135 int i1 = (i + 1) % 3;
136 int i2 = (i + 2) % 3;
137 int a = (t)[i1];
138 int b = (t)[i2];
139 Debug.Assert(a != b);
140 Debug.Assert(tris[t.n[i]].neib(b, a) == t.id);
141 }
142 }
143
144 public static void extrude(HullTriangle t0, int v, List<HullTriangle> tris)
145 {
146 int3 t = t0;
147 int n = tris.Count;
148 HullTriangle ta = new HullTriangle(v, t[1], t[2], tris);
149 ta.n = new int3(t0.n[0], n + 1, n + 2);
150 tris[t0.n[0]].setneib(t[1], t[2], n + 0);
151 HullTriangle tb = new HullTriangle(v, t[2], t[0], tris);
152 tb.n = new int3(t0.n[1], n + 2, n + 0);
153 tris[t0.n[1]].setneib(t[2], t[0], n + 1);
154 HullTriangle tc = new HullTriangle(v, t[0], t[1], tris);
155 tc.n = new int3(t0.n[2], n + 0, n + 1);
156 tris[t0.n[2]].setneib(t[0], t[1], n + 2);
157 checkit(ta, tris);
158 checkit(tb, tris);
159 checkit(tc, tris);
160 if (hasvert(tris[ta.n[0]], v))
161 removeb2b(ta, tris[ta.n[0]], tris);
162 if (hasvert(tris[tb.n[0]], v))
163 removeb2b(tb, tris[tb.n[0]], tris);
164 if (hasvert(tris[tc.n[0]], v))
165 removeb2b(tc, tris[tc.n[0]], tris);
166 t0.Dispose();
167 }
168
169 public static HullTriangle extrudable(float epsilon, List<HullTriangle> tris)
170 {
171 int i;
172 HullTriangle t = null;
173 for (i = 0; i < tris.Count; i++)
174 {
175 if (t == null || (tris.Count > i && (object)tris[i] != null && t.rise < tris[i].rise))
176 {
177 t = tris[i];
178 }
179 }
180 return (t.rise > epsilon) ? t : null;
181 }
182
183 public static Quaternion RotationArc(float3 v0, float3 v1)
184 {
185 Quaternion q = new Quaternion();
186 v0 = float3.normalize(v0); // Comment these two lines out if you know its not needed.
187 v1 = float3.normalize(v1); // If vector is already unit length then why do it again?
188 float3 c = float3.cross(v0, v1);
189 float d = float3.dot(v0, v1);
190 if (d <= -1.0f) // 180 about x axis
191 {
192 return new Quaternion(1f, 0f, 0f, 0f);
193 }
194 float s = (float)Math.Sqrt((1 + d) * 2f);
195 q.x = c.x / s;
196 q.y = c.y / s;
197 q.z = c.z / s;
198 q.w = s / 2.0f;
199 return q;
200 }
201
202 public static float3 PlaneLineIntersection(Plane plane, float3 p0, float3 p1)
203 {
204 // returns the point where the line p0-p1 intersects the plane n&d
205 float3 dif = p1 - p0;
206 float dn = float3.dot(plane.normal, dif);
207 float t = -(plane.dist + float3.dot(plane.normal, p0)) / dn;
208 return p0 + (dif * t);
209 }
210
211 public static float3 LineProject(float3 p0, float3 p1, float3 a)
212 {
213 float3 w = new float3();
214 w = p1 - p0;
215 float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z);
216 return p0 + w * t;
217 }
218
219 public static float3 PlaneProject(Plane plane, float3 point)
220 {
221 return point - plane.normal * (float3.dot(point, plane.normal) + plane.dist);
222 }
223
224 public static float LineProjectTime(float3 p0, float3 p1, float3 a)
225 {
226 float3 w = new float3();
227 w = p1 - p0;
228 float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z);
229 return t;
230 }
231
232 public static float3 ThreePlaneIntersection(Plane p0, Plane p1, Plane p2)
233 {
234 float3x3 mp = float3x3.Transpose(new float3x3(p0.normal, p1.normal, p2.normal));
235 float3x3 mi = float3x3.Inverse(mp);
236 float3 b = new float3(p0.dist, p1.dist, p2.dist);
237 return -b * mi;
238 }
239
240 public static bool PolyHit(List<float3> vert, float3 v0, float3 v1)
241 {
242 float3 impact = new float3();
243 float3 normal = new float3();
244 return PolyHit(vert, v0, v1, out impact, out normal);
245 }
246
247 public static bool PolyHit(List<float3> vert, float3 v0, float3 v1, out float3 impact)
248 {
249 float3 normal = new float3();
250 return PolyHit(vert, v0, v1, out impact, out normal);
251 }
252
253 public static bool PolyHit(List<float3> vert, float3 v0, float3 v1, out float3 impact, out float3 normal)
254 {
255 float3 the_point = new float3();
256
257 impact = null;
258 normal = null;
259
260 int i;
261 float3 nrml = new float3(0, 0, 0);
262 for (i = 0; i < vert.Count; i++)
263 {
264 int i1 = (i + 1) % vert.Count;
265 int i2 = (i + 2) % vert.Count;
266 nrml = nrml + float3.cross(vert[i1] - vert[i], vert[i2] - vert[i1]);
267 }
268
269 float m = float3.magnitude(nrml);
270 if (m == 0.0)
271 {
272 return false;
273 }
274 nrml = nrml * (1.0f / m);
275 float dist = -float3.dot(nrml, vert[0]);
276 float d0;
277 float d1;
278 if ((d0 = float3.dot(v0, nrml) + dist) < 0 || (d1 = float3.dot(v1, nrml) + dist) > 0)
279 {
280 return false;
281 }
282
283 // By using the cached plane distances d0 and d1
284 // we can optimize the following:
285 // the_point = planelineintersection(nrml,dist,v0,v1);
286 float a = d0 / (d0 - d1);
287 the_point = v0 * (1 - a) + v1 * a;
288
289
290 bool inside = true;
291 for (int j = 0; inside && j < vert.Count; j++)
292 {
293 // let inside = 0 if outside
294 float3 pp1 = new float3();
295 float3 pp2 = new float3();
296 float3 side = new float3();
297 pp1 = vert[j];
298 pp2 = vert[(j + 1) % vert.Count];
299 side = float3.cross((pp2 - pp1), (the_point - pp1));
300 inside = (float3.dot(nrml, side) >= 0.0);
301 }
302 if (inside)
303 {
304 if (normal != null)
305 {
306 normal = nrml;
307 }
308 if (impact != null)
309 {
310 impact = the_point;
311 }
312 }
313 return inside;
314 }
315
316 public static bool BoxInside(float3 p, float3 bmin, float3 bmax)
317 {
318 return (p.x >= bmin.x && p.x <= bmax.x && p.y >= bmin.y && p.y <= bmax.y && p.z >= bmin.z && p.z <= bmax.z);
319 }
320
321 public static bool BoxIntersect(float3 v0, float3 v1, float3 bmin, float3 bmax, float3 impact)
322 {
323 if (BoxInside(v0, bmin, bmax))
324 {
325 impact = v0;
326 return true;
327 }
328 if (v0.x <= bmin.x && v1.x >= bmin.x)
329 {
330 float a = (bmin.x - v0.x) / (v1.x - v0.x);
331 //v.x = bmin.x;
332 float vy = (1 - a) * v0.y + a * v1.y;
333 float vz = (1 - a) * v0.z + a * v1.z;
334 if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z)
335 {
336 impact.x = bmin.x;
337 impact.y = vy;
338 impact.z = vz;
339 return true;
340 }
341 }
342 else if (v0.x >= bmax.x && v1.x <= bmax.x)
343 {
344 float a = (bmax.x - v0.x) / (v1.x - v0.x);
345 //v.x = bmax.x;
346 float vy = (1 - a) * v0.y + a * v1.y;
347 float vz = (1 - a) * v0.z + a * v1.z;
348 if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z)
349 {
350 impact.x = bmax.x;
351 impact.y = vy;
352 impact.z = vz;
353 return true;
354 }
355 }
356 if (v0.y <= bmin.y && v1.y >= bmin.y)
357 {
358 float a = (bmin.y - v0.y) / (v1.y - v0.y);
359 float vx = (1 - a) * v0.x + a * v1.x;
360 //v.y = bmin.y;
361 float vz = (1 - a) * v0.z + a * v1.z;
362 if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z)
363 {
364 impact.x = vx;
365 impact.y = bmin.y;
366 impact.z = vz;
367 return true;
368 }
369 }
370 else if (v0.y >= bmax.y && v1.y <= bmax.y)
371 {
372 float a = (bmax.y - v0.y) / (v1.y - v0.y);
373 float vx = (1 - a) * v0.x + a * v1.x;
374 // vy = bmax.y;
375 float vz = (1 - a) * v0.z + a * v1.z;
376 if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z)
377 {
378 impact.x = vx;
379 impact.y = bmax.y;
380 impact.z = vz;
381 return true;
382 }
383 }
384 if (v0.z <= bmin.z && v1.z >= bmin.z)
385 {
386 float a = (bmin.z - v0.z) / (v1.z - v0.z);
387 float vx = (1 - a) * v0.x + a * v1.x;
388 float vy = (1 - a) * v0.y + a * v1.y;
389 // v.z = bmin.z;
390 if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x)
391 {
392 impact.x = vx;
393 impact.y = vy;
394 impact.z = bmin.z;
395 return true;
396 }
397 }
398 else if (v0.z >= bmax.z && v1.z <= bmax.z)
399 {
400 float a = (bmax.z - v0.z) / (v1.z - v0.z);
401 float vx = (1 - a) * v0.x + a * v1.x;
402 float vy = (1 - a) * v0.y + a * v1.y;
403 // v.z = bmax.z;
404 if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x)
405 {
406 impact.x = vx;
407 impact.y = vy;
408 impact.z = bmax.z;
409 return true;
410 }
411 }
412 return false;
413 }
414
415 public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint)
416 {
417 return DistanceBetweenLines(ustart, udir, vstart, vdir, upoint, null);
418 }
419
420 public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir)
421 {
422 return DistanceBetweenLines(ustart, udir, vstart, vdir, null, null);
423 }
424
425 public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint, float3 vpoint)
426 {
427 float3 cp = float3.normalize(float3.cross(udir, vdir));
428
429 float distu = -float3.dot(cp, ustart);
430 float distv = -float3.dot(cp, vstart);
431 float dist = (float)Math.Abs(distu - distv);
432 if (upoint != null)
433 {
434 Plane plane = new Plane();
435 plane.normal = float3.normalize(float3.cross(vdir, cp));
436 plane.dist = -float3.dot(plane.normal, vstart);
437 upoint = PlaneLineIntersection(plane, ustart, ustart + udir);
438 }
439 if (vpoint != null)
440 {
441 Plane plane = new Plane();
442 plane.normal = float3.normalize(float3.cross(udir, cp));
443 plane.dist = -float3.dot(plane.normal, ustart);
444 vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir);
445 }
446 return dist;
447 }
448
449 public static float3 TriNormal(float3 v0, float3 v1, float3 v2)
450 {
451 // return the normal of the triangle
452 // inscribed by v0, v1, and v2
453 float3 cp = float3.cross(v1 - v0, v2 - v1);
454 float m = float3.magnitude(cp);
455 if (m == 0)
456 return new float3(1, 0, 0);
457 return cp * (1.0f / m);
458 }
459
460 public static int PlaneTest(Plane p, float3 v, float planetestepsilon)
461 {
462 float a = float3.dot(v, p.normal) + p.dist;
463 int flag = (a > planetestepsilon) ? (2) : ((a < -planetestepsilon) ? (1) : (0));
464 return flag;
465 }
466
467 public static int SplitTest(ref ConvexH convex, Plane plane, float planetestepsilon)
468 {
469 int flag = 0;
470 for (int i = 0; i < convex.vertices.Count; i++)
471 {
472 flag |= PlaneTest(plane, convex.vertices[i], planetestepsilon);
473 }
474 return flag;
475 }
476
477 public static Quaternion VirtualTrackBall(float3 cop, float3 cor, float3 dir1, float3 dir2)
478 {
479 // routine taken from game programming gems.
480 // Implement track ball functionality to spin stuf on the screen
481 // cop center of projection
482 // cor center of rotation
483 // dir1 old mouse direction
484 // dir2 new mouse direction
485 // pretend there is a sphere around cor. Then find the points
486 // where dir1 and dir2 intersect that sphere. Find the
487 // rotation that takes the first point to the second.
488 float m;
489 // compute plane
490 float3 nrml = cor - cop;
491 float fudgefactor = 1.0f / (float3.magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop
492 nrml = float3.normalize(nrml);
493 float dist = -float3.dot(nrml, cor);
494 float3 u = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir1);
495 u = u - cor;
496 u = u * fudgefactor;
497 m = float3.magnitude(u);
498 if (m > 1)
499 {
500 u /= m;
501 }
502 else
503 {
504 u = u - (nrml * (float)Math.Sqrt(1 - m * m));
505 }
506 float3 v = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir2);
507 v = v - cor;
508 v = v * fudgefactor;
509 m = float3.magnitude(v);
510 if (m > 1)
511 {
512 v /= m;
513 }
514 else
515 {
516 v = v - (nrml * (float)Math.Sqrt(1 - m * m));
517 }
518 return RotationArc(u, v);
519 }
520
521 public static bool AssertIntact(ConvexH convex, float planetestepsilon)
522 {
523 int i;
524 int estart = 0;
525 for (i = 0; i < convex.edges.Count; i++)
526 {
527 if (convex.edges[estart].p != convex.edges[i].p)
528 {
529 estart = i;
530 }
531 int inext = i + 1;
532 if (inext >= convex.edges.Count || convex.edges[inext].p != convex.edges[i].p)
533 {
534 inext = estart;
535 }
536 Debug.Assert(convex.edges[inext].p == convex.edges[i].p);
537 int nb = convex.edges[i].ea;
538 Debug.Assert(nb != 255);
539 if (nb == 255 || nb == -1)
540 return false;
541 Debug.Assert(nb != -1);
542 Debug.Assert(i == convex.edges[nb].ea);
543 }
544 for (i = 0; i < convex.edges.Count; i++)
545 {
546 Debug.Assert((0) == PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon));
547 if ((0) != PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon))
548 return false;
549 if (convex.edges[estart].p != convex.edges[i].p)
550 {
551 estart = i;
552 }
553 int i1 = i + 1;
554 if (i1 >= convex.edges.Count || convex.edges[i1].p != convex.edges[i].p)
555 {
556 i1 = estart;
557 }
558 int i2 = i1 + 1;
559 if (i2 >= convex.edges.Count || convex.edges[i2].p != convex.edges[i].p)
560 {
561 i2 = estart;
562 }
563 if (i == i2) // i sliced tangent to an edge and created 2 meaningless edges
564 continue;
565 float3 localnormal = TriNormal(convex.vertices[convex.edges[i].v], convex.vertices[convex.edges[i1].v], convex.vertices[convex.edges[i2].v]);
566 Debug.Assert(float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) > 0);
567 if (float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) <= 0)
568 return false;
569 }
570 return true;
571 }
572
573 public static ConvexH test_btbq(float planetestepsilon)
574 {
575 // back to back quads
576 ConvexH convex = new ConvexH(4, 8, 2);
577 convex.vertices[0] = new float3(0, 0, 0);
578 convex.vertices[1] = new float3(1, 0, 0);
579 convex.vertices[2] = new float3(1, 1, 0);
580 convex.vertices[3] = new float3(0, 1, 0);
581 convex.facets[0] = new Plane(new float3(0, 0, 1), 0);
582 convex.facets[1] = new Plane(new float3(0, 0, -1), 0);
583 convex.edges[0] = new ConvexH.HalfEdge(7, 0, 0);
584 convex.edges[1] = new ConvexH.HalfEdge(6, 1, 0);
585 convex.edges[2] = new ConvexH.HalfEdge(5, 2, 0);
586 convex.edges[3] = new ConvexH.HalfEdge(4, 3, 0);
587
588 convex.edges[4] = new ConvexH.HalfEdge(3, 0, 1);
589 convex.edges[5] = new ConvexH.HalfEdge(2, 3, 1);
590 convex.edges[6] = new ConvexH.HalfEdge(1, 2, 1);
591 convex.edges[7] = new ConvexH.HalfEdge(0, 1, 1);
592 AssertIntact(convex, planetestepsilon);
593 return convex;
594 }
595
596 public static ConvexH test_cube()
597 {
598 ConvexH convex = new ConvexH(8, 24, 6);
599 convex.vertices[0] = new float3(0, 0, 0);
600 convex.vertices[1] = new float3(0, 0, 1);
601 convex.vertices[2] = new float3(0, 1, 0);
602 convex.vertices[3] = new float3(0, 1, 1);
603 convex.vertices[4] = new float3(1, 0, 0);
604 convex.vertices[5] = new float3(1, 0, 1);
605 convex.vertices[6] = new float3(1, 1, 0);
606 convex.vertices[7] = new float3(1, 1, 1);
607
608 convex.facets[0] = new Plane(new float3(-1, 0, 0), 0);
609 convex.facets[1] = new Plane(new float3(1, 0, 0), -1);
610 convex.facets[2] = new Plane(new float3(0, -1, 0), 0);
611 convex.facets[3] = new Plane(new float3(0, 1, 0), -1);
612 convex.facets[4] = new Plane(new float3(0, 0, -1), 0);
613 convex.facets[5] = new Plane(new float3(0, 0, 1), -1);
614
615 convex.edges[0] = new ConvexH.HalfEdge(11, 0, 0);
616 convex.edges[1] = new ConvexH.HalfEdge(23, 1, 0);
617 convex.edges[2] = new ConvexH.HalfEdge(15, 3, 0);
618 convex.edges[3] = new ConvexH.HalfEdge(16, 2, 0);
619
620 convex.edges[4] = new ConvexH.HalfEdge(13, 6, 1);
621 convex.edges[5] = new ConvexH.HalfEdge(21, 7, 1);
622 convex.edges[6] = new ConvexH.HalfEdge(9, 5, 1);
623 convex.edges[7] = new ConvexH.HalfEdge(18, 4, 1);
624
625 convex.edges[8] = new ConvexH.HalfEdge(19, 0, 2);
626 convex.edges[9] = new ConvexH.HalfEdge(6, 4, 2);
627 convex.edges[10] = new ConvexH.HalfEdge(20, 5, 2);
628 convex.edges[11] = new ConvexH.HalfEdge(0, 1, 2);
629
630 convex.edges[12] = new ConvexH.HalfEdge(22, 3, 3);
631 convex.edges[13] = new ConvexH.HalfEdge(4, 7, 3);
632 convex.edges[14] = new ConvexH.HalfEdge(17, 6, 3);
633 convex.edges[15] = new ConvexH.HalfEdge(2, 2, 3);
634
635 convex.edges[16] = new ConvexH.HalfEdge(3, 0, 4);
636 convex.edges[17] = new ConvexH.HalfEdge(14, 2, 4);
637 convex.edges[18] = new ConvexH.HalfEdge(7, 6, 4);
638 convex.edges[19] = new ConvexH.HalfEdge(8, 4, 4);
639
640 convex.edges[20] = new ConvexH.HalfEdge(10, 1, 5);
641 convex.edges[21] = new ConvexH.HalfEdge(5, 5, 5);
642 convex.edges[22] = new ConvexH.HalfEdge(12, 7, 5);
643 convex.edges[23] = new ConvexH.HalfEdge(1, 3, 5);
644
645 return convex;
646 }
647
648 public static ConvexH ConvexHMakeCube(float3 bmin, float3 bmax)
649 {
650 ConvexH convex = test_cube();
651 convex.vertices[0] = new float3(bmin.x, bmin.y, bmin.z);
652 convex.vertices[1] = new float3(bmin.x, bmin.y, bmax.z);
653 convex.vertices[2] = new float3(bmin.x, bmax.y, bmin.z);
654 convex.vertices[3] = new float3(bmin.x, bmax.y, bmax.z);
655 convex.vertices[4] = new float3(bmax.x, bmin.y, bmin.z);
656 convex.vertices[5] = new float3(bmax.x, bmin.y, bmax.z);
657 convex.vertices[6] = new float3(bmax.x, bmax.y, bmin.z);
658 convex.vertices[7] = new float3(bmax.x, bmax.y, bmax.z);
659
660 convex.facets[0] = new Plane(new float3(-1, 0, 0), bmin.x);
661 convex.facets[1] = new Plane(new float3(1, 0, 0), -bmax.x);
662 convex.facets[2] = new Plane(new float3(0, -1, 0), bmin.y);
663 convex.facets[3] = new Plane(new float3(0, 1, 0), -bmax.y);
664 convex.facets[4] = new Plane(new float3(0, 0, -1), bmin.z);
665 convex.facets[5] = new Plane(new float3(0, 0, 1), -bmax.z);
666 return convex;
667 }
668
669 public static ConvexH ConvexHCrop(ref ConvexH convex, Plane slice, float planetestepsilon)
670 {
671 int i;
672 int vertcountunder = 0;
673 int vertcountover = 0;
674 List<int> vertscoplanar = new List<int>(); // existing vertex members of convex that are coplanar
675 List<int> edgesplit = new List<int>(); // existing edges that members of convex that cross the splitplane
676
677 Debug.Assert(convex.edges.Count < 480);
678
679 EdgeFlag[] edgeflag = new EdgeFlag[512];
680 VertFlag[] vertflag = new VertFlag[256];
681 PlaneFlag[] planeflag = new PlaneFlag[128];
682 ConvexH.HalfEdge[] tmpunderedges = new ConvexH.HalfEdge[512];
683 Plane[] tmpunderplanes = new Plane[128];
684 Coplanar[] coplanaredges = new Coplanar[512];
685 int coplanaredges_num = 0;
686
687 List<float3> createdverts = new List<float3>();
688
689 // do the side-of-plane tests
690 for (i = 0; i < convex.vertices.Count; i++)
691 {
692 vertflag[i].planetest = (byte)PlaneTest(slice, convex.vertices[i], planetestepsilon);
693 if (vertflag[i].planetest == (0))
694 {
695 // ? vertscoplanar.Add(i);
696 vertflag[i].undermap = (byte)vertcountunder++;
697 vertflag[i].overmap = (byte)vertcountover++;
698 }
699 else if (vertflag[i].planetest == (1))
700 {
701 vertflag[i].undermap = (byte)vertcountunder++;
702 }
703 else
704 {
705 Debug.Assert(vertflag[i].planetest == (2));
706 vertflag[i].overmap = (byte)vertcountover++;
707 vertflag[i].undermap = 255; // for debugging purposes
708 }
709 }
710 int vertcountunderold = vertcountunder; // for debugging only
711
712 int under_edge_count = 0;
713 int underplanescount = 0;
714 int e0 = 0;
715
716 for (int currentplane = 0; currentplane < convex.facets.Count; currentplane++)
717 {
718 int estart = e0;
719 int enextface = 0;
720 int planeside = 0;
721 int e1 = e0 + 1;
722 int vout = -1;
723 int vin = -1;
724 int coplanaredge = -1;
725 do
726 {
727
728 if (e1 >= convex.edges.Count || convex.edges[e1].p != currentplane)
729 {
730 enextface = e1;
731 e1 = estart;
732 }
733 ConvexH.HalfEdge edge0 = convex.edges[e0];
734 ConvexH.HalfEdge edge1 = convex.edges[e1];
735 ConvexH.HalfEdge edgea = convex.edges[edge0.ea];
736
737 planeside |= vertflag[edge0.v].planetest;
738 //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) {
739 // assert(ecop==-1);
740 // ecop=e;
741 //}
742
743 if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (2))
744 {
745 // both endpoints over plane
746 edgeflag[e0].undermap = -1;
747 }
748 else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (1))
749 {
750 // at least one endpoint under, the other coplanar or under
751
752 edgeflag[e0].undermap = (short)under_edge_count;
753 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
754 tmpunderedges[under_edge_count].p = (byte)underplanescount;
755 if (edge0.ea < e0)
756 {
757 // connect the neighbors
758 Debug.Assert(edgeflag[edge0.ea].undermap != -1);
759 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
760 tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count;
761 }
762 under_edge_count++;
763 }
764 else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (0))
765 {
766 // both endpoints coplanar
767 // must check a 3rd point to see if UNDER
768 int e2 = e1 + 1;
769 if (e2 >= convex.edges.Count || convex.edges[e2].p != currentplane)
770 {
771 e2 = estart;
772 }
773 Debug.Assert(convex.edges[e2].p == currentplane);
774 ConvexH.HalfEdge edge2 = convex.edges[e2];
775 if (vertflag[edge2.v].planetest == (1))
776 {
777
778 edgeflag[e0].undermap = (short)under_edge_count;
779 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
780 tmpunderedges[under_edge_count].p = (byte)underplanescount;
781 tmpunderedges[under_edge_count].ea = -1;
782 // make sure this edge is added to the "coplanar" list
783 coplanaredge = under_edge_count;
784 vout = vertflag[edge0.v].undermap;
785 vin = vertflag[edge1.v].undermap;
786 under_edge_count++;
787 }
788 else
789 {
790 edgeflag[e0].undermap = -1;
791 }
792 }
793 else if (vertflag[edge0.v].planetest == (1) && vertflag[edge1.v].planetest == (2))
794 {
795 // first is under 2nd is over
796
797 edgeflag[e0].undermap = (short)under_edge_count;
798 tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
799 tmpunderedges[under_edge_count].p = (byte)underplanescount;
800 if (edge0.ea < e0)
801 {
802 Debug.Assert(edgeflag[edge0.ea].undermap != -1);
803 // connect the neighbors
804 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
805 tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count;
806 vout = tmpunderedges[edgeflag[edge0.ea].undermap].v;
807 }
808 else
809 {
810 Plane p0 = convex.facets[edge0.p];
811 Plane pa = convex.facets[edgea.p];
812 createdverts.Add(ThreePlaneIntersection(p0, pa, slice));
813 //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])));
814 //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]));
815 vout = vertcountunder++;
816 }
817 under_edge_count++;
818 /// hmmm something to think about: i might be able to output this edge regarless of
819 // wheter or not we know v-in yet. ok i;ll try this now:
820 tmpunderedges[under_edge_count].v = (byte)vout;
821 tmpunderedges[under_edge_count].p = (byte)underplanescount;
822 tmpunderedges[under_edge_count].ea = -1;
823 coplanaredge = under_edge_count;
824 under_edge_count++;
825
826 if (vin != -1)
827 {
828 // we previously processed an edge where we came under
829 // now we know about vout as well
830
831 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
832 }
833
834 }
835 else if (vertflag[edge0.v].planetest == (0) && vertflag[edge1.v].planetest == (2))
836 {
837 // first is coplanar 2nd is over
838
839 edgeflag[e0].undermap = -1;
840 vout = vertflag[edge0.v].undermap;
841 // I hate this but i have to make sure part of this face is UNDER before ouputting this vert
842 int k = estart;
843 Debug.Assert(edge0.p == currentplane);
844 while (!((planeside & 1) != 0) && k < convex.edges.Count && convex.edges[k].p == edge0.p)
845 {
846 planeside |= vertflag[convex.edges[k].v].planetest;
847 k++;
848 }
849 if ((planeside & 1) != 0)
850 {
851 tmpunderedges[under_edge_count].v = (byte)vout;
852 tmpunderedges[under_edge_count].p = (byte)underplanescount;
853 tmpunderedges[under_edge_count].ea = -1;
854 coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on
855 under_edge_count++;
856
857 }
858 }
859 else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (1))
860 {
861 // first is over next is under
862 // new vertex!!!
863 Debug.Assert(vin == -1);
864 if (e0 < edge0.ea)
865 {
866 Plane p0 = convex.facets[edge0.p];
867 Plane pa = convex.facets[edgea.p];
868 createdverts.Add(ThreePlaneIntersection(p0, pa, slice));
869 //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]));
870 //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])));
871 vin = vertcountunder++;
872 }
873 else
874 {
875 // find the new vertex that was created by edge[edge0.ea]
876 int nea = edgeflag[edge0.ea].undermap;
877 Debug.Assert(tmpunderedges[nea].p == tmpunderedges[nea + 1].p);
878 vin = tmpunderedges[nea + 1].v;
879 Debug.Assert(vin < vertcountunder);
880 Debug.Assert(vin >= vertcountunderold); // for debugging only
881 }
882 if (vout != -1)
883 {
884 // we previously processed an edge where we went over
885 // now we know vin too
886 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
887 }
888 // output edge
889 tmpunderedges[under_edge_count].v = (byte)vin;
890 tmpunderedges[under_edge_count].p = (byte)underplanescount;
891 edgeflag[e0].undermap = (short)under_edge_count;
892 if (e0 > edge0.ea)
893 {
894 Debug.Assert(edgeflag[edge0.ea].undermap != -1);
895 // connect the neighbors
896 tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap;
897 tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count;
898 }
899 Debug.Assert(edgeflag[e0].undermap == under_edge_count);
900 under_edge_count++;
901 }
902 else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (0))
903 {
904 // first is over next is coplanar
905
906 edgeflag[e0].undermap = -1;
907 vin = vertflag[edge1.v].undermap;
908 Debug.Assert(vin != -1);
909 if (vout != -1)
910 {
911 // we previously processed an edge where we came under
912 // now we know both endpoints
913 // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!!
914 }
915
916 }
917 else
918 {
919 Debug.Assert(false);
920 }
921
922
923 e0 = e1;
924 e1++; // do the modulo at the beginning of the loop
925
926 } while (e0 != estart);
927 e0 = enextface;
928 if ((planeside & 1) != 0)
929 {
930 planeflag[currentplane].undermap = (byte)underplanescount;
931 tmpunderplanes[underplanescount] = convex.facets[currentplane];
932 underplanescount++;
933 }
934 else
935 {
936 planeflag[currentplane].undermap = 0;
937 }
938 if (vout >= 0 && (planeside & 1) != 0)
939 {
940 Debug.Assert(vin >= 0);
941 Debug.Assert(coplanaredge >= 0);
942 Debug.Assert(coplanaredge != 511);
943 coplanaredges[coplanaredges_num].ea = (ushort)coplanaredge;
944 coplanaredges[coplanaredges_num].v0 = (byte)vin;
945 coplanaredges[coplanaredges_num].v1 = (byte)vout;
946 coplanaredges_num++;
947 }
948 }
949
950 // add the new plane to the mix:
951 if (coplanaredges_num > 0)
952 {
953 tmpunderplanes[underplanescount++] = slice;
954 }
955 for (i = 0; i < coplanaredges_num - 1; i++)
956 {
957 if (coplanaredges[i].v1 != coplanaredges[i + 1].v0)
958 {
959 int j = 0;
960 for (j = i + 2; j < coplanaredges_num; j++)
961 {
962 if (coplanaredges[i].v1 == coplanaredges[j].v0)
963 {
964 Coplanar tmp = coplanaredges[i + 1];
965 coplanaredges[i + 1] = coplanaredges[j];
966 coplanaredges[j] = tmp;
967 break;
968 }
969 }
970 if (j >= coplanaredges_num)
971 {
972 Debug.Assert(j < coplanaredges_num);
973 return null;
974 }
975 }
976 }
977
978 ConvexH punder = new ConvexH(vertcountunder, under_edge_count + coplanaredges_num, underplanescount);
979 ConvexH under = punder;
980
981 {
982 int k = 0;
983 for (i = 0; i < convex.vertices.Count; i++)
984 {
985 if (vertflag[i].planetest != (2))
986 {
987 under.vertices[k++] = convex.vertices[i];
988 }
989 }
990 i = 0;
991 while (k < vertcountunder)
992 {
993 under.vertices[k++] = createdverts[i++];
994 }
995 Debug.Assert(i == createdverts.Count);
996 }
997
998 for (i = 0; i < coplanaredges_num; i++)
999 {
1000 ConvexH.HalfEdge edge = under.edges[under_edge_count + i];
1001 edge.p = (byte)(underplanescount - 1);
1002 edge.ea = (short)coplanaredges[i].ea;
1003 edge.v = (byte)coplanaredges[i].v0;
1004 under.edges[under_edge_count + i] = edge;
1005
1006 tmpunderedges[coplanaredges[i].ea].ea = (short)(under_edge_count + i);
1007 }
1008
1009 under.edges = new List<ConvexH.HalfEdge>(tmpunderedges);
1010 under.facets = new List<Plane>(tmpunderplanes);
1011 return punder;
1012 }
1013
1014 public static ConvexH ConvexHDup(ConvexH src)
1015 {
1016 ConvexH dst = new ConvexH(src.vertices.Count, src.edges.Count, src.facets.Count);
1017 dst.vertices = new List<float3>(src.vertices.Count);
1018 foreach (float3 f in src.vertices)
1019 dst.vertices.Add(new float3(f));
1020 dst.edges = new List<ConvexH.HalfEdge>(src.edges.Count);
1021 foreach (ConvexH.HalfEdge e in src.edges)
1022 dst.edges.Add(new ConvexH.HalfEdge(e));
1023 dst.facets = new List<Plane>(src.facets.Count);
1024 foreach (Plane p in src.facets)
1025 dst.facets.Add(new Plane(p));
1026 return dst;
1027 }
1028
1029 public static int candidateplane(List<Plane> planes, int planes_count, ConvexH convex, float epsilon)
1030 {
1031 int p = 0;
1032 float md = 0;
1033 int i;
1034 for (i = 0; i < planes_count; i++)
1035 {
1036 float d = 0;
1037 for (int j = 0; j < convex.vertices.Count; j++)
1038 {
1039 d = Math.Max(d, float3.dot(convex.vertices[j], planes[i].normal) + planes[i].dist);
1040 }
1041 if (i == 0 || d > md)
1042 {
1043 p = i;
1044 md = d;
1045 }
1046 }
1047 return (md > epsilon) ? p : -1;
1048 }
1049
1050 public static float3 orth(float3 v)
1051 {
1052 float3 a = float3.cross(v, new float3(0f, 0f, 1f));
1053 float3 b = float3.cross(v, new float3(0f, 1f, 0f));
1054 return float3.normalize((float3.magnitude(a) > float3.magnitude(b)) ? a : b);
1055 }
1056
1057 public static int maxdir(List<float3> p, int count, float3 dir)
1058 {
1059 Debug.Assert(count != 0);
1060 int m = 0;
1061 float currDotm = float3.dot(p[0], dir);
1062 for (int i = 1; i < count; i++)
1063 {
1064 float currDoti = float3.dot(p[i], dir);
1065 if (currDoti > currDotm)
1066 {
1067 currDotm = currDoti;
1068 m = i;
1069 }
1070 }
1071 return m;
1072 }
1073
1074 public static int maxdirfiltered(List<float3> p, int count, float3 dir, byte[] allow)
1075 {
1076 //Debug.Assert(count != 0);
1077 int m = 0;
1078 float currDotm = float3.dot(p[0], dir);
1079 float currDoti;
1080
1081 while (allow[m] == 0)
1082 m++;
1083
1084 for (int i = 1; i < count; i++)
1085 {
1086 if (allow[i] != 0)
1087 {
1088 currDoti = float3.dot(p[i], dir);
1089 if (currDoti > currDotm)
1090 {
1091 currDotm = currDoti;
1092 m = i;
1093 }
1094 }
1095 }
1096 //Debug.Assert(m != -1);
1097 return m;
1098 }
1099
1100 public static int maxdirsterid(List<float3> p, int count, float3 dir, byte[] allow)
1101 {
1102 int m = -1;
1103 while (m == -1)
1104 {
1105 m = maxdirfiltered(p, count, dir, allow);
1106 if (allow[m] == 3)
1107 return m;
1108 float3 u = orth(dir);
1109 float3 v = float3.cross(u, dir);
1110 int ma = -1;
1111 for (float x = 0.0f; x <= 360.0f; x += 45.0f)
1112 {
1113 int mb;
1114 {
1115 float s = (float)Math.Sin((3.14159264f / 180.0f) * (x));
1116 float c = (float)Math.Cos((3.14159264f / 180.0f) * (x));
1117 mb = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow);
1118 }
1119 if (ma == m && mb == m)
1120 {
1121 allow[m] = 3;
1122 return m;
1123 }
1124 if (ma != -1 && ma != mb) // Yuck - this is really ugly
1125 {
1126 int mc = ma;
1127 for (float xx = x - 40.0f; xx <= x; xx += 5.0f)
1128 {
1129 float s = (float)Math.Sin((3.14159264f / 180.0f) * (xx));
1130 float c = (float)Math.Cos((3.14159264f / 180.0f) * (xx));
1131 int md = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow);
1132 if (mc == m && md == m)
1133 {
1134 allow[m] = 3;
1135 return m;
1136 }
1137 mc = md;
1138 }
1139 }
1140 ma = mb;
1141 }
1142 allow[m] = 0;
1143 m = -1;
1144 }
1145
1146 Debug.Assert(false);
1147 return m;
1148 }
1149
1150 public static int4 FindSimplex(List<float3> verts, byte[] allow)
1151 {
1152 float3[] basis = new float3[3];
1153 basis[0] = new float3(0.01f, 0.02f, 1.0f);
1154 int p0 = maxdirsterid(verts, verts.Count, basis[0], allow);
1155 int p1 = maxdirsterid(verts, verts.Count, -basis[0], allow);
1156 basis[0] = verts[p0] - verts[p1];
1157 if (p0 == p1 || basis[0] == new float3(0, 0, 0))
1158 return new int4(-1, -1, -1, -1);
1159 basis[1] = float3.cross(new float3(1, 0.02f, 0), basis[0]);
1160 basis[2] = float3.cross(new float3(-0.02f, 1, 0), basis[0]);
1161 basis[1] = float3.normalize((float3.magnitude(basis[1]) > float3.magnitude(basis[2])) ? basis[1] : basis[2]);
1162 int p2 = maxdirsterid(verts, verts.Count, basis[1], allow);
1163 if (p2 == p0 || p2 == p1)
1164 {
1165 p2 = maxdirsterid(verts, verts.Count, -basis[1], allow);
1166 }
1167 if (p2 == p0 || p2 == p1)
1168 return new int4(-1, -1, -1, -1);
1169 basis[1] = verts[p2] - verts[p0];
1170 basis[2] = float3.normalize(float3.cross(basis[1], basis[0]));
1171 int p3 = maxdirsterid(verts, verts.Count, basis[2], allow);
1172 if (p3 == p0 || p3 == p1 || p3 == p2)
1173 p3 = maxdirsterid(verts, verts.Count, -basis[2], allow);
1174 if (p3 == p0 || p3 == p1 || p3 == p2)
1175 return new int4(-1, -1, -1, -1);
1176 Debug.Assert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3));
1177 if (float3.dot(verts[p3] - verts[p0], float3.cross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0)
1178 {
1179 Swap(ref p2, ref p3);
1180 }
1181 return new int4(p0, p1, p2, p3);
1182 }
1183
1184 public static float GetDist(float px, float py, float pz, float3 p2)
1185 {
1186 float dx = px - p2.x;
1187 float dy = py - p2.y;
1188 float dz = pz - p2.z;
1189
1190 return dx * dx + dy * dy + dz * dz;
1191 }
1192
1193 public static void ReleaseHull(PHullResult result)
1194 {
1195 if (result.Indices != null)
1196 result.Indices = null;
1197 if (result.Vertices != null)
1198 result.Vertices = null;
1199 }
1200
1201 public static int calchullgen(List<float3> verts, int vlimit, List<HullTriangle> tris)
1202 {
1203 if (verts.Count < 4)
1204 return 0;
1205 if (vlimit == 0)
1206 vlimit = 1000000000;
1207 int j;
1208 float3 bmin = new float3(verts[0]);
1209 float3 bmax = new float3(verts[0]);
1210 List<int> isextreme = new List<int>(verts.Count);
1211 byte[] allow = new byte[verts.Count];
1212 for (j = 0; j < verts.Count; j++)
1213 {
1214 allow[j] = 1;
1215 isextreme.Add(0);
1216 bmin = float3.VectorMin(bmin, verts[j]);
1217 bmax = float3.VectorMax(bmax, verts[j]);
1218 }
1219 float epsilon = float3.magnitude(bmax - bmin) * 0.001f;
1220
1221 int4 p = FindSimplex(verts, allow);
1222 if (p.x == -1) // simplex failed
1223 return 0;
1224
1225 float3 center = (verts[p[0]] + verts[p[1]] + verts[p[2]] + verts[p[3]]) / 4.0f; // a valid interior point
1226 HullTriangle t0 = new HullTriangle(p[2], p[3], p[1], tris);
1227 t0.n = new int3(2, 3, 1);
1228 HullTriangle t1 = new HullTriangle(p[3], p[2], p[0], tris);
1229 t1.n = new int3(3, 2, 0);
1230 HullTriangle t2 = new HullTriangle(p[0], p[1], p[3], tris);
1231 t2.n = new int3(0, 1, 3);
1232 HullTriangle t3 = new HullTriangle(p[1], p[0], p[2], tris);
1233 t3.n = new int3(1, 0, 2);
1234 isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1;
1235 checkit(t0, tris);
1236 checkit(t1, tris);
1237 checkit(t2, tris);
1238 checkit(t3, tris);
1239
1240 for (j = 0; j < tris.Count; j++)
1241 {
1242 HullTriangle t = tris[j];
1243 Debug.Assert((object)t != null);
1244 Debug.Assert(t.vmax < 0);
1245 float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]);
1246 t.vmax = maxdirsterid(verts, verts.Count, n, allow);
1247 t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]);
1248 }
1249 HullTriangle te;
1250 vlimit -= 4;
1251 while (vlimit > 0 && (te = extrudable(epsilon, tris)) != null)
1252 {
1253 int3 ti = te;
1254 int v = te.vmax;
1255 Debug.Assert(isextreme[v] == 0); // wtf we've already done this vertex
1256 isextreme[v] = 1;
1257 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
1258 j = tris.Count;
1259 while (j-- != 0)
1260 {
1261 if (tris.Count <= j || (object)tris[j] == null)
1262 continue;
1263 int3 t = tris[j];
1264 if (above(verts, t, verts[v], 0.01f * epsilon))
1265 {
1266 extrude(tris[j], v, tris);
1267 }
1268 }
1269 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
1270 j = tris.Count;
1271 while (j-- != 0)
1272 {
1273 if (tris.Count <= j || (object)tris[j] == null)
1274 continue;
1275 if (!hasvert(tris[j], v))
1276 break;
1277 int3 nt = tris[j];
1278 if (above(verts, nt, center, 0.01f * epsilon) || float3.magnitude(float3.cross(verts[nt[1]] - verts[nt[0]], verts[nt[2]] - verts[nt[1]])) < epsilon * epsilon * 0.1f)
1279 {
1280 HullTriangle nb = tris[tris[j].n[0]];
1281 Debug.Assert(nb != null);
1282 Debug.Assert(!hasvert(nb, v));
1283 Debug.Assert(nb.id < j);
1284 extrude(nb, v, tris);
1285 j = tris.Count;
1286 }
1287 }
1288 j = tris.Count;
1289 while (j-- != 0)
1290 {
1291 HullTriangle t = tris[j];
1292 if (t == null)
1293 continue;
1294 if (t.vmax >= 0)
1295 break;
1296 float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]);
1297 t.vmax = maxdirsterid(verts, verts.Count, n, allow);
1298 if (isextreme[t.vmax] != 0)
1299 {
1300 t.vmax = -1; // already done that vertex - algorithm needs to be able to terminate.
1301 }
1302 else
1303 {
1304 t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]);
1305 }
1306 }
1307 vlimit--;
1308 }
1309 return 1;
1310 }
1311
1312 public static bool calchull(List<float3> verts, out List<int> tris_out, int vlimit, List<HullTriangle> tris)
1313 {
1314 tris_out = null;
1315
1316 int rc = calchullgen(verts, vlimit, tris);
1317 if (rc == 0)
1318 return false;
1319 List<int> ts = new List<int>();
1320 for (int i = 0; i < tris.Count; i++)
1321 {
1322 if ((object)tris[i] != null)
1323 {
1324 for (int j = 0; j < 3; j++)
1325 ts.Add((tris[i])[j]);
1326 tris[i] = null;
1327 }
1328 }
1329
1330 tris_out = ts;
1331 tris.Clear();
1332 return true;
1333 }
1334
1335 public static int calchullpbev(List<float3> verts, int vlimit, out List<Plane> planes, float bevangle, List<HullTriangle> tris)
1336 {
1337 int i;
1338 int j;
1339 planes = new List<Plane>();
1340 int rc = calchullgen(verts, vlimit, tris);
1341 if (rc == 0)
1342 return 0;
1343 for (i = 0; i < tris.Count; i++)
1344 {
1345 if (tris[i] != null)
1346 {
1347 Plane p = new Plane();
1348 HullTriangle t = tris[i];
1349 p.normal = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]);
1350 p.dist = -float3.dot(p.normal, verts[(t)[0]]);
1351 planes.Add(p);
1352 for (j = 0; j < 3; j++)
1353 {
1354 if (t.n[j] < t.id)
1355 continue;
1356 HullTriangle s = tris[t.n[j]];
1357 float3 snormal = TriNormal(verts[(s)[0]], verts[(s)[1]], verts[(s)[2]]);
1358 if (float3.dot(snormal, p.normal) >= Math.Cos(bevangle * (3.14159264f / 180.0f)))
1359 continue;
1360 float3 n = float3.normalize(snormal + p.normal);
1361 planes.Add(new Plane(n, -float3.dot(n, verts[maxdir(verts, verts.Count, n)])));
1362 }
1363 }
1364 }
1365
1366 tris.Clear();
1367 return 1;
1368 }
1369
1370 public static int overhull(List<Plane> planes, List<float3> verts, int maxplanes, out List<float3> verts_out, out List<int> faces_out, float inflate)
1371 {
1372 verts_out = null;
1373 faces_out = null;
1374
1375 int i;
1376 int j;
1377 if (verts.Count < 4)
1378 return 0;
1379 maxplanes = Math.Min(maxplanes, planes.Count);
1380 float3 bmin = new float3(verts[0]);
1381 float3 bmax = new float3(verts[0]);
1382 for (i = 0; i < verts.Count; i++)
1383 {
1384 bmin = float3.VectorMin(bmin, verts[i]);
1385 bmax = float3.VectorMax(bmax, verts[i]);
1386 }
1387 // float diameter = magnitude(bmax-bmin);
1388 // inflate *=diameter; // RELATIVE INFLATION
1389 bmin -= new float3(inflate, inflate, inflate);
1390 bmax += new float3(inflate, inflate, inflate);
1391 for (i = 0; i < planes.Count; i++)
1392 {
1393 planes[i].dist -= inflate;
1394 }
1395 float3 emin = new float3(bmin);
1396 float3 emax = new float3(bmax);
1397 float epsilon = float3.magnitude(emax - emin) * 0.025f;
1398 float planetestepsilon = float3.magnitude(emax - emin) * (0.001f);
1399 // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think.
1400 // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter));
1401 ConvexH c = ConvexHMakeCube(new float3(bmin), new float3(bmax));
1402 int k;
1403 while (maxplanes-- != 0 && (k = candidateplane(planes, planes.Count, c, epsilon)) >= 0)
1404 {
1405 ConvexH tmp = c;
1406 c = ConvexHCrop(ref tmp, planes[k], planetestepsilon);
1407 if (c == null) // might want to debug this case better!!!
1408 {
1409 c = tmp;
1410 break;
1411 }
1412 if (AssertIntact(c, planetestepsilon) == false) // might want to debug this case better too!!!
1413 {
1414 c = tmp;
1415 break;
1416 }
1417 tmp.edges = null;
1418 tmp.facets = null;
1419 tmp.vertices = null;
1420 }
1421
1422 Debug.Assert(AssertIntact(c, planetestepsilon));
1423 //return c;
1424 //C++ TO C# CONVERTER TODO TASK: The memory management function 'malloc' has no equivalent in C#:
1425 faces_out = new List<int>(); //(int)malloc(sizeof(int) * (1 + c.facets.Count + c.edges.Count)); // new int[1+c->facets.count+c->edges.count];
1426 int faces_count_out = 0;
1427 i = 0;
1428 faces_out[faces_count_out++] = -1;
1429 k = 0;
1430 while (i < c.edges.Count)
1431 {
1432 j = 1;
1433 while (j + i < c.edges.Count && c.edges[i].p == c.edges[i + j].p)
1434 {
1435 j++;
1436 }
1437 faces_out[faces_count_out++] = j;
1438 while (j-- != 0)
1439 {
1440 faces_out[faces_count_out++] = c.edges[i].v;
1441 i++;
1442 }
1443 k++;
1444 }
1445 faces_out[0] = k; // number of faces.
1446 Debug.Assert(k == c.facets.Count);
1447 Debug.Assert(faces_count_out == 1 + c.facets.Count + c.edges.Count);
1448 verts_out = c.vertices; // new float3[c->vertices.count];
1449 int verts_count_out = c.vertices.Count;
1450 for (i = 0; i < c.vertices.Count; i++)
1451 {
1452 verts_out[i] = new float3(c.vertices[i]);
1453 }
1454
1455 c.edges = null;
1456 c.facets = null;
1457 c.vertices = null;
1458 return 1;
1459 }
1460
1461 public static int overhullv(List<float3> verts, int maxplanes, out List<float3> verts_out, out List<int> faces_out, float inflate, float bevangle, int vlimit, List<HullTriangle> tris)
1462 {
1463 verts_out = null;
1464 faces_out = null;
1465
1466 if (verts.Count == 0)
1467 return 0;
1468 List<Plane> planes = new List<Plane>();
1469 int rc = calchullpbev(verts, vlimit, out planes, bevangle, tris);
1470 if (rc == 0)
1471 return 0;
1472 return overhull(planes, verts, maxplanes, out verts_out, out faces_out, inflate);
1473 }
1474
1475 public static void addPoint(ref uint vcount, List<float3> p, float x, float y, float z)
1476 {
1477 p.Add(new float3(x, y, z));
1478 vcount++;
1479 }
1480
1481 public static bool ComputeHull(List<float3> vertices, ref PHullResult result, int vlimit, float inflate)
1482 {
1483 List<HullTriangle> tris = new List<HullTriangle>();
1484 List<int> faces;
1485 List<float3> verts_out;
1486
1487 if (inflate == 0.0f)
1488 {
1489 List<int> tris_out;
1490 bool ret = calchull(vertices, out tris_out, vlimit, tris);
1491 if (ret == false)
1492 return false;
1493
1494 result.Indices = tris_out;
1495 result.Vertices = vertices;
1496 return true;
1497 }
1498 else
1499 {
1500 int ret = overhullv(vertices, 35, out verts_out, out faces, inflate, 120.0f, vlimit, tris);
1501 if (ret == 0)
1502 return false;
1503
1504 List<int3> tris2 = new List<int3>();
1505 int n = faces[0];
1506 int k = 1;
1507 for (int i = 0; i < n; i++)
1508 {
1509 int pn = faces[k++];
1510 for (int j = 2; j < pn; j++)
1511 tris2.Add(new int3(faces[k], faces[k + j - 1], faces[k + j]));
1512 k += pn;
1513 }
1514 Debug.Assert(tris2.Count == faces.Count - 1 - (n * 3));
1515
1516 result.Indices = new List<int>(tris2.Count * 3);
1517 for (int i = 0; i < tris2.Count; i++)
1518 {
1519 result.Indices.Add(tris2[i].x);
1520 result.Indices.Add(tris2[i].y);
1521 result.Indices.Add(tris2[i].z);
1522 }
1523 result.Vertices = verts_out;
1524
1525 return true;
1526 }
1527 }
1528
1529 private static bool CleanupVertices(List<float3> svertices, out List<float3> vertices, float normalepsilon, out float3 scale)
1530 {
1531 const float EPSILON = 0.000001f;
1532
1533 vertices = new List<float3>();
1534 scale = new float3(1f, 1f, 1f);
1535
1536 if (svertices.Count == 0)
1537 return false;
1538
1539 uint vcount = 0;
1540
1541 float[] recip = new float[3];
1542
1543 float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue };
1544 float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue };
1545
1546 for (int i = 0; i < svertices.Count; i++)
1547 {
1548 float3 p = svertices[i];
1549
1550 for (int j = 0; j < 3; j++)
1551 {
1552 if (p[j] < bmin[j])
1553 bmin[j] = p[j];
1554 if (p[j] > bmax[j])
1555 bmax[j] = p[j];
1556 }
1557 }
1558
1559 float dx = bmax[0] - bmin[0];
1560 float dy = bmax[1] - bmin[1];
1561 float dz = bmax[2] - bmin[2];
1562
1563 float3 center = new float3();
1564
1565 center.x = dx * 0.5f + bmin[0];
1566 center.y = dy * 0.5f + bmin[1];
1567 center.z = dz * 0.5f + bmin[2];
1568
1569 if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svertices.Count < 3)
1570 {
1571 float len = Single.MaxValue;
1572
1573 if (dx > EPSILON && dx < len)
1574 len = dx;
1575 if (dy > EPSILON && dy < len)
1576 len = dy;
1577 if (dz > EPSILON && dz < len)
1578 len = dz;
1579
1580 if (len == Single.MaxValue)
1581 {
1582 dx = dy = dz = 0.01f; // one centimeter
1583 }
1584 else
1585 {
1586 if (dx < EPSILON) // 1/5th the shortest non-zero edge.
1587 dx = len * 0.05f;
1588 if (dy < EPSILON)
1589 dy = len * 0.05f;
1590 if (dz < EPSILON)
1591 dz = len * 0.05f;
1592 }
1593
1594 float x1 = center[0] - dx;
1595 float x2 = center[0] + dx;
1596
1597 float y1 = center[1] - dy;
1598 float y2 = center[1] + dy;
1599
1600 float z1 = center[2] - dz;
1601 float z2 = center[2] + dz;
1602
1603 addPoint(ref vcount, vertices, x1, y1, z1);
1604 addPoint(ref vcount, vertices, x2, y1, z1);
1605 addPoint(ref vcount, vertices, x2, y2, z1);
1606 addPoint(ref vcount, vertices, x1, y2, z1);
1607 addPoint(ref vcount, vertices, x1, y1, z2);
1608 addPoint(ref vcount, vertices, x2, y1, z2);
1609 addPoint(ref vcount, vertices, x2, y2, z2);
1610 addPoint(ref vcount, vertices, x1, y2, z2);
1611
1612 return true; // return cube
1613 }
1614 else
1615 {
1616 scale.x = dx;
1617 scale.y = dy;
1618 scale.z = dz;
1619
1620 recip[0] = 1f / dx;
1621 recip[1] = 1f / dy;
1622 recip[2] = 1f / dz;
1623
1624 center.x *= recip[0];
1625 center.y *= recip[1];
1626 center.z *= recip[2];
1627 }
1628
1629 for (int i = 0; i < svertices.Count; i++)
1630 {
1631 float3 p = svertices[i];
1632
1633 float px = p[0];
1634 float py = p[1];
1635 float pz = p[2];
1636
1637 px = px * recip[0]; // normalize
1638 py = py * recip[1]; // normalize
1639 pz = pz * recip[2]; // normalize
1640
1641 if (true)
1642 {
1643 int j;
1644
1645 for (j = 0; j < vcount; j++)
1646 {
1647 float3 v = vertices[j];
1648
1649 float x = v[0];
1650 float y = v[1];
1651 float z = v[2];
1652
1653 float dx1 = Math.Abs(x - px);
1654 float dy1 = Math.Abs(y - py);
1655 float dz1 = Math.Abs(z - pz);
1656
1657 if (dx1 < normalepsilon && dy1 < normalepsilon && dz1 < normalepsilon)
1658 {
1659 // ok, it is close enough to the old one
1660 // now let us see if it is further from the center of the point cloud than the one we already recorded.
1661 // in which case we keep this one instead.
1662 float dist1 = GetDist(px, py, pz, center);
1663 float dist2 = GetDist(v[0], v[1], v[2], center);
1664
1665 if (dist1 > dist2)
1666 {
1667 v.x = px;
1668 v.y = py;
1669 v.z = pz;
1670 }
1671
1672 break;
1673 }
1674 }
1675
1676 if (j == vcount)
1677 {
1678 float3 dest = new float3(px, py, pz);
1679 vertices.Add(dest);
1680 vcount++;
1681 }
1682 }
1683 }
1684
1685 // ok..now make sure we didn't prune so many vertices it is now invalid.
1686 if (true)
1687 {
1688 float[] bmin2 = { Single.MaxValue, Single.MaxValue, Single.MaxValue };
1689 float[] bmax2 = { Single.MinValue, Single.MinValue, Single.MinValue };
1690
1691 for (int i = 0; i < vcount; i++)
1692 {
1693 float3 p = vertices[i];
1694 for (int j = 0; j < 3; j++)
1695 {
1696 if (p[j] < bmin2[j])
1697 bmin2[j] = p[j];
1698 if (p[j] > bmax2[j])
1699 bmax2[j] = p[j];
1700 }
1701 }
1702
1703 float dx2 = bmax2[0] - bmin2[0];
1704 float dy2 = bmax2[1] - bmin2[1];
1705 float dz2 = bmax2[2] - bmin2[2];
1706
1707 if (dx2 < EPSILON || dy2 < EPSILON || dz2 < EPSILON || vcount < 3)
1708 {
1709 float cx = dx2 * 0.5f + bmin2[0];
1710 float cy = dy2 * 0.5f + bmin2[1];
1711 float cz = dz2 * 0.5f + bmin2[2];
1712
1713 float len = Single.MaxValue;
1714
1715 if (dx2 >= EPSILON && dx2 < len)
1716 len = dx2;
1717 if (dy2 >= EPSILON && dy2 < len)
1718 len = dy2;
1719 if (dz2 >= EPSILON && dz2 < len)
1720 len = dz2;
1721
1722 if (len == Single.MaxValue)
1723 {
1724 dx2 = dy2 = dz2 = 0.01f; // one centimeter
1725 }
1726 else
1727 {
1728 if (dx2 < EPSILON) // 1/5th the shortest non-zero edge.
1729 dx2 = len * 0.05f;
1730 if (dy2 < EPSILON)
1731 dy2 = len * 0.05f;
1732 if (dz2 < EPSILON)
1733 dz2 = len * 0.05f;
1734 }
1735
1736 float x1 = cx - dx2;
1737 float x2 = cx + dx2;
1738
1739 float y1 = cy - dy2;
1740 float y2 = cy + dy2;
1741
1742 float z1 = cz - dz2;
1743 float z2 = cz + dz2;
1744
1745 vcount = 0; // add box
1746
1747 addPoint(ref vcount, vertices, x1, y1, z1);
1748 addPoint(ref vcount, vertices, x2, y1, z1);
1749 addPoint(ref vcount, vertices, x2, y2, z1);
1750 addPoint(ref vcount, vertices, x1, y2, z1);
1751 addPoint(ref vcount, vertices, x1, y1, z2);
1752 addPoint(ref vcount, vertices, x2, y1, z2);
1753 addPoint(ref vcount, vertices, x2, y2, z2);
1754 addPoint(ref vcount, vertices, x1, y2, z2);
1755
1756 return true;
1757 }
1758 }
1759
1760 return true;
1761 }
1762
1763 private static void BringOutYourDead(List<float3> verts, out List<float3> overts, List<int> indices)
1764 {
1765 int[] used = new int[verts.Count];
1766 int ocount = 0;
1767
1768 overts = new List<float3>();
1769
1770 for (int i = 0; i < indices.Count; i++)
1771 {
1772 int v = indices[i]; // original array index
1773
1774 Debug.Assert(v >= 0 && v < verts.Count);
1775
1776 if (used[v] != 0) // if already remapped
1777 {
1778 indices[i] = used[v] - 1; // index to new array
1779 }
1780 else
1781 {
1782 indices[i] = ocount; // new index mapping
1783
1784 overts.Add(verts[v]); // copy old vert to new vert array
1785
1786 ocount++; // increment output vert count
1787
1788 Debug.Assert(ocount >= 0 && ocount <= verts.Count);
1789
1790 used[v] = ocount; // assign new index remapping
1791 }
1792 }
1793 }
1794
1795 public static HullError CreateConvexHull(HullDesc desc, ref HullResult result)
1796 {
1797 HullError ret = HullError.QE_FAIL;
1798
1799 PHullResult hr = new PHullResult();
1800
1801 uint vcount = (uint)desc.Vertices.Count;
1802 if (vcount < 8)
1803 vcount = 8;
1804
1805 List<float3> vsource;
1806 float3 scale = new float3();
1807
1808 bool ok = CleanupVertices(desc.Vertices, out vsource, desc.NormalEpsilon, out scale); // normalize point cloud, remove duplicates!
1809
1810 if (ok)
1811 {
1812 if (true) // scale vertices back to their original size.
1813 {
1814 for (int i = 0; i < vsource.Count; i++)
1815 {
1816 float3 v = vsource[i];
1817 v.x *= scale[0];
1818 v.y *= scale[1];
1819 v.z *= scale[2];
1820 }
1821 }
1822
1823 float skinwidth = 0;
1824 if (desc.HasHullFlag(HullFlag.QF_SKIN_WIDTH))
1825 skinwidth = desc.SkinWidth;
1826
1827 ok = ComputeHull(vsource, ref hr, (int)desc.MaxVertices, skinwidth);
1828
1829 if (ok)
1830 {
1831 List<float3> vscratch;
1832 BringOutYourDead(hr.Vertices, out vscratch, hr.Indices);
1833
1834 ret = HullError.QE_OK;
1835
1836 if (desc.HasHullFlag(HullFlag.QF_TRIANGLES)) // if he wants the results as triangle!
1837 {
1838 result.Polygons = false;
1839 result.Indices = hr.Indices;
1840 result.OutputVertices = vscratch;
1841 }
1842 else
1843 {
1844 result.Polygons = true;
1845 result.OutputVertices = vscratch;
1846
1847 if (true)
1848 {
1849 List<int> source = hr.Indices;
1850 List<int> dest = new List<int>();
1851 for (int i = 0; i < hr.Indices.Count / 3; i++)
1852 {
1853 dest.Add(3);
1854 dest.Add(source[i * 3 + 0]);
1855 dest.Add(source[i * 3 + 1]);
1856 dest.Add(source[i * 3 + 2]);
1857 }
1858
1859 result.Indices = dest;
1860 }
1861 }
1862 }
1863 }
1864
1865 return ret;
1866 }
1867 }
1868}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt b/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt
new file mode 100644
index 0000000..714ae89
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt
@@ -0,0 +1,28 @@
1ConvexDecompositionDotNet
2-------------------------
3
4The MIT License
5
6Copyright (c) 2010 Intel Corporation.
7All rights reserved.
8
9Based on the convexdecomposition library from
10<http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
11
12Permission is hereby granted, free of charge, to any person obtaining a copy
13of this software and associated documentation files (the "Software"), to deal
14in the Software without restriction, including without limitation the rights
15to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
16copies of the Software, and to permit persons to whom the Software is
17furnished to do so, subject to the following conditions:
18
19The above copyright notice and this permission notice shall be included in
20all copies or substantial portions of the Software.
21
22THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
23IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
24FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
25AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
26LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
27OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
28THE SOFTWARE.
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs
new file mode 100644
index 0000000..d099676
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs
@@ -0,0 +1,99 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29
30namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
31{
32 public class Plane
33 {
34 public float3 normal = new float3();
35 public float dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
36
37 public Plane(float3 n, float d)
38 {
39 normal = new float3(n);
40 dist = d;
41 }
42
43 public Plane(Plane p)
44 {
45 normal = new float3(p.normal);
46 dist = p.dist;
47 }
48
49 public Plane()
50 {
51 dist = 0;
52 }
53
54 public void Transform(float3 position, Quaternion orientation)
55 {
56 // Transforms the plane to the space defined by the
57 // given position/orientation
58 float3 newNormal = Quaternion.Inverse(orientation) * normal;
59 float3 origin = Quaternion.Inverse(orientation) * (-normal * dist - position);
60
61 normal = newNormal;
62 dist = -float3.dot(newNormal, origin);
63 }
64
65 public override int GetHashCode()
66 {
67 return normal.GetHashCode() ^ dist.GetHashCode();
68 }
69
70 public override bool Equals(object obj)
71 {
72 Plane p = obj as Plane;
73 if (p == null)
74 return false;
75
76 return this == p;
77 }
78
79 public static bool operator ==(Plane a, Plane b)
80 {
81 return (a.normal == b.normal && a.dist == b.dist);
82 }
83
84 public static bool operator !=(Plane a, Plane b)
85 {
86 return !(a == b);
87 }
88
89 public static Plane PlaneFlip(Plane plane)
90 {
91 return new Plane(-plane.normal, -plane.dist);
92 }
93
94 public static bool coplanar(Plane a, Plane b)
95 {
96 return (a == b || a == PlaneFlip(b));
97 }
98 }
99}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs
new file mode 100644
index 0000000..31f0182
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs
@@ -0,0 +1,211 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Diagnostics;
31
32namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
33{
34 public enum PlaneTriResult : int
35 {
36 PTR_FRONT,
37 PTR_BACK,
38 PTR_SPLIT
39 }
40
41 public static class PlaneTri
42 {
43 private static float DistToPt(float3 p, float4 plane)
44 {
45 return p.x * plane.x + p.y * plane.y + p.z * plane.z + plane.w;
46 }
47
48 private static PlaneTriResult getSidePlane(float3 p, float4 plane, float epsilon)
49 {
50 float d = DistToPt(p, plane);
51
52 if ((d + epsilon) > 0f)
53 return PlaneTriResult.PTR_FRONT; // it is 'in front' within the provided epsilon value.
54
55 return PlaneTriResult.PTR_BACK;
56 }
57
58 private static void add(float3 p, float3[] dest, ref int pcount)
59 {
60 dest[pcount++] = new float3(p);
61 Debug.Assert(pcount <= 4);
62 }
63
64 // assumes that the points are on opposite sides of the plane!
65 private static void intersect(float3 p1, float3 p2, float3 split, float4 plane)
66 {
67 float dp1 = DistToPt(p1, plane);
68 float[] dir = new float[3];
69
70 dir[0] = p2[0] - p1[0];
71 dir[1] = p2[1] - p1[1];
72 dir[2] = p2[2] - p1[2];
73
74 float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2];
75 float dot2 = dp1 - plane[3];
76
77 float t = -(plane[3] + dot2) / dot1;
78
79 split.x = (dir[0] * t) + p1[0];
80 split.y = (dir[1] * t) + p1[1];
81 split.z = (dir[2] * t) + p1[2];
82 }
83
84 public static PlaneTriResult planeTriIntersection(float4 plane, FaceTri triangle, float epsilon, ref float3[] front, out int fcount, ref float3[] back, out int bcount)
85 {
86 fcount = 0;
87 bcount = 0;
88
89 // get the three vertices of the triangle.
90 float3 p1 = triangle.P1;
91 float3 p2 = triangle.P2;
92 float3 p3 = triangle.P3;
93
94 PlaneTriResult r1 = getSidePlane(p1, plane, epsilon); // compute the side of the plane each vertex is on
95 PlaneTriResult r2 = getSidePlane(p2, plane, epsilon);
96 PlaneTriResult r3 = getSidePlane(p3, plane, epsilon);
97
98 if (r1 == r2 && r1 == r3) // if all three vertices are on the same side of the plane.
99 {
100 if (r1 == PlaneTriResult.PTR_FRONT) // if all three are in front of the plane, then copy to the 'front' output triangle.
101 {
102 add(p1, front, ref fcount);
103 add(p2, front, ref fcount);
104 add(p3, front, ref fcount);
105 }
106 else
107 {
108 add(p1, back, ref bcount); // if all three are in 'back' then copy to the 'back' output triangle.
109 add(p2, back, ref bcount);
110 add(p3, back, ref bcount);
111 }
112 return r1; // if all three points are on the same side of the plane return result
113 }
114
115 // ok.. we need to split the triangle at the plane.
116
117 // First test ray segment P1 to P2
118 if (r1 == r2) // if these are both on the same side...
119 {
120 if (r1 == PlaneTriResult.PTR_FRONT)
121 {
122 add(p1, front, ref fcount);
123 add(p2, front, ref fcount);
124 }
125 else
126 {
127 add(p1, back, ref bcount);
128 add(p2, back, ref bcount);
129 }
130 }
131 else
132 {
133 float3 split = new float3();
134 intersect(p1, p2, split, plane);
135
136 if (r1 == PlaneTriResult.PTR_FRONT)
137 {
138
139 add(p1, front, ref fcount);
140 add(split, front, ref fcount);
141
142 add(split, back, ref bcount);
143 add(p2, back, ref bcount);
144
145 }
146 else
147 {
148 add(p1, back, ref bcount);
149 add(split, back, ref bcount);
150
151 add(split, front, ref fcount);
152 add(p2, front, ref fcount);
153 }
154
155 }
156
157 // Next test ray segment P2 to P3
158 if (r2 == r3) // if these are both on the same side...
159 {
160 if (r3 == PlaneTriResult.PTR_FRONT)
161 {
162 add(p3, front, ref fcount);
163 }
164 else
165 {
166 add(p3, back, ref bcount);
167 }
168 }
169 else
170 {
171 float3 split = new float3(); // split the point
172 intersect(p2, p3, split, plane);
173
174 if (r3 == PlaneTriResult.PTR_FRONT)
175 {
176 add(split, front, ref fcount);
177 add(split, back, ref bcount);
178
179 add(p3, front, ref fcount);
180 }
181 else
182 {
183 add(split, front, ref fcount);
184 add(split, back, ref bcount);
185
186 add(p3, back, ref bcount);
187 }
188 }
189
190 // Next test ray segment P3 to P1
191 if (r3 != r1) // if these are both on the same side...
192 {
193 float3 split = new float3(); // split the point
194 intersect(p3, p1, split, plane);
195
196 if (r1 == PlaneTriResult.PTR_FRONT)
197 {
198 add(split, front, ref fcount);
199 add(split, back, ref bcount);
200 }
201 else
202 {
203 add(split, front, ref fcount);
204 add(split, back, ref bcount);
205 }
206 }
207
208 return PlaneTriResult.PTR_SPLIT;
209 }
210 }
211}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..4285e8c
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs
@@ -0,0 +1,36 @@
1using System.Reflection;
2using System.Runtime.CompilerServices;
3using System.Runtime.InteropServices;
4
5// General Information about an assembly is controlled through the following
6// set of attributes. Change these attribute values to modify the information
7// associated with an assembly.
8[assembly: AssemblyTitle("ConvexDecompositionDotNet")]
9[assembly: AssemblyDescription("")]
10[assembly: AssemblyConfiguration("")]
11[assembly: AssemblyCompany("Intel Corporation")]
12[assembly: AssemblyProduct("ConvexDecompositionDotNet")]
13[assembly: AssemblyCopyright("Copyright © Intel Corporation 2010")]
14[assembly: AssemblyTrademark("")]
15[assembly: AssemblyCulture("")]
16
17// Setting ComVisible to false makes the types in this assembly not visible
18// to COM components. If you need to access a type in this assembly from
19// COM, set the ComVisible attribute to true on that type.
20[assembly: ComVisible(false)]
21
22// The following GUID is for the ID of the typelib if this project is exposed to COM
23[assembly: Guid("2a1c9467-1a17-4c8d-bf9f-4b4d86dd0cbb")]
24
25// Version information for an assembly consists of the following four values:
26//
27// Major Version
28// Minor Version
29// Build Number
30// Revision
31//
32// You can specify all the values or you can default the Build and Revision Numbers
33// by using the '*' as shown below:
34// [assembly: AssemblyVersion("1.0.*")]
35[assembly: AssemblyVersion("1.0.0.0")]
36[assembly: AssemblyFileVersion("1.0.0.0")]
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs
new file mode 100644
index 0000000..0ba8f17
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs
@@ -0,0 +1,209 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29
30namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
31{
32 public class Quaternion : float4
33 {
34 public Quaternion()
35 {
36 x = y = z = 0.0f;
37 w = 1.0f;
38 }
39
40 public Quaternion(float3 v, float t)
41 {
42 v = float3.normalize(v);
43 w = (float)Math.Cos(t / 2.0f);
44 v = v * (float)Math.Sin(t / 2.0f);
45 x = v.x;
46 y = v.y;
47 z = v.z;
48 }
49
50 public Quaternion(float _x, float _y, float _z, float _w)
51 {
52 x = _x;
53 y = _y;
54 z = _z;
55 w = _w;
56 }
57
58 public float angle()
59 {
60 return (float)Math.Acos(w) * 2.0f;
61 }
62
63 public float3 axis()
64 {
65 float3 a = new float3(x, y, z);
66 if (Math.Abs(angle()) < 0.0000001f)
67 return new float3(1f, 0f, 0f);
68 return a * (1 / (float)Math.Sin(angle() / 2.0f));
69 }
70
71 public float3 xdir()
72 {
73 return new float3(1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y));
74 }
75
76 public float3 ydir()
77 {
78 return new float3(2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x));
79 }
80
81 public float3 zdir()
82 {
83 return new float3(2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y));
84 }
85
86 public float3x3 getmatrix()
87 {
88 return new float3x3(xdir(), ydir(), zdir());
89 }
90
91 public static implicit operator float3x3(Quaternion q)
92 {
93 return q.getmatrix();
94 }
95
96 public static Quaternion operator *(Quaternion a, Quaternion b)
97 {
98 Quaternion c = new Quaternion();
99 c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
100 c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
101 c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
102 c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
103 return c;
104 }
105
106 public static float3 operator *(Quaternion q, float3 v)
107 {
108 // The following is equivalent to:
109 //return (q.getmatrix() * v);
110 float qx2 = q.x * q.x;
111 float qy2 = q.y * q.y;
112 float qz2 = q.z * q.z;
113
114 float qxqy = q.x * q.y;
115 float qxqz = q.x * q.z;
116 float qxqw = q.x * q.w;
117 float qyqz = q.y * q.z;
118 float qyqw = q.y * q.w;
119 float qzqw = q.z * q.w;
120 return new float3((1 - 2 * (qy2 + qz2)) * v.x + (2 * (qxqy - qzqw)) * v.y + (2 * (qxqz + qyqw)) * v.z, (2 * (qxqy + qzqw)) * v.x + (1 - 2 * (qx2 + qz2)) * v.y + (2 * (qyqz - qxqw)) * v.z, (2 * (qxqz - qyqw)) * v.x + (2 * (qyqz + qxqw)) * v.y + (1 - 2 * (qx2 + qy2)) * v.z);
121 }
122
123 public static Quaternion operator +(Quaternion a, Quaternion b)
124 {
125 return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
126 }
127
128 public static Quaternion operator *(Quaternion a, float b)
129 {
130 return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b);
131 }
132
133 public static Quaternion normalize(Quaternion a)
134 {
135 float m = (float)Math.Sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z);
136 if (m < 0.000000001f)
137 {
138 a.w = 1;
139 a.x = a.y = a.z = 0;
140 return a;
141 }
142 return a * (1f / m);
143 }
144
145 public static float dot(Quaternion a, Quaternion b)
146 {
147 return (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z);
148 }
149
150 public static Quaternion slerp(Quaternion a, Quaternion b, float interp)
151 {
152 if (dot(a, b) < 0.0)
153 {
154 a.w = -a.w;
155 a.x = -a.x;
156 a.y = -a.y;
157 a.z = -a.z;
158 }
159 float d = dot(a, b);
160 if (d >= 1.0)
161 {
162 return a;
163 }
164 float theta = (float)Math.Acos(d);
165 if (theta == 0.0f)
166 {
167 return (a);
168 }
169 return a * ((float)Math.Sin(theta - interp * theta) / (float)Math.Sin(theta)) + b * ((float)Math.Sin(interp * theta) / (float)Math.Sin(theta));
170 }
171
172 public static Quaternion Interpolate(Quaternion q0, Quaternion q1, float alpha)
173 {
174 return slerp(q0, q1, alpha);
175 }
176
177 public static Quaternion Inverse(Quaternion q)
178 {
179 return new Quaternion(-q.x, -q.y, -q.z, q.w);
180 }
181
182 public static Quaternion YawPitchRoll(float yaw, float pitch, float roll)
183 {
184 roll *= (3.14159264f / 180.0f);
185 yaw *= (3.14159264f / 180.0f);
186 pitch *= (3.14159264f / 180.0f);
187 return new Quaternion(new float3(0.0f, 0.0f, 1.0f), yaw) * new Quaternion(new float3(1.0f, 0.0f, 0.0f), pitch) * new Quaternion(new float3(0.0f, 1.0f, 0.0f), roll);
188 }
189
190 public static float Yaw(Quaternion q)
191 {
192 float3 v = q.ydir();
193 return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f);
194 }
195
196 public static float Pitch(Quaternion q)
197 {
198 float3 v = q.ydir();
199 return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f);
200 }
201
202 public static float Roll(Quaternion q)
203 {
204 q = new Quaternion(new float3(0.0f, 0.0f, 1.0f), -Yaw(q) * (3.14159264f / 180.0f)) * q;
205 q = new Quaternion(new float3(1.0f, 0.0f, 0.0f), -Pitch(q) * (3.14159264f / 180.0f)) * q;
206 return (float)Math.Atan2(-q.xdir().z, q.xdir().x) * (180.0f / 3.14159264f);
207 }
208 }
209}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt b/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt
new file mode 100644
index 0000000..fc53ae7
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt
@@ -0,0 +1,7 @@
1ConvexDecompositionDotNet
2=========================
3
4A C# port of the ConvexDecomposition library by John W. Ratcliff and Stan Melax.
5The original C++ version is available at <http://codesuppository.googlecode.com/>.
6See the blog post at <http://codesuppository.blogspot.com/2006/08/approximate-convexdecomposition.html>
7for a thorough explanation of generating convex hulls from concave meshes.
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs
new file mode 100644
index 0000000..9f06a9a
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs
@@ -0,0 +1,265 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30
31namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
32{
33 public class Rect3d
34 {
35 public float[] mMin = new float[3];
36 public float[] mMax = new float[3];
37
38 public Rect3d()
39 {
40 }
41
42 public Rect3d(float[] bmin, float[] bmax)
43 {
44 mMin[0] = bmin[0];
45 mMin[1] = bmin[1];
46 mMin[2] = bmin[2];
47
48 mMax[0] = bmax[0];
49 mMax[1] = bmax[1];
50 mMax[2] = bmax[2];
51 }
52
53 public void SetMin(float[] bmin)
54 {
55 mMin[0] = bmin[0];
56 mMin[1] = bmin[1];
57 mMin[2] = bmin[2];
58 }
59
60 public void SetMax(float[] bmax)
61 {
62 mMax[0] = bmax[0];
63 mMax[1] = bmax[1];
64 mMax[2] = bmax[2];
65 }
66
67 public void SetMin(float x, float y, float z)
68 {
69 mMin[0] = x;
70 mMin[1] = y;
71 mMin[2] = z;
72 }
73
74 public void SetMax(float x, float y, float z)
75 {
76 mMax[0] = x;
77 mMax[1] = y;
78 mMax[2] = z;
79 }
80 }
81
82 public static class SplitPlane
83 {
84 public static bool computeSplitPlane(List<float3> vertices, List<int> indices, ref float4 plane)
85 {
86 float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue };
87 float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue };
88
89 for (int i = 0; i < vertices.Count; i++)
90 {
91 float3 p = vertices[i];
92
93 if (p[0] < bmin[0])
94 bmin[0] = p[0];
95 if (p[1] < bmin[1])
96 bmin[1] = p[1];
97 if (p[2] < bmin[2])
98 bmin[2] = p[2];
99
100 if (p[0] > bmax[0])
101 bmax[0] = p[0];
102 if (p[1] > bmax[1])
103 bmax[1] = p[1];
104 if (p[2] > bmax[2])
105 bmax[2] = p[2];
106 }
107
108 float dx = bmax[0] - bmin[0];
109 float dy = bmax[1] - bmin[1];
110 float dz = bmax[2] - bmin[2];
111
112 float laxis = dx;
113
114 int axis = 0;
115
116 if (dy > dx)
117 {
118 axis = 1;
119 laxis = dy;
120 }
121
122 if (dz > dx && dz > dy)
123 {
124 axis = 2;
125 laxis = dz;
126 }
127
128 float[] p1 = new float[3];
129 float[] p2 = new float[3];
130 float[] p3 = new float[3];
131
132 p3[0] = p2[0] = p1[0] = bmin[0] + dx * 0.5f;
133 p3[1] = p2[1] = p1[1] = bmin[1] + dy * 0.5f;
134 p3[2] = p2[2] = p1[2] = bmin[2] + dz * 0.5f;
135
136 Rect3d b = new Rect3d(bmin, bmax);
137
138 Rect3d b1 = new Rect3d();
139 Rect3d b2 = new Rect3d();
140
141 splitRect(axis, b, b1, b2, p1);
142
143 switch (axis)
144 {
145 case 0:
146 p2[1] = bmin[1];
147 p2[2] = bmin[2];
148
149 if (dz > dy)
150 {
151 p3[1] = bmax[1];
152 p3[2] = bmin[2];
153 }
154 else
155 {
156 p3[1] = bmin[1];
157 p3[2] = bmax[2];
158 }
159
160 break;
161 case 1:
162 p2[0] = bmin[0];
163 p2[2] = bmin[2];
164
165 if (dx > dz)
166 {
167 p3[0] = bmax[0];
168 p3[2] = bmin[2];
169 }
170 else
171 {
172 p3[0] = bmin[0];
173 p3[2] = bmax[2];
174 }
175
176 break;
177 case 2:
178 p2[0] = bmin[0];
179 p2[1] = bmin[1];
180
181 if (dx > dy)
182 {
183 p3[0] = bmax[0];
184 p3[1] = bmin[1];
185 }
186 else
187 {
188 p3[0] = bmin[0];
189 p3[1] = bmax[1];
190 }
191
192 break;
193 }
194
195 computePlane(p1, p2, p3, plane);
196
197 return true;
198 }
199
200 internal static void computePlane(float[] A, float[] B, float[] C, float4 plane)
201 {
202 float vx = (B[0] - C[0]);
203 float vy = (B[1] - C[1]);
204 float vz = (B[2] - C[2]);
205
206 float wx = (A[0] - B[0]);
207 float wy = (A[1] - B[1]);
208 float wz = (A[2] - B[2]);
209
210 float vw_x = vy * wz - vz * wy;
211 float vw_y = vz * wx - vx * wz;
212 float vw_z = vx * wy - vy * wx;
213
214 float mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
215
216 if (mag < 0.000001f)
217 {
218 mag = 0;
219 }
220 else
221 {
222 mag = 1.0f / mag;
223 }
224
225 float x = vw_x * mag;
226 float y = vw_y * mag;
227 float z = vw_z * mag;
228
229 float D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2]));
230
231 plane.x = x;
232 plane.y = y;
233 plane.z = z;
234 plane.w = D;
235 }
236
237 public static void splitRect(int axis, Rect3d source, Rect3d b1, Rect3d b2, float[] midpoint)
238 {
239 switch (axis)
240 {
241 case 0:
242 b1.SetMin(source.mMin);
243 b1.SetMax(midpoint[0], source.mMax[1], source.mMax[2]);
244
245 b2.SetMin(midpoint[0], source.mMin[1], source.mMin[2]);
246 b2.SetMax(source.mMax);
247 break;
248 case 1:
249 b1.SetMin(source.mMin);
250 b1.SetMax(source.mMax[0], midpoint[1], source.mMax[2]);
251
252 b2.SetMin(source.mMin[0], midpoint[1], source.mMin[2]);
253 b2.SetMax(source.mMax);
254 break;
255 case 2:
256 b1.SetMin(source.mMin);
257 b1.SetMax(source.mMax[0], source.mMax[1], midpoint[2]);
258
259 b2.SetMin(source.mMin[0], source.mMin[1], midpoint[2]);
260 b2.SetMax(source.mMax);
261 break;
262 }
263 }
264 }
265}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs
new file mode 100644
index 0000000..6f17c9f
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs
@@ -0,0 +1,70 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30
31namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
32{
33 public class VertexPool
34 {
35 private List<float3> mVertices = new List<float3>();
36 private Dictionary<float3, int> mIndices = new Dictionary<float3, int>();
37
38 public int getIndex(float3 vtx)
39 {
40 int idx;
41 if (mIndices.TryGetValue(vtx, out idx))
42 return idx;
43
44 idx = mVertices.Count;
45 mVertices.Add(vtx);
46 mIndices.Add(vtx, idx);
47 return idx;
48 }
49
50 public float3 Get(int idx)
51 {
52 return mVertices[idx];
53 }
54
55 public int GetSize()
56 {
57 return mVertices.Count;
58 }
59
60 public List<float3> GetVertices()
61 {
62 return mVertices;
63 }
64
65 public void Clear()
66 {
67 mVertices.Clear();
68 }
69 }
70}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs
new file mode 100644
index 0000000..ce88fc8
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs
@@ -0,0 +1,70 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29
30namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
31{
32 public class float2
33 {
34 public float x;
35 public float y;
36
37 public float2()
38 {
39 }
40
41 public float2(float _x, float _y)
42 {
43 x = _x;
44 y = _y;
45 }
46
47 public float this[int i]
48 {
49 get
50 {
51 switch (i)
52 {
53 case 0: return x;
54 case 1: return y;
55 }
56 throw new ArgumentOutOfRangeException();
57 }
58 }
59
60 public static float2 operator -(float2 a, float2 b)
61 {
62 return new float2(a.x - b.x, a.y - b.y);
63 }
64
65 public static float2 operator +(float2 a, float2 b)
66 {
67 return new float2(a.x + b.x, a.y + b.y);
68 }
69 }
70}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs
new file mode 100644
index 0000000..4389114
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs
@@ -0,0 +1,444 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29
30namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
31{
32 public class float3 : IEquatable<float3>
33 {
34 public float x;
35 public float y;
36 public float z;
37
38 public float3()
39 {
40 x = 0;
41 y = 0;
42 z = 0;
43 }
44
45 public float3(float _x, float _y, float _z)
46 {
47 x = _x;
48 y = _y;
49 z = _z;
50 }
51
52 public float3(float3 f)
53 {
54 x = f.x;
55 y = f.y;
56 z = f.z;
57 }
58
59 public float this[int i]
60 {
61 get
62 {
63 switch (i)
64 {
65 case 0: return x;
66 case 1: return y;
67 case 2: return z;
68 }
69 throw new ArgumentOutOfRangeException();
70 }
71 }
72
73 public float Distance(float3 a)
74 {
75 float3 d = new float3(a.x - x, a.y - y, a.z - z);
76 return d.Length();
77 }
78
79 public float Distance2(float3 a)
80 {
81 float dx = a.x - x;
82 float dy = a.y - y;
83 float dz = a.z - z;
84 return dx * dx + dy * dy + dz * dz;
85 }
86
87 public float Length()
88 {
89 return (float)Math.Sqrt(x * x + y * y + z * z);
90 }
91
92 public float Area(float3 p1, float3 p2)
93 {
94 float A = Partial(p1);
95 A += p1.Partial(p2);
96 A += p2.Partial(this);
97 return A * 0.5f;
98 }
99
100 public float Partial(float3 p)
101 {
102 return (x * p.y) - (p.x * y);
103 }
104
105 // Given a point and a line (defined by two points), compute the closest point
106 // in the line. (The line is treated as infinitely long.)
107 public void NearestPointInLine(float3 point, float3 line0, float3 line1)
108 {
109 float3 nearestPoint = new float3();
110 float3 lineDelta = line1 - line0;
111
112 // Handle degenerate lines
113 if (lineDelta == float3.Zero)
114 {
115 nearestPoint = line0;
116 }
117 else
118 {
119 float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta);
120 nearestPoint = line0 + lineDelta * delta;
121 }
122
123 this.x = nearestPoint.x;
124 this.y = nearestPoint.y;
125 this.z = nearestPoint.z;
126 }
127
128 // Given a point and a line segment (defined by two points), compute the closest point
129 // in the line. Cap the point at the endpoints of the line segment.
130 public void NearestPointInLineSegment(float3 point, float3 line0, float3 line1)
131 {
132 float3 nearestPoint = new float3();
133 float3 lineDelta = line1 - line0;
134
135 // Handle degenerate lines
136 if (lineDelta == Zero)
137 {
138 nearestPoint = line0;
139 }
140 else
141 {
142 float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta);
143
144 // Clamp the point to conform to the segment's endpoints
145 if (delta < 0)
146 delta = 0;
147 else if (delta > 1)
148 delta = 1;
149
150 nearestPoint = line0 + lineDelta * delta;
151 }
152
153 this.x = nearestPoint.x;
154 this.y = nearestPoint.y;
155 this.z = nearestPoint.z;
156 }
157
158 // Given a point and a triangle (defined by three points), compute the closest point
159 // in the triangle. Clamp the point so it's confined to the area of the triangle.
160 public void NearestPointInTriangle(float3 point, float3 triangle0, float3 triangle1, float3 triangle2)
161 {
162 float3 nearestPoint = new float3();
163
164 float3 lineDelta0 = triangle1 - triangle0;
165 float3 lineDelta1 = triangle2 - triangle0;
166
167 // Handle degenerate triangles
168 if ((lineDelta0 == Zero) || (lineDelta1 == Zero))
169 {
170 nearestPoint.NearestPointInLineSegment(point, triangle1, triangle2);
171 }
172 else if (lineDelta0 == lineDelta1)
173 {
174 nearestPoint.NearestPointInLineSegment(point, triangle0, triangle1);
175 }
176 else
177 {
178 float3[] axis = new float3[3] { new float3(), new float3(), new float3() };
179 axis[0].NearestPointInLine(triangle0, triangle1, triangle2);
180 axis[1].NearestPointInLine(triangle1, triangle0, triangle2);
181 axis[2].NearestPointInLine(triangle2, triangle0, triangle1);
182
183 float3 axisDot = new float3();
184 axisDot.x = dot(triangle0 - axis[0], point - axis[0]);
185 axisDot.y = dot(triangle1 - axis[1], point - axis[1]);
186 axisDot.z = dot(triangle2 - axis[2], point - axis[2]);
187
188 bool bForce = true;
189 float bestMagnitude2 = 0;
190 float closeMagnitude2;
191 float3 closePoint = new float3();
192
193 if (axisDot.x < 0f)
194 {
195 closePoint.NearestPointInLineSegment(point, triangle1, triangle2);
196 closeMagnitude2 = point.Distance2(closePoint);
197 if (bForce || (bestMagnitude2 > closeMagnitude2))
198 {
199 bForce = false;
200 bestMagnitude2 = closeMagnitude2;
201 nearestPoint = closePoint;
202 }
203 }
204 if (axisDot.y < 0f)
205 {
206 closePoint.NearestPointInLineSegment(point, triangle0, triangle2);
207 closeMagnitude2 = point.Distance2(closePoint);
208 if (bForce || (bestMagnitude2 > closeMagnitude2))
209 {
210 bForce = false;
211 bestMagnitude2 = closeMagnitude2;
212 nearestPoint = closePoint;
213 }
214 }
215 if (axisDot.z < 0f)
216 {
217 closePoint.NearestPointInLineSegment(point, triangle0, triangle1);
218 closeMagnitude2 = point.Distance2(closePoint);
219 if (bForce || (bestMagnitude2 > closeMagnitude2))
220 {
221 bForce = false;
222 bestMagnitude2 = closeMagnitude2;
223 nearestPoint = closePoint;
224 }
225 }
226
227 // If bForce is true at this point, it means the nearest point lies
228 // inside the triangle; use the nearest-point-on-a-plane equation
229 if (bForce)
230 {
231 float3 normal;
232
233 // Get the normal of the polygon (doesn't have to be a unit vector)
234 normal = float3.cross(lineDelta0, lineDelta1);
235
236 float3 pointDelta = point - triangle0;
237 float delta = float3.dot(normal, pointDelta) / float3.dot(normal, normal);
238
239 nearestPoint = point - normal * delta;
240 }
241 }
242
243 this.x = nearestPoint.x;
244 this.y = nearestPoint.y;
245 this.z = nearestPoint.z;
246 }
247
248 public static float3 operator +(float3 a, float3 b)
249 {
250 return new float3(a.x + b.x, a.y + b.y, a.z + b.z);
251 }
252
253 public static float3 operator -(float3 a, float3 b)
254 {
255 return new float3(a.x - b.x, a.y - b.y, a.z - b.z);
256 }
257
258 public static float3 operator -(float3 a, float s)
259 {
260 return new float3(a.x - s, a.y - s, a.z - s);
261 }
262
263 public static float3 operator -(float3 v)
264 {
265 return new float3(-v.x, -v.y, -v.z);
266 }
267
268 public static float3 operator *(float3 v, float s)
269 {
270 return new float3(v.x * s, v.y * s, v.z * s);
271 }
272
273 public static float3 operator *(float s, float3 v)
274 {
275 return new float3(v.x * s, v.y * s, v.z * s);
276 }
277
278 public static float3 operator *(float3 v, float3x3 m)
279 {
280 return new float3((m.x.x * v.x + m.y.x * v.y + m.z.x * v.z), (m.x.y * v.x + m.y.y * v.y + m.z.y * v.z), (m.x.z * v.x + m.y.z * v.y + m.z.z * v.z));
281 }
282
283 public static float3 operator *(float3x3 m, float3 v)
284 {
285 return new float3(dot(m.x, v), dot(m.y, v), dot(m.z, v));
286 }
287
288 public static float3 operator /(float3 v, float s)
289 {
290 float sinv = 1.0f / s;
291 return new float3(v.x * sinv, v.y * sinv, v.z * sinv);
292 }
293
294 public bool Equals(float3 other)
295 {
296 return this == other;
297 }
298
299 public override bool Equals(object obj)
300 {
301 float3 f = obj as float3;
302 if (f == null)
303 return false;
304
305 return this == f;
306 }
307
308 public override int GetHashCode()
309 {
310 return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode();
311 }
312
313 public static bool operator ==(float3 a, float3 b)
314 {
315 // If both are null, or both are same instance, return true.
316 if (System.Object.ReferenceEquals(a, b))
317 return true;
318 // If one is null, but not both, return false.
319 if (((object)a == null) || ((object)b == null))
320 return false;
321
322 return (a.x == b.x && a.y == b.y && a.z == b.z);
323 }
324
325 public static bool operator !=(float3 a, float3 b)
326 {
327 return (a.x != b.x || a.y != b.y || a.z != b.z);
328 }
329
330 public static float dot(float3 a, float3 b)
331 {
332 return a.x * b.x + a.y * b.y + a.z * b.z;
333 }
334
335 public static float3 cmul(float3 v1, float3 v2)
336 {
337 return new float3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
338 }
339
340 public static float3 cross(float3 a, float3 b)
341 {
342 return new float3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
343 }
344
345 public static float3 Interpolate(float3 v0, float3 v1, float alpha)
346 {
347 return v0 * (1 - alpha) + v1 * alpha;
348 }
349
350 public static float3 Round(float3 a, int digits)
351 {
352 return new float3((float)Math.Round(a.x, digits), (float)Math.Round(a.y, digits), (float)Math.Round(a.z, digits));
353 }
354
355 public static float3 VectorMax(float3 a, float3 b)
356 {
357 return new float3(Math.Max(a.x, b.x), Math.Max(a.y, b.y), Math.Max(a.z, b.z));
358 }
359
360 public static float3 VectorMin(float3 a, float3 b)
361 {
362 return new float3(Math.Min(a.x, b.x), Math.Min(a.y, b.y), Math.Min(a.z, b.z));
363 }
364
365 public static float3 vabs(float3 v)
366 {
367 return new float3(Math.Abs(v.x), Math.Abs(v.y), Math.Abs(v.z));
368 }
369
370 public static float magnitude(float3 v)
371 {
372 return (float)Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
373 }
374
375 public static float3 normalize(float3 v)
376 {
377 float d = magnitude(v);
378 if (d == 0)
379 d = 0.1f;
380 d = 1 / d;
381 return new float3(v.x * d, v.y * d, v.z * d);
382 }
383
384 public static float3 safenormalize(float3 v)
385 {
386 if (magnitude(v) <= 0.0f)
387 return new float3(1, 0, 0);
388 else
389 return normalize(v);
390 }
391
392 public static float Yaw(float3 v)
393 {
394 return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f);
395 }
396
397 public static float Pitch(float3 v)
398 {
399 return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f);
400 }
401
402 public float ComputePlane(float3 A, float3 B, float3 C)
403 {
404 float vx, vy, vz, wx, wy, wz, vw_x, vw_y, vw_z, mag;
405
406 vx = (B.x - C.x);
407 vy = (B.y - C.y);
408 vz = (B.z - C.z);
409
410 wx = (A.x - B.x);
411 wy = (A.y - B.y);
412 wz = (A.z - B.z);
413
414 vw_x = vy * wz - vz * wy;
415 vw_y = vz * wx - vx * wz;
416 vw_z = vx * wy - vy * wx;
417
418 mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
419
420 if (mag < 0.000001f)
421 {
422 mag = 0;
423 }
424 else
425 {
426 mag = 1.0f / mag;
427 }
428
429 x = vw_x * mag;
430 y = vw_y * mag;
431 z = vw_z * mag;
432
433 float D = 0.0f - ((x * A.x) + (y * A.y) + (z * A.z));
434 return D;
435 }
436
437 public override string ToString()
438 {
439 return String.Format("<{0}, {1}, {2}>", x, y, z);
440 }
441
442 public static readonly float3 Zero = new float3();
443 }
444}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs
new file mode 100644
index 0000000..76cf063
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs
@@ -0,0 +1,195 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Diagnostics;
31
32namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
33{
34 public class float3x3
35 {
36 public float3 x = new float3();
37 public float3 y = new float3();
38 public float3 z = new float3();
39
40 public float3x3()
41 {
42 }
43
44 public float3x3(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz)
45 {
46 x = new float3(xx, xy, xz);
47 y = new float3(yx, yy, yz);
48 z = new float3(zx, zy, zz);
49 }
50
51 public float3x3(float3 _x, float3 _y, float3 _z)
52 {
53 x = new float3(_x);
54 y = new float3(_y);
55 z = new float3(_z);
56 }
57
58 public float3 this[int i]
59 {
60 get
61 {
62 switch (i)
63 {
64 case 0: return x;
65 case 1: return y;
66 case 2: return z;
67 }
68 throw new ArgumentOutOfRangeException();
69 }
70 }
71
72 public float this[int i, int j]
73 {
74 get
75 {
76 switch (i)
77 {
78 case 0:
79 switch (j)
80 {
81 case 0: return x.x;
82 case 1: return x.y;
83 case 2: return x.z;
84 }
85 break;
86 case 1:
87 switch (j)
88 {
89 case 0: return y.x;
90 case 1: return y.y;
91 case 2: return y.z;
92 }
93 break;
94 case 2:
95 switch (j)
96 {
97 case 0: return z.x;
98 case 1: return z.y;
99 case 2: return z.z;
100 }
101 break;
102 }
103 throw new ArgumentOutOfRangeException();
104 }
105 set
106 {
107 switch (i)
108 {
109 case 0:
110 switch (j)
111 {
112 case 0: x.x = value; return;
113 case 1: x.y = value; return;
114 case 2: x.z = value; return;
115 }
116 break;
117 case 1:
118 switch (j)
119 {
120 case 0: y.x = value; return;
121 case 1: y.y = value; return;
122 case 2: y.z = value; return;
123 }
124 break;
125 case 2:
126 switch (j)
127 {
128 case 0: z.x = value; return;
129 case 1: z.y = value; return;
130 case 2: z.z = value; return;
131 }
132 break;
133 }
134 throw new ArgumentOutOfRangeException();
135 }
136 }
137
138 public static float3x3 Transpose(float3x3 m)
139 {
140 return new float3x3(new float3(m.x.x, m.y.x, m.z.x), new float3(m.x.y, m.y.y, m.z.y), new float3(m.x.z, m.y.z, m.z.z));
141 }
142
143 public static float3x3 operator *(float3x3 a, float3x3 b)
144 {
145 return new float3x3(a.x * b, a.y * b, a.z * b);
146 }
147
148 public static float3x3 operator *(float3x3 a, float s)
149 {
150 return new float3x3(a.x * s, a.y * s, a.z * s);
151 }
152
153 public static float3x3 operator /(float3x3 a, float s)
154 {
155 float t = 1f / s;
156 return new float3x3(a.x * t, a.y * t, a.z * t);
157 }
158
159 public static float3x3 operator +(float3x3 a, float3x3 b)
160 {
161 return new float3x3(a.x + b.x, a.y + b.y, a.z + b.z);
162 }
163
164 public static float3x3 operator -(float3x3 a, float3x3 b)
165 {
166 return new float3x3(a.x - b.x, a.y - b.y, a.z - b.z);
167 }
168
169 public static float Determinant(float3x3 m)
170 {
171 return m.x.x * m.y.y * m.z.z + m.y.x * m.z.y * m.x.z + m.z.x * m.x.y * m.y.z - m.x.x * m.z.y * m.y.z - m.y.x * m.x.y * m.z.z - m.z.x * m.y.y * m.x.z;
172 }
173
174 public static float3x3 Inverse(float3x3 a)
175 {
176 float3x3 b = new float3x3();
177 float d = Determinant(a);
178 Debug.Assert(d != 0);
179 for (int i = 0; i < 3; i++)
180 {
181 for (int j = 0; j < 3; j++)
182 {
183 int i1 = (i + 1) % 3;
184 int i2 = (i + 2) % 3;
185 int j1 = (j + 1) % 3;
186 int j2 = (j + 2) % 3;
187
188 // reverse indexs i&j to take transpose
189 b[i, j] = (a[i1][j1] * a[i2][j2] - a[i1][j2] * a[i2][j1]) / d;
190 }
191 }
192 return b;
193 }
194 }
195}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs
new file mode 100644
index 0000000..fa60876
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs
@@ -0,0 +1,170 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29
30namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
31{
32 public class float4
33 {
34 public float x;
35 public float y;
36 public float z;
37 public float w;
38
39 public float4()
40 {
41 x = 0;
42 y = 0;
43 z = 0;
44 w = 0;
45 }
46
47 public float4(float _x, float _y, float _z, float _w)
48 {
49 x = _x;
50 y = _y;
51 z = _z;
52 w = _w;
53 }
54
55 public float4(float3 v, float _w)
56 {
57 x = v.x;
58 y = v.y;
59 z = v.z;
60 w = _w;
61 }
62
63 public float4(float4 f)
64 {
65 x = f.x;
66 y = f.y;
67 z = f.z;
68 w = f.w;
69 }
70
71 public float this[int i]
72 {
73 get
74 {
75 switch (i)
76 {
77 case 0: return x;
78 case 1: return y;
79 case 2: return z;
80 case 3: return w;
81 }
82 throw new ArgumentOutOfRangeException();
83 }
84 }
85
86 public float3 xyz()
87 {
88 return new float3(x, y, z);
89 }
90
91 public void setxyz(float3 xyz)
92 {
93 x = xyz.x;
94 y = xyz.y;
95 z = xyz.z;
96 }
97
98 public override int GetHashCode()
99 {
100 return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode();
101 }
102
103 public override bool Equals(object obj)
104 {
105 float4 f = obj as float4;
106 if (f == null)
107 return false;
108
109 return this == f;
110 }
111
112 public static float4 Homogenize(float3 v3)
113 {
114 return Homogenize(v3, 1.0f);
115 }
116
117 //C++ TO C# CONVERTER NOTE: C# does not allow default values for parameters. Overloaded methods are inserted above.
118 //ORIGINAL LINE: float4 Homogenize(const float3 &v3, const float &w =1.0f)
119 public static float4 Homogenize(float3 v3, float w)
120 {
121 return new float4(v3.x, v3.y, v3.z, w);
122 }
123
124 public static float4 cmul(float4 a, float4 b)
125 {
126 return new float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
127 }
128
129 public static float4 operator +(float4 a, float4 b)
130 {
131 return new float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
132 }
133 public static float4 operator -(float4 a, float4 b)
134 {
135 return new float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
136 }
137
138 public static float4 operator *(float4 v, float4x4 m)
139 {
140 return v.x * m.x + v.y * m.y + v.z * m.z + v.w * m.w; // yes this actually works
141 }
142
143 public static bool operator ==(float4 a, float4 b)
144 {
145 // If both are null, or both are same instance, return true.
146 if (System.Object.ReferenceEquals(a, b))
147 return true;
148 // If one is null, but not both, return false.
149 if (((object)a == null) || ((object)b == null))
150 return false;
151
152 return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
153 }
154
155 public static bool operator !=(float4 a, float4 b)
156 {
157 return !(a == b);
158 }
159
160 public static float4 operator *(float4 v, float s)
161 {
162 return new float4(v.x * s, v.y * s, v.z * s, v.w * s);
163 }
164
165 public static float4 operator *(float s, float4 v)
166 {
167 return new float4(v.x * s, v.y * s, v.z * s, v.w * s);
168 }
169 }
170}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs
new file mode 100644
index 0000000..7d1592f
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs
@@ -0,0 +1,284 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Linq;
31using System.Text;
32
33namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
34{
35 public class float4x4
36 {
37 public float4 x = new float4();
38 public float4 y = new float4();
39 public float4 z = new float4();
40 public float4 w = new float4();
41
42 public float4x4()
43 {
44 }
45
46 public float4x4(float4 _x, float4 _y, float4 _z, float4 _w)
47 {
48 x = new float4(_x);
49 y = new float4(_y);
50 z = new float4(_z);
51 w = new float4(_w);
52 }
53
54 public float4x4(
55 float m00, float m01, float m02, float m03,
56 float m10, float m11, float m12, float m13,
57 float m20, float m21, float m22, float m23,
58 float m30, float m31, float m32, float m33)
59 {
60 x = new float4(m00, m01, m02, m03);
61 y = new float4(m10, m11, m12, m13);
62 z = new float4(m20, m21, m22, m23);
63 w = new float4(m30, m31, m32, m33);
64 }
65
66 public float4x4(float4x4 m)
67 {
68 x = new float4(m.x);
69 y = new float4(m.y);
70 z = new float4(m.z);
71 w = new float4(m.w);
72 }
73
74 public float4 this[int i]
75 {
76 get
77 {
78 switch (i)
79 {
80 case 0: return x;
81 case 1: return y;
82 case 2: return z;
83 case 3: return w;
84 }
85 throw new ArgumentOutOfRangeException();
86 }
87 set
88 {
89 switch (i)
90 {
91 case 0: x = value; return;
92 case 1: y = value; return;
93 case 2: z = value; return;
94 case 3: w = value; return;
95 }
96 throw new ArgumentOutOfRangeException();
97 }
98 }
99
100 public override int GetHashCode()
101 {
102 return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode();
103 }
104
105 public override bool Equals(object obj)
106 {
107 float4x4 m = obj as float4x4;
108 if (m == null)
109 return false;
110
111 return this == m;
112 }
113
114 public static float4x4 operator *(float4x4 a, float4x4 b)
115 {
116 return new float4x4(a.x * b, a.y * b, a.z * b, a.w * b);
117 }
118
119 public static bool operator ==(float4x4 a, float4x4 b)
120 {
121 return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
122 }
123
124 public static bool operator !=(float4x4 a, float4x4 b)
125 {
126 return !(a == b);
127 }
128
129 public static float4x4 Inverse(float4x4 m)
130 {
131 float4x4 d = new float4x4();
132 //float dst = d.x.x;
133 float[] tmp = new float[12]; // temp array for pairs
134 float[] src = new float[16]; // array of transpose source matrix
135 float det; // determinant
136 // transpose matrix
137 for (int i = 0; i < 4; i++)
138 {
139 src[i] = m[i].x;
140 src[i + 4] = m[i].y;
141 src[i + 8] = m[i].z;
142 src[i + 12] = m[i].w;
143 }
144 // calculate pairs for first 8 elements (cofactors)
145 tmp[0] = src[10] * src[15];
146 tmp[1] = src[11] * src[14];
147 tmp[2] = src[9] * src[15];
148 tmp[3] = src[11] * src[13];
149 tmp[4] = src[9] * src[14];
150 tmp[5] = src[10] * src[13];
151 tmp[6] = src[8] * src[15];
152 tmp[7] = src[11] * src[12];
153 tmp[8] = src[8] * src[14];
154 tmp[9] = src[10] * src[12];
155 tmp[10] = src[8] * src[13];
156 tmp[11] = src[9] * src[12];
157 // calculate first 8 elements (cofactors)
158 d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
159 d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
160 d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
161 d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
162 d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
163 d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
164 d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
165 d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
166 d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
167 d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
168 d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
169 d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
170 d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
171 d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
172 d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
173 d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
174 // calculate pairs for second 8 elements (cofactors)
175 tmp[0] = src[2]*src[7];
176 tmp[1] = src[3]*src[6];
177 tmp[2] = src[1]*src[7];
178 tmp[3] = src[3]*src[5];
179 tmp[4] = src[1]*src[6];
180 tmp[5] = src[2]*src[5];
181 tmp[6] = src[0]*src[7];
182 tmp[7] = src[3]*src[4];
183 tmp[8] = src[0]*src[6];
184 tmp[9] = src[2]*src[4];
185 tmp[10] = src[0]*src[5];
186 tmp[11] = src[1]*src[4];
187 // calculate second 8 elements (cofactors)
188 d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
189 d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
190 d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
191 d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
192 d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
193 d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
194 d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
195 d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
196 d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
197 d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
198 d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
199 d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
200 d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
201 d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
202 d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
203 d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
204 // calculate determinant
205 det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w;
206 // calculate matrix inverse
207 det = 1/det;
208 for (int j = 0; j < 4; j++)
209 d[j] *= det;
210 return d;
211 }
212
213 public static float4x4 MatrixRigidInverse(float4x4 m)
214 {
215 float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
216 float4x4 rot = new float4x4(m);
217 rot.w = new float4(0f, 0f, 0f, 1f);
218 return trans_inverse * MatrixTranspose(rot);
219 }
220 public static float4x4 MatrixTranspose(float4x4 m)
221 {
222 return new float4x4(m.x.x, m.y.x, m.z.x, m.w.x, m.x.y, m.y.y, m.z.y, m.w.y, m.x.z, m.y.z, m.z.z, m.w.z, m.x.w, m.y.w, m.z.w, m.w.w);
223 }
224 public static float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf)
225 {
226 float h = 1.0f / (float)Math.Tan(fovy / 2.0f); // view space height
227 float w = h / aspect; // view space width
228 return new float4x4(w, 0, 0, 0, 0, h, 0, 0, 0, 0, zf / (zn - zf), -1, 0, 0, zn * zf / (zn - zf), 0);
229 }
230 public static float4x4 MatrixTranslation(float3 t)
231 {
232 return new float4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1);
233 }
234 public static float4x4 MatrixRotationZ(float angle_radians)
235 {
236 float s = (float)Math.Sin(angle_radians);
237 float c = (float)Math.Cos(angle_radians);
238 return new float4x4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
239 }
240 public static float4x4 MatrixLookAt(float3 eye, float3 at, float3 up)
241 {
242 float4x4 m = new float4x4();
243 m.w.w = 1.0f;
244 m.w.setxyz(eye);
245 m.z.setxyz(float3.normalize(eye - at));
246 m.x.setxyz(float3.normalize(float3.cross(up, m.z.xyz())));
247 m.y.setxyz(float3.cross(m.z.xyz(), m.x.xyz()));
248 return MatrixRigidInverse(m);
249 }
250
251 public static float4x4 MatrixFromQuatVec(Quaternion q, float3 v)
252 {
253 // builds a 4x4 transformation matrix based on orientation q and translation v
254 float qx2 = q.x * q.x;
255 float qy2 = q.y * q.y;
256 float qz2 = q.z * q.z;
257
258 float qxqy = q.x * q.y;
259 float qxqz = q.x * q.z;
260 float qxqw = q.x * q.w;
261 float qyqz = q.y * q.z;
262 float qyqw = q.y * q.w;
263 float qzqw = q.z * q.w;
264
265 return new float4x4(
266 1 - 2 * (qy2 + qz2),
267 2 * (qxqy + qzqw),
268 2 * (qxqz - qyqw),
269 0,
270 2 * (qxqy - qzqw),
271 1 - 2 * (qx2 + qz2),
272 2 * (qyqz + qxqw),
273 0,
274 2 * (qxqz + qyqw),
275 2 * (qyqz - qxqw),
276 1 - 2 * (qx2 + qy2),
277 0,
278 v.x,
279 v.y,
280 v.z,
281 1.0f);
282 }
283 }
284}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs
new file mode 100644
index 0000000..9c5760d
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs
@@ -0,0 +1,128 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29
30namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
31{
32 public class int3
33 {
34 public int x;
35 public int y;
36 public int z;
37
38 public int3()
39 {
40 }
41
42 public int3(int _x, int _y, int _z)
43 {
44 x = _x;
45 y = _y;
46 z = _z;
47 }
48
49 public int this[int i]
50 {
51 get
52 {
53 switch (i)
54 {
55 case 0: return x;
56 case 1: return y;
57 case 2: return z;
58 }
59 throw new ArgumentOutOfRangeException();
60 }
61 set
62 {
63 switch (i)
64 {
65 case 0: x = value; return;
66 case 1: y = value; return;
67 case 2: z = value; return;
68 }
69 throw new ArgumentOutOfRangeException();
70 }
71 }
72
73 public override int GetHashCode()
74 {
75 return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode();
76 }
77
78 public override bool Equals(object obj)
79 {
80 int3 i = obj as int3;
81 if (i == null)
82 return false;
83
84 return this == i;
85 }
86
87 public static bool operator ==(int3 a, int3 b)
88 {
89 // If both are null, or both are same instance, return true.
90 if (System.Object.ReferenceEquals(a, b))
91 return true;
92 // If one is null, but not both, return false.
93 if (((object)a == null) || ((object)b == null))
94 return false;
95
96 for (int i = 0; i < 3; i++)
97 {
98 if (a[i] != b[i])
99 return false;
100 }
101 return true;
102 }
103
104 public static bool operator !=(int3 a, int3 b)
105 {
106 return !(a == b);
107 }
108
109 public static int3 roll3(int3 a)
110 {
111 int tmp = a[0];
112 a[0] = a[1];
113 a[1] = a[2];
114 a[2] = tmp;
115 return a;
116 }
117
118 public static bool isa(int3 a, int3 b)
119 {
120 return (a == b || roll3(a) == b || a == roll3(b));
121 }
122
123 public static bool b2b(int3 a, int3 b)
124 {
125 return isa(a, new int3(b[2], b[1], b[0]));
126 }
127 }
128}
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs
new file mode 100644
index 0000000..c2b32e5
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs
@@ -0,0 +1,66 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29
30namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
31{
32 public class int4
33 {
34 public int x;
35 public int y;
36 public int z;
37 public int w;
38
39 public int4()
40 {
41 }
42
43 public int4(int _x, int _y, int _z, int _w)
44 {
45 x = _x;
46 y = _y;
47 z = _z;
48 w = _w;
49 }
50
51 public int this[int i]
52 {
53 get
54 {
55 switch (i)
56 {
57 case 0: return x;
58 case 1: return y;
59 case 2: return z;
60 case 3: return w;
61 }
62 throw new ArgumentOutOfRangeException();
63 }
64 }
65 }
66}