PageRenderTime 54ms CodeModel.GetById 23ms RepoModel.GetById 0ms app.codeStats 0ms

/Utilities/Datatypes/Plane.cs

#
C# | 471 lines | 295 code | 33 blank | 143 comment | 20 complexity | 1268be908a6bb440a190a9df5b04dbec MD5 | raw file
Possible License(s): Apache-2.0
  1. using System;
  2. using System.Diagnostics;
  3. using System.IO;
  4. using System.Runtime.InteropServices;
  5. using Delta.Utilities.Datatypes.Advanced;
  6. using Delta.Utilities.Helpers;
  7. using NUnit.Framework;
  8. namespace Delta.Utilities.Datatypes
  9. {
  10. /// <summary>
  11. /// Plane helper struct to manage planes with just a normal
  12. /// vector and D for the distance. Details can be found at:
  13. /// http://en.wikipedia.org/wiki/Plane_%28geometry%29
  14. /// </summary>
  15. [StructLayout(LayoutKind.Sequential)]
  16. [DebuggerDisplay("Plane(Normal={Normal.X}, {Normal.Y}, {Normal.Z}, D={D})")]
  17. public struct Plane : ISaveLoadBinary, IEquatable<Plane>, IIntersects
  18. {
  19. #region Constants
  20. private const float ZeroTolerance = 0.00001f;
  21. #endregion
  22. #region Normal (Public)
  23. /// <summary>
  24. /// Normal for the plane.
  25. /// </summary>
  26. public Vector Normal;
  27. #endregion
  28. #region Distance (Public)
  29. /// <summary>
  30. /// Distance of the plane from the origin.
  31. /// </summary>
  32. public float Distance;
  33. #endregion
  34. #region Constructors
  35. /// <summary>
  36. /// Create plane
  37. /// </summary>
  38. /// <param name="distance">distance</param>
  39. /// <param name="normal">normal</param>
  40. public Plane(Vector normal, float distance)
  41. {
  42. Normal = normal;
  43. Normal.Normalize();
  44. Distance = distance;
  45. }
  46. /// <summary>
  47. /// Create plane
  48. /// </summary>
  49. /// <param name="distance">distance</param>
  50. /// <param name="x">X</param>
  51. /// <param name="y">Y</param>
  52. /// <param name="z">Z</param>
  53. public Plane(float x, float y, float z, float distance)
  54. {
  55. Normal = new Vector(x, y, z);
  56. Normal.Normalize();
  57. Distance = distance;
  58. }
  59. /// <summary>
  60. /// Create plane
  61. /// </summary>
  62. /// <param name="point1">point1</param>
  63. /// <param name="point2">point2</param>
  64. /// <param name="point3">point3</param>
  65. public Plane(Vector point1, Vector point2, Vector point3)
  66. {
  67. // See http://en.wikipedia.org/wiki/Plane_%28geometry%29 for help.
  68. Vector diff1 = point2 - point1;
  69. Vector diff2 = point3 - point1;
  70. Normal = new Vector(
  71. (diff1.Y * diff2.Z) - (diff1.Z * diff2.Y),
  72. (diff1.Z * diff2.X) - (diff1.X * diff2.Z),
  73. (diff1.X * diff2.Y) - (diff1.Y * diff2.X));
  74. Normal.Normalize();
  75. Distance = -Vector.Dot(Normal, point1);
  76. }
  77. #endregion
  78. #region IEquatable<Plane> Members
  79. /// <summary>
  80. /// Equals
  81. /// </summary>
  82. /// <param name="other">Other</param>
  83. /// <returns>Value indicating the equality of two vectors</returns>
  84. public bool Equals(Plane other)
  85. {
  86. return Normal == other.Normal &&
  87. Distance == other.Distance;
  88. }
  89. #endregion
  90. #region IIntersects Members
  91. /// <summary>
  92. /// Intersects the current object with specified ray and returns true
  93. /// plus the intersectionPosition if the ray hits this object.
  94. /// </summary>
  95. /// <param name="ray">The ray to check with</param>
  96. /// <param name="intersectionPosition">The intersection vector</param>
  97. /// <returns>True if the ray intersected with this object</returns>
  98. public bool Intersects(Ray ray, out Vector intersectionPosition)
  99. {
  100. return ray.Intersects(this, out intersectionPosition);
  101. }
  102. /// <summary>
  103. /// Intersects the current object with specified plane and returns true
  104. /// plus the intersectionPosition if the plane intersects this object.
  105. /// </summary>
  106. /// <param name="plane">The plane to check with</param>
  107. /// <param name="intersectionPosition">The intersection vector</param>
  108. /// <returns>True if the plane intersected with this object</returns>
  109. public bool Intersects(Plane plane, out Vector intersectionPosition)
  110. {
  111. intersectionPosition = Vector.Zero;
  112. Vector direction = Vector.Cross(Normal, plane.Normal);
  113. // If direction is the zero vector, the planes are parallel
  114. // Intersects.None
  115. float denominator = Vector.Dot(direction, direction);
  116. // Denominator here is an extra check to tell us if they are parallel and
  117. // coincident. Otherwise we need to divide the point by the denominator.
  118. if (Math.Abs(denominator) < ZeroTolerance)
  119. {
  120. return false;
  121. }
  122. Vector temp = Distance * plane.Normal - plane.Distance * Normal;
  123. Vector point = Vector.Cross(temp, direction);
  124. intersectionPosition = point;
  125. return true;
  126. }
  127. /// <summary>
  128. /// Intersects the current object with the specified sphere.
  129. /// </summary>
  130. /// <param name="sphere">The sphere to check against</param>
  131. /// <returns>True if the sphere intersected with this object</returns>
  132. public bool Intersects(BoundingSphere sphere)
  133. {
  134. float objectsDistance = Vector.Dot(Normal, sphere.Center);
  135. objectsDistance += Distance;
  136. if (objectsDistance > sphere.Radius)
  137. {
  138. return false;
  139. }
  140. return true;
  141. }
  142. /// <summary>
  143. /// Intersects the current object with the specified box.
  144. /// </summary>
  145. /// <param name="box">The box to check against</param>
  146. /// <returns>True if the box intersected with this object</returns>
  147. public bool Intersects(BoundingBox box)
  148. {
  149. //Finding out where are the minimum and maximum points
  150. Vector minPoint = Vector.Zero;
  151. Vector maxPoint = Vector.Zero;
  152. if (Normal.X >= 0)
  153. {
  154. maxPoint.X = box.Min.X;
  155. minPoint.X = box.Max.X;
  156. }
  157. else
  158. {
  159. maxPoint.X = box.Max.X;
  160. minPoint.X = box.Min.X;
  161. }
  162. if (Normal.Y >= 0)
  163. {
  164. maxPoint.Y = box.Min.Y;
  165. minPoint.Y = box.Max.Y;
  166. }
  167. else
  168. {
  169. maxPoint.Y = box.Max.Y;
  170. minPoint.Y = box.Min.Y;
  171. }
  172. if (Normal.Z >= 0)
  173. {
  174. maxPoint.Z = box.Min.Z;
  175. minPoint.Z = box.Max.Z;
  176. }
  177. else
  178. {
  179. maxPoint.Z = box.Max.Z;
  180. minPoint.Z = box.Min.Z;
  181. }
  182. // Calculating the Dot product to swap the point to the Min/max values
  183. // If Distance < Max or Distance > Min, then its falling somewhere,
  184. // otherwise it is not intersecting.
  185. float objectDistance = Vector.Dot(Normal, maxPoint);
  186. if (objectDistance > Distance)
  187. {
  188. return false;
  189. }
  190. objectDistance = Vector.Dot(Normal, minPoint);
  191. if (objectDistance < Distance)
  192. {
  193. return false;
  194. }
  195. return true;
  196. }
  197. #endregion
  198. #region ISaveLoadBinary Members
  199. /// <summary>
  200. /// Loads the plane from a binary stream (just the normal and distance).
  201. /// </summary>
  202. /// <param name="reader">reader</param>
  203. public void Load(BinaryReader reader)
  204. {
  205. Normal.Load(reader);
  206. Distance = reader.ReadSingle();
  207. }
  208. /// <summary>
  209. /// Saves the plane into a binary stream (just the normal and distance).
  210. /// </summary>
  211. /// <param name="writer">Writer</param>
  212. public void Save(BinaryWriter writer)
  213. {
  214. Normal.Save(writer);
  215. writer.Write(Distance);
  216. }
  217. #endregion
  218. #region op_Equality (Operator)
  219. /// <summary>
  220. /// Check for equality
  221. /// </summary>
  222. /// <param name="value1">Value1</param>
  223. /// <param name="value2">Value2</param>
  224. /// <returns>True if the values are equal, false otherwise</returns>
  225. public static bool operator ==(Plane value1, Plane value2)
  226. {
  227. return value1.Normal == value2.Normal &&
  228. value1.Distance == value2.Distance;
  229. }
  230. #endregion
  231. #region op_Inequality (Operator)
  232. /// <summary>
  233. /// Check for inequality
  234. /// </summary>
  235. /// <param name="value1">Value1</param>
  236. /// <param name="value2">Value2</param>
  237. /// <returns>True if the values are not equal, false otherwise</returns>
  238. public static bool operator !=(Plane value1, Plane value2)
  239. {
  240. return value1.Normal != value2.Normal ||
  241. value1.Distance != value2.Distance;
  242. }
  243. #endregion
  244. #region DotCoordinate (Public)
  245. /// <summary>
  246. /// Dot coordinate
  247. /// </summary>
  248. /// <param name="value">Value</param>
  249. /// <returns>Result of the dot calculation.</returns>
  250. public float DotCoordinate(Vector value)
  251. {
  252. float resultMultiply;
  253. Vector.Dot(ref Normal, ref value, out resultMultiply);
  254. return resultMultiply + Distance;
  255. }
  256. /// <summary>
  257. /// Dot coordinate
  258. /// </summary>
  259. /// <param name="result">result</param>
  260. /// <param name="value">value</param>
  261. public void DotCoordinate(ref Vector value, out float result)
  262. {
  263. Vector.Dot(ref Normal, ref value, out result);
  264. result += Distance;
  265. }
  266. #endregion
  267. #region GetDistance (Public)
  268. /// <summary>
  269. /// Get distance from the given position to the plane.
  270. /// </summary>
  271. /// <param name="position">Position vector to check with.</param>
  272. /// <returns>The shortest distance between the position and the plane.
  273. /// </returns>
  274. public float GetDistance(Vector position)
  275. {
  276. return Vector.Dot(Normal, position) + Distance;
  277. }
  278. #endregion
  279. #region Normalize (Public)
  280. /// <summary>
  281. /// Normalize
  282. /// </summary>
  283. public void Normalize()
  284. {
  285. float distanceSquared = Normal.LengthSquared;
  286. if (distanceSquared != 0)
  287. {
  288. float distanceInverse = 1.0f / MathHelper.Sqrt(distanceSquared);
  289. Normal.X *= distanceInverse;
  290. Normal.Y *= distanceInverse;
  291. Normal.Z *= distanceInverse;
  292. Distance *= distanceInverse;
  293. }
  294. }
  295. #endregion
  296. #region Intersects (Public)
  297. /// <summary>
  298. /// Determines whether a vector point is intersecting with a plane
  299. /// otherwise defines its position.
  300. /// </summary>
  301. /// <param name="point">The point</param>
  302. /// <returns></returns>
  303. public PlaneIntersectionType Intersects(Vector point)
  304. {
  305. float objectsDistance = Vector.Dot(Normal, point);
  306. objectsDistance += Distance;
  307. if (objectsDistance > 0f)
  308. {
  309. return PlaneIntersectionType.Front;
  310. }
  311. if (objectsDistance < 0f)
  312. {
  313. return PlaneIntersectionType.Back;
  314. }
  315. return PlaneIntersectionType.Intersecting;
  316. }
  317. #endregion
  318. #region GetHashCode (Public)
  319. /// <summary>
  320. /// Get hash code
  321. /// </summary>
  322. /// <returns>hash code</returns>
  323. public override int GetHashCode()
  324. {
  325. return Normal.GetHashCode() ^ Distance.GetHashCode();
  326. }
  327. #endregion
  328. #region Equals (Public)
  329. /// <summary>
  330. /// Equals
  331. /// </summary>
  332. /// <param name="obj">Object to compare</param>
  333. /// <returns>True if obj is a plane and equal to this plane.</returns>
  334. public override bool Equals(object obj)
  335. {
  336. return (obj is Plane)
  337. ? Equals((Plane)obj)
  338. : base.Equals(obj);
  339. }
  340. #endregion
  341. /// <summary>
  342. /// Tests
  343. /// </summary>
  344. internal class PlaneTests
  345. {
  346. #region CreatePlane
  347. /// <summary>
  348. /// Test CreatePlane
  349. /// </summary>
  350. [Test]
  351. public void CreatePlane()
  352. {
  353. Vector normalVector = new Vector(10f, 3f, 59f);
  354. Vector normalizedNormalVector = Vector.Normalize(normalVector);
  355. Plane testPlane = new Plane(normalVector.X, normalVector.Y,
  356. normalVector.Z, 2f);
  357. Assert.NearlyEqual(testPlane.Normal.X, normalizedNormalVector.X);
  358. Assert.NearlyEqual(testPlane.Normal.Y, normalizedNormalVector.Y);
  359. Assert.NearlyEqual(testPlane.Normal.Z, normalizedNormalVector.Z);
  360. Assert.NearlyEqual(testPlane.Distance, 2f);
  361. testPlane = new Plane(normalVector, 2f);
  362. Assert.NearlyEqual(testPlane.Normal.X, normalizedNormalVector.X);
  363. Assert.NearlyEqual(testPlane.Normal.Y, normalizedNormalVector.Y);
  364. Assert.NearlyEqual(testPlane.Normal.Z, normalizedNormalVector.Z);
  365. Assert.NearlyEqual(testPlane.Distance, 2f);
  366. }
  367. #endregion
  368. #region TestIntersects
  369. /// <summary>
  370. /// Plane Intersection Tests
  371. /// </summary>
  372. [Test]
  373. public void TestIntersects()
  374. {
  375. // planes intersecting each other
  376. Vector pt; // output point
  377. Plane p1 = new Plane(Vector.UnitY, 1);
  378. Plane p2 = new Plane(Vector.UnitX, 1);
  379. Plane p3 = new Plane(new Vector(0, 1, 0), 4);
  380. Ray ray1 = new Ray(Vector.Zero, new Vector(3, 3, 0));
  381. Ray ray2 = new Ray(new Vector(-1, -1, 0), new Vector(-5, -5, 0));
  382. BoundingBox box = new BoundingBox(Vector.Zero, new Vector(2, 2, 2));
  383. BoundingSphere sphere = new BoundingSphere(new Vector(1, 0, 0), 2);
  384. // Plane to Plane intersection
  385. Assert.True(p1.Intersects(p2, out pt));
  386. Assert.False(p1.Intersects(p3, out pt));
  387. // Plane to Ray intersection
  388. Assert.True(p1.Intersects(ray1, out pt));
  389. Assert.False(p3.Intersects(ray2, out pt));
  390. // Plane to sphere intersection
  391. Assert.True(p2.Intersects(sphere));
  392. Assert.False(p3.Intersects(sphere));
  393. // PLane to Box Intersection
  394. Assert.True(p1.Intersects(box));
  395. Assert.True(p2.Intersects(box));
  396. Assert.False(p3.Intersects(box));
  397. }
  398. #endregion
  399. #region Equality
  400. /// <summary>
  401. /// Test Equality
  402. /// </summary>
  403. [Test]
  404. public void Equality()
  405. {
  406. Plane testPlane1 = new Plane(10f, 3f, 59f, 2f);
  407. Plane testPlane2 = new Plane(7f, 10f, 59f, 15f);
  408. Plane testPlane3 = new Plane(7f, 10f, 59f, 15f);
  409. //Asserting Planes
  410. Assert.True(testPlane2 == testPlane3);
  411. Assert.True(testPlane1 != testPlane2);
  412. Assert.False(testPlane1 == testPlane2);
  413. Assert.True(testPlane2.Equals(testPlane3));
  414. Assert.True(testPlane2.Equals((object)testPlane3));
  415. Assert.False(testPlane1.Equals((object)testPlane3));
  416. }
  417. #endregion
  418. #region DotCoordinate
  419. /// <summary>
  420. /// Test DotCoordinate. Calculation:
  421. /// (N.X * value.X) + (N.Y * value.Y) + (N.Z * value.Z) + Distance
  422. /// </summary>
  423. [Test]
  424. public void DotCoordinate()
  425. {
  426. Plane testPlane = new Plane(10f, 3f, 59f, 2f);
  427. Vector dotVector = new Vector(5f, 4f, 1f);
  428. float checkResult = (testPlane.Normal.X * dotVector.X) +
  429. (testPlane.Normal.Y * dotVector.Y) +
  430. (testPlane.Normal.Z * dotVector.Z) +
  431. testPlane.Distance;
  432. float dotCoordinate = testPlane.DotCoordinate(dotVector);
  433. Assert.NearlyEqual(dotCoordinate, checkResult);
  434. }
  435. #endregion
  436. }
  437. }
  438. }