diff options
author | Robert Adams | 2015-09-08 04:54:16 -0700 |
---|---|---|
committer | Robert Adams | 2015-09-08 04:54:16 -0700 |
commit | e5367d822be9b05e74c859afe2d2956a3e95aa33 (patch) | |
tree | e904050a30715df587aa527d7f313755177726a7 /OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs | |
parent | add lost admin_reset_land method (diff) | |
parent | Deleted access control spec from [LoginService] section of standalone config.... (diff) | |
download | opensim-SC_OLD-e5367d822be9b05e74c859afe2d2956a3e95aa33.zip opensim-SC_OLD-e5367d822be9b05e74c859afe2d2956a3e95aa33.tar.gz opensim-SC_OLD-e5367d822be9b05e74c859afe2d2956a3e95aa33.tar.bz2 opensim-SC_OLD-e5367d822be9b05e74c859afe2d2956a3e95aa33.tar.xz |
Merge of ubitworkvarnew with opensim/master as of 20150905.
This integrates the OpenSim refactoring to make physics, etc into modules.
AVN physics hasn't been moved to new location.
Does not compile yet.
Merge branch 'osmaster' into mbworknew1
Diffstat (limited to 'OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs | 1868 |
1 files changed, 1868 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs new file mode 100644 index 0000000..3903254 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs | |||
@@ -0,0 +1,1868 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.PhysicsModule.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 | } | ||