PageRenderTime 39ms CodeModel.GetById 14ms RepoModel.GetById 0ms app.codeStats 1ms

/Utilities/Math/AngleAxis.cs

#
C# | 319 lines | 202 code | 29 blank | 88 comment | 5 complexity | b0605d14eb3e59d2e3048d923352233b 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;
  6. using Delta.Utilities.Helpers;
  7. using NUnit.Framework;
  8. namespace Delta.Utilities.Math
  9. {
  10. /// <summary>
  11. /// AngleAxis helper class in case we need to manage axis calculations
  12. /// (e.g. coming from a 3D package). Not used much, our code uses mostly
  13. /// vectors, quaternions or just matrices.
  14. /// </summary>
  15. [StructLayout(LayoutKind.Explicit)]
  16. [DebuggerDisplay("AngleAxis(Angle={Angle}, Axis=({Axis.X}, {Axis.Y}, " +
  17. "{Axis.Z})")]
  18. public struct AxisAngle : ISaveLoadBinary, IEquatable<AxisAngle>
  19. {
  20. #region Constants
  21. /// <summary>
  22. /// Represents the size in bytes of each AxisAngle (4 * 4 = 16 bytes).
  23. /// </summary>
  24. public const int DataSize = 4 * 4;
  25. /// <summary>
  26. /// Returns a AxisAngle with all values filled to zero
  27. /// </summary>
  28. public static readonly AxisAngle Zero =
  29. new AxisAngle(0, 0, 0, 0);
  30. #endregion
  31. #region Axis (Public)
  32. /// <summary>
  33. /// Axis we want to rotate around, initially not set (all zero).
  34. /// </summary>
  35. [FieldOffset(0)]
  36. public Vector Axis;
  37. #endregion
  38. #region Angle (Public)
  39. /// <summary>
  40. /// Rotation angle around axis
  41. /// </summary>
  42. [FieldOffset(12)]
  43. public float Angle;
  44. #endregion
  45. #region LengthSquared (Public)
  46. /// <summary>
  47. /// Square Length of the axis
  48. /// </summary>
  49. public float LengthSquared
  50. {
  51. get
  52. {
  53. return Axis.LengthSquared;
  54. }
  55. }
  56. #endregion
  57. #region Length (Public)
  58. /// <summary>
  59. /// Length of the axis
  60. /// </summary>
  61. public float Length
  62. {
  63. get
  64. {
  65. return Axis.Length;
  66. }
  67. }
  68. #endregion
  69. #region IsNormalized (Public)
  70. /// <summary>
  71. /// Is the axis vector normalized?
  72. /// </summary>
  73. public bool IsNormalized
  74. {
  75. get
  76. {
  77. return Axis.IsNormalized;
  78. }
  79. }
  80. #endregion
  81. #region Constructors
  82. /// <summary>
  83. /// Create angle axis
  84. /// </summary>
  85. public AxisAngle(float setX, float setY, float setZ, float setAngle)
  86. {
  87. Axis = new Vector(setX, setY, setZ);
  88. Angle = setAngle;
  89. }
  90. /// <summary>
  91. /// Create angle axis, first 3 values are for the axis and the 4th one
  92. /// is for the angle.
  93. /// </summary>
  94. public AxisAngle(float[] setFromArray)
  95. {
  96. Axis = new Vector(setFromArray[0], setFromArray[1], setFromArray[2]);
  97. Angle = setFromArray[3];
  98. }
  99. /// <summary>
  100. /// Create angle axis
  101. /// </summary>
  102. public AxisAngle(Vector setAxis, float setAngle)
  103. {
  104. Axis = setAxis;
  105. Angle = setAngle;
  106. }
  107. /// <summary>
  108. /// Create angle axis
  109. /// </summary>
  110. public AxisAngle(Quaternion setFromQuaternion)
  111. {
  112. Angle = (float)System.Math.Acos(setFromQuaternion.W);
  113. float sinAngle = (float)System.Math.Sin(Angle);
  114. Angle *= 2.0f;
  115. setFromQuaternion.Normalize();
  116. if (System.Math.Abs(sinAngle) >
  117. float.Epsilon)
  118. {
  119. Axis = setFromQuaternion.Vector / sinAngle;
  120. }
  121. else
  122. {
  123. Axis = Vector.Zero;
  124. }
  125. }
  126. #endregion
  127. #region IEquatable<AxisAngle> Members
  128. /// <summary>
  129. /// Equals, quickly checks if another AxisAngle has the exact same values.
  130. /// </summary>
  131. public bool Equals(AxisAngle other)
  132. {
  133. return Axis == other.Axis &&
  134. Angle == other.Angle;
  135. }
  136. #endregion
  137. #region ISaveLoadBinary Members
  138. /// <summary>
  139. /// Load axis vector and angle from a stream.
  140. /// </summary>
  141. public void Load(BinaryReader reader)
  142. {
  143. Axis.Load(reader);
  144. Angle = reader.ReadSingle();
  145. }
  146. /// <summary>
  147. /// Save axis vector and angle to a stream.
  148. /// </summary>
  149. public void Save(BinaryWriter writer)
  150. {
  151. Axis.Save(writer);
  152. writer.Write(Angle);
  153. }
  154. #endregion
  155. #region op_Explicit (Operator)
  156. /// <summary>
  157. /// Op explicit
  158. /// </summary>
  159. public static explicit operator Quaternion(AxisAngle aa)
  160. {
  161. return new Quaternion(MathHelper.Sin(0.5f * aa.Angle) * aa.Axis,
  162. MathHelper.Cos(0.5f * aa.Angle));
  163. }
  164. // op_Explicit()
  165. /// <summary>
  166. /// Op explicit
  167. /// </summary>
  168. public static explicit operator Matrix(AxisAngle aa)
  169. {
  170. return (Matrix)(Quaternion)aa;
  171. }
  172. #endregion
  173. #region op_UnaryNegation (Operator)
  174. /// <summary>
  175. /// Arithmetic operator to negate
  176. /// </summary>
  177. public static AxisAngle operator -(AxisAngle aa)
  178. {
  179. return new AxisAngle(-aa.Axis, -aa.Angle);
  180. }
  181. #endregion
  182. #region op_UnaryPlus (Operator)
  183. /// <summary>
  184. /// Op unary plus
  185. /// </summary>
  186. public static AxisAngle operator +(AxisAngle aa)
  187. {
  188. return new AxisAngle(aa.Axis, aa.Angle);
  189. }
  190. #endregion
  191. #region op_Addition (Operator)
  192. /// <summary>
  193. /// Op addition
  194. /// </summary>
  195. public static AxisAngle operator +(AxisAngle value1, AxisAngle value2)
  196. {
  197. return new AxisAngle(value1.Axis + value2.Axis,
  198. value1.Angle + value2.Angle);
  199. }
  200. #endregion
  201. #region op_Subtraction (Operator)
  202. /// <summary>
  203. /// Op subtraction
  204. /// </summary>
  205. public static AxisAngle operator -(AxisAngle value1, AxisAngle value2)
  206. {
  207. return new AxisAngle(value1.Axis - value2.Axis,
  208. value1.Angle - value2.Angle);
  209. }
  210. #endregion
  211. #region op_Equality (Operator)
  212. /// <summary>
  213. /// Logic operators for equality
  214. /// </summary>
  215. public static bool operator ==(AxisAngle value1, AxisAngle value2)
  216. {
  217. return value1.Axis == value2.Axis &&
  218. System.Math.Abs(value1.Angle - value2.Angle) <= float.Epsilon;
  219. }
  220. #endregion
  221. #region op_Inequality (Operator)
  222. /// <summary>
  223. /// Op inequality
  224. /// </summary>
  225. public static bool operator !=(AxisAngle value1, AxisAngle value2)
  226. {
  227. return value1.Axis != value2.Axis ||
  228. System.Math.Abs(value1.Angle - value2.Angle) > float.Epsilon;
  229. }
  230. #endregion
  231. #region Normalize (Public)
  232. /// <summary>
  233. /// Normalize the axis vector
  234. /// </summary>
  235. public void Normalize()
  236. {
  237. Axis.Normalize();
  238. }
  239. #endregion
  240. #region Equals (Public)
  241. /// <summary>
  242. /// Equals check with an object, will only return true if it is a matrix
  243. /// and has the exact same values as this matrix.
  244. /// </summary>
  245. public override bool Equals(object obj)
  246. {
  247. // Note: We cannot use obj as Matrix because it is a struct, need to cast
  248. return (obj is AxisAngle)
  249. ? Equals((AxisAngle)obj)
  250. : false;
  251. }
  252. #endregion
  253. #region GetHashCode (Public)
  254. /// <summary>
  255. /// Get hash code
  256. /// </summary>
  257. public override int GetHashCode()
  258. {
  259. return Axis.GetHashCode() ^ Angle.GetHashCode();
  260. }
  261. #endregion
  262. #region CreateQuaternion (Public)
  263. /// <summary>
  264. /// Create from axis angle
  265. /// </summary>
  266. public Quaternion CreateQuaternion()
  267. {
  268. // Create the resulting quaternion by applying the angle
  269. return new Quaternion(
  270. MathHelper.Sin(Angle * 0.5f) * Axis,
  271. MathHelper.Cos(Angle * 0.5f));
  272. }
  273. #endregion
  274. internal class AngleAxisTests
  275. {
  276. #region CreateAngleAxis
  277. [Test]
  278. public void CreateAngleAxis()
  279. {
  280. AxisAngle xAxis = new AxisAngle(Vector.UnitX, 0.0f);
  281. Assert.Equal(Vector.UnitX, xAxis.Axis);
  282. Assert.Equal(0.0f, xAxis.Angle);
  283. Assert.True(xAxis.IsNormalized);
  284. Assert.Equal(new Quaternion(0, 0, 0, 1),
  285. xAxis.CreateQuaternion());
  286. }
  287. #endregion
  288. }
  289. }
  290. }