HullUtils.cs 73 KB

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