PageRenderTime 207ms CodeModel.GetById 81ms app.highlight 19ms RepoModel.GetById 61ms app.codeStats 9ms

/Utilities/Datatypes/Plane.cs

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