SplitPlane.cs 7.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265
  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 Rect3d
  32. {
  33. public float[] mMin = new float[3];
  34. public float[] mMax = new float[3];
  35. public Rect3d()
  36. {
  37. }
  38. public Rect3d(float[] bmin, float[] bmax)
  39. {
  40. mMin[0] = bmin[0];
  41. mMin[1] = bmin[1];
  42. mMin[2] = bmin[2];
  43. mMax[0] = bmax[0];
  44. mMax[1] = bmax[1];
  45. mMax[2] = bmax[2];
  46. }
  47. public void SetMin(float[] bmin)
  48. {
  49. mMin[0] = bmin[0];
  50. mMin[1] = bmin[1];
  51. mMin[2] = bmin[2];
  52. }
  53. public void SetMax(float[] bmax)
  54. {
  55. mMax[0] = bmax[0];
  56. mMax[1] = bmax[1];
  57. mMax[2] = bmax[2];
  58. }
  59. public void SetMin(float x, float y, float z)
  60. {
  61. mMin[0] = x;
  62. mMin[1] = y;
  63. mMin[2] = z;
  64. }
  65. public void SetMax(float x, float y, float z)
  66. {
  67. mMax[0] = x;
  68. mMax[1] = y;
  69. mMax[2] = z;
  70. }
  71. }
  72. public static class SplitPlane
  73. {
  74. public static bool computeSplitPlane(List<float3> vertices, List<int> indices, ref float4 plane)
  75. {
  76. float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue };
  77. float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue };
  78. for (int i = 0; i < vertices.Count; i++)
  79. {
  80. float3 p = vertices[i];
  81. if (p[0] < bmin[0])
  82. bmin[0] = p[0];
  83. if (p[1] < bmin[1])
  84. bmin[1] = p[1];
  85. if (p[2] < bmin[2])
  86. bmin[2] = p[2];
  87. if (p[0] > bmax[0])
  88. bmax[0] = p[0];
  89. if (p[1] > bmax[1])
  90. bmax[1] = p[1];
  91. if (p[2] > bmax[2])
  92. bmax[2] = p[2];
  93. }
  94. float dx = bmax[0] - bmin[0];
  95. float dy = bmax[1] - bmin[1];
  96. float dz = bmax[2] - bmin[2];
  97. float laxis = dx;
  98. int axis = 0;
  99. if (dy > dx)
  100. {
  101. axis = 1;
  102. laxis = dy;
  103. }
  104. if (dz > dx && dz > dy)
  105. {
  106. axis = 2;
  107. laxis = dz;
  108. }
  109. float[] p1 = new float[3];
  110. float[] p2 = new float[3];
  111. float[] p3 = new float[3];
  112. p3[0] = p2[0] = p1[0] = bmin[0] + dx * 0.5f;
  113. p3[1] = p2[1] = p1[1] = bmin[1] + dy * 0.5f;
  114. p3[2] = p2[2] = p1[2] = bmin[2] + dz * 0.5f;
  115. Rect3d b = new Rect3d(bmin, bmax);
  116. Rect3d b1 = new Rect3d();
  117. Rect3d b2 = new Rect3d();
  118. splitRect(axis, b, b1, b2, p1);
  119. switch (axis)
  120. {
  121. case 0:
  122. p2[1] = bmin[1];
  123. p2[2] = bmin[2];
  124. if (dz > dy)
  125. {
  126. p3[1] = bmax[1];
  127. p3[2] = bmin[2];
  128. }
  129. else
  130. {
  131. p3[1] = bmin[1];
  132. p3[2] = bmax[2];
  133. }
  134. break;
  135. case 1:
  136. p2[0] = bmin[0];
  137. p2[2] = bmin[2];
  138. if (dx > dz)
  139. {
  140. p3[0] = bmax[0];
  141. p3[2] = bmin[2];
  142. }
  143. else
  144. {
  145. p3[0] = bmin[0];
  146. p3[2] = bmax[2];
  147. }
  148. break;
  149. case 2:
  150. p2[0] = bmin[0];
  151. p2[1] = bmin[1];
  152. if (dx > dy)
  153. {
  154. p3[0] = bmax[0];
  155. p3[1] = bmin[1];
  156. }
  157. else
  158. {
  159. p3[0] = bmin[0];
  160. p3[1] = bmax[1];
  161. }
  162. break;
  163. }
  164. computePlane(p1, p2, p3, plane);
  165. return true;
  166. }
  167. internal static void computePlane(float[] A, float[] B, float[] C, float4 plane)
  168. {
  169. float vx = (B[0] - C[0]);
  170. float vy = (B[1] - C[1]);
  171. float vz = (B[2] - C[2]);
  172. float wx = (A[0] - B[0]);
  173. float wy = (A[1] - B[1]);
  174. float wz = (A[2] - B[2]);
  175. float vw_x = vy * wz - vz * wy;
  176. float vw_y = vz * wx - vx * wz;
  177. float vw_z = vx * wy - vy * wx;
  178. float mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
  179. if (mag < 0.000001f)
  180. {
  181. mag = 0;
  182. }
  183. else
  184. {
  185. mag = 1.0f / mag;
  186. }
  187. float x = vw_x * mag;
  188. float y = vw_y * mag;
  189. float z = vw_z * mag;
  190. float D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2]));
  191. plane.x = x;
  192. plane.y = y;
  193. plane.z = z;
  194. plane.w = D;
  195. }
  196. public static void splitRect(int axis, Rect3d source, Rect3d b1, Rect3d b2, float[] midpoint)
  197. {
  198. switch (axis)
  199. {
  200. case 0:
  201. b1.SetMin(source.mMin);
  202. b1.SetMax(midpoint[0], source.mMax[1], source.mMax[2]);
  203. b2.SetMin(midpoint[0], source.mMin[1], source.mMin[2]);
  204. b2.SetMax(source.mMax);
  205. break;
  206. case 1:
  207. b1.SetMin(source.mMin);
  208. b1.SetMax(source.mMax[0], midpoint[1], source.mMax[2]);
  209. b2.SetMin(source.mMin[0], midpoint[1], source.mMin[2]);
  210. b2.SetMax(source.mMax);
  211. break;
  212. case 2:
  213. b1.SetMin(source.mMin);
  214. b1.SetMax(source.mMax[0], source.mMax[1], midpoint[2]);
  215. b2.SetMin(source.mMin[0], source.mMin[1], midpoint[2]);
  216. b2.SetMax(source.mMax);
  217. break;
  218. }
  219. }
  220. }
  221. }