OrientedBox.cs 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531
  1. #if UNITY_EDITOR
  2. using UnityEngine;
  3. using System;
  4. using System.Collections.Generic;
  5. namespace O3DWB
  6. {
  7. public class OrientedBox
  8. {
  9. #region Private Variables
  10. private Box _modelSpaceBox;
  11. private Vector3 _center = Vector3.zero;
  12. private Quaternion _rotation = Quaternion.identity;
  13. private Vector3 _scale = Vector3.one;
  14. private bool _allowNegativeScale = false;
  15. #endregion
  16. #region Public Properties
  17. public Box ModelSpaceBox { get { return _modelSpaceBox; } }
  18. public Vector3 ModelSpaceExtents { get { return _modelSpaceBox.Extents; } }
  19. public Vector3 ScaledExtents { get { return Vector3.Scale(ModelSpaceExtents, Scale); } }
  20. public Vector3 ModelSpaceSize { get { return _modelSpaceBox.Size; } set { _modelSpaceBox.Size = value; } }
  21. public Vector3 ScaledSize { get { return Vector3.Scale(_modelSpaceBox.Size, Scale); } }
  22. public Quaternion Rotation { get { return _rotation; } set { _rotation = value; } }
  23. public Vector3 Center { get { return _center; } set { _center = value; } }
  24. public TransformMatrix TransformMatrix
  25. {
  26. get
  27. {
  28. Vector3 translation = Center - Rotation * Vector3.Scale(_modelSpaceBox.Center, Scale);
  29. return new TransformMatrix(translation, Rotation, Scale);
  30. }
  31. }
  32. public bool AllowNegativeScale
  33. {
  34. get { return _allowNegativeScale; }
  35. set
  36. {
  37. _allowNegativeScale = value;
  38. if (!_allowNegativeScale) Scale = Scale.GetVectorWithPositiveComponents();
  39. }
  40. }
  41. public Vector3 Scale
  42. {
  43. get { return _allowNegativeScale ? _scale : _scale.GetVectorWithPositiveComponents(); }
  44. set { _scale = value; }
  45. }
  46. #endregion
  47. #region Constructors
  48. public OrientedBox()
  49. {
  50. }
  51. public OrientedBox(Box modelSpaceBox)
  52. {
  53. _modelSpaceBox = modelSpaceBox;
  54. Center = _modelSpaceBox.Center;
  55. }
  56. public OrientedBox(Box modelSpaceBox, Quaternion rotation)
  57. {
  58. _modelSpaceBox = modelSpaceBox;
  59. Center = _modelSpaceBox.Center;
  60. Rotation = rotation;
  61. }
  62. public OrientedBox(Box modelSpaceBox, Transform transform)
  63. {
  64. _modelSpaceBox = modelSpaceBox;
  65. _center = _modelSpaceBox.Center;
  66. Transform(transform);
  67. }
  68. public OrientedBox(OrientedBox source)
  69. {
  70. _modelSpaceBox = source.ModelSpaceBox;
  71. _center = source.Center;
  72. _rotation = source.Rotation;
  73. _scale = source._scale;
  74. _allowNegativeScale = source.AllowNegativeScale;
  75. }
  76. #endregion
  77. #region Public Static Functions
  78. public static OrientedBox GetInvalid()
  79. {
  80. var orientedBox = new OrientedBox();
  81. orientedBox.MakeInvalid();
  82. return orientedBox;
  83. }
  84. #endregion
  85. #region Public Methods
  86. public void Transform(Transform transform)
  87. {
  88. Rotation = transform.rotation * Rotation;
  89. _scale = Vector3.Scale(transform.lossyScale, _scale);
  90. Center = transform.localToWorldMatrix.MultiplyPoint(Center);
  91. }
  92. public void Transform(TransformMatrix transformMatrix)
  93. {
  94. Rotation = transformMatrix.Rotation * Rotation;
  95. _scale = Vector3.Scale(transformMatrix.Scale, _scale);
  96. Center = transformMatrix.MultiplyPoint(Center);
  97. }
  98. public void Transform(Matrix4x4 transformMatrix)
  99. {
  100. Rotation = transformMatrix.GetRotation() * Rotation;
  101. _scale = Vector3.Scale(transformMatrix.GetXYZScale(), _scale);
  102. Center = transformMatrix.MultiplyPoint(Center);
  103. }
  104. public void SetTransformMatrix(TransformMatrix transformMatrix)
  105. {
  106. Rotation = transformMatrix.Rotation;
  107. _scale = transformMatrix.Scale;
  108. Center = transformMatrix.MultiplyPoint(_modelSpaceBox.Center);
  109. }
  110. public void Encapsulate(OrientedBox orientedBox)
  111. {
  112. List<Vector3> orientedBoxPoints = orientedBox.GetCenterAndCornerPoints();
  113. foreach(Vector3 point in orientedBoxPoints)
  114. {
  115. AddPoint(point);
  116. }
  117. }
  118. public Sphere GetEncapsulatingSphere()
  119. {
  120. return new Sphere(Center, ScaledExtents.magnitude);
  121. }
  122. public Box GetEncapsulatingBox()
  123. {
  124. List<Vector3> centerAndCornerPoints = GetCenterAndCornerPoints();
  125. return Box.FromPoints(centerAndCornerPoints);
  126. }
  127. public void AddPoint(Vector3 point)
  128. {
  129. _modelSpaceBox.AddPoint(GetPointInModelSpace(point));
  130. }
  131. public bool Intersects(OrientedBox otherBox)
  132. {
  133. Vector3 thisScale = Scale;
  134. Vector3 otherScale = otherBox.Scale;
  135. // Negative scale causes problems
  136. Scale = thisScale.GetVectorWithPositiveComponents();
  137. otherBox.Scale = otherScale.GetVectorWithPositiveComponents();
  138. TransformMatrix transformMatrix = TransformMatrix;
  139. Vector3 A0 = transformMatrix.GetNormalizedRightAxis();
  140. Vector3 A1 = transformMatrix.GetNormalizedUpAxis();
  141. Vector3 A2 = transformMatrix.GetNormalizedLookAxis();
  142. Vector3[] A = new Vector3[] { A0, A1, A2 };
  143. TransformMatrix otherTransformMatrix = otherBox.TransformMatrix;
  144. Vector3 B0 = otherTransformMatrix.GetNormalizedRightAxis();
  145. Vector3 B1 = otherTransformMatrix.GetNormalizedUpAxis();
  146. Vector3 B2 = otherTransformMatrix.GetNormalizedLookAxis();
  147. Vector3[] B = new Vector3[] { B0, B1, B2 };
  148. // Note: We're using column major matrices.
  149. float[,] R = new float[3, 3];
  150. for(int row = 0; row < 3; ++row)
  151. {
  152. for(int column = 0; column < 3; ++column)
  153. {
  154. R[row, column] = Vector3.Dot(A[row], B[column]);
  155. }
  156. }
  157. Vector3 scaledExtents = ScaledExtents;
  158. Vector3 AEx = new Vector3(scaledExtents.x, scaledExtents.y, scaledExtents.z);
  159. scaledExtents = otherBox.ScaledExtents;
  160. Vector3 BEx = new Vector3(scaledExtents.x, scaledExtents.y, scaledExtents.z);
  161. // Construct absolute rotation error matrix to account for cases when 2 local axes are parallel
  162. const float epsilon = 1e-4f;
  163. float[,] absR = new float[3, 3];
  164. for(int row = 0; row < 3; ++row)
  165. {
  166. for(int column = 0; column < 3; ++column)
  167. {
  168. absR[row, column] = Mathf.Abs(R[row, column]) + epsilon;
  169. }
  170. }
  171. Vector3 trVector = otherBox.Center - Center;
  172. Vector3 t = new Vector3(Vector3.Dot(trVector, A0), Vector3.Dot(trVector, A1), Vector3.Dot(trVector, A2));
  173. // Test extents projection on this box's local axes (A0, A1, A2)
  174. for(int axisIndex = 0; axisIndex < 3; ++axisIndex)
  175. {
  176. float bExtents = BEx[0] * absR[axisIndex, 0] + BEx[1] * absR[axisIndex, 1] + BEx[2] * absR[axisIndex, 2];
  177. if (Mathf.Abs(t[axisIndex]) > AEx[axisIndex] + bExtents) return false;
  178. }
  179. // Test extents projection on the other box's local axes (B0, B1, B2)
  180. for(int axisIndex = 0; axisIndex < 3; ++axisIndex)
  181. {
  182. float aExtents = AEx[0] * absR[0, axisIndex] + AEx[1] * absR[1, axisIndex] + AEx[2] * absR[2, axisIndex];
  183. if (Mathf.Abs(t[0] * R[0, axisIndex] +
  184. t[1] * R[1, axisIndex] +
  185. t[2] * R[2, axisIndex]) > aExtents + BEx[axisIndex]) return false;
  186. }
  187. // Test axis A0 x B0
  188. float ra = AEx[1] * absR[2, 0] + AEx[2] * absR[1, 0];
  189. float rb = BEx[1] * absR[0, 2] + BEx[2] * absR[0, 1];
  190. if (Mathf.Abs(t[2] * R[1, 0] - t[1] * R[2, 0]) > ra + rb) return false;
  191. // Test axis A0 x B1
  192. ra = AEx[1] * absR[2, 1] + AEx[2] * absR[1, 1];
  193. rb = BEx[0] * absR[0, 2] + BEx[2] * absR[0, 0];
  194. if (Mathf.Abs(t[2] * R[1, 1] - t[1] * R[2, 1]) > ra + rb) return false;
  195. // Test axis A0 x B2
  196. ra = AEx[1] * absR[2, 2] + AEx[2] * absR[1, 2];
  197. rb = BEx[0] * absR[0, 1] + BEx[1] * absR[0, 0];
  198. if (Mathf.Abs(t[2] * R[1, 2] - t[1] * R[2, 2]) > ra + rb) return false;
  199. // Test axis A1 x B0
  200. ra = AEx[0] * absR[2, 0] + AEx[2] * absR[0, 0];
  201. rb = BEx[1] * absR[1, 2] + BEx[2] * absR[1, 1];
  202. if (Mathf.Abs(t[0] * R[2, 0] - t[2] * R[0, 0]) > ra + rb) return false;
  203. // Test axis A1 x B1
  204. ra = AEx[0] * absR[2, 1] + AEx[2] * absR[0, 1];
  205. rb = BEx[0] * absR[1, 2] + BEx[2] * absR[1, 0];
  206. if (Mathf.Abs(t[0] * R[2, 1] - t[2] * R[0, 1]) > ra + rb) return false;
  207. // Test axis A1 x B2
  208. ra = AEx[0] * absR[2, 2] + AEx[2] * absR[0, 2];
  209. rb = BEx[0] * absR[1, 1] + BEx[1] * absR[1, 0];
  210. if (Mathf.Abs(t[0] * R[2, 2] - t[2] * R[0, 2]) > ra + rb) return false;
  211. // Test axis A2 x B0
  212. ra = AEx[0] * absR[1, 0] + AEx[1] * absR[0, 0];
  213. rb = BEx[1] * absR[2, 2] + BEx[2] * absR[2, 1];
  214. if (Math.Abs(t[1] * R[0, 0] - t[0] * R[1, 0]) > ra + rb) return false;
  215. // Test axis A2 x B1
  216. ra = AEx[0] * absR[1, 1] + AEx[1] * absR[0, 1];
  217. rb = BEx[0] * absR[2, 2] + BEx[2] * absR[2, 0];
  218. if (Math.Abs(t[1] * R[0, 1] - t[0] * R[1, 1]) > ra + rb) return false;
  219. // Test axis A2 x B2
  220. ra = AEx[0] * absR[1, 2] + AEx[1] * absR[0, 2];
  221. rb = BEx[0] * absR[2, 1] + BEx[1] * absR[2, 0];
  222. if (Math.Abs(t[1] * R[0, 2] - t[0] * R[1, 2]) > ra + rb) return false;
  223. Scale = thisScale;
  224. otherBox.Scale = otherScale;
  225. return true;
  226. }
  227. public bool AreAllBoxPointsOnOrInFrontOfAnyFacePlane(OrientedBox otherBox)
  228. {
  229. List<Vector3> otherBoxPoints = otherBox.GetCenterAndCornerPoints();
  230. List<Plane> allFacePlanes = GetBoxFacePlanes();
  231. foreach(Plane plane in allFacePlanes)
  232. {
  233. if (PlaneExtensions.AreAllPointsInFrontOrOnPlane(plane, otherBoxPoints)) return true;
  234. }
  235. return false;
  236. }
  237. public Vector3 GetClosestPointToPoint(Vector3 point)
  238. {
  239. Vector3 fromCenterToPoint = point - Center;
  240. Vector3 closestPoint = Center;
  241. Vector3 scaledExtents = ScaledExtents;
  242. Vector3[] localAxes = TransformMatrix.GetNormalizedLocalAxes();
  243. for(int axisIndex = 0; axisIndex < 3; ++axisIndex)
  244. {
  245. Vector3 localAxis = localAxes[axisIndex];
  246. float axisExtent = scaledExtents[axisIndex];
  247. float projection = Vector3.Dot(localAxis, fromCenterToPoint);
  248. if (projection > axisExtent) projection =axisExtent;
  249. else if (projection < -axisExtent) projection = -axisExtent;
  250. closestPoint += localAxis * projection;
  251. }
  252. return closestPoint;
  253. }
  254. public Vector3 GetPointInModelSpace(Vector3 point)
  255. {
  256. return TransformMatrix.MultiplyPointInverse(point);
  257. }
  258. public Vector3 GetDirectionInModelSpace(Vector3 direction)
  259. {
  260. return TransformMatrix.MultiplyVectorInverse(direction);
  261. }
  262. public Vector3 GetRotatedAndScaledSize()
  263. {
  264. return TransformMatrix.MultiplyVector(ModelSpaceSize);
  265. }
  266. public float GetRotatedAndScaledSizeAlongDirection(Vector3 direction)
  267. {
  268. direction.Normalize();
  269. return Mathf.Abs(Vector3.Dot(GetRotatedAndScaledSize(), direction));
  270. }
  271. public float GetSizeAlongDirection(Vector3 direction)
  272. {
  273. // ToDO: This is not actually correct (or is it? :D ). A better solution would probably be to
  274. // cast a ray from the origin of the box towards the face whose normal is most aligned
  275. // with the direction. The distance from the center of the box to the intersection points
  276. // gives us half the size.
  277. direction.Normalize();
  278. return direction.GetAbsDot(GetRotatedAndScaledSize());
  279. }
  280. public List<Vector3> GetCenterAndCornerPoints()
  281. {
  282. List<Vector3> points = _modelSpaceBox.GetCenterAndCornerPoints();
  283. return Vector3Extensions.GetTransformedPoints(points, TransformMatrix.ToMatrix4x4x);
  284. }
  285. public List<Vector3> GetCornerPoints()
  286. {
  287. List<Vector3> points = _modelSpaceBox.GetCornerPoints();
  288. return Vector3Extensions.GetTransformedPoints(points, TransformMatrix.ToMatrix4x4x);
  289. }
  290. public List<Vector3> GetCornerPointsProjectedOnPlane(Plane plane)
  291. {
  292. return plane.ProjectAllPoints(GetCornerPoints());
  293. }
  294. public Polygon3D Get3DPolygonFromCornerPointsProjectedOnPlane(Plane plane)
  295. {
  296. var polygon = new Polygon3D();
  297. polygon.SetPointsOnPolygonPlaneAndNormal(GetCornerPointsProjectedOnPlane(plane), plane.normal);
  298. return polygon;
  299. }
  300. public List<Vector2> GetScreenCenterAndCornerPoints(Camera camera)
  301. {
  302. return Vector2Extensions.GetScreenPoints(GetCenterAndCornerPoints(), camera);
  303. }
  304. public BoxFace GetBoxFaceClosestToPoint(Vector3 point)
  305. {
  306. Vector3 modelSpacePoint = GetPointInModelSpace(point);
  307. return _modelSpaceBox.GetBoxFaceClosestToPoint(modelSpacePoint);
  308. }
  309. public List<Plane> GetBoxFacePlanes()
  310. {
  311. List<Plane> facePlanes = new List<Plane>();
  312. Array boxFaces = Enum.GetValues(typeof(BoxFace));
  313. foreach(BoxFace boxFace in boxFaces)
  314. {
  315. facePlanes.Add(GetBoxFacePlane(boxFace));
  316. }
  317. return facePlanes;
  318. }
  319. public Plane GetBoxFacePlane(BoxFace boxFace)
  320. {
  321. Plane modelSpacePlane = _modelSpaceBox.GetBoxFacePlane(boxFace);
  322. Vector3 modelSpacePointOnPlane = _modelSpaceBox.GetBoxFaceCenter(boxFace);
  323. return modelSpacePlane.Transform(TransformMatrix.ToMatrix4x4x, modelSpacePointOnPlane);
  324. }
  325. public Vector2 GetBoxFaceSizeAlongFaceLocalXZAxes(BoxFace boxFace)
  326. {
  327. return _modelSpaceBox.GetBoxFaceSizeAlongFaceLocalXZAxes(boxFace, _scale);
  328. }
  329. public Vector3 GetBoxFaceCenter(BoxFace boxFace)
  330. {
  331. Vector3 modelSpaceCenter = _modelSpaceBox.GetBoxFaceCenter(boxFace);
  332. return TransformMatrix.MultiplyPoint(modelSpaceCenter);
  333. }
  334. public CoordinateSystem GetBoxFaceCoordinateSystem(BoxFace boxFace)
  335. {
  336. CoordinateSystem coordSystem = _modelSpaceBox.GetBoxFaceCoordinateSystem(boxFace);
  337. TransformMatrix transformMatrix = TransformMatrix;
  338. coordSystem.Transform(transformMatrix);
  339. return coordSystem;
  340. }
  341. public BoxFace GetBoxFaceWhichFacesNormal(Vector3 normal)
  342. {
  343. List<Plane> facePlanes = GetBoxFacePlanes();
  344. int planeIndex;
  345. PlaneExtensions.GetPlaneWhichFacesNormal(facePlanes, normal, out planeIndex);
  346. return (BoxFace)planeIndex;
  347. }
  348. public BoxFace GetBoxFaceMostAlignedWithNormal(Vector3 normal)
  349. {
  350. List<Plane> facePlanes = GetBoxFacePlanes();
  351. int planeIndex;
  352. PlaneExtensions.GetPlaneMostAlignedWithNormal(facePlanes, normal, out planeIndex);
  353. return (BoxFace)planeIndex;
  354. }
  355. public List<Vector3> GetBoxFaceCenterAndCornerPoints(BoxFace boxFace)
  356. {
  357. List<Vector3> modelSpacePoints = _modelSpaceBox.GetBoxFaceCenterAndCornerPoints(boxFace);
  358. return Vector3Extensions.GetTransformedPoints(modelSpacePoints, TransformMatrix.ToMatrix4x4x);
  359. }
  360. public List<Vector3> GetBoxFaceCornerPoints(BoxFace boxFace)
  361. {
  362. List<Vector3> modelSpacePoints = _modelSpaceBox.GetBoxFaceCornerPoints(boxFace);
  363. return Vector3Extensions.GetTransformedPoints(modelSpacePoints, TransformMatrix.ToMatrix4x4x);
  364. }
  365. public bool IsSpanningOrIsCloseInFrontOrOnAnyPlane(List<Plane> planes, float allowedClosestDistance)
  366. {
  367. foreach (Plane plane in planes)
  368. {
  369. if (IsSpanningOrIsCloseInFrontOrOnPlane(plane, allowedClosestDistance)) return true;
  370. }
  371. return false;
  372. }
  373. public bool IsSpanningOrIsCloseInFrontOrOnPlane(Plane plane, float allowedClosestDistance)
  374. {
  375. BoxPlaneClassificationResult boxClassificationResult = plane.ClassifyOrientedBox(this);
  376. if (boxClassificationResult == BoxPlaneClassificationResult.Spanning ||
  377. boxClassificationResult == BoxPlaneClassificationResult.OnPlane) return true;
  378. if (boxClassificationResult == BoxPlaneClassificationResult.Behind) return false;
  379. List<Vector3> centerAndCornerPoints = GetCenterAndCornerPoints();
  380. if (plane.IsAnyPointOnPlane(centerAndCornerPoints)) return true;
  381. Vector3 closestPointInFront;
  382. if (plane.GetClosestPointInFront(GetCenterAndCornerPoints(), out closestPointInFront))
  383. {
  384. return plane.GetDistanceToPoint(closestPointInFront) <= allowedClosestDistance;
  385. }
  386. return false;
  387. }
  388. public bool Raycast(Ray ray, out OrientedBoxRayHit boxRayHit)
  389. {
  390. boxRayHit = null;
  391. float t;
  392. if(Raycast(ray, out t))
  393. {
  394. boxRayHit = new OrientedBoxRayHit(ray, t, this);
  395. return true;
  396. }
  397. return false;
  398. }
  399. public bool Raycast(Ray ray, out float t)
  400. {
  401. TransformMatrix transformMatrix = TransformMatrix;
  402. Ray modelSpaceRay = ray.InverseTransform(transformMatrix.ToMatrix4x4x);
  403. float modelSpaceT;
  404. if (_modelSpaceBox.Raycast(modelSpaceRay, out modelSpaceT))
  405. {
  406. // Note: The intersection offset (i.e. T value) we have calculated so far is expressed in the box model space.
  407. // We have to calculate the intersection point in world space and use that to calculate the world space
  408. // T value which we will store in the output parameter.
  409. Vector3 modelSpaceIntersectionPoint = modelSpaceRay.GetPoint(modelSpaceT);
  410. Vector3 worldIntersectionPoint = transformMatrix.MultiplyPoint(modelSpaceIntersectionPoint);
  411. t = (ray.origin - worldIntersectionPoint).magnitude;
  412. return true;
  413. }
  414. else
  415. {
  416. t = 0.0f;
  417. return false;
  418. }
  419. }
  420. public bool ContainsPoint(Vector3 point)
  421. {
  422. Vector3 modelSpacePoint = GetPointInModelSpace(point);
  423. return _modelSpaceBox.ContainsPoint(modelSpacePoint);
  424. }
  425. public void MakeInvalid()
  426. {
  427. _modelSpaceBox.MakeInvalid();
  428. }
  429. public bool IsValid()
  430. {
  431. return _modelSpaceBox.IsValid();
  432. }
  433. public bool IsInvalid()
  434. {
  435. return _modelSpaceBox.IsInvalid();
  436. }
  437. #endregion
  438. }
  439. }
  440. #endif