CTri.cs 9.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341
  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. namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
  30. {
  31. public class Wpoint
  32. {
  33. public float3 mPoint;
  34. public float mWeight;
  35. public Wpoint(float3 p, float w)
  36. {
  37. mPoint = p;
  38. mWeight = w;
  39. }
  40. }
  41. public class CTri
  42. {
  43. private const int WSCALE = 4;
  44. public float3 mP1;
  45. public float3 mP2;
  46. public float3 mP3;
  47. public float3 mNear1;
  48. public float3 mNear2;
  49. public float3 mNear3;
  50. public float3 mNormal;
  51. public float mPlaneD;
  52. public float mConcavity;
  53. public float mC1;
  54. public float mC2;
  55. public float mC3;
  56. public int mI1;
  57. public int mI2;
  58. public int mI3;
  59. public int mProcessed; // already been added...
  60. public CTri(float3 p1, float3 p2, float3 p3, int i1, int i2, int i3)
  61. {
  62. mProcessed = 0;
  63. mI1 = i1;
  64. mI2 = i2;
  65. mI3 = i3;
  66. mP1 = new float3(p1);
  67. mP2 = new float3(p2);
  68. mP3 = new float3(p3);
  69. mNear1 = new float3();
  70. mNear2 = new float3();
  71. mNear3 = new float3();
  72. mNormal = new float3();
  73. mPlaneD = mNormal.ComputePlane(mP1, mP2, mP3);
  74. }
  75. public float Facing(CTri t)
  76. {
  77. return float3.dot(mNormal, t.mNormal);
  78. }
  79. public bool clip(float3 start, ref float3 end)
  80. {
  81. float3 sect = new float3();
  82. bool hit = lineIntersectsTriangle(start, end, mP1, mP2, mP3, ref sect);
  83. if (hit)
  84. end = sect;
  85. return hit;
  86. }
  87. public bool Concave(float3 p, ref float distance, ref float3 n)
  88. {
  89. n.NearestPointInTriangle(p, mP1, mP2, mP3);
  90. distance = p.Distance(n);
  91. return true;
  92. }
  93. public void addTri(int[] indices, int i1, int i2, int i3, ref int tcount)
  94. {
  95. indices[tcount * 3 + 0] = i1;
  96. indices[tcount * 3 + 1] = i2;
  97. indices[tcount * 3 + 2] = i3;
  98. tcount++;
  99. }
  100. public float getVolume()
  101. {
  102. int[] indices = new int[8 * 3];
  103. int tcount = 0;
  104. addTri(indices, 0, 1, 2, ref tcount);
  105. addTri(indices, 3, 4, 5, ref tcount);
  106. addTri(indices, 0, 3, 4, ref tcount);
  107. addTri(indices, 0, 4, 1, ref tcount);
  108. addTri(indices, 1, 4, 5, ref tcount);
  109. addTri(indices, 1, 5, 2, ref tcount);
  110. addTri(indices, 0, 3, 5, ref tcount);
  111. addTri(indices, 0, 5, 2, ref tcount);
  112. List<float3> vertices = new List<float3> { mP1, mP2, mP3, mNear1, mNear2, mNear3 };
  113. List<int> indexList = new List<int>(indices);
  114. float v = Concavity.computeMeshVolume(vertices, indexList);
  115. return v;
  116. }
  117. public float raySect(float3 p, float3 dir, ref float3 sect)
  118. {
  119. float4 plane = new float4();
  120. plane.x = mNormal.x;
  121. plane.y = mNormal.y;
  122. plane.z = mNormal.z;
  123. plane.w = mPlaneD;
  124. float3 dest = p + dir * 100000f;
  125. intersect(p, dest, ref sect, plane);
  126. return sect.Distance(p); // return the intersection distance
  127. }
  128. public float planeDistance(float3 p)
  129. {
  130. float4 plane = new float4();
  131. plane.x = mNormal.x;
  132. plane.y = mNormal.y;
  133. plane.z = mNormal.z;
  134. plane.w = mPlaneD;
  135. return DistToPt(p, plane);
  136. }
  137. public bool samePlane(CTri t)
  138. {
  139. const float THRESH = 0.001f;
  140. float dd = Math.Abs(t.mPlaneD - mPlaneD);
  141. if (dd > THRESH)
  142. return false;
  143. dd = Math.Abs(t.mNormal.x - mNormal.x);
  144. if (dd > THRESH)
  145. return false;
  146. dd = Math.Abs(t.mNormal.y - mNormal.y);
  147. if (dd > THRESH)
  148. return false;
  149. dd = Math.Abs(t.mNormal.z - mNormal.z);
  150. if (dd > THRESH)
  151. return false;
  152. return true;
  153. }
  154. public bool hasIndex(int i)
  155. {
  156. if (i == mI1 || i == mI2 || i == mI3)
  157. return true;
  158. return false;
  159. }
  160. public bool sharesEdge(CTri t)
  161. {
  162. bool ret = false;
  163. uint count = 0;
  164. if (t.hasIndex(mI1))
  165. count++;
  166. if (t.hasIndex(mI2))
  167. count++;
  168. if (t.hasIndex(mI3))
  169. count++;
  170. if (count >= 2)
  171. ret = true;
  172. return ret;
  173. }
  174. public float area()
  175. {
  176. float a = mConcavity * mP1.Area(mP2, mP3);
  177. return a;
  178. }
  179. public void addWeighted(List<Wpoint> list)
  180. {
  181. Wpoint p1 = new Wpoint(mP1, mC1);
  182. Wpoint p2 = new Wpoint(mP2, mC2);
  183. Wpoint p3 = new Wpoint(mP3, mC3);
  184. float3 d1 = mNear1 - mP1;
  185. float3 d2 = mNear2 - mP2;
  186. float3 d3 = mNear3 - mP3;
  187. d1 *= WSCALE;
  188. d2 *= WSCALE;
  189. d3 *= WSCALE;
  190. d1 = d1 + mP1;
  191. d2 = d2 + mP2;
  192. d3 = d3 + mP3;
  193. Wpoint p4 = new Wpoint(d1, mC1);
  194. Wpoint p5 = new Wpoint(d2, mC2);
  195. Wpoint p6 = new Wpoint(d3, mC3);
  196. list.Add(p1);
  197. list.Add(p2);
  198. list.Add(p3);
  199. list.Add(p4);
  200. list.Add(p5);
  201. list.Add(p6);
  202. }
  203. private static float DistToPt(float3 p, float4 plane)
  204. {
  205. float x = p.x;
  206. float y = p.y;
  207. float z = p.z;
  208. float d = x*plane.x + y*plane.y + z*plane.z + plane.w;
  209. return d;
  210. }
  211. private static void intersect(float3 p1, float3 p2, ref float3 split, float4 plane)
  212. {
  213. float dp1 = DistToPt(p1, plane);
  214. float3 dir = new float3();
  215. dir.x = p2[0] - p1[0];
  216. dir.y = p2[1] - p1[1];
  217. dir.z = p2[2] - p1[2];
  218. float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2];
  219. float dot2 = dp1 - plane[3];
  220. float t = -(plane[3] + dot2) / dot1;
  221. split.x = (dir[0] * t) + p1[0];
  222. split.y = (dir[1] * t) + p1[1];
  223. split.z = (dir[2] * t) + p1[2];
  224. }
  225. private static bool rayIntersectsTriangle(float3 p, float3 d, float3 v0, float3 v1, float3 v2, out float t)
  226. {
  227. t = 0f;
  228. float3 e1, e2, h, s, q;
  229. float a, f, u, v;
  230. e1 = v1 - v0;
  231. e2 = v2 - v0;
  232. h = float3.cross(d, e2);
  233. a = float3.dot(e1, h);
  234. if (a > -0.00001f && a < 0.00001f)
  235. return false;
  236. f = 1f / a;
  237. s = p - v0;
  238. u = f * float3.dot(s, h);
  239. if (u < 0.0f || u > 1.0f)
  240. return false;
  241. q = float3.cross(s, e1);
  242. v = f * float3.dot(d, q);
  243. if (v < 0.0f || u + v > 1.0f)
  244. return false;
  245. // at this stage we can compute t to find out where
  246. // the intersection point is on the line
  247. t = f * float3.dot(e2, q);
  248. if (t > 0f) // ray intersection
  249. return true;
  250. else // this means that there is a line intersection but not a ray intersection
  251. return false;
  252. }
  253. private static bool lineIntersectsTriangle(float3 rayStart, float3 rayEnd, float3 p1, float3 p2, float3 p3, ref float3 sect)
  254. {
  255. float3 dir = rayEnd - rayStart;
  256. float d = (float)Math.Sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
  257. float r = 1.0f / d;
  258. dir *= r;
  259. float t;
  260. bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, out t);
  261. if (ret)
  262. {
  263. if (t > d)
  264. {
  265. sect.x = rayStart.x + dir.x * t;
  266. sect.y = rayStart.y + dir.y * t;
  267. sect.z = rayStart.z + dir.z * t;
  268. }
  269. else
  270. {
  271. ret = false;
  272. }
  273. }
  274. return ret;
  275. }
  276. }
  277. }