PlaneTri.cs 7.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211
  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 enum PlaneTriResult : int
  33. {
  34. PTR_FRONT,
  35. PTR_BACK,
  36. PTR_SPLIT
  37. }
  38. public static class PlaneTri
  39. {
  40. private static float DistToPt(float3 p, float4 plane)
  41. {
  42. return p.x * plane.x + p.y * plane.y + p.z * plane.z + plane.w;
  43. }
  44. private static PlaneTriResult getSidePlane(float3 p, float4 plane, float epsilon)
  45. {
  46. float d = DistToPt(p, plane);
  47. if ((d + epsilon) > 0f)
  48. return PlaneTriResult.PTR_FRONT; // it is 'in front' within the provided epsilon value.
  49. return PlaneTriResult.PTR_BACK;
  50. }
  51. private static void add(float3 p, float3[] dest, ref int pcount)
  52. {
  53. dest[pcount++] = new float3(p);
  54. Debug.Assert(pcount <= 4);
  55. }
  56. // assumes that the points are on opposite sides of the plane!
  57. private static void intersect(float3 p1, float3 p2, float3 split, float4 plane)
  58. {
  59. float dp1 = DistToPt(p1, plane);
  60. float[] dir = new float[3];
  61. dir[0] = p2[0] - p1[0];
  62. dir[1] = p2[1] - p1[1];
  63. dir[2] = p2[2] - p1[2];
  64. float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2];
  65. float dot2 = dp1 - plane[3];
  66. float t = -(plane[3] + dot2) / dot1;
  67. split.x = (dir[0] * t) + p1[0];
  68. split.y = (dir[1] * t) + p1[1];
  69. split.z = (dir[2] * t) + p1[2];
  70. }
  71. public static PlaneTriResult planeTriIntersection(float4 plane, FaceTri triangle, float epsilon, ref float3[] front, out int fcount, ref float3[] back, out int bcount)
  72. {
  73. fcount = 0;
  74. bcount = 0;
  75. // get the three vertices of the triangle.
  76. float3 p1 = triangle.P1;
  77. float3 p2 = triangle.P2;
  78. float3 p3 = triangle.P3;
  79. PlaneTriResult r1 = getSidePlane(p1, plane, epsilon); // compute the side of the plane each vertex is on
  80. PlaneTriResult r2 = getSidePlane(p2, plane, epsilon);
  81. PlaneTriResult r3 = getSidePlane(p3, plane, epsilon);
  82. if (r1 == r2 && r1 == r3) // if all three vertices are on the same side of the plane.
  83. {
  84. if (r1 == PlaneTriResult.PTR_FRONT) // if all three are in front of the plane, then copy to the 'front' output triangle.
  85. {
  86. add(p1, front, ref fcount);
  87. add(p2, front, ref fcount);
  88. add(p3, front, ref fcount);
  89. }
  90. else
  91. {
  92. add(p1, back, ref bcount); // if all three are in 'back' then copy to the 'back' output triangle.
  93. add(p2, back, ref bcount);
  94. add(p3, back, ref bcount);
  95. }
  96. return r1; // if all three points are on the same side of the plane return result
  97. }
  98. // ok.. we need to split the triangle at the plane.
  99. // First test ray segment P1 to P2
  100. if (r1 == r2) // if these are both on the same side...
  101. {
  102. if (r1 == PlaneTriResult.PTR_FRONT)
  103. {
  104. add(p1, front, ref fcount);
  105. add(p2, front, ref fcount);
  106. }
  107. else
  108. {
  109. add(p1, back, ref bcount);
  110. add(p2, back, ref bcount);
  111. }
  112. }
  113. else
  114. {
  115. float3 split = new float3();
  116. intersect(p1, p2, split, plane);
  117. if (r1 == PlaneTriResult.PTR_FRONT)
  118. {
  119. add(p1, front, ref fcount);
  120. add(split, front, ref fcount);
  121. add(split, back, ref bcount);
  122. add(p2, back, ref bcount);
  123. }
  124. else
  125. {
  126. add(p1, back, ref bcount);
  127. add(split, back, ref bcount);
  128. add(split, front, ref fcount);
  129. add(p2, front, ref fcount);
  130. }
  131. }
  132. // Next test ray segment P2 to P3
  133. if (r2 == r3) // if these are both on the same side...
  134. {
  135. if (r3 == PlaneTriResult.PTR_FRONT)
  136. {
  137. add(p3, front, ref fcount);
  138. }
  139. else
  140. {
  141. add(p3, back, ref bcount);
  142. }
  143. }
  144. else
  145. {
  146. float3 split = new float3(); // split the point
  147. intersect(p2, p3, split, plane);
  148. if (r3 == PlaneTriResult.PTR_FRONT)
  149. {
  150. add(split, front, ref fcount);
  151. add(split, back, ref bcount);
  152. add(p3, front, ref fcount);
  153. }
  154. else
  155. {
  156. add(split, front, ref fcount);
  157. add(split, back, ref bcount);
  158. add(p3, back, ref bcount);
  159. }
  160. }
  161. // Next test ray segment P3 to P1
  162. if (r3 != r1) // if these are both on the same side...
  163. {
  164. float3 split = new float3(); // split the point
  165. intersect(p3, p1, split, plane);
  166. if (r1 == PlaneTriResult.PTR_FRONT)
  167. {
  168. add(split, front, ref fcount);
  169. add(split, back, ref bcount);
  170. }
  171. else
  172. {
  173. add(split, front, ref fcount);
  174. add(split, back, ref bcount);
  175. }
  176. }
  177. return PlaneTriResult.PTR_SPLIT;
  178. }
  179. }
  180. }