diff options
Diffstat (limited to 'OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs | 1868 |
1 files changed, 1868 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs new file mode 100644 index 0000000..c9ccfe2 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/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 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace 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 | } | ||