PlaneExtensions.cs 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569
  1. #if UNITY_EDITOR
  2. using UnityEngine;
  3. using System.Collections.Generic;
  4. namespace O3DWB
  5. {
  6. public static class PlaneExtensions
  7. {
  8. #region Extension Methods
  9. public static Plane Transform(this Plane plane, Matrix4x4 transformMatrix, Vector3 pointOnPlane)
  10. {
  11. Vector3 transformedPoint = transformMatrix.MultiplyPoint(pointOnPlane);
  12. Vector3 transformedNormal = transformMatrix.MultiplyVector(plane.normal);
  13. transformedNormal.Normalize();
  14. return new Plane(transformedNormal, transformedPoint);
  15. }
  16. public static Vector3 ProjectPoint(this Plane plane, Vector3 pointToProject)
  17. {
  18. float planeDistanceFromPoint = plane.GetDistanceToPoint(pointToProject);
  19. return pointToProject - plane.normal * planeDistanceFromPoint;
  20. }
  21. public static List<Vector3> ProjectAllPoints(this Plane plane, List<Vector3> pointsToProject)
  22. {
  23. if (pointsToProject.Count == 0) return new List<Vector3>();
  24. var projectedPoints = new List<Vector3>(pointsToProject.Count);
  25. foreach(Vector3 point in pointsToProject)
  26. {
  27. projectedPoints.Add(plane.ProjectPoint(point));
  28. }
  29. return projectedPoints;
  30. }
  31. public static bool AreAllPointsInFrontOrOnPlane(this Plane plane, List<Vector3> points)
  32. {
  33. foreach(Vector3 point in points)
  34. {
  35. if (plane.IsPointBehind(point)) return false;
  36. }
  37. return true;
  38. }
  39. public static bool AreAllPointsInFrontOrBehindPlane(this Plane plane, List<Vector3> points)
  40. {
  41. bool foundPointInFront = false;
  42. bool foundPointBehind = false;
  43. foreach (Vector3 point in points)
  44. {
  45. if (plane.IsPointOnPlane(point)) return false;
  46. if(plane.IsPointInFront(point))
  47. {
  48. if (foundPointBehind) return false;
  49. foundPointInFront = true;
  50. }
  51. else
  52. if(plane.IsPointBehind(point))
  53. {
  54. if (foundPointInFront) return false;
  55. foundPointBehind = true;
  56. }
  57. }
  58. return true;
  59. }
  60. public static List<Vector3> GetAllPointsInFront(this Plane plane, List<Vector3> points)
  61. {
  62. if(points.Count == 0) return new List<Vector3>();
  63. var allPointsInFront = new List<Vector3>(points.Count);
  64. foreach(Vector3 point in points)
  65. {
  66. if(plane.IsPointInFront(point)) allPointsInFront.Add(point);
  67. }
  68. return allPointsInFront;
  69. }
  70. public static List<Vector3> GetAllPointsBehind(this Plane plane, List<Vector3> points)
  71. {
  72. if(points.Count == 0) return new List<Vector3>();
  73. var allPointsBehind = new List<Vector3>(points.Count);
  74. foreach(Vector3 point in points)
  75. {
  76. if(plane.IsPointBehind(point)) allPointsBehind.Add(point);
  77. }
  78. return allPointsBehind;
  79. }
  80. public static Vector3 GetClosestPointToPlane(this Plane plane, List<Vector3> points)
  81. {
  82. float minDistanceToPoint = float.MaxValue;
  83. Vector3 closestPoint = Vector3.zero;
  84. foreach(Vector3 point in points)
  85. {
  86. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  87. if(planeDistanceToPoint < minDistanceToPoint)
  88. {
  89. minDistanceToPoint = planeDistanceToPoint;
  90. closestPoint = point;
  91. }
  92. }
  93. return closestPoint;
  94. }
  95. public static bool GetClosestPointInFront(this Plane plane, List<Vector3> points, out Vector3 closestPointInFront)
  96. {
  97. float minDistanceToPoint = float.MaxValue;
  98. closestPointInFront = Vector3.zero;
  99. bool foundPoint = false;
  100. foreach (Vector3 point in points)
  101. {
  102. if (plane.IsPointInFront(point))
  103. {
  104. foundPoint = true;
  105. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  106. if (planeDistanceToPoint < minDistanceToPoint)
  107. {
  108. minDistanceToPoint = planeDistanceToPoint;
  109. closestPointInFront = point;
  110. }
  111. }
  112. }
  113. return foundPoint;
  114. }
  115. public static bool GetClosestPointBehind(this Plane plane, List<Vector3> points, out Vector3 closestPointBehind)
  116. {
  117. float minDistanceToPoint = float.MaxValue;
  118. closestPointBehind = Vector3.zero;
  119. bool foundPoint = false;
  120. foreach (Vector3 point in points)
  121. {
  122. if (plane.IsPointBehind(point))
  123. {
  124. foundPoint = true;
  125. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  126. if (planeDistanceToPoint < minDistanceToPoint)
  127. {
  128. minDistanceToPoint = planeDistanceToPoint;
  129. closestPointBehind = point;
  130. }
  131. }
  132. }
  133. return foundPoint;
  134. }
  135. public static Vector3 GetFurthestPointFromPlane(this Plane plane, List<Vector3> points)
  136. {
  137. float maxDistanceToPoint = float.MinValue;
  138. Vector3 furthestPoint = Vector3.zero;
  139. foreach (Vector3 point in points)
  140. {
  141. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  142. if (planeDistanceToPoint > maxDistanceToPoint)
  143. {
  144. maxDistanceToPoint = planeDistanceToPoint;
  145. furthestPoint = point;
  146. }
  147. }
  148. return furthestPoint;
  149. }
  150. public static bool GetFurthestPointInFront(this Plane plane, List<Vector3> points, out Vector3 furthestPointInFront)
  151. {
  152. float maxDistanceToPoint = float.MinValue;
  153. furthestPointInFront = Vector3.zero;
  154. bool foundPoint = false;
  155. foreach (Vector3 point in points)
  156. {
  157. if (plane.IsPointInFront(point))
  158. {
  159. foundPoint = true;
  160. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  161. if (planeDistanceToPoint > maxDistanceToPoint)
  162. {
  163. maxDistanceToPoint = planeDistanceToPoint;
  164. furthestPointInFront = point;
  165. }
  166. }
  167. }
  168. return foundPoint;
  169. }
  170. public static int GetIndexOfFurthestPointInFront(this Plane plane, List<Vector3> points)
  171. {
  172. float maxDistanceToPoint = float.MinValue;
  173. int indexOfFurthestPointInFront = -1;
  174. for (int pointIndex = 0; pointIndex < points.Count; ++pointIndex)
  175. {
  176. Vector3 point = points[pointIndex];
  177. if (plane.IsPointInFront(point))
  178. {
  179. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  180. if (planeDistanceToPoint > maxDistanceToPoint)
  181. {
  182. maxDistanceToPoint = planeDistanceToPoint;
  183. indexOfFurthestPointInFront = pointIndex;
  184. }
  185. }
  186. }
  187. return indexOfFurthestPointInFront;
  188. }
  189. public static int GetIndexOfFurthestPointBehind(this Plane plane, List<Vector3> points)
  190. {
  191. float maxDistanceToPoint = float.MinValue;
  192. int indexOfFurthestPointBehind = -1;
  193. for (int pointIndex = 0; pointIndex < points.Count; ++pointIndex)
  194. {
  195. Vector3 point = points[pointIndex];
  196. if (plane.IsPointBehind(point))
  197. {
  198. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  199. if (planeDistanceToPoint > maxDistanceToPoint)
  200. {
  201. maxDistanceToPoint = planeDistanceToPoint;
  202. indexOfFurthestPointBehind = pointIndex;
  203. }
  204. }
  205. }
  206. return indexOfFurthestPointBehind;
  207. }
  208. public static int GetIndexOfClosestPointInFront(this Plane plane, List<Vector3> points)
  209. {
  210. float minDistance = float.MaxValue;
  211. int ptIndex = -1;
  212. for (int pointIndex = 0; pointIndex < points.Count; ++pointIndex)
  213. {
  214. Vector3 point = points[pointIndex];
  215. if (plane.IsPointInFront(point))
  216. {
  217. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  218. if (planeDistanceToPoint < minDistance)
  219. {
  220. minDistance = planeDistanceToPoint;
  221. ptIndex = pointIndex;
  222. }
  223. }
  224. }
  225. return ptIndex;
  226. }
  227. public static int GetIndexOfClosestPointInFrontOrOnPlane(this Plane plane, List<Vector3> points)
  228. {
  229. float minDistance = float.MaxValue;
  230. int ptIndex = -1;
  231. for (int pointIndex = 0; pointIndex < points.Count; ++pointIndex)
  232. {
  233. Vector3 point = points[pointIndex];
  234. if (plane.IsPointOnPlane(point) || plane.IsPointInFront(point))
  235. {
  236. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  237. if (planeDistanceToPoint < minDistance)
  238. {
  239. minDistance = planeDistanceToPoint;
  240. ptIndex = pointIndex;
  241. }
  242. }
  243. }
  244. return ptIndex;
  245. }
  246. public static bool GetFurthestPointBehind(this Plane plane, List<Vector3> points, out Vector3 furthestPointBehind)
  247. {
  248. float maxDistanceToPoint = float.MinValue;
  249. furthestPointBehind = Vector3.zero;
  250. bool foundPoint = false;
  251. foreach (Vector3 point in points)
  252. {
  253. if(plane.IsPointBehind(point))
  254. {
  255. foundPoint = true;
  256. float planeDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  257. if (planeDistanceToPoint > maxDistanceToPoint)
  258. {
  259. maxDistanceToPoint = planeDistanceToPoint;
  260. furthestPointBehind = point;
  261. }
  262. }
  263. }
  264. return foundPoint;
  265. }
  266. public static bool GetFirstPointOnPlane(this Plane plane, List<Vector3> points, out Vector3 firstPointInPlane)
  267. {
  268. firstPointInPlane = Vector3.zero;
  269. bool foundPoint = false;
  270. foreach (Vector3 point in points)
  271. {
  272. if (plane.IsPointOnPlane(point))
  273. {
  274. foundPoint = true;
  275. firstPointInPlane = point;
  276. break;
  277. }
  278. }
  279. return foundPoint;
  280. }
  281. public static float GetAbsDistanceToPoint(this Plane plane, Vector3 point)
  282. {
  283. return Mathf.Abs(plane.GetDistanceToPoint(point));
  284. }
  285. public static bool IsAnyPointOnPlane(this Plane plane, List<Vector3> points)
  286. {
  287. bool foundPoint = false;
  288. foreach (Vector3 point in points)
  289. {
  290. if (plane.IsPointOnPlane(point))
  291. {
  292. foundPoint = true;
  293. break;
  294. }
  295. }
  296. return foundPoint;
  297. }
  298. public static bool IsPointBehind(this Plane plane, Vector3 point)
  299. {
  300. return !plane.IsPointOnPlane(point) && plane.GetDistanceToPoint(point) < 0.0f;
  301. }
  302. public static bool IsPointInFront(this Plane plane, Vector3 point)
  303. {
  304. return !plane.IsPointOnPlane(point) && plane.GetDistanceToPoint(point) > 0.0f;
  305. }
  306. public static bool IsPointOnPlane(this Plane plane, Vector3 point, float epsilon = 1e-4f)
  307. {
  308. float absDistanceToPoint = Mathf.Abs(plane.GetDistanceToPoint(point));
  309. return absDistanceToPoint < epsilon;
  310. }
  311. public static Plane AdjustSoBoxSitsInFront(this Plane plane, OrientedBox orientedBox)
  312. {
  313. List<Vector3> boxCornerPoints = orientedBox.GetCornerPoints();
  314. Vector3 furthestPointBehind;
  315. if (plane.GetFurthestPointBehind(boxCornerPoints, out furthestPointBehind)) return new Plane(plane.normal, furthestPointBehind);
  316. else
  317. {
  318. Vector3 closestPointInFront, pointOnPlane;
  319. if (plane.GetFirstPointOnPlane(boxCornerPoints, out pointOnPlane)) return new Plane(plane.normal, pointOnPlane);
  320. if (plane.GetClosestPointInFront(boxCornerPoints, out closestPointInFront)) return new Plane(plane.normal, closestPointInFront);
  321. }
  322. // If there are no points behind or in front, it means all points are on the plane, so we return the umodified plane instance
  323. return plane;
  324. }
  325. public static Plane AdjustSoBoxSitsBehind(this Plane plane, OrientedBox orientedBox)
  326. {
  327. List<Vector3> boxCornerPoints = orientedBox.GetCornerPoints();
  328. Vector3 furthestPointInFront;
  329. if (plane.GetFurthestPointInFront(boxCornerPoints, out furthestPointInFront)) return new Plane(plane.normal, furthestPointInFront);
  330. else
  331. {
  332. Vector3 closestPointBehind, pointOnPlane;
  333. if (plane.GetFirstPointOnPlane(boxCornerPoints, out pointOnPlane)) return new Plane(plane.normal, pointOnPlane);
  334. if (plane.GetClosestPointBehind(boxCornerPoints, out closestPointBehind)) return new Plane(plane.normal, closestPointBehind);
  335. }
  336. // If there are no points behind or in front, it means all points are on the plane, so we return the umodified plane instance
  337. return plane;
  338. }
  339. public static PointPlaneClassificationResult ClassifyPoint(this Plane plane, Vector3 point)
  340. {
  341. if (plane.IsPointOnPlane(point)) return PointPlaneClassificationResult.OnPlane;
  342. if (plane.IsPointBehind(point)) return PointPlaneClassificationResult.Behind;
  343. return PointPlaneClassificationResult.InFront;
  344. }
  345. public static BoxPlaneClassificationResult ClassifyOrientedBox(this Plane plane, OrientedBox orientedBox)
  346. {
  347. List<Vector3> boxCenterAndCornerPoints = orientedBox.GetCenterAndCornerPoints();
  348. bool foundPointInFront = false;
  349. bool foundPointBehind = false;
  350. bool foundPointOnPlane = false;
  351. foreach(Vector3 boxPoint in boxCenterAndCornerPoints)
  352. {
  353. PointPlaneClassificationResult pointClassificationResult = plane.ClassifyPoint(boxPoint);
  354. if (pointClassificationResult == PointPlaneClassificationResult.InFront)
  355. {
  356. foundPointInFront = true;
  357. if (foundPointBehind) return BoxPlaneClassificationResult.Spanning;
  358. }
  359. else
  360. if (pointClassificationResult == PointPlaneClassificationResult.Behind)
  361. {
  362. foundPointBehind = true;
  363. if (foundPointInFront) return BoxPlaneClassificationResult.Spanning;
  364. }
  365. else foundPointOnPlane = true;
  366. }
  367. if(foundPointOnPlane && (!foundPointInFront && !foundPointBehind)) return BoxPlaneClassificationResult.OnPlane;
  368. else
  369. {
  370. if (foundPointInFront) return BoxPlaneClassificationResult.InFront;
  371. return BoxPlaneClassificationResult.Behind;
  372. }
  373. }
  374. public static BoxPlaneClassificationResult ClassifyBox(this Plane plane, Box box)
  375. {
  376. List<Vector3> boxCenterAndCornerPoints = box.GetCenterAndCornerPoints();
  377. bool foundPointInFront = false;
  378. bool foundPointBehind = false;
  379. bool foundPointOnPlane = false;
  380. foreach (Vector3 boxPoint in boxCenterAndCornerPoints)
  381. {
  382. PointPlaneClassificationResult pointClassificationResult = plane.ClassifyPoint(boxPoint);
  383. if (pointClassificationResult == PointPlaneClassificationResult.InFront)
  384. {
  385. foundPointInFront = true;
  386. if (foundPointBehind) return BoxPlaneClassificationResult.Spanning;
  387. }
  388. else
  389. if (pointClassificationResult == PointPlaneClassificationResult.Behind)
  390. {
  391. foundPointBehind = true;
  392. if (foundPointInFront) return BoxPlaneClassificationResult.Spanning;
  393. }
  394. else foundPointOnPlane = true;
  395. }
  396. if (foundPointOnPlane && (!foundPointInFront && !foundPointBehind)) return BoxPlaneClassificationResult.OnPlane;
  397. else
  398. {
  399. if (foundPointInFront) return BoxPlaneClassificationResult.InFront;
  400. return BoxPlaneClassificationResult.Behind;
  401. }
  402. }
  403. #endregion
  404. #region Utilities
  405. public static Plane GetPlaneWhichFacesNormal(List<Plane> planes, Vector3 normal, out int planeIndex)
  406. {
  407. if(planes.Count == 0)
  408. {
  409. planeIndex = -1;
  410. return new Plane();
  411. }
  412. normal.Normalize();
  413. float bestAlignment = 1.0f;
  414. planeIndex = -1;
  415. for (int plIndex = 0; plIndex < planes.Count; ++plIndex)
  416. {
  417. Plane plane = planes[plIndex];
  418. float alignment = Vector3.Dot(plane.normal, normal);
  419. if(alignment < 0.0f && alignment < bestAlignment)
  420. {
  421. bestAlignment = alignment;
  422. planeIndex = plIndex;
  423. if (Mathf.Abs(alignment + 1.0f) < 1e-4f) break;
  424. }
  425. }
  426. return planes[planeIndex];
  427. }
  428. public static Plane GetPlaneMostAlignedWithNormal(List<Plane> planes, Vector3 normal, out int planeIndex)
  429. {
  430. if (planes.Count == 0)
  431. {
  432. planeIndex = -1;
  433. return new Plane();
  434. }
  435. normal.Normalize();
  436. float bestAlignment = -1.0f;
  437. planeIndex = -1;
  438. for (int plIndex = 0; plIndex < planes.Count; ++plIndex)
  439. {
  440. Plane plane = planes[plIndex];
  441. float alignment = Vector3.Dot(plane.normal, normal);
  442. if (alignment > 0.0f && alignment > bestAlignment)
  443. {
  444. bestAlignment = alignment;
  445. planeIndex = plIndex;
  446. if (Mathf.Abs(alignment - 1.0f) < 1e-4f) break;
  447. }
  448. }
  449. return planes[planeIndex];
  450. }
  451. public static int GetIndexOfPlaneWhoseNormalIsMostAlignedWithDir(List<Plane> planes, Vector3 directionVector)
  452. {
  453. int planeIndex = -1;
  454. if (planes.Count == 0) return planeIndex;
  455. directionVector.Normalize();
  456. float bestAlignment = -1.0f;
  457. for (int plIndex = 0; plIndex < planes.Count; ++plIndex)
  458. {
  459. Plane plane = planes[plIndex];
  460. float alignment = Vector3.Dot(plane.normal, directionVector);
  461. if (alignment > 0.0f && alignment > bestAlignment)
  462. {
  463. bestAlignment = alignment;
  464. planeIndex = plIndex;
  465. if (plane.normal.IsAlignedWith(directionVector)) return planeIndex;
  466. }
  467. }
  468. return planeIndex;
  469. }
  470. #endregion
  471. }
  472. }
  473. #endif