Polygon3DQuickHull.cs 7.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180
  1. #if UNITY_EDITOR
  2. using UnityEngine;
  3. using System.Collections.Generic;
  4. namespace O3DWB
  5. {
  6. public class Polygon3DQuickHull
  7. {
  8. #region Private Variables
  9. private List<Vector3> _polygonPointsOnSamePlane;
  10. private Vector3 _polygonNormal;
  11. #endregion
  12. #region Constructors
  13. public Polygon3DQuickHull(List<Vector3> polygonPointsOnSamePlane, Vector3 polygonPlaneNormal)
  14. {
  15. _polygonPointsOnSamePlane = new List<Vector3>(polygonPointsOnSamePlane);
  16. _polygonNormal = polygonPlaneNormal;
  17. _polygonNormal.Normalize();
  18. }
  19. #endregion
  20. #region Public Methods
  21. public List<Segment3D> Calculate()
  22. {
  23. Vector3Extensions.EliminateDuplicatePoints(_polygonPointsOnSamePlane);
  24. if (_polygonPointsOnSamePlane.Count < 3) return new List<Segment3D>();
  25. List<Vector3> workingPointList = new List<Vector3>(_polygonPointsOnSamePlane);
  26. Vector3 minPoint, maxPoint;
  27. CalculateInitialMinMaxPoints(out minPoint, out maxPoint);
  28. workingPointList.RemoveAll(item => item == minPoint || item == maxPoint);
  29. List<Segment3D> allHullSegments = new List<Segment3D>();
  30. Segment3D minMaxSegment = new Segment3D(minPoint, maxPoint);
  31. Plane minMaxSegmentPlane = GetSegmentPlane(minMaxSegment);
  32. bool furthestPointIsBehind = false;
  33. int indexOfPointFurthestFromPlane = minMaxSegmentPlane.GetIndexOfFurthestPointInFront(workingPointList);
  34. if (indexOfPointFurthestFromPlane < 0)
  35. {
  36. indexOfPointFurthestFromPlane = minMaxSegmentPlane.GetIndexOfFurthestPointBehind(workingPointList);
  37. furthestPointIsBehind = true;
  38. }
  39. if (indexOfPointFurthestFromPlane >= 0)
  40. {
  41. Vector3 furthestPointFromPlane = workingPointList[indexOfPointFurthestFromPlane];
  42. workingPointList.RemoveAt(indexOfPointFurthestFromPlane);
  43. List<Segment3D> hullSegments = new List<Segment3D>();
  44. if(furthestPointIsBehind)
  45. {
  46. hullSegments.Add(new Segment3D(minMaxSegment.EndPoint, furthestPointFromPlane));
  47. hullSegments.Add(new Segment3D(furthestPointFromPlane, minMaxSegment.StartPoint));
  48. hullSegments.Add(minMaxSegment);
  49. QuickHull(workingPointList, hullSegments);
  50. }
  51. else
  52. {
  53. hullSegments.Add(new Segment3D(furthestPointFromPlane, minMaxSegment.EndPoint));
  54. hullSegments.Add(new Segment3D(minMaxSegment.StartPoint, furthestPointFromPlane));
  55. hullSegments.Add(new Segment3D(minMaxSegment.EndPoint, minMaxSegment.StartPoint)); // Reversed min-max segment
  56. QuickHull(workingPointList, hullSegments);
  57. }
  58. allHullSegments.AddRange(hullSegments);
  59. }
  60. return allHullSegments;
  61. }
  62. #endregion
  63. #region Private Methods
  64. private void CalculateInitialMinMaxPoints(out Vector3 minPoint, out Vector3 maxPoint)
  65. {
  66. // We will sort the points along the X axis, but we have to take the polygon's coordinate
  67. // system into account because if we use the global X axis, we will run into trouble when
  68. // all points of the polygon reside on the YZ plane (i.e. same X coordinate).
  69. Vector3 polyLocalRight, polyLocalLook;
  70. if(_polygonNormal.IsAlignedWith(Vector3.up))
  71. {
  72. polyLocalRight = Vector3.right;
  73. polyLocalLook = Vector3.Cross(polyLocalRight, Vector3.up);
  74. polyLocalLook.Normalize();
  75. }
  76. else
  77. {
  78. polyLocalRight = Vector3.Cross(_polygonNormal, Vector3.up);
  79. polyLocalRight.Normalize();
  80. polyLocalLook = Vector3.Cross(polyLocalRight, _polygonNormal);
  81. polyLocalLook.Normalize();
  82. }
  83. Quaternion polyRotation = Quaternion.LookRotation(polyLocalLook, _polygonNormal);
  84. TransformMatrix transformMatrix = new TransformMatrix(Vector3.zero, polyRotation, Vector3.one);
  85. // Note: We will work in polygon local space and transform the points to world space after we are done.
  86. minPoint = transformMatrix.MultiplyPointInverse(_polygonPointsOnSamePlane[0]);
  87. maxPoint = transformMatrix.MultiplyPointInverse(_polygonPointsOnSamePlane[0]);
  88. for(int ptIndex = 0; ptIndex < _polygonPointsOnSamePlane.Count; ++ptIndex)
  89. {
  90. Vector3 point = transformMatrix.MultiplyPointInverse(_polygonPointsOnSamePlane[ptIndex]);
  91. if (point.x < minPoint.x) minPoint = point;
  92. if (point.x > maxPoint.x) maxPoint = point;
  93. }
  94. minPoint = transformMatrix.MultiplyPoint(minPoint);
  95. maxPoint = transformMatrix.MultiplyPoint(maxPoint);
  96. }
  97. private void QuickHull(List<Vector3> workingPoints, List<Segment3D> hullSegments)
  98. {
  99. // First, remove all points which lie inside the hull
  100. for (int pointIndex = 0; pointIndex < workingPoints.Count; )
  101. {
  102. Vector3 point = workingPoints[pointIndex];
  103. if (IsPointInsideHull(hullSegments, point)) workingPoints.RemoveAt(pointIndex);
  104. else ++pointIndex;
  105. }
  106. // Loop thorugh each existing hull segment and create new segments as necessary
  107. var newSegments = new List<Segment3D>();
  108. for(int segmentIndex = 0; segmentIndex < hullSegments.Count;)
  109. {
  110. Segment3D currentSegment = hullSegments[segmentIndex];
  111. Plane currentSegmentPlane = GetSegmentPlane(currentSegment);
  112. // Expand the hull by creating new segments if necessary
  113. int indexOfFurthestPointInFront = currentSegmentPlane.GetIndexOfFurthestPointInFront(workingPoints);
  114. if (indexOfFurthestPointInFront >= 0)
  115. {
  116. Vector3 furthestPointInFront = workingPoints[indexOfFurthestPointInFront];
  117. hullSegments.RemoveAt(segmentIndex);
  118. Segment3D firstSegment = new Segment3D(currentSegment.StartPoint, furthestPointInFront);
  119. Segment3D secondSegment = new Segment3D(furthestPointInFront, currentSegment.EndPoint);
  120. newSegments.Add(firstSegment);
  121. newSegments.Add(secondSegment);
  122. workingPoints.RemoveAt(indexOfFurthestPointInFront);
  123. }
  124. else ++segmentIndex;
  125. }
  126. if (newSegments.Count != 0)
  127. {
  128. hullSegments.AddRange(newSegments);
  129. QuickHull(workingPoints, hullSegments);
  130. }
  131. }
  132. private bool IsPointInsideHull(List<Segment3D> currentHullSegments, Vector3 point)
  133. {
  134. for (int segmentIndex = 0; segmentIndex < currentHullSegments.Count; ++segmentIndex)
  135. {
  136. Segment3D currentSegment = currentHullSegments[segmentIndex];
  137. if (!IsPointBehindSegmentPlane(currentSegment, point)) return false;
  138. }
  139. return true;
  140. }
  141. private bool IsPointBehindSegmentPlane(Segment3D segment, Vector3 point)
  142. {
  143. Plane segmentPlane = GetSegmentPlane(segment);
  144. return segmentPlane.IsPointBehind(point);
  145. }
  146. private Plane GetSegmentPlane(Segment3D segment)
  147. {
  148. Vector3 segmentPlaneNormal = Vector3.Cross(segment.Direction, _polygonNormal);
  149. segmentPlaneNormal.Normalize();
  150. return new Plane(segmentPlaneNormal, segment.StartPoint);
  151. }
  152. #endregion
  153. }
  154. }
  155. #endif