aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs1868
1 files changed, 1868 insertions, 0 deletions
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}
generated by cgit v1.1 at 2024-12-14 18:44:44 +0000