/Research/Example projects/android2.1_TestOGL/src/com/example/atogl/MatrixUtils.java

https://bitbucket.org/DeveloperUX/behaviortree · Java · 280 lines · 137 code · 32 blank · 111 comment · 24 complexity · 41205932976c5f48459d458c6c5f5872 MD5 · raw file

  1. package com.example.atogl;
  2. public class MatrixUtils {
  3. /**
  4. * Returns the transpose of a 4x4 matrix
  5. * @param m The matrix to transpose
  6. * @param result The place to store the transposed matrix
  7. **/
  8. public static void transpose(float[][] m, float[][] result) {
  9. for (int i=0;i<4;i++)
  10. for (int j=0;j<4;j++)
  11. result[j][i] = m[i][j];
  12. }
  13. public static void transpose(float[]m, float[] result) {
  14. for (int i=0;i<4;i++)
  15. for (int j=0;j<4;j++)
  16. result[j*4+i] = m[i*4+j];
  17. }
  18. /**
  19. * Converts this vector into a normalized (unit length) vector
  20. * <b>Modifies the input parameter</b>
  21. * @param vector The vector to normalize
  22. **/
  23. public static void normalize(float[] vector) {
  24. scalarMultiply(vector, 1/magnitude(vector));
  25. }
  26. /**
  27. * Converts this vector into a normalized (unit length) vector
  28. * <b>Modifies the input parameter</b>
  29. * @param vector The vector to normalize
  30. **/
  31. public static void normalize(int[] vector) {
  32. scalarMultiply(vector, 1/magnitude(vector));
  33. }
  34. /**
  35. * Copy a vector from <code>from</code> into <code>to</code>
  36. * @param from The source
  37. * @param to The destination
  38. **/
  39. public static void copy(float[] from, float[] to) {
  40. for (int i=0;i<from.length;i++) {
  41. to[i] = from[i];
  42. }
  43. }
  44. /**
  45. * Multiply two matrices by each other and store the result.
  46. * result = m1 x m2
  47. * @param m1 The first matrix
  48. * @param m2 The second matrix
  49. * @param reuslt Where to store the product of m1 x m2
  50. **/
  51. public static void multiply(float[][] m1, float[][] m2, float[][] result) {
  52. for (int i=0;i<4;i++) {
  53. for (int j=0;j<4;j++) {
  54. result[i][j] =
  55. m1[i][0]*m2[0][j]+
  56. m1[i][1]*m2[1][j]+
  57. m1[i][2]*m2[2][j]+
  58. m1[i][3]*m2[3][j];
  59. }
  60. }
  61. }
  62. /**
  63. * Multiply a vector by a scalar. <b>Modifies the input vector</b>
  64. * @param vector The vector
  65. * @param scalar The scalar
  66. **/
  67. public static void scalarMultiply(float[] vector, float scalar) {
  68. for (int i=0;i<vector.length;i++)
  69. vector[i] *= scalar;
  70. }
  71. /**
  72. * Multiply a vector by a scalar. <b>Modifies the input vector</b>
  73. * @param vector The vector
  74. * @param scalar The scalar
  75. **/
  76. public static void scalarMultiply(int[] vector, int scalar) {
  77. for (int i=0;i<vector.length;i++)
  78. vector[i] = FixedPointUtils.multiply(vector[i],scalar);
  79. }
  80. /**
  81. * Create the identity matrix I
  82. * @param matrix The matrix to store the identity matrix in.
  83. **/
  84. public static void identity(float[][] matrix) {
  85. for (int i=0;i<4;i++)
  86. for (int j=0;j<4;j++)
  87. matrix[i][j] = (i==j)?1:0;
  88. }
  89. /**
  90. * Compute the dot product of two vectors
  91. * @param v1 The first vector
  92. * @param v2 The second vector
  93. * @return v1 dot v2
  94. **/
  95. public static float dot(float[] v1, float[] v2) {
  96. float res = 0;
  97. for (int i=0;i<v1.length;i++)
  98. res += v1[i]*v2[i];
  99. return res;
  100. }
  101. /**
  102. * Compute the cross product of two vectors
  103. * @param v1 The first vector
  104. * @param v2 The second vector
  105. * @param result Where to store the cross product
  106. **/
  107. public static void cross(float[] p1, float[] p2, float[] result) {
  108. result[0] = p1[1]*p2[2]-p2[1]*p1[2];
  109. result[1] = p1[2]*p2[0]-p2[2]*p1[0];
  110. result[2] = p1[0]*p2[1]-p2[0]*p1[1];
  111. }
  112. /**
  113. * Compute the cross product of two vectors
  114. * @param v1 The first vector
  115. * @param v2 The second vector
  116. * @param result Where to store the cross product
  117. **/
  118. public static void cross(int[] p1, int[] p2, int[] result) {
  119. result[0] = FixedPointUtils.multiply(p1[1],p2[2])-FixedPointUtils.multiply(p2[1],p1[2]);
  120. result[1] = FixedPointUtils.multiply(p1[2],p2[0])-FixedPointUtils.multiply(p2[2],p1[0]);
  121. result[2] = FixedPointUtils.multiply(p1[0],p2[1])-FixedPointUtils.multiply(p2[0],p1[1]);
  122. }
  123. /**
  124. * Compute the magnitude (length) of a vector
  125. * @param vector The vector
  126. * @return The magnitude of the vector
  127. **/
  128. public static float magnitude(float[] vector) {
  129. return (float)Math.sqrt(vector[0]*vector[0]+
  130. vector[1]*vector[1]+
  131. vector[2]*vector[2]);
  132. }
  133. /**
  134. * Compute the magnitude (length) of a vector
  135. * @param vector The vector
  136. * @return The magnitude of the vector
  137. **/
  138. public static int magnitude(int[] vector) {
  139. return FixedPointUtils.sqrt(FixedPointUtils.multiply(vector[0],vector[0])+
  140. FixedPointUtils.multiply(vector[1],vector[1])+
  141. FixedPointUtils.multiply(vector[2],vector[2]));
  142. }
  143. public static void rotateZ(float[] v, float angle, float[] out) {
  144. float[][] R = new float[4][4];
  145. R[0][0] = (float)Math.cos(angle);
  146. R[0][1] = (float)-Math.sin(angle);
  147. R[1][0] = (float)Math.sin(angle);
  148. R[1][1] = (float)Math.cos(angle);
  149. R[2][2] = R[3][3] = 1;
  150. multiply(R, v, out);
  151. }
  152. public static void rotateY(float[] v, float angle, float[] out) {
  153. float[][] R = new float[4][4];
  154. R[0][0] = (float)Math.cos(angle);
  155. R[0][2] = (float)-Math.sin(angle);
  156. R[1][0] = (float)Math.sin(angle);
  157. R[1][2] = (float)Math.cos(angle);
  158. R[1][1] = R[3][3] = 1;
  159. multiply(R, v, out);
  160. }
  161. /**
  162. * Multiply a vector and a matrix. result = matrix x vector
  163. * @param matrix The matrix.
  164. * @param vector The vector
  165. * @param result The result of the multiplication
  166. **/
  167. public static void multiply(float[][] matrix, float[] vector, float[] res)
  168. {
  169. for (int i=0;i<4;i++) {
  170. res[i] =
  171. matrix[i][0]*vector[0]+
  172. matrix[i][1]*vector[1]+
  173. matrix[i][2]*vector[2]+
  174. matrix[i][3]*vector[3];
  175. }
  176. }
  177. /**
  178. * Pretty print a matrix to stdout.
  179. * @param matrix The matrix
  180. **/
  181. public static void printMatrix(float[][] matrix) {
  182. for (int i=0;i<4;i++) {
  183. for (int j=0;j<4;j++)
  184. System.out.print(matrix[i][j]+"\t");
  185. System.out.println();
  186. }
  187. }
  188. /**
  189. * Homogenize a point (divide by its last element)
  190. * @param pt The point <b>Modified</b>
  191. **/
  192. public static void homogenize(float[] pt)
  193. {
  194. scalarMultiply(pt, 1/pt[3]);
  195. }
  196. /**
  197. * Pretty print a vector
  198. * @param vec The vector to print
  199. **/
  200. public static void printVector(float[] vec) {
  201. for (int i=0;i<vec.length;i++)
  202. System.out.println(vec[i]);
  203. }
  204. /**
  205. * Subtracts two vectors (a-b).
  206. * @param a The first vector
  207. * @param b The second vector
  208. * @param result Storage for the result, if null, store in a.
  209. **/
  210. public static void minus(float[] a, float[] b, float[] result) {
  211. float[] res = (result == null)?a:result;
  212. for (int i=0;i<Math.min(a.length,b.length);i++)
  213. res[i] = a[i]-b[i];
  214. }
  215. /**
  216. * Subtracts two vectors (a-b).
  217. * @param a The first vector
  218. * @param b The second vector
  219. * @param result Storage for the result, if null, store in a.
  220. **/
  221. public static void minus(int[] a, int[] b, int[] result) {
  222. int[] res = (result == null)?a:result;
  223. for (int i=0;i<Math.min(a.length,b.length);i++)
  224. res[i] = a[i]-b[i];
  225. }
  226. /**
  227. * Adds two vectors (a+b).
  228. * @param a The first vector
  229. * @param b The second vector
  230. * @param result Storage for the result, if null, store in a.
  231. **/
  232. public static void plus(float[] a, float[] b, float[] result) {
  233. float[] res = (result == null)?a:result;
  234. for (int i=0;i<a.length;i++)
  235. res[i] = a[i]+b[i];
  236. }
  237. /**
  238. * Adds two vectors (a+b).
  239. * @param a The first vector
  240. * @param b The second vector
  241. * @param result Storage for the result, if null, store in a.
  242. **/
  243. public static void plus(int[] a, int[] b, int[] result) {
  244. int[] res = (result == null)?a:result;
  245. for (int i=0;i<a.length;i++)
  246. res[i] = a[i]+b[i];
  247. }
  248. }