opensim-development – Blame information for rev 1
?pathlinks?
Rev | Author | Line No. | Line |
---|---|---|---|
1 | eva | 1 | /* The MIT License |
2 | * |
||
3 | * Copyright (c) 2010 Intel Corporation. |
||
4 | * All rights reserved. |
||
5 | * |
||
6 | * Based on the convexdecomposition library from |
||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. |
||
8 | * |
||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
||
10 | * of this software and associated documentation files (the "Software"), to deal |
||
11 | * in the Software without restriction, including without limitation the rights |
||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
||
13 | * copies of the Software, and to permit persons to whom the Software is |
||
14 | * furnished to do so, subject to the following conditions: |
||
15 | * |
||
16 | * The above copyright notice and this permission notice shall be included in |
||
17 | * all copies or substantial portions of the Software. |
||
18 | * |
||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
||
25 | * THE SOFTWARE. |
||
26 | */ |
||
27 | |||
28 | using System; |
||
29 | using System.Collections.Generic; |
||
30 | using System.Diagnostics; |
||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet |
||
33 | { |
||
34 | public static class HullUtils |
||
35 | { |
||
36 | public static int argmin(float[] a, int n) |
||
37 | { |
||
38 | int r = 0; |
||
39 | for (int i = 1; i < n; i++) |
||
40 | { |
||
41 | if (a[i] < a[r]) |
||
42 | { |
||
43 | r = i; |
||
44 | } |
||
45 | } |
||
46 | return r; |
||
47 | } |
||
48 | |||
49 | public static float clampf(float a) |
||
50 | { |
||
51 | return Math.Min(1.0f, Math.Max(0.0f, a)); |
||
52 | } |
||
53 | |||
54 | public static float Round(float a, float precision) |
||
55 | { |
||
56 | return (float)Math.Floor(0.5f + a / precision) * precision; |
||
57 | } |
||
58 | |||
59 | public static float Interpolate(float f0, float f1, float alpha) |
||
60 | { |
||
61 | return f0 * (1 - alpha) + f1 * alpha; |
||
62 | } |
||
63 | |||
64 | public static void Swap<T>(ref T a, ref T b) |
||
65 | { |
||
66 | T tmp = a; |
||
67 | a = b; |
||
68 | b = tmp; |
||
69 | } |
||
70 | |||
71 | public static bool above(List<float3> vertices, int3 t, float3 p, float epsilon) |
||
72 | { |
||
73 | float3 vtx = vertices[t.x]; |
||
74 | float3 n = TriNormal(vtx, vertices[t.y], vertices[t.z]); |
||
75 | return (float3.dot(n, p - vtx) > epsilon); // EPSILON??? |
||
76 | } |
||
77 | |||
78 | public static int hasedge(int3 t, int a, int b) |
||
79 | { |
||
80 | for (int i = 0; i < 3; i++) |
||
81 | { |
||
82 | int i1 = (i + 1) % 3; |
||
83 | if (t[i] == a && t[i1] == b) |
||
84 | return 1; |
||
85 | } |
||
86 | return 0; |
||
87 | } |
||
88 | |||
89 | public static bool hasvert(int3 t, int v) |
||
90 | { |
||
91 | return (t[0] == v || t[1] == v || t[2] == v); |
||
92 | } |
||
93 | |||
94 | public static int shareedge(int3 a, int3 b) |
||
95 | { |
||
96 | int i; |
||
97 | for (i = 0; i < 3; i++) |
||
98 | { |
||
99 | int i1 = (i + 1) % 3; |
||
100 | if (hasedge(a, b[i1], b[i]) != 0) |
||
101 | return 1; |
||
102 | } |
||
103 | return 0; |
||
104 | } |
||
105 | |||
106 | public static void b2bfix(HullTriangle s, HullTriangle t, List<HullTriangle> tris) |
||
107 | { |
||
108 | int i; |
||
109 | for (i = 0; i < 3; i++) |
||
110 | { |
||
111 | int i1 = (i + 1) % 3; |
||
112 | int i2 = (i + 2) % 3; |
||
113 | int a = (s)[i1]; |
||
114 | int b = (s)[i2]; |
||
115 | Debug.Assert(tris[s.neib(a, b)].neib(b, a) == s.id); |
||
116 | Debug.Assert(tris[t.neib(a, b)].neib(b, a) == t.id); |
||
117 | tris[s.neib(a, b)].setneib(b, a, t.neib(b, a)); |
||
118 | tris[t.neib(b, a)].setneib(a, b, s.neib(a, b)); |
||
119 | } |
||
120 | } |
||
121 | |||
122 | public static void removeb2b(HullTriangle s, HullTriangle t, List<HullTriangle> tris) |
||
123 | { |
||
124 | b2bfix(s, t, tris); |
||
125 | s.Dispose(); |
||
126 | t.Dispose(); |
||
127 | } |
||
128 | |||
129 | public static void checkit(HullTriangle t, List<HullTriangle> tris) |
||
130 | { |
||
131 | int i; |
||
132 | Debug.Assert(tris[t.id] == t); |
||
133 | for (i = 0; i < 3; i++) |
||
134 | { |
||
135 | int i1 = (i + 1) % 3; |
||
136 | int i2 = (i + 2) % 3; |
||
137 | int a = (t)[i1]; |
||
138 | int b = (t)[i2]; |
||
139 | Debug.Assert(a != b); |
||
140 | Debug.Assert(tris[t.n[i]].neib(b, a) == t.id); |
||
141 | } |
||
142 | } |
||
143 | |||
144 | public static void extrude(HullTriangle t0, int v, List<HullTriangle> tris) |
||
145 | { |
||
146 | int3 t = t0; |
||
147 | int n = tris.Count; |
||
148 | HullTriangle ta = new HullTriangle(v, t[1], t[2], tris); |
||
149 | ta.n = new int3(t0.n[0], n + 1, n + 2); |
||
150 | tris[t0.n[0]].setneib(t[1], t[2], n + 0); |
||
151 | HullTriangle tb = new HullTriangle(v, t[2], t[0], tris); |
||
152 | tb.n = new int3(t0.n[1], n + 2, n + 0); |
||
153 | tris[t0.n[1]].setneib(t[2], t[0], n + 1); |
||
154 | HullTriangle tc = new HullTriangle(v, t[0], t[1], tris); |
||
155 | tc.n = new int3(t0.n[2], n + 0, n + 1); |
||
156 | tris[t0.n[2]].setneib(t[0], t[1], n + 2); |
||
157 | checkit(ta, tris); |
||
158 | checkit(tb, tris); |
||
159 | checkit(tc, tris); |
||
160 | if (hasvert(tris[ta.n[0]], v)) |
||
161 | removeb2b(ta, tris[ta.n[0]], tris); |
||
162 | if (hasvert(tris[tb.n[0]], v)) |
||
163 | removeb2b(tb, tris[tb.n[0]], tris); |
||
164 | if (hasvert(tris[tc.n[0]], v)) |
||
165 | removeb2b(tc, tris[tc.n[0]], tris); |
||
166 | t0.Dispose(); |
||
167 | } |
||
168 | |||
169 | public static HullTriangle extrudable(float epsilon, List<HullTriangle> tris) |
||
170 | { |
||
171 | int i; |
||
172 | HullTriangle t = null; |
||
173 | for (i = 0; i < tris.Count; i++) |
||
174 | { |
||
175 | if (t == null || (tris.Count > i && (object)tris[i] != null && t.rise < tris[i].rise)) |
||
176 | { |
||
177 | t = tris[i]; |
||
178 | } |
||
179 | } |
||
180 | return (t.rise > epsilon) ? t : null; |
||
181 | } |
||
182 | |||
183 | public static Quaternion RotationArc(float3 v0, float3 v1) |
||
184 | { |
||
185 | Quaternion q = new Quaternion(); |
||
186 | v0 = float3.normalize(v0); // Comment these two lines out if you know its not needed. |
||
187 | v1 = float3.normalize(v1); // If vector is already unit length then why do it again? |
||
188 | float3 c = float3.cross(v0, v1); |
||
189 | float d = float3.dot(v0, v1); |
||
190 | if (d <= -1.0f) // 180 about x axis |
||
191 | { |
||
192 | return new Quaternion(1f, 0f, 0f, 0f); |
||
193 | } |
||
194 | float s = (float)Math.Sqrt((1 + d) * 2f); |
||
195 | q.x = c.x / s; |
||
196 | q.y = c.y / s; |
||
197 | q.z = c.z / s; |
||
198 | q.w = s / 2.0f; |
||
199 | return q; |
||
200 | } |
||
201 | |||
202 | public static float3 PlaneLineIntersection(Plane plane, float3 p0, float3 p1) |
||
203 | { |
||
204 | // returns the point where the line p0-p1 intersects the plane n&d |
||
205 | float3 dif = p1 - p0; |
||
206 | float dn = float3.dot(plane.normal, dif); |
||
207 | float t = -(plane.dist + float3.dot(plane.normal, p0)) / dn; |
||
208 | return p0 + (dif * t); |
||
209 | } |
||
210 | |||
211 | public static float3 LineProject(float3 p0, float3 p1, float3 a) |
||
212 | { |
||
213 | float3 w = new float3(); |
||
214 | w = p1 - p0; |
||
215 | float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z); |
||
216 | return p0 + w * t; |
||
217 | } |
||
218 | |||
219 | public static float3 PlaneProject(Plane plane, float3 point) |
||
220 | { |
||
221 | return point - plane.normal * (float3.dot(point, plane.normal) + plane.dist); |
||
222 | } |
||
223 | |||
224 | public static float LineProjectTime(float3 p0, float3 p1, float3 a) |
||
225 | { |
||
226 | float3 w = new float3(); |
||
227 | w = p1 - p0; |
||
228 | float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z); |
||
229 | return t; |
||
230 | } |
||
231 | |||
232 | public static float3 ThreePlaneIntersection(Plane p0, Plane p1, Plane p2) |
||
233 | { |
||
234 | float3x3 mp = float3x3.Transpose(new float3x3(p0.normal, p1.normal, p2.normal)); |
||
235 | float3x3 mi = float3x3.Inverse(mp); |
||
236 | float3 b = new float3(p0.dist, p1.dist, p2.dist); |
||
237 | return -b * mi; |
||
238 | } |
||
239 | |||
240 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1) |
||
241 | { |
||
242 | float3 impact = new float3(); |
||
243 | float3 normal = new float3(); |
||
244 | return PolyHit(vert, v0, v1, out impact, out normal); |
||
245 | } |
||
246 | |||
247 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1, out float3 impact) |
||
248 | { |
||
249 | float3 normal = new float3(); |
||
250 | return PolyHit(vert, v0, v1, out impact, out normal); |
||
251 | } |
||
252 | |||
253 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1, out float3 impact, out float3 normal) |
||
254 | { |
||
255 | float3 the_point = new float3(); |
||
256 | |||
257 | impact = null; |
||
258 | normal = null; |
||
259 | |||
260 | int i; |
||
261 | float3 nrml = new float3(0, 0, 0); |
||
262 | for (i = 0; i < vert.Count; i++) |
||
263 | { |
||
264 | int i1 = (i + 1) % vert.Count; |
||
265 | int i2 = (i + 2) % vert.Count; |
||
266 | nrml = nrml + float3.cross(vert[i1] - vert[i], vert[i2] - vert[i1]); |
||
267 | } |
||
268 | |||
269 | float m = float3.magnitude(nrml); |
||
270 | if (m == 0.0) |
||
271 | { |
||
272 | return false; |
||
273 | } |
||
274 | nrml = nrml * (1.0f / m); |
||
275 | float dist = -float3.dot(nrml, vert[0]); |
||
276 | float d0; |
||
277 | float d1; |
||
278 | if ((d0 = float3.dot(v0, nrml) + dist) < 0 || (d1 = float3.dot(v1, nrml) + dist) > 0) |
||
279 | { |
||
280 | return false; |
||
281 | } |
||
282 | |||
283 | // By using the cached plane distances d0 and d1 |
||
284 | // we can optimize the following: |
||
285 | // the_point = planelineintersection(nrml,dist,v0,v1); |
||
286 | float a = d0 / (d0 - d1); |
||
287 | the_point = v0 * (1 - a) + v1 * a; |
||
288 | |||
289 | |||
290 | bool inside = true; |
||
291 | for (int j = 0; inside && j < vert.Count; j++) |
||
292 | { |
||
293 | // let inside = 0 if outside |
||
294 | float3 pp1 = new float3(); |
||
295 | float3 pp2 = new float3(); |
||
296 | float3 side = new float3(); |
||
297 | pp1 = vert[j]; |
||
298 | pp2 = vert[(j + 1) % vert.Count]; |
||
299 | side = float3.cross((pp2 - pp1), (the_point - pp1)); |
||
300 | inside = (float3.dot(nrml, side) >= 0.0); |
||
301 | } |
||
302 | if (inside) |
||
303 | { |
||
304 | if (normal != null) |
||
305 | { |
||
306 | normal = nrml; |
||
307 | } |
||
308 | if (impact != null) |
||
309 | { |
||
310 | impact = the_point; |
||
311 | } |
||
312 | } |
||
313 | return inside; |
||
314 | } |
||
315 | |||
316 | public static bool BoxInside(float3 p, float3 bmin, float3 bmax) |
||
317 | { |
||
318 | return (p.x >= bmin.x && p.x <= bmax.x && p.y >= bmin.y && p.y <= bmax.y && p.z >= bmin.z && p.z <= bmax.z); |
||
319 | } |
||
320 | |||
321 | public static bool BoxIntersect(float3 v0, float3 v1, float3 bmin, float3 bmax, float3 impact) |
||
322 | { |
||
323 | if (BoxInside(v0, bmin, bmax)) |
||
324 | { |
||
325 | impact = v0; |
||
326 | return true; |
||
327 | } |
||
328 | if (v0.x <= bmin.x && v1.x >= bmin.x) |
||
329 | { |
||
330 | float a = (bmin.x - v0.x) / (v1.x - v0.x); |
||
331 | //v.x = bmin.x; |
||
332 | float vy = (1 - a) * v0.y + a * v1.y; |
||
333 | float vz = (1 - a) * v0.z + a * v1.z; |
||
334 | if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z) |
||
335 | { |
||
336 | impact.x = bmin.x; |
||
337 | impact.y = vy; |
||
338 | impact.z = vz; |
||
339 | return true; |
||
340 | } |
||
341 | } |
||
342 | else if (v0.x >= bmax.x && v1.x <= bmax.x) |
||
343 | { |
||
344 | float a = (bmax.x - v0.x) / (v1.x - v0.x); |
||
345 | //v.x = bmax.x; |
||
346 | float vy = (1 - a) * v0.y + a * v1.y; |
||
347 | float vz = (1 - a) * v0.z + a * v1.z; |
||
348 | if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z) |
||
349 | { |
||
350 | impact.x = bmax.x; |
||
351 | impact.y = vy; |
||
352 | impact.z = vz; |
||
353 | return true; |
||
354 | } |
||
355 | } |
||
356 | if (v0.y <= bmin.y && v1.y >= bmin.y) |
||
357 | { |
||
358 | float a = (bmin.y - v0.y) / (v1.y - v0.y); |
||
359 | float vx = (1 - a) * v0.x + a * v1.x; |
||
360 | //v.y = bmin.y; |
||
361 | float vz = (1 - a) * v0.z + a * v1.z; |
||
362 | if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z) |
||
363 | { |
||
364 | impact.x = vx; |
||
365 | impact.y = bmin.y; |
||
366 | impact.z = vz; |
||
367 | return true; |
||
368 | } |
||
369 | } |
||
370 | else if (v0.y >= bmax.y && v1.y <= bmax.y) |
||
371 | { |
||
372 | float a = (bmax.y - v0.y) / (v1.y - v0.y); |
||
373 | float vx = (1 - a) * v0.x + a * v1.x; |
||
374 | // vy = bmax.y; |
||
375 | float vz = (1 - a) * v0.z + a * v1.z; |
||
376 | if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z) |
||
377 | { |
||
378 | impact.x = vx; |
||
379 | impact.y = bmax.y; |
||
380 | impact.z = vz; |
||
381 | return true; |
||
382 | } |
||
383 | } |
||
384 | if (v0.z <= bmin.z && v1.z >= bmin.z) |
||
385 | { |
||
386 | float a = (bmin.z - v0.z) / (v1.z - v0.z); |
||
387 | float vx = (1 - a) * v0.x + a * v1.x; |
||
388 | float vy = (1 - a) * v0.y + a * v1.y; |
||
389 | // v.z = bmin.z; |
||
390 | if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x) |
||
391 | { |
||
392 | impact.x = vx; |
||
393 | impact.y = vy; |
||
394 | impact.z = bmin.z; |
||
395 | return true; |
||
396 | } |
||
397 | } |
||
398 | else if (v0.z >= bmax.z && v1.z <= bmax.z) |
||
399 | { |
||
400 | float a = (bmax.z - v0.z) / (v1.z - v0.z); |
||
401 | float vx = (1 - a) * v0.x + a * v1.x; |
||
402 | float vy = (1 - a) * v0.y + a * v1.y; |
||
403 | // v.z = bmax.z; |
||
404 | if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x) |
||
405 | { |
||
406 | impact.x = vx; |
||
407 | impact.y = vy; |
||
408 | impact.z = bmax.z; |
||
409 | return true; |
||
410 | } |
||
411 | } |
||
412 | return false; |
||
413 | } |
||
414 | |||
415 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint) |
||
416 | { |
||
417 | return DistanceBetweenLines(ustart, udir, vstart, vdir, upoint, null); |
||
418 | } |
||
419 | |||
420 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir) |
||
421 | { |
||
422 | return DistanceBetweenLines(ustart, udir, vstart, vdir, null, null); |
||
423 | } |
||
424 | |||
425 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint, float3 vpoint) |
||
426 | { |
||
427 | float3 cp = float3.normalize(float3.cross(udir, vdir)); |
||
428 | |||
429 | float distu = -float3.dot(cp, ustart); |
||
430 | float distv = -float3.dot(cp, vstart); |
||
431 | float dist = (float)Math.Abs(distu - distv); |
||
432 | if (upoint != null) |
||
433 | { |
||
434 | Plane plane = new Plane(); |
||
435 | plane.normal = float3.normalize(float3.cross(vdir, cp)); |
||
436 | plane.dist = -float3.dot(plane.normal, vstart); |
||
437 | upoint = PlaneLineIntersection(plane, ustart, ustart + udir); |
||
438 | } |
||
439 | if (vpoint != null) |
||
440 | { |
||
441 | Plane plane = new Plane(); |
||
442 | plane.normal = float3.normalize(float3.cross(udir, cp)); |
||
443 | plane.dist = -float3.dot(plane.normal, ustart); |
||
444 | vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir); |
||
445 | } |
||
446 | return dist; |
||
447 | } |
||
448 | |||
449 | public static float3 TriNormal(float3 v0, float3 v1, float3 v2) |
||
450 | { |
||
451 | // return the normal of the triangle |
||
452 | // inscribed by v0, v1, and v2 |
||
453 | float3 cp = float3.cross(v1 - v0, v2 - v1); |
||
454 | float m = float3.magnitude(cp); |
||
455 | if (m == 0) |
||
456 | return new float3(1, 0, 0); |
||
457 | return cp * (1.0f / m); |
||
458 | } |
||
459 | |||
460 | public static int PlaneTest(Plane p, float3 v, float planetestepsilon) |
||
461 | { |
||
462 | float a = float3.dot(v, p.normal) + p.dist; |
||
463 | int flag = (a > planetestepsilon) ? (2) : ((a < -planetestepsilon) ? (1) : (0)); |
||
464 | return flag; |
||
465 | } |
||
466 | |||
467 | public static int SplitTest(ref ConvexH convex, Plane plane, float planetestepsilon) |
||
468 | { |
||
469 | int flag = 0; |
||
470 | for (int i = 0; i < convex.vertices.Count; i++) |
||
471 | { |
||
472 | flag |= PlaneTest(plane, convex.vertices[i], planetestepsilon); |
||
473 | } |
||
474 | return flag; |
||
475 | } |
||
476 | |||
477 | public static Quaternion VirtualTrackBall(float3 cop, float3 cor, float3 dir1, float3 dir2) |
||
478 | { |
||
479 | // routine taken from game programming gems. |
||
480 | // Implement track ball functionality to spin stuf on the screen |
||
481 | // cop center of projection |
||
482 | // cor center of rotation |
||
483 | // dir1 old mouse direction |
||
484 | // dir2 new mouse direction |
||
485 | // pretend there is a sphere around cor. Then find the points |
||
486 | // where dir1 and dir2 intersect that sphere. Find the |
||
487 | // rotation that takes the first point to the second. |
||
488 | float m; |
||
489 | // compute plane |
||
490 | float3 nrml = cor - cop; |
||
491 | float fudgefactor = 1.0f / (float3.magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop |
||
492 | nrml = float3.normalize(nrml); |
||
493 | float dist = -float3.dot(nrml, cor); |
||
494 | float3 u = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir1); |
||
495 | u = u - cor; |
||
496 | u = u * fudgefactor; |
||
497 | m = float3.magnitude(u); |
||
498 | if (m > 1) |
||
499 | { |
||
500 | u /= m; |
||
501 | } |
||
502 | else |
||
503 | { |
||
504 | u = u - (nrml * (float)Math.Sqrt(1 - m * m)); |
||
505 | } |
||
506 | float3 v = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir2); |
||
507 | v = v - cor; |
||
508 | v = v * fudgefactor; |
||
509 | m = float3.magnitude(v); |
||
510 | if (m > 1) |
||
511 | { |
||
512 | v /= m; |
||
513 | } |
||
514 | else |
||
515 | { |
||
516 | v = v - (nrml * (float)Math.Sqrt(1 - m * m)); |
||
517 | } |
||
518 | return RotationArc(u, v); |
||
519 | } |
||
520 | |||
521 | public static bool AssertIntact(ConvexH convex, float planetestepsilon) |
||
522 | { |
||
523 | int i; |
||
524 | int estart = 0; |
||
525 | for (i = 0; i < convex.edges.Count; i++) |
||
526 | { |
||
527 | if (convex.edges[estart].p != convex.edges[i].p) |
||
528 | { |
||
529 | estart = i; |
||
530 | } |
||
531 | int inext = i + 1; |
||
532 | if (inext >= convex.edges.Count || convex.edges[inext].p != convex.edges[i].p) |
||
533 | { |
||
534 | inext = estart; |
||
535 | } |
||
536 | Debug.Assert(convex.edges[inext].p == convex.edges[i].p); |
||
537 | int nb = convex.edges[i].ea; |
||
538 | Debug.Assert(nb != 255); |
||
539 | if (nb == 255 || nb == -1) |
||
540 | return false; |
||
541 | Debug.Assert(nb != -1); |
||
542 | Debug.Assert(i == convex.edges[nb].ea); |
||
543 | } |
||
544 | for (i = 0; i < convex.edges.Count; i++) |
||
545 | { |
||
546 | Debug.Assert((0) == PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon)); |
||
547 | if ((0) != PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon)) |
||
548 | return false; |
||
549 | if (convex.edges[estart].p != convex.edges[i].p) |
||
550 | { |
||
551 | estart = i; |
||
552 | } |
||
553 | int i1 = i + 1; |
||
554 | if (i1 >= convex.edges.Count || convex.edges[i1].p != convex.edges[i].p) |
||
555 | { |
||
556 | i1 = estart; |
||
557 | } |
||
558 | int i2 = i1 + 1; |
||
559 | if (i2 >= convex.edges.Count || convex.edges[i2].p != convex.edges[i].p) |
||
560 | { |
||
561 | i2 = estart; |
||
562 | } |
||
563 | if (i == i2) // i sliced tangent to an edge and created 2 meaningless edges |
||
564 | continue; |
||
565 | float3 localnormal = TriNormal(convex.vertices[convex.edges[i].v], convex.vertices[convex.edges[i1].v], convex.vertices[convex.edges[i2].v]); |
||
566 | Debug.Assert(float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) > 0); |
||
567 | if (float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) <= 0) |
||
568 | return false; |
||
569 | } |
||
570 | return true; |
||
571 | } |
||
572 | |||
573 | public static ConvexH test_btbq(float planetestepsilon) |
||
574 | { |
||
575 | // back to back quads |
||
576 | ConvexH convex = new ConvexH(4, 8, 2); |
||
577 | convex.vertices[0] = new float3(0, 0, 0); |
||
578 | convex.vertices[1] = new float3(1, 0, 0); |
||
579 | convex.vertices[2] = new float3(1, 1, 0); |
||
580 | convex.vertices[3] = new float3(0, 1, 0); |
||
581 | convex.facets[0] = new Plane(new float3(0, 0, 1), 0); |
||
582 | convex.facets[1] = new Plane(new float3(0, 0, -1), 0); |
||
583 | convex.edges[0] = new ConvexH.HalfEdge(7, 0, 0); |
||
584 | convex.edges[1] = new ConvexH.HalfEdge(6, 1, 0); |
||
585 | convex.edges[2] = new ConvexH.HalfEdge(5, 2, 0); |
||
586 | convex.edges[3] = new ConvexH.HalfEdge(4, 3, 0); |
||
587 | |||
588 | convex.edges[4] = new ConvexH.HalfEdge(3, 0, 1); |
||
589 | convex.edges[5] = new ConvexH.HalfEdge(2, 3, 1); |
||
590 | convex.edges[6] = new ConvexH.HalfEdge(1, 2, 1); |
||
591 | convex.edges[7] = new ConvexH.HalfEdge(0, 1, 1); |
||
592 | AssertIntact(convex, planetestepsilon); |
||
593 | return convex; |
||
594 | } |
||
595 | |||
596 | public static ConvexH test_cube() |
||
597 | { |
||
598 | ConvexH convex = new ConvexH(8, 24, 6); |
||
599 | convex.vertices[0] = new float3(0, 0, 0); |
||
600 | convex.vertices[1] = new float3(0, 0, 1); |
||
601 | convex.vertices[2] = new float3(0, 1, 0); |
||
602 | convex.vertices[3] = new float3(0, 1, 1); |
||
603 | convex.vertices[4] = new float3(1, 0, 0); |
||
604 | convex.vertices[5] = new float3(1, 0, 1); |
||
605 | convex.vertices[6] = new float3(1, 1, 0); |
||
606 | convex.vertices[7] = new float3(1, 1, 1); |
||
607 | |||
608 | convex.facets[0] = new Plane(new float3(-1, 0, 0), 0); |
||
609 | convex.facets[1] = new Plane(new float3(1, 0, 0), -1); |
||
610 | convex.facets[2] = new Plane(new float3(0, -1, 0), 0); |
||
611 | convex.facets[3] = new Plane(new float3(0, 1, 0), -1); |
||
612 | convex.facets[4] = new Plane(new float3(0, 0, -1), 0); |
||
613 | convex.facets[5] = new Plane(new float3(0, 0, 1), -1); |
||
614 | |||
615 | convex.edges[0] = new ConvexH.HalfEdge(11, 0, 0); |
||
616 | convex.edges[1] = new ConvexH.HalfEdge(23, 1, 0); |
||
617 | convex.edges[2] = new ConvexH.HalfEdge(15, 3, 0); |
||
618 | convex.edges[3] = new ConvexH.HalfEdge(16, 2, 0); |
||
619 | |||
620 | convex.edges[4] = new ConvexH.HalfEdge(13, 6, 1); |
||
621 | convex.edges[5] = new ConvexH.HalfEdge(21, 7, 1); |
||
622 | convex.edges[6] = new ConvexH.HalfEdge(9, 5, 1); |
||
623 | convex.edges[7] = new ConvexH.HalfEdge(18, 4, 1); |
||
624 | |||
625 | convex.edges[8] = new ConvexH.HalfEdge(19, 0, 2); |
||
626 | convex.edges[9] = new ConvexH.HalfEdge(6, 4, 2); |
||
627 | convex.edges[10] = new ConvexH.HalfEdge(20, 5, 2); |
||
628 | convex.edges[11] = new ConvexH.HalfEdge(0, 1, 2); |
||
629 | |||
630 | convex.edges[12] = new ConvexH.HalfEdge(22, 3, 3); |
||
631 | convex.edges[13] = new ConvexH.HalfEdge(4, 7, 3); |
||
632 | convex.edges[14] = new ConvexH.HalfEdge(17, 6, 3); |
||
633 | convex.edges[15] = new ConvexH.HalfEdge(2, 2, 3); |
||
634 | |||
635 | convex.edges[16] = new ConvexH.HalfEdge(3, 0, 4); |
||
636 | convex.edges[17] = new ConvexH.HalfEdge(14, 2, 4); |
||
637 | convex.edges[18] = new ConvexH.HalfEdge(7, 6, 4); |
||
638 | convex.edges[19] = new ConvexH.HalfEdge(8, 4, 4); |
||
639 | |||
640 | convex.edges[20] = new ConvexH.HalfEdge(10, 1, 5); |
||
641 | convex.edges[21] = new ConvexH.HalfEdge(5, 5, 5); |
||
642 | convex.edges[22] = new ConvexH.HalfEdge(12, 7, 5); |
||
643 | convex.edges[23] = new ConvexH.HalfEdge(1, 3, 5); |
||
644 | |||
645 | return convex; |
||
646 | } |
||
647 | |||
648 | public static ConvexH ConvexHMakeCube(float3 bmin, float3 bmax) |
||
649 | { |
||
650 | ConvexH convex = test_cube(); |
||
651 | convex.vertices[0] = new float3(bmin.x, bmin.y, bmin.z); |
||
652 | convex.vertices[1] = new float3(bmin.x, bmin.y, bmax.z); |
||
653 | convex.vertices[2] = new float3(bmin.x, bmax.y, bmin.z); |
||
654 | convex.vertices[3] = new float3(bmin.x, bmax.y, bmax.z); |
||
655 | convex.vertices[4] = new float3(bmax.x, bmin.y, bmin.z); |
||
656 | convex.vertices[5] = new float3(bmax.x, bmin.y, bmax.z); |
||
657 | convex.vertices[6] = new float3(bmax.x, bmax.y, bmin.z); |
||
658 | convex.vertices[7] = new float3(bmax.x, bmax.y, bmax.z); |
||
659 | |||
660 | convex.facets[0] = new Plane(new float3(-1, 0, 0), bmin.x); |
||
661 | convex.facets[1] = new Plane(new float3(1, 0, 0), -bmax.x); |
||
662 | convex.facets[2] = new Plane(new float3(0, -1, 0), bmin.y); |
||
663 | convex.facets[3] = new Plane(new float3(0, 1, 0), -bmax.y); |
||
664 | convex.facets[4] = new Plane(new float3(0, 0, -1), bmin.z); |
||
665 | convex.facets[5] = new Plane(new float3(0, 0, 1), -bmax.z); |
||
666 | return convex; |
||
667 | } |
||
668 | |||
669 | public static ConvexH ConvexHCrop(ref ConvexH convex, Plane slice, float planetestepsilon) |
||
670 | { |
||
671 | int i; |
||
672 | int vertcountunder = 0; |
||
673 | int vertcountover = 0; |
||
674 | List<int> vertscoplanar = new List<int>(); // existing vertex members of convex that are coplanar |
||
675 | List<int> edgesplit = new List<int>(); // existing edges that members of convex that cross the splitplane |
||
676 | |||
677 | Debug.Assert(convex.edges.Count < 480); |
||
678 | |||
679 | EdgeFlag[] edgeflag = new EdgeFlag[512]; |
||
680 | VertFlag[] vertflag = new VertFlag[256]; |
||
681 | PlaneFlag[] planeflag = new PlaneFlag[128]; |
||
682 | ConvexH.HalfEdge[] tmpunderedges = new ConvexH.HalfEdge[512]; |
||
683 | Plane[] tmpunderplanes = new Plane[128]; |
||
684 | Coplanar[] coplanaredges = new Coplanar[512]; |
||
685 | int coplanaredges_num = 0; |
||
686 | |||
687 | List<float3> createdverts = new List<float3>(); |
||
688 | |||
689 | // do the side-of-plane tests |
||
690 | for (i = 0; i < convex.vertices.Count; i++) |
||
691 | { |
||
692 | vertflag[i].planetest = (byte)PlaneTest(slice, convex.vertices[i], planetestepsilon); |
||
693 | if (vertflag[i].planetest == (0)) |
||
694 | { |
||
695 | // ? vertscoplanar.Add(i); |
||
696 | vertflag[i].undermap = (byte)vertcountunder++; |
||
697 | vertflag[i].overmap = (byte)vertcountover++; |
||
698 | } |
||
699 | else if (vertflag[i].planetest == (1)) |
||
700 | { |
||
701 | vertflag[i].undermap = (byte)vertcountunder++; |
||
702 | } |
||
703 | else |
||
704 | { |
||
705 | Debug.Assert(vertflag[i].planetest == (2)); |
||
706 | vertflag[i].overmap = (byte)vertcountover++; |
||
707 | vertflag[i].undermap = 255; // for debugging purposes |
||
708 | } |
||
709 | } |
||
710 | int vertcountunderold = vertcountunder; // for debugging only |
||
711 | |||
712 | int under_edge_count = 0; |
||
713 | int underplanescount = 0; |
||
714 | int e0 = 0; |
||
715 | |||
716 | for (int currentplane = 0; currentplane < convex.facets.Count; currentplane++) |
||
717 | { |
||
718 | int estart = e0; |
||
719 | int enextface = 0; |
||
720 | int planeside = 0; |
||
721 | int e1 = e0 + 1; |
||
722 | int vout = -1; |
||
723 | int vin = -1; |
||
724 | int coplanaredge = -1; |
||
725 | do |
||
726 | { |
||
727 | |||
728 | if (e1 >= convex.edges.Count || convex.edges[e1].p != currentplane) |
||
729 | { |
||
730 | enextface = e1; |
||
731 | e1 = estart; |
||
732 | } |
||
733 | ConvexH.HalfEdge edge0 = convex.edges[e0]; |
||
734 | ConvexH.HalfEdge edge1 = convex.edges[e1]; |
||
735 | ConvexH.HalfEdge edgea = convex.edges[edge0.ea]; |
||
736 | |||
737 | planeside |= vertflag[edge0.v].planetest; |
||
738 | //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) { |
||
739 | // assert(ecop==-1); |
||
740 | // ecop=e; |
||
741 | //} |
||
742 | |||
743 | if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (2)) |
||
744 | { |
||
745 | // both endpoints over plane |
||
746 | edgeflag[e0].undermap = -1; |
||
747 | } |
||
748 | else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (1)) |
||
749 | { |
||
750 | // at least one endpoint under, the other coplanar or under |
||
751 | |||
752 | edgeflag[e0].undermap = (short)under_edge_count; |
||
753 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; |
||
754 | tmpunderedges[under_edge_count].p = (byte)underplanescount; |
||
755 | if (edge0.ea < e0) |
||
756 | { |
||
757 | // connect the neighbors |
||
758 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); |
||
759 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; |
||
760 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; |
||
761 | } |
||
762 | under_edge_count++; |
||
763 | } |
||
764 | else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (0)) |
||
765 | { |
||
766 | // both endpoints coplanar |
||
767 | // must check a 3rd point to see if UNDER |
||
768 | int e2 = e1 + 1; |
||
769 | if (e2 >= convex.edges.Count || convex.edges[e2].p != currentplane) |
||
770 | { |
||
771 | e2 = estart; |
||
772 | } |
||
773 | Debug.Assert(convex.edges[e2].p == currentplane); |
||
774 | ConvexH.HalfEdge edge2 = convex.edges[e2]; |
||
775 | if (vertflag[edge2.v].planetest == (1)) |
||
776 | { |
||
777 | |||
778 | edgeflag[e0].undermap = (short)under_edge_count; |
||
779 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; |
||
780 | tmpunderedges[under_edge_count].p = (byte)underplanescount; |
||
781 | tmpunderedges[under_edge_count].ea = -1; |
||
782 | // make sure this edge is added to the "coplanar" list |
||
783 | coplanaredge = under_edge_count; |
||
784 | vout = vertflag[edge0.v].undermap; |
||
785 | vin = vertflag[edge1.v].undermap; |
||
786 | under_edge_count++; |
||
787 | } |
||
788 | else |
||
789 | { |
||
790 | edgeflag[e0].undermap = -1; |
||
791 | } |
||
792 | } |
||
793 | else if (vertflag[edge0.v].planetest == (1) && vertflag[edge1.v].planetest == (2)) |
||
794 | { |
||
795 | // first is under 2nd is over |
||
796 | |||
797 | edgeflag[e0].undermap = (short)under_edge_count; |
||
798 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; |
||
799 | tmpunderedges[under_edge_count].p = (byte)underplanescount; |
||
800 | if (edge0.ea < e0) |
||
801 | { |
||
802 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); |
||
803 | // connect the neighbors |
||
804 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; |
||
805 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; |
||
806 | vout = tmpunderedges[edgeflag[edge0.ea].undermap].v; |
||
807 | } |
||
808 | else |
||
809 | { |
||
810 | Plane p0 = convex.facets[edge0.p]; |
||
811 | Plane pa = convex.facets[edgea.p]; |
||
812 | createdverts.Add(ThreePlaneIntersection(p0, pa, slice)); |
||
813 | //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); |
||
814 | //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); |
||
815 | vout = vertcountunder++; |
||
816 | } |
||
817 | under_edge_count++; |
||
818 | /// hmmm something to think about: i might be able to output this edge regarless of |
||
819 | // wheter or not we know v-in yet. ok i;ll try this now: |
||
820 | tmpunderedges[under_edge_count].v = (byte)vout; |
||
821 | tmpunderedges[under_edge_count].p = (byte)underplanescount; |
||
822 | tmpunderedges[under_edge_count].ea = -1; |
||
823 | coplanaredge = under_edge_count; |
||
824 | under_edge_count++; |
||
825 | |||
826 | if (vin != -1) |
||
827 | { |
||
828 | // we previously processed an edge where we came under |
||
829 | // now we know about vout as well |
||
830 | |||
831 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! |
||
832 | } |
||
833 | |||
834 | } |
||
835 | else if (vertflag[edge0.v].planetest == (0) && vertflag[edge1.v].planetest == (2)) |
||
836 | { |
||
837 | // first is coplanar 2nd is over |
||
838 | |||
839 | edgeflag[e0].undermap = -1; |
||
840 | vout = vertflag[edge0.v].undermap; |
||
841 | // I hate this but i have to make sure part of this face is UNDER before ouputting this vert |
||
842 | int k = estart; |
||
843 | Debug.Assert(edge0.p == currentplane); |
||
844 | while (!((planeside & 1) != 0) && k < convex.edges.Count && convex.edges[k].p == edge0.p) |
||
845 | { |
||
846 | planeside |= vertflag[convex.edges[k].v].planetest; |
||
847 | k++; |
||
848 | } |
||
849 | if ((planeside & 1) != 0) |
||
850 | { |
||
851 | tmpunderedges[under_edge_count].v = (byte)vout; |
||
852 | tmpunderedges[under_edge_count].p = (byte)underplanescount; |
||
853 | tmpunderedges[under_edge_count].ea = -1; |
||
854 | coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on |
||
855 | under_edge_count++; |
||
856 | |||
857 | } |
||
858 | } |
||
859 | else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (1)) |
||
860 | { |
||
861 | // first is over next is under |
||
862 | // new vertex!!! |
||
863 | Debug.Assert(vin == -1); |
||
864 | if (e0 < edge0.ea) |
||
865 | { |
||
866 | Plane p0 = convex.facets[edge0.p]; |
||
867 | Plane pa = convex.facets[edgea.p]; |
||
868 | createdverts.Add(ThreePlaneIntersection(p0, pa, slice)); |
||
869 | //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); |
||
870 | //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); |
||
871 | vin = vertcountunder++; |
||
872 | } |
||
873 | else |
||
874 | { |
||
875 | // find the new vertex that was created by edge[edge0.ea] |
||
876 | int nea = edgeflag[edge0.ea].undermap; |
||
877 | Debug.Assert(tmpunderedges[nea].p == tmpunderedges[nea + 1].p); |
||
878 | vin = tmpunderedges[nea + 1].v; |
||
879 | Debug.Assert(vin < vertcountunder); |
||
880 | Debug.Assert(vin >= vertcountunderold); // for debugging only |
||
881 | } |
||
882 | if (vout != -1) |
||
883 | { |
||
884 | // we previously processed an edge where we went over |
||
885 | // now we know vin too |
||
886 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! |
||
887 | } |
||
888 | // output edge |
||
889 | tmpunderedges[under_edge_count].v = (byte)vin; |
||
890 | tmpunderedges[under_edge_count].p = (byte)underplanescount; |
||
891 | edgeflag[e0].undermap = (short)under_edge_count; |
||
892 | if (e0 > edge0.ea) |
||
893 | { |
||
894 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); |
||
895 | // connect the neighbors |
||
896 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; |
||
897 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; |
||
898 | } |
||
899 | Debug.Assert(edgeflag[e0].undermap == under_edge_count); |
||
900 | under_edge_count++; |
||
901 | } |
||
902 | else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (0)) |
||
903 | { |
||
904 | // first is over next is coplanar |
||
905 | |||
906 | edgeflag[e0].undermap = -1; |
||
907 | vin = vertflag[edge1.v].undermap; |
||
908 | Debug.Assert(vin != -1); |
||
909 | if (vout != -1) |
||
910 | { |
||
911 | // we previously processed an edge where we came under |
||
912 | // now we know both endpoints |
||
913 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! |
||
914 | } |
||
915 | |||
916 | } |
||
917 | else |
||
918 | { |
||
919 | Debug.Assert(false); |
||
920 | } |
||
921 | |||
922 | |||
923 | e0 = e1; |
||
924 | e1++; // do the modulo at the beginning of the loop |
||
925 | |||
926 | } while (e0 != estart); |
||
927 | e0 = enextface; |
||
928 | if ((planeside & 1) != 0) |
||
929 | { |
||
930 | planeflag[currentplane].undermap = (byte)underplanescount; |
||
931 | tmpunderplanes[underplanescount] = convex.facets[currentplane]; |
||
932 | underplanescount++; |
||
933 | } |
||
934 | else |
||
935 | { |
||
936 | planeflag[currentplane].undermap = 0; |
||
937 | } |
||
938 | if (vout >= 0 && (planeside & 1) != 0) |
||
939 | { |
||
940 | Debug.Assert(vin >= 0); |
||
941 | Debug.Assert(coplanaredge >= 0); |
||
942 | Debug.Assert(coplanaredge != 511); |
||
943 | coplanaredges[coplanaredges_num].ea = (ushort)coplanaredge; |
||
944 | coplanaredges[coplanaredges_num].v0 = (byte)vin; |
||
945 | coplanaredges[coplanaredges_num].v1 = (byte)vout; |
||
946 | coplanaredges_num++; |
||
947 | } |
||
948 | } |
||
949 | |||
950 | // add the new plane to the mix: |
||
951 | if (coplanaredges_num > 0) |
||
952 | { |
||
953 | tmpunderplanes[underplanescount++] = slice; |
||
954 | } |
||
955 | for (i = 0; i < coplanaredges_num - 1; i++) |
||
956 | { |
||
957 | if (coplanaredges[i].v1 != coplanaredges[i + 1].v0) |
||
958 | { |
||
959 | int j = 0; |
||
960 | for (j = i + 2; j < coplanaredges_num; j++) |
||
961 | { |
||
962 | if (coplanaredges[i].v1 == coplanaredges[j].v0) |
||
963 | { |
||
964 | Coplanar tmp = coplanaredges[i + 1]; |
||
965 | coplanaredges[i + 1] = coplanaredges[j]; |
||
966 | coplanaredges[j] = tmp; |
||
967 | break; |
||
968 | } |
||
969 | } |
||
970 | if (j >= coplanaredges_num) |
||
971 | { |
||
972 | Debug.Assert(j < coplanaredges_num); |
||
973 | return null; |
||
974 | } |
||
975 | } |
||
976 | } |
||
977 | |||
978 | ConvexH punder = new ConvexH(vertcountunder, under_edge_count + coplanaredges_num, underplanescount); |
||
979 | ConvexH under = punder; |
||
980 | |||
981 | { |
||
982 | int k = 0; |
||
983 | for (i = 0; i < convex.vertices.Count; i++) |
||
984 | { |
||
985 | if (vertflag[i].planetest != (2)) |
||
986 | { |
||
987 | under.vertices[k++] = convex.vertices[i]; |
||
988 | } |
||
989 | } |
||
990 | i = 0; |
||
991 | while (k < vertcountunder) |
||
992 | { |
||
993 | under.vertices[k++] = createdverts[i++]; |
||
994 | } |
||
995 | Debug.Assert(i == createdverts.Count); |
||
996 | } |
||
997 | |||
998 | for (i = 0; i < coplanaredges_num; i++) |
||
999 | { |
||
1000 | ConvexH.HalfEdge edge = under.edges[under_edge_count + i]; |
||
1001 | edge.p = (byte)(underplanescount - 1); |
||
1002 | edge.ea = (short)coplanaredges[i].ea; |
||
1003 | edge.v = (byte)coplanaredges[i].v0; |
||
1004 | under.edges[under_edge_count + i] = edge; |
||
1005 | |||
1006 | tmpunderedges[coplanaredges[i].ea].ea = (short)(under_edge_count + i); |
||
1007 | } |
||
1008 | |||
1009 | under.edges = new List<ConvexH.HalfEdge>(tmpunderedges); |
||
1010 | under.facets = new List<Plane>(tmpunderplanes); |
||
1011 | return punder; |
||
1012 | } |
||
1013 | |||
1014 | public static ConvexH ConvexHDup(ConvexH src) |
||
1015 | { |
||
1016 | ConvexH dst = new ConvexH(src.vertices.Count, src.edges.Count, src.facets.Count); |
||
1017 | dst.vertices = new List<float3>(src.vertices.Count); |
||
1018 | foreach (float3 f in src.vertices) |
||
1019 | dst.vertices.Add(new float3(f)); |
||
1020 | dst.edges = new List<ConvexH.HalfEdge>(src.edges.Count); |
||
1021 | foreach (ConvexH.HalfEdge e in src.edges) |
||
1022 | dst.edges.Add(new ConvexH.HalfEdge(e)); |
||
1023 | dst.facets = new List<Plane>(src.facets.Count); |
||
1024 | foreach (Plane p in src.facets) |
||
1025 | dst.facets.Add(new Plane(p)); |
||
1026 | return dst; |
||
1027 | } |
||
1028 | |||
1029 | public static int candidateplane(List<Plane> planes, int planes_count, ConvexH convex, float epsilon) |
||
1030 | { |
||
1031 | int p = 0; |
||
1032 | float md = 0; |
||
1033 | int i; |
||
1034 | for (i = 0; i < planes_count; i++) |
||
1035 | { |
||
1036 | float d = 0; |
||
1037 | for (int j = 0; j < convex.vertices.Count; j++) |
||
1038 | { |
||
1039 | d = Math.Max(d, float3.dot(convex.vertices[j], planes[i].normal) + planes[i].dist); |
||
1040 | } |
||
1041 | if (i == 0 || d > md) |
||
1042 | { |
||
1043 | p = i; |
||
1044 | md = d; |
||
1045 | } |
||
1046 | } |
||
1047 | return (md > epsilon) ? p : -1; |
||
1048 | } |
||
1049 | |||
1050 | public static float3 orth(float3 v) |
||
1051 | { |
||
1052 | float3 a = float3.cross(v, new float3(0f, 0f, 1f)); |
||
1053 | float3 b = float3.cross(v, new float3(0f, 1f, 0f)); |
||
1054 | return float3.normalize((float3.magnitude(a) > float3.magnitude(b)) ? a : b); |
||
1055 | } |
||
1056 | |||
1057 | public static int maxdir(List<float3> p, int count, float3 dir) |
||
1058 | { |
||
1059 | Debug.Assert(count != 0); |
||
1060 | int m = 0; |
||
1061 | float currDotm = float3.dot(p[0], dir); |
||
1062 | for (int i = 1; i < count; i++) |
||
1063 | { |
||
1064 | float currDoti = float3.dot(p[i], dir); |
||
1065 | if (currDoti > currDotm) |
||
1066 | { |
||
1067 | currDotm = currDoti; |
||
1068 | m = i; |
||
1069 | } |
||
1070 | } |
||
1071 | return m; |
||
1072 | } |
||
1073 | |||
1074 | public static int maxdirfiltered(List<float3> p, int count, float3 dir, byte[] allow) |
||
1075 | { |
||
1076 | //Debug.Assert(count != 0); |
||
1077 | int m = 0; |
||
1078 | float currDotm = float3.dot(p[0], dir); |
||
1079 | float currDoti; |
||
1080 | |||
1081 | while (allow[m] == 0) |
||
1082 | m++; |
||
1083 | |||
1084 | for (int i = 1; i < count; i++) |
||
1085 | { |
||
1086 | if (allow[i] != 0) |
||
1087 | { |
||
1088 | currDoti = float3.dot(p[i], dir); |
||
1089 | if (currDoti > currDotm) |
||
1090 | { |
||
1091 | currDotm = currDoti; |
||
1092 | m = i; |
||
1093 | } |
||
1094 | } |
||
1095 | } |
||
1096 | //Debug.Assert(m != -1); |
||
1097 | return m; |
||
1098 | } |
||
1099 | |||
1100 | public static int maxdirsterid(List<float3> p, int count, float3 dir, byte[] allow) |
||
1101 | { |
||
1102 | int m = -1; |
||
1103 | while (m == -1) |
||
1104 | { |
||
1105 | m = maxdirfiltered(p, count, dir, allow); |
||
1106 | if (allow[m] == 3) |
||
1107 | return m; |
||
1108 | float3 u = orth(dir); |
||
1109 | float3 v = float3.cross(u, dir); |
||
1110 | int ma = -1; |
||
1111 | for (float x = 0.0f; x <= 360.0f; x += 45.0f) |
||
1112 | { |
||
1113 | int mb; |
||
1114 | { |
||
1115 | float s = (float)Math.Sin((3.14159264f / 180.0f) * (x)); |
||
1116 | float c = (float)Math.Cos((3.14159264f / 180.0f) * (x)); |
||
1117 | mb = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); |
||
1118 | } |
||
1119 | if (ma == m && mb == m) |
||
1120 | { |
||
1121 | allow[m] = 3; |
||
1122 | return m; |
||
1123 | } |
||
1124 | if (ma != -1 && ma != mb) // Yuck - this is really ugly |
||
1125 | { |
||
1126 | int mc = ma; |
||
1127 | for (float xx = x - 40.0f; xx <= x; xx += 5.0f) |
||
1128 | { |
||
1129 | float s = (float)Math.Sin((3.14159264f / 180.0f) * (xx)); |
||
1130 | float c = (float)Math.Cos((3.14159264f / 180.0f) * (xx)); |
||
1131 | int md = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); |
||
1132 | if (mc == m && md == m) |
||
1133 | { |
||
1134 | allow[m] = 3; |
||
1135 | return m; |
||
1136 | } |
||
1137 | mc = md; |
||
1138 | } |
||
1139 | } |
||
1140 | ma = mb; |
||
1141 | } |
||
1142 | allow[m] = 0; |
||
1143 | m = -1; |
||
1144 | } |
||
1145 | |||
1146 | Debug.Assert(false); |
||
1147 | return m; |
||
1148 | } |
||
1149 | |||
1150 | public static int4 FindSimplex(List<float3> verts, byte[] allow) |
||
1151 | { |
||
1152 | float3[] basis = new float3[3]; |
||
1153 | basis[0] = new float3(0.01f, 0.02f, 1.0f); |
||
1154 | int p0 = maxdirsterid(verts, verts.Count, basis[0], allow); |
||
1155 | int p1 = maxdirsterid(verts, verts.Count, -basis[0], allow); |
||
1156 | basis[0] = verts[p0] - verts[p1]; |
||
1157 | if (p0 == p1 || basis[0] == new float3(0, 0, 0)) |
||
1158 | return new int4(-1, -1, -1, -1); |
||
1159 | basis[1] = float3.cross(new float3(1, 0.02f, 0), basis[0]); |
||
1160 | basis[2] = float3.cross(new float3(-0.02f, 1, 0), basis[0]); |
||
1161 | basis[1] = float3.normalize((float3.magnitude(basis[1]) > float3.magnitude(basis[2])) ? basis[1] : basis[2]); |
||
1162 | int p2 = maxdirsterid(verts, verts.Count, basis[1], allow); |
||
1163 | if (p2 == p0 || p2 == p1) |
||
1164 | { |
||
1165 | p2 = maxdirsterid(verts, verts.Count, -basis[1], allow); |
||
1166 | } |
||
1167 | if (p2 == p0 || p2 == p1) |
||
1168 | return new int4(-1, -1, -1, -1); |
||
1169 | basis[1] = verts[p2] - verts[p0]; |
||
1170 | basis[2] = float3.normalize(float3.cross(basis[1], basis[0])); |
||
1171 | int p3 = maxdirsterid(verts, verts.Count, basis[2], allow); |
||
1172 | if (p3 == p0 || p3 == p1 || p3 == p2) |
||
1173 | p3 = maxdirsterid(verts, verts.Count, -basis[2], allow); |
||
1174 | if (p3 == p0 || p3 == p1 || p3 == p2) |
||
1175 | return new int4(-1, -1, -1, -1); |
||
1176 | Debug.Assert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3)); |
||
1177 | if (float3.dot(verts[p3] - verts[p0], float3.cross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0) |
||
1178 | { |
||
1179 | Swap(ref p2, ref p3); |
||
1180 | } |
||
1181 | return new int4(p0, p1, p2, p3); |
||
1182 | } |
||
1183 | |||
1184 | public static float GetDist(float px, float py, float pz, float3 p2) |
||
1185 | { |
||
1186 | float dx = px - p2.x; |
||
1187 | float dy = py - p2.y; |
||
1188 | float dz = pz - p2.z; |
||
1189 | |||
1190 | return dx * dx + dy * dy + dz * dz; |
||
1191 | } |
||
1192 | |||
1193 | public static void ReleaseHull(PHullResult result) |
||
1194 | { |
||
1195 | if (result.Indices != null) |
||
1196 | result.Indices = null; |
||
1197 | if (result.Vertices != null) |
||
1198 | result.Vertices = null; |
||
1199 | } |
||
1200 | |||
1201 | public static int calchullgen(List<float3> verts, int vlimit, List<HullTriangle> tris) |
||
1202 | { |
||
1203 | if (verts.Count < 4) |
||
1204 | return 0; |
||
1205 | if (vlimit == 0) |
||
1206 | vlimit = 1000000000; |
||
1207 | int j; |
||
1208 | float3 bmin = new float3(verts[0]); |
||
1209 | float3 bmax = new float3(verts[0]); |
||
1210 | List<int> isextreme = new List<int>(verts.Count); |
||
1211 | byte[] allow = new byte[verts.Count]; |
||
1212 | for (j = 0; j < verts.Count; j++) |
||
1213 | { |
||
1214 | allow[j] = 1; |
||
1215 | isextreme.Add(0); |
||
1216 | bmin = float3.VectorMin(bmin, verts[j]); |
||
1217 | bmax = float3.VectorMax(bmax, verts[j]); |
||
1218 | } |
||
1219 | float epsilon = float3.magnitude(bmax - bmin) * 0.001f; |
||
1220 | |||
1221 | int4 p = FindSimplex(verts, allow); |
||
1222 | if (p.x == -1) // simplex failed |
||
1223 | return 0; |
||
1224 | |||
1225 | float3 center = (verts[p[0]] + verts[p[1]] + verts[p[2]] + verts[p[3]]) / 4.0f; // a valid interior point |
||
1226 | HullTriangle t0 = new HullTriangle(p[2], p[3], p[1], tris); |
||
1227 | t0.n = new int3(2, 3, 1); |
||
1228 | HullTriangle t1 = new HullTriangle(p[3], p[2], p[0], tris); |
||
1229 | t1.n = new int3(3, 2, 0); |
||
1230 | HullTriangle t2 = new HullTriangle(p[0], p[1], p[3], tris); |
||
1231 | t2.n = new int3(0, 1, 3); |
||
1232 | HullTriangle t3 = new HullTriangle(p[1], p[0], p[2], tris); |
||
1233 | t3.n = new int3(1, 0, 2); |
||
1234 | isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1; |
||
1235 | checkit(t0, tris); |
||
1236 | checkit(t1, tris); |
||
1237 | checkit(t2, tris); |
||
1238 | checkit(t3, tris); |
||
1239 | |||
1240 | for (j = 0; j < tris.Count; j++) |
||
1241 | { |
||
1242 | HullTriangle t = tris[j]; |
||
1243 | Debug.Assert((object)t != null); |
||
1244 | Debug.Assert(t.vmax < 0); |
||
1245 | float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); |
||
1246 | t.vmax = maxdirsterid(verts, verts.Count, n, allow); |
||
1247 | t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]); |
||
1248 | } |
||
1249 | HullTriangle te; |
||
1250 | vlimit -= 4; |
||
1251 | while (vlimit > 0 && (te = extrudable(epsilon, tris)) != null) |
||
1252 | { |
||
1253 | int3 ti = te; |
||
1254 | int v = te.vmax; |
||
1255 | Debug.Assert(isextreme[v] == 0); // wtf we've already done this vertex |
||
1256 | isextreme[v] = 1; |
||
1257 | //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already |
||
1258 | j = tris.Count; |
||
1259 | while (j-- != 0) |
||
1260 | { |
||
1261 | if (tris.Count <= j || (object)tris[j] == null) |
||
1262 | continue; |
||
1263 | int3 t = tris[j]; |
||
1264 | if (above(verts, t, verts[v], 0.01f * epsilon)) |
||
1265 | { |
||
1266 | extrude(tris[j], v, tris); |
||
1267 | } |
||
1268 | } |
||
1269 | // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle |
||
1270 | j = tris.Count; |
||
1271 | while (j-- != 0) |
||
1272 | { |
||
1273 | if (tris.Count <= j || (object)tris[j] == null) |
||
1274 | continue; |
||
1275 | if (!hasvert(tris[j], v)) |
||
1276 | break; |
||
1277 | int3 nt = tris[j]; |
||
1278 | if (above(verts, nt, center, 0.01f * epsilon) || float3.magnitude(float3.cross(verts[nt[1]] - verts[nt[0]], verts[nt[2]] - verts[nt[1]])) < epsilon * epsilon * 0.1f) |
||
1279 | { |
||
1280 | HullTriangle nb = tris[tris[j].n[0]]; |
||
1281 | Debug.Assert(nb != null); |
||
1282 | Debug.Assert(!hasvert(nb, v)); |
||
1283 | Debug.Assert(nb.id < j); |
||
1284 | extrude(nb, v, tris); |
||
1285 | j = tris.Count; |
||
1286 | } |
||
1287 | } |
||
1288 | j = tris.Count; |
||
1289 | while (j-- != 0) |
||
1290 | { |
||
1291 | HullTriangle t = tris[j]; |
||
1292 | if (t == null) |
||
1293 | continue; |
||
1294 | if (t.vmax >= 0) |
||
1295 | break; |
||
1296 | float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); |
||
1297 | t.vmax = maxdirsterid(verts, verts.Count, n, allow); |
||
1298 | if (isextreme[t.vmax] != 0) |
||
1299 | { |
||
1300 | t.vmax = -1; // already done that vertex - algorithm needs to be able to terminate. |
||
1301 | } |
||
1302 | else |
||
1303 | { |
||
1304 | t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]); |
||
1305 | } |
||
1306 | } |
||
1307 | vlimit--; |
||
1308 | } |
||
1309 | return 1; |
||
1310 | } |
||
1311 | |||
1312 | public static bool calchull(List<float3> verts, out List<int> tris_out, int vlimit, List<HullTriangle> tris) |
||
1313 | { |
||
1314 | tris_out = null; |
||
1315 | |||
1316 | int rc = calchullgen(verts, vlimit, tris); |
||
1317 | if (rc == 0) |
||
1318 | return false; |
||
1319 | List<int> ts = new List<int>(); |
||
1320 | for (int i = 0; i < tris.Count; i++) |
||
1321 | { |
||
1322 | if ((object)tris[i] != null) |
||
1323 | { |
||
1324 | for (int j = 0; j < 3; j++) |
||
1325 | ts.Add((tris[i])[j]); |
||
1326 | tris[i] = null; |
||
1327 | } |
||
1328 | } |
||
1329 | |||
1330 | tris_out = ts; |
||
1331 | tris.Clear(); |
||
1332 | return true; |
||
1333 | } |
||
1334 | |||
1335 | public static int calchullpbev(List<float3> verts, int vlimit, out List<Plane> planes, float bevangle, List<HullTriangle> tris) |
||
1336 | { |
||
1337 | int i; |
||
1338 | int j; |
||
1339 | planes = new List<Plane>(); |
||
1340 | int rc = calchullgen(verts, vlimit, tris); |
||
1341 | if (rc == 0) |
||
1342 | return 0; |
||
1343 | for (i = 0; i < tris.Count; i++) |
||
1344 | { |
||
1345 | if (tris[i] != null) |
||
1346 | { |
||
1347 | Plane p = new Plane(); |
||
1348 | HullTriangle t = tris[i]; |
||
1349 | p.normal = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); |
||
1350 | p.dist = -float3.dot(p.normal, verts[(t)[0]]); |
||
1351 | planes.Add(p); |
||
1352 | for (j = 0; j < 3; j++) |
||
1353 | { |
||
1354 | if (t.n[j] < t.id) |
||
1355 | continue; |
||
1356 | HullTriangle s = tris[t.n[j]]; |
||
1357 | float3 snormal = TriNormal(verts[(s)[0]], verts[(s)[1]], verts[(s)[2]]); |
||
1358 | if (float3.dot(snormal, p.normal) >= Math.Cos(bevangle * (3.14159264f / 180.0f))) |
||
1359 | continue; |
||
1360 | float3 n = float3.normalize(snormal + p.normal); |
||
1361 | planes.Add(new Plane(n, -float3.dot(n, verts[maxdir(verts, verts.Count, n)]))); |
||
1362 | } |
||
1363 | } |
||
1364 | } |
||
1365 | |||
1366 | tris.Clear(); |
||
1367 | return 1; |
||
1368 | } |
||
1369 | |||
1370 | public static int overhull(List<Plane> planes, List<float3> verts, int maxplanes, out List<float3> verts_out, out List<int> faces_out, float inflate) |
||
1371 | { |
||
1372 | verts_out = null; |
||
1373 | faces_out = null; |
||
1374 | |||
1375 | int i; |
||
1376 | int j; |
||
1377 | if (verts.Count < 4) |
||
1378 | return 0; |
||
1379 | maxplanes = Math.Min(maxplanes, planes.Count); |
||
1380 | float3 bmin = new float3(verts[0]); |
||
1381 | float3 bmax = new float3(verts[0]); |
||
1382 | for (i = 0; i < verts.Count; i++) |
||
1383 | { |
||
1384 | bmin = float3.VectorMin(bmin, verts[i]); |
||
1385 | bmax = float3.VectorMax(bmax, verts[i]); |
||
1386 | } |
||
1387 | // float diameter = magnitude(bmax-bmin); |
||
1388 | // inflate *=diameter; // RELATIVE INFLATION |
||
1389 | bmin -= new float3(inflate, inflate, inflate); |
||
1390 | bmax += new float3(inflate, inflate, inflate); |
||
1391 | for (i = 0; i < planes.Count; i++) |
||
1392 | { |
||
1393 | planes[i].dist -= inflate; |
||
1394 | } |
||
1395 | float3 emin = new float3(bmin); |
||
1396 | float3 emax = new float3(bmax); |
||
1397 | float epsilon = float3.magnitude(emax - emin) * 0.025f; |
||
1398 | float planetestepsilon = float3.magnitude(emax - emin) * (0.001f); |
||
1399 | // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think. |
||
1400 | // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter)); |
||
1401 | ConvexH c = ConvexHMakeCube(new float3(bmin), new float3(bmax)); |
||
1402 | int k; |
||
1403 | while (maxplanes-- != 0 && (k = candidateplane(planes, planes.Count, c, epsilon)) >= 0) |
||
1404 | { |
||
1405 | ConvexH tmp = c; |
||
1406 | c = ConvexHCrop(ref tmp, planes[k], planetestepsilon); |
||
1407 | if (c == null) // might want to debug this case better!!! |
||
1408 | { |
||
1409 | c = tmp; |
||
1410 | break; |
||
1411 | } |
||
1412 | if (AssertIntact(c, planetestepsilon) == false) // might want to debug this case better too!!! |
||
1413 | { |
||
1414 | c = tmp; |
||
1415 | break; |
||
1416 | } |
||
1417 | tmp.edges = null; |
||
1418 | tmp.facets = null; |
||
1419 | tmp.vertices = null; |
||
1420 | } |
||
1421 | |||
1422 | Debug.Assert(AssertIntact(c, planetestepsilon)); |
||
1423 | //return c; |
||
1424 | //C++ TO C# CONVERTER TODO TASK: The memory management function 'malloc' has no equivalent in C#: |
||
1425 | faces_out = new List<int>(); //(int)malloc(sizeof(int) * (1 + c.facets.Count + c.edges.Count)); // new int[1+c->facets.count+c->edges.count]; |
||
1426 | int faces_count_out = 0; |
||
1427 | i = 0; |
||
1428 | faces_out[faces_count_out++] = -1; |
||
1429 | k = 0; |
||
1430 | while (i < c.edges.Count) |
||
1431 | { |
||
1432 | j = 1; |
||
1433 | while (j + i < c.edges.Count && c.edges[i].p == c.edges[i + j].p) |
||
1434 | { |
||
1435 | j++; |
||
1436 | } |
||
1437 | faces_out[faces_count_out++] = j; |
||
1438 | while (j-- != 0) |
||
1439 | { |
||
1440 | faces_out[faces_count_out++] = c.edges[i].v; |
||
1441 | i++; |
||
1442 | } |
||
1443 | k++; |
||
1444 | } |
||
1445 | faces_out[0] = k; // number of faces. |
||
1446 | Debug.Assert(k == c.facets.Count); |
||
1447 | Debug.Assert(faces_count_out == 1 + c.facets.Count + c.edges.Count); |
||
1448 | verts_out = c.vertices; // new float3[c->vertices.count]; |
||
1449 | int verts_count_out = c.vertices.Count; |
||
1450 | for (i = 0; i < c.vertices.Count; i++) |
||
1451 | { |
||
1452 | verts_out[i] = new float3(c.vertices[i]); |
||
1453 | } |
||
1454 | |||
1455 | c.edges = null; |
||
1456 | c.facets = null; |
||
1457 | c.vertices = null; |
||
1458 | return 1; |
||
1459 | } |
||
1460 | |||
1461 | public static int overhullv(List<float3> verts, int maxplanes, out List<float3> verts_out, out List<int> faces_out, float inflate, float bevangle, int vlimit, List<HullTriangle> tris) |
||
1462 | { |
||
1463 | verts_out = null; |
||
1464 | faces_out = null; |
||
1465 | |||
1466 | if (verts.Count == 0) |
||
1467 | return 0; |
||
1468 | List<Plane> planes = new List<Plane>(); |
||
1469 | int rc = calchullpbev(verts, vlimit, out planes, bevangle, tris); |
||
1470 | if (rc == 0) |
||
1471 | return 0; |
||
1472 | return overhull(planes, verts, maxplanes, out verts_out, out faces_out, inflate); |
||
1473 | } |
||
1474 | |||
1475 | public static void addPoint(ref uint vcount, List<float3> p, float x, float y, float z) |
||
1476 | { |
||
1477 | p.Add(new float3(x, y, z)); |
||
1478 | vcount++; |
||
1479 | } |
||
1480 | |||
1481 | public static bool ComputeHull(List<float3> vertices, ref PHullResult result, int vlimit, float inflate) |
||
1482 | { |
||
1483 | List<HullTriangle> tris = new List<HullTriangle>(); |
||
1484 | List<int> faces; |
||
1485 | List<float3> verts_out; |
||
1486 | |||
1487 | if (inflate == 0.0f) |
||
1488 | { |
||
1489 | List<int> tris_out; |
||
1490 | bool ret = calchull(vertices, out tris_out, vlimit, tris); |
||
1491 | if (ret == false) |
||
1492 | return false; |
||
1493 | |||
1494 | result.Indices = tris_out; |
||
1495 | result.Vertices = vertices; |
||
1496 | return true; |
||
1497 | } |
||
1498 | else |
||
1499 | { |
||
1500 | int ret = overhullv(vertices, 35, out verts_out, out faces, inflate, 120.0f, vlimit, tris); |
||
1501 | if (ret == 0) |
||
1502 | return false; |
||
1503 | |||
1504 | List<int3> tris2 = new List<int3>(); |
||
1505 | int n = faces[0]; |
||
1506 | int k = 1; |
||
1507 | for (int i = 0; i < n; i++) |
||
1508 | { |
||
1509 | int pn = faces[k++]; |
||
1510 | for (int j = 2; j < pn; j++) |
||
1511 | tris2.Add(new int3(faces[k], faces[k + j - 1], faces[k + j])); |
||
1512 | k += pn; |
||
1513 | } |
||
1514 | Debug.Assert(tris2.Count == faces.Count - 1 - (n * 3)); |
||
1515 | |||
1516 | result.Indices = new List<int>(tris2.Count * 3); |
||
1517 | for (int i = 0; i < tris2.Count; i++) |
||
1518 | { |
||
1519 | result.Indices.Add(tris2[i].x); |
||
1520 | result.Indices.Add(tris2[i].y); |
||
1521 | result.Indices.Add(tris2[i].z); |
||
1522 | } |
||
1523 | result.Vertices = verts_out; |
||
1524 | |||
1525 | return true; |
||
1526 | } |
||
1527 | } |
||
1528 | |||
1529 | private static bool CleanupVertices(List<float3> svertices, out List<float3> vertices, float normalepsilon, out float3 scale) |
||
1530 | { |
||
1531 | const float EPSILON = 0.000001f; |
||
1532 | |||
1533 | vertices = new List<float3>(); |
||
1534 | scale = new float3(1f, 1f, 1f); |
||
1535 | |||
1536 | if (svertices.Count == 0) |
||
1537 | return false; |
||
1538 | |||
1539 | uint vcount = 0; |
||
1540 | |||
1541 | float[] recip = new float[3]; |
||
1542 | |||
1543 | float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; |
||
1544 | float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue }; |
||
1545 | |||
1546 | for (int i = 0; i < svertices.Count; i++) |
||
1547 | { |
||
1548 | float3 p = svertices[i]; |
||
1549 | |||
1550 | for (int j = 0; j < 3; j++) |
||
1551 | { |
||
1552 | if (p[j] < bmin[j]) |
||
1553 | bmin[j] = p[j]; |
||
1554 | if (p[j] > bmax[j]) |
||
1555 | bmax[j] = p[j]; |
||
1556 | } |
||
1557 | } |
||
1558 | |||
1559 | float dx = bmax[0] - bmin[0]; |
||
1560 | float dy = bmax[1] - bmin[1]; |
||
1561 | float dz = bmax[2] - bmin[2]; |
||
1562 | |||
1563 | float3 center = new float3(); |
||
1564 | |||
1565 | center.x = dx * 0.5f + bmin[0]; |
||
1566 | center.y = dy * 0.5f + bmin[1]; |
||
1567 | center.z = dz * 0.5f + bmin[2]; |
||
1568 | |||
1569 | if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svertices.Count < 3) |
||
1570 | { |
||
1571 | float len = Single.MaxValue; |
||
1572 | |||
1573 | if (dx > EPSILON && dx < len) |
||
1574 | len = dx; |
||
1575 | if (dy > EPSILON && dy < len) |
||
1576 | len = dy; |
||
1577 | if (dz > EPSILON && dz < len) |
||
1578 | len = dz; |
||
1579 | |||
1580 | if (len == Single.MaxValue) |
||
1581 | { |
||
1582 | dx = dy = dz = 0.01f; // one centimeter |
||
1583 | } |
||
1584 | else |
||
1585 | { |
||
1586 | if (dx < EPSILON) // 1/5th the shortest non-zero edge. |
||
1587 | dx = len * 0.05f; |
||
1588 | if (dy < EPSILON) |
||
1589 | dy = len * 0.05f; |
||
1590 | if (dz < EPSILON) |
||
1591 | dz = len * 0.05f; |
||
1592 | } |
||
1593 | |||
1594 | float x1 = center[0] - dx; |
||
1595 | float x2 = center[0] + dx; |
||
1596 | |||
1597 | float y1 = center[1] - dy; |
||
1598 | float y2 = center[1] + dy; |
||
1599 | |||
1600 | float z1 = center[2] - dz; |
||
1601 | float z2 = center[2] + dz; |
||
1602 | |||
1603 | addPoint(ref vcount, vertices, x1, y1, z1); |
||
1604 | addPoint(ref vcount, vertices, x2, y1, z1); |
||
1605 | addPoint(ref vcount, vertices, x2, y2, z1); |
||
1606 | addPoint(ref vcount, vertices, x1, y2, z1); |
||
1607 | addPoint(ref vcount, vertices, x1, y1, z2); |
||
1608 | addPoint(ref vcount, vertices, x2, y1, z2); |
||
1609 | addPoint(ref vcount, vertices, x2, y2, z2); |
||
1610 | addPoint(ref vcount, vertices, x1, y2, z2); |
||
1611 | |||
1612 | return true; // return cube |
||
1613 | } |
||
1614 | else |
||
1615 | { |
||
1616 | scale.x = dx; |
||
1617 | scale.y = dy; |
||
1618 | scale.z = dz; |
||
1619 | |||
1620 | recip[0] = 1f / dx; |
||
1621 | recip[1] = 1f / dy; |
||
1622 | recip[2] = 1f / dz; |
||
1623 | |||
1624 | center.x *= recip[0]; |
||
1625 | center.y *= recip[1]; |
||
1626 | center.z *= recip[2]; |
||
1627 | } |
||
1628 | |||
1629 | for (int i = 0; i < svertices.Count; i++) |
||
1630 | { |
||
1631 | float3 p = svertices[i]; |
||
1632 | |||
1633 | float px = p[0]; |
||
1634 | float py = p[1]; |
||
1635 | float pz = p[2]; |
||
1636 | |||
1637 | px = px * recip[0]; // normalize |
||
1638 | py = py * recip[1]; // normalize |
||
1639 | pz = pz * recip[2]; // normalize |
||
1640 | |||
1641 | if (true) |
||
1642 | { |
||
1643 | int j; |
||
1644 | |||
1645 | for (j = 0; j < vcount; j++) |
||
1646 | { |
||
1647 | float3 v = vertices[j]; |
||
1648 | |||
1649 | float x = v[0]; |
||
1650 | float y = v[1]; |
||
1651 | float z = v[2]; |
||
1652 | |||
1653 | float dx1 = Math.Abs(x - px); |
||
1654 | float dy1 = Math.Abs(y - py); |
||
1655 | float dz1 = Math.Abs(z - pz); |
||
1656 | |||
1657 | if (dx1 < normalepsilon && dy1 < normalepsilon && dz1 < normalepsilon) |
||
1658 | { |
||
1659 | // ok, it is close enough to the old one |
||
1660 | // now let us see if it is further from the center of the point cloud than the one we already recorded. |
||
1661 | // in which case we keep this one instead. |
||
1662 | float dist1 = GetDist(px, py, pz, center); |
||
1663 | float dist2 = GetDist(v[0], v[1], v[2], center); |
||
1664 | |||
1665 | if (dist1 > dist2) |
||
1666 | { |
||
1667 | v.x = px; |
||
1668 | v.y = py; |
||
1669 | v.z = pz; |
||
1670 | } |
||
1671 | |||
1672 | break; |
||
1673 | } |
||
1674 | } |
||
1675 | |||
1676 | if (j == vcount) |
||
1677 | { |
||
1678 | float3 dest = new float3(px, py, pz); |
||
1679 | vertices.Add(dest); |
||
1680 | vcount++; |
||
1681 | } |
||
1682 | } |
||
1683 | } |
||
1684 | |||
1685 | // ok..now make sure we didn't prune so many vertices it is now invalid. |
||
1686 | if (true) |
||
1687 | { |
||
1688 | float[] bmin2 = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; |
||
1689 | float[] bmax2 = { Single.MinValue, Single.MinValue, Single.MinValue }; |
||
1690 | |||
1691 | for (int i = 0; i < vcount; i++) |
||
1692 | { |
||
1693 | float3 p = vertices[i]; |
||
1694 | for (int j = 0; j < 3; j++) |
||
1695 | { |
||
1696 | if (p[j] < bmin2[j]) |
||
1697 | bmin2[j] = p[j]; |
||
1698 | if (p[j] > bmax2[j]) |
||
1699 | bmax2[j] = p[j]; |
||
1700 | } |
||
1701 | } |
||
1702 | |||
1703 | float dx2 = bmax2[0] - bmin2[0]; |
||
1704 | float dy2 = bmax2[1] - bmin2[1]; |
||
1705 | float dz2 = bmax2[2] - bmin2[2]; |
||
1706 | |||
1707 | if (dx2 < EPSILON || dy2 < EPSILON || dz2 < EPSILON || vcount < 3) |
||
1708 | { |
||
1709 | float cx = dx2 * 0.5f + bmin2[0]; |
||
1710 | float cy = dy2 * 0.5f + bmin2[1]; |
||
1711 | float cz = dz2 * 0.5f + bmin2[2]; |
||
1712 | |||
1713 | float len = Single.MaxValue; |
||
1714 | |||
1715 | if (dx2 >= EPSILON && dx2 < len) |
||
1716 | len = dx2; |
||
1717 | if (dy2 >= EPSILON && dy2 < len) |
||
1718 | len = dy2; |
||
1719 | if (dz2 >= EPSILON && dz2 < len) |
||
1720 | len = dz2; |
||
1721 | |||
1722 | if (len == Single.MaxValue) |
||
1723 | { |
||
1724 | dx2 = dy2 = dz2 = 0.01f; // one centimeter |
||
1725 | } |
||
1726 | else |
||
1727 | { |
||
1728 | if (dx2 < EPSILON) // 1/5th the shortest non-zero edge. |
||
1729 | dx2 = len * 0.05f; |
||
1730 | if (dy2 < EPSILON) |
||
1731 | dy2 = len * 0.05f; |
||
1732 | if (dz2 < EPSILON) |
||
1733 | dz2 = len * 0.05f; |
||
1734 | } |
||
1735 | |||
1736 | float x1 = cx - dx2; |
||
1737 | float x2 = cx + dx2; |
||
1738 | |||
1739 | float y1 = cy - dy2; |
||
1740 | float y2 = cy + dy2; |
||
1741 | |||
1742 | float z1 = cz - dz2; |
||
1743 | float z2 = cz + dz2; |
||
1744 | |||
1745 | vcount = 0; // add box |
||
1746 | |||
1747 | addPoint(ref vcount, vertices, x1, y1, z1); |
||
1748 | addPoint(ref vcount, vertices, x2, y1, z1); |
||
1749 | addPoint(ref vcount, vertices, x2, y2, z1); |
||
1750 | addPoint(ref vcount, vertices, x1, y2, z1); |
||
1751 | addPoint(ref vcount, vertices, x1, y1, z2); |
||
1752 | addPoint(ref vcount, vertices, x2, y1, z2); |
||
1753 | addPoint(ref vcount, vertices, x2, y2, z2); |
||
1754 | addPoint(ref vcount, vertices, x1, y2, z2); |
||
1755 | |||
1756 | return true; |
||
1757 | } |
||
1758 | } |
||
1759 | |||
1760 | return true; |
||
1761 | } |
||
1762 | |||
1763 | private static void BringOutYourDead(List<float3> verts, out List<float3> overts, List<int> indices) |
||
1764 | { |
||
1765 | int[] used = new int[verts.Count]; |
||
1766 | int ocount = 0; |
||
1767 | |||
1768 | overts = new List<float3>(); |
||
1769 | |||
1770 | for (int i = 0; i < indices.Count; i++) |
||
1771 | { |
||
1772 | int v = indices[i]; // original array index |
||
1773 | |||
1774 | Debug.Assert(v >= 0 && v < verts.Count); |
||
1775 | |||
1776 | if (used[v] != 0) // if already remapped |
||
1777 | { |
||
1778 | indices[i] = used[v] - 1; // index to new array |
||
1779 | } |
||
1780 | else |
||
1781 | { |
||
1782 | indices[i] = ocount; // new index mapping |
||
1783 | |||
1784 | overts.Add(verts[v]); // copy old vert to new vert array |
||
1785 | |||
1786 | ocount++; // increment output vert count |
||
1787 | |||
1788 | Debug.Assert(ocount >= 0 && ocount <= verts.Count); |
||
1789 | |||
1790 | used[v] = ocount; // assign new index remapping |
||
1791 | } |
||
1792 | } |
||
1793 | } |
||
1794 | |||
1795 | public static HullError CreateConvexHull(HullDesc desc, ref HullResult result) |
||
1796 | { |
||
1797 | HullError ret = HullError.QE_FAIL; |
||
1798 | |||
1799 | PHullResult hr = new PHullResult(); |
||
1800 | |||
1801 | uint vcount = (uint)desc.Vertices.Count; |
||
1802 | if (vcount < 8) |
||
1803 | vcount = 8; |
||
1804 | |||
1805 | List<float3> vsource; |
||
1806 | float3 scale = new float3(); |
||
1807 | |||
1808 | bool ok = CleanupVertices(desc.Vertices, out vsource, desc.NormalEpsilon, out scale); // normalize point cloud, remove duplicates! |
||
1809 | |||
1810 | if (ok) |
||
1811 | { |
||
1812 | if (true) // scale vertices back to their original size. |
||
1813 | { |
||
1814 | for (int i = 0; i < vsource.Count; i++) |
||
1815 | { |
||
1816 | float3 v = vsource[i]; |
||
1817 | v.x *= scale[0]; |
||
1818 | v.y *= scale[1]; |
||
1819 | v.z *= scale[2]; |
||
1820 | } |
||
1821 | } |
||
1822 | |||
1823 | float skinwidth = 0; |
||
1824 | if (desc.HasHullFlag(HullFlag.QF_SKIN_WIDTH)) |
||
1825 | skinwidth = desc.SkinWidth; |
||
1826 | |||
1827 | ok = ComputeHull(vsource, ref hr, (int)desc.MaxVertices, skinwidth); |
||
1828 | |||
1829 | if (ok) |
||
1830 | { |
||
1831 | List<float3> vscratch; |
||
1832 | BringOutYourDead(hr.Vertices, out vscratch, hr.Indices); |
||
1833 | |||
1834 | ret = HullError.QE_OK; |
||
1835 | |||
1836 | if (desc.HasHullFlag(HullFlag.QF_TRIANGLES)) // if he wants the results as triangle! |
||
1837 | { |
||
1838 | result.Polygons = false; |
||
1839 | result.Indices = hr.Indices; |
||
1840 | result.OutputVertices = vscratch; |
||
1841 | } |
||
1842 | else |
||
1843 | { |
||
1844 | result.Polygons = true; |
||
1845 | result.OutputVertices = vscratch; |
||
1846 | |||
1847 | if (true) |
||
1848 | { |
||
1849 | List<int> source = hr.Indices; |
||
1850 | List<int> dest = new List<int>(); |
||
1851 | for (int i = 0; i < hr.Indices.Count / 3; i++) |
||
1852 | { |
||
1853 | dest.Add(3); |
||
1854 | dest.Add(source[i * 3 + 0]); |
||
1855 | dest.Add(source[i * 3 + 1]); |
||
1856 | dest.Add(source[i * 3 + 2]); |
||
1857 | } |
||
1858 | |||
1859 | result.Indices = dest; |
||
1860 | } |
||
1861 | } |
||
1862 | } |
||
1863 | } |
||
1864 | |||
1865 | return ret; |
||
1866 | } |
||
1867 | } |
||
1868 | } |