JOGL v2.6.0-rc-20250706
JOGL, High-Performance Graphics Binding for Java™ (public API).
Matrix4fb.java
Go to the documentation of this file.
1/**
2 * Copyright 2014-2023 JogAmp Community. All rights reserved.
3 *
4 * Redistribution and use in source and binary forms, with or without modification, are
5 * permitted provided that the following conditions are met:
6 *
7 * 1. Redistributions of source code must retain the above copyright notice, this list of
8 * conditions and the following disclaimer.
9 *
10 * 2. Redistributions in binary form must reproduce the above copyright notice, this list
11 * of conditions and the following disclaimer in the documentation and/or other materials
12 * provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
15 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
17 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 *
24 * The views and conclusions contained in the software and documentation are those of the
25 * authors and should not be interpreted as representing official policies, either expressed
26 * or implied, of JogAmp Community.
27 */
28
29package com.jogamp.opengl.test.junit.math;
30
31import java.nio.FloatBuffer;
32
33import com.jogamp.math.FloatUtil;
34import com.jogamp.math.FovHVHalves;
35import com.jogamp.math.Quaternion;
36import com.jogamp.math.Ray;
37import com.jogamp.math.Vec3f;
38import com.jogamp.math.Vec4f;
39import com.jogamp.math.VectorUtil;
40import com.jogamp.math.geom.AABBox;
41import com.jogamp.math.geom.Frustum;
42import com.jogamp.math.geom.Frustum.Plane;
43
44/**
45 * Basic 4x4 float matrix implementation using fields for intensive use-cases (host operations).
46 * <p>
47 * Implementation covers {@link FloatUtil} matrix functionality, exposed in an object oriented manner.
48 * </p>
49 * <p>
50 * Unlike {@link com.jogamp.opengl.util.PMVMatrix PMVMatrix}, this class only represents one single matrix
51 * without a complete {@link com.jogamp.opengl.fixedfunc.GLMatrixFunc GLMatrixFunc} implementation.
52 * </p>
53 * <p>
54 * For array operations the layout is expected in column-major order
55 * matching OpenGL's implementation, illustration:
56 * <pre>
57 Row-Major Column-Major (OpenGL):
58
59 | 0 1 2 tx |
60 | |
61 | 4 5 6 ty |
62 M = | |
63 | 8 9 10 tz |
64 | |
65 | 12 13 14 15 |
66
67 R C R C
68 m[0*4+3] = tx; m[0+4*3] = tx;
69 m[1*4+3] = ty; m[1+4*3] = ty;
70 m[2*4+3] = tz; m[2+4*3] = tz;
71
72 RC (std subscript order) RC (std subscript order)
73 m[0+3*4] = tx; m[0+3*4] = tx;
74 m[1+3*4] = ty; m[1+3*4] = ty;
75 m[2+3*4] = tz; m[2+3*4] = tz;
76
77 * </pre>
78 * </p>
79 * <p>
80 * <ul>
81 * <li><a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html">Matrix-FAQ</a></li>
82 * <li><a href="https://en.wikipedia.org/wiki/Matrix_%28mathematics%29">Wikipedia-Matrix</a></li>
83 * <li><a href="http://www.euclideanspace.com/maths/algebra/matrix/index.htm">euclideanspace.com-Matrix</a></li>
84 * </ul>
85 * </p>
86 * <p>
87 * Implementation utilizes unrolling of small vertices and matrices wherever possible
88 * while trying to access memory in a linear fashion for performance reasons, see:
89 * <ul>
90 * <li><a href="https://lessthanoptimal.github.io/Java-Matrix-Benchmark/">java-matrix-benchmark</a></li>
91 * <li><a href="https://github.com/lessthanoptimal/ejml">EJML Efficient Java Matrix Library</a></li>
92 * </ul>
93 * </p>
94 * @see com.jogamp.opengl.util.PMVMatrix
95 * @see FloatUtil
96 */
97public class Matrix4fb {
98
99 /**
100 * Creates a new identity matrix.
101 */
102 public Matrix4fb() {
103 loadIdentity();
104 }
105
106 /**
107 * Creates a new matrix copying the values of the given {@code src} matrix.
108 */
109 public Matrix4fb(final Matrix4fb src) {
110 load(src);
111 }
112
113 /**
114 * Creates a new matrix based on given float[4*4] column major order.
115 * @param m 4x4 matrix in column-major order
116 */
117 public Matrix4fb(final float[] m) {
118 load(m);
119 }
120
121 /**
122 * Creates a new matrix based on given float[4*4] column major order.
123 * @param m 4x4 matrix in column-major order
124 * @param m_off offset for matrix {@code m}
125 */
126 public Matrix4fb(final float[] m, final int m_off) {
127 load(m, m_off);
128 }
129
130 //
131 // Write to Matrix via load(..)
132 //
133
134 /**
135 * Set this matrix to identity.
136 * <pre>
137 Translation matrix (Column Order):
138 1 0 0 0
139 0 1 0 0
140 0 0 1 0
141 0 0 0 1
142 * </pre>
143 * @return this matrix for chaining
144 */
145 public final Matrix4fb loadIdentity() {
146 m[0+0*4] = m[1+1*4] = m[2+2*4] = m[3+3*4] = 1.0f;
147 m[0+1*4] = m[0+2*4] = m[0+3*4] =
148 m[1+0*4] = m[1+2*4] = m[1+3*4] =
149 m[2+0*4] = m[2+1*4] = m[2+3*4] =
150 m[3+0*4] = m[3+1*4] = m[3+2*4] = 0.0f;
151 return this;
152 }
153
154 /**
155 * Load the values of the given matrix {@code b} to this matrix.
156 * @param src the source values
157 * @return this matrix for chaining
158 */
159 public Matrix4fb load(final Matrix4fb src) {
160 System.arraycopy(src.m, 0, m, 0, 16);
161 return this;
162 }
163
164 /**
165 * Load the values of the given matrix {@code src} to this matrix.
166 * @param src 4x4 matrix float[16] in column-major order
167 * @return this matrix for chaining
168 */
169 public Matrix4fb load(final float[] src) {
170 System.arraycopy(src, 0, m, 0, 16);
171 return this;
172 }
173
174 /**
175 * Load the values of the given matrix {@code src} to this matrix.
176 * @param src 4x4 matrix float[16] in column-major order
177 * @param src_off offset for matrix {@code src}
178 * @return this matrix for chaining
179 */
180 public Matrix4fb load(final float[] src, final int src_off) {
181 System.arraycopy(src, src_off, m, 0, 16);
182 return this;
183 }
184
185 /**
186 * Load the values of the given matrix {@code src} to this matrix.
187 * <p>
188 * Implementation uses relative {@link FloatBuffer#get()},
189 * hence caller may want to issue {@link FloatBuffer#reset()} thereafter.
190 * </p>
191 * @param src 4x4 matrix {@link FloatBuffer} in column-major order
192 * @return this matrix for chaining
193 */
194 public Matrix4fb load(final FloatBuffer src) {
195 src.get(m, 0, 16);
196 return this;
197 }
198
199 //
200 // Read out Matrix via get(..)
201 //
202
203 /** Gets the ith component, 0 <= i < 16 */
204 public float get(final int i) {
205 return m[i];
206 }
207
208 /**
209 * Get the named column of the given column-major matrix to v_out.
210 * @param column named column to copy
211 * @param v_out the column-vector storage
212 * @return given result vector <i>v_out</i> for chaining
213 */
214 public Vec4f getColumn(final int column, final Vec4f v_out) {
215 v_out.set( get(0+column*4),
216 get(1+column*4),
217 get(2+column*4),
218 get(3+column*4) );
219 return v_out;
220 }
221
222 /**
223 * Get the named column of the given column-major matrix to v_out.
224 * @param column named column to copy
225 * @param v_out the column-vector storage
226 * @return given result vector <i>v_out</i> for chaining
227 */
228 public Vec3f getColumn(final int column, final Vec3f v_out) {
229 v_out.set( get(0+column*4),
230 get(1+column*4),
231 get(2+column*4) );
232 return v_out;
233 }
234
235 /**
236 * Get the named row of the given column-major matrix to v_out.
237 * @param row named row to copy
238 * @param v_out the row-vector storage
239 * @return given result vector <i>v_out</i> for chaining
240 */
241 public Vec4f getRow(final int row, final Vec4f v_out) {
242 v_out.set( get(row+0*4),
243 get(row+1*4),
244 get(row+2*4),
245 get(row+3*4) );
246 return v_out;
247 }
248
249 /**
250 * Get the named row of the given column-major matrix to v_out.
251 * @param row named row to copy
252 * @param v_out the row-vector storage
253 * @return given result vector <i>v_out</i> for chaining
254 */
255 public Vec3f getRow(final int row, final Vec3f v_out) {
256 v_out.set( get(row+0*4),
257 get(row+1*4),
258 get(row+2*4) );
259 return v_out;
260 }
261
262 /**
263 * Get this matrix into the given float[16] array at {@code dst_off} in column major order.
264 *
265 * @param dst float[16] array storage in column major order
266 * @param dst_off offset
267 * @return {@code dst} for chaining
268 */
269 public float[] get(final float[] dst, final int dst_off) {
270 System.arraycopy(m, 0, dst, dst_off, 16);
271 return dst;
272 }
273
274 /**
275 * Get this matrix into the given float[16] array in column major order.
276 *
277 * @param dst float[16] array storage in column major order
278 * @return {@code dst} for chaining
279 */
280 public float[] get(final float[] dst) {
281 System.arraycopy(m, 0, dst, 0, 16);
282 return dst;
283 }
284
285 /**
286 * Get this matrix into the given {@link FloatBuffer} in column major order.
287 * <p>
288 * Implementation uses relative {@link FloatBuffer#put(float)},
289 * hence caller may want to issue {@link FloatBuffer#reset()} thereafter.
290 * </p>
291 *
292 * @param dst {@link FloatBuffer} array storage in column major order
293 * @return {@code dst} for chaining
294 */
295 public FloatBuffer get(final FloatBuffer dst) {
296 dst.put(m, 0, 16);
297 return dst;
298 }
299
300 //
301 // Basic matrix operations
302 //
303
304 /**
305 * Returns the determinant of this matrix
306 * @return the matrix determinant
307 */
308 public float determinant() {
309 float ret = 0;
310 ret += m[0+0*4] * ( + m[1+1*4]*(m[2+2*4]*m[3+3*4] - m[2+3*4]*m[3+2*4]) - m[1+2*4]*(m[2+1*4]*m[3+3*4] - m[2+3*4]*m[3+1*4]) + m[1+3*4]*(m[2+1*4]*m[3+2*4] - m[2+2*4]*m[3+1*4]));
311 ret -= m[0+1*4] * ( + m[1+0*4]*(m[2+2*4]*m[3+3*4] - m[2+3*4]*m[3+2*4]) - m[1+2*4]*(m[2+0*4]*m[3+3*4] - m[2+3*4]*m[3+0*4]) + m[1+3*4]*(m[2+0*4]*m[3+2*4] - m[2+2*4]*m[3+0*4]));
312 ret += m[0+2*4] * ( + m[1+0*4]*(m[2+1*4]*m[3+3*4] - m[2+3*4]*m[3+1*4]) - m[1+1*4]*(m[2+0*4]*m[3+3*4] - m[2+3*4]*m[3+0*4]) + m[1+3*4]*(m[2+0*4]*m[3+1*4] - m[2+1*4]*m[3+0*4]));
313 ret -= m[0+3*4] * ( + m[1+0*4]*(m[2+1*4]*m[3+2*4] - m[2+2*4]*m[3+1*4]) - m[1+1*4]*(m[2+0*4]*m[3+2*4] - m[2+2*4]*m[3+0*4]) + m[1+2*4]*(m[2+0*4]*m[3+1*4] - m[2+1*4]*m[3+0*4]));
314 return ret;
315 }
316
317 /**
318 * Transpose this matrix.
319 *
320 * @return this matrix for chaining
321 */
322 public final Matrix4fb transpose() {
323 float tmp;
324
325 tmp = m[1+0*4];
326 m[1+0*4] = m[0+1*4];
327 m[0+1*4] = tmp;
328
329 tmp = m[2+0*4];
330 m[2+0*4] = m[0+2*4];
331 m[0+2*4] = tmp;
332
333 tmp = m[3+0*4];
334 m[3+0*4] = m[0+3*4];
335 m[0+3*4] = tmp;
336
337 tmp = m[2+1*4];
338 m[2+1*4] = m[1+2*4];
339 m[1+2*4] = tmp;
340
341 tmp = m[3+1*4];
342 m[3+1*4] = m[1+3*4];
343 m[1+3*4] = tmp;
344
345 tmp = m[3+2*4];
346 m[3+2*4] = m[2+3*4];
347 m[2+3*4] = tmp;
348
349 return this;
350 }
351
352 /**
353 * Transpose the given {@code src} matrix into this matrix.
354 *
355 * @param src source 4x4 matrix
356 * @return this matrix (result) for chaining
357 */
358 public final Matrix4fb transpose(final Matrix4fb src) {
359 if( src == this ) {
360 return transpose();
361 }
362 m[0+0*4] = src.m[0+0*4];
363 m[1+0*4] = src.m[0+1*4];
364 m[2+0*4] = src.m[0+2*4];
365 m[3+0*4] = src.m[0+3*4];
366
367 m[0+1*4] = src.m[1+0*4];
368 m[1+1*4] = src.m[1+1*4];
369 m[2+1*4] = src.m[1+2*4];
370 m[3+1*4] = src.m[1+3*4];
371
372 m[0+2*4] = src.m[2+0*4];
373 m[1+2*4] = src.m[2+1*4];
374 m[2+2*4] = src.m[2+2*4];
375 m[3+2*4] = src.m[2+3*4];
376
377 m[0+3*4] = src.m[3+0*4];
378 m[1+3*4] = src.m[3+1*4];
379 m[2+3*4] = src.m[3+2*4];
380 m[3+3*4] = src.m[3+3*4];
381 return this;
382 }
383
384 /**
385 * Invert this matrix.
386 * @return false if this matrix is singular and inversion not possible, otherwise true
387 */
388 public boolean invert() {
389 final float scale;
390 {
391 float max = Math.abs(m[0]);
392
393 for( int i = 1; i < 16; i++ ) {
394 final float a = Math.abs(m[i]);
395 if( a > max ) max = a;
396 }
397 if( 0 == max ) {
398 return false;
399 }
400 scale = 1.0f/max;
401 }
402
403 final float a00 = m[0+0*4]*scale;
404 final float a10 = m[1+0*4]*scale;
405 final float a20 = m[2+0*4]*scale;
406 final float a30 = m[3+0*4]*scale;
407
408 final float a01 = m[0+1*4]*scale;
409 final float a11 = m[1+1*4]*scale;
410 final float a21 = m[2+1*4]*scale;
411 final float a31 = m[3+1*4]*scale;
412
413 final float a02 = m[0+2*4]*scale;
414 final float a12 = m[1+2*4]*scale;
415 final float a22 = m[2+2*4]*scale;
416 final float a32 = m[3+2*4]*scale;
417
418 final float a03 = m[0+3*4]*scale;
419 final float a13 = m[1+3*4]*scale;
420 final float a23 = m[2+3*4]*scale;
421 final float a33 = m[3+3*4]*scale;
422
423 final float b00 = + a11*(a22*a33 - a23*a32) - a12*(a21*a33 - a23*a31) + a13*(a21*a32 - a22*a31);
424 final float b01 = -( + a10*(a22*a33 - a23*a32) - a12*(a20*a33 - a23*a30) + a13*(a20*a32 - a22*a30));
425 final float b02 = + a10*(a21*a33 - a23*a31) - a11*(a20*a33 - a23*a30) + a13*(a20*a31 - a21*a30);
426 final float b03 = -( + a10*(a21*a32 - a22*a31) - a11*(a20*a32 - a22*a30) + a12*(a20*a31 - a21*a30));
427
428 final float b10 = -( + a01*(a22*a33 - a23*a32) - a02*(a21*a33 - a23*a31) + a03*(a21*a32 - a22*a31));
429 final float b11 = + a00*(a22*a33 - a23*a32) - a02*(a20*a33 - a23*a30) + a03*(a20*a32 - a22*a30);
430 final float b12 = -( + a00*(a21*a33 - a23*a31) - a01*(a20*a33 - a23*a30) + a03*(a20*a31 - a21*a30));
431 final float b13 = + a00*(a21*a32 - a22*a31) - a01*(a20*a32 - a22*a30) + a02*(a20*a31 - a21*a30);
432
433 final float b20 = + a01*(a12*a33 - a13*a32) - a02*(a11*a33 - a13*a31) + a03*(a11*a32 - a12*a31);
434 final float b21 = -( + a00*(a12*a33 - a13*a32) - a02*(a10*a33 - a13*a30) + a03*(a10*a32 - a12*a30));
435 final float b22 = + a00*(a11*a33 - a13*a31) - a01*(a10*a33 - a13*a30) + a03*(a10*a31 - a11*a30);
436 final float b23 = -( + a00*(a11*a32 - a12*a31) - a01*(a10*a32 - a12*a30) + a02*(a10*a31 - a11*a30));
437
438 final float b30 = -( + a01*(a12*a23 - a13*a22) - a02*(a11*a23 - a13*a21) + a03*(a11*a22 - a12*a21));
439 final float b31 = + a00*(a12*a23 - a13*a22) - a02*(a10*a23 - a13*a20) + a03*(a10*a22 - a12*a20);
440 final float b32 = -( + a00*(a11*a23 - a13*a21) - a01*(a10*a23 - a13*a20) + a03*(a10*a21 - a11*a20));
441 final float b33 = + a00*(a11*a22 - a12*a21) - a01*(a10*a22 - a12*a20) + a02*(a10*a21 - a11*a20);
442
443 final float det = (a00*b00 + a01*b01 + a02*b02 + a03*b03) / scale;
444 if( 0 == det ) {
445 return false;
446 }
447 final float invdet = 1.0f / det;
448
449 m[0+0*4] = b00 * invdet;
450 m[1+0*4] = b01 * invdet;
451 m[2+0*4] = b02 * invdet;
452 m[3+0*4] = b03 * invdet;
453
454 m[0+1*4] = b10 * invdet;
455 m[1+1*4] = b11 * invdet;
456 m[2+1*4] = b12 * invdet;
457 m[3+1*4] = b13 * invdet;
458
459 m[0+2*4] = b20 * invdet;
460 m[1+2*4] = b21 * invdet;
461 m[2+2*4] = b22 * invdet;
462 m[3+2*4] = b23 * invdet;
463
464 m[0+3*4] = b30 * invdet;
465 m[1+3*4] = b31 * invdet;
466 m[2+3*4] = b32 * invdet;
467 m[3+3*4] = b33 * invdet;
468 return true;
469 }
470
471 /**
472 * Invert the {@code src} matrix values into this matrix
473 * @param src the source matrix, which values are to be inverted
474 * @return false if {@code src} matrix is singular and inversion not possible, otherwise true
475 */
476 public boolean invert(final Matrix4fb src) {
477 final float scale;
478 {
479 float max = Math.abs(src.m[0]);
480
481 for( int i = 1; i < 16; i++ ) {
482 final float a = Math.abs(src.m[i]);
483 if( a > max ) max = a;
484 }
485 if( 0 == max ) {
486 return false;
487 }
488 scale = 1.0f/max;
489 }
490
491 final float a00 = src.m[0+0*4]*scale;
492 final float a10 = src.m[1+0*4]*scale;
493 final float a20 = src.m[2+0*4]*scale;
494 final float a30 = src.m[3+0*4]*scale;
495
496 final float a01 = src.m[0+1*4]*scale;
497 final float a11 = src.m[1+1*4]*scale;
498 final float a21 = src.m[2+1*4]*scale;
499 final float a31 = src.m[3+1*4]*scale;
500
501 final float a02 = src.m[0+2*4]*scale;
502 final float a12 = src.m[1+2*4]*scale;
503 final float a22 = src.m[2+2*4]*scale;
504 final float a32 = src.m[3+2*4]*scale;
505
506 final float a03 = src.m[0+3*4]*scale;
507 final float a13 = src.m[1+3*4]*scale;
508 final float a23 = src.m[2+3*4]*scale;
509 final float a33 = src.m[3+3*4]*scale;
510
511 final float b00 = + a11*(a22*a33 - a23*a32) - a12*(a21*a33 - a23*a31) + a13*(a21*a32 - a22*a31);
512 final float b01 = -( + a10*(a22*a33 - a23*a32) - a12*(a20*a33 - a23*a30) + a13*(a20*a32 - a22*a30));
513 final float b02 = + a10*(a21*a33 - a23*a31) - a11*(a20*a33 - a23*a30) + a13*(a20*a31 - a21*a30);
514 final float b03 = -( + a10*(a21*a32 - a22*a31) - a11*(a20*a32 - a22*a30) + a12*(a20*a31 - a21*a30));
515
516 final float b10 = -( + a01*(a22*a33 - a23*a32) - a02*(a21*a33 - a23*a31) + a03*(a21*a32 - a22*a31));
517 final float b11 = + a00*(a22*a33 - a23*a32) - a02*(a20*a33 - a23*a30) + a03*(a20*a32 - a22*a30);
518 final float b12 = -( + a00*(a21*a33 - a23*a31) - a01*(a20*a33 - a23*a30) + a03*(a20*a31 - a21*a30));
519 final float b13 = + a00*(a21*a32 - a22*a31) - a01*(a20*a32 - a22*a30) + a02*(a20*a31 - a21*a30);
520
521 final float b20 = + a01*(a12*a33 - a13*a32) - a02*(a11*a33 - a13*a31) + a03*(a11*a32 - a12*a31);
522 final float b21 = -( + a00*(a12*a33 - a13*a32) - a02*(a10*a33 - a13*a30) + a03*(a10*a32 - a12*a30));
523 final float b22 = + a00*(a11*a33 - a13*a31) - a01*(a10*a33 - a13*a30) + a03*(a10*a31 - a11*a30);
524 final float b23 = -( + a00*(a11*a32 - a12*a31) - a01*(a10*a32 - a12*a30) + a02*(a10*a31 - a11*a30));
525
526 final float b30 = -( + a01*(a12*a23 - a13*a22) - a02*(a11*a23 - a13*a21) + a03*(a11*a22 - a12*a21));
527 final float b31 = + a00*(a12*a23 - a13*a22) - a02*(a10*a23 - a13*a20) + a03*(a10*a22 - a12*a20);
528 final float b32 = -( + a00*(a11*a23 - a13*a21) - a01*(a10*a23 - a13*a20) + a03*(a10*a21 - a11*a20));
529 final float b33 = + a00*(a11*a22 - a12*a21) - a01*(a10*a22 - a12*a20) + a02*(a10*a21 - a11*a20);
530
531 final float det = (a00*b00 + a01*b01 + a02*b02 + a03*b03) / scale;
532 if( 0 == det ) {
533 return false;
534 }
535 final float invdet = 1.0f / det;
536
537 m[0+0*4] = b00 * invdet;
538 m[1+0*4] = b01 * invdet;
539 m[2+0*4] = b02 * invdet;
540 m[3+0*4] = b03 * invdet;
541
542 m[0+1*4] = b10 * invdet;
543 m[1+1*4] = b11 * invdet;
544 m[2+1*4] = b12 * invdet;
545 m[3+1*4] = b13 * invdet;
546
547 m[0+2*4] = b20 * invdet;
548 m[1+2*4] = b21 * invdet;
549 m[2+2*4] = b22 * invdet;
550 m[3+2*4] = b23 * invdet;
551
552 m[0+3*4] = b30 * invdet;
553 m[1+3*4] = b31 * invdet;
554 m[2+3*4] = b32 * invdet;
555 m[3+3*4] = b33 * invdet;
556 return true;
557 }
558
559 /**
560 * Multiply matrix: [this] = [this] x [b]
561 * <p>
562 * Roughly 15% slower than {@link #mul(Matrix4fb, Matrix4fb)}
563 * Roughly 3% slower than {@link FloatUtil#multMatrix(float[], float[])}
564 * </p>
565 * @param b 4x4 matrix
566 * @return this matrix for chaining
567 * @see #mul(Matrix4fb, Matrix4fb)
568 */
569 public final Matrix4fb mul(final Matrix4fb b) {
570 final float b00 = b.m[0+0*4];
571 final float b10 = b.m[1+0*4];
572 final float b20 = b.m[2+0*4];
573 final float b30 = b.m[3+0*4];
574 final float b01 = b.m[0+1*4];
575 final float b11 = b.m[1+1*4];
576 final float b21 = b.m[2+1*4];
577 final float b31 = b.m[3+1*4];
578 final float b02 = b.m[0+2*4];
579 final float b12 = b.m[1+2*4];
580 final float b22 = b.m[2+2*4];
581 final float b32 = b.m[3+2*4];
582 final float b03 = b.m[0+3*4];
583 final float b13 = b.m[1+3*4];
584 final float b23 = b.m[2+3*4];
585 final float b33 = b.m[3+3*4];
586
587 float ai0=m[0+0*4]; // row-0, m[0+0*4]
588 float ai1=m[0+1*4];
589 float ai2=m[0+2*4];
590 float ai3=m[0+3*4];
591 m[0+0*4] = ai0 * b00 + ai1 * b10 + ai2 * b20 + ai3 * b30 ;
592 m[0+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ;
593 m[0+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ;
594 m[0+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ;
595
596 ai0=m[1+0*4]; //row-1, m[1+0*4]
597 ai1=m[1+1*4];
598 ai2=m[1+2*4];
599 ai3=m[1+3*4];
600 m[1+0*4] = ai0 * b00 + ai1 * b10 + ai2 * b20 + ai3 * b30 ;
601 m[1+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ;
602 m[1+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ;
603 m[1+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ;
604
605 ai0=m[2+0*4]; // row-2, m[2+0*4]
606 ai1=m[2+1*4];
607 ai2=m[2+2*4];
608 ai3=m[2+3*4];
609 m[2+0*4] = ai0 * b00 + ai1 * b10 + ai2 * b20 + ai3 * b30 ;
610 m[2+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ;
611 m[2+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ;
612 m[2+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ;
613
614 ai0=m[3+0*4]; // row-3, m[3+0*4]
615 ai1=m[3+1*4];
616 ai2=m[3+2*4];
617 ai3=m[3+3*4];
618 m[3+0*4] = ai0 * b00 + ai1 * b10 + ai2 * b20 + ai3 * b30 ;
619 m[3+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ;
620 m[3+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ;
621 m[3+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ;
622 return this;
623 }
624
625 /**
626 * Multiply matrix: [this] = [a] x [b]
627 * <p>
628 * Roughly 13% faster than {@link #mul(Matrix4fb)}
629 * Roughly 11% faster than {@link FloatUtil#multMatrix(float[], float[])}
630 * </p>
631 * @param a 4x4 matrix
632 * @param b 4x4 matrix
633 * @return this matrix for chaining
634 * @see #mul(Matrix4fb)
635 */
636 public final Matrix4fb mul(final Matrix4fb a, final Matrix4fb b) {
637 final float b00 = b.m[0+0*4];
638 final float b10 = b.m[1+0*4];
639 final float b20 = b.m[2+0*4];
640 final float b30 = b.m[3+0*4];
641 final float b01 = b.m[0+1*4];
642 final float b11 = b.m[1+1*4];
643 final float b21 = b.m[2+1*4];
644 final float b31 = b.m[3+1*4];
645 final float b02 = b.m[0+2*4];
646 final float b12 = b.m[1+2*4];
647 final float b22 = b.m[2+2*4];
648 final float b32 = b.m[3+2*4];
649 final float b03 = b.m[0+3*4];
650 final float b13 = b.m[1+3*4];
651 final float b23 = b.m[2+3*4];
652 final float b33 = b.m[3+3*4];
653
654 float ai0=a.m[0+0*4]; // row-0, m[0+0*4]
655 float ai1=a.m[0+1*4];
656 float ai2=a.m[0+2*4];
657 float ai3=a.m[0+3*4];
658 m[0+0*4] = ai0 * b00 + ai1 * b10 + ai2 * b20 + ai3 * b30 ;
659 m[0+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ;
660 m[0+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ;
661 m[0+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ;
662
663 ai0=a.m[1+0*4]; //row-1, m[1+0*4]
664 ai1=a.m[1+1*4];
665 ai2=a.m[1+2*4];
666 ai3=a.m[1+3*4];
667 m[1+0*4] = ai0 * b00 + ai1 * b10 + ai2 * b20 + ai3 * b30 ;
668 m[1+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ;
669 m[1+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ;
670 m[1+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ;
671
672 ai0=a.m[2+0*4]; // row-2, m[2+0*4]
673 ai1=a.m[2+1*4];
674 ai2=a.m[2+2*4];
675 ai3=a.m[2+3*4];
676 m[2+0*4] = ai0 * b00 + ai1 * b10 + ai2 * b20 + ai3 * b30 ;
677 m[2+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ;
678 m[2+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ;
679 m[2+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ;
680
681 ai0=a.m[3+0*4]; // row-3, m[3+0*4]
682 ai1=a.m[3+1*4];
683 ai2=a.m[3+2*4];
684 ai3=a.m[3+3*4];
685 m[3+0*4] = ai0 * b00 + ai1 * b10 + ai2 * b20 + ai3 * b30 ;
686 m[3+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ;
687 m[3+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ;
688 m[3+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ;
689 return this;
690 }
691
692 /**
693 * @param v_in 4-component column-vector
694 * @param v_out this * v_in
695 * @returns v_out for chaining
696 */
697 public final float[] mulVec4f(final float[/*4*/] v_in, final float[/*4*/] v_out) {
698 // (one matrix row in column-major order) X (column vector)
699 final float x = v_in[0], y = v_in[1], z = v_in[2], w = v_in[3];
700 v_out[0] = x * m[0+0*4] + y * m[0+1*4] + z * m[0+2*4] + w * m[0+3*4];
701 v_out[1] = x * m[1+0*4] + y * m[1+1*4] + z * m[1+2*4] + w * m[1+3*4];
702 v_out[2] = x * m[2+0*4] + y * m[2+1*4] + z * m[2+2*4] + w * m[2+3*4];
703 v_out[3] = x * m[3+0*4] + y * m[3+1*4] + z * m[3+2*4] + w * m[3+3*4];
704 return v_out;
705 }
706
707 /**
708 * @param v_in 4-component column-vector
709 * @param v_out this * v_in
710 * @returns v_out for chaining
711 */
712 public final Vec4f mulVec4f(final Vec4f v_in, final Vec4f v_out) {
713 // (one matrix row in column-major order) X (column vector)
714 final float x = v_in.x(), y = v_in.y(), z = v_in.z(), w = v_in.w();
715 v_out.set( x * m[0+0*4] + y * m[0+1*4] + z * m[0+2*4] + w * m[0+3*4],
716 x * m[1+0*4] + y * m[1+1*4] + z * m[1+2*4] + w * m[1+3*4],
717 x * m[2+0*4] + y * m[2+1*4] + z * m[2+2*4] + w * m[2+3*4],
718 x * m[3+0*4] + y * m[3+1*4] + z * m[3+2*4] + w * m[3+3*4] );
719 return v_out;
720 }
721
722 /**
723 * Affine 3f-vector transformation by 4x4 matrix
724 *
725 * 4x4 matrix multiplication with 3-component vector,
726 * using {@code 1} for for {@code v_in[3]} and dropping {@code v_out[3]},
727 * which shall be {@code 1}.
728 *
729 * @param v_in 3-component column-vector
730 * @param v_out m_in * v_in, 3-component column-vector
731 * @returns v_out for chaining
732 */
733 public final float[] mulVec3f(final float[/*3*/] v_in, final float[/*3*/] v_out) {
734 // (one matrix row in column-major order) X (column vector)
735 final float x = v_in[0], y = v_in[1], z = v_in[2];
736 v_out[0] = x * m[0+0*4] + y * m[0+1*4] + z * m[0+2*4] + 1f * m[0+3*4];
737 v_out[1] = x * m[1+0*4] + y * m[1+1*4] + z * m[1+2*4] + 1f * m[1+3*4];
738 v_out[2] = x * m[2+0*4] + y * m[2+1*4] + z * m[2+2*4] + 1f * m[2+3*4];
739 return v_out;
740 }
741
742 /**
743 * Affine 3f-vector transformation by 4x4 matrix
744 *
745 * 4x4 matrix multiplication with 3-component vector,
746 * using {@code 1} for for {@code v_in.w()} and dropping {@code v_out.w()},
747 * which shall be {@code 1}.
748 *
749 * @param v_in 3-component column-vector {@link Vec3f}
750 * @param v_out m_in * v_in, 3-component column-vector {@link Vec3f}
751 * @returns v_out for chaining
752 */
753 public final Vec3f mulVec3f(final Vec3f v_in, final Vec3f v_out) {
754 // (one matrix row in column-major order) X (column vector)
755 final float x = v_in.x(), y = v_in.y(), z = v_in.z();
756 v_out.set( x * m[0+0*4] + y * m[0+1*4] + z * m[0+2*4] + 1f * m[0+3*4],
757 x * m[1+0*4] + y * m[1+1*4] + z * m[1+2*4] + 1f * m[1+3*4],
758 x * m[2+0*4] + y * m[2+1*4] + z * m[2+2*4] + 1f * m[2+3*4] );
759 return v_out;
760 }
761
762 //
763 // Matrix setTo...(), affine + basic
764 //
765
766 /**
767 * Set this matrix to translation.
768 * <pre>
769 Translation matrix (Column Order):
770 1 0 0 0
771 0 1 0 0
772 0 0 1 0
773 x y z 1
774 * </pre>
775 * @param x x-axis translate
776 * @param y y-axis translate
777 * @param z z-axis translate
778 * @return this matrix for chaining
779 */
780 public final Matrix4fb setToTranslation(final float x, final float y, final float z) {
781 m[0+0*4] = m[1+1*4] = m[2+2*4] = m[3+3*4] = 1.0f;
782 m[0+3*4] = x;
783 m[1+3*4] = y;
784 m[2+3*4] = z;
785 m[0+1*4] = m[0+2*4] =
786 m[1+0*4] = m[1+2*4] =
787 m[2+0*4] = m[2+1*4] =
788 m[3+0*4] = m[3+1*4] = m[3+2*4] = 0.0f;
789 return this;
790 }
791
792 /**
793 * Set this matrix to translation.
794 * <pre>
795 Translation matrix (Column Order):
796 1 0 0 0
797 0 1 0 0
798 0 0 1 0
799 x y z 1
800 * </pre>
801 * @param t translate Vec3f
802 * @return this matrix for chaining
803 */
804 public final Matrix4fb setToTranslation(final Vec3f t) {
805 return setToTranslation(t.x(), t.y(), t.z());
806 }
807
808 /**
809 * Set this matrix to scale.
810 * <pre>
811 Scale matrix (Any Order):
812 x 0 0 0
813 0 y 0 0
814 0 0 z 0
815 0 0 0 1
816 * </pre>
817 * @param x x-axis scale
818 * @param y y-axis scale
819 * @param z z-axis scale
820 * @return this matrix for chaining
821 */
822 public final Matrix4fb setToScale(final float x, final float y, final float z) {
823 m[3+3*4] = 1.0f;
824 m[0+0*4] = x;
825 m[1+1*4] = y;
826 m[2+2*4] = z;
827 m[0+1*4] = m[0+2*4] = m[0+3*4] =
828 m[1+0*4] = m[1+2*4] = m[1+3*4] =
829 m[2+0*4] = m[2+1*4] = m[2+3*4] =
830 m[3+0*4] = m[3+1*4] = m[3+2*4] = 0.0f;
831 return this;
832 }
833
834 /**
835 * Set this matrix to rotation from the given axis and angle in radians.
836 * <pre>
837 Rotation matrix (Column Order):
838 xx(1-c)+c xy(1-c)+zs xz(1-c)-ys 0
839 xy(1-c)-zs yy(1-c)+c yz(1-c)+xs 0
840 xz(1-c)+ys yz(1-c)-xs zz(1-c)+c 0
841 0 0 0 1
842 * </pre>
843 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q38">Matrix-FAQ Q38</a>
844 * @param ang_rad angle in radians
845 * @param x x of rotation axis
846 * @param y y of rotation axis
847 * @param z z of rotation axis
848 * @return this matrix for chaining
849 */
850 public final Matrix4fb setToRotationAxis(final float ang_rad, float x, float y, float z) {
851 final float c = FloatUtil.cos(ang_rad);
852 final float ic= 1.0f - c;
853 final float s = FloatUtil.sin(ang_rad);
854
855 final float[] tmpVec3f = { x, y, z };
856 VectorUtil.normalizeVec3(tmpVec3f);
857 x = tmpVec3f[0]; y = tmpVec3f[1]; z = tmpVec3f[2];
858
859 final float xy = x*y;
860 final float xz = x*z;
861 final float xs = x*s;
862 final float ys = y*s;
863 final float yz = y*z;
864 final float zs = z*s;
865 m[0+0*4] = x*x*ic+c;
866 m[1+0*4] = xy*ic+zs;
867 m[2+0*4] = xz*ic-ys;
868 m[3+0*4] = 0;
869
870 m[0+1*4] = xy*ic-zs;
871 m[1+1*4] = y*y*ic+c;
872 m[2+1*4] = yz*ic+xs;
873 m[3+1*4] = 0;
874
875 m[0+2*4] = xz*ic+ys;
876 m[1+2*4] = yz*ic-xs;
877 m[2+2*4] = z*z*ic+c;
878 m[3+2*4] = 0;
879
880 m[0+3*4] = 0f;
881 m[1+3*4] = 0f;
882 m[2+3*4] = 0f;
883 m[3+3*4] = 1f;
884
885 return this;
886 }
887
888 /**
889 * Set this matrix to rotation from the given axis and angle in radians.
890 * <pre>
891 Rotation matrix (Column Order):
892 xx(1-c)+c xy(1-c)+zs xz(1-c)-ys 0
893 xy(1-c)-zs yy(1-c)+c yz(1-c)+xs 0
894 xz(1-c)+ys yz(1-c)-xs zz(1-c)+c 0
895 0 0 0 1
896 * </pre>
897 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q38">Matrix-FAQ Q38</a>
898 * @param ang_rad angle in radians
899 * @param axis rotation axis
900 * @return this matrix for chaining
901 */
902 public final Matrix4fb setToRotationAxis(final float ang_rad, final Vec3f axis) {
903 return setToRotationAxis(ang_rad, axis.x(), axis.y(), axis.z());
904 }
905
906 /**
907 * Set this matrix to rotation from the given Euler rotation angles in radians.
908 * <p>
909 * The rotations are applied in the given order:
910 * <ul>
911 * <li>y - heading</li>
912 * <li>z - attitude</li>
913 * <li>x - bank</li>
914 * </ul>
915 * </p>
916 * @param bankX the Euler pitch angle in radians. (rotation about the X axis)
917 * @param headingY the Euler yaw angle in radians. (rotation about the Y axis)
918 * @param attitudeZ the Euler roll angle in radians. (rotation about the Z axis)
919 * @return this matrix for chaining
920 * <p>
921 * Implementation does not use Quaternion and hence is exposed to
922 * <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q34">Gimbal-Lock</a>,
923 * consider using {@link #setToRotation(Quaternion)}.
924 * </p>
925 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q36">Matrix-FAQ Q36</a>
926 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/index.htm">euclideanspace.com-eulerToMatrix</a>
927 * @see #setToRotation(Quaternion)
928 */
929 public Matrix4fb setToRotationEuler(final float bankX, final float headingY, final float attitudeZ) {
930 // Assuming the angles are in radians.
931 final float ch = FloatUtil.cos(headingY);
932 final float sh = FloatUtil.sin(headingY);
933 final float ca = FloatUtil.cos(attitudeZ);
934 final float sa = FloatUtil.sin(attitudeZ);
935 final float cb = FloatUtil.cos(bankX);
936 final float sb = FloatUtil.sin(bankX);
937
938 m[0+0*4] = ch*ca;
939 m[1+0*4] = sa;
940 m[2+0*4] = -sh*ca;
941 m[3+0*4] = 0;
942
943 m[0+1*4] = sh*sb - ch*sa*cb;
944 m[1+1*4] = ca*cb;
945 m[2+1*4] = sh*sa*cb + ch*sb;
946 m[3+1*4] = 0;
947
948 m[0+2*4] = ch*sa*sb + sh*cb;
949 m[1+2*4] = -ca*sb;
950 m[2+2*4] = -sh*sa*sb + ch*cb;
951 m[3+2*4] = 0;
952
953 m[0+3*4] = 0;
954 m[1+3*4] = 0;
955 m[2+3*4] = 0;
956 m[3+3*4] = 1;
957
958 return this;
959 }
960
961 /**
962 * Set this matrix to rotation using the given Quaternion.
963 * <p>
964 * Implementation Details:
965 * <ul>
966 * <li> makes identity matrix if {@link #magnitudeSquared()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
967 * <li> The fields [m[0+0*4] .. m[2+2*4]] define the rotation</li>
968 * </ul>
969 * </p>
970 *
971 * @param q the Quaternion representing the rotation
972 * @return this matrix for chaining
973 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54">Matrix-FAQ Q54</a>
974 * @see Quaternion#toMatrix(float[])
975 * @see #getRotation()
976 */
977 public final Matrix4fb setToRotation(final Quaternion q) {
978 // pre-multiply scaled-reciprocal-magnitude to reduce multiplications
979 final float norm = q.magnitudeSquared();
980 if ( FloatUtil.isZero(norm) ) {
981 // identity matrix -> srecip = 0f
982 loadIdentity();
983 return this;
984 }
985 final float srecip;
986 if ( FloatUtil.isEqual(1f, norm) ) {
987 srecip = 2f;
988 } else {
989 srecip = 2.0f / norm;
990 }
991
992 final float x = q.x();
993 final float y = q.y();
994 final float z = q.z();
995 final float w = q.w();
996
997 final float xs = srecip * x;
998 final float ys = srecip * y;
999 final float zs = srecip * z;
1000
1001 final float xx = x * xs;
1002 final float xy = x * ys;
1003 final float xz = x * zs;
1004 final float xw = xs * w;
1005 final float yy = y * ys;
1006 final float yz = y * zs;
1007 final float yw = ys * w;
1008 final float zz = z * zs;
1009 final float zw = zs * w;
1010
1011 m[0+0*4] = 1f - ( yy + zz );
1012 m[0+1*4] = ( xy - zw );
1013 m[0+2*4] = ( xz + yw );
1014 m[0+3*4] = 0f;
1015
1016 m[1+0*4] = ( xy + zw );
1017 m[1+1*4] = 1f - ( xx + zz );
1018 m[1+2*4] = ( yz - xw );
1019 m[1+3*4] = 0f;
1020
1021 m[2+0*4] = ( xz - yw );
1022 m[2+1*4] = ( yz + xw );
1023 m[2+2*4] = 1f - ( xx + yy );
1024 m[2+3*4] = 0f;
1025
1026 m[3+0*4] = m[3+1*4] = m[3+2*4] = 0f;
1027 m[3+3*4] = 1f;
1028 return this;
1029 }
1030
1031 /**
1032 * Returns the rotation [m[0+0*4] .. m[2+2*4]] fields converted to a Quaternion.
1033 * @param res resulting Quaternion
1034 * @return the resulting Quaternion for chaining.
1035 * @see Quaternion#setFromMat(float, float, float, float, float, float, float, float, float)
1036 * @see #setToRotation(Quaternion)
1037 */
1038 public final Quaternion getRotation(final Quaternion res) {
1039 res.setFromMat(m[0+0*4], m[0+1*4], m[0+2*4], m[1+0*4], m[1+1*4], m[1+2*4], m[2+0*4], m[2+1*4], m[2+2*4]);
1040 return res;
1041 }
1042
1043 /**
1044 * Set this matrix to orthogonal projection.
1045 * <pre>
1046 Ortho matrix (Column Order):
1047 2/dx 0 0 0
1048 0 2/dy 0 0
1049 0 0 2/dz 0
1050 tx ty tz 1
1051 * </pre>
1052 * @param left
1053 * @param right
1054 * @param bottom
1055 * @param top
1056 * @param zNear
1057 * @param zFar
1058 * @return this matrix for chaining
1059 */
1060 public Matrix4fb setToOrtho(final float left, final float right,
1061 final float bottom, final float top,
1062 final float zNear, final float zFar) {
1063 {
1064 // m[0+0*4] = m[1+1*4] = m[2+2*4] = m[3+3*4] = 1f;
1065 m[1+0*4] = m[2+0*4] = m[3+0*4] = 0f;
1066 m[0+1*4] = m[2+1*4] = m[3+1*4] = 0f;
1067 m[0+2*4] = m[1+2*4] = m[3+2*4] = 0f;
1068 // m[0+3*4] = m[1+3*4] = m[2+3*4] = 0f;
1069 }
1070 final float dx=right-left;
1071 final float dy=top-bottom;
1072 final float dz=zFar-zNear;
1073 final float tx=-1.0f*(right+left)/dx;
1074 final float ty=-1.0f*(top+bottom)/dy;
1075 final float tz=-1.0f*(zFar+zNear)/dz;
1076
1077 m[0+0*4] = 2.0f/dx;
1078 m[1+1*4] = 2.0f/dy;
1079 m[2+2*4] = -2.0f/dz;
1080
1081 m[0+3*4] = tx;
1082 m[1+3*4] = ty;
1083 m[2+3*4] = tz;
1084 m[3+3*4] = 1f;
1085
1086 return this;
1087 }
1088
1089 /**
1090 * Set this matrix to frustum.
1091 * <pre>
1092 Frustum matrix (Column Order):
1093 2*zNear/dx 0 0 0
1094 0 2*zNear/dy 0 0
1095 A B C -1
1096 0 0 D 0
1097 * </pre>
1098 * @param left
1099 * @param right
1100 * @param bottom
1101 * @param top
1102 * @param zNear
1103 * @param zFar
1104 * @return this matrix for chaining
1105 * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear}
1106 * or {@code left == right}, or {@code bottom == top}.
1107 */
1108 public Matrix4fb setToFrustum(final float left, final float right,
1109 final float bottom, final float top,
1110 final float zNear, final float zFar) throws IllegalArgumentException {
1111 if( zNear <= 0.0f || zFar <= zNear ) {
1112 throw new IllegalArgumentException("Requirements zNear > 0 and zFar > zNear, but zNear "+zNear+", zFar "+zFar);
1113 }
1114 if( left == right || top == bottom) {
1115 throw new IllegalArgumentException("GL_INVALID_VALUE: top,bottom and left,right must not be equal");
1116 }
1117 {
1118 // m[0+0*4] = m[1+1*4] = m[2+2*4] = m[3+3*4] = 1f;
1119 m[1+0*4] = m[2+0*4] = m[3+0*4] = 0f;
1120 m[0+1*4] = m[2+1*4] = m[3+1*4] = 0f;
1121 m[0+3*4] = m[1+3*4] = 0f;
1122 }
1123 final float zNear2 = 2.0f*zNear;
1124 final float dx=right-left;
1125 final float dy=top-bottom;
1126 final float dz=zFar-zNear;
1127 final float A=(right+left)/dx;
1128 final float B=(top+bottom)/dy;
1129 final float C=-1.0f*(zFar+zNear)/dz;
1130 final float D=-2.0f*(zFar*zNear)/dz;
1131
1132 m[0+0*4] = zNear2/dx;
1133 m[1+1*4] = zNear2/dy;
1134
1135 m[0+2*4] = A;
1136 m[1+2*4] = B;
1137 m[2+2*4] = C;
1138 m[3+2*4] = -1.0f;
1139
1140 m[2+3*4] = D;
1141 m[3+3*4] = 0f;
1142
1143 return this;
1144 }
1145
1146 /**
1147 * Set this matrix to perspective {@link #setToFrustum(float, float, float, float, float, float) frustum} projection.
1148 *
1149 * @param fovy_rad angle in radians
1150 * @param aspect aspect ratio width / height
1151 * @param zNear
1152 * @param zFar
1153 * @return this matrix for chaining
1154 * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear}
1155 * @see #setToFrustum(float, float, float, float, float, float)
1156 */
1157 public Matrix4fb setToPerspective(final float fovy_rad, final float aspect, final float zNear, final float zFar) throws IllegalArgumentException {
1158 final float top = FloatUtil.tan(fovy_rad/2f) * zNear; // use tangent of half-fov !
1159 final float bottom = -1.0f * top; // -1f * fovhvTan.top * zNear
1160 final float left = aspect * bottom; // aspect * -1f * fovhvTan.top * zNear
1161 final float right = aspect * top; // aspect * fovhvTan.top * zNear
1162 return setToFrustum(left, right, bottom, top, zNear, zFar);
1163 }
1164
1165 /**
1166 * Set this matrix to perspective {@link #setToFrustum(float, float, float, float, float, float) frustum} projection.
1167 *
1168 * @param fovhv {@link FovHVHalves} field of view in both directions, may not be centered, either in radians or tangent
1169 * @param zNear
1170 * @param zFar
1171 * @return this matrix for chaining
1172 * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear}
1173 * @see #setToFrustum(float, float, float, float, float, float)
1174 * @see Frustum#updateByFovDesc(float[], int, boolean, Frustum.FovDesc)
1175 */
1176 public Matrix4fb setToPerspective(final FovHVHalves fovhv, final float zNear, final float zFar) throws IllegalArgumentException {
1177 final FovHVHalves fovhvTan = fovhv.toTangents(); // use tangent of half-fov !
1178 final float top = fovhvTan.top * zNear;
1179 final float bottom = -1.0f * fovhvTan.bottom * zNear;
1180 final float left = -1.0f * fovhvTan.left * zNear;
1181 final float right = fovhvTan.right * zNear;
1182 return setToFrustum(left, right, bottom, top, zNear, zFar);
1183 }
1184
1185 /**
1186 * Calculate the frustum planes in world coordinates
1187 * using the passed float[16] as premultiplied P*MV (column major order).
1188 * <p>
1189 * Frustum plane's normals will point to the inside of the viewing frustum,
1190 * as required by this class.
1191 * </p>
1192 */
1193 public void updateFrustumPlanes(final Frustum frustum) {
1194 // Left: a = m41 + m[1+1*4], b = m42 + m[1+2*4], c = m43 + m[1+3*4], d = m44 + m14 - [1..4] column-major
1195 // Left: a = m[3+0*4] + m[0+0*4], b = m[3+1*4] + m[0+1*4], c = m[3+2*4] + m[0+2*4], d = m[3+3*4] + m[0+3*4] - [0..3] column-major
1196 {
1197 final Frustum.Plane p = frustum.getPlanes()[Frustum.LEFT];
1198 final Vec3f p_n = p.n;
1199 p_n.set( m[3+0*4] + m[0+0*4],
1200 m[3+1*4] + m[0+1*4],
1201 m[3+2*4] + m[0+2*4] );
1202 p.d = m[3+3*4] + m[0+3*4];
1203 }
1204
1205 // Right: a = m41 - m[1+1*4], b = m42 - m[1+2*4], c = m43 - m[1+3*4], d = m44 - m14 - [1..4] column-major
1206 // Right: a = m[3+0*4] - m[0+0*4], b = m[3+1*4] - m[0+1*4], c = m[3+2*4] - m[0+2*4], d = m[3+3*4] - m[0+3*4] - [0..3] column-major
1207 {
1208 final Frustum.Plane p = frustum.getPlanes()[Frustum.RIGHT];
1209 final Vec3f p_n = p.n;
1210 p_n.set( m[3+0*4] - m[0+0*4],
1211 m[3+1*4] - m[0+1*4],
1212 m[3+2*4] - m[0+2*4] );
1213 p.d = m[3+3*4] - m[0+3*4];
1214 }
1215
1216 // Bottom: a = m41m21, b = m42m22, c = m43m[2+3*4], d = m44m24 - [1..4] column-major
1217 // Bottom: a = m30m10, b = m31m11, c = m32m12, d = m[3+3*4]m[1+3*4] - [0..3] column-major
1218 {
1219 final Frustum.Plane p = frustum.getPlanes()[Frustum.BOTTOM];
1220 final Vec3f p_n = p.n;
1221 p_n.set( m[3+0*4] + m[1+0*4],
1222 m[3+1*4] + m[1+1*4],
1223 m[3+2*4] + m[1+2*4] );
1224 p.d = m[3+3*4] + m[1+3*4];
1225 }
1226
1227 // Top: a = m41 - m[2+1*4], b = m42 - m[2+2*4], c = m43 - m[2+3*4], d = m44 - m24 - [1..4] column-major
1228 // Top: a = m[3+0*4] - m[1+0*4], b = m[3+1*4] - m[1+1*4], c = m[3+2*4] - m[1+2*4], d = m[3+3*4] - m[1+3*4] - [0..3] column-major
1229 {
1230 final Frustum.Plane p = frustum.getPlanes()[Frustum.TOP];
1231 final Vec3f p_n = p.n;
1232 p_n.set( m[3+0*4] - m[1+0*4],
1233 m[3+1*4] - m[1+1*4],
1234 m[3+2*4] - m[1+2*4] );
1235 p.d = m[3+3*4] - m[1+3*4];
1236 }
1237
1238 // Near: a = m41m31, b = m42m32, c = m43m[3+3*4], d = m44m34 - [1..4] column-major
1239 // Near: a = m30m20, b = m31m21, c = m32m22, d = m[3+3*4]m[2+3*4] - [0..3] column-major
1240 {
1241 final Frustum.Plane p = frustum.getPlanes()[Frustum.NEAR];
1242 final Vec3f p_n = p.n;
1243 p_n.set( m[3+0*4] + m[2+0*4],
1244 m[3+1*4] + m[2+1*4],
1245 m[3+2*4] + m[2+2*4] );
1246 p.d = m[3+3*4] + m[2+3*4];
1247 }
1248
1249 // Far: a = m41 - m[3+1*4], b = m42 - m[3+2*4], c = m43 - m[3+3*4], d = m44 - m34 - [1..4] column-major
1250 // Far: a = m[3+0*4] - m[2+0*4], b = m[3+1*4] - m[2+1*4], c = m32m22, d = m[3+3*4]m[2+3*4] - [0..3] column-major
1251 {
1252 final Frustum.Plane p = frustum.getPlanes()[Frustum.FAR];
1253 final Vec3f p_n = p.n;
1254 p_n.set( m[3+0*4] - m[2+0*4],
1255 m[3+1*4] - m[2+1*4],
1256 m[3+2*4] - m[2+2*4] );
1257 p.d = m[3+3*4] - m[2+3*4];
1258 }
1259
1260 // Normalize all planes
1261 for (int i = 0; i < 6; ++i) {
1262 final Plane p = frustum.getPlanes()[i];
1263 final Vec3f p_n = p.n;
1264 final float invLen = 1f / p_n.length();
1265 p_n.scale(invLen);
1266 p.d *= invLen;
1267 }
1268 }
1269
1270 /**
1271 * Make given matrix the <i>look-at</i> matrix based on given parameters.
1272 * <p>
1273 * Consist out of two matrix multiplications:
1274 * <pre>
1275 * <b>R</b> = <b>L</b> x <b>T</b>,
1276 * with <b>L</b> for <i>look-at</i> matrix and
1277 * <b>T</b> for eye translation.
1278 *
1279 * Result <b>R</b> can be utilized for <i>projection or modelview</i> multiplication, i.e.
1280 * <b>M</b> = <b>M</b> x <b>R</b>,
1281 * with <b>M</b> being the <i>projection or modelview</i> matrix.
1282 * </pre>
1283 * </p>
1284 * @param eye 3 component eye vector
1285 * @param center 3 component center vector
1286 * @param up 3 component up vector
1287 * @param tmp temporary Matrix4f used for multiplication
1288 * @return this matrix for chaining
1289 */
1290 public Matrix4fb setToLookAt(final Vec3f eye, final Vec3f center, final Vec3f up, final Matrix4fb tmp) {
1291 // normalized forward!
1292 final Vec3f fwd = new Vec3f( center.x() - eye.x(),
1293 center.y() - eye.y(),
1294 center.z() - eye.z() ).normalize();
1295
1296 /* Side = forward x up, normalized */
1297 final Vec3f side = fwd.cross(up).normalize();
1298
1299 /* Recompute up as: up = side x forward */
1300 final Vec3f up2 = side.cross(fwd);
1301
1302 m[0+0*4] = side.x();
1303 m[1+0*4] = up2.x();
1304 m[2+0*4] = -fwd.x();
1305 m[3+0*4] = 0;
1306
1307 m[0+1*4] = side.y();
1308 m[1+1*4] = up2.y();
1309 m[2+1*4] = -fwd.y();
1310 m[3+1*4] = 0;
1311
1312 m[0+2*4] = side.z();
1313 m[1+2*4] = up2.z();
1314 m[2+2*4] = -fwd.z();
1315 m[3+2*4] = 0;
1316
1317 m[0+3*4] = 0;
1318 m[1+3*4] = 0;
1319 m[2+3*4] = 0;
1320 m[3+3*4] = 1;
1321
1322 return mul( tmp.setToTranslation( -eye.x(), -eye.y(), -eye.z() ) );
1323 }
1324
1325 //
1326 // Matrix affine operations using setTo..()
1327 //
1328
1329 /**
1330 * Rotate this matrix about give axis and angle in radians, i.e. multiply by {@link #setToRotationAxis(float, float, float, float) axis-rotation matrix}.
1331 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q38">Matrix-FAQ Q38</a>
1332 * @param angrad angle in radians
1333 * @param x x of rotation axis
1334 * @param y y of rotation axis
1335 * @param z z of rotation axis
1336 * @param tmp temporary Matrix4f used for multiplication
1337 * @return this matrix for chaining
1338 */
1339 public final Matrix4fb rotate(final float ang_rad, final float x, final float y, final float z, final Matrix4fb tmp) {
1340 return mul( tmp.setToRotationAxis(ang_rad, x, y, z) );
1341 }
1342
1343 /**
1344 * Rotate this matrix about give axis and angle in radians, i.e. multiply by {@link #setToRotationAxis(float, Vec3f) axis-rotation matrix}.
1345 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q38">Matrix-FAQ Q38</a>
1346 * @param angrad angle in radians
1347 * @param axis rotation axis
1348 * @param tmp temporary Matrix4f used for multiplication
1349 * @return this matrix for chaining
1350 */
1351 public final Matrix4fb rotate(final float ang_rad, final Vec3f axis, final Matrix4fb tmp) {
1352 return mul( tmp.setToRotationAxis(ang_rad, axis) );
1353 }
1354
1355 /**
1356 * Rotate this matrix with the given {@link Quaternion}, i.e. multiply by {@link #setToRotation(Quaternion) Quaternion's rotation matrix}.
1357 * @param tmp temporary Matrix4f used for multiplication
1358 * @return this matrix for chaining
1359 */
1360 public final Matrix4fb rotate(final Quaternion quat, final Matrix4fb tmp) {
1361 return mul( tmp.setToRotation(quat) );
1362 }
1363
1364 /**
1365 * Translate this matrix, i.e. multiply by {@link #setToTranslation(float, float, float) translation matrix}.
1366 * @param x x translation
1367 * @param y y translation
1368 * @param z z translation
1369 * @param tmp temporary Matrix4f used for multiplication
1370 * @return this matrix for chaining
1371 */
1372 public final Matrix4fb translate(final float x, final float y, final float z, final Matrix4fb tmp) {
1373 return mul( tmp.setToTranslation(x, y, z) );
1374 }
1375
1376 /**
1377 * Translate this matrix, i.e. multiply by {@link #setToTranslation(Vec3f) translation matrix}.
1378 * @param t translation Vec3f
1379 * @param tmp temporary Matrix4f used for multiplication
1380 * @return this matrix for chaining
1381 */
1382 public final Matrix4fb translate(final Vec3f t, final Matrix4fb tmp) {
1383 return mul( tmp.setToTranslation(t) );
1384 }
1385
1386 /**
1387 * Scale this matrix, i.e. multiply by {@link #setToScale(float, float, float) scale matrix}.
1388 * @param x x scale
1389 * @param y y scale
1390 * @param z z scale
1391 * @param tmp temporary Matrix4f used for multiplication
1392 * @return this matrix for chaining
1393 */
1394 public final Matrix4fb scale(final float x, final float y, final float z, final Matrix4fb tmp) {
1395 return mul( tmp.setToScale(x, y, z) );
1396 }
1397
1398 /**
1399 * Scale this matrix, i.e. multiply by {@link #setToScale(float, float, float) scale matrix}.
1400 * @param s scale for x-, y- and z-axis
1401 * @param tmp temporary Matrix4f used for multiplication
1402 * @return this matrix for chaining
1403 */
1404 public final Matrix4fb scale(final float s, final Matrix4fb tmp) {
1405 return mul( tmp.setToScale(s, s, s) );
1406 }
1407
1408 //
1409 // Matrix Stack
1410 //
1411
1412 /**
1413 * Push the matrix to it's stack, while preserving this matrix values.
1414 * @see #pop()
1415 */
1416 public final void push() {
1417 stack.push(this);
1418 }
1419
1420 /**
1421 * Pop the current matrix from it's stack, replacing this matrix values.
1422 * @see #push()
1423 */
1424 public final void pop() {
1425 stack.pop(this);
1426 }
1427
1428 //
1429 // equals
1430 //
1431
1432 /**
1433 * Equals check using a given {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}.
1434 * <p>
1435 * Implementation considers following corner cases:
1436 * <ul>
1437 * <li>NaN == NaN</li>
1438 * <li>+Inf == +Inf</li>
1439 * <li>-Inf == -Inf</li>
1440 * </ul>
1441 * @param o comparison value
1442 * @param epsilon consider using {@link FloatUtil#EPSILON}
1443 * @return true if all components differ less than {@code epsilon}, otherwise false.
1444 */
1445 public boolean isEqual(final Matrix4fb o, final float epsilon) {
1446 if( this == o ) {
1447 return true;
1448 } else {
1449 for(int i=0; i<16; ++i) {
1450 if( !FloatUtil.isEqual(m[i], o.m[i], epsilon) ) {
1451 return false;
1452 }
1453 }
1454 return true;
1455 }
1456 }
1457
1458 /**
1459 * Equals check using {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}.
1460 * <p>
1461 * Implementation considers following corner cases:
1462 * <ul>
1463 * <li>NaN == NaN</li>
1464 * <li>+Inf == +Inf</li>
1465 * <li>-Inf == -Inf</li>
1466 * </ul>
1467 * @param o comparison value
1468 * @return true if all components differ less than {@link FloatUtil#EPSILON}, otherwise false.
1469 */
1470 public boolean isEqual(final Matrix4fb o) {
1471 return isEqual(o, FloatUtil.EPSILON);
1472 }
1473
1474 @Override
1475 public boolean equals(final Object o) {
1476 if( o instanceof Matrix4fb ) {
1477 return isEqual((Matrix4fb)o, FloatUtil.EPSILON);
1478 } else {
1479 return false;
1480 }
1481 }
1482
1483 //
1484 // Static multi Matrix ops
1485 //
1486
1487 /**
1488 * Map object coordinates to window coordinates.
1489 * <p>
1490 * Traditional <code>gluProject</code> implementation.
1491 * </p>
1492 *
1493 * @param obj object position, 3 component vector
1494 * @param mMv modelview matrix
1495 * @param mP projection matrix
1496 * @param viewport 4 component viewport vector
1497 * @param winPos 3 component window coordinate, the result
1498 * @return true if successful, otherwise false (z is 1)
1499 */
1500 public static boolean mapObjToWin(final Vec3f obj, final Matrix4fb mMv, final Matrix4fb mP,
1501 final int[] viewport, final float[] winPos)
1502 {
1503 final Vec4f vec4Tmp1 = new Vec4f(obj, 1f);
1504
1505 // vec4Tmp2 = Mv * o
1506 // rawWinPos = P * vec4Tmp2
1507 // rawWinPos = P * ( Mv * o )
1508 // rawWinPos = P * Mv * o
1509 final Vec4f vec4Tmp2 = mMv.mulVec4f(vec4Tmp1, new Vec4f());
1510 final Vec4f rawWinPos = mP.mulVec4f(vec4Tmp2, vec4Tmp1);
1511
1512 if (rawWinPos.w() == 0.0f) {
1513 return false;
1514 }
1515
1516 final float s = ( 1.0f / rawWinPos.w() ) * 0.5f;
1517
1518 // Map x, y and z to range 0-1 (w is ignored)
1519 rawWinPos.scale(s).add(0.5f, 0.5f, 0.5f, 0f);
1520
1521 // Map x,y to viewport
1522 winPos[0] = rawWinPos.x() * viewport[2] + viewport[0];
1523 winPos[1] = rawWinPos.y() * viewport[3] + viewport[1];
1524 winPos[2] = rawWinPos.z();
1525
1526 return true;
1527 }
1528
1529 /**
1530 * Map object coordinates to window coordinates.
1531 * <p>
1532 * Traditional <code>gluProject</code> implementation.
1533 * </p>
1534 *
1535 * @param obj object position, 3 component vector
1536 * @param mPMv [projection] x [modelview] matrix, i.e. P x Mv
1537 * @param viewport 4 component viewport vector
1538 * @param winPos 3 component window coordinate, the result
1539 * @return true if successful, otherwise false (z is 1)
1540 */
1541 public static boolean mapObjToWin(final Vec3f obj, final Matrix4fb mPMv,
1542 final int[] viewport, final float[] winPos)
1543 {
1544 final Vec4f vec4Tmp2 = new Vec4f(obj, 1f);
1545
1546 // rawWinPos = P * Mv * o
1547 final Vec4f rawWinPos = mPMv.mulVec4f(vec4Tmp2, new Vec4f());
1548
1549 if (rawWinPos.w() == 0.0f) {
1550 return false;
1551 }
1552
1553 final float s = ( 1.0f / rawWinPos.w() ) * 0.5f;
1554
1555 // Map x, y and z to range 0-1 (w is ignored)
1556 rawWinPos.scale(s).add(0.5f, 0.5f, 0.5f, 0f);
1557
1558 // Map x,y to viewport
1559 winPos[0] = rawWinPos.x() * viewport[2] + viewport[0];
1560 winPos[1] = rawWinPos.y() * viewport[3] + viewport[1];
1561 winPos[2] = rawWinPos.z();
1562
1563 return true;
1564 }
1565
1566 /**
1567 * Map window coordinates to object coordinates.
1568 * <p>
1569 * Traditional <code>gluUnProject</code> implementation.
1570 * </p>
1571 *
1572 * @param winx
1573 * @param winy
1574 * @param winz
1575 * @param mMv 4x4 modelview matrix
1576 * @param mP 4x4 projection matrix
1577 * @param viewport 4 component viewport vector
1578 * @param objPos 3 component object coordinate, the result
1579 * @param mat4Tmp 16 component matrix for temp storage
1580 * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z)
1581 */
1582 public static boolean mapWinToObj(final float winx, final float winy, final float winz,
1583 final Matrix4fb mMv, final Matrix4fb mP,
1584 final int[] viewport,
1585 final Vec3f objPos,
1586 final Matrix4fb mat4Tmp)
1587 {
1588 // invPMv = Inv(P x Mv)
1589 final Matrix4fb invPMv = mat4Tmp.mul(mP, mMv);
1590 if( !invPMv.invert() ) {
1591 return false;
1592 }
1593
1594 final Vec4f winPos = new Vec4f(winx, winy, winz, 1f);
1595
1596 // Map x and y from window coordinates
1597 winPos.add(-viewport[0], -viewport[1], 0f, 0f).mul(1f/viewport[2], 1f/viewport[3], 1f, 1f);
1598
1599 // Map to range -1 to 1
1600 winPos.mul(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f);
1601
1602 // rawObjPos = Inv(P x Mv) * winPos
1603 final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f());
1604
1605 if ( rawObjPos.w() == 0.0f ) {
1606 return false;
1607 }
1608 objPos.set( rawObjPos.scale( 1f / rawObjPos.w() ) );
1609
1610 return true;
1611 }
1612
1613 /**
1614 * Map window coordinates to object coordinates.
1615 * <p>
1616 * Traditional <code>gluUnProject</code> implementation.
1617 * </p>
1618 *
1619 * @param winx
1620 * @param winy
1621 * @param winz
1622 * @param invPMv inverse [projection] x [modelview] matrix, i.e. Inv(P x Mv)
1623 * @param viewport 4 component viewport vector
1624 * @param objPos 3 component object coordinate, the result
1625 * @param mat4Tmp 16 component matrix for temp storage
1626 * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z)
1627 */
1628 public static boolean mapWinToObj(final float winx, final float winy, final float winz,
1629 final Matrix4fb invPMv,
1630 final int[] viewport,
1631 final Vec3f objPos,
1632 final Matrix4fb mat4Tmp)
1633 {
1634 final Vec4f winPos = new Vec4f(winx, winy, winz, 1f);
1635
1636 // Map x and y from window coordinates
1637 winPos.add(-viewport[0], -viewport[1], 0f, 0f).mul(1f/viewport[2], 1f/viewport[3], 1f, 1f);
1638
1639 // Map to range -1 to 1
1640 winPos.mul(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f);
1641
1642 // rawObjPos = Inv(P x Mv) * winPos
1643 final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f());
1644
1645 if ( rawObjPos.w() == 0.0f ) {
1646 return false;
1647 }
1648 objPos.set( rawObjPos.scale( 1f / rawObjPos.w() ) );
1649
1650 return true;
1651 }
1652
1653 /**
1654 * Map two window coordinates to two object coordinates,
1655 * distinguished by their z component.
1656 * <p>
1657 * Traditional <code>gluUnProject</code> implementation.
1658 * </p>
1659 *
1660 * @param winx
1661 * @param winy
1662 * @param winz1
1663 * @param winz2
1664 * @param invPMv inverse [projection] x [modelview] matrix, i.e. Inv(P x Mv)
1665 * @param viewport 4 component viewport vector
1666 * @param objPos1 3 component object coordinate, the result
1667 * @param mat4Tmp 16 component matrix for temp storage
1668 * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z)
1669 */
1670 public static boolean mapWinToObj(final float winx, final float winy, final float winz1, final float winz2,
1671 final Matrix4fb invPMv,
1672 final int[] viewport,
1673 final Vec3f objPos1, final Vec3f objPos2,
1674 final Matrix4fb mat4Tmp)
1675 {
1676 final Vec4f winPos = new Vec4f(winx, winy, winz1, 1f);
1677
1678 // Map x and y from window coordinates
1679 winPos.add(-viewport[0], -viewport[1], 0f, 0f).mul(1f/viewport[2], 1f/viewport[3], 1f, 1f);
1680
1681 // Map to range -1 to 1
1682 winPos.mul(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f);
1683
1684 // rawObjPos = Inv(P x Mv) * winPos1
1685 final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f());
1686
1687 if ( rawObjPos.w() == 0.0f ) {
1688 return false;
1689 }
1690 objPos1.set( rawObjPos.scale( 1f / rawObjPos.w() ) );
1691
1692 //
1693 // winz2
1694 //
1695 // Map Z to range -1 to 1
1696 winPos.setZ( winz2 * 2f - 1f );
1697
1698 // rawObjPos = Inv(P x Mv) * winPos2
1699 invPMv.mulVec4f(winPos, rawObjPos);
1700
1701 if ( rawObjPos.w() == 0.0f ) {
1702 return false;
1703 }
1704 objPos2.set( rawObjPos.scale( 1f / rawObjPos.w() ) );
1705
1706 return true;
1707 }
1708
1709 /**
1710 * Map window coordinates to object coordinates.
1711 * <p>
1712 * Traditional <code>gluUnProject4</code> implementation.
1713 * </p>
1714 *
1715 * @param winx
1716 * @param winy
1717 * @param winz
1718 * @param clipw
1719 * @param mMv 4x4 modelview matrix
1720 * @param mP 4x4 projection matrix
1721 * @param viewport 4 component viewport vector
1722 * @param near
1723 * @param far
1724 * @param obj_pos 4 component object coordinate, the result
1725 * @param mat4Tmp 16 component matrix for temp storage
1726 * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z)
1727 */
1728 public static boolean mapWinToObj4(final float winx, final float winy, final float winz, final float clipw,
1729 final Matrix4fb mMv, final Matrix4fb mP,
1730 final int[] viewport,
1731 final float near, final float far,
1732 final Vec4f objPos,
1733 final Matrix4fb mat4Tmp)
1734 {
1735 // invPMv = Inv(P x Mv)
1736 final Matrix4fb invPMv = mat4Tmp.mul(mP, mMv);
1737 if( !invPMv.invert() ) {
1738 return false;
1739 }
1740
1741 final Vec4f winPos = new Vec4f(winx, winy, winz, clipw);
1742
1743 // Map x and y from window coordinates
1744 winPos.add(-viewport[0], -viewport[1], -near, 0f).mul(1f/viewport[2], 1f/viewport[3], 1f/(far-near), 1f);
1745
1746 // Map to range -1 to 1
1747 winPos.mul(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f);
1748
1749 // objPos = Inv(P x Mv) * winPos
1750 invPMv.mulVec4f(winPos, objPos);
1751
1752 if ( objPos.w() == 0.0f ) {
1753 return false;
1754 }
1755 return true;
1756 }
1757
1758 /**
1759 * Map two window coordinates w/ shared X/Y and distinctive Z
1760 * to a {@link Ray}. The resulting {@link Ray} maybe used for <i>picking</i>
1761 * using a {@link AABBox#getRayIntersection(Ray, float[]) bounding box}.
1762 * <p>
1763 * Notes for picking <i>winz0</i> and <i>winz1</i>:
1764 * <ul>
1765 * <li>see {@link FloatUtil#getZBufferEpsilon(int, float, float)}</li>
1766 * <li>see {@link FloatUtil#getZBufferValue(int, float, float, float)}</li>
1767 * <li>see {@link FloatUtil#getOrthoWinZ(float, float, float)}</li>
1768 * </ul>
1769 * </p>
1770 * @param winx
1771 * @param winy
1772 * @param winz0
1773 * @param winz1
1774 * @param mMv 4x4 modelview matrix
1775 * @param mP 4x4 projection matrix
1776 * @param viewport 4 component viewport vector
1777 * @param ray storage for the resulting {@link Ray}
1778 * @param mat4Tmp1 16 component matrix for temp storage
1779 * @param mat4Tmp2 16 component matrix for temp storage
1780 * @return true if successful, otherwise false (failed to invert matrix, or becomes z is infinity)
1781 */
1782 public static boolean mapWinToRay(final float winx, final float winy, final float winz0, final float winz1,
1783 final Matrix4fb mMv,
1784 final Matrix4fb mP,
1785 final int[] viewport,
1786 final Ray ray,
1787 final Matrix4fb mat4Tmp1, final Matrix4fb mat4Tmp2) {
1788 // invPMv = Inv(P x Mv)
1789 final Matrix4fb invPMv = mat4Tmp1.mul(mP, mMv);
1790 if( !invPMv.invert() ) {
1791 return false;
1792 }
1793
1794 if( mapWinToObj(winx, winy, winz0, winz1, invPMv, viewport,
1795 ray.orig, ray.dir, mat4Tmp2) ) {
1796 ray.dir.sub(ray.orig).normalize();
1797 return true;
1798 } else {
1799 return false;
1800 }
1801 }
1802
1803 //
1804 // String and internals
1805 //
1806
1807 /**
1808 * @param sb optional passed StringBuilder instance to be used
1809 * @param rowPrefix optional prefix for each row
1810 * @param f the format string of one floating point, i.e. "%10.5f", see {@link java.util.Formatter}
1811 * @return matrix string representation
1812 */
1813 public StringBuilder toString(final StringBuilder sb, final String rowPrefix, final String f) {
1814 final float[] tmp = new float[16];
1815 this.get(tmp);
1816 return FloatUtil.matrixToString(sb, rowPrefix, f,tmp, 0, 4, 4, false /* rowMajorOrder */);
1817 }
1818
1819 @Override
1820 public String toString() {
1821 return toString(null, null, "%10.5f").toString();
1822 }
1823
1824 private final float[] m = new float[16];
1825
1826 final Stack stack = new Stack(0, 16*16); // start w/ zero size, growSize is half GL-min size (32)
1827
1828 private static class Stack {
1829 private int position;
1830 private float[] buffer;
1831 private final int growSize;
1832
1833 /**
1834 * @param initialSize initial size
1835 * @param growSize grow size if {@link #position()} is reached, maybe <code>0</code>
1836 * in which case an {@link IndexOutOfBoundsException} is thrown.
1837 */
1838 public Stack(final int initialSize, final int growSize) {
1839 this.position = 0;
1840 this.growSize = growSize;
1841 this.buffer = new float[initialSize];
1842 }
1843
1844 private final void growIfNecessary(final int length) throws IndexOutOfBoundsException {
1845 if( position + length > buffer.length ) {
1846 if( 0 >= growSize ) {
1847 throw new IndexOutOfBoundsException("Out of fixed stack size: "+this);
1848 }
1849 final float[] newBuffer =
1850 new float[buffer.length + growSize];
1851 System.arraycopy(buffer, 0, newBuffer, 0, position);
1852 buffer = newBuffer;
1853 }
1854 }
1855
1856 public final Matrix4fb push(final Matrix4fb src) throws IndexOutOfBoundsException {
1857 growIfNecessary(16);
1858 src.get(buffer, position);
1859 position += 16;
1860 return src;
1861 }
1862
1863 public final Matrix4fb pop(final Matrix4fb dest) throws IndexOutOfBoundsException {
1864 position -= 16;
1865 dest.load(buffer, position);
1866 return dest;
1867 }
1868 }
1869}
Basic Float math utility functions.
Definition: FloatUtil.java:83
static float sin(final float a)
static final float EPSILON
Epsilon for floating point {@value}, as once computed via getMachineEpsilon() on an AMD-64 CPU.
static float cos(final float a)
static float tan(final float a)
static StringBuilder matrixToString(StringBuilder sb, final String rowPrefix, final String f, final FloatBuffer a, final int aOffset, final int rows, final int columns, final boolean rowMajorOrder)
static boolean isZero(final float a, final float epsilon)
Returns true if value is zero, i.e.
static boolean isEqual(final float a, final float b, final float epsilon)
Returns true if both values are equal, i.e.
Horizontal and vertical field of view (FOV) halves, allowing a non-centered projection.
final FovHVHalves toTangents()
Returns this instance in tangent values.
Quaternion implementation supporting Gimbal-Lock free rotations.
Definition: Quaternion.java:45
final float magnitudeSquared()
See magnitude() for special handling of epsilon, which is not applied here.
Definition: Quaternion.java:75
Quaternion setFromMat(final float m00, final float m01, final float m02, final float m10, final float m11, final float m12, final float m20, final float m21, final float m22)
Compute the quaternion from a 3x3 column rotation matrix.
Simple compound denoting a ray.
Definition: Ray.java:49
final Vec3f dir
Normalized direction vector of ray.
Definition: Ray.java:54
final Vec3f orig
Origin of Ray.
Definition: Ray.java:51
3D Vector based upon three float components.
Definition: Vec3f.java:37
Vec3f scale(final float s)
this = this * s, returns this.
Definition: Vec3f.java:218
Vec3f sub(final Vec3f b)
this = this - b, returns this.
Definition: Vec3f.java:268
Vec3f normalize()
Normalize this vector in place.
Definition: Vec3f.java:297
float length()
Return the length of this vector, a.k.a the norm or magnitude
Definition: Vec3f.java:283
Vec3f cross(final Vec3f arg)
Returns this cross arg; creates new vector.
Definition: Vec3f.java:343
Vec3f set(final Vec3f o)
this = o, returns this.
Definition: Vec3f.java:79
4D Vector based upon four float components.
Definition: Vec4f.java:37
Vec4f set(final Vec4f o)
this = o, returns this.
Definition: Vec4f.java:67
Vec4f scale(final float s)
this = this * s, returns this.
Definition: Vec4f.java:214
Vec4f mul(final float val)
Returns this * val; creates new vector.
Definition: Vec4f.java:170
Vec4f add(final float dx, final float dy, final float dz, final float dw)
this = this + { dx, dy, dz, dw }, returns this.
Definition: Vec4f.java:237
void setZ(final float z)
Definition: Vec4f.java:149
static float[] normalizeVec3(final float[] vector)
Normalize a vector in place.
Plane equation := dot(n, x - p) = 0 -> Ax + By + Cz + d == 0.
Definition: Frustum.java:158
final Vec3f n
Normal of the plane.
Definition: Frustum.java:160
Providing frustum planes derived by different inputs (P*MV, ..) used to classify objects.
Definition: Frustum.java:81
static final int NEAR
Index for near plane: {@value}.
Definition: Frustum.java:268
static final int BOTTOM
Index for bottom plane: {@value}.
Definition: Frustum.java:264
final Plane[] getPlanes()
Planes are ordered in the returned array as follows:
Definition: Frustum.java:289
static final int RIGHT
Index for right plane: {@value}.
Definition: Frustum.java:262
static final int TOP
Index for top plane: {@value}.
Definition: Frustum.java:266
static final int LEFT
Index for left plane: {@value}.
Definition: Frustum.java:260
static final int FAR
Index for far plane: {@value}.
Definition: Frustum.java:270
Basic 4x4 float matrix implementation using fields for intensive use-cases (host operations).
Definition: Matrix4fb.java:97
final Matrix4fb setToTranslation(final Vec3f t)
Set this matrix to translation.
Definition: Matrix4fb.java:804
Matrix4fb load(final float[] src, final int src_off)
Load the values of the given matrix src to this matrix.
Definition: Matrix4fb.java:180
final Matrix4fb translate(final Vec3f t, final Matrix4fb tmp)
Translate this matrix, i.e.
final Matrix4fb transpose(final Matrix4fb src)
Transpose the given src matrix into this matrix.
Definition: Matrix4fb.java:358
Matrix4fb()
Creates a new identity matrix.
Definition: Matrix4fb.java:102
boolean invert()
Invert this matrix.
Definition: Matrix4fb.java:388
Matrix4fb setToRotationEuler(final float bankX, final float headingY, final float attitudeZ)
Set this matrix to rotation from the given Euler rotation angles in radians.
Definition: Matrix4fb.java:929
float determinant()
Returns the determinant of this matrix.
Definition: Matrix4fb.java:308
static boolean mapObjToWin(final Vec3f obj, final Matrix4fb mMv, final Matrix4fb mP, final int[] viewport, final float[] winPos)
Map object coordinates to window coordinates.
final float[] mulVec3f(final float[] v_in, final float[] v_out)
Affine 3f-vector transformation by 4x4 matrix.
Definition: Matrix4fb.java:733
final Quaternion getRotation(final Quaternion res)
Returns the rotation [m[0+0*4] .
float get(final int i)
Gets the ith component, 0 <= i < 16.
Definition: Matrix4fb.java:204
boolean isEqual(final Matrix4fb o, final float epsilon)
Equals check using a given FloatUtil#EPSILON value and FloatUtil#isEqual(float, float,...
final Vec4f mulVec4f(final Vec4f v_in, final Vec4f v_out)
Definition: Matrix4fb.java:712
boolean invert(final Matrix4fb src)
Invert the src matrix values into this matrix.
Definition: Matrix4fb.java:476
final void push()
Push the matrix to it's stack, while preserving this matrix values.
void updateFrustumPlanes(final Frustum frustum)
Calculate the frustum planes in world coordinates using the passed float[16] as premultiplied P*MV (c...
Matrix4fb load(final Matrix4fb src)
Load the values of the given matrix b to this matrix.
Definition: Matrix4fb.java:159
final Matrix4fb setToRotationAxis(final float ang_rad, float x, float y, float z)
Set this matrix to rotation from the given axis and angle in radians.
Definition: Matrix4fb.java:850
static boolean mapWinToObj(final float winx, final float winy, final float winz, final Matrix4fb invPMv, final int[] viewport, final Vec3f objPos, final Matrix4fb mat4Tmp)
Map window coordinates to object coordinates.
Matrix4fb setToOrtho(final float left, final float right, final float bottom, final float top, final float zNear, final float zFar)
Set this matrix to orthogonal projection.
StringBuilder toString(final StringBuilder sb, final String rowPrefix, final String f)
final Matrix4fb rotate(final float ang_rad, final float x, final float y, final float z, final Matrix4fb tmp)
Rotate this matrix about give axis and angle in radians, i.e.
Matrix4fb(final float[] m, final int m_off)
Creates a new matrix based on given float[4*4] column major order.
Definition: Matrix4fb.java:126
static boolean mapObjToWin(final Vec3f obj, final Matrix4fb mPMv, final int[] viewport, final float[] winPos)
Map object coordinates to window coordinates.
final float[] mulVec4f(final float[] v_in, final float[] v_out)
Definition: Matrix4fb.java:697
Vec4f getRow(final int row, final Vec4f v_out)
Get the named row of the given column-major matrix to v_out.
Definition: Matrix4fb.java:241
Vec3f getColumn(final int column, final Vec3f v_out)
Get the named column of the given column-major matrix to v_out.
Definition: Matrix4fb.java:228
Vec4f getColumn(final int column, final Vec4f v_out)
Get the named column of the given column-major matrix to v_out.
Definition: Matrix4fb.java:214
final Matrix4fb mul(final Matrix4fb b)
Multiply matrix: [this] = [this] x [b].
Definition: Matrix4fb.java:569
final Matrix4fb rotate(final float ang_rad, final Vec3f axis, final Matrix4fb tmp)
Rotate this matrix about give axis and angle in radians, i.e.
final Matrix4fb transpose()
Transpose this matrix.
Definition: Matrix4fb.java:322
final Matrix4fb setToScale(final float x, final float y, final float z)
Set this matrix to scale.
Definition: Matrix4fb.java:822
Matrix4fb setToPerspective(final FovHVHalves fovhv, final float zNear, final float zFar)
Set this matrix to perspective frustum projection.
final Matrix4fb rotate(final Quaternion quat, final Matrix4fb tmp)
Rotate this matrix with the given Quaternion, i.e.
final Matrix4fb setToTranslation(final float x, final float y, final float z)
Set this matrix to translation.
Definition: Matrix4fb.java:780
static boolean mapWinToRay(final float winx, final float winy, final float winz0, final float winz1, final Matrix4fb mMv, final Matrix4fb mP, final int[] viewport, final Ray ray, final Matrix4fb mat4Tmp1, final Matrix4fb mat4Tmp2)
Map two window coordinates w/ shared X/Y and distinctive Z to a Ray.
final void pop()
Pop the current matrix from it's stack, replacing this matrix values.
static boolean mapWinToObj4(final float winx, final float winy, final float winz, final float clipw, final Matrix4fb mMv, final Matrix4fb mP, final int[] viewport, final float near, final float far, final Vec4f objPos, final Matrix4fb mat4Tmp)
Map window coordinates to object coordinates.
final Matrix4fb setToRotation(final Quaternion q)
Set this matrix to rotation using the given Quaternion.
Definition: Matrix4fb.java:977
final Matrix4fb setToRotationAxis(final float ang_rad, final Vec3f axis)
Set this matrix to rotation from the given axis and angle in radians.
Definition: Matrix4fb.java:902
Matrix4fb(final float[] m)
Creates a new matrix based on given float[4*4] column major order.
Definition: Matrix4fb.java:117
final Matrix4fb translate(final float x, final float y, final float z, final Matrix4fb tmp)
Translate this matrix, i.e.
final Matrix4fb scale(final float s, final Matrix4fb tmp)
Scale this matrix, i.e.
Matrix4fb setToLookAt(final Vec3f eye, final Vec3f center, final Vec3f up, final Matrix4fb tmp)
Make given matrix the look-at matrix based on given parameters.
static boolean mapWinToObj(final float winx, final float winy, final float winz, final Matrix4fb mMv, final Matrix4fb mP, final int[] viewport, final Vec3f objPos, final Matrix4fb mat4Tmp)
Map window coordinates to object coordinates.
Matrix4fb load(final FloatBuffer src)
Load the values of the given matrix src to this matrix.
Definition: Matrix4fb.java:194
Matrix4fb setToFrustum(final float left, final float right, final float bottom, final float top, final float zNear, final float zFar)
Set this matrix to frustum.
final Matrix4fb mul(final Matrix4fb a, final Matrix4fb b)
Multiply matrix: [this] = [a] x [b].
Definition: Matrix4fb.java:636
boolean isEqual(final Matrix4fb o)
Equals check using FloatUtil#EPSILON value and FloatUtil#isEqual(float, float, float).
Matrix4fb(final Matrix4fb src)
Creates a new matrix copying the values of the given src matrix.
Definition: Matrix4fb.java:109
Matrix4fb setToPerspective(final float fovy_rad, final float aspect, final float zNear, final float zFar)
Set this matrix to perspective frustum projection.
static boolean mapWinToObj(final float winx, final float winy, final float winz1, final float winz2, final Matrix4fb invPMv, final int[] viewport, final Vec3f objPos1, final Vec3f objPos2, final Matrix4fb mat4Tmp)
Map two window coordinates to two object coordinates, distinguished by their z component.
final Matrix4fb loadIdentity()
Set this matrix to identity.
Definition: Matrix4fb.java:145
Vec3f getRow(final int row, final Vec3f v_out)
Get the named row of the given column-major matrix to v_out.
Definition: Matrix4fb.java:255
Matrix4fb load(final float[] src)
Load the values of the given matrix src to this matrix.
Definition: Matrix4fb.java:169
final Matrix4fb scale(final float x, final float y, final float z, final Matrix4fb tmp)
Scale this matrix, i.e.
final Vec3f mulVec3f(final Vec3f v_in, final Vec3f v_out)
Affine 3f-vector transformation by 4x4 matrix.
Definition: Matrix4fb.java:753