PageRenderTime 27ms CodeModel.GetById 9ms app.highlight 14ms RepoModel.GetById 2ms app.codeStats 0ms

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