OpenSim
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Events Macros
HullUtils.cs
Go to the documentation of this file.
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.PhysicsModules.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++;
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 on Wed Apr 6 2016 04:34:49 for OpenSim by   doxygen 1.8.6