PageRenderTime 85ms CodeModel.GetById 30ms app.highlight 35ms RepoModel.GetById 15ms app.codeStats 0ms

/Utilities/Datatypes/Ray.cs

#
C# | 493 lines | 351 code | 29 blank | 113 comment | 30 complexity | 5e4dfe9a7998bb045a033a4ffd9f44bf MD5 | raw file
  1using System;
  2using System.Diagnostics;
  3using System.IO;
  4using System.Runtime.InteropServices;
  5using Delta.Utilities.Datatypes.Advanced;
  6using NUnit.Framework;
  7
  8namespace Delta.Utilities.Datatypes
  9{
 10	/// <summary>
 11	/// Ray struct, used to fire rays into a 3D scene to find out what we can
 12	/// hit with that ray (for mouse picking and other simple collision stuff)
 13	/// </summary>ZeroTolerance
 14	[StructLayout(LayoutKind.Sequential)]
 15	[DebuggerDisplay("Ray(RayStart={RayStart.X}, {RayStart.Y}, {RayStart.Z}, " +
 16	                 "Direction=({Direction.X}, {Direction.Y}, {Direction.Z})")]
 17	public struct Ray : ISaveLoadBinary, IEquatable<Ray>, IIntersects
 18	{
 19		#region Constants
 20		private const float ZeroTolerance = 0.00001f;
 21		#endregion
 22
 23		#region Position (Public)
 24		/// <summary>
 25		/// Gets or sets the starting point of the ray
 26		/// </summary>
 27		public Vector Position;
 28		#endregion
 29
 30		#region Direction (Public)
 31		/// <summary>
 32		/// Gets or sets the direction of the ray
 33		/// </summary>
 34		public Vector Direction;
 35		#endregion
 36
 37		#region Constructors
 38		/// <summary>
 39		/// Create ray
 40		/// </summary>
 41		/// <param name="rayStart">Ray start</param>
 42		/// <param name="direction">Direction</param>
 43		public Ray(Vector rayStart, Vector direction)
 44		{
 45			Position = rayStart;
 46			Direction = direction;
 47		}
 48		#endregion
 49
 50		#region IEquatable<Ray> Members
 51		/// <summary>
 52		/// Check if another ray is equal to this ray.
 53		/// </summary>
 54		/// <param name="other">Other ray</param>
 55		/// <returns>
 56		/// True if the other ray has the same values as this ray.
 57		/// </returns>
 58		public bool Equals(Ray other)
 59		{
 60			return this == other;
 61		}
 62		#endregion
 63
 64		#region IIntersects Members
 65		/// <summary>
 66		/// Intersects the current object with specified plane and returns true
 67		/// plus the intersectionPosition if the plane intersects this object.
 68		/// </summary>
 69		/// <param name="plane">The plane to check with</param>
 70		/// <param name="intersectionPosition">The intersection vector</param>
 71		/// <returns>True if the plane intersected with this object</returns>
 72		public bool Intersects(Plane plane, out Vector intersectionPosition)
 73		{
 74			intersectionPosition = Vector.Zero;
 75			float direction = Vector.Dot(plane.Normal, Direction);
 76			if (Math.Abs(direction) < ZeroTolerance)
 77			{
 78				intersectionPosition = Vector.Zero;
 79				return false;
 80			}
 81			float position = Vector.Dot(plane.Normal, Position);
 82			float distance = (plane.Distance - position) / direction;
 83			if (distance < 0f)
 84			{
 85				if (distance < -ZeroTolerance)
 86				{
 87					return false;
 88				}
 89				return true;
 90			}
 91			intersectionPosition = Position + (Direction * distance);
 92			return true;
 93		}
 94
 95		/// <summary>
 96		/// Intersects the current object with specified ray and returns true
 97		/// plus the intersectionPosition if the ray hits this object.
 98		/// </summary>
 99		/// <param name="ray">The ray to check with</param>
100		/// <param name="intersectionPosition">The intersection vector</param>
101		/// <returns>True if the ray intersected with this object</returns>
102		public bool Intersects(Ray ray, out Vector intersectionPosition)
103		{
104			Vector cross = Vector.Cross(Direction, ray.Direction);
105			intersectionPosition = Vector.Zero;
106			float denominator = cross.Length;
107			// 5Lines are parallel.
108			if (Math.Abs(denominator) < ZeroTolerance)
109			{
110				// Lines are parallel and on top of each other.
111				if (Math.Abs(ray.Position.X - Position.X) < ZeroTolerance &&
112				    Math.Abs(ray.Position.Y - Position.Y) < ZeroTolerance &&
113				    Math.Abs(ray.Position.Z - Position.Z) < ZeroTolerance)
114				{
115					return true;
116				}
117			}
118			denominator = denominator * denominator;
119			// 3x3 matrix for the first ray.
120			float m11 = ray.Position.X - Position.X;
121			float m12 = ray.Position.Y - Position.Y;
122			float m13 = ray.Position.Z - Position.Z;
123			float m21 = ray.Direction.X;
124			float m22 = ray.Direction.Y;
125			float m23 = ray.Direction.Z;
126			float m31 = cross.X;
127			float m32 = cross.Y;
128			float m33 = cross.Z;
129			// Determinant of first matrix.
130			float dets =
131				m11 * m22 * m33 +
132				m12 * m23 * m31 +
133				m13 * m21 * m32 -
134				m11 * m23 * m32 -
135				m12 * m21 * m33 -
136				m13 * m22 * m31;
137			// 3x3 matrix for the second ray.
138			m21 = Direction.X;
139			m22 = Direction.Y;
140			m23 = Direction.Z;
141			// Determinant of the second matrix.
142			float dett =
143				m11 * m22 * m33 +
144				m12 * m23 * m31 +
145				m13 * m21 * m32 -
146				m11 * m23 * m32 -
147				m12 * m21 * m33 -
148				m13 * m22 * m31;
149			// t values of the point of intersection.
150			float s = dets / denominator;
151			float t = dett / denominator;
152			//The points of intersection.
153			Vector point1 = Position + (s * Direction);
154			Vector point2 = ray.Position + (t * ray.Direction);
155			// If the points are not equal, no intersection has occurred.
156			if (Math.Abs(point2.X - point1.X) > ZeroTolerance ||
157			    Math.Abs(point2.Y - point1.Y) > ZeroTolerance ||
158			    Math.Abs(point2.Z - point1.Z) > ZeroTolerance)
159			{
160				return false;
161			}
162			intersectionPosition = point1;
163			return true;
164		}
165
166		/// <summary>
167		/// Intersects the current object with the specified sphere.
168		/// </summary>
169		/// <param name="sphere">The sphere to check against</param>
170		/// <returns>True if the sphere intersected with this object</returns>
171		public bool Intersects(BoundingSphere sphere)
172		{
173			Vector distance;
174			Vector.Subtract(ref Position, ref sphere.Center, out distance);
175			float b = Vector.Dot(distance, Direction);
176			float c = Vector.Dot(distance, distance) -
177			          (sphere.Radius * sphere.Radius);
178			if (c > 0f && b > 0f)
179			{
180				return false;
181			}
182			float discriminant = b * b - c;
183
184			if (discriminant < 0f)
185			{
186				return false;
187			}
188			float intersectionPoint = -b - (float)Math.Sqrt(discriminant);
189			if (intersectionPoint < 0f)
190			{
191				return true;
192			}
193			return false;
194		}
195
196		/// <summary>
197		/// Intersects the current object with the specified box.
198		/// </summary>
199		/// <param name="box">The box to check against</param>
200		/// <returns>True if the box intersected with this object</returns>
201		public bool Intersects(BoundingBox box)
202		{
203			float intersectionPoint = 0f;
204			float tmax = float.MaxValue;
205
206			if (Math.Abs(Direction.X) < ZeroTolerance)
207			{
208				if (Position.X < box.Min.X || Position.X > box.Max.X)
209				{
210					return false;
211				}
212			}
213			else
214			{
215				float inverse = 1.0f / Direction.X;
216				float t1 = (box.Min.X - Position.X) * inverse;
217				float t2 = (box.Max.X - Position.X) * inverse;
218
219				if (t1 > t2)
220				{
221					float temp = t1;
222					t1 = t2;
223					t2 = temp;
224				}
225				intersectionPoint = Math.Max(t1, intersectionPoint);
226				tmax = Math.Min(t2, tmax);
227				if (intersectionPoint > tmax)
228				{
229					return false;
230				}
231			}
232			if (Math.Abs(Direction.Y) < ZeroTolerance)
233			{
234				if (Position.Y < box.Min.Y || Position.Y > box.Max.Y)
235				{
236					return false;
237				}
238			}
239			else
240			{
241				float inverse = 1.0f / Direction.Y;
242				float t1 = (box.Min.Y - Position.Y) * inverse;
243				float t2 = (box.Max.Y - Position.Y) * inverse;
244
245				if (t1 > t2)
246				{
247					float temp = t1;
248					t1 = t2;
249					t2 = temp;
250				}
251				intersectionPoint = Math.Max(t1, intersectionPoint);
252				tmax = Math.Min(t2, tmax);
253				if (intersectionPoint > tmax)
254				{
255					return false;
256				}
257			}
258			if (Math.Abs(Direction.Z) < ZeroTolerance)
259			{
260				if (Position.Z < box.Min.Z || Position.Z > box.Max.Z)
261				{
262					return false;
263				}
264			}
265			else
266			{
267				float inverse = 1.0f / Direction.Z;
268				float t1 = (box.Min.Z - Position.Z) * inverse;
269				float t2 = (box.Max.Z - Position.Z) * inverse;
270				if (t1 > t2)
271				{
272					float temp = t1;
273					t1 = t2;
274					t2 = temp;
275				}
276				intersectionPoint = Math.Max(t1, intersectionPoint);
277				tmax = Math.Min(t2, tmax);
278
279				if (intersectionPoint > tmax)
280				{
281					return false;
282				}
283			}
284			return true;
285		}
286		#endregion
287
288		#region ISaveLoadBinary Members
289		/// <summary>
290		/// Load the position and direction of this ray from a stream.
291		/// </summary>
292		/// <param name="reader">reader</param>
293		public void Load(BinaryReader reader)
294		{
295			Position.Load(reader);
296			Direction.Load(reader);
297		}
298
299		/// <summary>
300		/// Save the position and direction of this ray into a stream.
301		/// </summary>
302		/// <param name="writer">writer</param>
303		public void Save(BinaryWriter writer)
304		{
305			Position.Save(writer);
306			Direction.Save(writer);
307		}
308		#endregion
309
310		#region op_Equality (Operator)
311		/// <summary>
312		/// Operator for equality
313		/// </summary>
314		/// <param name="value1">Ray 1</param>
315		/// <param name="value2">Ray 2</param>
316		/// <returns>True if both rays are the same</returns>
317		public static bool operator ==(Ray value1, Ray value2)
318		{
319			return (value1.Position == value2.Position) &&
320			       (value1.Direction == value2.Direction);
321		}
322		#endregion
323
324		#region op_Inequality (Operator)
325		/// <summary>
326		/// Operator for inequality
327		/// </summary>
328		/// <param name="value1">Ray 1</param>
329		/// <param name="value2">Ray 2</param>
330		/// <returns>True if both rays are not the same</returns>
331		public static bool operator !=(Ray value1, Ray value2)
332		{
333			return !(value1 == value2);
334		}
335		#endregion
336
337		#region Equals (Public)
338		/// <summary>
339		/// Equals
340		/// </summary>
341		/// <param name="obj">Other ray</param>
342		/// <returns>
343		/// True if the other ray has the same values as this ray.
344		/// </returns>
345		public override bool Equals(object obj)
346		{
347			bool flag = false;
348			if (obj is Ray)
349			{
350				flag = Equals((Ray)obj);
351			}
352
353			return flag;
354		}
355		#endregion
356
357		#region GetHashCode (Public)
358		/// <summary>
359		/// Get hash code
360		/// </summary>
361		public override int GetHashCode()
362		{
363			return Position.GetHashCode() ^ Direction.GetHashCode();
364		}
365		#endregion
366
367		#region ToString (Public)
368		/// <summary>
369		/// To string
370		/// </summary>
371		/// <returns>string</returns>
372		public override string ToString()
373		{
374			return "(Position:" + Position + " Direction:" + Direction + ")";
375		}
376		#endregion
377
378		/// <summary>
379		/// Tests
380		/// </summary>
381		internal class RayTests
382		{
383			#region TestProperties (Static)
384			/// <summary>
385			/// Test properties
386			/// </summary>
387			[Test]
388			public static void TestProperties()
389			{
390				var testRay = new Ray(Vector.Zero, new Vector(1f, 0.4f, 0f));
391				Assert.Equal(Vector.Zero, testRay.Position);
392				Assert.Equal(new Vector(1f, 0.4f, 0f), testRay.Direction);
393			}
394			#endregion
395
396			#region TestEquals (Static)
397			/// <summary>
398			/// Test equals
399			/// </summary>
400			[Test]
401			public static void TestEquals()
402			{
403				var testRay1 = new Ray(Vector.Zero, new Vector(1f, 0.4f, 0f));
404				var testRay2 = new Ray(Vector.UnitY, new Vector(1f, 0.4f, 0f));
405				Assert.True(testRay1.Equals(testRay1));
406				Assert.False(testRay1.Equals(testRay2));
407			}
408			#endregion
409
410			#region TestToString (Static)
411			/// <summary>
412			/// Test to string
413			/// </summary>
414			[Test]
415			public static void TestToString()
416			{
417				var testRay = new Ray(Vector.Zero, new Vector(1f, 0.4f, 0f));
418				Assert.Equal(
419					"(Position:(0.0000, 0.0000, 0.0000) Direction:(1.0000, 0.4000, 0.0000))",
420					testRay.ToString());
421			}
422			#endregion
423
424			#region TestIntersectPlane
425			/// <summary>
426			/// Test for Ray.Intersects(Plane)
427			/// </summary>
428			[Test]
429			public void TestIntersectPlane()
430			{
431				Vector point;
432				var testRay1 = new Ray(Vector.UnitZ, new Vector(0.1f, 0.1f, -0.2f));
433				var testRay2 = new Ray(Vector.UnitZ, new Vector(0.1f, 0.1f, 0.1f));
434				var testPlane = new Plane(Vector.UnitZ, 0);
435				bool IsIntersect = testRay1.Intersects(testPlane, out point);
436				bool IsIntersectNot = testRay2.Intersects(testPlane, out point);
437				Assert.Equal(IsIntersect, true);
438				Assert.Equal(IsIntersectNot, false);
439			}
440			#endregion
441
442			#region TestIntersectsBox
443			/// <summary>
444			/// Test Ray.Intersects(Box)
445			/// </summary>
446			[Test]
447			public void TestIntersectsBox()
448			{
449				var testRay1 = new Ray(new Vector(0f, -10f, 10f),
450					new Vector(0f, 10f, -10f));
451				var testRay2 = new Ray(new Vector(0f, -10f, 10f), Vector.Zero);
452				var testBox = new BoundingBox(
453					new Vector(-0.5f, -0.5f, -0.5f),
454					new Vector(0.5f, 0.5f, 0.5f));
455				Assert.True(testRay1.Intersects(testBox));
456				Assert.False(testRay2.Intersects(testBox));
457			}
458			#endregion
459
460			#region TestIntersectSphere
461			/// <summary>
462			/// Test Ray.Intersects(Sphere)
463			/// </summary>
464			[Test]
465			public void TestIntersectSphere()
466			{
467				var testRay1 = new Ray(Vector.Half,
468					new Vector(0f, 10f, 10f));
469				var testRay2 = new Ray(new Vector(0f, -10f, 10f), Vector.Zero);
470				var sphere = new BoundingSphere(Vector.Half, 5.0f);
471				Assert.True(testRay1.Intersects(sphere));
472				Assert.False(testRay2.Intersects(sphere));
473			}
474			#endregion
475
476			#region TestIntersectRay
477			/// <summary>
478			/// Test Ray.Intersects(Ray)
479			/// </summary>
480			[Test]
481			public void TestIntersectRay()
482			{
483				Vector point1, point2;
484				var one = new Ray(new Vector(-1, -1, 2), new Vector(-2, -1, 2));
485				var two = new Ray(new Vector(7, 7, 0), new Vector(4, 4, 0));
486				var three = new Ray(new Vector(6, 5, 0), new Vector(4, 7, 0));
487				Assert.True(two.Intersects(three, out point1));
488				Assert.False(one.Intersects(two, out point2));
489			}
490			#endregion
491		}
492	}
493}