Matrix4x4Extensions.cs 4.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144
  1. #if UNITY_EDITOR
  2. using UnityEngine;
  3. namespace O3DWB
  4. {
  5. public static class Matrix4x4Extensions
  6. {
  7. #region Extension Methods
  8. public static Vector3 GetTranslation(this Matrix4x4 matrix)
  9. {
  10. return matrix.GetColumn(3);
  11. }
  12. public static Matrix4x4 SetTranslation(this Matrix4x4 matrix, Vector3 translation)
  13. {
  14. matrix.SetColumn(3, new Vector4(translation.x, translation.y, translation.z, 1.0f));
  15. return matrix;
  16. }
  17. public static Quaternion GetRotation(this Matrix4x4 matrix)
  18. {
  19. Vector3 upAxis = matrix.GetNormalizedUpAxis();
  20. Vector3 lookAxis = matrix.GetNormalizedLookAxis();
  21. return Quaternion.LookRotation(lookAxis, upAxis);
  22. }
  23. public static Matrix4x4 SetRotation(this Matrix4x4 matrix, Quaternion rotation)
  24. {
  25. Vector3 translation = matrix.GetTranslation();
  26. Vector3 scale = matrix.GetXYZScale();
  27. return Matrix4x4.TRS(translation, rotation, scale);
  28. }
  29. // Note: Returns only positive scale values even if the matrix contains negative scale.
  30. public static Vector3 GetXYZScale(this Matrix4x4 matrix)
  31. {
  32. return new Vector3(matrix.GetColumn(0).magnitude,
  33. matrix.GetColumn(1).magnitude,
  34. matrix.GetColumn(2).magnitude);
  35. }
  36. // Note: Works only with scale matrices (no rotation data).
  37. public static Vector3 GetSignedXYZScale(this Matrix4x4 matrix)
  38. {
  39. return new Vector3(matrix.GetColumn(0)[0], matrix.GetColumn(1)[1], matrix.GetColumn(2)[2]);
  40. }
  41. public static Matrix4x4 SetXYZScale(this Matrix4x4 matrix, Vector3 scale)
  42. {
  43. Vector3 translation = matrix.GetTranslation();
  44. Quaternion rotation = matrix.GetRotation();
  45. return Matrix4x4.TRS(translation, rotation, scale);
  46. }
  47. public static Matrix4x4 SetXYZScale(this Matrix4x4 matrix, float scale)
  48. {
  49. return matrix.SetXYZScale(new Vector3(scale, scale, scale));
  50. }
  51. public static Vector3 GetNormalizedRightAxis(this Matrix4x4 matrix)
  52. {
  53. Vector3 rightAxis = matrix.GetColumn(0);
  54. rightAxis.Normalize();
  55. return rightAxis;
  56. }
  57. public static Vector3 GetNormalizedUpAxis(this Matrix4x4 matrix)
  58. {
  59. Vector3 upAxis = matrix.GetColumn(1);
  60. upAxis.Normalize();
  61. return upAxis;
  62. }
  63. public static Vector3 GetNormalizedLookAxis(this Matrix4x4 matrix)
  64. {
  65. Vector3 lookAxis = matrix.GetColumn(2);
  66. lookAxis.Normalize();
  67. return lookAxis;
  68. }
  69. public static Vector3 GetNormalizedAxis(this Matrix4x4 matrix, int axisIndex)
  70. {
  71. Vector3 normalizedAxis = matrix.GetColumn(axisIndex);
  72. normalizedAxis.Normalize();
  73. return normalizedAxis;
  74. }
  75. public static Vector3[] GetNormalizedLocalAxes(this Matrix4x4 matrix)
  76. {
  77. Vector3[] localAxes = new Vector3[3];
  78. localAxes[0] = matrix.GetNormalizedRightAxis();
  79. localAxes[1] = matrix.GetNormalizedUpAxis();
  80. localAxes[2] = matrix.GetNormalizedLookAxis();
  81. return localAxes;
  82. }
  83. public static int GetIndexOfAxisMostAlignedWith(this Matrix4x4 matrix, Vector3 referenceAxis)
  84. {
  85. int bestAxisIndex = -1;
  86. float bestAbsDotProduct = -1.0f;
  87. referenceAxis.Normalize();
  88. for(int axisIndex = 0; axisIndex < 3; ++axisIndex)
  89. {
  90. Vector3 axis = matrix.GetNormalizedAxis(axisIndex);
  91. if(axis.IsAlignedWith(referenceAxis)) return axisIndex;
  92. else
  93. {
  94. float absDotProduct = axis.GetAbsDot(referenceAxis);
  95. if (absDotProduct > bestAbsDotProduct)
  96. {
  97. bestAbsDotProduct = absDotProduct;
  98. bestAxisIndex = axisIndex;
  99. }
  100. }
  101. }
  102. return bestAxisIndex;
  103. }
  104. public static int GetIndexOfAxisAlignedWith(this Matrix4x4 matrix, Vector3 referenceAxis)
  105. {
  106. referenceAxis.Normalize();
  107. for (int axisIndex = 0; axisIndex < 3; ++axisIndex)
  108. {
  109. Vector3 axis = matrix.GetNormalizedAxis(axisIndex);
  110. if (axis.IsAlignedWith(referenceAxis)) return axisIndex;
  111. }
  112. return -1;
  113. }
  114. #endregion
  115. }
  116. }
  117. #endif