From 15e60161787224e85172685f74dc0ac195969b51 Mon Sep 17 00:00:00 2001 From: Sven Gothel Date: Wed, 5 Apr 2023 09:42:28 +0200 Subject: Math: Complete Matrix4f w/ Vec[234]f and adopt it throughout Quaternion, Ray, AABBox, Frustum, Stereo*, ... adding hook to PMVMatrix Motivation was to simplify matrix + vector math usage, ease review and avoid usage bugs. Matrix4f implementation uses dedicated float fields instead of an array. Performance didn't increase much, as JVM >= 11(?) has some optimizations to drop the array bounds check. AMD64 + OpenJDK17 - Matrix4f.mul(a, b) got a roughly ~10% enhancement over FloatUtil.multMatrix(a, b, dest) - Matrix4f.mul(b) roughly ~3% slower than FloatUtil.multMatrix(a, b, dest) - FloatUtil.multMatrix(a, a_off, b, b_off, dest) is considerable slower than all - Matrix4f.invert(..) roughly ~3% slower than FloatUtil.invertMatrix(..) RaspberryPi 4b aarch64 + OpenJDK17 - Matrix4f.mul(a, b) got a roughly ~10% enhancement over FloatUtil.multMatrix(a, b, dest) - Matrix4f.mul(b) roughly ~20% slower than FloatUtil.multMatrix(a, b) - FloatUtil.multMatrix(a, a_off, b, b_off, dest) is considerable slower than all - Matrix4f.invert(..) roughly ~4% slower than FloatUtil.invertMatrix(..) Conclusion - Matrix4f.mul(b) needs to be revised (esp for aarch64) - Matrix4f.invert(..) should also not be slower .. --- make/scripts/make.jogl.all.linux-x86_64.sh | 3 + make/scripts/tests.sh | 13 +- .../com/jogamp/opengl/demos/av/MovieSBSStereo.java | 68 +- .../com/jogamp/opengl/demos/av/StereoDemo01.java | 9 +- .../com/jogamp/opengl/demos/es2/GearsES2.java | 85 +- .../jogamp/opengl/demos/graph/GPUTextNewtDemo.java | 2 +- .../demos/graph/GPUTextRendererListenerBase01.java | 3 +- src/jogl/classes/com/jogamp/graph/font/Font.java | 6 +- .../jogamp/graph/geom/plane/AffineTransform.java | 52 +- .../classes/com/jogamp/opengl/math/FloatUtil.java | 286 ++- .../classes/com/jogamp/opengl/math/Matrix4.java | 172 -- .../classes/com/jogamp/opengl/math/Matrix4f.java | 1878 ++++++++++++++++++++ .../classes/com/jogamp/opengl/math/Quaternion.java | 358 ++-- src/jogl/classes/com/jogamp/opengl/math/Ray.java | 10 +- src/jogl/classes/com/jogamp/opengl/math/Vec2f.java | 81 +- src/jogl/classes/com/jogamp/opengl/math/Vec3f.java | 112 +- src/jogl/classes/com/jogamp/opengl/math/Vec4f.java | 348 ++++ .../classes/com/jogamp/opengl/math/VectorUtil.java | 12 +- .../com/jogamp/opengl/math/geom/AABBox.java | 433 +++-- .../com/jogamp/opengl/math/geom/Frustum.java | 121 +- .../classes/com/jogamp/opengl/util/PMVMatrix.java | 128 +- .../jogamp/opengl/util/stereo/EyeParameter.java | 17 +- .../util/stereo/LocationSensorParameter.java | 9 +- .../jogamp/opengl/util/stereo/StereoDevice.java | 15 +- .../com/jogamp/opengl/util/stereo/StereoUtil.java | 44 +- .../com/jogamp/opengl/util/stereo/ViewerPose.java | 20 +- .../stereo/generic/GenericStereoDeviceFactory.java | 7 +- .../jogamp/graph/font/typecast/TypecastFont.java | 4 +- .../jogamp/graph/font/typecast/TypecastGlyph.java | 8 +- .../graph/font/typecast/TypecastHMetrics.java | 4 +- src/jogl/classes/jogamp/opengl/ProjectFloat.java | 6 +- .../opengl/util/stereo/GenericStereoDevice.java | 11 +- .../util/stereo/GenericStereoDeviceRenderer.java | 5 +- .../jogamp/opengl/oculusvr/OVRStereoDevice.java | 7 +- .../opengl/oculusvr/OVRStereoDeviceRenderer.java | 5 +- .../junit/graph/GPUTextRendererListenerBase01.java | 2 +- .../opengl/test/junit/graph/TestFontsNEWT00.java | 2 +- .../opengl/test/junit/jogl/demos/es2/GearsES2.java | 89 +- .../TriangleInstancedRendererWithShaderState.java | 83 +- .../gl4/TrianglesInstancedRendererHardcoded.java | 107 +- .../opengl/test/junit/jogl/math/Matrix4fb.java | 1784 +++++++++++++++++++ .../jogl/math/TestFloatUtilProject01NOUI.java | 32 +- .../jogl/math/TestFloatUtilProject02NOUI.java | 4 +- .../test/junit/jogl/math/TestMatrix4f01NOUI.java | 177 ++ .../junit/jogl/math/TestMatrix4f02MulNOUI.java | 275 +++ .../jogl/math/TestMatrix4f03InversionNOUI.java | 403 +++++ .../test/junit/jogl/math/TestPMVMatrix03NOUI.java | 8 +- .../test/junit/jogl/math/TestQuaternion01NOUI.java | 660 ++++--- 48 files changed, 6592 insertions(+), 1376 deletions(-) delete mode 100644 src/jogl/classes/com/jogamp/opengl/math/Matrix4.java create mode 100644 src/jogl/classes/com/jogamp/opengl/math/Matrix4f.java create mode 100644 src/jogl/classes/com/jogamp/opengl/math/Vec4f.java create mode 100644 src/test/com/jogamp/opengl/test/junit/jogl/math/Matrix4fb.java create mode 100644 src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f01NOUI.java create mode 100644 src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f02MulNOUI.java create mode 100644 src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f03InversionNOUI.java diff --git a/make/scripts/make.jogl.all.linux-x86_64.sh b/make/scripts/make.jogl.all.linux-x86_64.sh index f43112d25..9e91bb171 100755 --- a/make/scripts/make.jogl.all.linux-x86_64.sh +++ b/make/scripts/make.jogl.all.linux-x86_64.sh @@ -31,6 +31,9 @@ fi # # -Dsetup.addNativeOpenMAX=true \ # -Dsetup.addNativeKD=true \ +# +# -Doculusvr.enabled=true \ +# #LD_LIBRARY_PATH=/opt-linux-x86_64/mesa-7.8.1/lib64 diff --git a/make/scripts/tests.sh b/make/scripts/tests.sh index 9724c7632..2038ceee2 100644 --- a/make/scripts/tests.sh +++ b/make/scripts/tests.sh @@ -495,7 +495,7 @@ function testawtswt() { # # Stereo # -#testnoawt com.jogamp.opengl.test.junit.jogl.stereo.StereoDemo01 $* +#testnoawt com.jogamp.opengl.demos.av.StereoDemo01 $* # # # HiDPI @@ -538,6 +538,7 @@ function testawtswt() { #testnoawt com.jogamp.opengl.test.junit.jogl.demos.gl2.newt.TestTeapotNEWT $* #testnoawt com.jogamp.opengl.test.junit.jogl.demos.gl3.newt.TestGeomShader01TextureGL3NEWT $* #testnoawt com.jogamp.opengl.test.junit.jogl.demos.gl4.newt.TestTessellationShader01GL4NEWT $* +#testnoawt com.jogamp.opengl.test.junit.jogl.demos.gl4.newt.TestInstancedReneringGL4NEWT $* # # av demos @@ -546,6 +547,7 @@ function testawtswt() { #testnoawt com.jogamp.opengl.demos.av.MovieCube $* #testnoawt com.jogamp.opengl.demos.av.MovieSimple $* #testnoawt com.jogamp.opengl.demos.av.CrossFadePlayer $* +#testnoawt com.jogamp.opengl.demos.av.StereoDemo01 $* # # performance tests @@ -574,6 +576,9 @@ function testawtswt() { #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestFloatUtil01NOUI $* #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestFloatUtil02MatrixMatrixMultNOUI $* #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestFloatUtil03InversionNOUI $* +#testnoawt com.jogamp.opengl.test.junit.jogl.math.TestMatrix4f01NOUI $* +testnoawt com.jogamp.opengl.test.junit.jogl.math.TestMatrix4f02MulNOUI $* +#testnoawt com.jogamp.opengl.test.junit.jogl.math.TestMatrix4f03InversionNOUI $* #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestPMVMatrix01NEWT $* #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestPMVMatrix02NOUI $* #testnoawt com.jogamp.opengl.test.junit.jogl.math.TestPMVMatrix03NOUI $* @@ -972,7 +977,9 @@ function testawtswt() { #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo00 $* #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo01 $* #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo02 $* +#testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo03 $* #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo10 $* +#testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo11 $* #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo20 $* #testnoawt com.jogamp.opengl.demos.av.MovieCube $* @@ -1014,8 +1021,10 @@ function testawtswt() { #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo01 $* #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo01b $* #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo02 $* -testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo03 $* +#testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo03 $* +#testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo03b $* #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo10 $* +#testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo11 $* #testnoawt com.jogamp.opengl.demos.graph.ui.UISceneDemo20 $* #testnoawt com.jogamp.opengl.demos.av.MovieCube $* diff --git a/src/demos/com/jogamp/opengl/demos/av/MovieSBSStereo.java b/src/demos/com/jogamp/opengl/demos/av/MovieSBSStereo.java index 3a59216d5..3a0955a05 100644 --- a/src/demos/com/jogamp/opengl/demos/av/MovieSBSStereo.java +++ b/src/demos/com/jogamp/opengl/demos/av/MovieSBSStereo.java @@ -61,9 +61,9 @@ import com.jogamp.opengl.GLExtensions; import com.jogamp.opengl.GLProfile; import com.jogamp.opengl.JoglVersion; import com.jogamp.opengl.demos.graph.TextRendererGLELBase; -import com.jogamp.opengl.math.FloatUtil; +import com.jogamp.opengl.math.Matrix4f; import com.jogamp.opengl.math.Quaternion; -import com.jogamp.opengl.math.VectorUtil; +import com.jogamp.opengl.math.Vec3f; import com.jogamp.opengl.util.CustomGLEventListener; import com.jogamp.opengl.util.GLArrayDataServer; import com.jogamp.opengl.util.PMVMatrix; @@ -641,15 +641,15 @@ public class MovieSBSStereo implements StereoGLEventListener { pmvMatrix.glTranslatef(0, 0, zoom0); } - private final float[] mat4Tmp1 = new float[16]; - private final float[] mat4Tmp2 = new float[16]; - private final float[] vec3Tmp1 = new float[3]; - private final float[] vec3Tmp2 = new float[3]; - private final float[] vec3Tmp3 = new float[3]; + private final Matrix4f mat4Tmp1 = new Matrix4f(); + private final Matrix4f mat4Tmp2 = new Matrix4f(); + private final Vec3f vec3Tmp1 = new Vec3f(); + private final Vec3f vec3Tmp2 = new Vec3f(); + private final Vec3f vec3Tmp3 = new Vec3f(); GLArrayDataServer interleavedVBOCurrent = null; - private static final float[] vec3ScalePos = new float[] { 4f, 4f, 4f }; + private static final float scalePos = 4f; @Override public void reshapeForEye(final GLAutoDrawable drawable, final int x, final int y, final int width, final int height, @@ -663,26 +663,38 @@ public class MovieSBSStereo implements StereoGLEventListener { if(null == mPlayer) { return; } if(null == st) { return; } - pmvMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION); - final float[] mat4Projection = FloatUtil.makePerspective(mat4Tmp1, 0, true, eyeParam.fovhv, zNear, zFar); - pmvMatrix.glLoadMatrixf(mat4Projection, 0); - - pmvMatrix.glMatrixMode(GLMatrixFunc.GL_MODELVIEW); - final Quaternion rollPitchYaw = new Quaternion(); - final float[] shiftedEyePos = rollPitchYaw.rotateVector(vec3Tmp1, 0, viewerPose.position, 0); - VectorUtil.scaleVec3(shiftedEyePos, shiftedEyePos, vec3ScalePos); // amplify viewerPose position - VectorUtil.addVec3(shiftedEyePos, shiftedEyePos, eyeParam.positionOffset); - - rollPitchYaw.mult(viewerPose.orientation); - final float[] up = rollPitchYaw.rotateVector(vec3Tmp2, 0, VectorUtil.VEC3_UNIT_Y, 0); - final float[] forward = rollPitchYaw.rotateVector(vec3Tmp3, 0, VectorUtil.VEC3_UNIT_Z_NEG, 0); - final float[] center = VectorUtil.addVec3(forward, shiftedEyePos, forward); - - final float[] mLookAt = FloatUtil.makeLookAt(mat4Tmp1, 0, shiftedEyePos, 0, center, 0, up, 0, mat4Tmp2); - final float[] mViewAdjust = FloatUtil.makeTranslation(mat4Tmp2, true, eyeParam.distNoseToPupilX, eyeParam.distMiddleToPupilY, eyeParam.eyeReliefZ); - final float[] mat4Modelview = FloatUtil.multMatrix(mViewAdjust, mLookAt); - pmvMatrix.glLoadMatrixf(mat4Modelview, 0); - pmvMatrix.glTranslatef(0, 0, zoom0); + { + // + // Projection + // + final Matrix4f mat4 = new Matrix4f(); + pmvMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION); + mat4.setToPerspective(eyeParam.fovhv, zNear, zFar); + pmvMatrix.glLoadMatrixf(mat4); + + // + // Modelview + // + pmvMatrix.glMatrixMode(GLMatrixFunc.GL_MODELVIEW); + final Quaternion rollPitchYaw = new Quaternion(); + // private final float eyeYaw = FloatUtil.PI; // 180 degrees in radians + // rollPitchYaw.rotateByAngleY(eyeYaw); + final Vec3f shiftedEyePos = rollPitchYaw.rotateVector(viewerPose.position, vec3Tmp1); + shiftedEyePos.scale(scalePos); // amplify viewerPose position + shiftedEyePos.add(eyeParam.positionOffset); + + rollPitchYaw.mult(viewerPose.orientation); + final Vec3f up = rollPitchYaw.rotateVector(Vec3f.UNIT_Y, vec3Tmp2); + final Vec3f forward = rollPitchYaw.rotateVector(Vec3f.UNIT_Z_NEG, vec3Tmp3); // -> center + final Vec3f center = forward.add(shiftedEyePos); + + final Matrix4f mLookAt = mat4Tmp2.setToLookAt(shiftedEyePos, center, up, mat4Tmp1); + mat4.mul( mat4Tmp1.setToTranslation( eyeParam.distNoseToPupilX, + eyeParam.distMiddleToPupilY, + eyeParam.eyeReliefZ ), mLookAt); + mat4.translate(0, 0, zoom0, mat4Tmp1); + pmvMatrix.glLoadMatrixf(mat4); + } st.useProgram(gl, true); st.uniform(gl, pmvMatrixUniform); st.useProgram(gl, false); diff --git a/src/demos/com/jogamp/opengl/demos/av/StereoDemo01.java b/src/demos/com/jogamp/opengl/demos/av/StereoDemo01.java index 88a49a09a..139c9f140 100644 --- a/src/demos/com/jogamp/opengl/demos/av/StereoDemo01.java +++ b/src/demos/com/jogamp/opengl/demos/av/StereoDemo01.java @@ -52,6 +52,7 @@ import com.jogamp.newt.event.KeyEvent; import com.jogamp.newt.opengl.GLWindow; import com.jogamp.newt.opengl.util.stereo.StereoDeviceUtil; import com.jogamp.opengl.math.FovHVHalves; +import com.jogamp.opengl.math.Vec3f; import com.jogamp.opengl.util.Animator; import com.jogamp.opengl.util.AnimatorBase; import com.jogamp.opengl.util.av.GLMediaPlayer; @@ -289,9 +290,9 @@ public class StereoDemo01 { } final boolean usesLenses = 0 != ( StereoDeviceRenderer.DISTORTION_BARREL & stereoDevice.getMinimumDistortionBits() ); - final float[] eyePositionOffset = null != movieSimple && usesLenses ? new float[] { 0f, 0.3f, 0f } // better fixed movie position w/ lenses - : stereoDevice.getDefaultEyePositionOffset(); // default - System.err.println("Eye Position Offset: "+Arrays.toString(eyePositionOffset)); + final Vec3f eyePositionOffset = null != movieSimple && usesLenses ? new Vec3f( 0f, 0.3f, 0f ) // better fixed movie position w/ lenses + : stereoDevice.getDefaultEyePositionOffset(); // default + System.err.println("Eye Position Offset: "+eyePositionOffset); final int textureUnit = 0; final int reqDistortionBits; @@ -382,7 +383,7 @@ public class StereoDemo01 { System.err.println("Window.1.surfaceSize: "+window.getSurfaceWidth()+" x "+window.getSurfaceHeight()); if( useAnimator ) { - animator.setUpdateFPSFrames(60, System.err); + animator.setUpdateFPSFrames(60*10, System.err); } final long t0 = System.currentTimeMillis(); diff --git a/src/demos/com/jogamp/opengl/demos/es2/GearsES2.java b/src/demos/com/jogamp/opengl/demos/es2/GearsES2.java index 02c451e9f..5ad6ede76 100644 --- a/src/demos/com/jogamp/opengl/demos/es2/GearsES2.java +++ b/src/demos/com/jogamp/opengl/demos/es2/GearsES2.java @@ -33,9 +33,9 @@ import com.jogamp.newt.event.GestureHandler.GestureEvent; import com.jogamp.opengl.GLRendererQuirks; import com.jogamp.opengl.JoglVersion; import com.jogamp.opengl.demos.GearsObject; -import com.jogamp.opengl.math.FloatUtil; +import com.jogamp.opengl.math.Matrix4f; import com.jogamp.opengl.math.Quaternion; -import com.jogamp.opengl.math.VectorUtil; +import com.jogamp.opengl.math.Vec3f; import com.jogamp.opengl.util.CustomGLEventListener; import com.jogamp.opengl.util.PMVMatrix; import com.jogamp.opengl.util.TileRendererBase; @@ -397,49 +397,58 @@ public class GearsES2 implements StereoGLEventListener, TileRendererBase.TileRen } // private boolean useAndroidDebug = false; - private final float[] mat4Tmp1 = new float[16]; - private final float[] mat4Tmp2 = new float[16]; - private final float[] vec3Tmp1 = new float[3]; - private final float[] vec3Tmp2 = new float[3]; - private final float[] vec3Tmp3 = new float[3]; + private final Matrix4f mat4Tmp1 = new Matrix4f(); + private final Matrix4f mat4Tmp2 = new Matrix4f(); + private final Vec3f vec3Tmp1 = new Vec3f(); + private final Vec3f vec3Tmp2 = new Vec3f(); + private final Vec3f vec3Tmp3 = new Vec3f(); - private static final float[] vec3ScalePos = new float[] { 20f, 20f, 20f }; + private static final float scalePos = 20f; @Override public void reshapeForEye(final GLAutoDrawable drawable, final int x, final int y, final int width, final int height, final EyeParameter eyeParam, final ViewerPose viewerPose) { final GL2ES2 gl = drawable.getGL().getGL2ES2(); - pmvMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION); - final float[] mat4Projection = FloatUtil.makePerspective(mat4Tmp1, 0, true, eyeParam.fovhv, zNear, zFar); - if( flipVerticalInGLOrientation && gl.getContext().getGLDrawable().isGLOriented() ) { - pmvMatrix.glLoadIdentity(); - pmvMatrix.glScalef(1f, -1f, 1f); - pmvMatrix.glMultMatrixf(mat4Projection, 0); - } else { - pmvMatrix.glLoadMatrixf(mat4Projection, 0); - } - - pmvMatrix.glMatrixMode(GLMatrixFunc.GL_MODELVIEW); - final Quaternion rollPitchYaw = new Quaternion(); - // private final float eyeYaw = FloatUtil.PI; // 180 degrees in radians - // rollPitchYaw.rotateByAngleY(eyeYaw); - // final float[] shiftedEyePos = rollPitchYaw.rotateVector(vec3Tmp1, 0, viewerPose.position, 0); - final float[] shiftedEyePos = VectorUtil.copyVec3(vec3Tmp1, 0, viewerPose.position, 0); - VectorUtil.scaleVec3(shiftedEyePos, shiftedEyePos, vec3ScalePos); // amplify viewerPose position - VectorUtil.addVec3(shiftedEyePos, shiftedEyePos, eyeParam.positionOffset); - - rollPitchYaw.mult(viewerPose.orientation); - final float[] up = rollPitchYaw.rotateVector(vec3Tmp2, 0, VectorUtil.VEC3_UNIT_Y, 0); - final float[] forward = rollPitchYaw.rotateVector(vec3Tmp3, 0, VectorUtil.VEC3_UNIT_Z_NEG, 0); - final float[] center = VectorUtil.addVec3(forward, shiftedEyePos, forward); - - final float[] mLookAt = FloatUtil.makeLookAt(mat4Tmp1, 0, shiftedEyePos, 0, center, 0, up, 0, mat4Tmp2); - final float[] mViewAdjust = FloatUtil.makeTranslation(mat4Tmp2, true, eyeParam.distNoseToPupilX, eyeParam.distMiddleToPupilY, eyeParam.eyeReliefZ); - final float[] mat4Modelview = FloatUtil.multMatrix(mViewAdjust, mLookAt); - - pmvMatrix.glLoadMatrixf(mat4Modelview, 0); - pmvMatrix.glTranslatef(0.0f, 0.0f, -zViewDist); + { + // + // Projection + // + final Matrix4f mat4 = new Matrix4f(); + pmvMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION); + if( flipVerticalInGLOrientation && gl.getContext().getGLDrawable().isGLOriented() ) { + mat4Tmp1.setToScale(1f, -1f, 1f); + mat4Tmp2.setToPerspective(eyeParam.fovhv, zNear, zFar); + mat4.mul(mat4Tmp1, mat4Tmp2); + } else { + mat4.setToPerspective(eyeParam.fovhv, zNear, zFar); + } + pmvMatrix.glLoadMatrixf(mat4); + + // + // Modelview + // + pmvMatrix.glMatrixMode(GLMatrixFunc.GL_MODELVIEW); + final Quaternion rollPitchYaw = new Quaternion(); + // private final float eyeYaw = FloatUtil.PI; // 180 degrees in radians + // rollPitchYaw.rotateByAngleY(eyeYaw); + // final Vec3f shiftedEyePos = rollPitchYaw.rotateVector(viewerPose.position, vec3Tmp1); + final Vec3f shiftedEyePos = vec3Tmp1.set(viewerPose.position); + shiftedEyePos.scale(scalePos); // amplify viewerPose position + shiftedEyePos.add(eyeParam.positionOffset); + + rollPitchYaw.mult(viewerPose.orientation); + final Vec3f up = rollPitchYaw.rotateVector(Vec3f.UNIT_Y, vec3Tmp2); + final Vec3f forward = rollPitchYaw.rotateVector(Vec3f.UNIT_Z_NEG, vec3Tmp3); // -> center + final Vec3f center = forward.add(shiftedEyePos); + + final Matrix4f mLookAt = mat4Tmp2.setToLookAt(shiftedEyePos, center, up, mat4Tmp1); + mat4.mul( mat4Tmp1.setToTranslation( eyeParam.distNoseToPupilX, + eyeParam.distMiddleToPupilY, + eyeParam.eyeReliefZ ), mLookAt); + mat4.translate(0, 0, -zViewDist, mat4Tmp1); + pmvMatrix.glLoadMatrixf(mat4); + } st.useProgram(gl, true); st.uniform(gl, pmvMatrixUniform); st.useProgram(gl, false); diff --git a/src/demos/com/jogamp/opengl/demos/graph/GPUTextNewtDemo.java b/src/demos/com/jogamp/opengl/demos/graph/GPUTextNewtDemo.java index ad5434b09..5952326c8 100644 --- a/src/demos/com/jogamp/opengl/demos/graph/GPUTextNewtDemo.java +++ b/src/demos/com/jogamp/opengl/demos/graph/GPUTextNewtDemo.java @@ -155,7 +155,7 @@ public class GPUTextNewtDemo { final AABBox fontNameBox = font2.getMetricBounds(GPUTextRendererListenerBase01.textX1); System.err.println("GPU Text Newt Demo: "+font2.fullString()); System.err.println("GPU Text Newt Demo: screen-dpi: "+sDPI[0]+"x"+sDPI[1]+", font "+font_ptpi+" pt, "+font_ppi+" pixel"); - System.err.println("GPU Text Newt Demo: textX2: "+fontNameBox+" em, "+fontNameBox.scale(font_ppi, new float[3])+" px"); + System.err.println("GPU Text Newt Demo: textX2: "+fontNameBox+" em, "+fontNameBox.scale(font_ppi)+" px"); final MonitorDevice monitor = window.getMainMonitor(); System.err.println("GPU Text Newt Demo: "+monitor); // window.setSurfaceSize((int)(fontNameBox.getWidth()*1.1f), (int)(fontNameBox.getHeight()*2f)); diff --git a/src/demos/com/jogamp/opengl/demos/graph/GPUTextRendererListenerBase01.java b/src/demos/com/jogamp/opengl/demos/graph/GPUTextRendererListenerBase01.java index dadbcd373..cab25b83d 100644 --- a/src/demos/com/jogamp/opengl/demos/graph/GPUTextRendererListenerBase01.java +++ b/src/demos/com/jogamp/opengl/demos/graph/GPUTextRendererListenerBase01.java @@ -51,6 +51,7 @@ import com.jogamp.newt.Window; import com.jogamp.newt.event.KeyEvent; import com.jogamp.newt.event.KeyListener; import com.jogamp.newt.opengl.GLWindow; +import com.jogamp.opengl.math.Vec3f; import com.jogamp.opengl.math.geom.AABBox; import com.jogamp.opengl.util.PMVMatrix; @@ -491,7 +492,7 @@ public abstract class GPUTextRendererListenerBase01 extends GPURendererListenerB System.err.println("Matrix: " + getXTran() + "/" + getYTran() + " x"+getZTran() + " @"+getAngle() +" fontSize "+fontSizeCenter); if(bbox) { System.err.println("bbox em: "+font.getMetricBounds(text2)); - System.err.println("bbox px: "+font.getMetricBounds(text2).scale(nearPlaneS * FontScale.toPixels(fontSizeCenter, dpiV), new float[3])); + System.err.println("bbox px: "+font.getMetricBounds(text2).scale( nearPlaneS * FontScale.toPixels(fontSizeCenter, dpiV) ) ); } } diff --git a/src/jogl/classes/com/jogamp/graph/font/Font.java b/src/jogl/classes/com/jogamp/graph/font/Font.java index 4399bbad7..3d7e9f6a1 100644 --- a/src/jogl/classes/com/jogamp/graph/font/Font.java +++ b/src/jogl/classes/com/jogamp/graph/font/Font.java @@ -29,6 +29,7 @@ package com.jogamp.graph.font; import com.jogamp.graph.curve.OutlineShape; import com.jogamp.graph.geom.plane.AffineTransform; +import com.jogamp.opengl.math.Vec3f; import com.jogamp.opengl.math.geom.AABBox; /** @@ -151,7 +152,7 @@ public interface Font { * @param dest AABBox instance set to this metrics boundary in font em-size [0..1] * @return the given and set AABBox 'dest' in font units */ - AABBox getBounds(final AABBox dest, final float[] tmpV3); + AABBox getBounds(final AABBox dest); } /** @@ -193,10 +194,9 @@ public interface Font { /** * Return the AABBox in font em-size [0..1], copying into given dest. * @param dest AABBox instance set to this metrics boundary in font em-size [0..1] - * @param tmpV3 caller provided temporary 3-component vector * @return the given and set AABBox 'dest' in font em-size [0..1] */ - AABBox getBounds(final AABBox dest, float[] tmpV3); + AABBox getBounds(final AABBox dest); /** * Return the AABBox in font em-size [0..1], creating a new copy. diff --git a/src/jogl/classes/com/jogamp/graph/geom/plane/AffineTransform.java b/src/jogl/classes/com/jogamp/graph/geom/plane/AffineTransform.java index 62cda0322..74036f97d 100644 --- a/src/jogl/classes/com/jogamp/graph/geom/plane/AffineTransform.java +++ b/src/jogl/classes/com/jogamp/graph/geom/plane/AffineTransform.java @@ -16,6 +16,7 @@ */ /** * @author Denis M. Kishenko + * @author Sven Gothel, (c) 2010-2023 */ package com.jogamp.graph.geom.plane; @@ -23,6 +24,8 @@ package com.jogamp.graph.geom.plane; import com.jogamp.graph.geom.Vertex; import com.jogamp.opengl.math.FloatUtil; +import com.jogamp.opengl.math.Vec2f; +import com.jogamp.opengl.math.Vec3f; import com.jogamp.opengl.math.geom.AABBox; public class AffineTransform implements Cloneable { @@ -53,17 +56,17 @@ public class AffineTransform implements Cloneable { /** * The values of transformation matrix */ - float m00; - float m10; - float m01; - float m11; - float m02; - float m12; + private float m00; + private float m10; + private float m01; + private float m11; + private float m02; + private float m12; /** * The transformation type */ - transient int type; + private transient int type; public AffineTransform() { setToIdentity(); @@ -404,10 +407,10 @@ public class AffineTransform implements Cloneable { * @return dst for chaining */ public final AABBox transform(final AABBox src, final AABBox dst) { - final float[] srcLo = src.getLow(); - final float[] srcHi = src.getHigh(); - dst.setSize(srcLo[0] * m00 + srcLo[1] * m01 + m02, srcLo[0] * m10 + srcLo[1] * m11 + m12, srcLo[2], - srcHi[0] * m00 + srcHi[1] * m01 + m02, srcHi[0] * m10 + srcHi[1] * m11 + m12, srcHi[2]); + final Vec3f lo = src.getLow(); + final Vec3f hi = src.getHigh(); + dst.setSize(lo.x() * m00 + lo.y() * m01 + m02, lo.x() * m10 + lo.y() * m11 + m12, lo.z(), + hi.x() * m00 + hi.y() * m01 + m02, hi.x() * m10 + hi.y() * m11 + m12, hi.z()); return dst; } @@ -474,6 +477,33 @@ public class AffineTransform implements Cloneable { } } + /** + * @param src source of transformation + * @param dst destination of transformation, maybe be equal to src + * @return dst for chaining + */ + public final Vec2f transform(final Vec2f src, final Vec2f dst) { + final float x = src.x(); + final float y = src.y(); + dst.setX( x * m00 + y * m01 + m02 ); + dst.setY( x * m10 + y * m11 + m12 ); + return dst; + } + + /** + * @param src source of transformation + * @param dst destination of transformation, maybe be equal to src + * @return dst for chaining + */ + public final Vec3f transform(final Vec3f src, final Vec3f dst) { + final float x = src.x(); + final float y = src.y(); + dst.setX( x * m00 + y * m01 + m02 ); + dst.setY( x * m10 + y * m11 + m12 ); + dst.setZ( src.z() ); // just copy z + return dst; + } + /** * * @param src diff --git a/src/jogl/classes/com/jogamp/opengl/math/FloatUtil.java b/src/jogl/classes/com/jogamp/opengl/math/FloatUtil.java index 7aba7fa73..f793629d6 100644 --- a/src/jogl/classes/com/jogamp/opengl/math/FloatUtil.java +++ b/src/jogl/classes/com/jogamp/opengl/math/FloatUtil.java @@ -1,5 +1,5 @@ /** - * Copyright 2010 JogAmp Community. All rights reserved. + * Copyright 2010-2023 JogAmp Community. All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, are * permitted provided that the following conditions are met: @@ -44,20 +44,26 @@ import com.jogamp.opengl.math.geom.Frustum; * Implementation assumes linear matrix layout in column-major order * matching OpenGL's implementation, illustration: * 
-    Row-Major                  Column-Major (OpenGL):
+    Row-Major                       Column-Major (OpenGL):
 
-        |  0   1   2   3 |         |  0   4   8  12 |
-        |                |         |                |
-        |  4   5   6   7 |         |  1   5   9  13 |
-    M = |                |     M = |                |
-        |  8   9  10  11 |         |  2   6  10  14 |
-        |                |         |                |
-        | 12  13  14  15 |         |  3   7  11  15 |
+        |  0   1   2  tx |
+        |                |
+        |  4   5   6  ty |
+    M = |                |
+        |  8   9  10  tz |
+        |                |
+        | 12  13  14  15 |
 
-           C   R                      C   R
+           R   C                      R   C
          m[0*4+3] = tx;             m[0+4*3] = tx;
          m[1*4+3] = ty;             m[1+4*3] = ty;
          m[2*4+3] = tz;             m[2+4*3] = tz;
+
+          RC (std subscript order)   RC (std subscript order)
+         m03 = tx;                  m03 = tx;
+         m13 = ty;                  m13 = ty;
+         m23 = tz;                  m23 = tz;
+
  *
*

*

@@ -71,7 +77,7 @@ import com.jogamp.opengl.math.geom.Frustum; * Implementation utilizes unrolling of small vertices and matrices wherever possible * while trying to access memory in a linear fashion for performance reasons, see: *

*

@@ -579,35 +585,6 @@ public final class FloatUtil { return makeFrustum(m, m_off, initM, left, right, bottom, top, zNear, zFar); } - /** - * Make given matrix the perspective {@link #makeFrustum(float[], int, boolean, float, float, float, float, float, float) frustum} - * matrix based on given parameters. - *

- * All matrix fields are only set if initM is true. - *

- * - * @param m 4x4 matrix in column-major order (also result) - * @param m_offset offset in given array m, i.e. start of the 4x4 matrix - * @param initM if true, given matrix will be initialized w/ identity matrix, - * otherwise only the frustum fields are set. - * @param fovhv {@link FovHVHalves} field of view in both directions, may not be centered, either in radians or tangent - * @param zNear - * @param zFar - * @return given matrix for chaining - * @throws GLException if {@code zNear <= 0} or {@code zFar <= zNear} - * @see #makeFrustum(float[], int, boolean, float, float, float, float, float, float) - * @see Frustum#updateByFovDesc(float[], int, boolean, Frustum.FovDesc) - */ - public static float[] makePerspective(final float[] m, final int m_offset, final boolean initM, - final FovHVHalves fovhv, final float zNear, final float zFar) throws GLException { - final FovHVHalves fovhvTan = fovhv.toTangents(); // use tangent of half-fov ! - final float top = fovhvTan.top * zNear; - final float bottom = -1.0f * fovhvTan.bottom * zNear; - final float left = -1.0f * fovhvTan.left * zNear; - final float right = fovhvTan.right * zNear; - return makeFrustum(m, m_offset, initM, left, right, bottom, top, zNear, zFar); - } - /** * Make given matrix the look-at matrix based on given parameters. *

@@ -1072,12 +1049,12 @@ public final class FloatUtil { * @param vec4Tmp2 4 component vector for temp storage * @return true if successful, otherwise false (z is 1) */ - public static boolean mapObjToWinCoords(final float objx, final float objy, final float objz, - final float[] modelMatrix, final int modelMatrix_offset, - final float[] projMatrix, final int projMatrix_offset, - final int[] viewport, final int viewport_offset, - final float[] win_pos, final int win_pos_offset, - final float[/*4*/] vec4Tmp1, final float[/*4*/] vec4Tmp2) { + public static boolean mapObjToWin(final float objx, final float objy, final float objz, + final float[] modelMatrix, final int modelMatrix_offset, + final float[] projMatrix, final int projMatrix_offset, + final int[] viewport, final int viewport_offset, + final float[] win_pos, final int win_pos_offset, + final float[/*4*/] vec4Tmp1, final float[/*4*/] vec4Tmp2) { vec4Tmp1[0] = objx; vec4Tmp1[1] = objy; vec4Tmp1[2] = objz; @@ -1120,18 +1097,15 @@ public final class FloatUtil { * @param objz * @param mat4PMv [projection] x [modelview] matrix, i.e. P x Mv * @param viewport 4 component viewport vector - * @param viewport_offset * @param win_pos 3 component window coordinate, the result - * @param win_pos_offset * @param vec4Tmp1 4 component vector for temp storage * @param vec4Tmp2 4 component vector for temp storage * @return true if successful, otherwise false (z is 1) */ - public static boolean mapObjToWinCoords(final float objx, final float objy, final float objz, - final float[/*16*/] mat4PMv, - final int[] viewport, final int viewport_offset, - final float[] win_pos, final int win_pos_offset, - final float[/*4*/] vec4Tmp1, final float[/*4*/] vec4Tmp2) { + public static boolean mapObjToWin(final float objx, final float objy, final float objz, + final float[/*16*/] mat4PMv, + final int[] viewport, final float[] win_pos, + final float[/*4*/] vec4Tmp1, final float[/*4*/] vec4Tmp2) { vec4Tmp2[0] = objx; vec4Tmp2[1] = objy; vec4Tmp2[2] = objz; @@ -1152,9 +1126,9 @@ public final class FloatUtil { vec4Tmp1[2] = vec4Tmp1[2] * vec4Tmp1[3] + 0.5f; // Map x,y to viewport - win_pos[0+win_pos_offset] = vec4Tmp1[0] * viewport[2+viewport_offset] + viewport[0+viewport_offset]; - win_pos[1+win_pos_offset] = vec4Tmp1[1] * viewport[3+viewport_offset] + viewport[1+viewport_offset]; - win_pos[2+win_pos_offset] = vec4Tmp1[2]; + win_pos[0] = vec4Tmp1[0] * viewport[2] + viewport[0]; + win_pos[1] = vec4Tmp1[1] * viewport[3] + viewport[1]; + win_pos[2] = vec4Tmp1[2]; return true; } @@ -1180,12 +1154,12 @@ public final class FloatUtil { * @param mat4Tmp2 16 component matrix for temp storage * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z) */ - public static boolean mapWinToObjCoords(final float winx, final float winy, final float winz, - final float[] modelMatrix, final int modelMatrix_offset, - final float[] projMatrix, final int projMatrix_offset, - final int[] viewport, final int viewport_offset, - final float[] obj_pos, final int obj_pos_offset, - final float[/*16*/] mat4Tmp1, final float[/*16*/] mat4Tmp2) { + public static boolean mapWinToObj(final float winx, final float winy, final float winz, + final float[] modelMatrix, final int modelMatrix_offset, + final float[] projMatrix, final int projMatrix_offset, + final int[] viewport, final int viewport_offset, + final float[] obj_pos, final int obj_pos_offset, + final float[/*16*/] mat4Tmp1, final float[/*16*/] mat4Tmp2) { // mat4Tmp1 = P x Mv multMatrix(projMatrix, projMatrix_offset, modelMatrix, modelMatrix_offset, mat4Tmp1, 0); @@ -1242,11 +1216,11 @@ public final class FloatUtil { * @param vec4Tmp2 4 component vector for temp storage * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z) */ - public static boolean mapWinToObjCoords(final float winx, final float winy, final float winz, - final float[/*16*/] mat4PMvI, - final int[] viewport, final int viewport_offset, - final float[] obj_pos, final int obj_pos_offset, - final float[/*4*/] vec4Tmp1, final float[/*4*/] vec4Tmp2) { + public static boolean mapWinToObj(final float winx, final float winy, final float winz, + final float[/*16*/] mat4PMvI, + final int[] viewport, final int viewport_offset, + final float[] obj_pos, final int obj_pos_offset, + final float[/*4*/] vec4Tmp1, final float[/*4*/] vec4Tmp2) { vec4Tmp1[0] = winx; vec4Tmp1[1] = winy; vec4Tmp1[2] = winz; @@ -1296,19 +1270,17 @@ public final class FloatUtil { * @param vec4Tmp2 4 component vector for temp storage * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z) */ - public static boolean mapWinToObjCoords(final float winx, final float winy, final float winz1, final float winz2, - final float[/*16*/] mat4PMvI, - final int[] viewport, final int viewport_offset, - final float[] obj1_pos, final int obj1_pos_offset, - final float[] obj2_pos, final int obj2_pos_offset, - final float[/*4*/] vec4Tmp1, final float[/*4*/] vec4Tmp2) { + public static boolean mapWinToObj(final float winx, final float winy, final float winz1, final float winz2, + final float[/*16*/] mat4PMvI, final int[] viewport, + final Vec3f objPos1, final Vec3f objPos2, + final float[/*4*/] vec4Tmp1, final float[/*4*/] vec4Tmp2) { vec4Tmp1[0] = winx; vec4Tmp1[1] = winy; vec4Tmp1[3] = 1.0f; // Map x and y from window coordinates - vec4Tmp1[0] = (vec4Tmp1[0] - viewport[0+viewport_offset]) / viewport[2+viewport_offset]; - vec4Tmp1[1] = (vec4Tmp1[1] - viewport[1+viewport_offset]) / viewport[3+viewport_offset]; + vec4Tmp1[0] = (vec4Tmp1[0] - viewport[0]) / viewport[2]; + vec4Tmp1[1] = (vec4Tmp1[1] - viewport[1]) / viewport[3]; // Map to range -1 to 1 vec4Tmp1[0] = vec4Tmp1[0] * 2 - 1; @@ -1329,15 +1301,14 @@ public final class FloatUtil { vec4Tmp2[3] = 1.0f / vec4Tmp2[3]; - obj1_pos[0+obj1_pos_offset] = vec4Tmp2[0] * vec4Tmp2[3]; - obj1_pos[1+obj1_pos_offset] = vec4Tmp2[1] * vec4Tmp2[3]; - obj1_pos[2+obj1_pos_offset] = vec4Tmp2[2] * vec4Tmp2[3]; + objPos1.set( vec4Tmp2[0] * vec4Tmp2[3], + vec4Tmp2[1] * vec4Tmp2[3], + vec4Tmp2[2] * vec4Tmp2[3] ); // // winz2 // - vec4Tmp1[2] = winz2; - vec4Tmp1[2] = vec4Tmp1[2] * 2 - 1; + vec4Tmp1[2] = winz2 * 2 - 1; // object raw coords = Inv(P x Mv) * winPos -> mat4Tmp2 multMatrixVec(mat4PMvI, vec4Tmp1, vec4Tmp2); @@ -1348,9 +1319,9 @@ public final class FloatUtil { vec4Tmp2[3] = 1.0f / vec4Tmp2[3]; - obj2_pos[0+obj2_pos_offset] = vec4Tmp2[0] * vec4Tmp2[3]; - obj2_pos[1+obj2_pos_offset] = vec4Tmp2[1] * vec4Tmp2[3]; - obj2_pos[2+obj2_pos_offset] = vec4Tmp2[2] * vec4Tmp2[3]; + objPos2.set( vec4Tmp2[0] * vec4Tmp2[3], + vec4Tmp2[1] * vec4Tmp2[3], + vec4Tmp2[2] * vec4Tmp2[3] ); return true; } @@ -1379,13 +1350,13 @@ public final class FloatUtil { * @param mat4Tmp2 16 component matrix for temp storage * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z) */ - public static boolean mapWinToObjCoords(final float winx, final float winy, final float winz, final float clipw, - final float[] modelMatrix, final int modelMatrix_offset, - final float[] projMatrix, final int projMatrix_offset, - final int[] viewport, final int viewport_offset, - final float near, final float far, - final float[] obj_pos, final int obj_pos_offset, - final float[/*16*/] mat4Tmp1, final float[/*16*/] mat4Tmp2) { + public static boolean mapWinToObj4(final float winx, final float winy, final float winz, final float clipw, + final float[] modelMatrix, final int modelMatrix_offset, + final float[] projMatrix, final int projMatrix_offset, + final int[] viewport, final int viewport_offset, + final float near, final float far, + final float[] obj_pos, final int obj_pos_offset, + final float[/*16*/] mat4Tmp1, final float[/*16*/] mat4Tmp2) { // mat4Tmp1 = P x Mv multMatrix(projMatrix, projMatrix_offset, modelMatrix, modelMatrix_offset, mat4Tmp1, 0); @@ -1397,7 +1368,7 @@ public final class FloatUtil { mat4Tmp2[0] = winx; mat4Tmp2[1] = winy; mat4Tmp2[2] = winz; - mat4Tmp2[3] = 1.0f; + mat4Tmp2[3] = clipw; // Map x and y from window coordinates mat4Tmp2[0] = (mat4Tmp2[0] - viewport[0+viewport_offset]) / viewport[2+viewport_offset]; @@ -1417,8 +1388,6 @@ public final class FloatUtil { return false; } - mat4Tmp2[3+raw_off] = 1.0f / mat4Tmp2[3+raw_off]; - obj_pos[0+obj_pos_offset] = mat4Tmp2[0+raw_off]; obj_pos[1+obj_pos_offset] = mat4Tmp2[1+raw_off]; obj_pos[2+obj_pos_offset] = mat4Tmp2[2+raw_off]; @@ -1459,7 +1428,7 @@ public final class FloatUtil { public static boolean mapWinToRay(final float winx, final float winy, final float winz0, final float winz1, final float[] modelMatrix, final int modelMatrix_offset, final float[] projMatrix, final int projMatrix_offset, - final int[] viewport, final int viewport_offset, + final int[] viewport, final Ray ray, final float[/*16*/] mat4Tmp1, final float[/*16*/] mat4Tmp2, final float[/*4*/] vec4Tmp2) { // mat4Tmp1 = P x Mv @@ -1469,11 +1438,9 @@ public final class FloatUtil { if ( null == invertMatrix(mat4Tmp1, mat4Tmp1) ) { return false; } - if( mapWinToObjCoords(winx, winy, winz0, winz1, mat4Tmp1, - viewport, viewport_offset, - ray.orig, 0, ray.dir, 0, - mat4Tmp2, vec4Tmp2) ) { - VectorUtil.normalizeVec3( VectorUtil.subVec3(ray.dir, ray.dir, ray.orig) ); + if( mapWinToObj(winx, winy, winz0, winz1, mat4Tmp1, viewport, + ray.orig, ray.dir, mat4Tmp2, vec4Tmp2) ) { + ray.dir.sub(ray.orig).normalize(); return true; } else { return false; @@ -1485,9 +1452,8 @@ public final class FloatUtil { * @param a 4x4 matrix in column-major order * @param b 4x4 matrix in column-major order * @param d result a*b in column-major order - * @return given result matrix d for chaining */ - public static float[] multMatrix(final float[] a, final int a_off, final float[] b, final int b_off, final float[] d, final int d_off) { + public static void multMatrix(final float[] a, final int a_off, final float[] b, final int b_off, final float[] d, final int d_off) { final float b00 = b[b_off+0+0*4]; final float b10 = b[b_off+1+0*4]; final float b20 = b[b_off+2+0*4]; @@ -1540,8 +1506,6 @@ public final class FloatUtil { d[d_off+3+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ; d[d_off+3+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ; d[d_off+3+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ; - - return d; } /** @@ -1612,9 +1576,8 @@ public final class FloatUtil { * Multiply matrix: [a] = [a] x [b] * @param a 4x4 matrix in column-major order (also result) * @param b 4x4 matrix in column-major order - * @return given result matrix a for chaining */ - public static float[] multMatrix(final float[] a, final int a_off, final float[] b, final int b_off) { + public static void multMatrix(final float[] a, final int a_off, final float[] b, final int b_off) { final float b00 = b[b_off+0+0*4]; final float b10 = b[b_off+1+0*4]; final float b20 = b[b_off+2+0*4]; @@ -1667,8 +1630,6 @@ public final class FloatUtil { a[a_off+3+1*4] = ai0 * b01 + ai1 * b11 + ai2 * b21 + ai3 * b31 ; a[a_off+3+2*4] = ai0 * b02 + ai1 * b12 + ai2 * b22 + ai3 * b32 ; a[a_off+3+3*4] = ai0 * b03 + ai1 * b13 + ai2 * b23 + ai3 * b33 ; - - return a; } /** @@ -1778,11 +1739,10 @@ public final class FloatUtil { * @param m_in_off * @param v_in 4-component column-vector * @param v_out m_in * v_in - * @return given result vector v_out for chaining */ - public static float[] multMatrixVec(final float[] m_in, final int m_in_off, - final float[] v_in, final int v_in_off, - final float[] v_out, final int v_out_off) { + public static void multMatrixVec(final float[] m_in, final int m_in_off, + final float[] v_in, final int v_in_off, + final float[] v_out, final int v_out_off) { // (one matrix row in column-major order) X (column vector) v_out[0 + v_out_off] = v_in[0+v_in_off] * m_in[0*4+m_in_off ] + v_in[1+v_in_off] * m_in[1*4+m_in_off ] + v_in[2+v_in_off] * m_in[2*4+m_in_off ] + v_in[3+v_in_off] * m_in[3*4+m_in_off ]; @@ -1798,8 +1758,31 @@ public final class FloatUtil { final int m_in_off_3 = 3+m_in_off; v_out[3 + v_out_off] = v_in[0+v_in_off] * m_in[0*4+m_in_off_3] + v_in[1+v_in_off] * m_in[1*4+m_in_off_3] + v_in[2+v_in_off] * m_in[2*4+m_in_off_3] + v_in[3+v_in_off] * m_in[3*4+m_in_off_3]; + } - return v_out; + /** + * @param m_in 4x4 matrix in column-major order + * @param m_in_off + * @param v_in 4-component column-vector + * @param v_out m_in * v_in + */ + public static void multMatrixVec(final float[] m_in, final int m_in_off, + final float[] v_in, final float[] v_out) { + // (one matrix row in column-major order) X (column vector) + v_out[0] = v_in[0] * m_in[0*4+m_in_off ] + v_in[1] * m_in[1*4+m_in_off ] + + v_in[2] * m_in[2*4+m_in_off ] + v_in[3] * m_in[3*4+m_in_off ]; + + final int m_in_off_1 = 1+m_in_off; + v_out[1] = v_in[0] * m_in[0*4+m_in_off_1] + v_in[1] * m_in[1*4+m_in_off_1] + + v_in[2] * m_in[2*4+m_in_off_1] + v_in[3] * m_in[3*4+m_in_off_1]; + + final int m_in_off_2 = 2+m_in_off; + v_out[2] = v_in[0] * m_in[0*4+m_in_off_2] + v_in[1] * m_in[1*4+m_in_off_2] + + v_in[2] * m_in[2*4+m_in_off_2] + v_in[3] * m_in[3*4+m_in_off_2]; + + final int m_in_off_3 = 3+m_in_off; + v_out[3] = v_in[0] * m_in[0*4+m_in_off_3] + v_in[1] * m_in[1*4+m_in_off_3] + + v_in[2] * m_in[2*4+m_in_off_3] + v_in[3] * m_in[3*4+m_in_off_3]; } /** @@ -1845,46 +1828,59 @@ public final class FloatUtil { } /** - * Copy the named column of the given column-major matrix to v_out. - *

- * v_out may be 3 or 4 components long, hence the 4th row may not be stored. - *

- * @param m_in input column-major matrix - * @param m_in_off offset to input matrix - * @param column named column to copy - * @param v_out the column-vector storage, at least 3 components long - * @param v_out_off offset to storage + * Affine 3f-vector transformation by 4x4 matrix + * + * 4x4 matrix multiplication with 3-component vector, + * using {@code 1} for for {@code v_in[3]} and dropping {@code v_out[3]}, + * which shall be {@code 1}. + * + * @param m_in 4x4 matrix in column-major order + * @param m_in_off + * @param v_in 3-component column-vector + * @param v_out m_in * v_in, 3-component column-vector * @return given result vector v_out for chaining */ - public static float[] copyMatrixColumn(final float[] m_in, final int m_in_off, final int column, final float[] v_out, final int v_out_off) { - v_out[0+v_out_off]=m_in[0+column*4+m_in_off]; - v_out[1+v_out_off]=m_in[1+column*4+m_in_off]; - v_out[2+v_out_off]=m_in[2+column*4+m_in_off]; - if( v_out.length > 3+v_out_off ) { - v_out[3+v_out_off]=m_in[3+column*4+m_in_off]; - } + public static float[] multMatrixVec3(final float[] m_in, final int m_in_off, + final float[] v_in, final float[] v_out) { + // (one matrix row in column-major order) X (column vector) + v_out[0] = v_in[0] * m_in[0*4+m_in_off ] + v_in[1] * m_in[1*4+m_in_off ] + + v_in[2] * m_in[2*4+m_in_off ] + 1f * m_in[3*4+m_in_off ]; + + final int m_in_off_1 = 1+m_in_off; + v_out[1] = v_in[0] * m_in[0*4+m_in_off_1] + v_in[1] * m_in[1*4+m_in_off_1] + + v_in[2] * m_in[2*4+m_in_off_1] + 1f * m_in[3*4+m_in_off_1]; + + final int m_in_off_2 = 2+m_in_off; + v_out[2] = v_in[0] * m_in[0*4+m_in_off_2] + v_in[1] * m_in[1*4+m_in_off_2] + + v_in[2] * m_in[2*4+m_in_off_2] + 1f * m_in[3*4+m_in_off_2]; + return v_out; } /** - * Copy the named row of the given column-major matrix to v_out. - *

- * v_out may be 3 or 4 components long, hence the 4th column may not be stored. - *

- * @param m_in input column-major matrix - * @param m_in_off offset to input matrix - * @param row named row to copy - * @param v_out the row-vector storage, at least 3 components long - * @param v_out_off offset to storage + * Affine 3f-vector transformation by 4x4 matrix + * + * 4x4 matrix multiplication with 3-component vector, + * using {@code 1} for for {@code v_in[3]} and dropping {@code v_out[3]}, + * which shall be {@code 1}. + * + * @param m_in 4x4 matrix in column-major order + * @param m_in_off + * @param v_in 3-component column-vector + * @param v_out m_in * v_in, 3-component column-vector * @return given result vector v_out for chaining */ - public static float[] copyMatrixRow(final float[] m_in, final int m_in_off, final int row, final float[] v_out, final int v_out_off) { - v_out[0+v_out_off]=m_in[row+0*4+m_in_off]; - v_out[1+v_out_off]=m_in[row+1*4+m_in_off]; - v_out[2+v_out_off]=m_in[row+2*4+m_in_off]; - if( v_out.length > 3+v_out_off ) { - v_out[3+v_out_off]=m_in[row+3*4+m_in_off]; - } + public static float[] multMatrixVec3(final float[] m_in, final float[] v_in, final float[] v_out) { + // (one matrix row in column-major order) X (column vector) + v_out[0] = v_in[0] * m_in[0*4 ] + v_in[1] * m_in[1*4 ] + + v_in[2] * m_in[2*4 ] + 1f * m_in[3*4 ]; + + v_out[1] = v_in[0] * m_in[0*4+1] + v_in[1] * m_in[1*4+1] + + v_in[2] * m_in[2*4+1] + 1f * m_in[3*4+1]; + + v_out[2] = v_in[0] * m_in[0*4+2] + v_in[1] * m_in[1*4+2] + + v_in[2] * m_in[2*4+2] + 1f * m_in[3*4+2]; + return v_out; } @@ -2276,7 +2272,7 @@ public final class FloatUtil { } /** - * Return true if value is zero, i.e. it's absolute value < epsilon. + * Return true if value is zero, i.e. it's absolute value < {@link #EPSILON}. * @see #EPSILON */ public static boolean isZero(final float a) { diff --git a/src/jogl/classes/com/jogamp/opengl/math/Matrix4.java b/src/jogl/classes/com/jogamp/opengl/math/Matrix4.java deleted file mode 100644 index a080d4442..000000000 --- a/src/jogl/classes/com/jogamp/opengl/math/Matrix4.java +++ /dev/null @@ -1,172 +0,0 @@ -/** - * Copyright 2014 JogAmp Community. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, this list - * of conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * The views and conclusions contained in the software and documentation are those of the - * authors and should not be interpreted as representing official policies, either expressed - * or implied, of JogAmp Community. - */ - -package com.jogamp.opengl.math; - -import com.jogamp.opengl.GLException; -import com.jogamp.opengl.fixedfunc.GLMatrixFunc; - -import com.jogamp.opengl.util.PMVMatrix; - -/** - * Simple float array-backed float 4x4 matrix - * exposing {@link FloatUtil} matrix functionality in an object oriented manner. - *

- * Unlike {@link PMVMatrix}, this class only represents one single matrix - * without a complete {@link GLMatrixFunc} implementation, - * allowing this class to be more lightweight. - *

- *

- * Implementation is not mature - WIP and subject to change. - *

- */ -public class Matrix4 { - - public Matrix4() { - matrix = new float[16]; - matrixTxSx = new float[16]; - mat4Tmp1 = new float[16]; - vec4Tmp1 = new float[4]; - FloatUtil.makeIdentity(matrixTxSx); - loadIdentity(); - } - - public final float[] getMatrix() { - return matrix; - } - - public final void loadIdentity() { - FloatUtil.makeIdentity(matrix); - } - - /** - * Multiply matrix: [this] = [this] x [m] - * @param m 4x4 matrix in column-major order - */ - public final void multMatrix(final float[] m, final int m_offset) { - FloatUtil.multMatrix(matrix, 0, m, m_offset); - } - - /** - * Multiply matrix: [this] = [this] x [m] - * @param m 4x4 matrix in column-major order - */ - public final void multMatrix(final float[] m) { - FloatUtil.multMatrix(matrix, m); - } - - /** - * Multiply matrix: [this] = [this] x [m] - * @param m 4x4 matrix in column-major order - */ - public final void multMatrix(final Matrix4 m) { - FloatUtil.multMatrix(matrix, m.getMatrix()); - } - - /** - * @param v_in 4-component column-vector - * @param v_out this * v_in - */ - public final void multVec(final float[] v_in, final float[] v_out) { - FloatUtil.multMatrixVec(matrix, v_in, v_out); - } - - /** - * @param v_in 4-component column-vector - * @param v_out this * v_in - */ - public final void multVec(final float[] v_in, final int v_in_offset, final float[] v_out, final int v_out_offset) { - FloatUtil.multMatrixVec(matrix, 0, v_in, v_in_offset, v_out, v_out_offset); - } - - public final void translate(final float x, final float y, final float z) { - multMatrix(FloatUtil.makeTranslation(matrixTxSx, false, x, y, z)); - } - - public final void scale(final float x, final float y, final float z) { - multMatrix(FloatUtil.makeScale(matrixTxSx, false, x, y, z)); - } - - public final void rotate(final float angrad, final float x, final float y, final float z) { - multMatrix(FloatUtil.makeRotationAxis(mat4Tmp1, 0, angrad, x, y, z, vec4Tmp1)); - } - - /** - * Rotate the current matrix with the given {@link Quaternion}'s rotation {@link Quaternion#toMatrix(float[], int) matrix representation}. - */ - public final void rotate(final Quaternion quat) { - multMatrix(quat.toMatrix(mat4Tmp1, 0)); - } - - public final void transpose() { - System.arraycopy(matrix, 0, mat4Tmp1, 0, 16); - FloatUtil.transposeMatrix(mat4Tmp1, matrix); - } - - public final float determinant() { - return FloatUtil.matrixDeterminant(matrix); - } - - public final boolean invert() { - return null != FloatUtil.invertMatrix(matrix, matrix); - } - - public final void makeOrtho(final float left, final float right, final float bottom, final float top, final float zNear, final float zFar) { - multMatrix( FloatUtil.makeOrtho(mat4Tmp1, 0, true, left, right, bottom, top, zNear, zFar) ); - } - - /** - * @param left - * @param right - * @param bottom - * @param top - * @param zNear - * @param zFar - * @throws GLException if {@code zNear <= 0} or {@code zFar <= zNear} - * or {@code left == right}, or {@code bottom == top}. - * @see FloatUtil#makeFrustum(float[], int, boolean, float, float, float, float, float, float) - */ - public final void makeFrustum(final float left, final float right, final float bottom, final float top, final float zNear, final float zFar) throws GLException { - multMatrix( FloatUtil.makeFrustum(mat4Tmp1, 0, true, left, right, bottom, top, zNear, zFar) ); - } - - /** - * @param fovy_rad - * @param aspect - * @param zNear - * @param zFar - * @throws GLException if {@code zNear <= 0} or {@code zFar <= zNear} - * @see FloatUtil#makePerspective(float[], int, boolean, float, float, float, float) - */ - public final void makePerspective(final float fovy_rad, final float aspect, final float zNear, final float zFar) throws GLException { - multMatrix( FloatUtil.makePerspective(mat4Tmp1, 0, true, fovy_rad, aspect, zNear, zFar) ); - } - - private final float[] matrix, matrixTxSx; - private final float[] mat4Tmp1, vec4Tmp1; -} diff --git a/src/jogl/classes/com/jogamp/opengl/math/Matrix4f.java b/src/jogl/classes/com/jogamp/opengl/math/Matrix4f.java new file mode 100644 index 000000000..5951c7d98 --- /dev/null +++ b/src/jogl/classes/com/jogamp/opengl/math/Matrix4f.java @@ -0,0 +1,1878 @@ +/** + * Copyright 2014-2023 JogAmp Community. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are + * permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of + * conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list + * of conditions and the following disclaimer in the documentation and/or other materials + * provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * The views and conclusions contained in the software and documentation are those of the + * authors and should not be interpreted as representing official policies, either expressed + * or implied, of JogAmp Community. + */ + +package com.jogamp.opengl.math; + +import java.nio.FloatBuffer; + +import com.jogamp.opengl.math.geom.AABBox; +import com.jogamp.opengl.math.geom.Frustum; +import com.jogamp.opengl.math.geom.Frustum.Plane; + +/** + * Basic 4x4 float matrix implementation using fields for intensive use-cases (host operations). + *

+ * Implementation covers {@link FloatUtil} matrix functionality, exposed in an object oriented manner. + *

+ *

+ * Unlike {@link com.jogamp.opengl.util.PMVMatrix PMVMatrix}, this class only represents one single matrix + * without a complete {@link com.jogamp.opengl.fixedfunc.GLMatrixFunc GLMatrixFunc} implementation. + *

+ *

+ * For array operations the layout is expected in column-major order + * matching OpenGL's implementation, illustration: + * 

+    Row-Major                       Column-Major (OpenGL):
+
+        |  0   1   2  tx |
+        |                |
+        |  4   5   6  ty |
+    M = |                |
+        |  8   9  10  tz |
+        |                |
+        | 12  13  14  15 |
+
+           R   C                      R   C
+         m[0*4+3] = tx;             m[0+4*3] = tx;
+         m[1*4+3] = ty;             m[1+4*3] = ty;
+         m[2*4+3] = tz;             m[2+4*3] = tz;
+
+          RC (std subscript order)   RC (std subscript order)
+         m03 = tx;                  m03 = tx;
+         m13 = ty;                  m13 = ty;
+         m23 = tz;                  m23 = tz;
+
+ *
+ *

+ *

+ *

+ *

+ *

+ * Implementation utilizes unrolling of small vertices and matrices wherever possible + * while trying to access memory in a linear fashion for performance reasons, see: + *

+ *

+ * @see com.jogamp.opengl.util.PMVMatrix + * @see FloatUtil + */ +public class Matrix4f { + + /** + * Creates a new identity matrix. + */ + public Matrix4f() { + m00 = m11 = m22 = m33 = 1.0f; + // remaining fields have default init to zero + } + + /** + * Creates a new matrix copying the values of the given {@code src} matrix. + */ + public Matrix4f(final Matrix4f src) { + load(src); + } + + /** + * Creates a new matrix based on given float[4*4] column major order. + * @param m 4x4 matrix in column-major order + */ + public Matrix4f(final float[] m) { + load(m); + } + + /** + * Creates a new matrix based on given float[4*4] column major order. + * @param m 4x4 matrix in column-major order + * @param m_off offset for matrix {@code m} + */ + public Matrix4f(final float[] m, final int m_off) { + load(m, m_off); + } + + // + // Write to Matrix via load(..) + // + + /** + * Set this matrix to identity. + * 
+      Translation matrix (Column Order):
+      1 0 0 0
+      0 1 0 0
+      0 0 1 0
+      0 0 0 1
+     *
+ * @return this matrix for chaining + */ + public final Matrix4f loadIdentity() { + m00 = m11 = m22 = m33 = 1.0f; + m01 = m02 = m03 = + m10 = m12 = m13 = + m20 = m21 = m23 = + m30 = m31 = m32 = 0.0f; + return this; + } + + /** + * Load the values of the given matrix {@code b} to this matrix. + * @param src the source values + * @return this matrix for chaining + */ + public Matrix4f load(final Matrix4f src) { + m00 = src.m00; m10 = src.m10; m20 = src.m20; m30 = src.m30; + m01 = src.m01; m11 = src.m11; m21 = src.m21; m31 = src.m31; + m02 = src.m02; m12 = src.m12; m22 = src.m22; m32 = src.m32; + m03 = src.m03; m13 = src.m13; m23 = src.m23; m33 = src.m33; + return this; + } + + /** + * Load the values of the given matrix {@code src} to this matrix. + * @param src 4x4 matrix float[16] in column-major order + * @return this matrix for chaining + */ + public Matrix4f load(final float[] src) { + m00 = src[0+0*4]; // column 0 + m10 = src[1+0*4]; + m20 = src[2+0*4]; + m30 = src[3+0*4]; + m01 = src[0+1*4]; // column 1 + m11 = src[1+1*4]; + m21 = src[2+1*4]; + m31 = src[3+1*4]; + m02 = src[0+2*4]; // column 2 + m12 = src[1+2*4]; + m22 = src[2+2*4]; + m32 = src[3+2*4]; + m03 = src[0+3*4]; // column 3 + m13 = src[1+3*4]; + m23 = src[2+3*4]; + m33 = src[3+3*4]; + return this; + } + + /** + * Load the values of the given matrix {@code src} to this matrix. + * @param src 4x4 matrix float[16] in column-major order + * @param src_off offset for matrix {@code src} + * @return this matrix for chaining + */ + public Matrix4f load(final float[] src, final int src_off) { + m00 = src[src_off+0+0*4]; + m10 = src[src_off+1+0*4]; + m20 = src[src_off+2+0*4]; + m30 = src[src_off+3+0*4]; + m01 = src[src_off+0+1*4]; + m11 = src[src_off+1+1*4]; + m21 = src[src_off+2+1*4]; + m31 = src[src_off+3+1*4]; + m02 = src[src_off+0+2*4]; + m12 = src[src_off+1+2*4]; + m22 = src[src_off+2+2*4]; + m32 = src[src_off+3+2*4]; + m03 = src[src_off+0+3*4]; + m13 = src[src_off+1+3*4]; + m23 = src[src_off+2+3*4]; + m33 = src[src_off+3+3*4]; + return this; + } + + /** + * Load the values of the given matrix {@code src} to this matrix. + *

+ * Implementation uses relative {@link FloatBuffer#get()}, + * hence caller may want to issue {@link FloatBuffer#reset()} thereafter. + *

+ * @param src 4x4 matrix {@link FloatBuffer} in column-major order + * @return this matrix for chaining + */ + public Matrix4f load(final FloatBuffer src) { + m00 = src.get(); + m10 = src.get(); + m20 = src.get(); + m30 = src.get(); + m01 = src.get(); + m11 = src.get(); + m21 = src.get(); + m31 = src.get(); + m02 = src.get(); + m12 = src.get(); + m22 = src.get(); + m32 = src.get(); + m03 = src.get(); + m13 = src.get(); + m23 = src.get(); + m33 = src.get(); + return this; + } + + // + // Read out Matrix via get(..) + // + + /** Gets the ith component, 0 <= i < 16 */ + public float get(final int i) { + switch (i) { + case 0+4*0: return m00; + case 1+4*0: return m10; + case 2+4*0: return m20; + case 3+4*0: return m30; + + case 0+4*1: return m01; + case 1+4*1: return m11; + case 2+4*1: return m21; + case 3+4*1: return m31; + + case 0+4*2: return m02; + case 1+4*2: return m12; + case 2+4*2: return m22; + case 3+4*2: return m32; + + case 0+4*3: return m03; + case 1+4*3: return m13; + case 2+4*3: return m23; + case 3+4*3: return m33; + + default: throw new IndexOutOfBoundsException(); + } + } + + /** + * Get the named column of the given column-major matrix to v_out. + * @param column named column to copy + * @param v_out the column-vector storage + * @return given result vector v_out for chaining + */ + public Vec4f getColumn(final int column, final Vec4f v_out) { + v_out.set( get(0+column*4), + get(1+column*4), + get(2+column*4), + get(3+column*4) ); + return v_out; + } + + /** + * Get the named column of the given column-major matrix to v_out. + * @param column named column to copy + * @param v_out the column-vector storage + * @return given result vector v_out for chaining + */ + public Vec3f getColumn(final int column, final Vec3f v_out) { + v_out.set( get(0+column*4), + get(1+column*4), + get(2+column*4) ); + return v_out; + } + + /** + * Get the named row of the given column-major matrix to v_out. + * @param row named row to copy + * @param v_out the row-vector storage + * @return given result vector v_out for chaining + */ + public Vec4f getRow(final int row, final Vec4f v_out) { + v_out.set( get(row+0*4), + get(row+1*4), + get(row+2*4), + get(row+3*4) ); + return v_out; + } + + /** + * Get the named row of the given column-major matrix to v_out. + * @param row named row to copy + * @param v_out the row-vector storage + * @return given result vector v_out for chaining + */ + public Vec3f getRow(final int row, final Vec3f v_out) { + v_out.set( get(row+0*4), + get(row+1*4), + get(row+2*4) ); + return v_out; + } + + /** + * Get this matrix into the given float[16] array at {@code dst_off} in column major order. + * + * @param dst float[16] array storage in column major order + * @param dst_off offset + */ + public void get(final float[] dst, final int dst_off) { + dst[dst_off+0+0*4] = m00; + dst[dst_off+1+0*4] = m10; + dst[dst_off+2+0*4] = m20; + dst[dst_off+3+0*4] = m30; + dst[dst_off+0+1*4] = m01; + dst[dst_off+1+1*4] = m11; + dst[dst_off+2+1*4] = m21; + dst[dst_off+3+1*4] = m31; + dst[dst_off+0+2*4] = m02; + dst[dst_off+1+2*4] = m12; + dst[dst_off+2+2*4] = m22; + dst[dst_off+3+2*4] = m32; + dst[dst_off+0+3*4] = m03; + dst[dst_off+1+3*4] = m13; + dst[dst_off+2+3*4] = m23; + dst[dst_off+3+3*4] = m33; + } + + /** + * Get this matrix into the given float[16] array in column major order. + * + * @param dst float[16] array storage in column major order + */ + public void get(final float[] dst) { + dst[0+0*4] = m00; + dst[1+0*4] = m10; + dst[2+0*4] = m20; + dst[3+0*4] = m30; + dst[0+1*4] = m01; + dst[1+1*4] = m11; + dst[2+1*4] = m21; + dst[3+1*4] = m31; + dst[0+2*4] = m02; + dst[1+2*4] = m12; + dst[2+2*4] = m22; + dst[3+2*4] = m32; + dst[0+3*4] = m03; + dst[1+3*4] = m13; + dst[2+3*4] = m23; + dst[3+3*4] = m33; + } + + /** + * Get this matrix into the given {@link FloatBuffer} in column major order. + *

+ * Implementation uses relative {@link FloatBuffer#put(float)}, + * hence caller may want to issue {@link FloatBuffer#reset()} thereafter. + *

+ * + * @param dst {@link FloatBuffer} array storage in column major order + */ + public void get(final FloatBuffer dst) { + dst.put( m00 ); + dst.put( m10 ); + dst.put( m20 ); + dst.put( m30 ); + dst.put( m01 ); + dst.put( m11 ); + dst.put( m21 ); + dst.put( m31 ); + dst.put( m02 ); + dst.put( m12 ); + dst.put( m22 ); + dst.put( m32 ); + dst.put( m03 ); + dst.put( m13 ); + dst.put( m23 ); + dst.put( m33 ); + } + + // + // Basic matrix operations + // + + /** + * Returns the determinant of this matrix + * @return the matrix determinant + */ + public float determinant() { + float ret = 0; + ret += m00 * ( + m11*(m22*m33 - m23*m32) - m12*(m21*m33 - m23*m31) + m13*(m21*m32 - m22*m31)); + ret -= m01 * ( + m10*(m22*m33 - m23*m32) - m12*(m20*m33 - m23*m30) + m13*(m20*m32 - m22*m30)); + ret += m02 * ( + m10*(m21*m33 - m23*m31) - m11*(m20*m33 - m23*m30) + m13*(m20*m31 - m21*m30)); + ret -= m03 * ( + m10*(m21*m32 - m22*m31) - m11*(m20*m32 - m22*m30) + m12*(m20*m31 - m21*m30)); + return ret; + } + + /** + * Transpose this matrix. + * + * @return this matrix for chaining + */ + public final Matrix4f transpose() { + float tmp; + + tmp = m10; + m10 = m01; + m01 = tmp; + + tmp = m20; + m20 = m02; + m02 = tmp; + + tmp = m30; + m30 = m03; + m03 = tmp; + + tmp = m21; + m21 = m12; + m12 = tmp; + + tmp = m31; + m31 = m13; + m13 = tmp; + + tmp = m32; + m32 = m23; + m23 = tmp; + + return this; + } + + /** + * Transpose the given {@code src} matrix into this matrix. + * + * @param src source 4x4 matrix + * @return this matrix (result) for chaining + */ + public final Matrix4f transpose(final Matrix4f src) { + if( src == this ) { + return transpose(); + } + m00 = src.m00; + m10 = src.m01; + m20 = src.m02; + m30 = src.m03; + + m01 = src.m10; + m11 = src.m11; + m21 = src.m12; + m31 = src.m13; + + m02 = src.m20; + m12 = src.m21; + m22 = src.m22; + m32 = src.m23; + + m03 = src.m30; + m13 = src.m31; + m23 = src.m32; + m33 = src.m33; + return this; + } + + /** + * Invert this matrix. + * @return false if this matrix is singular and inversion not possible, otherwise true + */ + public boolean invert() { + final float scale; + { + float a = Math.abs(m00); + float max = a; + + a = Math.abs(m01); if( a > max ) max = a; + a = Math.abs(m02); if( a > max ) max = a; + a = Math.abs(m03); if( a > max ) max = a; + + a = Math.abs(m10); if( a > max ) max = a; + a = Math.abs(m11); if( a > max ) max = a; + a = Math.abs(m12); if( a > max ) max = a; + a = Math.abs(m13); if( a > max ) max = a; + + a = Math.abs(m20); if( a > max ) max = a; + a = Math.abs(m21); if( a > max ) max = a; + a = Math.abs(m22); if( a > max ) max = a; + a = Math.abs(m23); if( a > max ) max = a; + + a = Math.abs(m30); if( a > max ) max = a; + a = Math.abs(m31); if( a > max ) max = a; + a = Math.abs(m32); if( a > max ) max = a; + a = Math.abs(m33); if( a > max ) max = a; + + if( 0 == max ) { + return false; + } + scale = 1.0f/max; + } + + final float a00 = m00*scale; + final float a10 = m10*scale; + final float a20 = m20*scale; + final float a30 = m30*scale; + + final float a01 = m01*scale; + final float a11 = m11*scale; + final float a21 = m21*scale; + final float a31 = m31*scale; + + final float a02 = m02*scale; + final float a12 = m12*scale; + final float a22 = m22*scale; + final float a32 = m32*scale; + + final float a03 = m03*scale; + final float a13 = m13*scale; + final float a23 = m23*scale; + final float a33 = m33*scale; + + final float b00 = + a11*(a22*a33 - a23*a32) - a12*(a21*a33 - a23*a31) + a13*(a21*a32 - a22*a31); + final float b01 = -( + a10*(a22*a33 - a23*a32) - a12*(a20*a33 - a23*a30) + a13*(a20*a32 - a22*a30)); + final float b02 = + a10*(a21*a33 - a23*a31) - a11*(a20*a33 - a23*a30) + a13*(a20*a31 - a21*a30); + final float b03 = -( + a10*(a21*a32 - a22*a31) - a11*(a20*a32 - a22*a30) + a12*(a20*a31 - a21*a30)); + + final float b10 = -( + a01*(a22*a33 - a23*a32) - a02*(a21*a33 - a23*a31) + a03*(a21*a32 - a22*a31)); + final float b11 = + a00*(a22*a33 - a23*a32) - a02*(a20*a33 - a23*a30) + a03*(a20*a32 - a22*a30); + final float b12 = -( + a00*(a21*a33 - a23*a31) - a01*(a20*a33 - a23*a30) + a03*(a20*a31 - a21*a30)); + final float b13 = + a00*(a21*a32 - a22*a31) - a01*(a20*a32 - a22*a30) + a02*(a20*a31 - a21*a30); + + final float b20 = + a01*(a12*a33 - a13*a32) - a02*(a11*a33 - a13*a31) + a03*(a11*a32 - a12*a31); + final float b21 = -( + a00*(a12*a33 - a13*a32) - a02*(a10*a33 - a13*a30) + a03*(a10*a32 - a12*a30)); + final float b22 = + a00*(a11*a33 - a13*a31) - a01*(a10*a33 - a13*a30) + a03*(a10*a31 - a11*a30); + final float b23 = -( + a00*(a11*a32 - a12*a31) - a01*(a10*a32 - a12*a30) + a02*(a10*a31 - a11*a30)); + + final float b30 = -( + a01*(a12*a23 - a13*a22) - a02*(a11*a23 - a13*a21) + a03*(a11*a22 - a12*a21)); + final float b31 = + a00*(a12*a23 - a13*a22) - a02*(a10*a23 - a13*a20) + a03*(a10*a22 - a12*a20); + final float b32 = -( + a00*(a11*a23 - a13*a21) - a01*(a10*a23 - a13*a20) + a03*(a10*a21 - a11*a20)); + final float b33 = + a00*(a11*a22 - a12*a21) - a01*(a10*a22 - a12*a20) + a02*(a10*a21 - a11*a20); + + final float det = (a00*b00 + a01*b01 + a02*b02 + a03*b03) / scale; + + if( 0 == det ) { + return false; + } + + m00 = b00 / det; + m10 = b01 / det; + m20 = b02 / det; + m30 = b03 / det; + + m01 = b10 / det; + m11 = b11 / det; + m21 = b12 / det; + m31 = b13 / det; + + m02 = b20 / det; + m12 = b21 / det; + m22 = b22 / det; + m32 = b23 / det; + + m03 = b30 / det; + m13 = b31 / det; + m23 = b32 / det; + m33 = b33 / det; + return true; + } + + /** + * Invert the {@code src} matrix values into this matrix + * @param src the source matrix, which values are to be inverted + * @return false if {@code src} matrix is singular and inversion not possible, otherwise true + */ + public boolean invert(final Matrix4f src) { + return load(src).invert(); + } + + /** + * Multiply matrix: [this] = [this] x [b] + *

+ * Roughly 15% slower than {@link #mul(Matrix4f, Matrix4f)} + * Roughly 3% slower than {@link FloatUtil#multMatrix(float[], float[])} + *

+ * @param b 4x4 matrix + * @return this matrix for chaining + * @see #mul(Matrix4f, Matrix4f) + */ + public final Matrix4f mul(final Matrix4f b) { + // return mul(new Matrix4f(this), b); // <- roughly half speed + float ai0=m00; // row-0, m[0+0*4] + float ai1=m01; + float ai2=m02; + float ai3=m03; + m00 = ai0 * b.m00 + ai1 * b.m10 + ai2 * b.m20 + ai3 * b.m30 ; + m01 = ai0 * b.m01 + ai1 * b.m11 + ai2 * b.m21 + ai3 * b.m31 ; + m02 = ai0 * b.m02 + ai1 * b.m12 + ai2 * b.m22 + ai3 * b.m32 ; + m03 = ai0 * b.m03 + ai1 * b.m13 + ai2 * b.m23 + ai3 * b.m33 ; + + ai0=m10; //row-1, m[1+0*4] + ai1=m11; + ai2=m12; + ai3=m13; + m10 = ai0 * b.m00 + ai1 * b.m10 + ai2 * b.m20 + ai3 * b.m30 ; + m11 = ai0 * b.m01 + ai1 * b.m11 + ai2 * b.m21 + ai3 * b.m31 ; + m12 = ai0 * b.m02 + ai1 * b.m12 + ai2 * b.m22 + ai3 * b.m32 ; + m13 = ai0 * b.m03 + ai1 * b.m13 + ai2 * b.m23 + ai3 * b.m33 ; + + ai0=m20; // row-2, m[2+0*4] + ai1=m21; + ai2=m22; + ai3=m23; + m20 = ai0 * b.m00 + ai1 * b.m10 + ai2 * b.m20 + ai3 * b.m30 ; + m21 = ai0 * b.m01 + ai1 * b.m11 + ai2 * b.m21 + ai3 * b.m31 ; + m22 = ai0 * b.m02 + ai1 * b.m12 + ai2 * b.m22 + ai3 * b.m32 ; + m23 = ai0 * b.m03 + ai1 * b.m13 + ai2 * b.m23 + ai3 * b.m33 ; + + ai0=m30; // row-3, m[3+0*4] + ai1=m31; + ai2=m32; + ai3=m33; + m30 = ai0 * b.m00 + ai1 * b.m10 + ai2 * b.m20 + ai3 * b.m30 ; + m31 = ai0 * b.m01 + ai1 * b.m11 + ai2 * b.m21 + ai3 * b.m31 ; + m32 = ai0 * b.m02 + ai1 * b.m12 + ai2 * b.m22 + ai3 * b.m32 ; + m33 = ai0 * b.m03 + ai1 * b.m13 + ai2 * b.m23 + ai3 * b.m33 ; + return this; + } + + /** + * Multiply matrix: [this] = [a] x [b] + *

+ * Roughly 13% faster than {@link #mul(Matrix4f)} + * Roughly 11% faster than {@link FloatUtil#multMatrix(float[], float[])} + *

+ * @param a 4x4 matrix, can't be this matrix + * @param b 4x4 matrix, can't be this matrix + * @return this matrix for chaining + * @see #mul(Matrix4f) + */ + public final Matrix4f mul(final Matrix4f a, final Matrix4f b) { + // row-0, m[0+0*4] + m00 = a.m00 * b.m00 + a.m01 * b.m10 + a.m02 * b.m20 + a.m03 * b.m30 ; + m01 = a.m00 * b.m01 + a.m01 * b.m11 + a.m02 * b.m21 + a.m03 * b.m31 ; + m02 = a.m00 * b.m02 + a.m01 * b.m12 + a.m02 * b.m22 + a.m03 * b.m32 ; + m03 = a.m00 * b.m03 + a.m01 * b.m13 + a.m02 * b.m23 + a.m03 * b.m33 ; + + //row-1, m[1+0*4] + m10 = a.m10 * b.m00 + a.m11 * b.m10 + a.m12 * b.m20 + a.m13 * b.m30 ; + m11 = a.m10 * b.m01 + a.m11 * b.m11 + a.m12 * b.m21 + a.m13 * b.m31 ; + m12 = a.m10 * b.m02 + a.m11 * b.m12 + a.m12 * b.m22 + a.m13 * b.m32 ; + m13 = a.m10 * b.m03 + a.m11 * b.m13 + a.m12 * b.m23 + a.m13 * b.m33 ; + + // row-2, m[2+0*4] + m20 = a.m20 * b.m00 + a.m21 * b.m10 + a.m22 * b.m20 + a.m23 * b.m30 ; + m21 = a.m20 * b.m01 + a.m21 * b.m11 + a.m22 * b.m21 + a.m23 * b.m31 ; + m22 = a.m20 * b.m02 + a.m21 * b.m12 + a.m22 * b.m22 + a.m23 * b.m32 ; + m23 = a.m20 * b.m03 + a.m21 * b.m13 + a.m22 * b.m23 + a.m23 * b.m33 ; + + // row-3, m[3+0*4] + m30 = a.m30 * b.m00 + a.m31 * b.m10 + a.m32 * b.m20 + a.m33 * b.m30 ; + m31 = a.m30 * b.m01 + a.m31 * b.m11 + a.m32 * b.m21 + a.m33 * b.m31 ; + m32 = a.m30 * b.m02 + a.m31 * b.m12 + a.m32 * b.m22 + a.m33 * b.m32 ; + m33 = a.m30 * b.m03 + a.m31 * b.m13 + a.m32 * b.m23 + a.m33 * b.m33 ; + + return this; + } + + /** + * @param v_in 4-component column-vector + * @param v_out this * v_in + * @returns v_out for chaining + */ + public final float[] mulVec4f(final float[/*4*/] v_in, final float[/*4*/] v_out) { + // (one matrix row in column-major order) X (column vector) + final float x = v_in[0], y = v_in[1], z = v_in[2], w = v_in[3]; + v_out[0] = x * m00 + y * m01 + z * m02 + w * m03; + v_out[1] = x * m10 + y * m11 + z * m12 + w * m13; + v_out[2] = x * m20 + y * m21 + z * m22 + w * m23; + v_out[3] = x * m30 + y * m31 + z * m32 + w * m33; + return v_out; + } + + /** + * @param v_in 4-component column-vector + * @param v_out this * v_in + * @returns v_out for chaining + */ + public final Vec4f mulVec4f(final Vec4f v_in, final Vec4f v_out) { + // (one matrix row in column-major order) X (column vector) + final float x = v_in.x(), y = v_in.y(), z = v_in.z(), w = v_in.w(); + v_out.set( x * m00 + y * m01 + z * m02 + w * m03, + x * m10 + y * m11 + z * m12 + w * m13, + x * m20 + y * m21 + z * m22 + w * m23, + x * m30 + y * m31 + z * m32 + w * m33 ); + return v_out; + } + + /** + * Affine 3f-vector transformation by 4x4 matrix + * + * 4x4 matrix multiplication with 3-component vector, + * using {@code 1} for for {@code v_in[3]} and dropping {@code v_out[3]}, + * which shall be {@code 1}. + * + * @param v_in 3-component column-vector + * @param v_out m_in * v_in, 3-component column-vector + * @returns v_out for chaining + */ + public final float[] mulVec3f(final float[/*3*/] v_in, final float[/*3*/] v_out) { + // (one matrix row in column-major order) X (column vector) + final float x = v_in[0], y = v_in[1], z = v_in[2]; + v_out[0] = x * m00 + y * m01 + z * m02 + 1f * m03; + v_out[1] = x * m10 + y * m11 + z * m12 + 1f * m13; + v_out[2] = x * m20 + y * m21 + z * m22 + 1f * m23; + return v_out; + } + + /** + * Affine 3f-vector transformation by 4x4 matrix + * + * 4x4 matrix multiplication with 3-component vector, + * using {@code 1} for for {@code v_in.w()} and dropping {@code v_out.w()}, + * which shall be {@code 1}. + * + * @param v_in 3-component column-vector {@link Vec3f} + * @param v_out m_in * v_in, 3-component column-vector {@link Vec3f} + * @returns v_out for chaining + */ + public final Vec3f mulVec3f(final Vec3f v_in, final Vec3f v_out) { + // (one matrix row in column-major order) X (column vector) + final float x = v_in.x(), y = v_in.y(), z = v_in.z(); + v_out.set( x * m00 + y * m01 + z * m02 + 1f * m03, + x * m10 + y * m11 + z * m12 + 1f * m13, + x * m20 + y * m21 + z * m22 + 1f * m23 ); + return v_out; + } + + // + // Matrix setTo...(), affine + basic + // + + /** + * Set this matrix to translation. + * 
+      Translation matrix (Column Order):
+      1 0 0 0
+      0 1 0 0
+      0 0 1 0
+      x y z 1
+     *
+ * @param x x-axis translate + * @param y y-axis translate + * @param z z-axis translate + * @return this matrix for chaining + */ + public final Matrix4f setToTranslation(final float x, final float y, final float z) { + m00 = m11 = m22 = m33 = 1.0f; + m03 = x; + m13 = y; + m23 = z; + m01 = m02 = + m10 = m12 = + m20 = m21 = + m30 = m31 = m32 = 0.0f; + return this; + } + + /** + * Set this matrix to translation. + * 
+      Translation matrix (Column Order):
+      1 0 0 0
+      0 1 0 0
+      0 0 1 0
+      x y z 1
+     *
+ * @param t translate Vec3f + * @return this matrix for chaining + */ + public final Matrix4f setToTranslation(final Vec3f t) { + return setToTranslation(t.x(), t.y(), t.z()); + } + + /** + * Set this matrix to scale. + * 
+      Scale matrix (Any Order):
+      x 0 0 0
+      0 y 0 0
+      0 0 z 0
+      0 0 0 1
+     *
+ * @param x x-axis scale + * @param y y-axis scale + * @param z z-axis scale + * @return this matrix for chaining + */ + public final Matrix4f setToScale(final float x, final float y, final float z) { + m33 = 1.0f; + m00 = x; + m11 = y; + m22 = z; + m01 = m02 = m03 = + m10 = m12 = m13 = + m20 = m21 = m23 = + m30 = m31 = m32 = 0.0f; + return this; + } + + /** + * Set this matrix to rotation from the given axis and angle in radians. + * 
+        Rotation matrix (Column Order):
+        xx(1-c)+c  xy(1-c)+zs xz(1-c)-ys 0
+        xy(1-c)-zs yy(1-c)+c  yz(1-c)+xs 0
+        xz(1-c)+ys yz(1-c)-xs zz(1-c)+c  0
+        0          0          0          1
+     *
+ * @see Matrix-FAQ Q38 + * @param ang_rad angle in radians + * @param x x of rotation axis + * @param y y of rotation axis + * @param z z of rotation axis + * @return this matrix for chaining + */ + public final Matrix4f setToRotationAxis(final float ang_rad, float x, float y, float z) { + final float c = FloatUtil.cos(ang_rad); + final float ic= 1.0f - c; + final float s = FloatUtil.sin(ang_rad); + + final float[] tmpVec3f = { x, y, z }; + VectorUtil.normalizeVec3(tmpVec3f); + x = tmpVec3f[0]; y = tmpVec3f[1]; z = tmpVec3f[2]; + + final float xy = x*y; + final float xz = x*z; + final float xs = x*s; + final float ys = y*s; + final float yz = y*z; + final float zs = z*s; + m00 = x*x*ic+c; + m10 = xy*ic+zs; + m20 = xz*ic-ys; + m30 = 0; + + m01 = xy*ic-zs; + m11 = y*y*ic+c; + m21 = yz*ic+xs; + m31 = 0; + + m02 = xz*ic+ys; + m12 = yz*ic-xs; + m22 = z*z*ic+c; + m32 = 0; + + m03 = 0f; + m13 = 0f; + m23 = 0f; + m33 = 1f; + + return this; + } + + /** + * Set this matrix to rotation from the given axis and angle in radians. + * 
+        Rotation matrix (Column Order):
+        xx(1-c)+c  xy(1-c)+zs xz(1-c)-ys 0
+        xy(1-c)-zs yy(1-c)+c  yz(1-c)+xs 0
+        xz(1-c)+ys yz(1-c)-xs zz(1-c)+c  0
+        0          0          0          1
+     *
+ * @see Matrix-FAQ Q38 + * @param ang_rad angle in radians + * @param axis rotation axis + * @return this matrix for chaining + */ + public final Matrix4f setToRotationAxis(final float ang_rad, final Vec3f axis) { + return setToRotationAxis(ang_rad, axis.x(), axis.y(), axis.z()); + } + + /** + * Set this matrix to rotation from the given Euler rotation angles in radians. + *

+ * The rotations are applied in the given order: + *

+ *

+ * @param bankX the Euler pitch angle in radians. (rotation about the X axis) + * @param headingY the Euler yaw angle in radians. (rotation about the Y axis) + * @param attitudeZ the Euler roll angle in radians. (rotation about the Z axis) + * @return this matrix for chaining + *

+ * Implementation does not use Quaternion and hence is exposed to + * Gimbal-Lock, + * consider using {@link #setToRotation(Quaternion)}. + *

+ * @see Matrix-FAQ Q36 + * @see euclideanspace.com-eulerToMatrix + * @see #setToRotation(Quaternion) + */ + public Matrix4f setToRotationEuler(final float bankX, final float headingY, final float attitudeZ) { + // Assuming the angles are in radians. + final float ch = FloatUtil.cos(headingY); + final float sh = FloatUtil.sin(headingY); + final float ca = FloatUtil.cos(attitudeZ); + final float sa = FloatUtil.sin(attitudeZ); + final float cb = FloatUtil.cos(bankX); + final float sb = FloatUtil.sin(bankX); + + m00 = ch*ca; + m10 = sa; + m20 = -sh*ca; + m30 = 0; + + m01 = sh*sb - ch*sa*cb; + m11 = ca*cb; + m21 = sh*sa*cb + ch*sb; + m31 = 0; + + m02 = ch*sa*sb + sh*cb; + m12 = -ca*sb; + m22 = -sh*sa*sb + ch*cb; + m32 = 0; + + m03 = 0; + m13 = 0; + m23 = 0; + m33 = 1; + + return this; + } + + /** + * Set this matrix to rotation using the given Quaternion. + *

+ * Implementation Details: + *

+ *

+ * + * @param q the Quaternion representing the rotation + * @return this matrix for chaining + * @see Matrix-FAQ Q54 + * @see Quaternion#toMatrix(float[], int) + * @see #getRotation() + */ + public final Matrix4f setToRotation(final Quaternion q) { + // pre-multiply scaled-reciprocal-magnitude to reduce multiplications + final float norm = q.magnitudeSquared(); + if ( FloatUtil.isZero(norm, FloatUtil.EPSILON) ) { + // identity matrix -> srecip = 0f + loadIdentity(); + return this; + } + final float srecip; + if ( FloatUtil.isEqual(1f, norm, FloatUtil.EPSILON) ) { + srecip = 2f; + } else { + srecip = 2.0f / norm; + } + + final float x = q.x(); + final float y = q.y(); + final float z = q.z(); + final float w = q.w(); + + final float xs = srecip * x; + final float ys = srecip * y; + final float zs = srecip * z; + + final float xx = x * xs; + final float xy = x * ys; + final float xz = x * zs; + final float xw = xs * w; + final float yy = y * ys; + final float yz = y * zs; + final float yw = ys * w; + final float zz = z * zs; + final float zw = zs * w; + + m00 = 1f - ( yy + zz ); + m01 = ( xy - zw ); + m02 = ( xz + yw ); + m03 = 0f; + + m10 = ( xy + zw ); + m11 = 1f - ( xx + zz ); + m12 = ( yz - xw ); + m13 = 0f; + + m20 = ( xz - yw ); + m21 = ( yz + xw ); + m22 = 1f - ( xx + yy ); + m23 = 0f; + + m30 = m31 = m32 = 0f; + m33 = 1f; + return this; + } + + /** + * Returns the rotation [m00 .. m22] fields converted to a Quaternion. + * @param res resulting Quaternion + * @return the resulting Quaternion for chaining. + * @see Quaternion#setFromMatrix(float, float, float, float, float, float, float, float, float) + * @see #setToRotation(Quaternion) + */ + public final Quaternion getRotation(final Quaternion res) { + res.setFromMatrix(m00, m01, m02, m10, m11, m12, m20, m21, m22); + return res; + } + + /** + * Set this matrix to orthogonal projection. + * 
+      Ortho matrix (Column Order):
+      2/dx  0     0    0
+      0     2/dy  0    0
+      0     0     2/dz 0
+      tx    ty    tz   1
+     *
+ * @param left + * @param right + * @param bottom + * @param top + * @param zNear + * @param zFar + * @return this matrix for chaining + */ + public Matrix4f setToOrtho(final float left, final float right, + final float bottom, final float top, + final float zNear, final float zFar) { + { + // m00 = m11 = m22 = m33 = 1f; + m10 = m20 = m30 = 0f; + m01 = m21 = m31 = 0f; + m02 = m12 = m32 = 0f; + // m03 = m13 = m23 = 0f; + } + final float dx=right-left; + final float dy=top-bottom; + final float dz=zFar-zNear; + final float tx=-1.0f*(right+left)/dx; + final float ty=-1.0f*(top+bottom)/dy; + final float tz=-1.0f*(zFar+zNear)/dz; + + m00 = 2.0f/dx; + m11 = 2.0f/dy; + m22 = -2.0f/dz; + + m03 = tx; + m13 = ty; + m23 = tz; + m33 = 1f; + + return this; + } + + /** + * Set this matrix to frustum. + * 
+      Frustum matrix (Column Order):
+      2*zNear/dx   0          0   0
+      0            2*zNear/dy 0   0
+      A            B          C  -1
+      0            0          D   0
+     *
+ * @param left + * @param right + * @param bottom + * @param top + * @param zNear + * @param zFar + * @return this matrix for chaining + * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear} + * or {@code left == right}, or {@code bottom == top}. + */ + public Matrix4f setToFrustum(final float left, final float right, + final float bottom, final float top, + final float zNear, final float zFar) throws IllegalArgumentException { + if( zNear <= 0.0f || zFar <= zNear ) { + throw new IllegalArgumentException("Requirements zNear > 0 and zFar > zNear, but zNear "+zNear+", zFar "+zFar); + } + if( left == right || top == bottom) { + throw new IllegalArgumentException("GL_INVALID_VALUE: top,bottom and left,right must not be equal"); + } + { + // m00 = m11 = m22 = m33 = 1f; + m10 = m20 = m30 = 0f; + m01 = m21 = m31 = 0f; + m03 = m13 = 0f; + } + final float zNear2 = 2.0f*zNear; + final float dx=right-left; + final float dy=top-bottom; + final float dz=zFar-zNear; + final float A=(right+left)/dx; + final float B=(top+bottom)/dy; + final float C=-1.0f*(zFar+zNear)/dz; + final float D=-2.0f*(zFar*zNear)/dz; + + m00 = zNear2/dx; + m11 = zNear2/dy; + + m02 = A; + m12 = B; + m22 = C; + m32 = -1.0f; + + m23 = D; + m33 = 0f; + + return this; + } + + /** + * Set this matrix to perspective {@link #setToFrustum(float, float, float, float, float, float) frustum} projection. + * + * @param fovy_rad angle in radians + * @param aspect aspect ratio width / height + * @param zNear + * @param zFar + * @return this matrix for chaining + * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear} + * @see #setToFrustum(float, float, float, float, float, float) + */ + public Matrix4f setToPerspective(final float fovy_rad, final float aspect, final float zNear, final float zFar) throws IllegalArgumentException { + final float top = FloatUtil.tan(fovy_rad/2f) * zNear; // use tangent of half-fov ! + final float bottom = -1.0f * top; // -1f * fovhvTan.top * zNear + final float left = aspect * bottom; // aspect * -1f * fovhvTan.top * zNear + final float right = aspect * top; // aspect * fovhvTan.top * zNear + return setToFrustum(left, right, bottom, top, zNear, zFar); + } + + /** + * Set this matrix to perspective {@link #setToFrustum(float, float, float, float, float, float) frustum} projection. + * + * @param fovhv {@link FovHVHalves} field of view in both directions, may not be centered, either in radians or tangent + * @param zNear + * @param zFar + * @return this matrix for chaining + * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear} + * @see #setToFrustum(float, float, float, float, float, float) + * @see Frustum#updateByFovDesc(float[], int, boolean, Frustum.FovDesc) + */ + public Matrix4f setToPerspective(final FovHVHalves fovhv, final float zNear, final float zFar) throws IllegalArgumentException { + final FovHVHalves fovhvTan = fovhv.toTangents(); // use tangent of half-fov ! + final float top = fovhvTan.top * zNear; + final float bottom = -1.0f * fovhvTan.bottom * zNear; + final float left = -1.0f * fovhvTan.left * zNear; + final float right = fovhvTan.right * zNear; + return setToFrustum(left, right, bottom, top, zNear, zFar); + } + + /** + * Calculate the frustum planes in world coordinates + * using the passed float[16] as premultiplied P*MV (column major order). + *

+ * Frustum plane's normals will point to the inside of the viewing frustum, + * as required by this class. + *

+ */ + public void updateFrustumPlanes(final Frustum frustum) { + // Left: a = m41 + m11, b = m42 + m12, c = m43 + m13, d = m44 + m14 - [1..4] column-major + // Left: a = m30 + m00, b = m31 + m01, c = m32 + m02, d = m33 + m03 - [0..3] column-major + { + final Frustum.Plane p = frustum.getPlanes()[Frustum.LEFT]; + final Vec3f p_n = p.n; + p_n.set( m30 + m00, + m31 + m01, + m32 + m02 ); + p.d = m33 + m03; + } + + // Right: a = m41 - m11, b = m42 - m12, c = m43 - m13, d = m44 - m14 - [1..4] column-major + // Right: a = m30 - m00, b = m31 - m01, c = m32 - m02, d = m33 - m03 - [0..3] column-major + { + final Frustum.Plane p = frustum.getPlanes()[Frustum.RIGHT]; + final Vec3f p_n = p.n; + p_n.set( m30 - m00, + m31 - m01, + m32 - m02 ); + p.d = m33 - m03; + } + + // Bottom: a = m41m21, b = m42m22, c = m43m23, d = m44m24 - [1..4] column-major + // Bottom: a = m30m10, b = m31m11, c = m32m12, d = m33m13 - [0..3] column-major + { + final Frustum.Plane p = frustum.getPlanes()[Frustum.BOTTOM]; + final Vec3f p_n = p.n; + p_n.set( m30 + m10, + m31 + m11, + m32 + m12 ); + p.d = m33 + m13; + } + + // Top: a = m41 - m21, b = m42 - m22, c = m43 - m23, d = m44 - m24 - [1..4] column-major + // Top: a = m30 - m10, b = m31 - m11, c = m32 - m12, d = m33 - m13 - [0..3] column-major + { + final Frustum.Plane p = frustum.getPlanes()[Frustum.TOP]; + final Vec3f p_n = p.n; + p_n.set( m30 - m10, + m31 - m11, + m32 - m12 ); + p.d = m33 - m13; + } + + // Near: a = m41m31, b = m42m32, c = m43m33, d = m44m34 - [1..4] column-major + // Near: a = m30m20, b = m31m21, c = m32m22, d = m33m23 - [0..3] column-major + { + final Frustum.Plane p = frustum.getPlanes()[Frustum.NEAR]; + final Vec3f p_n = p.n; + p_n.set( m30 + m20, + m31 + m21, + m32 + m22 ); + p.d = m33 + m23; + } + + // Far: a = m41 - m31, b = m42 - m32, c = m43 - m33, d = m44 - m34 - [1..4] column-major + // Far: a = m30 - m20, b = m31 - m21, c = m32m22, d = m33m23 - [0..3] column-major + { + final Frustum.Plane p = frustum.getPlanes()[Frustum.FAR]; + final Vec3f p_n = p.n; + p_n.set( m30 - m20, + m31 - m21, + m32 - m22 ); + p.d = m33 - m23; + } + + // Normalize all planes + for (int i = 0; i < 6; ++i) { + final Plane p = frustum.getPlanes()[i]; + final Vec3f p_n = p.n; + final float invLen = 1f / p_n.length(); + p_n.scale(invLen); + p.d *= invLen; + } + } + + /** + * Make given matrix the look-at matrix based on given parameters. + *

+ * Consist out of two matrix multiplications: + * 

+     *   R = L x T,
+     *   with L for look-at matrix and
+     *        T for eye translation.
+     *
+     *   Result R can be utilized for projection or modelview multiplication, i.e.
+     *          M = M x R,
+     *          with M being the projection or modelview matrix.
+     *
+ *

+ * @param eye 3 component eye vector + * @param center 3 component center vector + * @param up 3 component up vector + * @param tmp temporary Matrix4f used for multiplication + * @return this matrix for chaining + */ + public Matrix4f setToLookAt(final Vec3f eye, final Vec3f center, final Vec3f up, final Matrix4f tmp) { + // normalized forward! + final Vec3f fwd = new Vec3f( center.x() - eye.x(), + center.y() - eye.y(), + center.z() - eye.z() ).normalize(); + + /* Side = forward x up, normalized */ + final Vec3f side = fwd.cross(up).normalize(); + + /* Recompute up as: up = side x forward */ + final Vec3f up2 = side.cross(fwd); + + m00 = side.x(); + m10 = up2.x(); + m20 = -fwd.x(); + m30 = 0; + + m01 = side.y(); + m11 = up2.y(); + m21 = -fwd.y(); + m31 = 0; + + m02 = side.z(); + m12 = up2.z(); + m22 = -fwd.z(); + m32 = 0; + + m03 = 0; + m13 = 0; + m23 = 0; + m33 = 1; + + return mul( tmp.setToTranslation( -eye.x(), -eye.y(), -eye.z() ) ); + } + + // + // Matrix affine operations using setTo..() + // + + /** + * Rotate this matrix about give axis and angle in radians, i.e. multiply by {@link #setToRotationAxis(float, float, float, float) axis-rotation matrix}. + * @see Matrix-FAQ Q38 + * @param angrad angle in radians + * @param x x of rotation axis + * @param y y of rotation axis + * @param z z of rotation axis + * @param tmp temporary Matrix4f used for multiplication + * @return this matrix for chaining + */ + public final Matrix4f rotate(final float ang_rad, final float x, final float y, final float z, final Matrix4f tmp) { + return mul( tmp.setToRotationAxis(ang_rad, x, y, z) ); + } + + /** + * Rotate this matrix about give axis and angle in radians, i.e. multiply by {@link #setToRotationAxis(float, Vec3f) axis-rotation matrix}. + * @see Matrix-FAQ Q38 + * @param angrad angle in radians + * @param axis rotation axis + * @param tmp temporary Matrix4f used for multiplication + * @return this matrix for chaining + */ + public final Matrix4f rotate(final float ang_rad, final Vec3f axis, final Matrix4f tmp) { + return mul( tmp.setToRotationAxis(ang_rad, axis) ); + } + + /** + * Rotate this matrix with the given {@link Quaternion}, i.e. multiply by {@link #setToRotation(Quaternion) Quaternion's rotation matrix}. + * @param tmp temporary Matrix4f used for multiplication + * @return this matrix for chaining + */ + public final Matrix4f rotate(final Quaternion quat, final Matrix4f tmp) { + return mul( tmp.setToRotation(quat) ); + } + + /** + * Translate this matrix, i.e. multiply by {@link #setToTranslation(float, float, float) translation matrix}. + * @param x x translation + * @param y y translation + * @param z z translation + * @param tmp temporary Matrix4f used for multiplication + * @return this matrix for chaining + */ + public final Matrix4f translate(final float x, final float y, final float z, final Matrix4f tmp) { + return mul( tmp.setToTranslation(x, y, z) ); + } + + /** + * Translate this matrix, i.e. multiply by {@link #setToTranslation(Vec3f) translation matrix}. + * @param t translation Vec3f + * @param tmp temporary Matrix4f used for multiplication + * @return this matrix for chaining + */ + public final Matrix4f translate(final Vec3f t, final Matrix4f tmp) { + return mul( tmp.setToTranslation(t) ); + } + + /** + * Scale this matrix, i.e. multiply by {@link #setToScale(float, float, float) scale matrix}. + * @param x x scale + * @param y y scale + * @param z z scale + * @param tmp temporary Matrix4f used for multiplication + * @return this matrix for chaining + */ + public final Matrix4f scale(final float x, final float y, final float z, final Matrix4f tmp) { + return mul( tmp.setToScale(x, y, z) ); + } + + /** + * Scale this matrix, i.e. multiply by {@link #setToScale(float, float, float) scale matrix}. + * @param s scale for x-, y- and z-axis + * @param tmp temporary Matrix4f used for multiplication + * @return this matrix for chaining + */ + public final Matrix4f scale(final float s, final Matrix4f tmp) { + return mul( tmp.setToScale(s, s, s) ); + } + + // + // Matrix Stack + // + + /** + * Push the matrix to it's stack, while preserving this matrix values. + * @see #pop() + */ + public final void push() { + stack.push(this); + } + + /** + * Pop the current matrix from it's stack, replacing this matrix values. + * @see #push() + */ + public final void pop() { + stack.pop(this); + } + + // + // equals + // + + /** + * Equals check using a given {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}. + *

+ * Implementation considers following corner cases: + *

+ * @param o comparison value + * @param epsilon consider using {@link FloatUtil#EPSILON} + * @return true if all components differ less than {@code epsilon}, otherwise false. + */ + public boolean isEqual(final Matrix4f o, final float epsilon) { + if( this == o ) { + return true; + } else { + return FloatUtil.isEqual(m00, o.m00, epsilon) && + FloatUtil.isEqual(m01, o.m01, epsilon) && + FloatUtil.isEqual(m02, o.m02, epsilon) && + FloatUtil.isEqual(m03, o.m03, epsilon) && + FloatUtil.isEqual(m10, o.m10, epsilon) && + FloatUtil.isEqual(m11, o.m11, epsilon) && + FloatUtil.isEqual(m12, o.m12, epsilon) && + FloatUtil.isEqual(m13, o.m13, epsilon) && + FloatUtil.isEqual(m20, o.m20, epsilon) && + FloatUtil.isEqual(m21, o.m21, epsilon) && + FloatUtil.isEqual(m22, o.m22, epsilon) && + FloatUtil.isEqual(m23, o.m23, epsilon) && + FloatUtil.isEqual(m30, o.m30, epsilon) && + FloatUtil.isEqual(m31, o.m31, epsilon) && + FloatUtil.isEqual(m32, o.m32, epsilon) && + FloatUtil.isEqual(m33, o.m33, epsilon); + } + } + + /** + * Equals check using {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}. + *

+ * Implementation considers following corner cases: + *

+ * @param o comparison value + * @return true if all components differ less than {@link FloatUtil#EPSILON}, otherwise false. + */ + public boolean isEqual(final Matrix4f o) { + return isEqual(o, FloatUtil.EPSILON); + } + + @Override + public boolean equals(final Object o) { + if( o instanceof Matrix4f ) { + return isEqual((Matrix4f)o, FloatUtil.EPSILON); + } else { + return false; + } + } + + // + // Static multi Matrix ops + // + + /** + * Map object coordinates to window coordinates. + *

+ * Traditional gluProject implementation. + *

+ * + * @param obj object position, 3 component vector + * @param mMv modelview matrix + * @param mP projection matrix + * @param viewport 4 component viewport vector + * @param winPos 3 component window coordinate, the result + * @return true if successful, otherwise false (z is 1) + */ + public static boolean mapObjToWin(final Vec3f obj, final Matrix4f mMv, final Matrix4f mP, + final int[] viewport, final float[] winPos) + { + final Vec4f vec4Tmp1 = new Vec4f(obj, 1f); + + // vec4Tmp2 = Mv * o + // rawWinPos = P * vec4Tmp2 + // rawWinPos = P * ( Mv * o ) + // rawWinPos = P * Mv * o + final Vec4f vec4Tmp2 = mMv.mulVec4f(vec4Tmp1, new Vec4f()); + final Vec4f rawWinPos = mP.mulVec4f(vec4Tmp2, vec4Tmp1); + + if (rawWinPos.w() == 0.0f) { + return false; + } + + final float s = ( 1.0f / rawWinPos.w() ) * 0.5f; + + // Map x, y and z to range 0-1 (w is ignored) + rawWinPos.scale(s).add(0.5f, 0.5f, 0.5f, 0f); + + // Map x,y to viewport + winPos[0] = rawWinPos.x() * viewport[2] + viewport[0]; + winPos[1] = rawWinPos.y() * viewport[3] + viewport[1]; + winPos[2] = rawWinPos.z(); + + return true; + } + + /** + * Map object coordinates to window coordinates. + *

+ * Traditional gluProject implementation. + *

+ * + * @param obj object position, 3 component vector + * @param mPMv [projection] x [modelview] matrix, i.e. P x Mv + * @param viewport 4 component viewport vector + * @param winPos 3 component window coordinate, the result + * @return true if successful, otherwise false (z is 1) + */ + public static boolean mapObjToWin(final Vec3f obj, final Matrix4f mPMv, + final int[] viewport, final float[] winPos) + { + final Vec4f vec4Tmp2 = new Vec4f(obj, 1f); + + // rawWinPos = P * Mv * o + final Vec4f rawWinPos = mPMv.mulVec4f(vec4Tmp2, new Vec4f()); + + if (rawWinPos.w() == 0.0f) { + return false; + } + + final float s = ( 1.0f / rawWinPos.w() ) * 0.5f; + + // Map x, y and z to range 0-1 (w is ignored) + rawWinPos.scale(s).add(0.5f, 0.5f, 0.5f, 0f); + + // Map x,y to viewport + winPos[0] = rawWinPos.x() * viewport[2] + viewport[0]; + winPos[1] = rawWinPos.y() * viewport[3] + viewport[1]; + winPos[2] = rawWinPos.z(); + + return true; + } + + /** + * Map window coordinates to object coordinates. + *

+ * Traditional gluUnProject implementation. + *

+ * + * @param winx + * @param winy + * @param winz + * @param mMv 4x4 modelview matrix + * @param mP 4x4 projection matrix + * @param viewport 4 component viewport vector + * @param objPos 3 component object coordinate, the result + * @param mat4Tmp 16 component matrix for temp storage + * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z) + */ + public static boolean mapWinToObj(final float winx, final float winy, final float winz, + final Matrix4f mMv, final Matrix4f mP, + final int[] viewport, + final Vec3f objPos, + final Matrix4f mat4Tmp) + { + // invPMv = Inv(P x Mv) + final Matrix4f invPMv = mat4Tmp.mul(mP, mMv); + if( !invPMv.invert() ) { + return false; + } + + final Vec4f winPos = new Vec4f(winx, winy, winz, 1f); + + // Map x and y from window coordinates + winPos.add(-viewport[0], -viewport[1], 0f, 0f).scale(1f/viewport[2], 1f/viewport[3], 1f, 1f); + + // Map to range -1 to 1 + winPos.scale(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f); + + // rawObjPos = Inv(P x Mv) * winPos + final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f()); + + if ( rawObjPos.w() == 0.0f ) { + return false; + } + objPos.set( rawObjPos.scale( 1f / rawObjPos.w() ) ); + + return true; + } + + /** + * Map window coordinates to object coordinates. + *

+ * Traditional gluUnProject implementation. + *

+ * + * @param winx + * @param winy + * @param winz + * @param invPMv inverse [projection] x [modelview] matrix, i.e. Inv(P x Mv) + * @param viewport 4 component viewport vector + * @param objPos 3 component object coordinate, the result + * @param mat4Tmp 16 component matrix for temp storage + * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z) + */ + public static boolean mapWinToObj(final float winx, final float winy, final float winz, + final Matrix4f invPMv, + final int[] viewport, + final Vec3f objPos, + final Matrix4f mat4Tmp) + { + final Vec4f winPos = new Vec4f(winx, winy, winz, 1f); + + // Map x and y from window coordinates + winPos.add(-viewport[0], -viewport[1], 0f, 0f).scale(1f/viewport[2], 1f/viewport[3], 1f, 1f); + + // Map to range -1 to 1 + winPos.scale(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f); + + // rawObjPos = Inv(P x Mv) * winPos + final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f()); + + if ( rawObjPos.w() == 0.0f ) { + return false; + } + objPos.set( rawObjPos.scale( 1f / rawObjPos.w() ) ); + + return true; + } + + /** + * Map two window coordinates to two object coordinates, + * distinguished by their z component. + *

+ * Traditional gluUnProject implementation. + *

+ * + * @param winx + * @param winy + * @param winz1 + * @param winz2 + * @param invPMv inverse [projection] x [modelview] matrix, i.e. Inv(P x Mv) + * @param viewport 4 component viewport vector + * @param objPos1 3 component object coordinate, the result + * @param mat4Tmp 16 component matrix for temp storage + * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z) + */ + public static boolean mapWinToObj(final float winx, final float winy, final float winz1, final float winz2, + final Matrix4f invPMv, + final int[] viewport, + final Vec3f objPos1, final Vec3f objPos2, + final Matrix4f mat4Tmp) + { + final Vec4f winPos = new Vec4f(winx, winy, winz1, 1f); + + // Map x and y from window coordinates + winPos.add(-viewport[0], -viewport[1], 0f, 0f).scale(1f/viewport[2], 1f/viewport[3], 1f, 1f); + + // Map to range -1 to 1 + winPos.scale(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f); + + // rawObjPos = Inv(P x Mv) * winPos1 + final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f()); + + if ( rawObjPos.w() == 0.0f ) { + return false; + } + objPos1.set( rawObjPos.scale( 1f / rawObjPos.w() ) ); + + // + // winz2 + // + // Map Z to range -1 to 1 + winPos.setZ( winz2 * 2f - 1f ); + + // rawObjPos = Inv(P x Mv) * winPos2 + invPMv.mulVec4f(winPos, rawObjPos); + + if ( rawObjPos.w() == 0.0f ) { + return false; + } + objPos2.set( rawObjPos.scale( 1f / rawObjPos.w() ) ); + + return true; + } + + /** + * Map window coordinates to object coordinates. + *

+ * Traditional gluUnProject4 implementation. + *

+ * + * @param winx + * @param winy + * @param winz + * @param clipw + * @param mMv 4x4 modelview matrix + * @param mP 4x4 projection matrix + * @param viewport 4 component viewport vector + * @param near + * @param far + * @param obj_pos 4 component object coordinate, the result + * @param mat4Tmp 16 component matrix for temp storage + * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z) + */ + public static boolean mapWinToObj4(final float winx, final float winy, final float winz, final float clipw, + final Matrix4f mMv, final Matrix4f mP, + final int[] viewport, + final float near, final float far, + final Vec4f objPos, + final Matrix4f mat4Tmp) + { + // invPMv = Inv(P x Mv) + final Matrix4f invPMv = mat4Tmp.mul(mP, mMv); + if( !invPMv.invert() ) { + return false; + } + + final Vec4f winPos = new Vec4f(winx, winy, winz, clipw); + + // Map x and y from window coordinates + winPos.add(-viewport[0], -viewport[1], -near, 0f).scale(1f/viewport[2], 1f/viewport[3], 1f/(far-near), 1f); + + // Map to range -1 to 1 + winPos.scale(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f); + + // objPos = Inv(P x Mv) * winPos + invPMv.mulVec4f(winPos, objPos); + + if ( objPos.w() == 0.0f ) { + return false; + } + return true; + } + + /** + * Map two window coordinates w/ shared X/Y and distinctive Z + * to a {@link Ray}. The resulting {@link Ray} maybe used for picking + * using a {@link AABBox#getRayIntersection(Ray, float[]) bounding box}. + *

+ * Notes for picking winz0 and winz1: + *

+ *

+ * @param winx + * @param winy + * @param winz0 + * @param winz1 + * @param mMv 4x4 modelview matrix + * @param mP 4x4 projection matrix + * @param viewport 4 component viewport vector + * @param ray storage for the resulting {@link Ray} + * @param mat4Tmp1 16 component matrix for temp storage + * @param mat4Tmp2 16 component matrix for temp storage + * @return true if successful, otherwise false (failed to invert matrix, or becomes z is infinity) + */ + public static boolean mapWinToRay(final float winx, final float winy, final float winz0, final float winz1, + final Matrix4f mMv, + final Matrix4f mP, + final int[] viewport, + final Ray ray, + final Matrix4f mat4Tmp1, final Matrix4f mat4Tmp2) { + // invPMv = Inv(P x Mv) + final Matrix4f invPMv = mat4Tmp1.mul(mP, mMv); + if( !invPMv.invert() ) { + return false; + } + + if( mapWinToObj(winx, winy, winz0, winz1, invPMv, viewport, + ray.orig, ray.dir, mat4Tmp2) ) { + ray.dir.sub(ray.orig).normalize(); + return true; + } else { + return false; + } + } + + // + // String and internals + // + + /** + * @param sb optional passed StringBuilder instance to be used + * @param rowPrefix optional prefix for each row + * @param f the format string of one floating point, i.e. "%10.5f", see {@link java.util.Formatter} + * @return matrix string representation + */ + public StringBuilder toString(final StringBuilder sb, final String rowPrefix, final String f) { + final float[] tmp = new float[16]; + this.get(tmp); + return FloatUtil.matrixToString(sb, rowPrefix, f,tmp, 0, 4, 4, false /* rowMajorOrder */); + } + + @Override + public String toString() { + return toString(null, null, "%10.5f").toString(); + } + + private float m00, m10, m20, m30; + private float m01, m11, m21, m31; + private float m02, m12, m22, m32; + private float m03, m13, m23, m33; + + final Stack stack = new Stack(0, 16*16); // start w/ zero size, growSize is half GL-min size (32) + + private static class Stack { + private int position; + private float[] buffer; + private final int growSize; + + /** + * @param initialSize initial size + * @param growSize grow size if {@link #position()} is reached, maybe 0 + * in which case an {@link IndexOutOfBoundsException} is thrown. + */ + public Stack(final int initialSize, final int growSize) { + this.position = 0; + this.growSize = growSize; + this.buffer = new float[initialSize]; + } + + private final void growIfNecessary(final int length) throws IndexOutOfBoundsException { + if( position + length > buffer.length ) { + if( 0 >= growSize ) { + throw new IndexOutOfBoundsException("Out of fixed stack size: "+this); + } + final float[] newBuffer = + new float[buffer.length + growSize]; + System.arraycopy(buffer, 0, newBuffer, 0, position); + buffer = newBuffer; + } + } + + public final Matrix4f push(final Matrix4f src) throws IndexOutOfBoundsException { + growIfNecessary(16); + src.get(buffer, position); + position += 16; + return src; + } + + public final Matrix4f pop(final Matrix4f dest) throws IndexOutOfBoundsException { + position -= 16; + dest.load(buffer, position); + return dest; + } + } +} diff --git a/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java b/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java index 849477f54..2bb0f96c6 100644 --- a/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java +++ b/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java @@ -1,5 +1,5 @@ /** - * Copyright 2010 JogAmp Community. All rights reserved. + * Copyright 2010-2023 JogAmp Community. All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, are * permitted provided that the following conditions are met: @@ -99,7 +99,7 @@ public class Quaternion { return FloatUtil.sqrt(magnitudeSQ); } - public final float getW() { + public final float w() { return w; } @@ -107,7 +107,7 @@ public class Quaternion { this.w = w; } - public final float getX() { + public final float x() { return x; } @@ -115,7 +115,7 @@ public class Quaternion { this.x = x; } - public final float getY() { + public final float y() { return y; } @@ -123,7 +123,7 @@ public class Quaternion { this.y = y; } - public final float getZ() { + public final float z() { return z; } @@ -142,15 +142,15 @@ public class Quaternion { * Returns the dot product of this quaternion with the given quaternion */ public final float dot(final Quaternion quat) { - return dot(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); + return dot(quat.x(), quat.y(), quat.z(), quat.w()); } /** * Returns true if this quaternion has identity. *

* Implementation uses {@link FloatUtil#EPSILON epsilon} to compare - * {@link #getW() W} {@link FloatUtil#isEqual(float, float, float) against 1f} and - * {@link #getX() X}, {@link #getY() Y} and {@link #getZ() Z} + * {@link #w() W} {@link FloatUtil#isEqual(float, float, float) against 1f} and + * {@link #x() X}, {@link #y() Y} and {@link #z() Z} * {@link FloatUtil#isZero(float, float) against zero}. *

*/ @@ -404,12 +404,12 @@ public class Quaternion { * *

* For details see {@link #rotateByEuler(float, float, float)}. - * @param angradXYZ euler angel array in radians + * @param angradXYZ euler angle array in radians * @return this quaternion for chaining. * @see #rotateByEuler(float, float, float) */ - public final Quaternion rotateByEuler(final float[] angradXYZ) { - return rotateByEuler(angradXYZ[0], angradXYZ[1], angradXYZ[2]); + public final Quaternion rotateByEuler(final Vec3f angradXYZ) { + return rotateByEuler(angradXYZ.x(), angradXYZ.y(), angradXYZ.z()); } /** @@ -450,48 +450,42 @@ public class Quaternion { /*** * Rotate the given vector by this quaternion + * @param vecIn vector to be rotated + * @param vecOut result storage for rotated vector, maybe equal to vecIn for in-place rotation * - * @param vecOut result float[3] storage for rotated vector, maybe equal to vecIn for in-place rotation - * @param vecOutOffset offset in result storage - * @param vecIn float[3] vector to be rotated - * @param vecInOffset offset in vecIn * @return the given vecOut store for chaining * @see Matrix-FAQ Q63 */ - public final float[] rotateVector(final float[] vecOut, final int vecOutOffset, final float[] vecIn, final int vecInOffset) { - if ( VectorUtil.isVec3Zero(vecIn, vecInOffset, FloatUtil.EPSILON) ) { - vecOut[0+vecOutOffset] = 0f; - vecOut[1+vecOutOffset] = 0f; - vecOut[2+vecOutOffset] = 0f; + public final Vec3f rotateVector(final Vec3f vecIn, final Vec3f vecOut) { + if( vecIn.isZero() ) { + vecOut.set(0, 0, 0); } else { - final float vecX = vecIn[0+vecInOffset]; - final float vecY = vecIn[1+vecInOffset]; - final float vecZ = vecIn[2+vecInOffset]; + final float vecX = vecIn.x(); + final float vecY = vecIn.y(); + final float vecZ = vecIn.z(); final float x_x = x*x; final float y_y = y*y; final float z_z = z*z; final float w_w = w*w; - vecOut[0+vecOutOffset] = w_w * vecX - + x_x * vecX - - z_z * vecX - - y_y * vecX - + 2f * ( y*w*vecZ - z*w*vecY + y*x*vecY + z*x*vecZ ); + vecOut.setX( w_w * vecX + + x_x * vecX + - z_z * vecX + - y_y * vecX + + 2f * ( y*w*vecZ - z*w*vecY + y*x*vecY + z*x*vecZ ) ); ; - vecOut[1+vecOutOffset] = y_y * vecY - - z_z * vecY - + w_w * vecY - - x_x * vecY - + 2f * ( x*y*vecX + z*y*vecZ + w*z*vecX - x*w*vecZ ); - ; - - vecOut[2+vecOutOffset] = z_z * vecZ - - y_y * vecZ - - x_x * vecZ - + w_w * vecZ - + 2f * ( x*z*vecX + y*z*vecY - w*y*vecX + w*x*vecY ); - ; + vecOut.setY( y_y * vecY + - z_z * vecY + + w_w * vecY + - x_x * vecY + + 2f * ( x*y*vecX + z*y*vecZ + w*z*vecX - x*w*vecZ ) );; + + vecOut.setZ( z_z * vecZ + - y_y * vecZ + - x_x * vecZ + + w_w * vecZ + + 2f * ( x*z*vecX + y*z*vecY - w*y*vecX + w*x*vecY ) ); } return vecOut; } @@ -593,21 +587,19 @@ public class Quaternion { * @return this quaternion for chaining. * @see euclideanspace.com-LookUp */ - public Quaternion setLookAt(final float[] directionIn, final float[] upIn, - final float[] xAxisOut, final float[] yAxisOut, final float[] zAxisOut) { + public Quaternion setLookAt(final Vec3f directionIn, final Vec3f upIn, + final Vec3f xAxisOut, final Vec3f yAxisOut, final Vec3f zAxisOut) { // Z = norm(dir) - VectorUtil.normalizeVec3(zAxisOut, directionIn); + zAxisOut.set(directionIn).normalize(); // X = upIn x Z // (borrow yAxisOut for upNorm) - VectorUtil.normalizeVec3(yAxisOut, upIn); - VectorUtil.crossVec3(xAxisOut, yAxisOut, zAxisOut); - VectorUtil.normalizeVec3(xAxisOut); + yAxisOut.set(upIn).normalize(); + xAxisOut.cross(yAxisOut, zAxisOut).normalize(); // Y = Z x X // - VectorUtil.crossVec3(yAxisOut, zAxisOut, xAxisOut); - VectorUtil.normalizeVec3(yAxisOut); + yAxisOut.cross(zAxisOut, xAxisOut).normalize(); /** final float m00 = xAxisOut[0]; @@ -642,42 +634,42 @@ public class Quaternion { *

* @param v1 not normalized * @param v2 not normalized - * @param tmpPivotVec float[3] temp storage for cross product - * @param tmpNormalVec float[3] temp storage to normalize vector + * @param tmpPivotVec temp storage for cross product + * @param tmpNormalVec temp storage to normalize vector * @return this quaternion for chaining. */ - public final Quaternion setFromVectors(final float[] v1, final float[] v2, final float[] tmpPivotVec, final float[] tmpNormalVec) { - final float factor = VectorUtil.normVec3(v1) * VectorUtil.normVec3(v2); + public final Quaternion setFromVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec, final Vec3f tmpNormalVec) { + final float factor = v1.length() * v2.length(); if ( FloatUtil.isZero(factor, FloatUtil.EPSILON ) ) { return setIdentity(); } else { - final float dot = VectorUtil.dotVec3(v1, v2) / factor; // normalize + final float dot = v1.dot(v2) / factor; // normalize final float theta = FloatUtil.acos(Math.max(-1.0f, Math.min(dot, 1.0f))); // clipping [-1..1] - VectorUtil.crossVec3(tmpPivotVec, v1, v2); + tmpPivotVec.cross(v1, v2); - if ( dot < 0.0f && FloatUtil.isZero( VectorUtil.normVec3(tmpPivotVec), FloatUtil.EPSILON ) ) { + if ( dot < 0.0f && FloatUtil.isZero( tmpPivotVec.length(), FloatUtil.EPSILON ) ) { // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector // perpendicular to this vector will rotate vector a onto vector b. // // The following guarantees the dot-product will be 0.0. int dominantIndex; - if (Math.abs(v1[0]) > Math.abs(v1[1])) { - if (Math.abs(v1[0]) > Math.abs(v1[2])) { + if (Math.abs(v1.x()) > Math.abs(v1.y())) { + if (Math.abs(v1.x()) > Math.abs(v1.z())) { dominantIndex = 0; } else { dominantIndex = 2; } } else { - if (Math.abs(v1[1]) > Math.abs(v1[2])) { + if (Math.abs(v1.y()) > Math.abs(v1.z())) { dominantIndex = 1; } else { dominantIndex = 2; } } - tmpPivotVec[dominantIndex] = -v1[(dominantIndex + 1) % 3]; - tmpPivotVec[(dominantIndex + 1) % 3] = v1[dominantIndex]; - tmpPivotVec[(dominantIndex + 2) % 3] = 0f; + tmpPivotVec.set( dominantIndex, -v1.get( (dominantIndex + 1) % 3 ) ); + tmpPivotVec.set( (dominantIndex + 1) % 3, v1.get( dominantIndex ) ); + tmpPivotVec.set( (dominantIndex + 2) % 3, 0f ); } return setFromAngleAxis(theta, tmpPivotVec, tmpNormalVec); } @@ -698,41 +690,41 @@ public class Quaternion { *

* @param v1 normalized * @param v2 normalized - * @param tmpPivotVec float[3] temp storage for cross product + * @param tmpPivotVec temp storage for cross product * @return this quaternion for chaining. */ - public final Quaternion setFromNormalVectors(final float[] v1, final float[] v2, final float[] tmpPivotVec) { - final float factor = VectorUtil.normVec3(v1) * VectorUtil.normVec3(v2); + public final Quaternion setFromNormalVectors(final Vec3f v1, final Vec3f v2, final Vec3f tmpPivotVec) { + final float factor = v1.length() * v2.length(); if ( FloatUtil.isZero(factor, FloatUtil.EPSILON ) ) { return setIdentity(); } else { - final float dot = VectorUtil.dotVec3(v1, v2) / factor; // normalize + final float dot = v1.dot(v2) / factor; // normalize final float theta = FloatUtil.acos(Math.max(-1.0f, Math.min(dot, 1.0f))); // clipping [-1..1] - VectorUtil.crossVec3(tmpPivotVec, v1, v2); + tmpPivotVec.cross(v1, v2); - if ( dot < 0.0f && FloatUtil.isZero( VectorUtil.normVec3(tmpPivotVec), FloatUtil.EPSILON ) ) { + if ( dot < 0.0f && FloatUtil.isZero( tmpPivotVec.length(), FloatUtil.EPSILON ) ) { // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector // perpendicular to this vector will rotate vector a onto vector b. // // The following guarantees the dot-product will be 0.0. int dominantIndex; - if (Math.abs(v1[0]) > Math.abs(v1[1])) { - if (Math.abs(v1[0]) > Math.abs(v1[2])) { + if (Math.abs(v1.x()) > Math.abs(v1.y())) { + if (Math.abs(v1.x()) > Math.abs(v1.z())) { dominantIndex = 0; } else { dominantIndex = 2; } } else { - if (Math.abs(v1[1]) > Math.abs(v1[2])) { + if (Math.abs(v1.y()) > Math.abs(v1.z())) { dominantIndex = 1; } else { dominantIndex = 2; } } - tmpPivotVec[dominantIndex] = -v1[(dominantIndex + 1) % 3]; - tmpPivotVec[(dominantIndex + 1) % 3] = v1[dominantIndex]; - tmpPivotVec[(dominantIndex + 2) % 3] = 0f; + tmpPivotVec.set( dominantIndex, -v1.get( (dominantIndex + 1) % 3 ) ); + tmpPivotVec.set( (dominantIndex + 1) % 3, v1.get( dominantIndex ) ); + tmpPivotVec.set( (dominantIndex + 2) % 3, 0f ); } return setFromAngleNormalAxis(theta, tmpPivotVec); } @@ -748,14 +740,14 @@ public class Quaternion { *

* @param angle rotation angle (rads) * @param vector axis vector not normalized - * @param tmpV3f float[3] temp storage to normalize vector + * @param tmpV3f temp storage to normalize vector * @return this quaternion for chaining. * * @see Matrix-FAQ Q56 - * @see #toAngleAxis(float[]) + * @see #toAngleAxis(Vec3f) */ - public final Quaternion setFromAngleAxis(final float angle, final float[] vector, final float[] tmpV3f) { - VectorUtil.normalizeVec3(tmpV3f, vector); + public final Quaternion setFromAngleAxis(final float angle, final Vec3f vector, final Vec3f tmpV3f) { + tmpV3f.set(vector).normalize(); return setFromAngleNormalAxis(angle, tmpV3f); } @@ -772,17 +764,17 @@ public class Quaternion { * @return this quaternion for chaining. * * @see Matrix-FAQ Q56 - * @see #toAngleAxis(float[]) + * @see #toAngleAxis(Vec3f) */ - public final Quaternion setFromAngleNormalAxis(final float angle, final float[] vector) { - if ( VectorUtil.isVec3Zero(vector, 0, FloatUtil.EPSILON) ) { + public final Quaternion setFromAngleNormalAxis(final float angle, final Vec3f vector) { + if( vector.isZero() ) { setIdentity(); } else { final float halfangle = angle * 0.5f; final float sin = FloatUtil.sin(halfangle); - x = vector[0] * sin; - y = vector[1] * sin; - z = vector[2] * sin; + x = vector.x() * sin; + y = vector.y() * sin; + z = vector.z() * sin; w = FloatUtil.cos(halfangle); } return this; @@ -791,24 +783,22 @@ public class Quaternion { /** * Transform the rotational quaternion to axis based rotation angles * - * @param axis float[3] storage for computed axis + * @param axis storage for computed axis * @return the rotation angle in radians - * @see #setFromAngleAxis(float, float[], float[]) + * @see #setFromAngleAxis(float, Vec3f, Vec3f) */ - public final float toAngleAxis(final float[] axis) { + public final float toAngleAxis(final Vec3f axis) { final float sqrLength = x*x + y*y + z*z; float angle; if ( FloatUtil.isZero(sqrLength, FloatUtil.EPSILON) ) { // length is ~0 angle = 0.0f; - axis[0] = 1.0f; - axis[1] = 0.0f; - axis[2] = 0.0f; + axis.set( 1.0f, 0.0f, 0.0f ); } else { angle = FloatUtil.acos(w) * 2.0f; final float invLength = 1.0f / FloatUtil.sqrt(sqrLength); - axis[0] = x * invLength; - axis[1] = y * invLength; - axis[2] = z * invLength; + axis.set( x * invLength, + y * invLength, + z * invLength ); } return angle; } @@ -816,7 +806,7 @@ public class Quaternion { /** * Initializes this quaternion from the given Euler rotation array angradXYZ in radians. *

- * The angradXYZ array is laid out in natural order: + * The angradXYZ vector is laid out in natural order: *

*

* For details see {@link #setFromEuler(float, float, float)}. - * @param angradXYZ euler angel array in radians + * @param angradXYZ euler angle vector in radians holding x-bank, y-heading and z-attitude * @return this quaternion for chaining. * @see #setFromEuler(float, float, float) */ - public final Quaternion setFromEuler(final float[] angradXYZ) { - return setFromEuler(angradXYZ[0], angradXYZ[1], angradXYZ[2]); + public final Quaternion setFromEuler(final Vec3f angradXYZ) { + return setFromEuler(angradXYZ.x(), angradXYZ.y(), angradXYZ.z()); } /** @@ -857,7 +847,7 @@ public class Quaternion { * @see Matrix-FAQ Q60 * @see Gems * @see euclideanspace.com-eulerToQuaternion - * @see #toEuler(float[]) + * @see #toEuler(Vec3f) */ public final Quaternion setFromEuler(final float bankX, final float headingY, final float attitudeZ) { if ( VectorUtil.isZero(bankX, headingY, attitudeZ, FloatUtil.EPSILON) ) { @@ -889,57 +879,44 @@ public class Quaternion { /** * Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ). + *

+ * The result array is laid out in natural order: + *

+ *

* - * @param result the float[] array storing the computed angles. - * @return the double[] array, filled with heading, attitude and bank in that order.. + * @param result euler angle result vector for radians x-bank, y-heading and z-attitude + * @return the Vec3f `result` filled with x-bank, y-heading and z-attitude * @see euclideanspace.com-quaternionToEuler * @see #setFromEuler(float, float, float) */ - public float[] toEuler(final float[] result) { + public Vec3f toEuler(final Vec3f result) { final float sqw = w*w; final float sqx = x*x; final float sqy = y*y; final float sqz = z*z; - final float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise - // is correction factor + final float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise, is correction factor final float test = x*y + z*w; if (test > 0.499f * unit) { // singularity at north pole - result[0] = 0f; - result[1] = 2f * FloatUtil.atan2(x, w); - result[2] = FloatUtil.HALF_PI; + result.set( 0f, // x-bank + 2f * FloatUtil.atan2(x, w), // y-heading + FloatUtil.HALF_PI ); // z-attitude } else if (test < -0.499f * unit) { // singularity at south pole - result[0] = 0f; - result[1] = -2 * FloatUtil.atan2(x, w); - result[2] = -FloatUtil.HALF_PI; + result.set( 0f, // x-bank + -2 * FloatUtil.atan2(x, w), // y-heading + -FloatUtil.HALF_PI ); // z-attitude } else { - result[0] = FloatUtil.atan2(2f * x * w - 2 * y * z, -sqx + sqy - sqz + sqw); - result[1] = FloatUtil.atan2(2f * y * w - 2 * x * z, sqx - sqy - sqz + sqw); - result[2] = FloatUtil.asin( 2f * test / unit); + result.set( FloatUtil.atan2(2f * x * w - 2 * y * z, -sqx + sqy - sqz + sqw), // x-bank + FloatUtil.atan2(2f * y * w - 2 * x * z, sqx - sqy - sqz + sqw), // y-heading + FloatUtil.asin( 2f * test / unit) ); // z-attitude } return result; } - /** - * Initializes this quaternion from a 4x4 column rotation matrix - *

- * See Graphics Gems Code,
- * MatrixTrace. - *

- *

- * Buggy Matrix-FAQ Q55 - *

- * - * @param m 4x4 column matrix - * @return this quaternion for chaining. - * @see #toMatrix(float[], int) - */ - public final Quaternion setFromMatrix(final float[] m, final int m_off) { - return setFromMatrix(m[0+0*4+m_off], m[0+1*4+m_off], m[0+2*4+m_off], - m[1+0*4+m_off], m[1+1*4+m_off], m[1+2*4+m_off], - m[2+0*4+m_off], m[2+1*4+m_off], m[2+2*4+m_off]); - } - /** * Compute the quaternion from a 3x3 column rotation matrix *

@@ -951,7 +928,7 @@ public class Quaternion { *

* * @return this quaternion for chaining. - * @see #toMatrix(float[], int) + * @see #setFromMatrix(Matrix4f) */ public Quaternion setFromMatrix(final float m00, final float m01, final float m02, final float m10, final float m11, final float m12, @@ -995,6 +972,24 @@ public class Quaternion { return this; } + /** + * Compute the quaternion from a 3x3 column rotation matrix + *

+ * See Graphics Gems Code,
+ * MatrixTrace. + *

+ *

+ * Buggy Matrix-FAQ Q55 + *

+ * + * @return this quaternion for chaining. + * @see Matrix4f#getRotation(Quaternion) + * @see #setFromMatrix(float, float, float, float, float, float, float, float, float) + */ + public Quaternion setFromMatrix(final Matrix4f m) { + return m.getRotation(this); + } + /** * Transform this quaternion to a normalized 4x4 column matrix representing the rotation. *

@@ -1008,7 +1003,8 @@ public class Quaternion { * @param mat_offset * @return the given matrix store * @see Matrix-FAQ Q54 - * @see #setFromMatrix(float[], int) + * @see #setFromMatrix(Matrix4f) + * @see #setFromMatrix(float, float, float, float, float, float, float, float, float) */ public final float[] toMatrix(final float[] matrix, final int mat_offset) { // pre-multiply scaled-reciprocal-magnitude to reduce multiplications @@ -1061,58 +1057,22 @@ public class Quaternion { } /** - * @param index the 3x3 rotation matrix column to retrieve from this quaternion (normalized). Must be between 0 and 2. - * @param result the vector object to store the result in. - * @return the result column-vector for chaining. + * Transform this quaternion to a normalized 4x4 column matrix representing the rotation. + *

+ * Implementation Details: + *

+ *

+ * + * @param matrix store for the resulting normalized column matrix 4x4 + * @return the given matrix store + * @see Matrix-FAQ Q54 + * @see #setFromMatrix(float, float, float, float, float, float, float, float, float) + * @see Matrix4f#setToRotation(Quaternion) */ - public float[] copyMatrixColumn(final int index, final float[] result, final int resultOffset) { - // pre-multipliy scaled-reciprocal-magnitude to reduce multiplications - final float norm = magnitudeSquared(); - final float srecip; - if ( FloatUtil.isZero(norm, FloatUtil.EPSILON) ) { - srecip= 0f; - } else if ( FloatUtil.isEqual(1f, norm, FloatUtil.EPSILON) ) { - srecip= 2f; - } else { - srecip= 2.0f / norm; - } - - // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs - // will be used 2-4 times each. - final float xs = x * srecip; - final float ys = y * srecip; - final float zs = z * srecip; - final float xx = x * xs; - final float xy = x * ys; - final float xz = x * zs; - final float xw = w * xs; - final float yy = y * ys; - final float yz = y * zs; - final float yw = w * ys; - final float zz = z * zs; - final float zw = w * zs; - - // using s=2/norm (instead of 1/norm) saves 3 multiplications by 2 here - switch (index) { - case 0: - result[0+resultOffset] = 1.0f - (yy + zz); - result[1+resultOffset] = xy + zw; - result[2+resultOffset] = xz - yw; - break; - case 1: - result[0+resultOffset] = xy - zw; - result[1+resultOffset] = 1.0f - (xx + zz); - result[2+resultOffset] = yz + xw; - break; - case 2: - result[0+resultOffset] = xz + yw; - result[1+resultOffset] = yz - xw; - result[2+resultOffset] = 1.0f - (xx + yy); - break; - default: - throw new IllegalArgumentException("Invalid column index. " + index); - } - return result; + public final Matrix4f toMatrix(final Matrix4f matrix) { + return matrix.setToRotation(this); } /** @@ -1126,10 +1086,10 @@ public class Quaternion { * @param zAxis vector representing the orthogonal z-axis of the coordinate system. * @return this quaternion for chaining. */ - public final Quaternion setFromAxes(final float[] xAxis, final float[] yAxis, final float[] zAxis) { - return setFromMatrix(xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); + public final Quaternion setFromAxes(final Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis) { + return setFromMatrix(xAxis.x(), yAxis.x(), zAxis.x(), + xAxis.y(), yAxis.y(), zAxis.y(), + xAxis.z(), yAxis.z(), zAxis.z()); } /** @@ -1140,11 +1100,11 @@ public class Quaternion { * @param zAxis vector representing the orthogonal z-axis of the coordinate system. * @param tmpMat4 temporary float[4] matrix, used to transform this quaternion to a matrix. */ - public void toAxes(final float[] xAxis, final float[] yAxis, final float[] zAxis, final float[] tmpMat4) { - toMatrix(tmpMat4, 0); - FloatUtil.copyMatrixColumn(tmpMat4, 0, 2, zAxis, 0); - FloatUtil.copyMatrixColumn(tmpMat4, 0, 1, yAxis, 0); - FloatUtil.copyMatrixColumn(tmpMat4, 0, 0, xAxis, 0); + public void toAxes(final Vec3f xAxis, final Vec3f yAxis, final Vec3f zAxis, final Matrix4f tmpMat4) { + tmpMat4.setToRotation(this); + tmpMat4.getColumn(2, zAxis); + tmpMat4.getColumn(1, yAxis); + tmpMat4.getColumn(0, xAxis); } /** @@ -1154,6 +1114,7 @@ public class Quaternion { * @param m 3x3 column matrix * @return true if representing a rotational matrix, false otherwise */ + @Deprecated public final boolean isRotationMatrix3f(final float[] m) { final float epsilon = 0.01f; // margin to allow for rounding errors if (FloatUtil.abs(m[0] * m[3] + m[3] * m[4] + m[6] * m[7]) > epsilon) @@ -1171,6 +1132,7 @@ public class Quaternion { return (FloatUtil.abs(determinant3f(m) - 1) < epsilon); } + @Deprecated private final float determinant3f(final float[] m) { return m[0] * m[4] * m[8] + m[3] * m[7] * m[2] + m[6] * m[1] * m[5] - m[0] * m[7] * m[5] - m[3] * m[1] * m[8] - m[6] * m[4] * m[2]; @@ -1193,10 +1155,10 @@ public class Quaternion { return false; } final Quaternion comp = (Quaternion) o; - return Math.abs(x - comp.getX()) <= ALLOWED_DEVIANCE && - Math.abs(y - comp.getY()) <= ALLOWED_DEVIANCE && - Math.abs(z - comp.getZ()) <= ALLOWED_DEVIANCE && - Math.abs(w - comp.getW()) <= ALLOWED_DEVIANCE; + return Math.abs(x - comp.x()) <= ALLOWED_DEVIANCE && + Math.abs(y - comp.y()) <= ALLOWED_DEVIANCE && + Math.abs(z - comp.z()) <= ALLOWED_DEVIANCE && + Math.abs(w - comp.w()) <= ALLOWED_DEVIANCE; } @Override public final int hashCode() { diff --git a/src/jogl/classes/com/jogamp/opengl/math/Ray.java b/src/jogl/classes/com/jogamp/opengl/math/Ray.java index a528f0763..25a7d9a70 100644 --- a/src/jogl/classes/com/jogamp/opengl/math/Ray.java +++ b/src/jogl/classes/com/jogamp/opengl/math/Ray.java @@ -47,14 +47,14 @@ import com.jogamp.opengl.math.geom.AABBox; *

*/ public class Ray { - /** Origin of Ray, float[3]. */ - public final float[] orig = new float[3]; + /** Origin of Ray. */ + public final Vec3f orig = new Vec3f(); - /** Normalized direction vector of ray, float[3]. */ - public final float[] dir = new float[3]; + /** Normalized direction vector of ray. */ + public final Vec3f dir = new Vec3f(); @Override public String toString() { - return "Ray[orig["+orig[0]+", "+orig[1]+", "+orig[2]+"], dir["+dir[0]+", "+dir[1]+", "+dir[2]+"]]"; + return "Ray[orig["+orig+"], dir["+dir+"]]"; } } \ No newline at end of file diff --git a/src/jogl/classes/com/jogamp/opengl/math/Vec2f.java b/src/jogl/classes/com/jogamp/opengl/math/Vec2f.java index 0b67102fb..0c7854216 100644 --- a/src/jogl/classes/com/jogamp/opengl/math/Vec2f.java +++ b/src/jogl/classes/com/jogamp/opengl/math/Vec2f.java @@ -48,6 +48,11 @@ public final class Vec2f { set(o); } + /** Creating new Vec2f using Vec3f, dropping z. */ + public Vec2f(final Vec3f o) { + set(o); + } + public Vec2f copy() { return new Vec2f(this); } @@ -66,6 +71,12 @@ public final class Vec2f { this.y = o.y; } + /** this = o while dropping z, returns this. */ + public void set(final Vec3f o) { + this.x = o.x(); + this.y = o.y(); + } + /** this = { x, y }, returns this. */ public void set(final float x, final float y) { this.x = x; @@ -88,6 +99,13 @@ public final class Vec2f { } } + /** xy = this, returns xy. */ + public float[] get(final float[/*2*/] xy) { + xy[0] = this.x; + xy[1] = this.y; + return xy; + } + /** Gets the ith component, 0 <= i < 2 */ public float get(final int i) { switch (i) { @@ -108,10 +126,17 @@ public final class Vec2f { return new Vec2f(this).scale(val); } - /** this = this * val, returns this. */ - public Vec2f scale(final float val) { - x *= val; - y *= val; + /** this = this * s, returns this. */ + public Vec2f scale(final float s) { + x *= s; + y *= s; + return this; + } + + /** this = this * { sx, sy }, returns this. */ + public Vec2f scale(final float sx, final float sy) { + x *= sx; + y *= sy; return this; } @@ -158,6 +183,7 @@ public final class Vec2f { return this; } + /** Return true if all components are zero, i.e. it's absolute value < {@link #EPSILON}. */ public boolean isZero() { return FloatUtil.isZero(x) && FloatUtil.isZero(y); } @@ -282,6 +308,53 @@ public final class Vec2f { return true; } + /** + * Equals check using a given {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}. + *

+ * Implementation considers following corner cases: + *

+ * @param o comparison value + * @param epsilon consider using {@link FloatUtil#EPSILON} + * @return true if all components differ less than {@code epsilon}, otherwise false. + */ + public boolean isEqual(final Vec2f o, final float epsilon) { + if( this == o ) { + return true; + } else { + return FloatUtil.isEqual(x, o.x, epsilon) && + FloatUtil.isEqual(y, o.y, epsilon); + } + } + + /** + * Equals check using {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}. + *

+ * Implementation considers following corner cases: + *

+ * @param o comparison value + * @return true if all components differ less than {@link FloatUtil#EPSILON}, otherwise false. + */ + public boolean isEqual(final Vec2f o) { + return isEqual(o, FloatUtil.EPSILON); + } + + @Override + public boolean equals(final Object o) { + if( o instanceof Vec2f ) { + return isEqual((Vec2f)o, FloatUtil.EPSILON); + } else { + return false; + } + } + @Override public String toString() { return x + " / " + y; diff --git a/src/jogl/classes/com/jogamp/opengl/math/Vec3f.java b/src/jogl/classes/com/jogamp/opengl/math/Vec3f.java index 9cead54c2..d5c725ad7 100644 --- a/src/jogl/classes/com/jogamp/opengl/math/Vec3f.java +++ b/src/jogl/classes/com/jogamp/opengl/math/Vec3f.java @@ -35,6 +35,12 @@ package com.jogamp.opengl.math; * and its data layout from JOAL's Vec3f. */ public final class Vec3f { + public static final Vec3f ONE = new Vec3f(VectorUtil.VEC3_ONE); + public static final Vec3f UNIT_Y = new Vec3f(VectorUtil.VEC3_UNIT_Y); + public static final Vec3f UNIT_Y_NEG = new Vec3f(VectorUtil.VEC3_UNIT_Y_NEG); + public static final Vec3f UNIT_Z = new Vec3f(VectorUtil.VEC3_UNIT_Z); + public static final Vec3f UNIT_Z_NEG = new Vec3f(VectorUtil.VEC3_UNIT_Z_NEG); + private float x; private float y; private float z; @@ -45,6 +51,16 @@ public final class Vec3f { set(o); } + /** Creating new Vec3f using Vec4f, dropping w. */ + public Vec3f(final Vec4f o) { + set(o); + } + + /** Creating new Vec3f using { Vec2f, z}. */ + public Vec3f(final Vec2f o, final float z) { + set(o, z); + } + public Vec3f copy() { return new Vec3f(this); } @@ -65,6 +81,22 @@ public final class Vec3f { return this; } + /** this = { o, z }, returns this. */ + public Vec3f set(final Vec2f o, final float z) { + this.x = o.x(); + this.y = o.y(); + this.z = z; + return this; + } + + /** this = o while dropping w, returns this. */ + public Vec3f set(final Vec4f o) { + this.x = o.x(); + this.y = o.y(); + this.z = o.z(); + return this; + } + /** this = { x, y, z }, returns this. */ public Vec3f set(final float x, final float y, final float z) { this.x = x; @@ -91,6 +123,14 @@ public final class Vec3f { } } + /** xyz = this, returns xyz. */ + public float[] get(final float[/*3*/] xyz) { + xyz[0] = this.x; + xyz[1] = this.y; + xyz[2] = this.z; + return xyz; + } + /** Gets the ith component, 0 <= i < 3 */ public float get(final int i) { switch (i) { @@ -114,11 +154,19 @@ public final class Vec3f { return new Vec3f(this).scale(val); } - /** this = this * val, returns this. */ - public Vec3f scale(final float val) { - x *= val; - y *= val; - z *= val; + /** this = this * s, returns this. */ + public Vec3f scale(final float s) { + x *= s; + y *= s; + z *= s; + return this; + } + + /** this = this * { sx, sy, sz }, returns this. */ + public Vec3f scale(final float sx, final float sy, final float sz) { + x *= sx; + y *= sy; + z *= sz; return this; } @@ -169,7 +217,8 @@ public final class Vec3f { return this; } - public boolean is_zero() { + /** Return true if all components are zero, i.e. it's absolute value < {@link #EPSILON}. */ + public boolean isZero() { return FloatUtil.isZero(x) && FloatUtil.isZero(y) && FloatUtil.isZero(z); } @@ -266,8 +315,57 @@ public final class Vec3f { public boolean intersects(final Vec3f o) { if( Math.abs(x-o.x) >= FloatUtil.EPSILON || Math.abs(y-o.y) >= FloatUtil.EPSILON || Math.abs(z-o.z) >= FloatUtil.EPSILON ) { return false; + } else { + return true; + } + } + + /** + * Equals check using a given {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}. + *

+ * Implementation considers following corner cases: + *

+ * @param o comparison value + * @param epsilon consider using {@link FloatUtil#EPSILON} + * @return true if all components differ less than {@code epsilon}, otherwise false. + */ + public boolean isEqual(final Vec3f o, final float epsilon) { + if( this == o ) { + return true; + } else { + return FloatUtil.isEqual(x, o.x, epsilon) && + FloatUtil.isEqual(y, o.y, epsilon) && + FloatUtil.isEqual(z, o.z, epsilon); + } + } + + /** + * Equals check using {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}. + *

+ * Implementation considers following corner cases: + *

+ * @param o comparison value + * @return true if all components differ less than {@link FloatUtil#EPSILON}, otherwise false. + */ + public boolean isEqual(final Vec3f o) { + return isEqual(o, FloatUtil.EPSILON); + } + + @Override + public boolean equals(final Object o) { + if( o instanceof Vec3f ) { + return isEqual((Vec3f)o, FloatUtil.EPSILON); + } else { + return false; } - return true; } @Override diff --git a/src/jogl/classes/com/jogamp/opengl/math/Vec4f.java b/src/jogl/classes/com/jogamp/opengl/math/Vec4f.java new file mode 100644 index 000000000..1a20015a9 --- /dev/null +++ b/src/jogl/classes/com/jogamp/opengl/math/Vec4f.java @@ -0,0 +1,348 @@ +/** + * Copyright 2022-2023 JogAmp Community. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are + * permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of + * conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list + * of conditions and the following disclaimer in the documentation and/or other materials + * provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * The views and conclusions contained in the software and documentation are those of the + * authors and should not be interpreted as representing official policies, either expressed + * or implied, of JogAmp Community. + */ + +package com.jogamp.opengl.math; + +/** + * 4D Vector based upon four float components. + * + * Implementation borrowed from [gfxbox2](https://jausoft.com/cgit/cs_class/gfxbox2.git/tree/include/pixel/pixel3f.hpp#n29) + * and its data layout from JOAL's Vec3f. + */ +public final class Vec4f { + private float x; + private float y; + private float z; + private float w; + + public Vec4f() {} + + public Vec4f(final Vec4f o) { + set(o); + } + + /** Creating new Vec4f using { o, w }. */ + public Vec4f(final Vec3f o, final float w) { + set(o, w); + } + + public Vec4f copy() { + return new Vec4f(this); + } + + public Vec4f(final float[/*4*/] xyzw) { + set(xyzw); + } + + public Vec4f(final float x, final float y, final float z, final float w) { + set(x, y, z, w); + } + + /** this = o, returns this. */ + public Vec4f set(final Vec4f o) { + this.x = o.x; + this.y = o.y; + this.z = o.z; + this.w = o.w; + return this; + } + + /** this = { o, w }, returns this. */ + public Vec4f set(final Vec3f o, final float w) { + this.x = o.x(); + this.y = o.y(); + this.z = o.z(); + this.w = w; + return this; + } + + /** this = { x, y, z, w }, returns this. */ + public Vec4f set(final float x, final float y, final float z, final float w) { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + return this; + } + + /** this = xyzw, returns this. */ + public Vec4f set(final float[/*4*/] xyzw) { + this.x = xyzw[0]; + this.y = xyzw[1]; + this.z = xyzw[2]; + this.w = xyzw[3]; + return this; + } + + /** Sets the ith component, 0 <= i < 4 */ + public void set(final int i, final float val) { + switch (i) { + case 0: x = val; break; + case 1: y = val; break; + case 2: z = val; break; + case 3: w = val; break; + default: throw new IndexOutOfBoundsException(); + } + } + + /** xyzw = this, returns xyzw. */ + public float[] get(final float[/*4*/] xyzw) { + xyzw[0] = this.x; + xyzw[1] = this.y; + xyzw[2] = this.z; + xyzw[3] = this.w; + return xyzw; + } + + /** Gets the ith component, 0 <= i < 4 */ + public float get(final int i) { + switch (i) { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return w; + default: throw new IndexOutOfBoundsException(); + } + } + + public float x() { return x; } + public float y() { return y; } + public float z() { return z; } + public float w() { return w; } + + public void setX(final float x) { this.x = x; } + public void setY(final float y) { this.y = y; } + public void setZ(final float z) { this.z = z; } + public void setW(final float w) { this.w = w; } + + /** Returns this * val; creates new vector */ + public Vec4f mul(final float val) { + return new Vec4f(this).scale(val); + } + + /** this = this * s, returns this. */ + public Vec4f scale(final float s) { + x *= s; + y *= s; + z *= s; + w *= s; + return this; + } + + /** this = this * { sx, sy, sz, sw }, returns this. */ + public Vec4f scale(final float sx, final float sy, final float sz, final float sw) { + x *= sx; + y *= sy; + z *= sz; + w *= sw; + return this; + } + + /** Returns this + arg; creates new vector */ + public Vec4f plus(final Vec4f arg) { + return new Vec4f(this).add(arg); + } + + /** this = this + { dx, dy, dz, dw }, returns this. */ + public Vec4f add(final float dx, final float dy, final float dz, final float dw) { + x += dx; + y += dy; + z += dz; + w += dw; + return this; + } + + /** this = this + b, returns this. */ + public Vec4f add(final Vec4f b) { + x += b.x; + y += b.y; + z += b.z; + w += b.w; + return this; + } + + /** Returns this - arg; creates new vector */ + public Vec4f minus(final Vec4f arg) { + return new Vec4f(this).sub(arg); + } + + /** this = this - b, returns this. */ + public Vec4f sub(final Vec4f b) { + x -= b.x; + y -= b.y; + z -= b.z; + w -= b.w; + return this; + } + + /** Return true if all components are zero, i.e. it's absolute value < {@link #EPSILON}. */ + public boolean isZero() { + return FloatUtil.isZero(x) && FloatUtil.isZero(y) && FloatUtil.isZero(z) && FloatUtil.isZero(w); + } + + /** + * Return the length of this vector, a.k.a the norm or magnitude + */ + public float length() { + return (float) Math.sqrt(lengthSq()); + } + + /** + * Return the squared length of this vector, a.k.a the squared norm or squared magnitude + */ + public float lengthSq() { + return x*x + y*y + z*z + w*w; + } + + /** + * Normalize this vector in place + */ + public Vec4f normalize() { + final float lengthSq = lengthSq(); + if ( FloatUtil.isZero( lengthSq ) ) { + x = 0.0f; + y = 0.0f; + z = 0.0f; + w = 0.0f; + } else { + final float invSqr = 1.0f / (float)Math.sqrt(lengthSq); + x *= invSqr; + y *= invSqr; + z *= invSqr; + w *= invSqr; + } + return this; + } + + /** + * Return the squared distance between this vector and the given one. + *

+ * When comparing the relative distance between two points it is usually sufficient to compare the squared + * distances, thus avoiding an expensive square root operation. + *

+ */ + public float distSq(final Vec4f o) { + final float dx = x - o.x; + final float dy = y - o.y; + final float dz = z - o.z; + final float dw = w - o.w; + return dx*dx + dy*dy + dz*dz + dw*dw; + } + + /** + * Return the distance between this vector and the given one. + */ + public float dist(final Vec4f o) { + return (float)Math.sqrt(distSq(o)); + } + + + /** + * Return the dot product of this vector and the given one + * @return the dot product as float + */ + public float dot(final Vec4f o) { + return x*o.x + y*o.y + z*o.z + w*o.w; + } + + /** + * Return the cosines of the angle between two vectors + */ + public float cosAngle(final Vec4f o) { + return dot(o) / ( length() * o.length() ) ; + } + + /** + * Return the angle between two vectors in radians + */ + public float angle(final Vec4f o) { + return (float) Math.acos( cosAngle(o) ); + } + + public boolean intersects(final Vec4f o) { + if( Math.abs(x-o.x) >= FloatUtil.EPSILON || Math.abs(y-o.y) >= FloatUtil.EPSILON || Math.abs(z-o.z) >= FloatUtil.EPSILON || + Math.abs(w-o.w) >= FloatUtil.EPSILON) { + return false; + } + return true; + } + + /** + * Equals check using a given {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}. + *

+ * Implementation considers following corner cases: + *

+ * @param o comparison value + * @param epsilon consider using {@link FloatUtil#EPSILON} + * @return true if all components differ less than {@code epsilon}, otherwise false. + */ + public boolean isEqual(final Vec4f o, final float epsilon) { + if( this == o ) { + return true; + } else { + return FloatUtil.isEqual(x, o.x, epsilon) && + FloatUtil.isEqual(y, o.y, epsilon) && + FloatUtil.isEqual(z, o.z, epsilon) && + FloatUtil.isEqual(w, o.w, epsilon); + } + } + + /** + * Equals check using {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}. + *

+ * Implementation considers following corner cases: + *

+ * @param o comparison value + * @return true if all components differ less than {@link FloatUtil#EPSILON}, otherwise false. + */ + public boolean isEqual(final Vec4f o) { + return isEqual(o, FloatUtil.EPSILON); + } + + @Override + public boolean equals(final Object o) { + if( o instanceof Vec4f ) { + return isEqual((Vec4f)o, FloatUtil.EPSILON); + } else { + return false; + } + } + + @Override + public String toString() { + return x + " / " + y + " / " + z + " / " + w; + } +} diff --git a/src/jogl/classes/com/jogamp/opengl/math/VectorUtil.java b/src/jogl/classes/com/jogamp/opengl/math/VectorUtil.java index a07153155..8edbd0cd7 100644 --- a/src/jogl/classes/com/jogamp/opengl/math/VectorUtil.java +++ b/src/jogl/classes/com/jogamp/opengl/math/VectorUtil.java @@ -1,5 +1,5 @@ /** - * Copyright 2010 JogAmp Community. All rights reserved. + * Copyright 2010-2023 JogAmp Community. All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, are * permitted provided that the following conditions are met: @@ -34,7 +34,6 @@ import com.jogamp.graph.geom.plane.Winding; public final class VectorUtil { public static final float[] VEC3_ONE = { 1f, 1f, 1f }; - public static final float[] VEC3_ZERO = { 0f, 0f, 0f }; public static final float[] VEC3_UNIT_Y = { 0f, 1f, 0f }; public static final float[] VEC3_UNIT_Y_NEG = { 0f, -1f, 0f }; public static final float[] VEC3_UNIT_Z = { 0f, 0f, 1f }; @@ -1025,14 +1024,15 @@ public final class VectorUtil { * @param epsilon * @return resulting intersecting if exists, otherwise null */ - public static float[] line2PlaneIntersection(final float[] result, final Ray ray, final float[/*4*/] plane, final float epsilon) { - final float tmp = dotVec3(ray.dir, plane) ; + public static Vec3f line2PlaneIntersection(final Vec3f result, final Ray ray, final Vec4f plane, final float epsilon) { + final Vec3f plane3 = new Vec3f(plane); + final float tmp = ray.dir.dot(plane3); if ( Math.abs(tmp) < epsilon ) { return null; // ray is parallel to plane } - scaleVec3 ( result, ray.dir, -( dotVec3(ray.orig, plane) + plane[3] ) / tmp ) ; - return addVec3(result, result, ray.orig); + result.set( ray.dir ); + return result.scale( -( ray.orig.dot(plane3) + plane.w() ) / tmp ).add(ray.orig); } /** Compute intersection between two segments diff --git a/src/jogl/classes/com/jogamp/opengl/math/geom/AABBox.java b/src/jogl/classes/com/jogamp/opengl/math/geom/AABBox.java index f858b1c0d..9e8edfbf0 100644 --- a/src/jogl/classes/com/jogamp/opengl/math/geom/AABBox.java +++ b/src/jogl/classes/com/jogamp/opengl/math/geom/AABBox.java @@ -1,5 +1,5 @@ /** - * Copyright 2010 JogAmp Community. All rights reserved. + * Copyright 2010-2023 JogAmp Community. All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, are * permitted provided that the following conditions are met: @@ -29,9 +29,11 @@ package com.jogamp.opengl.math.geom; import com.jogamp.graph.geom.plane.AffineTransform; import com.jogamp.opengl.math.FloatUtil; +import com.jogamp.opengl.math.Matrix4f; import com.jogamp.opengl.math.Quaternion; import com.jogamp.opengl.math.Ray; -import com.jogamp.opengl.math.VectorUtil; +import com.jogamp.opengl.math.Vec3f; +import com.jogamp.opengl.util.PMVMatrix; /** @@ -51,9 +53,9 @@ import com.jogamp.opengl.math.VectorUtil; */ public class AABBox { private static final boolean DEBUG = FloatUtil.DEBUG; - private final float[] low = new float[3]; - private final float[] high = new float[3]; - private final float[] center = new float[3]; + private final Vec3f low = new Vec3f(); + private final Vec3f high = new Vec3f(); + private final Vec3f center = new Vec3f(); /** * Create an Axis Aligned bounding box (AABBox) with the @@ -109,42 +111,34 @@ public class AABBox { public final AABBox reset() { setLow(Float.MAX_VALUE,Float.MAX_VALUE,Float.MAX_VALUE); setHigh(-1*Float.MAX_VALUE,-1*Float.MAX_VALUE,-1*Float.MAX_VALUE); - center[0] = 0f; - center[1] = 0f; - center[2] = 0f; + center.set( 0f, 0f, 0f); return this; } /** Get the max xyz-coordinates - * @return a float array containing the max xyz coordinates + * @return max xyz coordinates */ - public final float[] getHigh() { + public final Vec3f getHigh() { return high; } private final void setHigh(final float hx, final float hy, final float hz) { - this.high[0] = hx; - this.high[1] = hy; - this.high[2] = hz; + this.high.set(hx, hy, hz); } /** Get the min xyz-coordinates - * @return a float array containing the min xyz coordinates + * @return min xyz coordinates */ - public final float[] getLow() { + public final Vec3f getLow() { return low; } private final void setLow(final float lx, final float ly, final float lz) { - this.low[0] = lx; - this.low[1] = ly; - this.low[2] = lz; + this.low.set(lx, ly, lz); } private final void computeCenter() { - center[0] = (high[0] + low[0])/2f; - center[1] = (high[1] + low[1])/2f; - center[2] = (high[2] + low[2])/2f; + center.set(high).add(low).scale(1f/2f); } /** @@ -154,9 +148,9 @@ public class AABBox { * @return this AABBox for chaining */ public final AABBox copy(final AABBox src) { - System.arraycopy(src.low, 0, low, 0, 3); - System.arraycopy(src.high, 0, high, 0, 3); - System.arraycopy(src.center, 0, center, 0, 3); + low.set(src.low); + high.set(src.high); + center.set(src.center); return this; } @@ -186,12 +180,23 @@ public class AABBox { */ public final AABBox setSize(final float lx, final float ly, final float lz, final float hx, final float hy, final float hz) { - this.low[0] = lx; - this.low[1] = ly; - this.low[2] = lz; - this.high[0] = hx; - this.high[1] = hy; - this.high[2] = hz; + this.low.set(lx, ly, lz); + this.high.set(hx, hy, hz); + computeCenter(); + return this; + } + + /** + * Set size of the AABBox specifying the coordinates + * of the low and high. + * + * @param low min xyz-coordinates + * @param high max xyz-coordinates + * @return this AABBox for chaining + */ + public final AABBox setSize(final Vec3f low, final Vec3f high) { + this.low.set(low); + this.high.set(high); computeCenter(); return this; } @@ -202,25 +207,30 @@ public class AABBox { * @return this AABBox for chaining */ public final AABBox resize(final AABBox newBox) { - final float[] newLow = newBox.getLow(); - final float[] newHigh = newBox.getHigh(); + final Vec3f newLow = newBox.getLow(); + final Vec3f newHigh = newBox.getHigh(); /** test low */ - if (newLow[0] < low[0]) - low[0] = newLow[0]; - if (newLow[1] < low[1]) - low[1] = newLow[1]; - if (newLow[2] < low[2]) - low[2] = newLow[2]; + if (newLow.x() < low.x()) { + low.setX( newLow.x() ); + } + if (newLow.y() < low.y()) { + low.setY( newLow.y() ); + } + if (newLow.z() < low.z()) { + low.setZ( newLow.z() ); + } /** test high */ - if (newHigh[0] > high[0]) - high[0] = newHigh[0]; - if (newHigh[1] > high[1]) - high[1] = newHigh[1]; - if (newHigh[2] > high[2]) - high[2] = newHigh[2]; - + if (newHigh.x() > high.x()) { + high.setX( newHigh.x() ); + } + if (newHigh.y() > high.y()) { + high.setY( newHigh.y() ); + } + if (newHigh.z() > high.z()) { + high.setZ( newHigh.z() ); + } computeCenter(); return this; } @@ -229,34 +239,32 @@ public class AABBox { * Resize the AABBox to encapsulate another AABox, which will be transformed on the fly first. * @param newBox AABBox to be encapsulated in * @param t the {@link AffineTransform} applied on newBox on the fly - * @param tmpV3 temp float[3] storage + * @param tmpV3 temporary storage * @return this AABBox for chaining */ - public final AABBox resize(final AABBox newBox, final AffineTransform t, final float[] tmpV3) { + public final AABBox resize(final AABBox newBox, final AffineTransform t, final Vec3f tmpV3) { /** test low */ { - final float[] newBoxLow = newBox.getLow(); + final Vec3f newBoxLow = newBox.getLow(); t.transform(newBoxLow, tmpV3); - tmpV3[2] = newBoxLow[2]; - if (tmpV3[0] < low[0]) - low[0] = tmpV3[0]; - if (tmpV3[1] < low[1]) - low[1] = tmpV3[1]; - if (tmpV3[2] < low[2]) - low[2] = tmpV3[2]; + if (tmpV3.x() < low.x()) + low.setX( tmpV3.x() ); + if (tmpV3.y() < low.y()) + low.setY( tmpV3.y() ); + if (tmpV3.z() < low.z()) + low.setZ( tmpV3.z() ); } /** test high */ { - final float[] newBoxHigh = newBox.getHigh(); + final Vec3f newBoxHigh = newBox.getHigh(); t.transform(newBoxHigh, tmpV3); - tmpV3[2] = newBoxHigh[2]; - if (tmpV3[0] > high[0]) - high[0] = tmpV3[0]; - if (tmpV3[1] > high[1]) - high[1] = tmpV3[1]; - if (tmpV3[2] > high[2]) - high[2] = tmpV3[2]; + if (tmpV3.x() > high.x()) + high.setX( tmpV3.x() ); + if (tmpV3.y() > high.y()) + high.setY( tmpV3.y() ); + if (tmpV3.z() > high.z()) + high.setZ( tmpV3.z() ); } computeCenter(); @@ -273,25 +281,25 @@ public class AABBox { */ public final AABBox resize(final float x, final float y, final float z) { /** test low */ - if (x < low[0]) { - low[0] = x; + if (x < low.x()) { + low.setX( x ); } - if (y < low[1]) { - low[1] = y; + if (y < low.y()) { + low.setY( y ); } - if (z < low[2]) { - low[2] = z; + if (z < low.z()) { + low.setZ( z ); } /** test high */ - if (x > high[0]) { - high[0] = x; + if (x > high.x()) { + high.setX( x ); } - if (y > high[1]) { - high[1] = y; + if (y > high.y()) { + high.setY( y ); } - if (z > high[2]) { - high[2] = z; + if (z > high.z()) { + high.setZ( z ); } computeCenter(); @@ -319,6 +327,16 @@ public class AABBox { return resize(xyz[0], xyz[1], xyz[2]); } + /** + * Resize the AABBox to encapsulate the passed + * xyz-coordinates. + * @param xyz xyz-axis coordinate values + * @return this AABBox for chaining + */ + public final AABBox resize(final Vec3f xyz) { + return resize(xyz.x(), xyz.y(), xyz.z()); + } + /** * Check if the x & y coordinates are bounded/contained * by this AABBox @@ -328,10 +346,10 @@ public class AABBox { * y belong to (low.y, high.y) */ public final boolean contains(final float x, final float y) { - if(xhigh[0]){ + if(xhigh.x()){ return false; } - if(yhigh[1]){ + if(yhigh.y()){ return false; } return true; @@ -347,13 +365,13 @@ public class AABBox { * y belong to (low.y, high.y) and z belong to (low.z, high.z) */ public final boolean contains(final float x, final float y, final float z) { - if(xhigh[0]){ + if(xhigh.x()){ return false; } - if(yhigh[1]){ + if(yhigh.y()){ return false; } - if(zhigh[2]){ + if(zhigh.z()){ return false; } return true; @@ -390,13 +408,13 @@ public class AABBox { /** * Check if {@link Ray} intersects this bounding box. *

- * Versions uses the SAT[1], testing 6 axes. + * Versions uses the SAT.y(), testing 6 axes. * Original code for OBBs from MAGIC. - * Rewritten for AABBs and reorganized for early exits[2]. + * Rewritten for AABBs and reorganized for early exits.z(). *

* 
-     * [1] SAT = Separating Axis Theorem
-     * [2] http://www.codercorner.com/RayAABB.cpp
+     * .y() SAT = Separating Axis Theorem
+     * .z() http://www.codercorner.com/RayAABB.cpp
      *
* @param ray * @return @@ -405,19 +423,19 @@ public class AABBox { // diff[XYZ] -> VectorUtil.subVec3(diff, ray.orig, center); // ext[XYZ] -> extend VectorUtil.subVec3(ext, high, center); - final float dirX = ray.dir[0]; - final float diffX = ray.orig[0] - center[0]; - final float extX = high[0] - center[0]; + final float dirX = ray.dir.x(); + final float diffX = ray.orig.x() - center.x(); + final float extX = high.x() - center.x(); if( Math.abs(diffX) > extX && diffX*dirX >= 0f ) return false; - final float dirY = ray.dir[1]; - final float diffY = ray.orig[1] - center[1]; - final float extY = high[1] - center[1]; + final float dirY = ray.dir.y(); + final float diffY = ray.orig.y() - center.y(); + final float extY = high.y() - center.y(); if( Math.abs(diffY) > extY && diffY*dirY >= 0f ) return false; - final float dirZ = ray.dir[2]; - final float diffZ = ray.orig[2] - center[2]; - final float extZ = high[2] - center[2]; + final float dirZ = ray.dir.z(); + final float diffZ = ray.orig.z() - center.z(); + final float extZ = high.z() - center.z(); if( Math.abs(diffZ) > extZ && diffZ*dirZ >= 0f ) return false; final float absDirY = Math.abs(dirY); @@ -442,7 +460,7 @@ public class AABBox { * or null if none exist. *

*

    - *
  • Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990 [2]
    • + *
  • Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990 .z()
    • *
  • Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)
    • *
  • Epsilon value added by Klaus Hartmann.
    • *
@@ -458,8 +476,8 @@ public class AABBox { * Report bugs: p.terdiman@codercorner.com (original author) *

* 
-     * [1] http://www.codercorner.com/RayAABB.cpp
-     * [2] http://tog.acm.org/resources/GraphicsGems/gems/RayBox.c
+     * .y() http://www.codercorner.com/RayAABB.cpp
+     * .z() http://tog.acm.org/resources/GraphicsGems/gems/RayBox.c
      *
* @param result vec3 * @param ray @@ -467,45 +485,45 @@ public class AABBox { * @param assumeIntersection if true, method assumes an intersection, i.e. by pre-checking via {@link #intersectsRay(Ray)}. * In this case method will not validate a possible non-intersection and just computes * coordinates. - * @param tmp1V3 temp vec3 - * @param tmp2V3 temp vec3 - * @param tmp3V3 temp vec3 * @return float[3] result of intersection coordinates, or null if none exists */ - public final float[] getRayIntersection(final float[] result, final Ray ray, final float epsilon, - final boolean assumeIntersection, - final float[] tmp1V3, final float[] tmp2V3, final float[] tmp3V3) { + public final Vec3f getRayIntersection(final Vec3f result, final Ray ray, final float epsilon, + final boolean assumeIntersection) { final float[] maxT = { -1f, -1f, -1f }; - final float[] origin = ray.orig; - final float[] dir = ray.dir; + final Vec3f origin = ray.orig; + final Vec3f dir = ray.dir; boolean inside = true; // Find candidate planes. for(int i=0; i<3; i++) { - if(origin[i] < low[i]) { - result[i] = low[i]; + final float origin_i = origin.get(i); + final float dir_i = dir.get(i); + final float low_i = low.get(i); + final float high_i = high.get(i); + if(origin_i < low_i) { + result.set(i, low_i); inside = false; // Calculate T distances to candidate planes - if( 0 != Float.floatToIntBits(dir[i]) ) { - maxT[i] = (low[i] - origin[i]) / dir[i]; + if( 0 != Float.floatToIntBits(dir_i) ) { + maxT[i] = (low_i - origin_i) / dir_i; } - } else if(origin[i] > high[i]) { - result[i] = high[i]; + } else if(origin_i > high_i) { + result.set(i, high_i); inside = false; // Calculate T distances to candidate planes - if( 0 != Float.floatToIntBits(dir[i]) ) { - maxT[i] = (high[i] - origin[i]) / dir[i]; + if( 0 != Float.floatToIntBits(dir_i) ) { + maxT[i] = (high_i - origin_i) / dir_i; } } } // Ray origin inside bounding box if(inside) { - System.arraycopy(origin, 0, result, 0, 3); + result.set(origin); return result; } @@ -530,22 +548,22 @@ public class AABBox { } */ switch( whichPlane ) { case 0: - result[1] = origin[1] + maxT[whichPlane] * dir[1]; - if(result[1] < low[1] - epsilon || result[1] > high[1] + epsilon) { return null; } - result[2] = origin[2] + maxT[whichPlane] * dir[2]; - if(result[2] < low[2] - epsilon || result[2] > high[2] + epsilon) { return null; } + result.setY( origin.y() + maxT[whichPlane] * dir.y() ); + if(result.y() < low.y() - epsilon || result.y() > high.y() + epsilon) { return null; } + result.setZ( origin.z() + maxT[whichPlane] * dir.z() ); + if(result.z() < low.z() - epsilon || result.z() > high.z() + epsilon) { return null; } break; case 1: - result[0] = origin[0] + maxT[whichPlane] * dir[0]; - if(result[0] < low[0] - epsilon || result[0] > high[0] + epsilon) { return null; } - result[2] = origin[2] + maxT[whichPlane] * dir[2]; - if(result[2] < low[2] - epsilon || result[2] > high[2] + epsilon) { return null; } + result.setX( origin.x() + maxT[whichPlane] * dir.x() ); + if(result.x() < low.x() - epsilon || result.x() > high.x() + epsilon) { return null; } + result.setZ( origin.z() + maxT[whichPlane] * dir.z() ); + if(result.z() < low.z() - epsilon || result.z() > high.z() + epsilon) { return null; } break; case 2: - result[0] = origin[0] + maxT[whichPlane] * dir[0]; - if(result[0] < low[0] - epsilon || result[0] > high[0] + epsilon) { return null; } - result[1] = origin[1] + maxT[whichPlane] * dir[1]; - if(result[1] < low[1] - epsilon || result[1] > high[1] + epsilon) { return null; } + result.setX( origin.x() + maxT[whichPlane] * dir.x() ); + if(result.x() < low.x() - epsilon || result.x() > high.x() + epsilon) { return null; } + result.setY( origin.y() + maxT[whichPlane] * dir.y() ); + if(result.y() < low.y() - epsilon || result.y() > high.y() + epsilon) { return null; } break; default: throw new InternalError("XXX"); @@ -553,16 +571,16 @@ public class AABBox { } else { switch( whichPlane ) { case 0: - result[1] = origin[1] + maxT[whichPlane] * dir[1]; - result[2] = origin[2] + maxT[whichPlane] * dir[2]; + result.setY( origin.y() + maxT[whichPlane] * dir.y() ); + result.setZ( origin.z() + maxT[whichPlane] * dir.z() ); break; case 1: - result[0] = origin[0] + maxT[whichPlane] * dir[0]; - result[2] = origin[2] + maxT[whichPlane] * dir[2]; + result.setX( origin.x() + maxT[whichPlane] * dir.x() ); + result.setZ( origin.z() + maxT[whichPlane] * dir.z() ); break; case 2: - result[0] = origin[0] + maxT[whichPlane] * dir[0]; - result[1] = origin[1] + maxT[whichPlane] * dir[1]; + result.setX( origin.x() + maxT[whichPlane] * dir.x() ); + result.setY( origin.y() + maxT[whichPlane] * dir.y() ); break; default: throw new InternalError("XXX"); @@ -577,14 +595,14 @@ public class AABBox { * @return a float representing the size of the AABBox */ public final float getSize() { - return VectorUtil.distVec3(low, high); + return low.dist(high); } /** * Get the Center of this AABBox * @return the xyz-coordinates of the center of the AABBox */ - public final float[] getCenter() { + public final Vec3f getCenter() { return center; } @@ -594,24 +612,17 @@ public class AABBox { * high and low is recomputed by scaling its distance to fixed center. *

* @param size a constant float value - * @param tmpV3 caller provided temporary 3-component vector * @return this AABBox for chaining * @see #scale2(float, float[]) */ - public final AABBox scale(final float size, final float[] tmpV3) { - tmpV3[0] = high[0] - center[0]; - tmpV3[1] = high[1] - center[1]; - tmpV3[2] = high[2] - center[2]; - - VectorUtil.scaleVec3(tmpV3, tmpV3, size); // in-place scale - VectorUtil.addVec3(high, center, tmpV3); + public final AABBox scale(final float size) { + final Vec3f tmp = new Vec3f(); + tmp.set(high).sub(center).scale(size); + high.set(center).add(tmp); - tmpV3[0] = low[0] - center[0]; - tmpV3[1] = low[1] - center[1]; - tmpV3[2] = low[2] - center[2]; + tmp.set(low).sub(center).scale(size); + low.set(center).add(tmp); - VectorUtil.scaleVec3(tmpV3, tmpV3, size); // in-place scale - VectorUtil.addVec3(low, center, tmpV3); return this; } @@ -621,13 +632,12 @@ public class AABBox { * high and low is scaled and center recomputed. *

* @param size a constant float value - * @param tmpV3 caller provided temporary 3-component vector * @return this AABBox for chaining * @see #scale(float, float[]) */ - public final AABBox scale2(final float size, final float[] tmpV3) { - VectorUtil.scaleVec3(high, high, size); // in-place scale - VectorUtil.scaleVec3(low, low, size); // in-place scale + public final AABBox scale2(final float size) { + high.scale(size); + low.scale(size); computeCenter(); return this; } @@ -637,9 +647,9 @@ public class AABBox { * @param t the float[3] translation vector * @return this AABBox for chaining */ - public final AABBox translate(final float[] t) { - VectorUtil.addVec3(low, low, t); // in-place translate - VectorUtil.addVec3(high, high, t); // in-place translate + public final AABBox translate(final Vec3f t) { + low.add(t); + high.add(t); computeCenter(); return this; } @@ -650,46 +660,46 @@ public class AABBox { * @return this AABBox for chaining */ public final AABBox rotate(final Quaternion quat) { - quat.rotateVector(low, 0, low, 0); - quat.rotateVector(high, 0, high, 0); + quat.rotateVector(low, low); + quat.rotateVector(high, high); computeCenter(); return this; } public final float getMinX() { - return low[0]; + return low.x(); } public final float getMinY() { - return low[1]; + return low.y(); } public final float getMinZ() { - return low[2]; + return low.z(); } public final float getMaxX() { - return high[0]; + return high.x(); } public final float getMaxY() { - return high[1]; + return high.y(); } public final float getMaxZ() { - return high[2]; + return high.z(); } public final float getWidth(){ - return high[0] - low[0]; + return high.x() - low.x(); } public final float getHeight() { - return high[1] - low[1]; + return high.y() - low.y(); } public final float getDepth() { - return high[2] - low[2]; + return high.z() - low.z(); } @Override @@ -701,29 +711,65 @@ public class AABBox { return false; } final AABBox other = (AABBox) obj; - return VectorUtil.isVec2Equal(low, 0, other.low, 0, FloatUtil.EPSILON) && - VectorUtil.isVec3Equal(high, 0, other.high, 0, FloatUtil.EPSILON) ; + return low.isEqual(other.low) && high.isEqual(other.high); } @Override public final int hashCode() { throw new InternalError("hashCode not designed"); } + public AABBox transform(final AABBox result, final float[/*16*/] mat4, final int mat4_off, + final float[] vec3Tmp0, final float[] vec3Tmp1) { + result.reset(); + FloatUtil.multMatrixVec3(mat4, mat4_off, low.get(vec3Tmp0), vec3Tmp1); + result.resize(vec3Tmp1); + + FloatUtil.multMatrixVec3(mat4, mat4_off, high.get(vec3Tmp0), vec3Tmp1); + result.resize(vec3Tmp1); + + result.computeCenter(); + return result; + } + + public AABBox transformMv(final AABBox result, final PMVMatrix pmv, + final float[] vec3Tmp0, final float[] vec3Tmp1) { + result.reset(); + pmv.multMvMatVec3f(low.get(vec3Tmp0), vec3Tmp1); + result.resize(vec3Tmp1); + + pmv.multMvMatVec3f(high.get(vec3Tmp0), vec3Tmp1); + result.resize(vec3Tmp1); + + result.computeCenter(); + return result; + } + + public AABBox transform(final AABBox result, final Matrix4f mat, + final Vec3f vec3Tmp) { + result.reset(); + result.resize( mat.mulVec3f(low, vec3Tmp) ); + + result.resize( mat.mulVec3f(high, vec3Tmp) ); + + result.computeCenter(); + return result; + } + /** * Assume this bounding box as being in object space and * compute the window bounding box. *

* If useCenterZ is true, - * only 4 {@link FloatUtil#mapObjToWinCoords(float, float, float, float[], int[], int, float[], int, float[], float[]) mapObjToWinCoords} + * only 4 {@link FloatUtil#mapObjToWin(float, float, float, float[], int[], float[], float[], float[]) mapObjToWinCoords} * operations are made on points [1..4] using {@link #getCenter()}'s z-value. - * Otherwise 8 {@link FloatUtil#mapObjToWinCoords(float, float, float, float[], int[], int, float[], int, float[], float[]) mapObjToWinCoords} + * Otherwise 8 {@link FloatUtil#mapObjToWin(float, float, float, float[], int[], float[], float[], float[]) mapObjToWinCoords} * operation on all 8 points are performed. *

* 
-     *  [2] ------ [4]
+     *  .z() ------ [4]
      *   |          |
      *   |          |
-     *  [1] ------ [3]
+     *  .y() ------ [3]
      *
* @param mat4PMv P x Mv matrix * @param view @@ -736,42 +782,42 @@ public class AABBox { public AABBox mapToWindow(final AABBox result, final float[/*16*/] mat4PMv, final int[] view, final boolean useCenterZ, final float[] vec3Tmp0, final float[] vec4Tmp1, final float[] vec4Tmp2) { { - // System.err.printf("AABBox.mapToWindow.0: view[%d, %d, %d, %d], this %s%n", view[0], view[1], view[2], view[3], toString()); - final float objZ = useCenterZ ? center[2] : getMinZ(); - FloatUtil.mapObjToWinCoords(getMinX(), getMinY(), objZ, mat4PMv, view, 0, vec3Tmp0, 0, vec4Tmp1, vec4Tmp2); - // System.err.printf("AABBox.mapToWindow.p1: %f, %f, %f -> %f, %f, %f%n", getMinX(), getMinY(), objZ, vec3Tmp0[0], vec3Tmp0[1], vec3Tmp0[2]); + // System.err.printf("AABBox.mapToWindow.0: view[%d, %d, %d, %d], this %s%n", view.x(), view.y(), view.z(), view[3], toString()); + final float objZ = useCenterZ ? center.z() : getMinZ(); + FloatUtil.mapObjToWin(getMinX(), getMinY(), objZ, mat4PMv, view, vec3Tmp0, vec4Tmp1, vec4Tmp2); + // System.err.printf("AABBox.mapToWindow.p1: %f, %f, %f -> %f, %f, %f%n", getMinX(), getMinY(), objZ, vec3Tmp0.x(), vec3Tmp0.y(), vec3Tmp0.z()); // System.err.println("AABBox.mapToWindow.p1:"); // System.err.println(FloatUtil.matrixToString(null, " mat4PMv", "%10.5f", mat4PMv, 0, 4, 4, false /* rowMajorOrder */)); result.reset(); - result.resize(vec3Tmp0, 0); + result.resize(vec3Tmp0); - FloatUtil.mapObjToWinCoords(getMinX(), getMaxY(), objZ, mat4PMv, view, 0, vec3Tmp0, 0, vec4Tmp1, vec4Tmp2); - // System.err.printf("AABBox.mapToWindow.p2: %f, %f, %f -> %f, %f, %f%n", getMinX(), getMaxY(), objZ, vec3Tmp0[0], vec3Tmp0[1], vec3Tmp0[2]); - result.resize(vec3Tmp0, 0); + FloatUtil.mapObjToWin(getMinX(), getMaxY(), objZ, mat4PMv, view, vec3Tmp0, vec4Tmp1, vec4Tmp2); + // System.err.printf("AABBox.mapToWindow.p2: %f, %f, %f -> %f, %f, %f%n", getMinX(), getMaxY(), objZ, vec3Tmp0.x(), vec3Tmp0.y(), vec3Tmp0.z()); + result.resize(vec3Tmp0); - FloatUtil.mapObjToWinCoords(getMaxX(), getMinY(), objZ, mat4PMv, view, 0, vec3Tmp0, 0, vec4Tmp1, vec4Tmp2); - // System.err.printf("AABBox.mapToWindow.p3: %f, %f, %f -> %f, %f, %f%n", getMaxX(), getMinY(), objZ, vec3Tmp0[0], vec3Tmp0[1], vec3Tmp0[2]); - result.resize(vec3Tmp0, 0); + FloatUtil.mapObjToWin(getMaxX(), getMinY(), objZ, mat4PMv, view, vec3Tmp0, vec4Tmp1, vec4Tmp2); + // System.err.printf("AABBox.mapToWindow.p3: %f, %f, %f -> %f, %f, %f%n", getMaxX(), getMinY(), objZ, vec3Tmp0.x(), vec3Tmp0.y(), vec3Tmp0.z()); + result.resize(vec3Tmp0); - FloatUtil.mapObjToWinCoords(getMaxX(), getMaxY(), objZ, mat4PMv, view, 0, vec3Tmp0, 0, vec4Tmp1, vec4Tmp2); - // System.err.printf("AABBox.mapToWindow.p4: %f, %f, %f -> %f, %f, %f%n", getMaxX(), getMaxY(), objZ, vec3Tmp0[0], vec3Tmp0[1], vec3Tmp0[2]); - result.resize(vec3Tmp0, 0); + FloatUtil.mapObjToWin(getMaxX(), getMaxY(), objZ, mat4PMv, view, vec3Tmp0, vec4Tmp1, vec4Tmp2); + // System.err.printf("AABBox.mapToWindow.p4: %f, %f, %f -> %f, %f, %f%n", getMaxX(), getMaxY(), objZ, vec3Tmp0.x(), vec3Tmp0.y(), vec3Tmp0.z()); + result.resize(vec3Tmp0); } if( !useCenterZ ) { final float objZ = getMaxZ(); - FloatUtil.mapObjToWinCoords(getMinX(), getMinY(), objZ, mat4PMv, view, 0, vec3Tmp0, 0, vec4Tmp1, vec4Tmp2); - result.resize(vec3Tmp0, 0); + FloatUtil.mapObjToWin(getMinX(), getMinY(), objZ, mat4PMv, view, vec3Tmp0, vec4Tmp1, vec4Tmp2); + result.resize(vec3Tmp0); - FloatUtil.mapObjToWinCoords(getMinX(), getMaxY(), objZ, mat4PMv, view, 0, vec3Tmp0, 0, vec4Tmp1, vec4Tmp2); - result.resize(vec3Tmp0, 0); + FloatUtil.mapObjToWin(getMinX(), getMaxY(), objZ, mat4PMv, view, vec3Tmp0, vec4Tmp1, vec4Tmp2); + result.resize(vec3Tmp0); - FloatUtil.mapObjToWinCoords(getMaxX(), getMinY(), objZ, mat4PMv, view, 0, vec3Tmp0, 0, vec4Tmp1, vec4Tmp2); - result.resize(vec3Tmp0, 0); + FloatUtil.mapObjToWin(getMaxX(), getMinY(), objZ, mat4PMv, view, vec3Tmp0, vec4Tmp1, vec4Tmp2); + result.resize(vec3Tmp0); - FloatUtil.mapObjToWinCoords(getMaxX(), getMaxY(), objZ, mat4PMv, view, 0, vec3Tmp0, 0, vec4Tmp1, vec4Tmp2); - result.resize(vec3Tmp0, 0); + FloatUtil.mapObjToWin(getMaxX(), getMaxY(), objZ, mat4PMv, view, vec3Tmp0, vec4Tmp1, vec4Tmp2); + result.resize(vec3Tmp0); } if( DEBUG ) { System.err.printf("AABBox.mapToWindow: view[%d, %d], this %s -> %s%n", view[0], view[1], toString(), result.toString()); @@ -782,7 +828,6 @@ public class AABBox { @Override public final String toString() { return "[ dim "+getWidth()+" x "+getHeight()+" x "+getDepth()+ - ", box "+low[0]+" / "+low[1]+" / "+low[2]+" .. "+high[0]+" / "+high[1]+" / "+high[2]+ - ", ctr "+center[0]+" / "+center[1]+" / "+center[2]+" ]"; + ", box "+low+" .. "+high+", ctr "+center+" ]"; } } diff --git a/src/jogl/classes/com/jogamp/opengl/math/geom/Frustum.java b/src/jogl/classes/com/jogamp/opengl/math/geom/Frustum.java index 8b0fa559e..4d098cb72 100644 --- a/src/jogl/classes/com/jogamp/opengl/math/geom/Frustum.java +++ b/src/jogl/classes/com/jogamp/opengl/math/geom/Frustum.java @@ -1,5 +1,5 @@ /** - * Copyright 2010 JogAmp Community. All rights reserved. + * Copyright 2010-2023 JogAmp Community. All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, are * permitted provided that the following conditions are met: @@ -29,9 +29,11 @@ package com.jogamp.opengl.math.geom; import jogamp.common.os.PlatformPropsImpl; -import com.jogamp.common.os.Platform; import com.jogamp.opengl.math.FloatUtil; import com.jogamp.opengl.math.FovHVHalves; +import com.jogamp.opengl.math.Matrix4f; +import com.jogamp.opengl.math.Vec3f; +import com.jogamp.opengl.math.geom.Frustum.FovDesc; /** * Providing frustum {@link #getPlanes() planes} derived by different inputs @@ -103,6 +105,7 @@ public class Frustum { this.zNear = zNear; this.zFar = zFar; } + @Override public final String toString() { return "FrustumFovDesc["+fovhv.toStringInDegrees()+", Z["+zNear+" - "+zFar+"]]"; } @@ -134,7 +137,7 @@ public class Frustum { */ public static class Plane { /** Normal of the plane */ - public final float[] n = new float[3]; + public final Vec3f n = new Vec3f(); /** Distance to origin */ public float d; @@ -155,17 +158,22 @@ public class Frustum { *

**/ public final float distanceTo(final float x, final float y, final float z) { - return n[0] * x + n[1] * y + n[2] * z + d; + return n.x() * x + n.y() * y + n.z() * z + d; } /** Return distance of plane to given point, see {@link #distanceTo(float, float, float)}. */ public final float distanceTo(final float[] p) { - return n[0] * p[0] + n[1] * p[1] + n[2] * p[2] + d; + return n.x() * p[0] + n.y() * p[1] + n.z() * p[2] + d; + } + + /** Return distance of plane to given point, see {@link #distanceTo(float, float, float)}. */ + public final float distanceTo(final Vec3f p) { + return n.x() * p.x() + n.y() * p.y() + n.z() * p.z() + d; } @Override public String toString() { - return "Plane[ [ " + n[0] + ", " + n[1] + ", " + n[2] + " ], " + d + "]"; + return "Plane[ [ " + n + " ], " + d + "]"; } } @@ -221,9 +229,9 @@ public class Frustum { * Operation Details: *
    *
  • The given {@link FovDesc} will be transformed - * into the given float[16] as a perspective matrix (column major order) first, - * see {@link FloatUtil#makePerspective(float[], int, boolean, FovHVHalves, float, float)}.
    • - *
  • Then the float[16] perspective matrix is used to {@link #updateByPMV(float[], int)} this instance.
    • + * into the given perspective matrix (column major order) first, + * see {@link Matrix4f#setToPerspective(FovHVHalves, float, float)}. + *
  • Then the perspective matrix is used to {@link Matrix4f#updateFrustumPlanes(Frustum)} this instance.
    • *
*

*

@@ -232,17 +240,15 @@ public class Frustum { *

* * @param m 4x4 matrix in column-major order (also result) - * @param m_offset offset in given array m, i.e. start of the 4x4 matrix - * @param initM if true, given matrix will be initialized w/ identity matrix, - * otherwise only the frustum fields are set. * @param fovDesc {@link Frustum} {@link FovDesc} * @return given matrix for chaining - * @see FloatUtil#makePerspective(float[], int, boolean, FovHVHalves, float, float) + * @see Matrix4f#setToPerspective(FovHVHalves, float, float) + * @see Matrix4f#updateFrustumPlanes(Frustum) + * @see Matrix4f#getFrustum(Frustum, FovDesc) */ - public float[] updateByFovDesc(final float[] m, final int m_offset, final boolean initM, - final FovDesc fovDesc) { - FloatUtil.makePerspective(m, m_offset, initM, fovDesc.fovhv, fovDesc.zNear, fovDesc.zFar); - updateByPMV(m, 0); + public Matrix4f updateByFovDesc(final Matrix4f m, final FovDesc fovDesc) { + m.setToPerspective(fovDesc.fovhv, fovDesc.zNear, fovDesc.zFar); + m.updateFrustumPlanes(this); return m; } @@ -259,10 +265,10 @@ public class Frustum { // Left: a = m30 + m00, b = m31 + m01, c = m32 + m02, d = m33 + m03 - [0..3] column-major { final Plane p = planes[LEFT]; - final float[] p_n = p.n; - p_n[0] = pmv[ pmv_off + 3 + 0 * 4 ] + pmv[ pmv_off + 0 + 0 * 4 ]; - p_n[1] = pmv[ pmv_off + 3 + 1 * 4 ] + pmv[ pmv_off + 0 + 1 * 4 ]; - p_n[2] = pmv[ pmv_off + 3 + 2 * 4 ] + pmv[ pmv_off + 0 + 2 * 4 ]; + final Vec3f p_n = p.n; + p_n.set( pmv[ pmv_off + 3 + 0 * 4 ] + pmv[ pmv_off + 0 + 0 * 4 ], + pmv[ pmv_off + 3 + 1 * 4 ] + pmv[ pmv_off + 0 + 1 * 4 ], + pmv[ pmv_off + 3 + 2 * 4 ] + pmv[ pmv_off + 0 + 2 * 4 ] ); p.d = pmv[ pmv_off + 3 + 3 * 4 ] + pmv[ pmv_off + 0 + 3 * 4 ]; } @@ -270,10 +276,10 @@ public class Frustum { // Right: a = m30 - m00, b = m31 - m01, c = m32 - m02, d = m33 - m03 - [0..3] column-major { final Plane p = planes[RIGHT]; - final float[] p_n = p.n; - p_n[0] = pmv[ pmv_off + 3 + 0 * 4 ] - pmv[ pmv_off + 0 + 0 * 4 ]; - p_n[1] = pmv[ pmv_off + 3 + 1 * 4 ] - pmv[ pmv_off + 0 + 1 * 4 ]; - p_n[2] = pmv[ pmv_off + 3 + 2 * 4 ] - pmv[ pmv_off + 0 + 2 * 4 ]; + final Vec3f p_n = p.n; + p_n.set( pmv[ pmv_off + 3 + 0 * 4 ] - pmv[ pmv_off + 0 + 0 * 4 ], + pmv[ pmv_off + 3 + 1 * 4 ] - pmv[ pmv_off + 0 + 1 * 4 ], + pmv[ pmv_off + 3 + 2 * 4 ] - pmv[ pmv_off + 0 + 2 * 4 ] ); p.d = pmv[ pmv_off + 3 + 3 * 4 ] - pmv[ pmv_off + 0 + 3 * 4 ]; } @@ -281,10 +287,10 @@ public class Frustum { // Bottom: a = m30 + m10, b = m31 + m11, c = m32 + m12, d = m33 + m13 - [0..3] column-major { final Plane p = planes[BOTTOM]; - final float[] p_n = p.n; - p_n[0] = pmv[ pmv_off + 3 + 0 * 4 ] + pmv[ pmv_off + 1 + 0 * 4 ]; - p_n[1] = pmv[ pmv_off + 3 + 1 * 4 ] + pmv[ pmv_off + 1 + 1 * 4 ]; - p_n[2] = pmv[ pmv_off + 3 + 2 * 4 ] + pmv[ pmv_off + 1 + 2 * 4 ]; + final Vec3f p_n = p.n; + p_n.set( pmv[ pmv_off + 3 + 0 * 4 ] + pmv[ pmv_off + 1 + 0 * 4 ], + pmv[ pmv_off + 3 + 1 * 4 ] + pmv[ pmv_off + 1 + 1 * 4 ], + pmv[ pmv_off + 3 + 2 * 4 ] + pmv[ pmv_off + 1 + 2 * 4 ] ); p.d = pmv[ pmv_off + 3 + 3 * 4 ] + pmv[ pmv_off + 1 + 3 * 4 ]; } @@ -292,10 +298,10 @@ public class Frustum { // Top: a = m30 - m10, b = m31 - m11, c = m32 - m12, d = m33 - m13 - [0..3] column-major { final Plane p = planes[TOP]; - final float[] p_n = p.n; - p_n[0] = pmv[ pmv_off + 3 + 0 * 4 ] - pmv[ pmv_off + 1 + 0 * 4 ]; - p_n[1] = pmv[ pmv_off + 3 + 1 * 4 ] - pmv[ pmv_off + 1 + 1 * 4 ]; - p_n[2] = pmv[ pmv_off + 3 + 2 * 4 ] - pmv[ pmv_off + 1 + 2 * 4 ]; + final Vec3f p_n = p.n; + p_n.set( pmv[ pmv_off + 3 + 0 * 4 ] - pmv[ pmv_off + 1 + 0 * 4 ], + pmv[ pmv_off + 3 + 1 * 4 ] - pmv[ pmv_off + 1 + 1 * 4 ], + pmv[ pmv_off + 3 + 2 * 4 ] - pmv[ pmv_off + 1 + 2 * 4 ] ); p.d = pmv[ pmv_off + 3 + 3 * 4 ] - pmv[ pmv_off + 1 + 3 * 4 ]; } @@ -303,10 +309,10 @@ public class Frustum { // Near: a = m30 + m20, b = m31 + m21, c = m32 + m22, d = m33 + m23 - [0..3] column-major { final Plane p = planes[NEAR]; - final float[] p_n = p.n; - p_n[0] = pmv[ pmv_off + 3 + 0 * 4 ] + pmv[ pmv_off + 2 + 0 * 4 ]; - p_n[1] = pmv[ pmv_off + 3 + 1 * 4 ] + pmv[ pmv_off + 2 + 1 * 4 ]; - p_n[2] = pmv[ pmv_off + 3 + 2 * 4 ] + pmv[ pmv_off + 2 + 2 * 4 ]; + final Vec3f p_n = p.n; + p_n.set( pmv[ pmv_off + 3 + 0 * 4 ] + pmv[ pmv_off + 2 + 0 * 4 ], + pmv[ pmv_off + 3 + 1 * 4 ] + pmv[ pmv_off + 2 + 1 * 4 ], + pmv[ pmv_off + 3 + 2 * 4 ] + pmv[ pmv_off + 2 + 2 * 4 ] ); p.d = pmv[ pmv_off + 3 + 3 * 4 ] + pmv[ pmv_off + 2 + 3 * 4 ]; } @@ -314,38 +320,35 @@ public class Frustum { // Far: a = m30 - m20, b = m31 - m21, c = m32 + m22, d = m33 + m23 - [0..3] column-major { final Plane p = planes[FAR]; - final float[] p_n = p.n; - p_n[0] = pmv[ pmv_off + 3 + 0 * 4 ] - pmv[ pmv_off + 2 + 0 * 4 ]; - p_n[1] = pmv[ pmv_off + 3 + 1 * 4 ] - pmv[ pmv_off + 2 + 1 * 4 ]; - p_n[2] = pmv[ pmv_off + 3 + 2 * 4 ] - pmv[ pmv_off + 2 + 2 * 4 ]; + final Vec3f p_n = p.n; + p_n.set( pmv[ pmv_off + 3 + 0 * 4 ] - pmv[ pmv_off + 2 + 0 * 4 ], + pmv[ pmv_off + 3 + 1 * 4 ] - pmv[ pmv_off + 2 + 1 * 4 ], + pmv[ pmv_off + 3 + 2 * 4 ] - pmv[ pmv_off + 2 + 2 * 4 ] ); p.d = pmv[ pmv_off + 3 + 3 * 4 ] - pmv[ pmv_off + 2 + 3 * 4 ]; } // Normalize all planes for (int i = 0; i < 6; ++i) { final Plane p = planes[i]; - final float[] p_n = p.n; - final double invl = Math.sqrt(p_n[0] * p_n[0] + p_n[1] * p_n[1] + p_n[2] * p_n[2]); - - p_n[0] /= invl; - p_n[1] /= invl; - p_n[2] /= invl; - p.d /= invl; + final Vec3f p_n = p.n; + final float invLen = 1f / p_n.length(); + p_n.scale(invLen); + p.d *= invLen; } } private static final boolean isOutsideImpl(final Plane p, final AABBox box) { - final float[] low = box.getLow(); - final float[] high = box.getHigh(); - - if ( p.distanceTo(low[0], low[1], low[2]) > 0.0f || - p.distanceTo(high[0], low[1], low[2]) > 0.0f || - p.distanceTo(low[0], high[1], low[2]) > 0.0f || - p.distanceTo(high[0], high[1], low[2]) > 0.0f || - p.distanceTo(low[0], low[1], high[2]) > 0.0f || - p.distanceTo(high[0], low[1], high[2]) > 0.0f || - p.distanceTo(low[0], high[1], high[2]) > 0.0f || - p.distanceTo(high[0], high[1], high[2]) > 0.0f ) { + final Vec3f lo = box.getLow(); + final Vec3f hi = box.getHigh(); + + if ( p.distanceTo(lo.x(), lo.y(), lo.z()) > 0.0f || + p.distanceTo(hi.x(), lo.y(), lo.z()) > 0.0f || + p.distanceTo(lo.x(), hi.y(), lo.z()) > 0.0f || + p.distanceTo(hi.x(), hi.y(), lo.z()) > 0.0f || + p.distanceTo(lo.x(), lo.y(), hi.z()) > 0.0f || + p.distanceTo(hi.x(), lo.y(), hi.z()) > 0.0f || + p.distanceTo(lo.x(), hi.y(), hi.z()) > 0.0f || + p.distanceTo(hi.x(), hi.y(), hi.z()) > 0.0f ) { return false; } return true; diff --git a/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java b/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java index f8f1d3930..1aa305c2e 100644 --- a/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java +++ b/src/jogl/classes/com/jogamp/opengl/util/PMVMatrix.java @@ -46,6 +46,7 @@ import jogamp.common.os.PlatformPropsImpl; import com.jogamp.common.nio.Buffers; import com.jogamp.common.util.FloatStack; import com.jogamp.opengl.math.FloatUtil; +import com.jogamp.opengl.math.Matrix4f; import com.jogamp.opengl.math.Quaternion; import com.jogamp.opengl.math.Ray; import com.jogamp.opengl.math.geom.AABBox; @@ -208,9 +209,9 @@ public final class PMVMatrix implements GLMatrixFunc { // Mvit Modelview-Inverse-Transpose matrixArray = new float[5*16]; - mP_offset = 0*16; - mMv_offset = 1*16; - mTex_offset = 4*16; + // mP_offset = 0*16; + // mMv_offset = 1*16; + // mTex_offset = 4*16; matrixPMvMvit = Buffers.slice2Float(matrixArray, 0*16, 4*16); // P + Mv + Mvi + Mvit matrixPMvMvi = Buffers.slice2Float(matrixArray, 0*16, 3*16); // P + Mv + Mvi @@ -241,6 +242,8 @@ public final class PMVMatrix implements GLMatrixFunc { * Issues {@link #glLoadIdentity()} on all matrices, * i.e. {@link GLMatrixFunc#GL_MODELVIEW GL_MODELVIEW}, {@link GLMatrixFunc#GL_PROJECTION GL_PROJECTION} or {@link GL#GL_TEXTURE GL_TEXTURE} * and resets all internal states. + * + * Leaves {@link GLMatrixFunc#GL_MODELVIEW GL_MODELVIEW} the active matrix mode. */ public final void reset() { FloatUtil.makeIdentity(matrixArray, mMv_offset); @@ -403,7 +406,6 @@ public final class PMVMatrix implements GLMatrixFunc { } } - /** * Multiplies the {@link #glGetPMatrixf() P} and {@link #glGetMvMatrixf() Mv} matrix, i.e. * 
@@ -432,6 +434,71 @@ public final class PMVMatrix implements GLMatrixFunc {
         return mat4MvP;
     }
 
+    /**
+     * v_out = Mv * v_in
+     * @param v_in float[4] input vector
+     * @param v_out float[4] output vector
+     */
+    public final void multMvMatVec4f(final float[/*4*/] v_in, final float[/*4*/] v_out) {
+        FloatUtil.multMatrixVec(matrixArray, mMv_offset, v_in, v_out);
+    }
+
+    /**
+     * v_out = Mv * v_in
+     *
+     * Affine 3f-vector transformation by 4x4 matrix, see {@link FloatUtil#multMatrixVec3(float[], int, float[], float[])}.
+     *
+     * @param v_in float[3] input vector
+     * @param v_out float[3] output vector
+     */
+    public final void multMvMatVec3f(final float[/*3*/] v_in, final float[/*3*/] v_out) {
+        FloatUtil.multMatrixVec3(matrixArray, mMv_offset, v_in, v_out);
+    }
+
+    /**
+     * v_out = P * v_in
+     * @param v_in float[4] input vector
+     * @param v_out float[4] output vector
+     */
+    public final void multPMatVec4f(final float[/*4*/] v_in, final float[/*4*/] v_out) {
+        FloatUtil.multMatrixVec(matrixArray, v_in, v_out); // mP_offset := 0
+    }
+
+    /**
+     * v_out = P * v_in
+     *
+     * Affine 3f-vector transformation by 4x4 matrix, see {@link FloatUtil#multMatrixVec3(float[], int, float[], float[])}.
+     *
+     * @param v_in float[3] input vector
+     * @param v_out float[3] output vector
+     */
+    public final void multPMatVec3f(final float[/*3*/] v_in, final float[/*3*/] v_out) {
+        FloatUtil.multMatrixVec3(matrixArray, v_in, v_out); // mP_offset := 0
+    }
+
+    /**
+     * v_out = P * Mv * v_in
+     * @param v_in float[4] input vector
+     * @param v_out float[4] output vector
+     */
+    public final void multPMvMatVec4f(final float[/*4*/] v_in, final float[/*4*/] v_out) {
+        FloatUtil.multMatrixVec(matrixArray, mMv_offset, v_in, mat4Tmp1);
+        FloatUtil.multMatrixVec(matrixArray, mat4Tmp1, v_out); // mP_offset := 0
+    }
+
+    /**
+     * v_out = P * Mv * v_in
+     *
+     * Affine 3f-vector transformation by 4x4 matrix, see {@link FloatUtil#multMatrixVec3(float[], int, float[], float[])}.
+     *
+     * @param v_in float[3] input vector
+     * @param v_out float[3] output vector
+     */
+    public final void multPMvMatVec3f(final float[/*3*/] v_in, final float[/*3*/] v_out) {
+        FloatUtil.multMatrixVec3(matrixArray, mMv_offset, v_in, mat4Tmp1);
+        FloatUtil.multMatrixVec3(matrixArray, mat4Tmp1, v_out); // mP_offset := 0
+    }
+
     //
     // GLMatrixFunc implementation
     //
@@ -533,6 +600,24 @@ public final class PMVMatrix implements GLMatrixFunc {
         m.position(spos);
     }
 
+    /**
+     * Load the current matrix with the values of the given {@link Matrix4f}.
+     */
+    public final void glLoadMatrixf(final Matrix4f m) {
+        if(matrixMode==GL_MODELVIEW) {
+            m.get(matrixArray, mMv_offset);
+            dirtyBits |= DIRTY_INVERSE_MODELVIEW | DIRTY_INVERSE_TRANSPOSED_MODELVIEW | DIRTY_FRUSTUM ;
+            modifiedBits |= MODIFIED_MODELVIEW;
+        } else if(matrixMode==GL_PROJECTION) {
+            m.get(matrixArray, mP_offset);
+            dirtyBits |= DIRTY_FRUSTUM ;
+            modifiedBits |= MODIFIED_PROJECTION;
+        } else if(matrixMode==GL.GL_TEXTURE) {
+            m.get(matrixArray, mTex_offset);
+            modifiedBits |= MODIFIED_TEXTURE;
+        }
+    }
+
     /**
      * Load the current matrix with the values of the given {@link Quaternion}'s rotation {@link Quaternion#toMatrix(float[], int) matrix representation}.
      */
@@ -633,6 +718,21 @@ public final class PMVMatrix implements GLMatrixFunc {
         }
     }
 
+    public final void glMultMatrixf(final Matrix4f m) {
+        if(matrixMode==GL_MODELVIEW) {
+            new Matrix4f(matrixArray, mMv_offset).mul(m).get(matrixArray, mMv_offset);
+            dirtyBits |= DIRTY_INVERSE_MODELVIEW | DIRTY_INVERSE_TRANSPOSED_MODELVIEW | DIRTY_FRUSTUM ;
+            modifiedBits |= MODIFIED_MODELVIEW;
+        } else if(matrixMode==GL_PROJECTION) {
+            new Matrix4f(matrixArray, mP_offset).mul(m).get(matrixArray, mP_offset);
+            dirtyBits |= DIRTY_FRUSTUM ;
+            modifiedBits |= MODIFIED_PROJECTION;
+        } else if(matrixMode==GL.GL_TEXTURE) {
+            new Matrix4f(matrixArray, mTex_offset).mul(m).get(matrixArray, mTex_offset);
+            modifiedBits |= MODIFIED_TEXTURE;
+        }
+    }
+
     @Override
     public final void glTranslatef(final float x, final float y, final float z) {
         glMultMatrixf(FloatUtil.makeTranslation(matrixTxSx, false, x, y, z), 0);
@@ -645,7 +745,7 @@ public final class PMVMatrix implements GLMatrixFunc {
 
     @Override
     public final void glRotatef(final float ang_deg, final float x, final float y, final float z) {
-        glMultMatrixf(FloatUtil.makeRotationAxis(mat4Tmp1, 0, ang_deg * FloatUtil.PI / 180.0f, x, y, z, mat4Tmp2), 0);
+        glMultMatrixf(FloatUtil.makeRotationAxis(mat4Tmp1, 0, FloatUtil.adegToRad(ang_deg), x, y, z, mat4Tmp2), 0);
     }
 
     /**
@@ -728,7 +828,7 @@ public final class PMVMatrix implements GLMatrixFunc {
     public final boolean gluProject(final float objx, final float objy, final float objz,
                                     final int[] viewport, final int viewport_offset,
                                     final float[] win_pos, final int win_pos_offset ) {
-        return FloatUtil.mapObjToWinCoords(objx, objy, objz,
+        return FloatUtil.mapObjToWin(objx, objy, objz,
                           matrixArray, mMv_offset,
                           matrixArray, mP_offset,
                           viewport, viewport_offset,
@@ -754,7 +854,7 @@ public final class PMVMatrix implements GLMatrixFunc {
     public final boolean gluUnProject(final float winx, final float winy, final float winz,
                                       final int[] viewport, final int viewport_offset,
                                       final float[] obj_pos, final int obj_pos_offset) {
-        return FloatUtil.mapWinToObjCoords(winx, winy, winz,
+        return FloatUtil.mapWinToObj(winx, winy, winz,
                                            matrixArray, mMv_offset,
                                            matrixArray, mP_offset,
                                            viewport, viewport_offset,
@@ -788,7 +888,7 @@ public final class PMVMatrix implements GLMatrixFunc {
                                  final int[] viewport, final int viewport_offset,
                                  final float near, final float far,
                                  final float[] obj_pos, final int obj_pos_offset ) {
-        return FloatUtil.mapWinToObjCoords(winx, winy, winz, clipw,
+        return FloatUtil.mapWinToObj4(winx, winy, winz, clipw,
                 matrixArray, mMv_offset,
                 matrixArray, mP_offset,
                 viewport, viewport_offset,
@@ -842,14 +942,12 @@ public final class PMVMatrix implements GLMatrixFunc {
      * @return true if successful, otherwise false (failed to invert matrix, or becomes z is infinity)
      */
     public final boolean gluUnProjectRay(final float winx, final float winy, final float winz0, final float winz1,
-                                         final int[] viewport, final int viewport_offset,
+                                         final int[] viewport,
                                          final Ray ray) {
         return FloatUtil.mapWinToRay(winx, winy, winz0, winz1,
                 matrixArray, mMv_offset,
-                matrixArray, mP_offset,
-                viewport, viewport_offset,
-                ray,
-                mat4Tmp1, mat4Tmp2, mat4Tmp3);
+                matrixArray, mP_offset, viewport,
+                ray, mat4Tmp1, mat4Tmp2, mat4Tmp3);
     }
 
     public StringBuilder toString(StringBuilder sb, final String f) {
@@ -1066,8 +1164,10 @@ public final class PMVMatrix implements GLMatrixFunc {
         return res;
     }
 
+    private static final int mP_offset   = 0*16;
+    private static final int mMv_offset  = 1*16;
+    private static final int mTex_offset = 4*16;
     private final float[] matrixArray;
-    private final int mP_offset, mMv_offset, mTex_offset;
     private final FloatBuffer matrixPMvMvit, matrixPMvMvi, matrixPMv, matrixP, matrixTex, matrixMv, matrixMvi, matrixMvit;
     private final float[] matrixTxSx;
     private final float[] mat4Tmp1, mat4Tmp2, mat4Tmp3;
diff --git a/src/jogl/classes/com/jogamp/opengl/util/stereo/EyeParameter.java b/src/jogl/classes/com/jogamp/opengl/util/stereo/EyeParameter.java
index 43a6cfc58..e0f465da7 100644
--- a/src/jogl/classes/com/jogamp/opengl/util/stereo/EyeParameter.java
+++ b/src/jogl/classes/com/jogamp/opengl/util/stereo/EyeParameter.java
@@ -1,5 +1,5 @@
 /**
- * Copyright 2014 JogAmp Community. All rights reserved.
+ * Copyright 2014-2023 JogAmp Community. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without modification, are
  * permitted provided that the following conditions are met:
@@ -28,6 +28,7 @@
 package com.jogamp.opengl.util.stereo;
 
 import com.jogamp.opengl.math.FovHVHalves;
+import com.jogamp.opengl.math.Vec3f;
 
 /**
  * Constant single eye parameter of the viewer, relative to its {@link ViewerPose}.
@@ -36,8 +37,8 @@ public final class EyeParameter {
     /** Eye number, 0 for the left eye and 1 for the right eye. */
     public final int number;
 
-    /** float[3] eye position vector used to define eye height in meter relative to actor. */
-    public final float[] positionOffset;
+    /** eye position vector used to define eye height in meter relative to actor. */
+    public final Vec3f positionOffset;
 
     /** Field of view in both directions, may not be centered, either {@link FovHVHalves#inTangents} or radians. */
     public final FovHVHalves fovhv;
@@ -51,18 +52,18 @@ public final class EyeParameter {
     /** Z-axis eye relief in meter. */
     public final float eyeReliefZ;
 
-    public EyeParameter(final int number, final float[] positionOffset, final FovHVHalves fovhv,
+    public EyeParameter(final int number, final Vec3f positionOffset, final FovHVHalves fovhv,
                         final float distNoseToPupil, final float verticalDelta, final float eyeRelief) {
         this.number = number;
-        this.positionOffset = new float[3];
-        System.arraycopy(positionOffset, 0, this.positionOffset, 0, 3);
+        this.positionOffset = new Vec3f(positionOffset);
         this.fovhv = fovhv;
         this.distNoseToPupilX = distNoseToPupil;
         this.distMiddleToPupilY = verticalDelta;
         this.eyeReliefZ = eyeRelief;
     }
+    @Override
     public final String toString() {
-        return "EyeParam[num "+number+", posOff["+positionOffset[0]+", "+positionOffset[1]+", "+positionOffset[2]+"], "+fovhv+
-                      ", distPupil[noseX "+distNoseToPupilX+", middleY "+distMiddleToPupilY+", reliefZ "+eyeReliefZ+"]]";
+        return "EyeParam[num "+number+", posOff["+positionOffset+"], "+fovhv+
+               ", distPupil[noseX "+distNoseToPupilX+", middleY "+distMiddleToPupilY+", reliefZ "+eyeReliefZ+"]]";
     }
 }
\ No newline at end of file
diff --git a/src/jogl/classes/com/jogamp/opengl/util/stereo/LocationSensorParameter.java b/src/jogl/classes/com/jogamp/opengl/util/stereo/LocationSensorParameter.java
index b795927cd..6294adee1 100644
--- a/src/jogl/classes/com/jogamp/opengl/util/stereo/LocationSensorParameter.java
+++ b/src/jogl/classes/com/jogamp/opengl/util/stereo/LocationSensorParameter.java
@@ -1,5 +1,5 @@
 /**
- * Copyright 2015 JogAmp Community. All rights reserved.
+ * Copyright 2015-2023 JogAmp Community. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without modification, are
  * permitted provided that the following conditions are met:
@@ -27,6 +27,7 @@
  */
 package com.jogamp.opengl.util.stereo;
 
+import com.jogamp.opengl.math.Matrix4f;
 import com.jogamp.opengl.math.geom.Frustum;
 
 /**
@@ -38,13 +39,15 @@ public final class LocationSensorParameter {
     /** The {@link Frustum}'s {@link Frustum.FovDesc} description of the location sensor. */
     public final Frustum.FovDesc frustumDesc;
     /** The {@link Frustum}'s float[16] projection matrix of the location sensor. */
-    public final float[] frustumProjMat;
+    public final Matrix4f frustumProjMat;
 
     public LocationSensorParameter(final Frustum.FovDesc fovDesc) {
         this.frustumDesc = fovDesc;
         this.frustum = new Frustum();
-        this.frustumProjMat = frustum.updateByFovDesc(new float[16], 0, true, fovDesc);
+        this.frustumProjMat = frustum.updateByFovDesc(new Matrix4f(), fovDesc);
     }
+
+    @Override
     public final String toString() {
         return "LocationSensor["+frustumDesc+"]";
     }
diff --git a/src/jogl/classes/com/jogamp/opengl/util/stereo/StereoDevice.java b/src/jogl/classes/com/jogamp/opengl/util/stereo/StereoDevice.java
index b6112650a..85e752302 100644
--- a/src/jogl/classes/com/jogamp/opengl/util/stereo/StereoDevice.java
+++ b/src/jogl/classes/com/jogamp/opengl/util/stereo/StereoDevice.java
@@ -1,5 +1,5 @@
 /**
- * Copyright 2014 JogAmp Community. All rights reserved.
+ * Copyright 2014-2023 JogAmp Community. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without modification, are
  * permitted provided that the following conditions are met:
@@ -33,6 +33,7 @@ import com.jogamp.nativewindow.util.PointImmutable;
 import jogamp.opengl.Debug;
 
 import com.jogamp.opengl.math.FovHVHalves;
+import com.jogamp.opengl.math.Vec3f;
 
 /**
  * Interface describing a native stereoscopic device
@@ -94,7 +95,7 @@ public interface StereoDevice {
     public int getRequiredRotation();
 
     /**
-     * Return the device default eye position offset for {@link #createRenderer(int, int, float[], FovHVHalves[], float)}.
+     * Return the device default eye position offset for {@link #createRenderer(int, int, Vec3f, FovHVHalves[], float)}.
      * 

      * Result is an array of float values for
      * 
    
@@ -105,7 +106,7 @@ public interface StereoDevice {
      * 
    
      * @return
      */
-    public float[] getDefaultEyePositionOffset();
+    public Vec3f getDefaultEyePositionOffset();
 
     /**
      * Returns the device default {@link FovHVHalves} for all supported eyes
@@ -198,7 +199,7 @@ public interface StereoDevice {
      * Returns the supported distortion compensation of the {@link StereoDeviceRenderer},
      * e.g. {@link StereoDeviceRenderer#DISTORTION_BARREL}, {@link StereoDeviceRenderer#DISTORTION_CHROMATIC}, etc.
      * @see StereoDeviceRenderer#getDistortionBits()
-     * @see #createRenderer(int, int, float[], FovHVHalves[], float, int)
+     * @see #createRenderer(int, int, Vec3f, FovHVHalves[], float, int)
      * @see #getRecommendedDistortionBits()
      * @see #getMinimumDistortionBits()
      */
@@ -212,7 +213,7 @@ public interface StereoDevice {
      * User shall use the recommended distortion compensation to achieve a distortion free view.
      * 
    
      * @see StereoDeviceRenderer#getDistortionBits()
-     * @see #createRenderer(int, int, float[], FovHVHalves[], float, int)
+     * @see #createRenderer(int, int, Vec3f, FovHVHalves[], float, int)
      * @see #getSupportedDistortionBits()
      * @see #getMinimumDistortionBits()
      */
@@ -227,7 +228,7 @@ public interface StereoDevice {
      * @see #getSupportedDistortionBits()
      * @see #getRecommendedDistortionBits()
      * @see StereoDeviceRenderer#getDistortionBits()
-     * @see #createRenderer(int, int, float[], FovHVHalves[], float, int)
+     * @see #createRenderer(int, int, Vec3f, FovHVHalves[], float, int)
      */
     public int getMinimumDistortionBits();
 
@@ -245,6 +246,6 @@ public interface StereoDevice {
      * @return
      */
     public StereoDeviceRenderer createRenderer(final int distortionBits,
-                                               final int textureCount, final float[] eyePositionOffset,
+                                               final int textureCount, final Vec3f eyePositionOffset,
                                                final FovHVHalves[] eyeFov, final float pixelsPerDisplayPixel, final int textureUnit);
 }
diff --git a/src/jogl/classes/com/jogamp/opengl/util/stereo/StereoUtil.java b/src/jogl/classes/com/jogamp/opengl/util/stereo/StereoUtil.java
index b6f76a343..63cb3e1e7 100644
--- a/src/jogl/classes/com/jogamp/opengl/util/stereo/StereoUtil.java
+++ b/src/jogl/classes/com/jogamp/opengl/util/stereo/StereoUtil.java
@@ -1,5 +1,5 @@
 /**
- * Copyright 2014 JogAmp Community. All rights reserved.
+ * Copyright 2014-2023 JogAmp Community. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without modification, are
  * permitted provided that the following conditions are met:
@@ -27,9 +27,9 @@
  */
 package com.jogamp.opengl.util.stereo;
 
-import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Matrix4f;
 import com.jogamp.opengl.math.Quaternion;
-import com.jogamp.opengl.math.VectorUtil;
+import com.jogamp.opengl.math.Vec3f;
 import com.jogamp.opengl.util.CustomGLEventListener;
 import com.jogamp.opengl.util.stereo.StereoDeviceRenderer.Eye;
 
@@ -137,24 +137,24 @@ public class StereoUtil {
      * @param eye
      * @param zNear frustum near value
      * @param zFar frustum far value
-     * @param mat4Projection float[16] projection matrix result
-     * @param mat4Modelview float[16] modelview matrix result
+     * @param mat4Projection projection matrix result
+     * @param mat4Modelview modelview matrix result
      */
     public static void getSBSUpstreamPMV(final ViewerPose viewerPose, final Eye eye,
                                          final float zNear, final float zFar,
-                                         final float[] mat4Projection, final float[] mat4Modelview) {
-        final float[] mat4Tmp1 = new float[16];
-        final float[] mat4Tmp2 = new float[16];
-        final float[] vec3Tmp1 = new float[3];
-        final float[] vec3Tmp2 = new float[3];
-        final float[] vec3Tmp3 = new float[3];
+                                         final Matrix4f mat4Projection, final Matrix4f mat4Modelview) {
+        final Matrix4f mat4Tmp1 = new Matrix4f();
+        final Matrix4f mat4Tmp2 = new Matrix4f();
+        final Vec3f vec3Tmp1 = new Vec3f();
+        final Vec3f vec3Tmp2 = new Vec3f();
+        final Vec3f vec3Tmp3 = new Vec3f();
 
         final EyeParameter eyeParam = eye.getEyeParameter();
 
         //
         // Projection
         //
-        FloatUtil.makePerspective(mat4Projection, 0, true, eyeParam.fovhv, zNear, zFar);
+        mat4Projection.setToPerspective(eyeParam.fovhv, zNear, zFar);
 
         //
         // Modelview
@@ -162,21 +162,17 @@ public class StereoUtil {
         final Quaternion rollPitchYaw = new Quaternion();
         // private final float eyeYaw = FloatUtil.PI; // 180 degrees in radians
         // rollPitchYaw.rotateByAngleY(eyeYaw);
-        final float[] shiftedEyePos = rollPitchYaw.rotateVector(vec3Tmp1, 0, viewerPose.position, 0);
-        VectorUtil.addVec3(shiftedEyePos, shiftedEyePos, eyeParam.positionOffset);
+        final Vec3f shiftedEyePos = rollPitchYaw.rotateVector(viewerPose.position, vec3Tmp1).add(eyeParam.positionOffset);
 
         rollPitchYaw.mult(viewerPose.orientation);
-        final float[] up = rollPitchYaw.rotateVector(vec3Tmp2, 0, VectorUtil.VEC3_UNIT_Y, 0);
-        final float[] forward = rollPitchYaw.rotateVector(vec3Tmp3, 0, VectorUtil.VEC3_UNIT_Z_NEG, 0);
-        final float[] center = VectorUtil.addVec3(forward, shiftedEyePos, forward);
+        final Vec3f up = rollPitchYaw.rotateVector(Vec3f.UNIT_Y, vec3Tmp2);
+        final Vec3f forward = rollPitchYaw.rotateVector(Vec3f.UNIT_Z_NEG, vec3Tmp3); // -> center
+        final Vec3f center = forward.add(shiftedEyePos);
 
-        final float[] mLookAt = FloatUtil.makeLookAt(mat4Tmp2, 0, shiftedEyePos, 0, center, 0, up, 0, mat4Tmp1);
-        final float[] mViewAdjust = FloatUtil.makeTranslation(mat4Modelview, true,
-                                                              eyeParam.distNoseToPupilX,
-                                                              eyeParam.distMiddleToPupilY,
-                                                              eyeParam.eyeReliefZ);
-
-        /* mat4Modelview = */ FloatUtil.multMatrix(mViewAdjust, mLookAt);
+        final Matrix4f mLookAt = mat4Tmp2.setToLookAt(shiftedEyePos, center, up, mat4Tmp1);
+        mat4Modelview.mul( mat4Tmp1.setToTranslation( eyeParam.distNoseToPupilX,
+                                                      eyeParam.distMiddleToPupilY,
+                                                      eyeParam.eyeReliefZ ), mLookAt);
     }
 
 }
diff --git a/src/jogl/classes/com/jogamp/opengl/util/stereo/ViewerPose.java b/src/jogl/classes/com/jogamp/opengl/util/stereo/ViewerPose.java
index 10ee4c994..5d2cf925c 100644
--- a/src/jogl/classes/com/jogamp/opengl/util/stereo/ViewerPose.java
+++ b/src/jogl/classes/com/jogamp/opengl/util/stereo/ViewerPose.java
@@ -1,5 +1,5 @@
 /**
- * Copyright 2014 JogAmp Community. All rights reserved.
+ * Copyright 2014-2023 JogAmp Community. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without modification, are
  * permitted provided that the following conditions are met:
@@ -28,13 +28,14 @@
 package com.jogamp.opengl.util.stereo;
 
 import com.jogamp.opengl.math.Quaternion;
+import com.jogamp.opengl.math.Vec3f;
 
 /**
  * {@link #position} and {@link #orientation} of viewer.
  */
 public final class ViewerPose {
     /**
-     * float[3] position of viewer in meter.
+     * position of viewer in meter.
      * 
    
      * Apply the following to resolve the actual eye position:
      * 
      
@@ -43,13 +44,13 @@ public final class ViewerPose {
      * 
    
      * 
    
      */
-    public final float[] position;
+    public final Vec3f position;
 
     /** Orientation of viewer. */
     public final Quaternion orientation;
 
     public ViewerPose() {
-        this.position = new float[3];
+        this.position = new Vec3f();
         this.orientation = new Quaternion();
     }
     public ViewerPose(final float[] position, final Quaternion orientation) {
@@ -64,11 +65,14 @@ public final class ViewerPose {
     }
     /** Set position and orientation of this instance. */
     public final void setPosition(final float posX, final float posY, final float posZ) {
-        position[0] = posX;
-        position[1] = posY;
-        position[2] = posZ;
+        position.set( posX, posY, posZ );
     }
+    /** Set position and orientation of this instance. */
+    public final void setPosition(final Vec3f pos) {
+        position.set( pos );
+    }
+    @Override
     public final String toString() {
-        return "ViewerPose[pos["+position[0]+", "+position[1]+", "+position[2]+"], "+orientation+"]";
+        return "ViewerPose[pos["+position+"], "+orientation+"]";
     }
 }
\ No newline at end of file
diff --git a/src/jogl/classes/com/jogamp/opengl/util/stereo/generic/GenericStereoDeviceFactory.java b/src/jogl/classes/com/jogamp/opengl/util/stereo/generic/GenericStereoDeviceFactory.java
index 957758e78..0cdef8770 100644
--- a/src/jogl/classes/com/jogamp/opengl/util/stereo/generic/GenericStereoDeviceFactory.java
+++ b/src/jogl/classes/com/jogamp/opengl/util/stereo/generic/GenericStereoDeviceFactory.java
@@ -35,6 +35,7 @@ import com.jogamp.nativewindow.util.Dimension;
 import com.jogamp.nativewindow.util.DimensionImmutable;
 import com.jogamp.opengl.math.FloatUtil;
 import com.jogamp.opengl.math.FovHVHalves;
+import com.jogamp.opengl.math.Vec3f;
 import com.jogamp.opengl.util.stereo.EyeParameter;
 import com.jogamp.opengl.util.stereo.StereoDevice;
 import com.jogamp.opengl.util.stereo.StereoDeviceConfig;
@@ -53,7 +54,7 @@ public class GenericStereoDeviceFactory extends StereoDeviceFactory {
      */
     public static GenericStereoDeviceConfig createMono(final String name,
                              final DimensionImmutable surfaceSizeInPixel, final float[] screenSizeInMeters,
-                             final float[] defaultEyePositionOffset) {
+                             final Vec3f defaultEyePositionOffset) {
         final float pupilCenterFromScreenTopInMeters = screenSizeInMeters[1] / 2f;
         final float d2r = FloatUtil.PI / 180.0f;
         return new GenericStereoDeviceConfig(
@@ -90,7 +91,7 @@ public class GenericStereoDeviceFactory extends StereoDeviceFactory {
     public static GenericStereoDeviceConfig createStereoSBS(final String name,
                              final DimensionImmutable surfaceSizeInPixel, final float[] screenSizeInMeters,
                              final float interpupillaryDistanceInMeters, final float fovy,
-                             final float[] defaultEyePositionOffset) {
+                             final Vec3f defaultEyePositionOffset) {
         final float pupilCenterFromScreenTopInMeters = screenSizeInMeters[1] / 2f;
         final float d2r = FloatUtil.PI / 180.0f;
 
@@ -138,7 +139,7 @@ public class GenericStereoDeviceFactory extends StereoDeviceFactory {
                              final DimensionImmutable surfaceSizeInPixel, final float[] screenSizeInMeters,
                              final float interpupillaryDistanceInMeters, final float fovy,
                              final DimensionImmutable eyeTextureSize,
-                             final float[] defaultEyePositionOffset) {
+                             final Vec3f defaultEyePositionOffset) {
         DistortionMesh.Producer lenseDistMeshProduce = null;
         try {
             lenseDistMeshProduce =
diff --git a/src/jogl/classes/jogamp/graph/font/typecast/TypecastFont.java b/src/jogl/classes/jogamp/graph/font/typecast/TypecastFont.java
index 1bc0fe0f4..cc0aece27 100644
--- a/src/jogl/classes/jogamp/graph/font/typecast/TypecastFont.java
+++ b/src/jogl/classes/jogamp/graph/font/typecast/TypecastFont.java
@@ -250,7 +250,7 @@ class TypecastFont implements Font {
 
     @Override
     public AABBox getMetricBounds(final CharSequence string) {
-        return getMetricBoundsFU(string).scale2(1.0f/metrics.getUnitsPerEM(), new float[3]);
+        return getMetricBoundsFU(string).scale2(1.0f/metrics.getUnitsPerEM());
     }
 
     @Override
@@ -290,7 +290,7 @@ class TypecastFont implements Font {
     }
     @Override
     public AABBox getGlyphBounds(final CharSequence string, final AffineTransform tmp1, final AffineTransform tmp2) {
-        return getGlyphBoundsFU(string, tmp1, tmp2).scale2(1.0f/metrics.getUnitsPerEM(), new float[3]);
+        return getGlyphBoundsFU(string, tmp1, tmp2).scale2(1.0f/metrics.getUnitsPerEM());
     }
 
     @Override
diff --git a/src/jogl/classes/jogamp/graph/font/typecast/TypecastGlyph.java b/src/jogl/classes/jogamp/graph/font/typecast/TypecastGlyph.java
index 6747cca82..560d0902b 100644
--- a/src/jogl/classes/jogamp/graph/font/typecast/TypecastGlyph.java
+++ b/src/jogl/classes/jogamp/graph/font/typecast/TypecastGlyph.java
@@ -148,14 +148,14 @@ public final class TypecastGlyph implements Font.Glyph {
     public final AABBox getBoundsFU(final AABBox dest) { return dest.copy(bbox); }
 
     @Override
-    public final AABBox getBounds(final AABBox dest, final float[] tmpV3) {
-        return dest.copy(bbox).scale2(1.0f/font.getMetrics().getUnitsPerEM(), tmpV3);
+    public final AABBox getBounds(final AABBox dest) {
+        return dest.copy(bbox).scale2(1.0f/font.getMetrics().getUnitsPerEM());
     }
 
     @Override
     public final AABBox getBounds() {
-        final AABBox dest = new AABBox();
-        return dest.copy(bbox).scale2(1.0f/font.getMetrics().getUnitsPerEM(), new float[2]);
+        final AABBox dest = new AABBox(bbox);
+        return dest.scale2(1.0f/font.getMetrics().getUnitsPerEM());
     }
 
     @Override
diff --git a/src/jogl/classes/jogamp/graph/font/typecast/TypecastHMetrics.java b/src/jogl/classes/jogamp/graph/font/typecast/TypecastHMetrics.java
index 5e56d1932..11f1ce7c7 100644
--- a/src/jogl/classes/jogamp/graph/font/typecast/TypecastHMetrics.java
+++ b/src/jogl/classes/jogamp/graph/font/typecast/TypecastHMetrics.java
@@ -119,7 +119,7 @@ final class TypecastHMetrics implements Metrics {
     }
 
     @Override
-    public AABBox getBounds(final AABBox dest, final float[] tmpV3) {
-        return dest.setSize(bbox.getLow(), bbox.getHigh()).scale2(unitsPerEM_inv, tmpV3);
+    public AABBox getBounds(final AABBox dest) {
+        return dest.copy(bbox).scale2(unitsPerEM_inv);
     }
 }
diff --git a/src/jogl/classes/jogamp/opengl/ProjectFloat.java b/src/jogl/classes/jogamp/opengl/ProjectFloat.java
index 5ec5a8e3f..3fcc75f3d 100644
--- a/src/jogl/classes/jogamp/opengl/ProjectFloat.java
+++ b/src/jogl/classes/jogamp/opengl/ProjectFloat.java
@@ -227,7 +227,7 @@ public class ProjectFloat {
                             final float[] projMatrix, final int projMatrix_offset,
                             final int[] viewport, final int viewport_offset,
                             final float[] win_pos, final int win_pos_offset ) {
-    return FloatUtil.mapObjToWinCoords(objx, objy, objz,
+    return FloatUtil.mapObjToWin(objx, objy, objz,
                       modelMatrix, modelMatrix_offset,
                       projMatrix, projMatrix_offset,
                       viewport, viewport_offset,
@@ -347,7 +347,7 @@ public class ProjectFloat {
                               final float[] projMatrix, final int projMatrix_offset,
                               final int[] viewport, final int viewport_offset,
                               final float[] obj_pos, final int obj_pos_offset) {
-    return FloatUtil.mapWinToObjCoords(winx, winy, winz,
+    return FloatUtil.mapWinToObj(winx, winy, winz,
                                    modelMatrix, modelMatrix_offset,
                                    projMatrix, projMatrix_offset,
                                    viewport, viewport_offset,
@@ -505,7 +505,7 @@ public class ProjectFloat {
                                 final int[] viewport, final int viewport_offset,
                                 final float near, final float far,
                                 final float[] obj_pos, final int obj_pos_offset ) {
-    return FloatUtil.mapWinToObjCoords(winx, winy, winz, clipw,
+    return FloatUtil.mapWinToObj4(winx, winy, winz, clipw,
                                        modelMatrix, modelMatrix_offset,
                                        projMatrix, projMatrix_offset,
                                        viewport, viewport_offset,
diff --git a/src/jogl/classes/jogamp/opengl/util/stereo/GenericStereoDevice.java b/src/jogl/classes/jogamp/opengl/util/stereo/GenericStereoDevice.java
index bfe93b59c..60adc7d74 100644
--- a/src/jogl/classes/jogamp/opengl/util/stereo/GenericStereoDevice.java
+++ b/src/jogl/classes/jogamp/opengl/util/stereo/GenericStereoDevice.java
@@ -34,6 +34,7 @@ import com.jogamp.nativewindow.util.PointImmutable;
 import com.jogamp.nativewindow.util.Rectangle;
 import com.jogamp.nativewindow.util.RectangleImmutable;
 import com.jogamp.opengl.math.FovHVHalves;
+import com.jogamp.opengl.math.Vec3f;
 import com.jogamp.opengl.util.stereo.StereoDeviceConfig;
 import com.jogamp.opengl.util.stereo.EyeParameter;
 import com.jogamp.opengl.util.stereo.LocationSensorParameter;
@@ -63,9 +64,9 @@ public class GenericStereoDevice implements StereoDevice {
     private static final GenericStereoDeviceConfig[] configs;
 
     static {
-        final float[] DEFAULT_EYE_POSITION_OFFSET_STEREO_LENSES = { 0.0f, 1.6f, -5.0f };  // 1.6 up, 5 forward
-        final float[] DEFAULT_EYE_POSITION_OFFSET_STEREO        = { 0.0f, 0.3f,  3.0f };  // 0.3 up, 3 back
-        final float[] DEFAULT_EYE_POSITION_OFFSET_MONO          = { 0.0f, 0.0f,  3.0f };  //         3 back
+        final Vec3f DEFAULT_EYE_POSITION_OFFSET_STEREO_LENSES = new Vec3f( 0.0f, 1.6f, -5.0f );  // 1.6 up, 5 forward
+        final Vec3f DEFAULT_EYE_POSITION_OFFSET_STEREO        = new Vec3f( 0.0f, 0.3f,  3.0f );  // 0.3 up, 3 back
+        final Vec3f DEFAULT_EYE_POSITION_OFFSET_MONO          = new Vec3f( 0.0f, 0.0f,  3.0f );  //         3 back
 
         final DimensionImmutable surfaceSizeInPixelDK1 = new Dimension(1280, 800);
         final float[] screenSizeInMetersDK1 = new float[] { 0.14976f, 0.0936f };
@@ -175,7 +176,7 @@ public class GenericStereoDevice implements StereoDevice {
     public int getRequiredRotation() { return 0; }
 
     @Override
-    public float[] getDefaultEyePositionOffset() { return config.defaultEyeParam[0].positionOffset; }
+    public Vec3f getDefaultEyePositionOffset() { return config.defaultEyeParam[0].positionOffset; }
 
     @Override
     public final FovHVHalves[] getDefaultFOV() { return defaultEyeFov; }
@@ -264,7 +265,7 @@ public class GenericStereoDevice implements StereoDevice {
 
     @Override
     public final StereoDeviceRenderer createRenderer(final int distortionBits,
-                                                     final int textureCount, final float[] eyePositionOffset,
+                                                     final int textureCount, final Vec3f eyePositionOffset,
                                                      final FovHVHalves[] eyeFov, final float pixelsPerDisplayPixel,
                                                      final int textureUnit) {
        final EyeParameter[] eyeParam = new EyeParameter[eyeFov.length];
diff --git a/src/jogl/classes/jogamp/opengl/util/stereo/GenericStereoDeviceRenderer.java b/src/jogl/classes/jogamp/opengl/util/stereo/GenericStereoDeviceRenderer.java
index 21567a0f1..3c6f5e37e 100644
--- a/src/jogl/classes/jogamp/opengl/util/stereo/GenericStereoDeviceRenderer.java
+++ b/src/jogl/classes/jogamp/opengl/util/stereo/GenericStereoDeviceRenderer.java
@@ -45,6 +45,7 @@ import jogamp.common.os.PlatformPropsImpl;
 import com.jogamp.common.nio.Buffers;
 import com.jogamp.common.os.Platform;
 import com.jogamp.opengl.JoglVersion;
+import com.jogamp.opengl.math.Vec3f;
 import com.jogamp.opengl.util.GLArrayDataServer;
 import com.jogamp.opengl.util.glsl.ShaderCode;
 import com.jogamp.opengl.util.glsl.ShaderProgram;
@@ -90,7 +91,7 @@ public class GenericStereoDeviceRenderer implements StereoDeviceRenderer {
         public final EyeParameter getEyeParameter() { return eyeParameter; }
 
         /* pp */ GenericEye(final GenericStereoDevice device, final int distortionBits,
-                            final float[] eyePositionOffset, final EyeParameter eyeParam,
+                            final Vec3f eyePositionOffset, final EyeParameter eyeParam,
                             final DimensionImmutable textureSize, final RectangleImmutable eyeViewport) {
             this.eyeName = eyeParam.number;
             this.distortionBits = distortionBits;
@@ -387,7 +388,7 @@ public class GenericStereoDeviceRenderer implements StereoDeviceRenderer {
     private static final DimensionImmutable zeroSize = new Dimension(0, 0);
 
     /* pp */ GenericStereoDeviceRenderer(final GenericStereoDevice context, final int distortionBits,
-                                       final int textureCount, final float[] eyePositionOffset,
+                                       final int textureCount, final Vec3f eyePositionOffset,
                                        final EyeParameter[] eyeParam, final float pixelsPerDisplayPixel, final int textureUnit,
                                        final DimensionImmutable[] eyeTextureSizes, final DimensionImmutable totalTextureSize,
                                        final RectangleImmutable[] eyeViewports) {
diff --git a/src/oculusvr/classes/jogamp/opengl/oculusvr/OVRStereoDevice.java b/src/oculusvr/classes/jogamp/opengl/oculusvr/OVRStereoDevice.java
index 5025e80c5..335123bb9 100644
--- a/src/oculusvr/classes/jogamp/opengl/oculusvr/OVRStereoDevice.java
+++ b/src/oculusvr/classes/jogamp/opengl/oculusvr/OVRStereoDevice.java
@@ -40,6 +40,7 @@ import com.jogamp.oculusvr.ovrHmdDesc;
 import com.jogamp.oculusvr.ovrSizei;
 import com.jogamp.oculusvr.ovrTrackingState;
 import com.jogamp.opengl.math.FovHVHalves;
+import com.jogamp.opengl.math.Vec3f;
 import com.jogamp.opengl.math.geom.Frustum;
 import com.jogamp.opengl.util.stereo.LocationSensorParameter;
 import com.jogamp.opengl.util.stereo.StereoDevice;
@@ -49,7 +50,7 @@ import com.jogamp.opengl.util.stereo.StereoUtil;
 
 public class OVRStereoDevice implements StereoDevice {
     /** 1.6 up, 5 forward */
-    private static final float[] DEFAULT_EYE_POSITION_OFFSET = { 0.0f, 1.6f, -5.0f };
+    private static final Vec3f DEFAULT_EYE_POSITION_OFFSET = new Vec3f( 0.0f, 1.6f, -5.0f );
 
     private final StereoDeviceFactory factory;
     public final int deviceIndex;
@@ -181,7 +182,7 @@ public class OVRStereoDevice implements StereoDevice {
     public int getRequiredRotation() { return requiredRotation; }
 
     @Override
-    public float[] getDefaultEyePositionOffset() { return DEFAULT_EYE_POSITION_OFFSET; }
+    public Vec3f getDefaultEyePositionOffset() { return DEFAULT_EYE_POSITION_OFFSET; }
 
     @Override
     public final FovHVHalves[] getDefaultFOV() { return defaultEyeFov; }
@@ -300,7 +301,7 @@ public class OVRStereoDevice implements StereoDevice {
 
     @Override
     public final StereoDeviceRenderer createRenderer(final int distortionBits,
-                                                     final int textureCount, final float[] eyePositionOffset,
+                                                     final int textureCount, final Vec3f eyePositionOffset,
                                                      final FovHVHalves[] eyeFov, final float pixelsPerDisplayPixel, final int textureUnit) {
         final ovrFovPort ovrEyeFov0 = OVRUtil.getOVRFovPort(eyeFov[0]);
         final ovrFovPort ovrEyeFov1 = OVRUtil.getOVRFovPort(eyeFov[1]);
diff --git a/src/oculusvr/classes/jogamp/opengl/oculusvr/OVRStereoDeviceRenderer.java b/src/oculusvr/classes/jogamp/opengl/oculusvr/OVRStereoDeviceRenderer.java
index 95565dd0f..1430a9c45 100644
--- a/src/oculusvr/classes/jogamp/opengl/oculusvr/OVRStereoDeviceRenderer.java
+++ b/src/oculusvr/classes/jogamp/opengl/oculusvr/OVRStereoDeviceRenderer.java
@@ -59,6 +59,7 @@ import com.jogamp.oculusvr.ovrVector2f;
 import com.jogamp.oculusvr.ovrVector3f;
 import com.jogamp.opengl.JoglVersion;
 import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Vec3f;
 import com.jogamp.opengl.util.GLArrayDataServer;
 import com.jogamp.opengl.util.glsl.ShaderCode;
 import com.jogamp.opengl.util.glsl.ShaderProgram;
@@ -107,7 +108,7 @@ public class OVRStereoDeviceRenderer implements StereoDeviceRenderer {
         public final EyeParameter getEyeParameter() { return eyeParameter; }
 
         /* pp */ OVREye(final ovrHmdDesc hmdDesc, final int distortionBits,
-                        final float[] eyePositionOffset, final ovrEyeRenderDesc eyeDesc,
+                        final Vec3f eyePositionOffset, final ovrEyeRenderDesc eyeDesc,
                         final ovrSizei ovrTextureSize, final RectangleImmutable eyeViewport) {
             this.eyeName = eyeDesc.getEye();
             this.distortionBits = distortionBits;
@@ -398,7 +399,7 @@ public class OVRStereoDeviceRenderer implements StereoDeviceRenderer {
     }
 
     /* pp */ OVRStereoDeviceRenderer(final OVRStereoDevice context, final int distortionBits,
-                             final int textureCount, final float[] eyePositionOffset,
+                             final int textureCount, final Vec3f eyePositionOffset,
                              final ovrEyeRenderDesc[] eyeRenderDescs,
                              final DimensionImmutable[] eyeTextureSizes, final DimensionImmutable totalTextureSize,
                              final RectangleImmutable[] eyeViewports, final int textureUnit) {
diff --git a/src/test/com/jogamp/opengl/test/junit/graph/GPUTextRendererListenerBase01.java b/src/test/com/jogamp/opengl/test/junit/graph/GPUTextRendererListenerBase01.java
index 90700e35a..9af3bd99b 100644
--- a/src/test/com/jogamp/opengl/test/junit/graph/GPUTextRendererListenerBase01.java
+++ b/src/test/com/jogamp/opengl/test/junit/graph/GPUTextRendererListenerBase01.java
@@ -491,7 +491,7 @@ public abstract class GPUTextRendererListenerBase01 extends GPURendererListenerB
         System.err.println("Matrix: " + getXTran() + "/" + getYTran() + " x"+getZTran() + " @"+getAngle() +" fontSize "+fontSizeCenter);
         if(bbox) {
             System.err.println("bbox em: "+font.getMetricBounds(text2));
-            System.err.println("bbox px: "+font.getMetricBounds(text2).scale(nearPlaneS * FontScale.toPixels(fontSizeCenter, dpiV), new float[3]));
+            System.err.println("bbox px: "+font.getMetricBounds(text2).scale(nearPlaneS * FontScale.toPixels(fontSizeCenter, dpiV)));
         }
     }
 
diff --git a/src/test/com/jogamp/opengl/test/junit/graph/TestFontsNEWT00.java b/src/test/com/jogamp/opengl/test/junit/graph/TestFontsNEWT00.java
index 25af3d910..afdecfaca 100644
--- a/src/test/com/jogamp/opengl/test/junit/graph/TestFontsNEWT00.java
+++ b/src/test/com/jogamp/opengl/test/junit/graph/TestFontsNEWT00.java
@@ -102,7 +102,7 @@ public class TestFontsNEWT00 extends UITestCase {
         System.err.println("            px "+s0_px+", "+s1_px);
         System.err.println("      AABBox");
         System.err.println("        funits "+glyph.getBoundsFU());
-        System.err.println("            em "+glyph.getBounds(new AABBox(), new float[3]));
+        System.err.println("            em "+glyph.getBounds(new AABBox()));
 
         Assert.assertEquals(s0, s1);
 
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/demos/es2/GearsES2.java b/src/test/com/jogamp/opengl/test/junit/jogl/demos/es2/GearsES2.java
index 3dc951829..88a3d7326 100644
--- a/src/test/com/jogamp/opengl/test/junit/jogl/demos/es2/GearsES2.java
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/demos/es2/GearsES2.java
@@ -32,9 +32,9 @@ import com.jogamp.newt.event.PinchToZoomGesture;
 import com.jogamp.newt.event.GestureHandler.GestureEvent;
 import com.jogamp.opengl.GLRendererQuirks;
 import com.jogamp.opengl.JoglVersion;
-import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Matrix4f;
 import com.jogamp.opengl.math.Quaternion;
-import com.jogamp.opengl.math.VectorUtil;
+import com.jogamp.opengl.math.Vec3f;
 import com.jogamp.opengl.test.junit.jogl.demos.GearsObject;
 import com.jogamp.opengl.util.CustomGLEventListener;
 import com.jogamp.opengl.util.PMVMatrix;
@@ -405,49 +405,59 @@ public class GearsES2 implements StereoGLEventListener, TileRendererBase.TileRen
     }
     // private boolean useAndroidDebug = false;
 
-    private final float[] mat4Tmp1 = new float[16];
-    private final float[] mat4Tmp2 = new float[16];
-    private final float[] vec3Tmp1 = new float[3];
-    private final float[] vec3Tmp2 = new float[3];
-    private final float[] vec3Tmp3 = new float[3];
+    private final Matrix4f mat4Tmp1 = new Matrix4f();
+    private final Matrix4f mat4Tmp2 = new Matrix4f();
+    private final Vec3f vec3Tmp1 = new Vec3f();
+    private final Vec3f vec3Tmp2 = new Vec3f();
+    private final Vec3f vec3Tmp3 = new Vec3f();
 
-    private static final float[] vec3ScalePos = new float[] { 20f, 20f, 20f };
+    private static final float scalePos = 20f;
 
     @Override
     public void reshapeForEye(final GLAutoDrawable drawable, final int x, final int y, final int width, final int height,
                               final EyeParameter eyeParam, final ViewerPose viewerPose) {
         final GL2ES2 gl = drawable.getGL().getGL2ES2();
-        pmvMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
-        final float[] mat4Projection = FloatUtil.makePerspective(mat4Tmp1, 0, true, eyeParam.fovhv, zNear, zFar);
-        if( flipVerticalInGLOrientation && gl.getContext().getGLDrawable().isGLOriented() ) {
-            pmvMatrix.glLoadIdentity();
-            pmvMatrix.glScalef(1f, -1f, 1f);
-            pmvMatrix.glMultMatrixf(mat4Projection, 0);
-        } else {
-            pmvMatrix.glLoadMatrixf(mat4Projection, 0);
-        }
-
-        pmvMatrix.glMatrixMode(GLMatrixFunc.GL_MODELVIEW);
-
-        final Quaternion rollPitchYaw = new Quaternion();
-        // private final float eyeYaw = FloatUtil.PI; // 180 degrees in radians
-        // rollPitchYaw.rotateByAngleY(eyeYaw);
-        // final float[] shiftedEyePos = rollPitchYaw.rotateVector(vec3Tmp1, 0, viewerPose.position, 0);
-        final float[] shiftedEyePos = VectorUtil.copyVec3(vec3Tmp1, 0, viewerPose.position, 0);
-        VectorUtil.scaleVec3(shiftedEyePos, shiftedEyePos, vec3ScalePos); // amplify viewerPose position
-        VectorUtil.addVec3(shiftedEyePos, shiftedEyePos, eyeParam.positionOffset);
-
-        rollPitchYaw.mult(viewerPose.orientation);
-        final float[] up = rollPitchYaw.rotateVector(vec3Tmp2, 0, VectorUtil.VEC3_UNIT_Y, 0);
-        final float[] forward = rollPitchYaw.rotateVector(vec3Tmp3, 0, VectorUtil.VEC3_UNIT_Z_NEG, 0);
-        final float[] center = VectorUtil.addVec3(forward, shiftedEyePos, forward);
 
-        final float[] mLookAt = FloatUtil.makeLookAt(mat4Tmp1, 0, shiftedEyePos, 0, center, 0, up, 0, mat4Tmp2);
-        final float[] mViewAdjust = FloatUtil.makeTranslation(mat4Tmp2, true, eyeParam.distNoseToPupilX, eyeParam.distMiddleToPupilY, eyeParam.eyeReliefZ);
-        final float[] mat4Modelview = FloatUtil.multMatrix(mViewAdjust, mLookAt);
+        {
+            //
+            // Projection
+            //
+            final Matrix4f mat4 = new Matrix4f();
+            pmvMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
+            if( flipVerticalInGLOrientation && gl.getContext().getGLDrawable().isGLOriented() ) {
+                mat4Tmp1.setToScale(1f, -1f, 1f);
+                mat4Tmp2.setToPerspective(eyeParam.fovhv, zNear, zFar);
+                mat4.mul(mat4Tmp1, mat4Tmp2);
 
-        pmvMatrix.glLoadMatrixf(mat4Modelview, 0);
-        pmvMatrix.glTranslatef(0.0f, 0.0f, -zViewDist);
+            } else {
+                mat4.setToPerspective(eyeParam.fovhv, zNear, zFar);
+            }
+            pmvMatrix.glLoadMatrixf(mat4);
+
+            //
+            // Modelview
+            //
+            pmvMatrix.glMatrixMode(GLMatrixFunc.GL_MODELVIEW);
+            final Quaternion rollPitchYaw = new Quaternion();
+            // private final float eyeYaw = FloatUtil.PI; // 180 degrees in radians
+            // rollPitchYaw.rotateByAngleY(eyeYaw);
+            // final Vec3f shiftedEyePos = rollPitchYaw.rotateVector(viewerPose.position, vec3Tmp1);
+            final Vec3f shiftedEyePos = vec3Tmp1.set(viewerPose.position);
+            shiftedEyePos.scale(scalePos); // amplify viewerPose position
+            shiftedEyePos.add(eyeParam.positionOffset);
+
+            rollPitchYaw.mult(viewerPose.orientation);
+            final Vec3f up = rollPitchYaw.rotateVector(Vec3f.UNIT_Y, vec3Tmp2);
+            final Vec3f forward = rollPitchYaw.rotateVector(Vec3f.UNIT_Z_NEG, vec3Tmp3); // -> center
+            final Vec3f center = forward.add(shiftedEyePos);
+
+            final Matrix4f mLookAt = mat4Tmp2.setToLookAt(shiftedEyePos, center, up, mat4Tmp1);
+            mat4.mul( mat4Tmp1.setToTranslation( eyeParam.distNoseToPupilX,
+                                                 eyeParam.distMiddleToPupilY,
+                                                 eyeParam.eyeReliefZ ), mLookAt);
+            mat4.translate(0, 0, -zViewDist, mat4Tmp1);
+            pmvMatrix.glLoadMatrixf(mat4);
+        }
         st.useProgram(gl, true);
         st.uniform(gl, pmvMatrixUniform);
         st.useProgram(gl, false);
@@ -592,6 +602,7 @@ public class GearsES2 implements StereoGLEventListener, TileRendererBase.TileRen
     }
 
     class GearsKeyAdapter extends KeyAdapter {
+        @Override
         public void keyPressed(final KeyEvent e) {
             final int kc = e.getKeyCode();
             if(KeyEvent.VK_LEFT == kc) {
@@ -636,6 +647,7 @@ public class GearsES2 implements StereoGLEventListener, TileRendererBase.TileRen
             }
         }
 
+        @Override
         public void mousePressed(final MouseEvent e) {
             if( e.getPointerCount()==1 ) {
                 prevMouseX = e.getX();
@@ -648,9 +660,11 @@ public class GearsES2 implements StereoGLEventListener, TileRendererBase.TileRen
             }
         }
 
+        @Override
         public void mouseReleased(final MouseEvent e) {
         }
 
+        @Override
         public void mouseMoved(final MouseEvent e) {
             if( e.isConfined() ) {
                 navigate(e);
@@ -662,6 +676,7 @@ public class GearsES2 implements StereoGLEventListener, TileRendererBase.TileRen
             }
         }
 
+        @Override
         public void mouseDragged(final MouseEvent e) {
             navigate(e);
         }
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/demos/gl4/TriangleInstancedRendererWithShaderState.java b/src/test/com/jogamp/opengl/test/junit/jogl/demos/gl4/TriangleInstancedRendererWithShaderState.java
index 560e8aa23..841f2037c 100644
--- a/src/test/com/jogamp/opengl/test/junit/jogl/demos/gl4/TriangleInstancedRendererWithShaderState.java
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/demos/gl4/TriangleInstancedRendererWithShaderState.java
@@ -9,7 +9,6 @@ import java.util.Random;
 
 import com.jogamp.opengl.DebugGL4;
 import com.jogamp.opengl.GL;
-import com.jogamp.opengl.GL2;
 import com.jogamp.opengl.GL2ES2;
 import com.jogamp.opengl.GL4;
 import com.jogamp.opengl.GLAutoDrawable;
@@ -17,8 +16,8 @@ import com.jogamp.opengl.GLEventListener;
 import com.jogamp.opengl.GLException;
 import com.jogamp.opengl.GLUniformData;
 import com.jogamp.opengl.TraceGL4;
-
-import com.jogamp.opengl.math.Matrix4;
+import com.jogamp.opengl.fixedfunc.GLMatrixFunc;
+import com.jogamp.opengl.math.Matrix4f;
 import com.jogamp.opengl.util.GLArrayDataClient;
 import com.jogamp.opengl.util.GLArrayDataServer;
 import com.jogamp.opengl.util.PMVMatrix;
@@ -44,7 +43,7 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 
 	private static final int NO_OF_INSTANCE = 30;
 	private final FloatBuffer triangleTransform = FloatBuffer.allocate(16 * NO_OF_INSTANCE);
-	private final Matrix4[] mat = new Matrix4[NO_OF_INSTANCE];
+	private final Matrix4f[] mat = new Matrix4f[NO_OF_INSTANCE];
 	private final float[] rotationSpeed = new float[NO_OF_INSTANCE];
 
 	private static final boolean useTraceGL = false;
@@ -53,13 +52,13 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 
 	private boolean isInitialized = false;
 
-	public TriangleInstancedRendererWithShaderState(IInstancedRenderingView view) {
+	public TriangleInstancedRendererWithShaderState(final IInstancedRenderingView view) {
 		this.view = view;
 
 		if(useTraceGL) {
 			try {
 				stream = new PrintStream(new FileOutputStream(new File("instanced-with-st.txt")));
-			} catch (IOException e1) {
+			} catch (final IOException e1) {
 				e1.printStackTrace();
 			}
 		}
@@ -68,23 +67,24 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 	}
 
 	private void initTransform() {
-		Random rnd = new Random();
+		final Random rnd = new Random();
+		final Matrix4f tmp = new Matrix4f();
 		for(int i = 0; i < NO_OF_INSTANCE; i++) {
 			rotationSpeed[i] = 0.3f * rnd.nextFloat();
-			mat[i] = new Matrix4();
+			mat[i] = new Matrix4f();
 			mat[i].loadIdentity();
-			float scale = 1f + 4 * rnd.nextFloat();
-			mat[i].scale(scale, scale, scale);
+			final float scale = 1f + 4 * rnd.nextFloat();
+			mat[i].scale(scale, tmp);
 			//setup initial position of each triangle
 			mat[i].translate(20f * rnd.nextFloat() - 10f,
 							 10f * rnd.nextFloat() -  5f,
-							 0f);
+							 0f, tmp);
 		}
 	}
 
 	@Override
-	public void init(GLAutoDrawable drawable) {
-		GL4 gl = drawable.getGL().getGL4();
+	public void init(final GLAutoDrawable drawable) {
+		final GL4 gl = drawable.getGL().getGL4();
 		drawable.setGL(new DebugGL4(gl));
 		if(useTraceGL) {
 			drawable.setGL(new TraceGL4(gl, stream));
@@ -96,9 +96,9 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 
 		System.err.println("Chosen GLCapabilities: " + drawable.getChosenGLCapabilities());
 		System.err.println("INIT GL IS: " + gl.getClass().getName());
-		System.err.println("GL_VENDOR: " + gl.glGetString(GL4.GL_VENDOR));
-		System.err.println("GL_RENDERER: " + gl.glGetString(GL4.GL_RENDERER));
-		System.err.println("GL_VERSION: " + gl.glGetString(GL4.GL_VERSION));
+		System.err.println("GL_VENDOR: " + gl.glGetString(GL.GL_VENDOR));
+		System.err.println("GL_RENDERER: " + gl.glGetString(GL.GL_RENDERER));
+		System.err.println("GL_VERSION: " + gl.glGetString(GL.GL_VERSION));
 
 		initShader(gl);
         projectionMatrix = new PMVMatrix();
@@ -125,14 +125,14 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 	}
 
 	@Override
-	public void display(GLAutoDrawable drawable) {
+	public void display(final GLAutoDrawable drawable) {
 		if(!isInitialized ) return;
 
-		GL4 gl = drawable.getGL().getGL4();
-		gl.glClear(GL4.GL_COLOR_BUFFER_BIT | GL4.GL_DEPTH_BUFFER_BIT);
+		final GL4 gl = drawable.getGL().getGL4();
+		gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT);
 
 		st.useProgram(gl, true);
-		projectionMatrix.glMatrixMode(GL2.GL_PROJECTION);
+		projectionMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
 		projectionMatrix.glPushMatrix();
 
 		float winScale = 0.1f;
@@ -151,7 +151,7 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 			colorsVBO.enableBuffer(gl, true);
 		}
 		//gl.glVertexAttribDivisor() is not required since each instance has the same attribute (color).
-		gl.glDrawArraysInstanced(GL4.GL_TRIANGLES, 0, 3, NO_OF_INSTANCE);
+		gl.glDrawArraysInstanced(GL.GL_TRIANGLES, 0, 3, NO_OF_INSTANCE);
 		if(useInterleaved) {
 			interleavedVBO.enableBuffer(gl, false);
 		} else {
@@ -162,42 +162,43 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 	}
 
 	@Override
-	public void reshape(GLAutoDrawable drawable, int x, int y, int width, int height) {
-		GL4 gl3 = drawable.getGL().getGL4();
+	public void reshape(final GLAutoDrawable drawable, final int x, final int y, final int width, final int height) {
+		final GL4 gl3 = drawable.getGL().getGL4();
 		gl3.glViewport(0, 0, width, height);
 		aspect = (float) width / (float) height;
 
-		projectionMatrix.glMatrixMode(GL2.GL_PROJECTION);
+		projectionMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
 		projectionMatrix.glLoadIdentity();
 		projectionMatrix.gluPerspective(45, aspect, 0.001f, 20f);
 		projectionMatrix.gluLookAt(0, 0, -10, 0, 0, 0, 0, 1, 0);
 	}
 
 	@Override
-	public void dispose(GLAutoDrawable drawable){
-		GL4 gl = drawable.getGL().getGL4();
+	public void dispose(final GLAutoDrawable drawable){
+		final GL4 gl = drawable.getGL().getGL4();
 		st.destroy(gl);
 	}
 
 	private void generateTriangleTransform() {
 		triangleTransform.clear();
+		final Matrix4f tmp = new Matrix4f();
 		for(int i = 0; i < NO_OF_INSTANCE; i++) {
-			mat[i].rotate(rotationSpeed[i], 0, 0, 1);
-			triangleTransform.put(mat[i].getMatrix());
+			mat[i].rotate(rotationSpeed[i], 0, 0, 1, tmp);
+			mat[i].get(triangleTransform);
 		}
 		triangleTransform.rewind();
 	}
 
-	private void initVBO_nonInterleaved(GL4 gl) {
-		int VERTEX_COUNT = 3;
+	private void initVBO_nonInterleaved(final GL4 gl) {
+		final int VERTEX_COUNT = 3;
 
-        verticesVBO = GLArrayDataClient.createGLSL("mgl_Vertex", 3, GL4.GL_FLOAT, false, VERTEX_COUNT);
-        FloatBuffer verticeBuf = (FloatBuffer)verticesVBO.getBuffer();
+        verticesVBO = GLArrayDataClient.createGLSL("mgl_Vertex", 3, GL.GL_FLOAT, false, VERTEX_COUNT);
+        final FloatBuffer verticeBuf = (FloatBuffer)verticesVBO.getBuffer();
         verticeBuf.put(vertices);
         verticesVBO.seal(gl, true);
 
-        colorsVBO = GLArrayDataClient.createGLSL("mgl_Color",  4, GL4.GL_FLOAT, false, VERTEX_COUNT);
-        FloatBuffer colorBuf = (FloatBuffer)colorsVBO.getBuffer();
+        colorsVBO = GLArrayDataClient.createGLSL("mgl_Color",  4, GL.GL_FLOAT, false, VERTEX_COUNT);
+        final FloatBuffer colorBuf = (FloatBuffer)colorsVBO.getBuffer();
         colorBuf.put(colors);
         colorsVBO.seal(gl, true);
 
@@ -209,13 +210,13 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 		st.useProgram(gl, false);
 	}
 
-	private void initVBO_interleaved(GL4 gl) {
-		int VERTEX_COUNT = 3;
+	private void initVBO_interleaved(final GL4 gl) {
+		final int VERTEX_COUNT = 3;
 		interleavedVBO = GLArrayDataServer.createGLSLInterleaved(3 + 4, GL.GL_FLOAT, false, VERTEX_COUNT, GL.GL_STATIC_DRAW);
         interleavedVBO.addGLSLSubArray("mgl_Vertex", 3, GL.GL_ARRAY_BUFFER);
         interleavedVBO.addGLSLSubArray("mgl_Color",  4, GL.GL_ARRAY_BUFFER);
 
-        FloatBuffer ib = (FloatBuffer)interleavedVBO.getBuffer();
+        final FloatBuffer ib = (FloatBuffer)interleavedVBO.getBuffer();
 
         for(int i = 0; i < VERTEX_COUNT; i++) {
             ib.put(vertices,  i*3, 3);
@@ -227,11 +228,11 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
 		st.useProgram(gl, false);
 	}
 
-    private void initShader(GL4 gl) {
+    private void initShader(final GL4 gl) {
         // Create & Compile the shader objects
-        ShaderCode vp0 = ShaderCode.create(gl, GL2ES2.GL_VERTEX_SHADER, this.getClass(),
+        final ShaderCode vp0 = ShaderCode.create(gl, GL2ES2.GL_VERTEX_SHADER, this.getClass(),
                                             "shader", "shader/bin", shaderBasename, true);
-        ShaderCode fp0 = ShaderCode.create(gl, GL2ES2.GL_FRAGMENT_SHADER, this.getClass(),
+        final ShaderCode fp0 = ShaderCode.create(gl, GL2ES2.GL_FRAGMENT_SHADER, this.getClass(),
                                             "shader", "shader/bin", shaderBasename, true);
         vp0.replaceInShaderSource("NO_OF_INSTANCE", String.valueOf(NO_OF_INSTANCE));
 
@@ -241,7 +242,7 @@ public class TriangleInstancedRendererWithShaderState implements GLEventListener
         //vp0.dumpShaderSource(System.out);
 
         // Create & Link the shader program
-        ShaderProgram sp = new ShaderProgram();
+        final ShaderProgram sp = new ShaderProgram();
         sp.add(vp0);
         sp.add(fp0);
         if(!sp.link(gl, System.err)) {
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/demos/gl4/TrianglesInstancedRendererHardcoded.java b/src/test/com/jogamp/opengl/test/junit/jogl/demos/gl4/TrianglesInstancedRendererHardcoded.java
index fb0a78832..a1f8ff5f6 100644
--- a/src/test/com/jogamp/opengl/test/junit/jogl/demos/gl4/TrianglesInstancedRendererHardcoded.java
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/demos/gl4/TrianglesInstancedRendererHardcoded.java
@@ -8,14 +8,15 @@ import java.nio.FloatBuffer;
 import java.util.Random;
 
 import com.jogamp.opengl.DebugGL4;
-import com.jogamp.opengl.GL2;
+import com.jogamp.opengl.GL;
+import com.jogamp.opengl.GL2ES2;
 import com.jogamp.opengl.GL4;
 import com.jogamp.opengl.GLAutoDrawable;
 import com.jogamp.opengl.GLEventListener;
 import com.jogamp.opengl.TraceGL4;
-
+import com.jogamp.opengl.fixedfunc.GLMatrixFunc;
 import com.jogamp.common.nio.Buffers;
-import com.jogamp.opengl.math.Matrix4;
+import com.jogamp.opengl.math.Matrix4f;
 import com.jogamp.opengl.util.PMVMatrix;
 
 public class TrianglesInstancedRendererHardcoded implements GLEventListener {
@@ -32,7 +33,7 @@ public class TrianglesInstancedRendererHardcoded implements GLEventListener {
 
 	private static final int NO_OF_INSTANCE = 30;
 	private final FloatBuffer triangleTransform = FloatBuffer.allocate(16 * NO_OF_INSTANCE);
-	private final Matrix4[] mat = new Matrix4[NO_OF_INSTANCE];
+	private final Matrix4f[] mat = new Matrix4f[NO_OF_INSTANCE];
 	private final float[] rotationSpeed = new float[NO_OF_INSTANCE];
 
 	private int[] vbo;
@@ -42,22 +43,22 @@ public class TrianglesInstancedRendererHardcoded implements GLEventListener {
 
 	private static final boolean useTraceGL = false;
 
-	public TrianglesInstancedRendererHardcoded(IInstancedRenderingView view) {
+	public TrianglesInstancedRendererHardcoded(final IInstancedRenderingView view) {
 		this.view = view;
 		initTransform();
 
 		if(useTraceGL) {
 			try {
 				stream = new PrintStream(new FileOutputStream(new File("instanced.txt")));
-			} catch (IOException e1) {
+			} catch (final IOException e1) {
 				e1.printStackTrace();
 			}
 		}
 	}
 
 	@Override
-	public void init(GLAutoDrawable drawable) {
-		GL4 gl = drawable.getGL().getGL4();
+	public void init(final GLAutoDrawable drawable) {
+		final GL4 gl = drawable.getGL().getGL4();
 		drawable.setGL(new DebugGL4(gl));
 		if(useTraceGL) {
 			drawable.setGL(new TraceGL4(gl, stream));
@@ -69,27 +70,27 @@ public class TrianglesInstancedRendererHardcoded implements GLEventListener {
 
 		System.err.println("Chosen GLCapabilities: " + drawable.getChosenGLCapabilities());
 		System.err.println("INIT GL IS: " + gl.getClass().getName());
-		System.err.println("GL_VENDOR: " + gl.glGetString(GL4.GL_VENDOR));
-		System.err.println("GL_RENDERER: " + gl.glGetString(GL4.GL_RENDERER));
-		System.err.println("GL_VERSION: " + gl.glGetString(GL4.GL_VERSION));
+		System.err.println("GL_VENDOR: " + gl.glGetString(GL.GL_VENDOR));
+		System.err.println("GL_RENDERER: " + gl.glGetString(GL.GL_RENDERER));
+		System.err.println("GL_VERSION: " + gl.glGetString(GL.GL_VERSION));
 
 		try {
 			initShaders(gl);
-		} catch (IOException e) {
+		} catch (final IOException e) {
 			e.printStackTrace();
 		}
 		initVBO(gl);
 	}
 
 	@Override
-	public void display(GLAutoDrawable drawable) {
+	public void display(final GLAutoDrawable drawable) {
 
-		GL4 gl = drawable.getGL().getGL4();
-		gl.glClear(GL4.GL_COLOR_BUFFER_BIT | GL4.GL_DEPTH_BUFFER_BIT);
+		final GL4 gl = drawable.getGL().getGL4();
+		gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT);
 
 		gl.glUseProgram(shaderProgram);
 
-		projectionMatrix.glMatrixMode(GL2.GL_PROJECTION);
+		projectionMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
 
 		projectionMatrix.glPushMatrix();
 		float winScale = 0.1f;
@@ -104,28 +105,28 @@ public class TrianglesInstancedRendererHardcoded implements GLEventListener {
 		gl.glUniformMatrix4fv(transformMatrixLocation, NO_OF_INSTANCE, false, triangleTransform);
 
 		gl.glBindVertexArray(vao[0]);
-		gl.glDrawArraysInstanced(GL4.GL_TRIANGLES, 0, 3, NO_OF_INSTANCE);
+		gl.glDrawArraysInstanced(GL.GL_TRIANGLES, 0, 3, NO_OF_INSTANCE);
 		gl.glBindVertexArray(0);
 		gl.glUseProgram(0);
 	}
 
 	@Override
-	public void reshape(GLAutoDrawable drawable, int x, int y, int width, int height) {
+	public void reshape(final GLAutoDrawable drawable, final int x, final int y, final int width, final int height) {
 		System.out.println("Window resized to width=" + width + " height=" + height);
-		GL4 gl3 = drawable.getGL().getGL4();
+		final GL4 gl3 = drawable.getGL().getGL4();
 		gl3.glViewport(0, 0, width, height);
 		aspect = (float) width / (float) height;
 
 		projectionMatrix = new PMVMatrix();
-		projectionMatrix.glMatrixMode(GL2.GL_PROJECTION);
+		projectionMatrix.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
 		projectionMatrix.glLoadIdentity();
 		projectionMatrix.gluPerspective(45, aspect, 0.001f, 20f);
 		projectionMatrix.gluLookAt(0, 0, -10, 0, 0, 0, 0, 1, 0);
 	}
 
 	@Override
-	public void dispose(GLAutoDrawable drawable){
-		GL4 gl = drawable.getGL().getGL4();
+	public void dispose(final GLAutoDrawable drawable){
+		final GL4 gl = drawable.getGL().getGL4();
 		gl.glUseProgram(0);
 		gl.glDeleteBuffers(2, vbo, 0);
 		gl.glDetachShader(shaderProgram, vertShader);
@@ -136,22 +137,23 @@ public class TrianglesInstancedRendererHardcoded implements GLEventListener {
 	}
 
 	private void initTransform() {
-		Random rnd = new Random();
+		final Random rnd = new Random();
+		final Matrix4f tmp = new Matrix4f();
 		for(int i = 0; i < NO_OF_INSTANCE; i++) {
 			rotationSpeed[i] = 0.3f * rnd.nextFloat();
-			mat[i] = new Matrix4();
+			mat[i] = new Matrix4f();
 			mat[i].loadIdentity();
-			float scale = 1f + 4 * rnd.nextFloat();
-			mat[i].scale(scale, scale, scale);
+			final float scale = 1f + 4 * rnd.nextFloat();
+			mat[i].scale(scale, tmp);
 			//setup initial position of each triangle
 			mat[i].translate(20f * rnd.nextFloat() - 10f,
 							 10f * rnd.nextFloat() -  5f,
-							 0f);
+							 0f, tmp);
 		}
 	}
 
-	private void initVBO(GL4 gl) {
-		FloatBuffer interleavedBuffer = Buffers.newDirectFloatBuffer(vertices.length + colors.length);
+	private void initVBO(final GL4 gl) {
+		final FloatBuffer interleavedBuffer = Buffers.newDirectFloatBuffer(vertices.length + colors.length);
 		for(int i = 0; i < vertices.length/3; i++) {
 			for(int j = 0; j < 3; j++) {
 				interleavedBuffer.put(vertices[i*3 + j]);
@@ -167,54 +169,54 @@ public class TrianglesInstancedRendererHardcoded implements GLEventListener {
 		gl.glBindVertexArray(vao[0]);
 		vbo = new int[1];
 		gl.glGenBuffers(1, vbo, 0);
-		gl.glBindBuffer(GL4.GL_ARRAY_BUFFER, vbo[0]);
-		gl.glBufferData(GL4.GL_ARRAY_BUFFER, interleavedBuffer.limit() * Buffers.SIZEOF_FLOAT, interleavedBuffer, GL4.GL_STATIC_DRAW);
+		gl.glBindBuffer(GL.GL_ARRAY_BUFFER, vbo[0]);
+		gl.glBufferData(GL.GL_ARRAY_BUFFER, interleavedBuffer.limit() * Buffers.SIZEOF_FLOAT, interleavedBuffer, GL.GL_STATIC_DRAW);
 
 		gl.glEnableVertexAttribArray(locPos);
 		gl.glEnableVertexAttribArray(locCol);
 
-		int stride = Buffers.SIZEOF_FLOAT * (3+4);
-		gl.glVertexAttribPointer( locPos, 3, GL4.GL_FLOAT, false, stride, 0);
-		gl.glVertexAttribPointer( locCol, 4, GL4.GL_FLOAT, false, stride, Buffers.SIZEOF_FLOAT * 3);
+		final int stride = Buffers.SIZEOF_FLOAT * (3+4);
+		gl.glVertexAttribPointer( locPos, 3, GL.GL_FLOAT, false, stride, 0);
+		gl.glVertexAttribPointer( locCol, 4, GL.GL_FLOAT, false, stride, Buffers.SIZEOF_FLOAT * 3);
 	}
 
-	private void initShaders(GL4 gl) throws IOException {
-		vertShader = gl.glCreateShader(GL4.GL_VERTEX_SHADER);
-		fragShader = gl.glCreateShader(GL4.GL_FRAGMENT_SHADER);
+	private void initShaders(final GL4 gl) throws IOException {
+		vertShader = gl.glCreateShader(GL2ES2.GL_VERTEX_SHADER);
+		fragShader = gl.glCreateShader(GL2ES2.GL_FRAGMENT_SHADER);
 
-		String[] vlines = new String[] { vertexShaderString };
-		int[] vlengths = new int[] { vlines[0].length() };
+		final String[] vlines = new String[] { vertexShaderString };
+		final int[] vlengths = new int[] { vlines[0].length() };
 		gl.glShaderSource(vertShader, vlines.length, vlines, vlengths, 0);
 		gl.glCompileShader(vertShader);
 
-		int[] compiled = new int[1];
-		gl.glGetShaderiv(vertShader, GL4.GL_COMPILE_STATUS, compiled, 0);
+		final int[] compiled = new int[1];
+		gl.glGetShaderiv(vertShader, GL2ES2.GL_COMPILE_STATUS, compiled, 0);
 		if(compiled[0] != 0) {
 			System.out.println("Vertex shader compiled");
 		} else {
-			int[] logLength = new int[1];
-			gl.glGetShaderiv(vertShader, GL4.GL_INFO_LOG_LENGTH, logLength, 0);
+			final int[] logLength = new int[1];
+			gl.glGetShaderiv(vertShader, GL2ES2.GL_INFO_LOG_LENGTH, logLength, 0);
 
-			byte[] log = new byte[logLength[0]];
+			final byte[] log = new byte[logLength[0]];
 			gl.glGetShaderInfoLog(vertShader, logLength[0], (int[])null, 0, log, 0);
 
 			System.err.println("Error compiling the vertex shader: " + new String(log));
 			System.exit(1);
 		}
 
-		String[] flines = new String[] { fragmentShaderString };
-		int[] flengths = new int[] { flines[0].length() };
+		final String[] flines = new String[] { fragmentShaderString };
+		final int[] flengths = new int[] { flines[0].length() };
 		gl.glShaderSource(fragShader, flines.length, flines, flengths, 0);
 		gl.glCompileShader(fragShader);
 
-		gl.glGetShaderiv(fragShader, GL4.GL_COMPILE_STATUS, compiled, 0);
+		gl.glGetShaderiv(fragShader, GL2ES2.GL_COMPILE_STATUS, compiled, 0);
 		if(compiled[0] != 0){
 			System.out.println("Fragment shader compiled.");
 		} else {
-			int[] logLength = new int[1];
-			gl.glGetShaderiv(fragShader, GL4.GL_INFO_LOG_LENGTH, logLength, 0);
+			final int[] logLength = new int[1];
+			gl.glGetShaderiv(fragShader, GL2ES2.GL_INFO_LOG_LENGTH, logLength, 0);
 
-			byte[] log = new byte[logLength[0]];
+			final byte[] log = new byte[logLength[0]];
 			gl.glGetShaderInfoLog(fragShader, logLength[0], (int[])null, 0, log, 0);
 
 			System.err.println("Error compiling the fragment shader: " + new String(log));
@@ -238,11 +240,12 @@ public class TrianglesInstancedRendererHardcoded implements GLEventListener {
 
 	private void generateTriangleTransform() {
 		triangleTransform.clear();
+		final Matrix4f tmp = new Matrix4f();
 		for(int i = 0; i < NO_OF_INSTANCE; i++) {
 			//		mat[i].translate(0.1f, 0.1f, 0);
-			mat[i].rotate(rotationSpeed[i], 0, 0, 1);
+			mat[i].rotate(rotationSpeed[i], 0, 0, 1, tmp);
 			//		mat[i].translate(-0.1f, -0.1f, 0);
-			triangleTransform.put(mat[i].getMatrix());
+			mat[i].get(triangleTransform);
 		}
 		triangleTransform.flip();
 	}
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/Matrix4fb.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/Matrix4fb.java
new file mode 100644
index 000000000..fb4c11b7c
--- /dev/null
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/Matrix4fb.java
@@ -0,0 +1,1784 @@
+/**
+ * Copyright 2014-2023 JogAmp Community. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, are
+ * permitted provided that the following conditions are met:
+ *
+ *    1. Redistributions of source code must retain the above copyright notice, this list of
+ *       conditions and the following disclaimer.
+ *
+ *    2. Redistributions in binary form must reproduce the above copyright notice, this list
+ *       of conditions and the following disclaimer in the documentation and/or other materials
+ *       provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * The views and conclusions contained in the software and documentation are those of the
+ * authors and should not be interpreted as representing official policies, either expressed
+ * or implied, of JogAmp Community.
+ */
+
+package com.jogamp.opengl.test.junit.jogl.math;
+
+import java.nio.FloatBuffer;
+
+import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.FovHVHalves;
+import com.jogamp.opengl.math.Quaternion;
+import com.jogamp.opengl.math.Ray;
+import com.jogamp.opengl.math.Vec3f;
+import com.jogamp.opengl.math.Vec4f;
+import com.jogamp.opengl.math.VectorUtil;
+import com.jogamp.opengl.math.geom.AABBox;
+import com.jogamp.opengl.math.geom.Frustum;
+import com.jogamp.opengl.math.geom.Frustum.Plane;
+
+/**
+ * Basic 4x4 float matrix implementation using fields for intensive use-cases (host operations).
+ * 
    
+ * Implementation covers {@link FloatUtil} matrix functionality, exposed in an object oriented manner.
+ * 
    
+ * 
    
+ * Unlike {@link com.jogamp.opengl.util.PMVMatrix PMVMatrix}, this class only represents one single matrix
+ * without a complete {@link com.jogamp.opengl.fixedfunc.GLMatrixFunc GLMatrixFunc} implementation.
+ * 
    
+ * 
    
+ * For array operations the layout is expected in column-major order
+ * matching OpenGL's implementation, illustration:
+ * 
    +    Row-Major                       Column-Major (OpenGL):
+
+        |  0   1   2  tx |
+        |                |
+        |  4   5   6  ty |
+    M = |                |
+        |  8   9  10  tz |
+        |                |
+        | 12  13  14  15 |
+
+           R   C                      R   C
+         m[0*4+3] = tx;             m[0+4*3] = tx;
+         m[1*4+3] = ty;             m[1+4*3] = ty;
+         m[2*4+3] = tz;             m[2+4*3] = tz;
+
+          RC (std subscript order)   RC (std subscript order)
+         m[0+3*4] = tx;                  m[0+3*4] = tx;
+         m[1+3*4] = ty;                  m[1+3*4] = ty;
+         m[2+3*4] = tz;                  m[2+3*4] = tz;
+
+ * 
    
+ * 
    
+ * 
    
+ * 
    
+ * 
    
+ * 
    
+ * Implementation utilizes unrolling of small vertices and matrices wherever possible
+ * while trying to access memory in a linear fashion for performance reasons, see:
+ * 
    
+ * 
    
+ * @see com.jogamp.opengl.util.PMVMatrix
+ * @see FloatUtil
+ */
+public class Matrix4fb {
+
+    /**
+     * Creates a new identity matrix.
+     */
+    public Matrix4fb() {
+        loadIdentity();
+    }
+
+    /**
+     * Creates a new matrix copying the values of the given {@code src} matrix.
+     */
+    public Matrix4fb(final Matrix4fb src) {
+        load(src);
+    }
+
+    /**
+     * Creates a new matrix based on given float[4*4] column major order.
+     * @param m 4x4 matrix in column-major order
+     */
+    public Matrix4fb(final float[] m) {
+        load(m);
+    }
+
+    /**
+     * Creates a new matrix based on given float[4*4] column major order.
+     * @param m 4x4 matrix in column-major order
+     * @param m_off offset for matrix {@code m}
+     */
+    public Matrix4fb(final float[] m, final int m_off) {
+        load(m, m_off);
+    }
+
+    //
+    // Write to Matrix via load(..)
+    //
+
+    /**
+     * Set this matrix to identity.
+     * 
    +      Translation matrix (Column Order):
+      1 0 0 0
+      0 1 0 0
+      0 0 1 0
+      0 0 0 1
+     * 
    
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb loadIdentity() {
+       m[0+0*4] = m[1+1*4] = m[2+2*4] = m[3+3*4] = 1.0f;
+       m[0+1*4] = m[0+2*4] = m[0+3*4] =
+       m[1+0*4] = m[1+2*4] = m[1+3*4] =
+       m[2+0*4] = m[2+1*4] = m[2+3*4] =
+       m[3+0*4] = m[3+1*4] = m[3+2*4] = 0.0f;
+       return this;
+    }
+
+    /**
+     * Load the values of the given matrix {@code b} to this matrix.
+     * @param src the source values
+     * @return this matrix for chaining
+     */
+    public Matrix4fb load(final Matrix4fb src) {
+        System.arraycopy(src.m, 0, m, 0, 16);
+        return this;
+    }
+
+    /**
+     * Load the values of the given matrix {@code src} to this matrix.
+     * @param src 4x4 matrix float[16] in column-major order
+     * @return this matrix for chaining
+     */
+    public Matrix4fb load(final float[] src) {
+        System.arraycopy(src, 0, m, 0, 16);
+        return this;
+    }
+
+    /**
+     * Load the values of the given matrix {@code src} to this matrix.
+     * @param src 4x4 matrix float[16] in column-major order
+     * @param src_off offset for matrix {@code src}
+     * @return this matrix for chaining
+     */
+    public Matrix4fb load(final float[] src, final int src_off) {
+        System.arraycopy(src, src_off, m, 0, 16);
+        return this;
+    }
+
+    /**
+     * Load the values of the given matrix {@code src} to this matrix.
+     * 
    
+     * Implementation uses relative {@link FloatBuffer#get()},
+     * hence caller may want to issue {@link FloatBuffer#reset()} thereafter.
+     * 
    
+     * @param src 4x4 matrix {@link FloatBuffer} in column-major order
+     * @return this matrix for chaining
+     */
+    public Matrix4fb load(final FloatBuffer src) {
+        src.get(m, 0, 16);
+        return this;
+    }
+
+    //
+    // Read out Matrix via get(..)
+    //
+
+    /** Gets the ith component, 0 <= i < 16 */
+    public float get(final int i) {
+        return m[i];
+    }
+
+    /**
+     * Get the named column of the given column-major matrix to v_out.
+     * @param column named column to copy
+     * @param v_out the column-vector storage
+     * @return given result vector v_out for chaining
+     */
+    public Vec4f getColumn(final int column, final Vec4f v_out) {
+        v_out.set( get(0+column*4),
+                   get(1+column*4),
+                   get(2+column*4),
+                   get(3+column*4) );
+        return v_out;
+    }
+
+    /**
+     * Get the named column of the given column-major matrix to v_out.
+     * @param column named column to copy
+     * @param v_out the column-vector storage
+     * @return given result vector v_out for chaining
+     */
+    public Vec3f getColumn(final int column, final Vec3f v_out) {
+        v_out.set( get(0+column*4),
+                   get(1+column*4),
+                   get(2+column*4) );
+        return v_out;
+    }
+
+    /**
+     * Get the named row of the given column-major matrix to v_out.
+     * @param row named row to copy
+     * @param v_out the row-vector storage
+     * @return given result vector v_out for chaining
+     */
+    public Vec4f getRow(final int row, final Vec4f v_out) {
+        v_out.set( get(row+0*4),
+                   get(row+1*4),
+                   get(row+2*4),
+                   get(row+3*4) );
+        return v_out;
+    }
+
+    /**
+     * Get the named row of the given column-major matrix to v_out.
+     * @param row named row to copy
+     * @param v_out the row-vector storage
+     * @return given result vector v_out for chaining
+     */
+    public Vec3f getRow(final int row, final Vec3f v_out) {
+        v_out.set( get(row+0*4),
+                   get(row+1*4),
+                   get(row+2*4) );
+        return v_out;
+    }
+
+    /**
+     * Get this matrix into the given float[16] array at {@code dst_off} in column major order.
+     *
+     * @param dst float[16] array storage in column major order
+     * @param dst_off offset
+     */
+    public void get(final float[] dst, final int dst_off) {
+        System.arraycopy(m, 0, dst, dst_off, 16);
+    }
+
+    /**
+     * Get this matrix into the given float[16] array in column major order.
+     *
+     * @param dst float[16] array storage in column major order
+     */
+    public void get(final float[] dst) {
+        System.arraycopy(m, 0, dst, 0, 16);
+    }
+
+    /**
+     * Get this matrix into the given {@link FloatBuffer} in column major order.
+     * 
    
+     * Implementation uses relative {@link FloatBuffer#put(float)},
+     * hence caller may want to issue {@link FloatBuffer#reset()} thereafter.
+     * 
    
+     *
+     * @param dst {@link FloatBuffer} array storage in column major order
+     */
+    public void get(final FloatBuffer dst) {
+        dst.put(m, 0, 16);
+    }
+
+    //
+    // Basic matrix operations
+    //
+
+    /**
+     * Returns the determinant of this matrix
+     * @return the matrix determinant
+     */
+    public float determinant() {
+        float ret = 0;
+        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]));
+        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]));
+        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]));
+        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]));
+        return ret;
+    }
+
+    /**
+     * Transpose this matrix.
+     *
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb transpose() {
+        float tmp;
+
+        tmp = m[1+0*4];
+        m[1+0*4] = m[0+1*4];
+        m[0+1*4] = tmp;
+
+        tmp = m[2+0*4];
+        m[2+0*4] = m[0+2*4];
+        m[0+2*4] = tmp;
+
+        tmp = m[3+0*4];
+        m[3+0*4] = m[0+3*4];
+        m[0+3*4] = tmp;
+
+        tmp = m[2+1*4];
+        m[2+1*4] = m[1+2*4];
+        m[1+2*4] = tmp;
+
+        tmp = m[3+1*4];
+        m[3+1*4] = m[1+3*4];
+        m[1+3*4] = tmp;
+
+        tmp = m[3+2*4];
+        m[3+2*4] = m[2+3*4];
+        m[2+3*4] = tmp;
+
+        return this;
+    }
+
+    /**
+     * Transpose the given {@code src} matrix into this matrix.
+     *
+     * @param src source 4x4 matrix
+     * @return this matrix (result) for chaining
+     */
+    public final Matrix4fb transpose(final Matrix4fb src) {
+        if( src == this ) {
+            return transpose();
+        }
+        m[0+0*4] = src.m[0+0*4];
+        m[1+0*4] = src.m[0+1*4];
+        m[2+0*4] = src.m[0+2*4];
+        m[3+0*4] = src.m[0+3*4];
+
+        m[0+1*4] = src.m[1+0*4];
+        m[1+1*4] = src.m[1+1*4];
+        m[2+1*4] = src.m[1+2*4];
+        m[3+1*4] = src.m[1+3*4];
+
+        m[0+2*4] = src.m[2+0*4];
+        m[1+2*4] = src.m[2+1*4];
+        m[2+2*4] = src.m[2+2*4];
+        m[3+2*4] = src.m[2+3*4];
+
+        m[0+3*4] = src.m[3+0*4];
+        m[1+3*4] = src.m[3+1*4];
+        m[2+3*4] = src.m[3+2*4];
+        m[3+3*4] = src.m[3+3*4];
+        return this;
+    }
+
+    /**
+     * Invert this matrix.
+     * @return false if this matrix is singular and inversion not possible, otherwise true
+     */
+    public boolean invert() {
+        final float scale;
+      {
+          float max = Math.abs(m[0]);
+
+          for( int i = 1; i < 16; i++ ) {
+              final float a = Math.abs(m[i]);
+              if( a > max ) max = a;
+          }
+          if( 0 == max ) {
+              return false;
+          }
+          scale = 1.0f/max;
+      }
+
+        final float a00 = m[0+0*4]*scale;
+        final float a10 = m[1+0*4]*scale;
+        final float a20 = m[2+0*4]*scale;
+        final float a30 = m[3+0*4]*scale;
+
+        final float a01 = m[0+1*4]*scale;
+        final float a11 = m[1+1*4]*scale;
+        final float a21 = m[2+1*4]*scale;
+        final float a31 = m[3+1*4]*scale;
+
+        final float a02 = m[0+2*4]*scale;
+        final float a12 = m[1+2*4]*scale;
+        final float a22 = m[2+2*4]*scale;
+        final float a32 = m[3+2*4]*scale;
+
+        final float a03 = m[0+3*4]*scale;
+        final float a13 = m[1+3*4]*scale;
+        final float a23 = m[2+3*4]*scale;
+        final float a33 = m[3+3*4]*scale;
+
+        final float b00 = + a11*(a22*a33 - a23*a32) - a12*(a21*a33 - a23*a31) + a13*(a21*a32 - a22*a31);
+        final float b01 = -( + a10*(a22*a33 - a23*a32) - a12*(a20*a33 - a23*a30) + a13*(a20*a32 - a22*a30));
+        final float b02 = + a10*(a21*a33 - a23*a31) - a11*(a20*a33 - a23*a30) + a13*(a20*a31 - a21*a30);
+        final float b03 = -( + a10*(a21*a32 - a22*a31) - a11*(a20*a32 - a22*a30) + a12*(a20*a31 - a21*a30));
+
+        final float b10 = -( + a01*(a22*a33 - a23*a32) - a02*(a21*a33 - a23*a31) + a03*(a21*a32 - a22*a31));
+        final float b11 = + a00*(a22*a33 - a23*a32) - a02*(a20*a33 - a23*a30) + a03*(a20*a32 - a22*a30);
+        final float b12 = -( + a00*(a21*a33 - a23*a31) - a01*(a20*a33 - a23*a30) + a03*(a20*a31 - a21*a30));
+        final float b13 = + a00*(a21*a32 - a22*a31) - a01*(a20*a32 - a22*a30) + a02*(a20*a31 - a21*a30);
+
+        final float b20 = + a01*(a12*a33 - a13*a32) - a02*(a11*a33 - a13*a31) + a03*(a11*a32 - a12*a31);
+        final float b21 = -( + a00*(a12*a33 - a13*a32) - a02*(a10*a33 - a13*a30) + a03*(a10*a32 - a12*a30));
+        final float b22 = + a00*(a11*a33 - a13*a31) - a01*(a10*a33 - a13*a30) + a03*(a10*a31 - a11*a30);
+        final float b23 = -( + a00*(a11*a32 - a12*a31) - a01*(a10*a32 - a12*a30) + a02*(a10*a31 - a11*a30));
+
+        final float b30 = -( + a01*(a12*a23 - a13*a22) - a02*(a11*a23 - a13*a21) + a03*(a11*a22 - a12*a21));
+        final float b31 = + a00*(a12*a23 - a13*a22) - a02*(a10*a23 - a13*a20) + a03*(a10*a22 - a12*a20);
+        final float b32 = -( + a00*(a11*a23 - a13*a21) - a01*(a10*a23 - a13*a20) + a03*(a10*a21 - a11*a20));
+        final float b33 = + a00*(a11*a22 - a12*a21) - a01*(a10*a22 - a12*a20) + a02*(a10*a21 - a11*a20);
+
+        final float det = (a00*b00 + a01*b01 + a02*b02 + a03*b03) / scale;
+
+        if( 0 == det ) {
+            return false;
+        }
+
+        m[0+0*4] = b00 / det;
+        m[1+0*4] = b01 / det;
+        m[2+0*4] = b02 / det;
+        m[3+0*4] = b03 / det;
+
+        m[0+1*4] = b10 / det;
+        m[1+1*4] = b11 / det;
+        m[2+1*4] = b12 / det;
+        m[3+1*4] = b13 / det;
+
+        m[0+2*4] = b20 / det;
+        m[1+2*4] = b21 / det;
+        m[2+2*4] = b22 / det;
+        m[3+2*4] = b23 / det;
+
+        m[0+3*4] = b30 / det;
+        m[1+3*4] = b31 / det;
+        m[2+3*4] = b32 / det;
+        m[3+3*4] = b33 / det;
+        return true;
+    }
+
+    /**
+     * Invert the {@code src} matrix values into this matrix
+     * @param src the source matrix, which values are to be inverted
+     * @return false if {@code src} matrix is singular and inversion not possible, otherwise true
+     */
+    public boolean invert(final Matrix4fb src) {
+        return load(src).invert();
+    }
+
+    /**
+     * Multiply matrix: [this] = [this] x [b]
+     * 
    
+     * Roughly 15% slower than {@link #mul(Matrix4fb, Matrix4fb)}
+     * Roughly  3% slower than {@link FloatUtil#multMatrix(float[], float[])}
+     * 
    
+     * @param b 4x4 matrix
+     * @return this matrix for chaining
+     * @see #mul(Matrix4fb, Matrix4fb)
+     */
+    public final Matrix4fb mul(final Matrix4fb b) {
+        final float b00 = b.m[0+0*4];
+        final float b10 = b.m[1+0*4];
+        final float b20 = b.m[2+0*4];
+        final float b30 = b.m[3+0*4];
+        final float b01 = b.m[0+1*4];
+        final float b11 = b.m[1+1*4];
+        final float b21 = b.m[2+1*4];
+        final float b31 = b.m[3+1*4];
+        final float b02 = b.m[0+2*4];
+        final float b12 = b.m[1+2*4];
+        final float b22 = b.m[2+2*4];
+        final float b32 = b.m[3+2*4];
+        final float b03 = b.m[0+3*4];
+        final float b13 = b.m[1+3*4];
+        final float b23 = b.m[2+3*4];
+        final float b33 = b.m[3+3*4];
+
+        float ai0=m[0+0*4]; // row-0, m[0+0*4]
+        float ai1=m[0+1*4];
+        float ai2=m[0+2*4];
+        float ai3=m[0+3*4];
+        m[0+0*4] = ai0 * b00  +  ai1 * b10  +  ai2 * b20  +  ai3 * b30 ;
+        m[0+1*4] = ai0 * b01  +  ai1 * b11  +  ai2 * b21  +  ai3 * b31 ;
+        m[0+2*4] = ai0 * b02  +  ai1 * b12  +  ai2 * b22  +  ai3 * b32 ;
+        m[0+3*4] = ai0 * b03  +  ai1 * b13  +  ai2 * b23  +  ai3 * b33 ;
+
+        ai0=m[1+0*4]; //row-1, m[1+0*4]
+        ai1=m[1+1*4];
+        ai2=m[1+2*4];
+        ai3=m[1+3*4];
+        m[1+0*4] = ai0 * b00  +  ai1 * b10  +  ai2 * b20  +  ai3 * b30 ;
+        m[1+1*4] = ai0 * b01  +  ai1 * b11  +  ai2 * b21  +  ai3 * b31 ;
+        m[1+2*4] = ai0 * b02  +  ai1 * b12  +  ai2 * b22  +  ai3 * b32 ;
+        m[1+3*4] = ai0 * b03  +  ai1 * b13  +  ai2 * b23  +  ai3 * b33 ;
+
+        ai0=m[2+0*4]; // row-2, m[2+0*4]
+        ai1=m[2+1*4];
+        ai2=m[2+2*4];
+        ai3=m[2+3*4];
+        m[2+0*4] = ai0 * b00  +  ai1 * b10  +  ai2 * b20  +  ai3 * b30 ;
+        m[2+1*4] = ai0 * b01  +  ai1 * b11  +  ai2 * b21  +  ai3 * b31 ;
+        m[2+2*4] = ai0 * b02  +  ai1 * b12  +  ai2 * b22  +  ai3 * b32 ;
+        m[2+3*4] = ai0 * b03  +  ai1 * b13  +  ai2 * b23  +  ai3 * b33 ;
+
+        ai0=m[3+0*4]; // row-3, m[3+0*4]
+        ai1=m[3+1*4];
+        ai2=m[3+2*4];
+        ai3=m[3+3*4];
+        m[3+0*4] = ai0 * b00  +  ai1 * b10  +  ai2 * b20  +  ai3 * b30 ;
+        m[3+1*4] = ai0 * b01  +  ai1 * b11  +  ai2 * b21  +  ai3 * b31 ;
+        m[3+2*4] = ai0 * b02  +  ai1 * b12  +  ai2 * b22  +  ai3 * b32 ;
+        m[3+3*4] = ai0 * b03  +  ai1 * b13  +  ai2 * b23  +  ai3 * b33 ;
+        return this;
+    }
+
+    /**
+     * Multiply matrix: [this] = [a] x [b]
+     * 
    
+     * Roughly 13% faster than {@link #mul(Matrix4fb)}
+     * Roughly 11% faster than {@link FloatUtil#multMatrix(float[], float[])}
+     * 
    
+     * @param a 4x4 matrix
+     * @param b 4x4 matrix
+     * @return this matrix for chaining
+     * @see #mul(Matrix4fb)
+     */
+    public final Matrix4fb mul(final Matrix4fb a, final Matrix4fb b) {
+        final float b00 = b.m[0+0*4];
+        final float b10 = b.m[1+0*4];
+        final float b20 = b.m[2+0*4];
+        final float b30 = b.m[3+0*4];
+        final float b01 = b.m[0+1*4];
+        final float b11 = b.m[1+1*4];
+        final float b21 = b.m[2+1*4];
+        final float b31 = b.m[3+1*4];
+        final float b02 = b.m[0+2*4];
+        final float b12 = b.m[1+2*4];
+        final float b22 = b.m[2+2*4];
+        final float b32 = b.m[3+2*4];
+        final float b03 = b.m[0+3*4];
+        final float b13 = b.m[1+3*4];
+        final float b23 = b.m[2+3*4];
+        final float b33 = b.m[3+3*4];
+
+        float ai0=a.m[0+0*4]; // row-0, m[0+0*4]
+        float ai1=a.m[0+1*4];
+        float ai2=a.m[0+2*4];
+        float ai3=a.m[0+3*4];
+        m[0+0*4] = ai0 * b00  +  ai1 * b10  +  ai2 * b20  +  ai3 * b30 ;
+        m[0+1*4] = ai0 * b01  +  ai1 * b11  +  ai2 * b21  +  ai3 * b31 ;
+        m[0+2*4] = ai0 * b02  +  ai1 * b12  +  ai2 * b22  +  ai3 * b32 ;
+        m[0+3*4] = ai0 * b03  +  ai1 * b13  +  ai2 * b23  +  ai3 * b33 ;
+
+        ai0=a.m[1+0*4]; //row-1, m[1+0*4]
+        ai1=a.m[1+1*4];
+        ai2=a.m[1+2*4];
+        ai3=a.m[1+3*4];
+        m[1+0*4] = ai0 * b00  +  ai1 * b10  +  ai2 * b20  +  ai3 * b30 ;
+        m[1+1*4] = ai0 * b01  +  ai1 * b11  +  ai2 * b21  +  ai3 * b31 ;
+        m[1+2*4] = ai0 * b02  +  ai1 * b12  +  ai2 * b22  +  ai3 * b32 ;
+        m[1+3*4] = ai0 * b03  +  ai1 * b13  +  ai2 * b23  +  ai3 * b33 ;
+
+        ai0=a.m[2+0*4]; // row-2, m[2+0*4]
+        ai1=a.m[2+1*4];
+        ai2=a.m[2+2*4];
+        ai3=a.m[2+3*4];
+        m[2+0*4] = ai0 * b00  +  ai1 * b10  +  ai2 * b20  +  ai3 * b30 ;
+        m[2+1*4] = ai0 * b01  +  ai1 * b11  +  ai2 * b21  +  ai3 * b31 ;
+        m[2+2*4] = ai0 * b02  +  ai1 * b12  +  ai2 * b22  +  ai3 * b32 ;
+        m[2+3*4] = ai0 * b03  +  ai1 * b13  +  ai2 * b23  +  ai3 * b33 ;
+
+        ai0=a.m[3+0*4]; // row-3, m[3+0*4]
+        ai1=a.m[3+1*4];
+        ai2=a.m[3+2*4];
+        ai3=a.m[3+3*4];
+        m[3+0*4] = ai0 * b00  +  ai1 * b10  +  ai2 * b20  +  ai3 * b30 ;
+        m[3+1*4] = ai0 * b01  +  ai1 * b11  +  ai2 * b21  +  ai3 * b31 ;
+        m[3+2*4] = ai0 * b02  +  ai1 * b12  +  ai2 * b22  +  ai3 * b32 ;
+        m[3+3*4] = ai0 * b03  +  ai1 * b13  +  ai2 * b23  +  ai3 * b33 ;
+        return this;
+    }
+
+    /**
+     * @param v_in 4-component column-vector
+     * @param v_out this * v_in
+     * @returns v_out for chaining
+     */
+    public final float[] mulVec4f(final float[/*4*/] v_in, final float[/*4*/] v_out) {
+        // (one matrix row in column-major order) X (column vector)
+        final float x = v_in[0], y = v_in[1], z = v_in[2], w = v_in[3];
+        v_out[0] = x * m[0+0*4] + y * m[0+1*4] + z * m[0+2*4] + w * m[0+3*4];
+        v_out[1] = x * m[1+0*4] + y * m[1+1*4] + z * m[1+2*4] + w * m[1+3*4];
+        v_out[2] = x * m[2+0*4] + y * m[2+1*4] + z * m[2+2*4] + w * m[2+3*4];
+        v_out[3] = x * m[3+0*4] + y * m[3+1*4] + z * m[3+2*4] + w * m[3+3*4];
+        return v_out;
+    }
+
+    /**
+     * @param v_in 4-component column-vector
+     * @param v_out this * v_in
+     * @returns v_out for chaining
+     */
+    public final Vec4f mulVec4f(final Vec4f v_in, final Vec4f v_out) {
+        // (one matrix row in column-major order) X (column vector)
+        final float x = v_in.x(), y = v_in.y(), z = v_in.z(), w = v_in.w();
+        v_out.set( x * m[0+0*4] + y * m[0+1*4] + z * m[0+2*4] + w * m[0+3*4],
+                   x * m[1+0*4] + y * m[1+1*4] + z * m[1+2*4] + w * m[1+3*4],
+                   x * m[2+0*4] + y * m[2+1*4] + z * m[2+2*4] + w * m[2+3*4],
+                   x * m[3+0*4] + y * m[3+1*4] + z * m[3+2*4] + w * m[3+3*4] );
+        return v_out;
+    }
+
+    /**
+     * Affine 3f-vector transformation by 4x4 matrix
+     *
+     * 4x4 matrix multiplication with 3-component vector,
+     * using {@code 1} for for {@code v_in[3]} and dropping {@code v_out[3]},
+     * which shall be {@code 1}.
+     *
+     * @param v_in 3-component column-vector
+     * @param v_out m_in * v_in, 3-component column-vector
+     * @returns v_out for chaining
+     */
+    public final float[] mulVec3f(final float[/*3*/] v_in, final float[/*3*/] v_out) {
+        // (one matrix row in column-major order) X (column vector)
+        final float x = v_in[0], y = v_in[1], z = v_in[2];
+        v_out[0] = x * m[0+0*4] + y * m[0+1*4] + z * m[0+2*4] + 1f * m[0+3*4];
+        v_out[1] = x * m[1+0*4] + y * m[1+1*4] + z * m[1+2*4] + 1f * m[1+3*4];
+        v_out[2] = x * m[2+0*4] + y * m[2+1*4] + z * m[2+2*4] + 1f * m[2+3*4];
+        return v_out;
+    }
+
+    /**
+     * Affine 3f-vector transformation by 4x4 matrix
+     *
+     * 4x4 matrix multiplication with 3-component vector,
+     * using {@code 1} for for {@code v_in.w()} and dropping {@code v_out.w()},
+     * which shall be {@code 1}.
+     *
+     * @param v_in 3-component column-vector {@link Vec3f}
+     * @param v_out m_in * v_in, 3-component column-vector {@link Vec3f}
+     * @returns v_out for chaining
+     */
+    public final Vec3f mulVec3f(final Vec3f v_in, final Vec3f v_out) {
+        // (one matrix row in column-major order) X (column vector)
+        final float x = v_in.x(), y = v_in.y(), z = v_in.z();
+        v_out.set( x * m[0+0*4] + y * m[0+1*4] + z * m[0+2*4] + 1f * m[0+3*4],
+                   x * m[1+0*4] + y * m[1+1*4] + z * m[1+2*4] + 1f * m[1+3*4],
+                   x * m[2+0*4] + y * m[2+1*4] + z * m[2+2*4] + 1f * m[2+3*4] );
+        return v_out;
+    }
+
+    //
+    // Matrix setTo...(), affine + basic
+    //
+
+    /**
+     * Set this matrix to translation.
+     * 
    +      Translation matrix (Column Order):
+      1 0 0 0
+      0 1 0 0
+      0 0 1 0
+      x y z 1
+     * 
    
+     * @param x x-axis translate
+     * @param y y-axis translate
+     * @param z z-axis translate
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb setToTranslation(final float x, final float y, final float z) {
+        m[0+0*4] = m[1+1*4] = m[2+2*4] = m[3+3*4] = 1.0f;
+        m[0+3*4] = x;
+        m[1+3*4] = y;
+        m[2+3*4] = z;
+        m[0+1*4] = m[0+2*4] =
+        m[1+0*4] = m[1+2*4] =
+        m[2+0*4] = m[2+1*4] =
+        m[3+0*4] = m[3+1*4] = m[3+2*4] = 0.0f;
+        return this;
+    }
+
+    /**
+     * Set this matrix to translation.
+     * 
    +      Translation matrix (Column Order):
+      1 0 0 0
+      0 1 0 0
+      0 0 1 0
+      x y z 1
+     * 
    
+     * @param t translate Vec3f
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb setToTranslation(final Vec3f t) {
+        return setToTranslation(t.x(), t.y(), t.z());
+    }
+
+    /**
+     * Set this matrix to scale.
+     * 
    +      Scale matrix (Any Order):
+      x 0 0 0
+      0 y 0 0
+      0 0 z 0
+      0 0 0 1
+     * 
    
+     * @param x x-axis scale
+     * @param y y-axis scale
+     * @param z z-axis scale
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb setToScale(final float x, final float y, final float z) {
+        m[3+3*4] = 1.0f;
+        m[0+0*4] = x;
+        m[1+1*4] = y;
+        m[2+2*4] = z;
+        m[0+1*4] = m[0+2*4] = m[0+3*4] =
+        m[1+0*4] = m[1+2*4] = m[1+3*4] =
+        m[2+0*4] = m[2+1*4] = m[2+3*4] =
+        m[3+0*4] = m[3+1*4] = m[3+2*4] = 0.0f;
+        return this;
+    }
+
+    /**
+     * Set this matrix to rotation from the given axis and angle in radians.
+     * 
    +        Rotation matrix (Column Order):
+        xx(1-c)+c  xy(1-c)+zs xz(1-c)-ys 0
+        xy(1-c)-zs yy(1-c)+c  yz(1-c)+xs 0
+        xz(1-c)+ys yz(1-c)-xs zz(1-c)+c  0
+        0          0          0          1
+     * 
    
+     * @see Matrix-FAQ Q38
+     * @param ang_rad angle in radians
+     * @param x x of rotation axis
+     * @param y y of rotation axis
+     * @param z z of rotation axis
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb setToRotationAxis(final float ang_rad, float x, float y, float z) {
+        final float c = FloatUtil.cos(ang_rad);
+        final float ic= 1.0f - c;
+        final float s = FloatUtil.sin(ang_rad);
+
+        final float[] tmpVec3f = { x, y, z };
+        VectorUtil.normalizeVec3(tmpVec3f);
+        x = tmpVec3f[0]; y = tmpVec3f[1]; z = tmpVec3f[2];
+
+        final float xy = x*y;
+        final float xz = x*z;
+        final float xs = x*s;
+        final float ys = y*s;
+        final float yz = y*z;
+        final float zs = z*s;
+        m[0+0*4] = x*x*ic+c;
+        m[1+0*4] = xy*ic+zs;
+        m[2+0*4] = xz*ic-ys;
+        m[3+0*4] = 0;
+
+        m[0+1*4] = xy*ic-zs;
+        m[1+1*4] = y*y*ic+c;
+        m[2+1*4] = yz*ic+xs;
+        m[3+1*4] = 0;
+
+        m[0+2*4] = xz*ic+ys;
+        m[1+2*4] = yz*ic-xs;
+        m[2+2*4] = z*z*ic+c;
+        m[3+2*4] = 0;
+
+        m[0+3*4] = 0f;
+        m[1+3*4] = 0f;
+        m[2+3*4] = 0f;
+        m[3+3*4] = 1f;
+
+        return this;
+    }
+
+    /**
+     * Set this matrix to rotation from the given axis and angle in radians.
+     * 
    +        Rotation matrix (Column Order):
+        xx(1-c)+c  xy(1-c)+zs xz(1-c)-ys 0
+        xy(1-c)-zs yy(1-c)+c  yz(1-c)+xs 0
+        xz(1-c)+ys yz(1-c)-xs zz(1-c)+c  0
+        0          0          0          1
+     * 
    
+     * @see Matrix-FAQ Q38
+     * @param ang_rad angle in radians
+     * @param axis rotation axis
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb setToRotationAxis(final float ang_rad, final Vec3f axis) {
+        return setToRotationAxis(ang_rad, axis.x(), axis.y(), axis.z());
+    }
+
+    /**
+     * Set this matrix to rotation from the given Euler rotation angles in radians.
+     * 
    
+     * The rotations are applied in the given order:
+     * 
      
+     *  
    • y - heading
      • 
+     *  
    • z - attitude
      • 
+     *  
    • x - bank
      • 
+     * 
    
+     * 
    
+     * @param bankX the Euler pitch angle in radians. (rotation about the X axis)
+     * @param headingY the Euler yaw angle in radians. (rotation about the Y axis)
+     * @param attitudeZ the Euler roll angle in radians. (rotation about the Z axis)
+     * @return this matrix for chaining
+     * 
    
+     * Implementation does not use Quaternion and hence is exposed to
+     * Gimbal-Lock,
+     * consider using {@link #setToRotation(Quaternion)}.
+     * 
    
+     * @see Matrix-FAQ Q36
+     * @see euclideanspace.com-eulerToMatrix
+     * @see #setToRotation(Quaternion)
+     */
+    public Matrix4fb setToRotationEuler(final float bankX, final float headingY, final float attitudeZ) {
+        // Assuming the angles are in radians.
+        final float ch = FloatUtil.cos(headingY);
+        final float sh = FloatUtil.sin(headingY);
+        final float ca = FloatUtil.cos(attitudeZ);
+        final float sa = FloatUtil.sin(attitudeZ);
+        final float cb = FloatUtil.cos(bankX);
+        final float sb = FloatUtil.sin(bankX);
+
+        m[0+0*4] =  ch*ca;
+        m[1+0*4] =  sa;
+        m[2+0*4] = -sh*ca;
+        m[3+0*4] =  0;
+
+        m[0+1*4] =  sh*sb    - ch*sa*cb;
+        m[1+1*4] =  ca*cb;
+        m[2+1*4] =  sh*sa*cb + ch*sb;
+        m[3+1*4] =  0;
+
+        m[0+2*4] =  ch*sa*sb + sh*cb;
+        m[1+2*4] = -ca*sb;
+        m[2+2*4] = -sh*sa*sb + ch*cb;
+        m[3+2*4] =  0;
+
+        m[0+3*4] =  0;
+        m[1+3*4] =  0;
+        m[2+3*4] =  0;
+        m[3+3*4] =  1;
+
+        return this;
+    }
+
+    /**
+     * Set this matrix to rotation using the given Quaternion.
+     * 
    
+     * Implementation Details:
+     * 
      
+     *   
    •  makes identity matrix if {@link #magnitudeSquared()} is {@link FloatUtil#isZero(float, float) is zero} using {@link FloatUtil#EPSILON epsilon}
      • 
+     *   
    •  The fields [m[0+0*4] .. m[2+2*4]] define the rotation
      • 
+     * 
    
+     * 
    
+     *
+     * @param q the Quaternion representing the rotation
+     * @return this matrix for chaining
+     * @see Matrix-FAQ Q54
+     * @see Quaternion#toMatrix(float[], int)
+     * @see #getRotation()
+     */
+    public final Matrix4fb setToRotation(final Quaternion q) {
+        // pre-multiply scaled-reciprocal-magnitude to reduce multiplications
+        final float norm = q.magnitudeSquared();
+        if ( FloatUtil.isZero(norm, FloatUtil.EPSILON) ) {
+            // identity matrix -> srecip = 0f
+            loadIdentity();
+            return this;
+        }
+        final float srecip;
+        if ( FloatUtil.isEqual(1f, norm, FloatUtil.EPSILON) ) {
+            srecip = 2f;
+        } else {
+            srecip = 2.0f / norm;
+        }
+
+        final float x = q.x();
+        final float y = q.y();
+        final float z = q.z();
+        final float w = q.w();
+
+        final float xs = srecip * x;
+        final float ys = srecip * y;
+        final float zs = srecip * z;
+
+        final float xx = x  * xs;
+        final float xy = x  * ys;
+        final float xz = x  * zs;
+        final float xw = xs * w;
+        final float yy = y  * ys;
+        final float yz = y  * zs;
+        final float yw = ys * w;
+        final float zz = z  * zs;
+        final float zw = zs * w;
+
+        m[0+0*4] = 1f - ( yy + zz );
+        m[0+1*4] =      ( xy - zw );
+        m[0+2*4] =      ( xz + yw );
+        m[0+3*4] = 0f;
+
+        m[1+0*4] =      ( xy + zw );
+        m[1+1*4] = 1f - ( xx + zz );
+        m[1+2*4] =      ( yz - xw );
+        m[1+3*4] = 0f;
+
+        m[2+0*4] =      ( xz - yw );
+        m[2+1*4] =      ( yz + xw );
+        m[2+2*4] = 1f - ( xx + yy );
+        m[2+3*4] = 0f;
+
+        m[3+0*4] = m[3+1*4] = m[3+2*4] = 0f;
+        m[3+3*4] = 1f;
+        return this;
+    }
+
+    /**
+     * Returns the rotation [m[0+0*4] .. m[2+2*4]] fields converted to a Quaternion.
+     * @param res resulting Quaternion
+     * @return the resulting Quaternion for chaining.
+     * @see Quaternion#setFromMatrix(float, float, float, float, float, float, float, float, float)
+     * @see #setToRotation(Quaternion)
+     */
+    public final Quaternion getRotation(final Quaternion res) {
+        res.setFromMatrix(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]);
+        return res;
+    }
+
+    /**
+     * Set this matrix to orthogonal projection.
+     * 
    +      Ortho matrix (Column Order):
+      2/dx  0     0    0
+      0     2/dy  0    0
+      0     0     2/dz 0
+      tx    ty    tz   1
+     * 
    
+     * @param left
+     * @param right
+     * @param bottom
+     * @param top
+     * @param zNear
+     * @param zFar
+     * @return this matrix for chaining
+     */
+    public Matrix4fb setToOrtho(final float left, final float right,
+                               final float bottom, final float top,
+                               final float zNear, final float zFar) {
+        {
+            // m[0+0*4] = m[1+1*4] = m[2+2*4] = m[3+3*4] = 1f;
+            m[1+0*4] = m[2+0*4] = m[3+0*4] = 0f;
+            m[0+1*4] = m[2+1*4] = m[3+1*4] = 0f;
+            m[0+2*4] = m[1+2*4] = m[3+2*4] = 0f;
+            // m[0+3*4] = m[1+3*4] = m[2+3*4] = 0f;
+        }
+        final float dx=right-left;
+        final float dy=top-bottom;
+        final float dz=zFar-zNear;
+        final float tx=-1.0f*(right+left)/dx;
+        final float ty=-1.0f*(top+bottom)/dy;
+        final float tz=-1.0f*(zFar+zNear)/dz;
+
+        m[0+0*4] =  2.0f/dx;
+        m[1+1*4] =  2.0f/dy;
+        m[2+2*4] = -2.0f/dz;
+
+        m[0+3*4] = tx;
+        m[1+3*4] = ty;
+        m[2+3*4] = tz;
+        m[3+3*4] = 1f;
+
+        return this;
+    }
+
+    /**
+     * Set this matrix to frustum.
+     * 
    +      Frustum matrix (Column Order):
+      2*zNear/dx   0          0   0
+      0            2*zNear/dy 0   0
+      A            B          C  -1
+      0            0          D   0
+     * 
    
+     * @param left
+     * @param right
+     * @param bottom
+     * @param top
+     * @param zNear
+     * @param zFar
+     * @return this matrix for chaining
+     * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear}
+     *                                  or {@code left == right}, or {@code bottom == top}.
+     */
+    public Matrix4fb setToFrustum(final float left, final float right,
+                                 final float bottom, final float top,
+                                 final float zNear, final float zFar) throws IllegalArgumentException {
+        if( zNear <= 0.0f || zFar <= zNear ) {
+            throw new IllegalArgumentException("Requirements zNear > 0 and zFar > zNear, but zNear "+zNear+", zFar "+zFar);
+        }
+        if( left == right || top == bottom) {
+            throw new IllegalArgumentException("GL_INVALID_VALUE: top,bottom and left,right must not be equal");
+        }
+        {
+            // m[0+0*4] = m[1+1*4] = m[2+2*4] = m[3+3*4] = 1f;
+            m[1+0*4] = m[2+0*4] = m[3+0*4] = 0f;
+            m[0+1*4] = m[2+1*4] = m[3+1*4] = 0f;
+            m[0+3*4] = m[1+3*4] = 0f;
+        }
+        final float zNear2 = 2.0f*zNear;
+        final float dx=right-left;
+        final float dy=top-bottom;
+        final float dz=zFar-zNear;
+        final float A=(right+left)/dx;
+        final float B=(top+bottom)/dy;
+        final float C=-1.0f*(zFar+zNear)/dz;
+        final float D=-2.0f*(zFar*zNear)/dz;
+
+        m[0+0*4] = zNear2/dx;
+        m[1+1*4] = zNear2/dy;
+
+        m[0+2*4] = A;
+        m[1+2*4] = B;
+        m[2+2*4] = C;
+        m[3+2*4] = -1.0f;
+
+        m[2+3*4] = D;
+        m[3+3*4] = 0f;
+
+        return this;
+    }
+
+    /**
+     * Set this matrix to perspective {@link #setToFrustum(float, float, float, float, float, float) frustum} projection.
+     *
+     * @param fovy_rad angle in radians
+     * @param aspect aspect ratio width / height
+     * @param zNear
+     * @param zFar
+     * @return this matrix for chaining
+     * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear}
+     * @see #setToFrustum(float, float, float, float, float, float)
+     */
+    public Matrix4fb setToPerspective(final float fovy_rad, final float aspect, final float zNear, final float zFar) throws IllegalArgumentException {
+        final float top    =  FloatUtil.tan(fovy_rad/2f) * zNear; // use tangent of half-fov !
+        final float bottom =  -1.0f * top;    //          -1f * fovhvTan.top * zNear
+        final float left   = aspect * bottom; // aspect * -1f * fovhvTan.top * zNear
+        final float right  = aspect * top;    // aspect * fovhvTan.top * zNear
+        return setToFrustum(left, right, bottom, top, zNear, zFar);
+    }
+
+    /**
+     * Set this matrix to perspective {@link #setToFrustum(float, float, float, float, float, float) frustum} projection.
+     *
+     * @param fovhv {@link FovHVHalves} field of view in both directions, may not be centered, either in radians or tangent
+     * @param zNear
+     * @param zFar
+     * @return this matrix for chaining
+     * @throws IllegalArgumentException if {@code zNear <= 0} or {@code zFar <= zNear}
+     * @see #setToFrustum(float, float, float, float, float, float)
+     * @see Frustum#updateByFovDesc(float[], int, boolean, Frustum.FovDesc)
+     */
+    public Matrix4fb setToPerspective(final FovHVHalves fovhv, final float zNear, final float zFar) throws IllegalArgumentException {
+        final FovHVHalves fovhvTan = fovhv.toTangents();  // use tangent of half-fov !
+        final float top    =         fovhvTan.top    * zNear;
+        final float bottom = -1.0f * fovhvTan.bottom * zNear;
+        final float left   = -1.0f * fovhvTan.left   * zNear;
+        final float right  =         fovhvTan.right  * zNear;
+        return setToFrustum(left, right, bottom, top, zNear, zFar);
+    }
+
+    /**
+     * Calculate the frustum planes in world coordinates
+     * using the passed float[16] as premultiplied P*MV (column major order).
+     * 
    
+     * Frustum plane's normals will point to the inside of the viewing frustum,
+     * as required by this class.
+     * 
    
+     */
+    public void updateFrustumPlanes(final Frustum frustum) {
+        // 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
+        // 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
+        {
+            final Frustum.Plane p = frustum.getPlanes()[Frustum.LEFT];
+            final Vec3f p_n = p.n;
+            p_n.set( m[3+0*4] + m[0+0*4],
+                     m[3+1*4] + m[0+1*4],
+                     m[3+2*4] + m[0+2*4] );
+            p.d    = m[3+3*4] + m[0+3*4];
+        }
+
+        // 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
+        // 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
+        {
+            final Frustum.Plane p = frustum.getPlanes()[Frustum.RIGHT];
+            final Vec3f p_n = p.n;
+            p_n.set( m[3+0*4] - m[0+0*4],
+                     m[3+1*4] - m[0+1*4],
+                     m[3+2*4] - m[0+2*4] );
+            p.d    = m[3+3*4] - m[0+3*4];
+        }
+
+        // Bottom: a = m41m21, b = m42m22, c = m43m[2+3*4], d = m44m24  - [1..4] column-major
+        // Bottom: a = m30m10, b = m31m11, c = m32m12, d = m[3+3*4]m[1+3*4]  - [0..3] column-major
+        {
+            final Frustum.Plane p = frustum.getPlanes()[Frustum.BOTTOM];
+            final Vec3f p_n = p.n;
+            p_n.set( m[3+0*4] + m[1+0*4],
+                     m[3+1*4] + m[1+1*4],
+                     m[3+2*4] + m[1+2*4] );
+            p.d    = m[3+3*4] + m[1+3*4];
+        }
+
+        // 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
+        // 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
+        {
+            final Frustum.Plane p = frustum.getPlanes()[Frustum.TOP];
+            final Vec3f p_n = p.n;
+            p_n.set( m[3+0*4] - m[1+0*4],
+                     m[3+1*4] - m[1+1*4],
+                     m[3+2*4] - m[1+2*4] );
+            p.d    = m[3+3*4] - m[1+3*4];
+        }
+
+        // Near:  a = m41m31, b = m42m32, c = m43m[3+3*4], d = m44m34  - [1..4] column-major
+        // Near:  a = m30m20, b = m31m21, c = m32m22, d = m[3+3*4]m[2+3*4]  - [0..3] column-major
+        {
+            final Frustum.Plane p = frustum.getPlanes()[Frustum.NEAR];
+            final Vec3f p_n = p.n;
+            p_n.set( m[3+0*4] + m[2+0*4],
+                     m[3+1*4] + m[2+1*4],
+                     m[3+2*4] + m[2+2*4] );
+            p.d    = m[3+3*4] + m[2+3*4];
+        }
+
+        // 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
+        // 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
+        {
+            final Frustum.Plane p = frustum.getPlanes()[Frustum.FAR];
+            final Vec3f p_n = p.n;
+            p_n.set( m[3+0*4] - m[2+0*4],
+                     m[3+1*4] - m[2+1*4],
+                     m[3+2*4] - m[2+2*4] );
+            p.d    = m[3+3*4] - m[2+3*4];
+        }
+
+        // Normalize all planes
+        for (int i = 0; i < 6; ++i) {
+            final Plane p = frustum.getPlanes()[i];
+            final Vec3f p_n = p.n;
+            final float invLen = 1f / p_n.length();
+            p_n.scale(invLen);
+            p.d *= invLen;
+        }
+    }
+
+    /**
+     * Make given matrix the look-at matrix based on given parameters.
+     * 
    
+     * Consist out of two matrix multiplications:
+     * 
    +     *   R = L x T,
+     *   with L for look-at matrix and
+     *        T for eye translation.
+     *
+     *   Result R can be utilized for projection or modelview multiplication, i.e.
+     *          M = M x R,
+     *          with M being the projection or modelview matrix.
+     * 
    
+     * 
    
+     * @param eye 3 component eye vector
+     * @param center 3 component center vector
+     * @param up 3 component up vector
+     * @param tmp temporary Matrix4f used for multiplication
+     * @return this matrix for chaining
+     */
+    public Matrix4fb setToLookAt(final Vec3f eye, final Vec3f center, final Vec3f up, final Matrix4fb tmp) {
+        // normalized forward!
+        final Vec3f fwd = new Vec3f( center.x() - eye.x(),
+                                     center.y() - eye.y(),
+                                     center.z() - eye.z() ).normalize();
+
+        /* Side = forward x up, normalized */
+        final Vec3f side = fwd.cross(up).normalize();
+
+        /* Recompute up as: up = side x forward */
+        final Vec3f up2 = side.cross(fwd);
+
+        m[0+0*4] = side.x();
+        m[1+0*4] = up2.x();
+        m[2+0*4] = -fwd.x();
+        m[3+0*4] = 0;
+
+        m[0+1*4] = side.y();
+        m[1+1*4] = up2.y();
+        m[2+1*4] = -fwd.y();
+        m[3+1*4] = 0;
+
+        m[0+2*4] = side.z();
+        m[1+2*4] = up2.z();
+        m[2+2*4] = -fwd.z();
+        m[3+2*4] = 0;
+
+        m[0+3*4] = 0;
+        m[1+3*4] = 0;
+        m[2+3*4] = 0;
+        m[3+3*4] = 1;
+
+        return mul( tmp.setToTranslation( -eye.x(), -eye.y(), -eye.z() ) );
+    }
+
+    //
+    // Matrix affine operations using setTo..()
+    //
+
+    /**
+     * Rotate this matrix about give axis and angle in radians, i.e. multiply by {@link #setToRotationAxis(float, float, float, float) axis-rotation matrix}.
+     * @see Matrix-FAQ Q38
+     * @param angrad angle in radians
+     * @param x x of rotation axis
+     * @param y y of rotation axis
+     * @param z z of rotation axis
+     * @param tmp temporary Matrix4f used for multiplication
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb rotate(final float ang_rad, final float x, final float y, final float z, final Matrix4fb tmp) {
+        return mul( tmp.setToRotationAxis(ang_rad, x, y, z) );
+    }
+
+    /**
+     * Rotate this matrix about give axis and angle in radians, i.e. multiply by {@link #setToRotationAxis(float, Vec3f) axis-rotation matrix}.
+     * @see Matrix-FAQ Q38
+     * @param angrad angle in radians
+     * @param axis rotation axis
+     * @param tmp temporary Matrix4f used for multiplication
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb rotate(final float ang_rad, final Vec3f axis, final Matrix4fb tmp) {
+        return mul( tmp.setToRotationAxis(ang_rad, axis) );
+    }
+
+    /**
+     * Rotate this matrix with the given {@link Quaternion}, i.e. multiply by {@link #setToRotation(Quaternion) Quaternion's rotation matrix}.
+     * @param tmp temporary Matrix4f used for multiplication
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb rotate(final Quaternion quat, final Matrix4fb tmp) {
+        return mul( tmp.setToRotation(quat) );
+    }
+
+    /**
+     * Translate this matrix, i.e. multiply by {@link #setToTranslation(float, float, float) translation matrix}.
+     * @param x x translation
+     * @param y y translation
+     * @param z z translation
+     * @param tmp temporary Matrix4f used for multiplication
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb translate(final float x, final float y, final float z, final Matrix4fb tmp) {
+        return mul( tmp.setToTranslation(x, y, z) );
+    }
+
+    /**
+     * Translate this matrix, i.e. multiply by {@link #setToTranslation(Vec3f) translation matrix}.
+     * @param t translation Vec3f
+     * @param tmp temporary Matrix4f used for multiplication
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb translate(final Vec3f t, final Matrix4fb tmp) {
+        return mul( tmp.setToTranslation(t) );
+    }
+
+    /**
+     * Scale this matrix, i.e. multiply by {@link #setToScale(float, float, float) scale matrix}.
+     * @param x x scale
+     * @param y y scale
+     * @param z z scale
+     * @param tmp temporary Matrix4f used for multiplication
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb scale(final float x, final float y, final float z, final Matrix4fb tmp) {
+        return mul( tmp.setToScale(x, y, z) );
+    }
+
+    /**
+     * Scale this matrix, i.e. multiply by {@link #setToScale(float, float, float) scale matrix}.
+     * @param s scale for x-, y- and z-axis
+     * @param tmp temporary Matrix4f used for multiplication
+     * @return this matrix for chaining
+     */
+    public final Matrix4fb scale(final float s, final Matrix4fb tmp) {
+        return mul( tmp.setToScale(s, s, s) );
+    }
+
+    //
+    // Matrix Stack
+    //
+
+    /**
+     * Push the matrix to it's stack, while preserving this matrix values.
+     * @see #pop()
+     */
+    public final void push() {
+        stack.push(this);
+    }
+
+    /**
+     * Pop the current matrix from it's stack, replacing this matrix values.
+     * @see #push()
+     */
+    public final void pop() {
+        stack.pop(this);
+    }
+
+    //
+    // equals
+    //
+
+    /**
+     * Equals check using a given {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}.
+     * 
    
+     * Implementation considers following corner cases:
+     * 
      
+     *    
    • NaN == NaN
      • 
+     *    
    • +Inf == +Inf
      • 
+     *    
    • -Inf == -Inf
      • 
+     * 
    
+     * @param o comparison value
+     * @param epsilon consider using {@link FloatUtil#EPSILON}
+     * @return true if all components differ less than {@code epsilon}, otherwise false.
+     */
+    public boolean isEqual(final Matrix4fb o, final float epsilon) {
+        if( this == o ) {
+            return true;
+        } else {
+            for(int i=0; i<16; ++i) {
+                if( !FloatUtil.isEqual(m[i], o.m[i], epsilon) ) {
+                    return false;
+                }
+            }
+            return true;
+        }
+    }
+
+    /**
+     * Equals check using {@link FloatUtil#EPSILON} value and {@link FloatUtil#isEqual(float, float, float)}.
+     * 
    
+     * Implementation considers following corner cases:
+     * 
      
+     *    
    • NaN == NaN
      • 
+     *    
    • +Inf == +Inf
      • 
+     *    
    • -Inf == -Inf
      • 
+     * 
    
+     * @param o comparison value
+     * @return true if all components differ less than {@link FloatUtil#EPSILON}, otherwise false.
+     */
+    public boolean isEqual(final Matrix4fb o) {
+        return isEqual(o, FloatUtil.EPSILON);
+    }
+
+    @Override
+    public boolean equals(final Object o) {
+        if( o instanceof Matrix4fb ) {
+            return isEqual((Matrix4fb)o, FloatUtil.EPSILON);
+        } else {
+            return false;
+        }
+    }
+
+    //
+    // Static multi Matrix ops
+    //
+
+    /**
+     * Map object coordinates to window coordinates.
+     * 
    
+     * Traditional gluProject implementation.
+     * 
    
+     *
+     * @param obj object position, 3 component vector
+     * @param mMv modelview matrix
+     * @param mP projection matrix
+     * @param viewport 4 component viewport vector
+     * @param winPos 3 component window coordinate, the result
+     * @return true if successful, otherwise false (z is 1)
+     */
+    public static boolean mapObjToWin(final Vec3f obj, final Matrix4fb mMv, final Matrix4fb mP,
+                                      final int[] viewport, final float[] winPos)
+    {
+        final Vec4f vec4Tmp1 = new Vec4f(obj, 1f);
+
+        // vec4Tmp2 = Mv * o
+        // rawWinPos = P  * vec4Tmp2
+        // rawWinPos = P * ( Mv * o )
+        // rawWinPos = P * Mv * o
+        final Vec4f vec4Tmp2 = mMv.mulVec4f(vec4Tmp1, new Vec4f());
+        final Vec4f rawWinPos = mP.mulVec4f(vec4Tmp2, vec4Tmp1);
+
+        if (rawWinPos.w() == 0.0f) {
+            return false;
+        }
+
+        final float s = ( 1.0f / rawWinPos.w() ) * 0.5f;
+
+        // Map x, y and z to range 0-1 (w is ignored)
+        rawWinPos.scale(s).add(0.5f, 0.5f, 0.5f, 0f);
+
+        // Map x,y to viewport
+        winPos[0] = rawWinPos.x() * viewport[2] + viewport[0];
+        winPos[1] = rawWinPos.y() * viewport[3] + viewport[1];
+        winPos[2] = rawWinPos.z();
+
+        return true;
+    }
+
+    /**
+     * Map object coordinates to window coordinates.
+     * 
    
+     * Traditional gluProject implementation.
+     * 
    
+     *
+     * @param obj object position, 3 component vector
+     * @param mPMv [projection] x [modelview] matrix, i.e. P x Mv
+     * @param viewport 4 component viewport vector
+     * @param winPos 3 component window coordinate, the result
+     * @return true if successful, otherwise false (z is 1)
+     */
+    public static boolean mapObjToWin(final Vec3f obj, final Matrix4fb mPMv,
+                                      final int[] viewport, final float[] winPos)
+    {
+        final Vec4f vec4Tmp2 = new Vec4f(obj, 1f);
+
+        // rawWinPos = P * Mv * o
+        final Vec4f rawWinPos = mPMv.mulVec4f(vec4Tmp2, new Vec4f());
+
+        if (rawWinPos.w() == 0.0f) {
+            return false;
+        }
+
+        final float s = ( 1.0f / rawWinPos.w() ) * 0.5f;
+
+        // Map x, y and z to range 0-1 (w is ignored)
+        rawWinPos.scale(s).add(0.5f, 0.5f, 0.5f, 0f);
+
+        // Map x,y to viewport
+        winPos[0] = rawWinPos.x() * viewport[2] + viewport[0];
+        winPos[1] = rawWinPos.y() * viewport[3] + viewport[1];
+        winPos[2] = rawWinPos.z();
+
+        return true;
+    }
+
+    /**
+     * Map window coordinates to object coordinates.
+     * 
    
+     * Traditional gluUnProject implementation.
+     * 
    
+     *
+     * @param winx
+     * @param winy
+     * @param winz
+     * @param mMv 4x4 modelview matrix
+     * @param mP 4x4 projection matrix
+     * @param viewport 4 component viewport vector
+     * @param objPos 3 component object coordinate, the result
+     * @param mat4Tmp 16 component matrix for temp storage
+     * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z)
+     */
+    public 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)
+    {
+        // invPMv = Inv(P x Mv)
+        final Matrix4fb invPMv = mat4Tmp.mul(mP, mMv);
+        if( !invPMv.invert() ) {
+            return false;
+        }
+
+        final Vec4f winPos = new Vec4f(winx, winy, winz, 1f);
+
+        // Map x and y from window coordinates
+        winPos.add(-viewport[0], -viewport[1], 0f, 0f).scale(1f/viewport[2], 1f/viewport[3], 1f, 1f);
+
+        // Map to range -1 to 1
+        winPos.scale(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f);
+
+        // rawObjPos = Inv(P x Mv) *  winPos
+        final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f());
+
+        if ( rawObjPos.w() == 0.0f ) {
+            return false;
+        }
+        objPos.set( rawObjPos.scale( 1f / rawObjPos.w() ) );
+
+        return true;
+    }
+
+    /**
+     * Map window coordinates to object coordinates.
+     * 
    
+     * Traditional gluUnProject implementation.
+     * 
    
+     *
+     * @param winx
+     * @param winy
+     * @param winz
+     * @param invPMv inverse [projection] x [modelview] matrix, i.e. Inv(P x Mv)
+     * @param viewport 4 component viewport vector
+     * @param objPos 3 component object coordinate, the result
+     * @param mat4Tmp 16 component matrix for temp storage
+     * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z)
+     */
+    public static boolean mapWinToObj(final float winx, final float winy, final float winz,
+                                      final Matrix4fb invPMv,
+                                      final int[] viewport,
+                                      final Vec3f objPos,
+                                      final Matrix4fb mat4Tmp)
+    {
+        final Vec4f winPos = new Vec4f(winx, winy, winz, 1f);
+
+        // Map x and y from window coordinates
+        winPos.add(-viewport[0], -viewport[1], 0f, 0f).scale(1f/viewport[2], 1f/viewport[3], 1f, 1f);
+
+        // Map to range -1 to 1
+        winPos.scale(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f);
+
+        // rawObjPos = Inv(P x Mv) *  winPos
+        final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f());
+
+        if ( rawObjPos.w() == 0.0f ) {
+            return false;
+        }
+        objPos.set( rawObjPos.scale( 1f / rawObjPos.w() ) );
+
+        return true;
+    }
+
+    /**
+     * Map two window coordinates to two object coordinates,
+     * distinguished by their z component.
+     * 
    
+     * Traditional gluUnProject implementation.
+     * 
    
+     *
+     * @param winx
+     * @param winy
+     * @param winz1
+     * @param winz2
+     * @param invPMv inverse [projection] x [modelview] matrix, i.e. Inv(P x Mv)
+     * @param viewport 4 component viewport vector
+     * @param objPos1 3 component object coordinate, the result
+     * @param mat4Tmp 16 component matrix for temp storage
+     * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z)
+     */
+    public 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)
+    {
+        final Vec4f winPos = new Vec4f(winx, winy, winz1, 1f);
+
+        // Map x and y from window coordinates
+        winPos.add(-viewport[0], -viewport[1], 0f, 0f).scale(1f/viewport[2], 1f/viewport[3], 1f, 1f);
+
+        // Map to range -1 to 1
+        winPos.scale(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f);
+
+        // rawObjPos = Inv(P x Mv) *  winPos1
+        final Vec4f rawObjPos = invPMv.mulVec4f(winPos, new Vec4f());
+
+        if ( rawObjPos.w() == 0.0f ) {
+            return false;
+        }
+        objPos1.set( rawObjPos.scale( 1f / rawObjPos.w() ) );
+
+        //
+        // winz2
+        //
+        // Map Z to range -1 to 1
+        winPos.setZ( winz2 * 2f - 1f );
+
+        // rawObjPos = Inv(P x Mv) *  winPos2
+        invPMv.mulVec4f(winPos, rawObjPos);
+
+        if ( rawObjPos.w() == 0.0f ) {
+            return false;
+        }
+        objPos2.set( rawObjPos.scale( 1f / rawObjPos.w() ) );
+
+        return true;
+    }
+
+    /**
+     * Map window coordinates to object coordinates.
+     * 
    
+     * Traditional gluUnProject4 implementation.
+     * 
    
+     *
+     * @param winx
+     * @param winy
+     * @param winz
+     * @param clipw
+     * @param mMv 4x4 modelview matrix
+     * @param mP 4x4 projection matrix
+     * @param viewport 4 component viewport vector
+     * @param near
+     * @param far
+     * @param obj_pos 4 component object coordinate, the result
+     * @param mat4Tmp 16 component matrix for temp storage
+     * @return true if successful, otherwise false (failed to invert matrix, or becomes infinity due to zero z)
+     */
+    public 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)
+    {
+        // invPMv = Inv(P x Mv)
+        final Matrix4fb invPMv = mat4Tmp.mul(mP, mMv);
+        if( !invPMv.invert() ) {
+            return false;
+        }
+
+        final Vec4f winPos = new Vec4f(winx, winy, winz, clipw);
+
+        // Map x and y from window coordinates
+        winPos.add(-viewport[0], -viewport[1], -near, 0f).scale(1f/viewport[2], 1f/viewport[3], 1f/(far-near), 1f);
+
+        // Map to range -1 to 1
+        winPos.scale(2f, 2f, 2f, 1f).add(-1f, -1f, -1f, 0f);
+
+        // objPos = Inv(P x Mv) *  winPos
+        invPMv.mulVec4f(winPos, objPos);
+
+        if ( objPos.w() == 0.0f ) {
+            return false;
+        }
+        return true;
+    }
+
+    /**
+     * Map two window coordinates w/ shared X/Y and distinctive Z
+     * to a {@link Ray}. The resulting {@link Ray} maybe used for picking
+     * using a {@link AABBox#getRayIntersection(Ray, float[]) bounding box}.
+     * 
    
+     * Notes for picking winz0 and winz1:
+     * 
      
+     *   
    • see {@link FloatUtil#getZBufferEpsilon(int, float, float)}
      • 
+     *   
    • see {@link FloatUtil#getZBufferValue(int, float, float, float)}
      • 
+     *   
    • see {@link FloatUtil#getOrthoWinZ(float, float, float)}
      • 
+     * 
    
+     * 
    
+     * @param winx
+     * @param winy
+     * @param winz0
+     * @param winz1
+     * @param mMv 4x4 modelview matrix
+     * @param mP 4x4 projection matrix
+     * @param viewport 4 component viewport vector
+     * @param ray storage for the resulting {@link Ray}
+     * @param mat4Tmp1 16 component matrix for temp storage
+     * @param mat4Tmp2 16 component matrix for temp storage
+     * @return true if successful, otherwise false (failed to invert matrix, or becomes z is infinity)
+     */
+    public 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) {
+        // invPMv = Inv(P x Mv)
+        final Matrix4fb invPMv = mat4Tmp1.mul(mP, mMv);
+        if( !invPMv.invert() ) {
+            return false;
+        }
+
+        if( mapWinToObj(winx, winy, winz0, winz1, invPMv, viewport,
+                        ray.orig, ray.dir, mat4Tmp2) ) {
+            ray.dir.sub(ray.orig).normalize();
+            return true;
+        } else {
+            return false;
+        }
+    }
+
+    //
+    // String and internals
+    //
+
+    /**
+     * @param sb optional passed StringBuilder instance to be used
+     * @param rowPrefix optional prefix for each row
+     * @param f the format string of one floating point, i.e. "%10.5f", see {@link java.util.Formatter}
+     * @return matrix string representation
+     */
+    public StringBuilder toString(final StringBuilder sb, final String rowPrefix, final String f) {
+        final float[] tmp = new float[16];
+        this.get(tmp);
+        return FloatUtil.matrixToString(sb, rowPrefix, f,tmp, 0, 4, 4, false /* rowMajorOrder */);
+    }
+
+    @Override
+    public String toString() {
+        return toString(null, null, "%10.5f").toString();
+    }
+
+    private final float[] m = new float[16];
+
+    final Stack stack = new Stack(0, 16*16); // start w/ zero size, growSize is half GL-min size (32)
+
+    private static class Stack {
+        private int position;
+        private float[] buffer;
+        private final int growSize;
+
+        /**
+         * @param initialSize initial size
+         * @param growSize grow size if {@link #position()} is reached, maybe 0
+         *        in which case an {@link IndexOutOfBoundsException} is thrown.
+         */
+        public Stack(final int initialSize, final int growSize) {
+            this.position = 0;
+            this.growSize = growSize;
+            this.buffer = new float[initialSize];
+        }
+
+        private final void growIfNecessary(final int length) throws IndexOutOfBoundsException {
+            if( position + length > buffer.length ) {
+                if( 0 >= growSize ) {
+                    throw new IndexOutOfBoundsException("Out of fixed stack size: "+this);
+                }
+                final float[] newBuffer =
+                        new float[buffer.length + growSize];
+                System.arraycopy(buffer, 0, newBuffer, 0, position);
+                buffer = newBuffer;
+            }
+        }
+
+        public final Matrix4fb push(final Matrix4fb src) throws IndexOutOfBoundsException {
+            growIfNecessary(16);
+            src.get(buffer, position);
+            position += 16;
+            return src;
+        }
+
+        public final Matrix4fb pop(final Matrix4fb dest) throws IndexOutOfBoundsException {
+            position -= 16;
+            dest.load(buffer, position);
+            return dest;
+        }
+    }
+}
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtilProject01NOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtilProject01NOUI.java
index 05126676d..107190840 100644
--- a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtilProject01NOUI.java
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtilProject01NOUI.java
@@ -76,12 +76,12 @@ public class TestFloatUtilProject01NOUI extends JunitTracer {
 
         m.gluProject(1f, 0f, 0f, viewport, 0, winA00, 0);
         System.err.println("A.0.0 - Project 1,0 -->" + Arrays.toString(winA00));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4PMv, viewport, 0, winB00, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4PMv, viewport, winB00, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.0 - Project 1,0 -->" + Arrays.toString(winB00));
 
         m.gluProject(0f, 0f, 0f, viewport, 0, winA01, 0);
         System.err.println("A.0.1 - Project 0,0 -->" + Arrays.toString(winA01));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4PMv, viewport, 0, winB01, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4PMv, viewport, winB01, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.1 - Project 0,0 -->" + Arrays.toString(winB01));
 
         m.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
@@ -93,12 +93,12 @@ public class TestFloatUtilProject01NOUI extends JunitTracer {
 
         m.gluProject(1f, 0f, 0f, viewport, 0, winA10, 0);
         System.err.println("A.1.0 - Project 1,0 -->" +Arrays.toString(winA10));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4PMv, viewport, 0, winB10, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4PMv, viewport, winB10, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.0 - Project 1,0 -->" +Arrays.toString(winB10));
 
         m.gluProject(0f, 0f, 0f, viewport, 0, winA11, 0);
         System.err.println("A.1.1 - Project 0,0 -->" +Arrays.toString(winA11));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4PMv, viewport, 0, winB11, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4PMv, viewport, winB11, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.1 - Project 0,0 -->" +Arrays.toString(winB11));
 
         Assert.assertArrayEquals("A/B 0.0 Project 1,0 failure", winB00, winA00, epsilon);
@@ -135,12 +135,12 @@ public class TestFloatUtilProject01NOUI extends JunitTracer {
 
         m.gluProject(1f, 0f, 0f, viewport, 0, winA00, 0);
         System.err.println("A.0.0 - Project 1,0 -->" + Arrays.toString(winA00));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB00, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB00, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.0 - Project 1,0 -->" + Arrays.toString(winB00));
 
         m.gluProject(0f, 0f, 0f, viewport, 0, winA01, 0);
         System.err.println("A.0.1 - Project 0,0 -->" + Arrays.toString(winA01));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB01, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB01, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.1 - Project 0,0 -->" + Arrays.toString(winB01));
 
         m.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
@@ -154,12 +154,12 @@ public class TestFloatUtilProject01NOUI extends JunitTracer {
 
         m.gluProject(1f, 0f, 0f, viewport, 0, winA10, 0);
         System.err.println("A.1.0 - Project 1,0 -->" +Arrays.toString(winA10));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB10, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB10, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.0 - Project 1,0 -->" +Arrays.toString(winB10));
 
         m.gluProject(0f, 0f, 0f, viewport, 0, winA11, 0);
         System.err.println("A.1.1 - Project 0,0 -->" +Arrays.toString(winA11));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB11, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB11, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.1 - Project 0,0 -->" +Arrays.toString(winB11));
 
         Assert.assertArrayEquals("A/B 0.0 Project 1,0 failure", winB00, winA00, epsilon);
@@ -197,12 +197,12 @@ public class TestFloatUtilProject01NOUI extends JunitTracer {
 
         glu.gluProject(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winA00, 0);
         System.err.println("A.0.0 - Project 1,0 -->" + Arrays.toString(winA00));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB00, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB00, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.0 - Project 1,0 -->" + Arrays.toString(winB00));
 
         glu.gluProject(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winA01, 0);
         System.err.println("A.0.1 - Project 0,0 -->" + Arrays.toString(winA01));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB01, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB01, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.1 - Project 0,0 -->" + Arrays.toString(winB01));
 
         m.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
@@ -216,12 +216,12 @@ public class TestFloatUtilProject01NOUI extends JunitTracer {
 
         glu.gluProject(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winA10, 0);
         System.err.println("A.1.0 - Project 1,0 -->" +Arrays.toString(winA10));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB10, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB10, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.0 - Project 1,0 -->" +Arrays.toString(winB10));
 
         glu.gluProject(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winA11, 0);
         System.err.println("A.1.1 - Project 0,0 -->" +Arrays.toString(winA11));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB11, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB11, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.1 - Project 0,0 -->" +Arrays.toString(winB11));
 
         Assert.assertArrayEquals("A/B 0.0 Project 1,0 failure", winB00, winA00, epsilon);
@@ -261,12 +261,12 @@ public class TestFloatUtilProject01NOUI extends JunitTracer {
 
         glu.gluProject(1f, 0f, 0f, d_mat4Mv, 0, d_mat4P, 0, viewport, 0, winA00, 0);
         System.err.println("A.0.0 - Project 1,0 -->" + Arrays.toString(winA00));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB00, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB00, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.0 - Project 1,0 -->" + Arrays.toString(winB00));
 
         glu.gluProject(0f, 0f, 0f, d_mat4Mv, 0, d_mat4P, 0, viewport, 0, winA01, 0);
         System.err.println("A.0.1 - Project 0,0 -->" + Arrays.toString(winA01));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB01, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB01, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.1 - Project 0,0 -->" + Arrays.toString(winB01));
 
         m.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
@@ -282,12 +282,12 @@ public class TestFloatUtilProject01NOUI extends JunitTracer {
 
         glu.gluProject(1f, 0f, 0f, d_mat4Mv, 0, d_mat4P, 0, viewport, 0, winA10, 0);
         System.err.println("A.1.0 - Project 1,0 -->" +Arrays.toString(winA10));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB10, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB10, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.0 - Project 1,0 -->" +Arrays.toString(winB10));
 
         glu.gluProject(0f, 0f, 0f, d_mat4Mv, 0, d_mat4P, 0, viewport, 0, winA11, 0);
         System.err.println("A.1.1 - Project 0,0 -->" +Arrays.toString(winA11));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB11, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4Mv, 0, mat4P, 0, viewport, 0, winB11, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.1 - Project 0,0 -->" +Arrays.toString(winB11));
 
         double[] d_winBxx = Buffers.getDoubleArray(winB00, 0, null, 0, -1);
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtilProject02NOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtilProject02NOUI.java
index ed3f68f5d..c715a41a0 100644
--- a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtilProject02NOUI.java
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestFloatUtilProject02NOUI.java
@@ -74,7 +74,7 @@ public class TestFloatUtilProject02NOUI extends JunitTracer {
         System.err.println("pP");
         System.err.println(FloatUtil.matrixToString(null, "", "%25.20ff", mat4P,  0, 4, 4, true/* rowMajorOrder */));
 
-        FloatUtil.mapObjToWinCoords(objPos[0], objPos[1], objPos[2], mat4Mv, 0, mat4P, 0, viewport, 0, winHas, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(objPos[0], objPos[1], objPos[2], mat4Mv, 0, mat4P, 0, viewport, 0, winHas, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.0 - Project 1,0 -->" + Arrays.toString(winHas));
 
         Assert.assertEquals("A/B 0.0 Project 1,0 failure.x", winExp[0], Math.round(winHas[0]));
@@ -112,7 +112,7 @@ public class TestFloatUtilProject02NOUI extends JunitTracer {
         System.err.println("pP");
         System.err.println(FloatUtil.matrixToString(null, "", "%25.20ff", mat4P,  0, 4, 4, true/* rowMajorOrder */));
 
-        FloatUtil.mapObjToWinCoords(objPos[0], objPos[1], objPos[2], mat4Mv, 0, mat4P, 0, viewport, 0, winHas, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(objPos[0], objPos[1], objPos[2], mat4Mv, 0, mat4P, 0, viewport, 0, winHas, 0, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.0 - Project 1,0 -->" + Arrays.toString(winHas));
 
         Assert.assertEquals("A/B 0.0 Project 1,0 failure.x", winExp[0], Math.round(winHas[0]));
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f01NOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f01NOUI.java
new file mode 100644
index 000000000..2657bce03
--- /dev/null
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f01NOUI.java
@@ -0,0 +1,177 @@
+/**
+ * Copyright 2012-2023 JogAmp Community. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, are
+ * permitted provided that the following conditions are met:
+ *
+ *    1. Redistributions of source code must retain the above copyright notice, this list of
+ *       conditions and the following disclaimer.
+ *
+ *    2. Redistributions in binary form must reproduce the above copyright notice, this list
+ *       of conditions and the following disclaimer in the documentation and/or other materials
+ *       provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * The views and conclusions contained in the software and documentation are those of the
+ * authors and should not be interpreted as representing official policies, either expressed
+ * or implied, of JogAmp Community.
+ */
+
+package com.jogamp.opengl.test.junit.jogl.math;
+
+import org.junit.Assert;
+import org.junit.Test;
+import org.junit.FixMethodOrder;
+import org.junit.runners.MethodSorters;
+
+import com.jogamp.junit.util.JunitTracer;
+import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Matrix4f;
+import com.jogamp.opengl.math.Vec3f;
+
+@FixMethodOrder(MethodSorters.NAME_ASCENDING)
+public class TestMatrix4f01NOUI extends JunitTracer {
+
+    final float[] mI_0 = new float[]{    1,    0,    0,    0,
+                                         0,    1,    0,    0,
+                                         0,    0,    1,    0,
+                                         0,    0,    0,    1 };
+    final Matrix4f mI = new Matrix4f(mI_0);
+
+    final float[] m1_0 = new float[]{    1,    3,    4,    0,
+                                         6,    7,    8,    5,
+                                        98,    7,    6,    9,
+                                        54,    3,    2,    5 };
+    final Matrix4f m1 = new Matrix4f(m1_0);
+
+    final float[] m1T_0 = new float[]{   1,    6,   98,   54,
+                                         3,    7,    7,    3,
+                                         4,    8,    6,    2,
+                                         0,    5,    9,    5 };
+    final Matrix4f m1T = new Matrix4f(m1T_0);
+
+    final float[] m2_0 = new float[]{    1,    6,   98,   54,
+                                         3,    7,    7,    3,
+                                         4,    8,    6,    2,
+                                         0,    5,    9,    5 };
+    final Matrix4f m2 = new Matrix4f(m2_0);
+
+    final float[] m2xm1_0 =
+                       new float[]{   26,   59,  143,   71,
+                                      59,  174,  730,  386,
+                                     143,  730, 9770, 5370,
+                                      71,  386, 5370, 2954 };
+    final Matrix4f m2xm1 = new Matrix4f(m2xm1_0);
+
+    final float[] m1xm2_0 =
+                       new float[]{12557,  893,  748, 1182,
+                                     893,  116,  116,  113,
+                                     748,  116,  120,  104,
+                                    1182,  113,  104,  131 };
+    final Matrix4f m1xm2 = new Matrix4f(m1xm2_0);
+
+    @Test
+    public void test00_load_get() {
+        {
+            final Matrix4f m = new Matrix4f();
+            Assert.assertEquals(mI, m);
+        }
+        {
+            final float[] f16 = new float[16];
+            m1.get(f16);
+            Assert.assertArrayEquals(m1_0, f16, FloatUtil.EPSILON);
+            final Matrix4f m = new Matrix4f();
+            m.load(f16);
+            Assert.assertEquals(m1, m);
+        }
+    }
+
+    @Test
+    public void test01_mul(){
+        {
+            final float[] r_0 = new float[16];
+            FloatUtil.multMatrix(m1_0, 0, m2_0, 0, r_0, 0);
+            Assert.assertArrayEquals(m1xm2_0, r_0, 0f);
+
+            Assert.assertEquals(m1xm2, new Matrix4f(m1).mul(m2));
+            Assert.assertEquals(m1xm2, new Matrix4f().mul(m1, m2));
+        }
+        {
+            final float[] r_0 = new float[16];
+            FloatUtil.multMatrix(m2_0, 0, m1_0, 0, r_0, 0);
+            Assert.assertArrayEquals(m2xm1_0, r_0, 0f);
+
+            Assert.assertEquals(m2xm1, new Matrix4f(m2).mul(m1));
+            Assert.assertEquals(m2xm1, new Matrix4f().mul(m2, m1));
+        }
+    }
+
+    @Test
+    public void test02_transpose() {
+        Assert.assertEquals(m1T, new Matrix4f(m1).transpose());
+        Assert.assertEquals(m1T, new Matrix4f().transpose(m1));
+    }
+
+    @Test
+    public void test80LookAtNegZIsNoOp() throws Exception {
+        final Matrix4f tmp = new Matrix4f();
+        final Matrix4f m = new Matrix4f();
+        // Look towards -z
+        m.setToLookAt(
+                new Vec3f(0, 0, 0),  // eye
+                new Vec3f(0, 0, -1), // center
+                new Vec3f(0, 1, 0),  // up
+                tmp);
+
+        /**
+         * The 3 rows of the matrix (= the 3 columns of the array/buffer) should be: side, up, -forward.
+         */
+        final Matrix4f exp = new Matrix4f(
+                new float[] {
+                        1, 0, 0, 0,
+                        0, 1, 0, 0,
+                        0, 0, 1, 0,
+                        0, 0, 0, 1
+                } );
+
+        Assert.assertEquals(exp, m);
+    }
+
+    @Test
+    public void test81LookAtPosY() throws Exception {
+        final Matrix4f tmp = new Matrix4f();
+        final Matrix4f m = new Matrix4f();
+        // Look towards -z
+        m.setToLookAt(
+                new Vec3f(0, 0, 0),  // eye
+                new Vec3f(0, 1, 0),  // center
+                new Vec3f(0, 0, 1),  // up
+                tmp);
+
+        /**
+         * The 3 rows of the matrix (= the 3 columns of the array/buffer) should be: side, up, -forward.
+         */
+        final Matrix4f exp = new Matrix4f(
+                new float[] {
+                        1, 0, 0, 0,
+                        0, 0, -1, 0,
+                        0, 1, 0, 0,
+                        0, 0, 0, 1
+                } );
+
+        Assert.assertEquals(exp, m);
+    }
+
+    public static void main(final String args[]) {
+        org.junit.runner.JUnitCore.main(TestMatrix4f01NOUI.class.getName());
+    }
+}
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f02MulNOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f02MulNOUI.java
new file mode 100644
index 000000000..5180451a1
--- /dev/null
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f02MulNOUI.java
@@ -0,0 +1,275 @@
+/**
+ * Copyright 2014 JogAmp Community. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, are
+ * permitted provided that the following conditions are met:
+ *
+ *    1. Redistributions of source code must retain the above copyright notice, this list of
+ *       conditions and the following disclaimer.
+ *
+ *    2. Redistributions in binary form must reproduce the above copyright notice, this list
+ *       of conditions and the following disclaimer in the documentation and/or other materials
+ *       provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * The views and conclusions contained in the software and documentation are those of the
+ * authors and should not be interpreted as representing official policies, either expressed
+ * or implied, of JogAmp Community.
+ */
+
+package com.jogamp.opengl.test.junit.jogl.math;
+
+import org.junit.Assert;
+import org.junit.Test;
+import org.junit.FixMethodOrder;
+import org.junit.runners.MethodSorters;
+
+import com.jogamp.common.os.Platform;
+import com.jogamp.junit.util.JunitTracer;
+import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Matrix4f;
+
+@FixMethodOrder(MethodSorters.NAME_ASCENDING)
+public class TestMatrix4f02MulNOUI extends JunitTracer {
+
+    final float[] m1_0 = new float[]{    1,    3,    4,    0,
+                                         6,    7,    8,    5,
+                                        98,    7,    6,    9,
+                                        54,    3,    2,    5 };
+    final Matrix4f m1 = new Matrix4f(m1_0);
+    final Matrix4fb n1 = new Matrix4fb(m1_0);
+
+
+    final float[] m2_0 = new float[]{    1,    6,   98,   54,
+                                         3,    7,    7,    3,
+                                         4,    8,    6,    2,
+                                         0,    5,    9,    5 };
+    final Matrix4f m2 = new Matrix4f(m2_0);
+    final Matrix4fb n2 = new Matrix4fb(m2_0);
+
+    final float[] m2xm1_0 =
+                       new float[]{   26,   59,  143,   71,
+                                      59,  174,  730,  386,
+                                     143,  730, 9770, 5370,
+                                      71,  386, 5370, 2954 };
+    final Matrix4f m2xm1 = new Matrix4f(m2xm1_0);
+    final Matrix4fb n2xn1 = new Matrix4fb(m2xm1_0);
+
+    final float[] m1xm2_0 =
+                       new float[]{12557,  893,  748, 1182,
+                                     893,  116,  116,  113,
+                                     748,  116,  120,  104,
+                                    1182,  113,  104,  131 };
+    final Matrix4f m1xm2 = new Matrix4f(m1xm2_0);
+    final Matrix4fb n1xn2 = new Matrix4fb(m1xm2_0);
+
+    @Test
+    public void test01_mul(){
+        {
+            final float[] r_0 = new float[16];
+            FloatUtil.multMatrix(m1_0, 0, m2_0, 0, r_0, 0);
+            Assert.assertArrayEquals(m1xm2_0, r_0, 0f);
+
+            Assert.assertEquals(m1xm2, new Matrix4f(m1).mul(m2));
+            Assert.assertEquals(m1xm2, new Matrix4f().mul(m1, m2));
+
+            Assert.assertEquals(n1xn2, new Matrix4fb(n1).mul(n2));
+            Assert.assertEquals(n1xn2, new Matrix4fb().mul(n1, n2));
+        }
+        {
+            final float[] r_0 = new float[16];
+            FloatUtil.multMatrix(m2_0, 0, m1_0, 0, r_0, 0);
+            Assert.assertArrayEquals(m2xm1_0, r_0, 0f);
+
+            Assert.assertEquals(m2xm1, new Matrix4f(m2).mul(m1));
+            Assert.assertEquals(m2xm1, new Matrix4f().mul(m2, m1));
+
+            Assert.assertEquals(n2xn1, new Matrix4fb(n2).mul(n1));
+            Assert.assertEquals(n2xn1, new Matrix4fb().mul(n2, n1));
+        }
+    }
+
+    @Test
+    public void test05Perf01(){
+        final float[] res = new float[16];
+
+        final Matrix4f res_m = new Matrix4f();
+        final Matrix4fb res_n = new Matrix4fb();
+
+        final int warmups = 1000;
+        final int loops = 10*1000*1000;
+        long tI1 = 0;
+        long tI2 = 0;
+        long tI4a = 0;
+        long tI4b = 0;
+        long tI5a = 0;
+        long tI5b = 0;
+
+        // warm-up
+        for(int i=0; i Math.abs(temp[i4+i])) {
+                    swap = j;
+                }
+            }
+
+            if (swap != i) {
+                final int swap4 = swap*4;
+                //
+                // Swap rows.
+                //
+                for (k = 0; k < 4; k++) {
+                    t = temp[i4+k];
+                    temp[i4+k] = temp[swap4+k];
+                    temp[swap4+k] = t;
+
+                    t = mres[i4+k+mres_offset];
+                    mres[i4+k+mres_offset] = mres[swap4+k+mres_offset];
+                    mres[swap4+k+mres_offset] = t;
+                }
+            }
+
+            if (temp[i4+i] == 0) {
+                //
+                // No non-zero pivot. The matrix is singular, which shouldn't
+                // happen. This means the user gave us a bad matrix.
+                //
+                return null;
+            }
+
+            t = temp[i4+i];
+            for (k = 0; k < 4; k++) {
+                temp[i4+k] /= t;
+                mres[i4+k+mres_offset] /= t;
+            }
+            for (j = 0; j < 4; j++) {
+                if (j != i) {
+                    final int j4 = j*4;
+                    t = temp[j4+i];
+                    for (k = 0; k < 4; k++) {
+                        temp[j4+k] -= temp[i4+k] * t;
+                        mres[j4+k+mres_offset] -= mres[i4+k+mres_offset]*t;
+                    }
+                }
+            }
+        }
+        return mres;
+    }
+
+    public static void main(final String args[]) {
+        org.junit.runner.JUnitCore.main(TestMatrix4f02MulNOUI.class.getName());
+    }
+}
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f03InversionNOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f03InversionNOUI.java
new file mode 100644
index 000000000..fee38bb54
--- /dev/null
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestMatrix4f03InversionNOUI.java
@@ -0,0 +1,403 @@
+/**
+ * Copyright 2014 JogAmp Community. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, are
+ * permitted provided that the following conditions are met:
+ *
+ *    1. Redistributions of source code must retain the above copyright notice, this list of
+ *       conditions and the following disclaimer.
+ *
+ *    2. Redistributions in binary form must reproduce the above copyright notice, this list
+ *       of conditions and the following disclaimer in the documentation and/or other materials
+ *       provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY JogAmp Community ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL JogAmp Community OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * The views and conclusions contained in the software and documentation are those of the
+ * authors and should not be interpreted as representing official policies, either expressed
+ * or implied, of JogAmp Community.
+ */
+
+package com.jogamp.opengl.test.junit.jogl.math;
+
+import org.junit.Assert;
+import org.junit.Test;
+import org.junit.FixMethodOrder;
+import org.junit.runners.MethodSorters;
+
+import com.jogamp.common.os.Platform;
+import com.jogamp.junit.util.JunitTracer;
+import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Matrix4f;
+
+@FixMethodOrder(MethodSorters.NAME_ASCENDING)
+public class TestMatrix4f03InversionNOUI extends JunitTracer {
+
+    @Test
+    public void test01Ident(){
+        final float[] res1 = new float[16];
+        final float[] res2 = new float[16];
+        final float[] temp = new float[16];
+
+        final float[] identity = new float[] { 1, 0, 0, 0,
+                                               0, 1, 0, 0,
+                                               0, 0, 1, 0,
+                                               0, 0, 0, 1 };
+
+        FloatUtil.invertMatrix(identity, 0, res1, 0);
+        System.err.println(FloatUtil.matrixToString(null, "inv-1: ", "%10.7f", res1, 0, 4, 4, false /* rowMajorOrder */));
+        invertMatrix(identity, 0, res2, 0, temp);
+        System.err.println(FloatUtil.matrixToString(null, "inv-2: ", "%10.7f", res2, 0, 4, 4, false /* rowMajorOrder */));
+
+        Assert.assertArrayEquals("I1/I2 failure", res1, res2, FloatUtil.INV_DEVIANCE);
+        Assert.assertArrayEquals("I2 failure", identity, res2, FloatUtil.INV_DEVIANCE);
+        Assert.assertArrayEquals("I1 failure", identity, res1, FloatUtil.INV_DEVIANCE);
+
+        final Matrix4f res3 = new Matrix4f(identity);
+        Assert.assertTrue( res3.invert() );
+        System.err.println(res3.toString(null, "inv-4: ", "%10.7f"));
+        Assert.assertEquals(new Matrix4f(res1), res3);
+        Assert.assertEquals(new Matrix4f(), res3);
+
+        final Matrix4fb res4 = new Matrix4fb(identity);
+        Assert.assertTrue( res4.invert() );
+        System.err.println(res4.toString(null, "inv-5: ", "%10.7f"));
+        Assert.assertEquals(new Matrix4fb(res1), res4);
+        Assert.assertEquals(new Matrix4fb(), res4);
+    }
+
+    private void testImpl(final float[] matrix) {
+        final float[] inv1_0 = new float[16];
+        final float[] inv1_1 = new float[16];
+        final float[] inv1_2 = new float[16];
+        final float[] inv2_0 = new float[16];
+        final float[] inv2_1 = new float[16];
+        final float[] inv2_2 = new float[16];
+        final float[] temp = new float[16];
+
+        System.err.println(FloatUtil.matrixToString(null, "orig  : ", "%10.7f", matrix, 0, 4, 4, false /* rowMajorOrder */));
+        invertMatrix(matrix, 0, inv1_0, 0, temp);
+        invertMatrix(inv1_0, 0, inv2_0, 0, temp);
+        System.err.println(FloatUtil.matrixToString(null, "inv1_0: ", "%10.7f", inv1_0, 0, 4, 4, false /* rowMajorOrder */));
+        System.err.println(FloatUtil.matrixToString(null, "inv2_0: ", "%10.7f", inv2_0, 0, 4, 4, false /* rowMajorOrder */));
+        FloatUtil.invertMatrix(matrix, 0, inv1_1, 0);
+        FloatUtil.invertMatrix(inv1_1, 0, inv2_1, 0);
+        System.err.println(FloatUtil.matrixToString(null, "inv1_1: ", "%10.7f", inv1_1, 0, 4, 4, false /* rowMajorOrder */));
+        System.err.println(FloatUtil.matrixToString(null, "inv2_1: ", "%10.7f", inv2_1, 0, 4, 4, false /* rowMajorOrder */));
+        FloatUtil.invertMatrix(matrix, inv1_2);
+        FloatUtil.invertMatrix(inv1_2, inv2_2);
+        System.err.println(FloatUtil.matrixToString(null, "inv1_2: ", "%10.7f", inv1_2, 0, 4, 4, false /* rowMajorOrder */));
+        System.err.println(FloatUtil.matrixToString(null, "inv2_2: ", "%10.7f", inv2_2, 0, 4, 4, false /* rowMajorOrder */));
+
+        Assert.assertArrayEquals("I1_1/I1_2 failure", inv1_1, inv1_2, FloatUtil.INV_DEVIANCE);
+        Assert.assertArrayEquals("I2_1/I2_2 failure", inv2_1, inv2_2, FloatUtil.INV_DEVIANCE);
+
+        Assert.assertArrayEquals("I1_0/I1_1 failure", inv1_0, inv1_2, FloatUtil.INV_DEVIANCE);
+        Assert.assertArrayEquals("I2_0/I2_1 failure", inv2_0, inv2_2, FloatUtil.INV_DEVIANCE);
+
+        Assert.assertArrayEquals("I1 failure", matrix, inv2_0, FloatUtil.INV_DEVIANCE);
+        Assert.assertArrayEquals("I2 failure", matrix, inv2_2, FloatUtil.INV_DEVIANCE);
+        Assert.assertArrayEquals("I2 failure", matrix, inv2_1, FloatUtil.INV_DEVIANCE);
+
+        final Matrix4f matrix_m = new Matrix4f(matrix);
+        final Matrix4f inv1_4a = new Matrix4f(matrix_m);
+        Assert.assertTrue( inv1_4a.invert() );
+        final Matrix4f inv2_4a = new Matrix4f(inv1_4a);
+        Assert.assertTrue( inv2_4a.invert() );
+        System.err.println(inv1_4a.toString(null, "inv1_4a: ", "%10.7f"));
+        System.err.println(inv2_4a.toString(null, "inv2_4a: ", "%10.7f"));
+
+        Assert.assertEquals(new Matrix4f(inv1_2), inv1_4a);
+        Assert.assertEquals(new Matrix4f(inv2_2), inv2_4a);
+        Assert.assertTrue("I4 failure: "+matrix_m+" != "+inv2_4a, matrix_m.isEqual(inv2_4a, FloatUtil.INV_DEVIANCE));
+
+        final Matrix4f inv1_4b = new Matrix4f();
+        Assert.assertTrue( inv1_4b.invert(matrix_m) );
+        final Matrix4f inv2_4b = new Matrix4f();
+        Assert.assertTrue( inv2_4b.invert(inv1_4b) );
+        System.err.println(inv1_4b.toString(null, "inv1_4b: ", "%10.7f"));
+        System.err.println(inv2_4b.toString(null, "inv2_4b: ", "%10.7f"));
+
+        Assert.assertEquals(new Matrix4f(inv1_2), inv1_4b);
+        Assert.assertEquals(new Matrix4f(inv2_2), inv2_4b);
+        Assert.assertTrue("I4 failure: "+matrix_m+" != "+inv2_4b, matrix_m.isEqual(inv2_4b, FloatUtil.INV_DEVIANCE));
+
+        //
+        //
+
+        final Matrix4fb matrix_n = new Matrix4fb(matrix);
+        final Matrix4fb inv1_5a = new Matrix4fb(matrix_n);
+        Assert.assertTrue( inv1_5a.invert() );
+        final Matrix4fb inv2_5a = new Matrix4fb(inv1_5a);
+        Assert.assertTrue( inv2_5a.invert() );
+        System.err.println(inv1_5a.toString(null, "inv1_5a: ", "%10.7f"));
+        System.err.println(inv2_5a.toString(null, "inv2_5a: ", "%10.7f"));
+
+        Assert.assertEquals(new Matrix4fb(inv1_2), inv1_5a);
+        Assert.assertEquals(new Matrix4fb(inv2_2), inv2_5a);
+        Assert.assertTrue("I5 failure: "+matrix_n+" != "+inv2_5a, matrix_n.isEqual(inv2_5a, FloatUtil.INV_DEVIANCE));
+
+        final Matrix4fb inv1_5b = new Matrix4fb();
+        Assert.assertTrue( inv1_5b.invert(matrix_n) );
+        final Matrix4fb inv2_5b = new Matrix4fb();
+        Assert.assertTrue( inv2_5b.invert(inv1_5b) );
+        System.err.println(inv1_5b.toString(null, "inv1_5b: ", "%10.7f"));
+        System.err.println(inv2_5b.toString(null, "inv2_5b: ", "%10.7f"));
+
+        Assert.assertEquals(new Matrix4fb(inv1_2), inv1_5b);
+        Assert.assertEquals(new Matrix4fb(inv2_2), inv2_5b);
+        Assert.assertTrue("I5 failure: "+matrix_n+" != "+inv2_5b, matrix_n.isEqual(inv2_5b, FloatUtil.INV_DEVIANCE));
+    }
+
+    @Test
+    public void test02(){
+        final float[] p = new float[] { 2.3464675f, 0,          0,        0,
+                                  0,          2.4142134f, 0,        0,
+                                  0,          0,         -1.0002f, -1,
+                                  0,          0,        -20.002f,   0 };
+        testImpl(p);
+    }
+
+    @Test
+    public void test03(){
+        final float[] mv = new float[] {
+                                   1, 0,    0, 0,
+                                   0, 1,    0, 0,
+                                   0, 0,    1, 0,
+                                   0, 0, -200, 1 } ;
+        testImpl(mv);
+    }
+
+    @Test
+    public void test04(){
+        final float[] p = new float[] {
+                                  2.3464675f, 0,          0,        0,
+                                  0,          2.4142134f, 0,        0,
+                                  0,          0,         -1.0002f, -1,
+                                  0,          0,        -20.002f,   0 };
+
+        testImpl(p);
+    }
+
+    @Test
+    public void test05Perf01(){
+        final float[] p1 = new float[] {
+                                   2.3464675f, 0,          0,        0,
+                                   0,          2.4142134f, 0,        0,
+                                   0,          0,         -1.0002f, -1,
+                                   0,          0,        -20.002f,   0 };
+        final Matrix4f p1_m = new Matrix4f(p1);
+        final Matrix4fb p1_n = new Matrix4fb(p1);
+
+        final float[] p2 = new float[]{
+                                    26,   59,  143,   71,
+                                    59,  174,  730,  386,
+                                   143,  730, 9770, 5370,
+                                    71,  386, 5370, 2954 };
+        final Matrix4f p2_m = new Matrix4f(p2);
+        final Matrix4fb p2_n = new Matrix4fb(p2);
+
+        final float[] res = new float[16];
+        final float[] temp = new float[16];
+
+        final Matrix4f res_m = new Matrix4f();
+        final Matrix4fb res_n = new Matrix4fb();
+
+        final int warmups = 1000;
+        final int loops = 10*1000*1000;
+        long tI0 = 0;
+        long tI1 = 0;
+        long tI2 = 0;
+        long tI4a = 0;
+        long tI4b = 0;
+        long tI5a = 0;
+        long tI5b = 0;
+
+        // warm-up
+        for(int i=0; i res
+            invertMatrix(p1, 0, res, 0, temp);
+
+            // I0: p2 -> res
+            invertMatrix(p2, 0, res, 0, temp);
+        }
+        tI0 = Platform.currentTimeMillis() - t_0;
+
+        // warm-up
+        for(int i=0; i res
+            FloatUtil.invertMatrix(p1, 0, res, 0);
+
+            // I1: p2 -> res
+            FloatUtil.invertMatrix(p2, 0, res, 0);
+        }
+        tI1 = Platform.currentTimeMillis() - t_0;
+
+        // warm-up
+        for(int i=0; i res
+            FloatUtil.invertMatrix(p1, res);
+
+            // I2: p2 -> res
+            FloatUtil.invertMatrix(p2, res);
+        }
+        tI2 = Platform.currentTimeMillis() - t_0;
+
+        //
+        // Matrix4f
+        //
+
+        // warm-up
+        for(int i=0; i Math.abs(temp[i4+i])) {
+                    swap = j;
+                }
+            }
+
+            if (swap != i) {
+                final int swap4 = swap*4;
+                //
+                // Swap rows.
+                //
+                for (k = 0; k < 4; k++) {
+                    t = temp[i4+k];
+                    temp[i4+k] = temp[swap4+k];
+                    temp[swap4+k] = t;
+
+                    t = mres[i4+k+mres_offset];
+                    mres[i4+k+mres_offset] = mres[swap4+k+mres_offset];
+                    mres[swap4+k+mres_offset] = t;
+                }
+            }
+
+            if (temp[i4+i] == 0) {
+                //
+                // No non-zero pivot. The matrix is singular, which shouldn't
+                // happen. This means the user gave us a bad matrix.
+                //
+                return null;
+            }
+
+            t = temp[i4+i];
+            for (k = 0; k < 4; k++) {
+                temp[i4+k] /= t;
+                mres[i4+k+mres_offset] /= t;
+            }
+            for (j = 0; j < 4; j++) {
+                if (j != i) {
+                    final int j4 = j*4;
+                    t = temp[j4+i];
+                    for (k = 0; k < 4; k++) {
+                        temp[j4+k] -= temp[i4+k] * t;
+                        mres[j4+k+mres_offset] -= mres[i4+k+mres_offset]*t;
+                    }
+                }
+            }
+        }
+        return mres;
+    }
+
+    public static void main(final String args[]) {
+        org.junit.runner.JUnitCore.main(TestMatrix4f03InversionNOUI.class.getName());
+    }
+}
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestPMVMatrix03NOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestPMVMatrix03NOUI.java
index 63cb5b539..9468afbd2 100644
--- a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestPMVMatrix03NOUI.java
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestPMVMatrix03NOUI.java
@@ -44,12 +44,12 @@ public class TestPMVMatrix03NOUI extends JunitTracer {
 
         m.gluProject(1f, 0f, 0f, viewport, 0, winA00, 0);
         System.err.println("A.0.0 - Project 1,0 -->" + Arrays.toString(winA00));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4PMv, viewport, 0, winB00, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4PMv, viewport, winB00, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.0 - Project 1,0 -->" + Arrays.toString(winB00));
 
         m.gluProject(0f, 0f, 0f, viewport, 0, winA01, 0);
         System.err.println("A.0.1 - Project 0,0 -->" + Arrays.toString(winA01));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4PMv, viewport, 0, winB01, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4PMv, viewport, winB01, vec4Tmp1, vec4Tmp2);
         System.err.println("B.0.1 - Project 0,0 -->" + Arrays.toString(winB01));
 
         m.glMatrixMode(GLMatrixFunc.GL_PROJECTION);
@@ -61,12 +61,12 @@ public class TestPMVMatrix03NOUI extends JunitTracer {
 
         m.gluProject(1f, 0f, 0f, viewport, 0, winA10, 0);
         System.err.println("A.1.0 - Project 1,0 -->" +Arrays.toString(winA10));
-        FloatUtil.mapObjToWinCoords(1f, 0f, 0f, mat4PMv, viewport, 0, winB10, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(1f, 0f, 0f, mat4PMv, viewport, winB10, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.0 - Project 1,0 -->" +Arrays.toString(winB10));
 
         m.gluProject(0f, 0f, 0f, viewport, 0, winA11, 0);
         System.err.println("A.1.1 - Project 0,0 -->" +Arrays.toString(winA11));
-        FloatUtil.mapObjToWinCoords(0f, 0f, 0f, mat4PMv, viewport, 0, winB11, 0, vec4Tmp1, vec4Tmp2);
+        FloatUtil.mapObjToWin(0f, 0f, 0f, mat4PMv, viewport, winB11, vec4Tmp1, vec4Tmp2);
         System.err.println("B.1.1 - Project 0,0 -->" +Arrays.toString(winB11));
 
         Assert.assertArrayEquals("A/B 0.0 Project 1,0 failure", winB00, winA00, epsilon);
diff --git a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestQuaternion01NOUI.java b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestQuaternion01NOUI.java
index 80d2e088f..ecda4778d 100644
--- a/src/test/com/jogamp/opengl/test/junit/jogl/math/TestQuaternion01NOUI.java
+++ b/src/test/com/jogamp/opengl/test/junit/jogl/math/TestQuaternion01NOUI.java
@@ -37,7 +37,10 @@ import org.junit.runners.MethodSorters;
 
 import com.jogamp.junit.util.JunitTracer;
 import com.jogamp.opengl.math.FloatUtil;
+import com.jogamp.opengl.math.Matrix4f;
 import com.jogamp.opengl.math.Quaternion;
+import com.jogamp.opengl.math.Vec3f;
+import com.jogamp.opengl.math.Vec4f;
 import com.jogamp.opengl.math.VectorUtil;
 
 @FixMethodOrder(MethodSorters.NAME_ASCENDING)
@@ -46,20 +49,20 @@ public class TestQuaternion01NOUI extends JunitTracer {
 
     static final Quaternion QUAT_IDENT = new Quaternion(0f, 0f, 0f, 1f);
 
-    static final float[] ZERO       = new float[] {  0f,  0f,  0f };
-    static final float[] ONE        = new float[] {  1f,  1f,  1f };
-    static final float[] NEG_ONE    = new float[] { -1f, -1f, -1f };
-    static final float[] UNIT_X     = new float[] {  1f,  0f,  0f };
-    static final float[] UNIT_Y     = new float[] {  0f,  1f,  0f };
-    static final float[] UNIT_Z     = new float[] {  0f,  0f,  1f };
-    static final float[] NEG_UNIT_X = new float[] { -1f,  0f,  0f };
-    static final float[] NEG_UNIT_Y = new float[] {  0f, -1f,  0f };
-    static final float[] NEG_UNIT_Z = new float[] {  0f,  0f, -1f };
+    static final Vec3f ZERO       = new Vec3f (  0f,  0f,  0f );
+    static final Vec3f ONE        = new Vec3f (  1f,  1f,  1f );
+    static final Vec3f NEG_ONE    = new Vec3f ( -1f, -1f, -1f );
+    static final Vec3f UNIT_X     = new Vec3f (  1f,  0f,  0f );
+    static final Vec3f UNIT_Y     = new Vec3f (  0f,  1f,  0f );
+    static final Vec3f UNIT_Z     = new Vec3f (  0f,  0f,  1f );
+    static final Vec3f NEG_UNIT_X = new Vec3f ( -1f,  0f,  0f );
+    static final Vec3f NEG_UNIT_Y = new Vec3f (  0f, -1f,  0f );
+    static final Vec3f NEG_UNIT_Z = new Vec3f (  0f,  0f, -1f );
 
-    static final float[] NEG_ONE_v4 = new float[] { -1f, -1f, -1f, 0f };
-    static final float[] ONE_v4     = new float[] {  1f,  1f,  1f, 0f };
+    static final Vec4f NEG_ONE_v4 = new Vec4f ( -1f, -1f, -1f, 0f );
+    static final Vec4f ONE_v4     = new Vec4f (  1f,  1f,  1f, 0f );
 
-    static final float MACH_EPSILON = FloatUtil.getMachineEpsilon();
+    static final float MACH_EPSILON = FloatUtil.EPSILON;
 
     //
     // Basic
@@ -76,8 +79,9 @@ public class TestQuaternion01NOUI extends JunitTracer {
     @Test
     public void test02RotateZeroVector() {
         final Quaternion quat = new Quaternion();
-        final float[] rotVec0 = quat.rotateVector(new float[3], 0, ZERO, 0);
-        Assert.assertArrayEquals(ZERO, rotVec0, FloatUtil.EPSILON);
+        final Vec3f ZERO = new Vec3f(0, 0, 0);
+        final Vec3f rotVec0 = quat.rotateVector(ZERO, new Vec3f());
+        Assert.assertEquals(ZERO, rotVec0);
     }
 
     @Test
@@ -110,24 +114,24 @@ public class TestQuaternion01NOUI extends JunitTracer {
 
     @Test
     public void test10AngleAxis() {
-        final float[] tmpV3f = new float[3];
-        final Quaternion quat1 = new Quaternion().setFromAngleAxis(FloatUtil.HALF_PI, new float[] { 2, 0, 0 }, tmpV3f );
-        final Quaternion quat2 = new Quaternion().setFromAngleNormalAxis(FloatUtil.HALF_PI, new float[] { 1, 0, 0 } );
+        final Vec3f tmpV3f = new Vec3f();
+        final Quaternion quat1 = new Quaternion().setFromAngleAxis(FloatUtil.HALF_PI, new Vec3f ( 2, 0, 0 ), tmpV3f );
+        final Quaternion quat2 = new Quaternion().setFromAngleNormalAxis(FloatUtil.HALF_PI, new Vec3f ( 1, 0, 0 ) );
 
         Assert.assertEquals(quat2, quat1);
         // System.err.println("M "+quat2.magnitude()+", 1-M "+(1f-quat2.magnitude())+", Eps "+FloatUtil.EPSILON);
         Assert.assertEquals(0f, 1 - quat2.magnitude(), FloatUtil.EPSILON);
         Assert.assertTrue(1 - quat1.magnitude() <= FloatUtil.EPSILON);
 
-        final float[] vecOut1 = new float[3];
-        final float[] vecOut2 = new float[3];
-        quat1.rotateVector(vecOut1, 0, ONE, 0);
-        quat2.rotateVector(vecOut2, 0, ONE, 0);
-        Assert.assertArrayEquals(vecOut1, vecOut2, FloatUtil.EPSILON);
-        Assert.assertEquals(0f, Math.abs( VectorUtil.distVec3(vecOut1, vecOut2) ), FloatUtil.EPSILON );
+        final Vec3f vecOut1 = new Vec3f();
+        final Vec3f vecOut2 = new Vec3f();
+        quat1.rotateVector(Vec3f.ONE, vecOut1);
+        quat2.rotateVector(Vec3f.ONE, vecOut2);
+        Assert.assertEquals(vecOut1, vecOut2);
+        Assert.assertEquals(0f, Math.abs( vecOut1.dist(vecOut2) ), FloatUtil.EPSILON );
 
-        quat1.rotateVector(vecOut1, 0, UNIT_Z, 0);
-        Assert.assertEquals(0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_Y, vecOut1) ), FloatUtil.EPSILON );
+        quat1.rotateVector(Vec3f.UNIT_Z, vecOut1);
+        Assert.assertEquals(0f, Math.abs( Vec3f.UNIT_Y_NEG.dist(vecOut1) ), FloatUtil.EPSILON );
 
         quat2.setFromAngleAxis(FloatUtil.HALF_PI, ZERO, tmpV3f);
         Assert.assertEquals(QUAT_IDENT, quat2);
@@ -139,14 +143,14 @@ public class TestQuaternion01NOUI extends JunitTracer {
         quat1.set(0, 0, 0, 0);
         angle = quat1.toAngleAxis(vecOut1);
         Assert.assertTrue(0.0f == angle);
-        Assert.assertArrayEquals(UNIT_X, vecOut1, FloatUtil.EPSILON);
+        Assert.assertEquals(UNIT_X, vecOut1);
     }
 
     @Test
     public void test11FromVectorToVector() {
-        final float[] tmp0V3f = new float[3];
-        final float[] tmp1V3f = new float[3];
-        final float[] vecOut = new float[3];
+        final Vec3f tmp0V3f = new Vec3f();
+        final Vec3f tmp1V3f = new Vec3f();
+        final Vec3f vecOut = new Vec3f();
         final Quaternion quat = new Quaternion();
         quat.setFromVectors(UNIT_Z, UNIT_X, tmp0V3f, tmp1V3f);
 
@@ -158,21 +162,21 @@ public class TestQuaternion01NOUI extends JunitTracer {
         Assert.assertEquals(quat2, quat);
 
         quat.setFromVectors(UNIT_Z, NEG_UNIT_Z, tmp0V3f, tmp1V3f);
-        quat.rotateVector(vecOut, 0, UNIT_Z, 0);
+        quat.rotateVector(UNIT_Z, vecOut);
         // System.err.println("vecOut: "+Arrays.toString(vecOut));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_Z, vecOut) ), Quaternion.ALLOWED_DEVIANCE );
+        Assert.assertEquals( 0f, Math.abs( NEG_UNIT_Z.dist(vecOut) ), Quaternion.ALLOWED_DEVIANCE );
 
         quat.setFromVectors(UNIT_X, NEG_UNIT_X, tmp0V3f, tmp1V3f);
-        quat.rotateVector(vecOut, 0, UNIT_X, 0);
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_X, vecOut) ), Quaternion.ALLOWED_DEVIANCE );
+        quat.rotateVector(UNIT_X, vecOut);
+        Assert.assertEquals( 0f, Math.abs( NEG_UNIT_X.dist(vecOut) ), Quaternion.ALLOWED_DEVIANCE );
 
         quat.setFromVectors(UNIT_Y, NEG_UNIT_Y, tmp0V3f, tmp1V3f);
-        quat.rotateVector(vecOut, 0, UNIT_Y, 0);
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_Y, vecOut) ), Quaternion.ALLOWED_DEVIANCE );
+        quat.rotateVector(UNIT_Y, vecOut);
+        Assert.assertEquals( 0f, Math.abs( NEG_UNIT_Y.dist(vecOut) ), Quaternion.ALLOWED_DEVIANCE );
 
         quat.setFromVectors(ONE, NEG_ONE, tmp0V3f, tmp1V3f);
-        quat.rotateVector(vecOut, 0, ONE, 0);
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_ONE, vecOut) ), Quaternion.ALLOWED_DEVIANCE );
+        quat.rotateVector(ONE, vecOut);
+        Assert.assertEquals( 0f, Math.abs( NEG_ONE.dist(vecOut) ), Quaternion.ALLOWED_DEVIANCE );
 
         quat.setFromVectors(ZERO, ZERO, tmp0V3f, tmp1V3f);
         Assert.assertEquals(QUAT_IDENT, quat);
@@ -182,14 +186,14 @@ public class TestQuaternion01NOUI extends JunitTracer {
     public void test12FromAndToEulerAngles() {
         // Y.Z.X -> X.Y.Z
         final Quaternion quat = new Quaternion();
-        final float[] angles0Exp = new float[] { 0f, FloatUtil.HALF_PI, 0f};
+        final Vec3f angles0Exp = new Vec3f( 0f, FloatUtil.HALF_PI, 0f );
         quat.setFromEuler(angles0Exp);
         Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
 
-        final float[] angles0Has = quat.toEuler(new float[3]);
+        final Vec3f angles0Has = quat.toEuler(new Vec3f());
         // System.err.println("exp0 "+Arrays.toString(angles0Exp));
         // System.err.println("has0 "+Arrays.toString(angles0Has));
-        Assert.assertArrayEquals(angles0Exp, angles0Has, FloatUtil.EPSILON);
+        Assert.assertEquals(angles0Exp, angles0Has);
 
         final Quaternion quat2 = new Quaternion();
         quat2.setFromEuler(angles0Has);
@@ -197,28 +201,28 @@ public class TestQuaternion01NOUI extends JunitTracer {
 
         ///
 
-        final float[] angles1Exp = new float[] { 0f, 0f, -FloatUtil.HALF_PI };
+        final Vec3f angles1Exp = new Vec3f(0f, 0f, -FloatUtil.HALF_PI);
         quat.setFromEuler(angles1Exp);
         Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
 
-        final float[] angles1Has = quat.toEuler(new float[3]);
+        final Vec3f angles1Has = quat.toEuler(new Vec3f());
         // System.err.println("exp1 "+Arrays.toString(angles1Exp));
         // System.err.println("has1 "+Arrays.toString(angles1Has));
-        Assert.assertArrayEquals(angles1Exp, angles1Has, FloatUtil.EPSILON);
+        Assert.assertEquals(angles1Exp, angles1Has);
 
         quat2.setFromEuler(angles1Has);
         Assert.assertEquals(quat, quat2);
 
         ///
 
-        final float[] angles2Exp = new float[] { FloatUtil.HALF_PI, 0f, 0f };
+        final Vec3f angles2Exp = new Vec3f(FloatUtil.HALF_PI, 0f, 0f);
         quat.setFromEuler(angles2Exp);
         Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
 
-        final float[] angles2Has = quat.toEuler(new float[3]);
+        final Vec3f angles2Has = quat.toEuler(new Vec3f());
         // System.err.println("exp2 "+Arrays.toString(angles2Exp));
         // System.err.println("has2 "+Arrays.toString(angles2Has));
-        Assert.assertArrayEquals(angles2Exp, angles2Has, FloatUtil.EPSILON);
+        Assert.assertEquals(angles2Exp, angles2Has);
 
         quat2.setFromEuler(angles2Has);
         Assert.assertEquals(quat, quat2);
@@ -230,233 +234,307 @@ public class TestQuaternion01NOUI extends JunitTracer {
         quat.setFromEuler(0, FloatUtil.HALF_PI, 0); // 90 degrees y-axis
         Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
 
-        final float[] v2 = quat.rotateVector(new float[3], 0, UNIT_X, 0);
-        Assert.assertEquals(0f, Math.abs(VectorUtil.distVec3(NEG_UNIT_Z, v2)), FloatUtil.EPSILON);
+        final Vec3f v2 = quat.rotateVector(UNIT_X, new Vec3f());
+        Assert.assertEquals(0f, Math.abs( NEG_UNIT_Z.dist(v2)), FloatUtil.EPSILON);
 
         quat.setFromEuler(0, 0, -FloatUtil.HALF_PI);
         Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
-        quat.rotateVector(v2, 0, UNIT_X, 0);
-        Assert.assertEquals(0f, Math.abs(VectorUtil.distVec3(NEG_UNIT_Y, v2)), FloatUtil.EPSILON);
+        quat.rotateVector(UNIT_X, v2);
+        Assert.assertEquals(0f, Math.abs( NEG_UNIT_Y.dist(v2)), FloatUtil.EPSILON);
 
         quat.setFromEuler(FloatUtil.HALF_PI, 0, 0);
         Assert.assertEquals(1.0f, quat.magnitude(), FloatUtil.EPSILON);
-        quat.rotateVector(v2, 0, UNIT_Y, 0);
-        Assert.assertEquals(0f, Math.abs(VectorUtil.distVec3(UNIT_Z, v2)), FloatUtil.EPSILON);
+        quat.rotateVector(UNIT_Y, v2);
+        Assert.assertEquals(0f, Math.abs( UNIT_Z.dist(v2)), FloatUtil.EPSILON);
     }
 
     @Test
     public void test14Matrix() {
-        final float[] vecHas = new float[3];
-        final float[] vecOut2 = new float[4];
-        float[] mat1 = new float[4*4];
-        final float[] mat2 = new float[4*4];
+        final Vec3f vecHas = new Vec3f();
+        final Vec3f vecOut3 = new Vec3f();
+        final Vec4f vecOut4 = new Vec4f();
+        final Matrix4f mat1 = new Matrix4f();;
+        final Matrix4f mat2 = new Matrix4f();
         final Quaternion quat = new Quaternion();
 
         //
         // IDENTITY CHECK
         //
-        FloatUtil.makeIdentity(mat1);
+        mat1.loadIdentity();
         quat.set(0, 0, 0, 0);
-        quat.toMatrix(mat2, 0);
-        Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+        quat.toMatrix(mat2);
+        Assert.assertEquals(mat1, mat2);
 
         //
         // 90 degrees rotation on X
         //
 
         float a = FloatUtil.HALF_PI;
-        mat1 = new float[] { // Column Order
-                1,  0,                 0,                0, //
-                0,  FloatUtil.cos(a),  FloatUtil.sin(a), 0, //
-                0, -FloatUtil.sin(a),  FloatUtil.cos(a), 0,
-                0,  0,                 0,                1  };
+        final float[] mat1_0 = new float[] { // Column Order
+            1,  0,                 0,                0, //
+            0,  FloatUtil.cos(a),  FloatUtil.sin(a), 0, //
+            0, -FloatUtil.sin(a),  FloatUtil.cos(a), 0,
+            0,  0,                 0,                1  };
+        mat1.load( mat1_0 );
+        {
+            // Matrix4f load() <-> toFloats()
+            final float[] mat2_0 = new float[16];
+            mat1.get(mat2_0);
+            Assert.assertArrayEquals(mat1_0, mat2_0, FloatUtil.EPSILON);
+        }
         {
             // Validate Matrix via Euler rotation on Quaternion!
             quat.setFromEuler(a, 0f, 0f);
-            quat.toMatrix(mat2, 0);
-            // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
-            Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
-            quat.rotateVector(vecHas, 0, UNIT_Y, 0);
+            {
+                // quat.toMatrix(float[])
+                final float[] mat2_0 = new float[16];
+                quat.toMatrix(mat2_0, 0);
+                Assert.assertArrayEquals(mat1_0, mat2_0, FloatUtil.EPSILON);
+            }
+            {
+                // quat.toMatrix(float[]) and Matrix4f.load()
+                final float[] mat2_0 = new float[16];
+                quat.toMatrix(mat2_0, 0);
+                Assert.assertArrayEquals(mat1_0, mat2_0, FloatUtil.EPSILON);
+                mat2.load(mat2_0);
+                Assert.assertEquals(mat1, mat2);
+            }
+            {
+                // Quaternion.toMatrix(Matrix4f)
+                quat.toMatrix(mat2);
+                // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
+                Assert.assertEquals(mat1, mat2);
+            }
+            quat.rotateVector(UNIT_Y, vecHas);
             // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
             // System.err.println("has0 "+Arrays.toString(vecHas));
-            Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(UNIT_Z, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+            Assert.assertEquals( 0f, Math.abs( UNIT_Z.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE );
         }
-        quat.setFromMatrix(mat1, 0);
-        quat.rotateVector(vecHas, 0, UNIT_Y, 0);
+        mat1.getRotation(quat);
+        quat.setFromMatrix(mat1);
+        quat.rotateVector(UNIT_Y, vecHas);
         // System.err.println("exp0 "+Arrays.toString(UNIT_Z));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(UNIT_Z, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+        Assert.assertEquals( 0f, Math.abs( UNIT_Z.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE );
 
-        quat.toMatrix(mat2, 0);
+        quat.toMatrix(mat2);
         // System.err.println(FloatUtil.matrixToString(null, null, "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
-        Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+        Assert.assertEquals(mat1, mat2);
 
-        quat.rotateVector(vecHas, 0, NEG_ONE, 0);
-        FloatUtil.multMatrixVec(mat2, NEG_ONE_v4, vecOut2);
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );
+        quat.rotateVector(NEG_ONE, vecHas);
+        {
+            // 1st use float[] math
+            final float[] vecHas_0 = new float[3];
+            vecHas.get(vecHas_0);
+            final float[] mat2_0 = new float[16];
+            quat.toMatrix(mat2_0, 0);
+            final float[] NEG_ONE_0 = new float[3];
+            NEG_ONE.get(NEG_ONE_0);
+            final float[] vecOut3_0 = new float[3];
+            FloatUtil.multMatrixVec3(mat2_0, NEG_ONE_0, vecOut3_0);
+            Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecHas_0, vecOut3_0) ), Quaternion.ALLOWED_DEVIANCE );
+            Assert.assertArrayEquals(vecHas_0, vecOut3_0, FloatUtil.EPSILON);
+
+            // 2nd use Vec3f math
+            mat2.mulVec3f(NEG_ONE, vecOut3);
+            Assert.assertEquals( 0f, Math.abs( vecHas.dist(vecOut3) ), Quaternion.ALLOWED_DEVIANCE );
+            Assert.assertEquals(vecHas, vecOut3);
+
+            // 3rd compare both
+            final float[] vecOut3_1 = new float[3];
+            vecOut3.get(vecOut3_1);
+            Assert.assertArrayEquals(vecOut3_0, vecOut3_1, FloatUtil.EPSILON);
+        }
+        {
+            // 1st use float[] math
+            final float[] vecHas_0 = new float[4];
+            vecHas.get(vecHas_0); // w is 0
+            final float[] mat2_0 = new float[16];
+            quat.toMatrix(mat2_0, 0);
+            final float[] NEG_ONE_v4_0 = new float[4];
+            NEG_ONE_v4.get(NEG_ONE_v4_0);
+            final float[] vecOut4_0 = new float[4];
+            FloatUtil.multMatrixVec(mat2_0, NEG_ONE_v4_0, vecOut4_0);
+            Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecHas_0, vecOut4_0) ), Quaternion.ALLOWED_DEVIANCE );
+            Assert.assertArrayEquals(vecHas_0, vecOut4_0, FloatUtil.EPSILON);
+
+            // 2nd use Vec4f math
+            mat2.mulVec4f(NEG_ONE_v4, vecOut4);
+            vecOut3.set(vecOut4);
+            Assert.assertEquals( 0f, Math.abs( vecHas.dist(vecOut3) ), Quaternion.ALLOWED_DEVIANCE );
+            Assert.assertEquals(vecHas, vecOut3);
+
+            // 3rd compare both
+            final float[] vecOut4_1 = new float[4];
+            vecOut4.get(vecOut4_1);
+            Assert.assertArrayEquals(vecOut4_0, vecOut4_1, FloatUtil.EPSILON);
+        }
 
         //
         // 180 degrees rotation on X
         //
         a = FloatUtil.PI;
-        mat1 = new float[] { // Column Order
+        mat1.load( new float[] { // Column Order
                 1,  0,                 0,                0, //
                 0,   FloatUtil.cos(a), FloatUtil.sin(a), 0, //
                 0,  -FloatUtil.sin(a), FloatUtil.cos(a), 0,
-                0,  0,                 0,                1 };
+                0,  0,                 0,                1 } );
         {
             // Validate Matrix via Euler rotation on Quaternion!
             quat.setFromEuler(a, 0f, 0f);
-            quat.toMatrix(mat2, 0);
+            quat.toMatrix(mat2);
             // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
-            Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
-            quat.rotateVector(vecHas, 0, UNIT_Y, 0);
+            Assert.assertEquals(mat1, mat2);
+            quat.rotateVector(UNIT_Y, vecHas);
             // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
             // System.err.println("has0 "+Arrays.toString(vecHas));
-            Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_Y, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+            Assert.assertEquals( 0f, Math.abs( NEG_UNIT_Y.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE );
         }
-        quat.setFromMatrix(mat1, 0);
-        quat.rotateVector(vecHas, 0, UNIT_Y, 0);
+        quat.setFromMatrix(mat1);
+        quat.rotateVector(UNIT_Y, vecHas);
         // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_Y));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_Y, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+        Assert.assertEquals( 0f, Math.abs( NEG_UNIT_Y.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE );
 
-        quat.toMatrix(mat2, 0);
+        quat.toMatrix(mat2);
         // System.err.println(FloatUtil.matrixToString(null, null, "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
-        Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+        Assert.assertEquals(mat1, mat2);
 
-        quat.rotateVector(vecHas, 0, ONE, 0);
-        FloatUtil.multMatrixVec(mat2, ONE_v4, vecOut2);
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );
+        quat.rotateVector(ONE, vecHas);
+        mat2.mulVec4f(ONE_v4, vecOut4);
+        vecOut3.set(vecOut4);
+        Assert.assertEquals( 0f, Math.abs( vecHas.dist(vecOut3) ), Quaternion.ALLOWED_DEVIANCE );
 
         //
         // 180 degrees rotation on Y
         //
         a = FloatUtil.PI;
-        mat1 = new float[] { // Column Order
+        mat1.load( new float[] { // Column Order
                  FloatUtil.cos(a), 0,  -FloatUtil.sin(a), 0, //
                  0,                1,   0,                0, //
                  FloatUtil.sin(a), 0,   FloatUtil.cos(a), 0,
-                 0,                0,   0,                1 };
+                 0,                0,   0,                1 } );
         {
             // Validate Matrix via Euler rotation on Quaternion!
             quat.setFromEuler(0f, a, 0f);
-            quat.toMatrix(mat2, 0);
+            quat.toMatrix(mat2);
             // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
-            Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
-            quat.rotateVector(vecHas, 0, UNIT_X, 0);
+            Assert.assertEquals(mat1, mat2);
+            quat.rotateVector(UNIT_X, vecHas);
             // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
             // System.err.println("has0 "+Arrays.toString(vecHas));
-            Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_X, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+            Assert.assertEquals( 0f, Math.abs( NEG_UNIT_X.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE );
         }
-        quat.setFromMatrix(mat1, 0);
-        quat.rotateVector(vecHas, 0, UNIT_X, 0);
+        quat.setFromMatrix(mat1);
+        quat.rotateVector(UNIT_X, vecHas);
         // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_X, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+        Assert.assertEquals( 0f, Math.abs( NEG_UNIT_X.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE );
 
-        quat.toMatrix(mat2, 0);
+        quat.toMatrix(mat2);
         // System.err.println(FloatUtil.matrixToString(null, "matr-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
-        Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+        Assert.assertEquals(mat1, mat2);
 
-        quat.rotateVector(vecHas, 0, NEG_ONE, 0);
-        FloatUtil.multMatrixVec(mat2, NEG_ONE_v4, vecOut2);
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );
+        quat.rotateVector(NEG_ONE, vecHas);
+        mat2.mulVec4f(NEG_ONE_v4, vecOut4);
+        vecOut3.set(vecOut4);
+        Assert.assertEquals( 0f, Math.abs( vecHas.dist(vecOut3) ), Quaternion.ALLOWED_DEVIANCE );
 
         //
         // 180 degrees rotation on Z
         //
         a = FloatUtil.PI;
-        mat1 = new float[] { // Column Order
+        mat1.load( new float[] { // Column Order
                   FloatUtil.cos(a), FloatUtil.sin(a), 0, 0, //
                  -FloatUtil.sin(a), FloatUtil.cos(a), 0, 0,
                   0,                0,                1, 0,
-                  0,                0,                0, 1 };
+                  0,                0,                0, 1 } );
         {
             // Validate Matrix via Euler rotation on Quaternion!
             quat.setFromEuler(0f, 0f, a);
-            quat.toMatrix(mat2, 0);
+            quat.toMatrix(mat2);
             // System.err.println(FloatUtil.matrixToString(null, "quat-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
-            Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
-            quat.rotateVector(vecHas, 0, UNIT_X, 0);
+            Assert.assertEquals(mat1, mat2);
+            quat.rotateVector(UNIT_X, vecHas);
             // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
             // System.err.println("has0 "+Arrays.toString(vecHas));
-            Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_X, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+            Assert.assertEquals( 0f, Math.abs( NEG_UNIT_X.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE );
         }
-        quat.setFromMatrix(mat1, 0);
-        quat.rotateVector(vecHas, 0, UNIT_X, 0);
+        quat.setFromMatrix(mat1);
+        quat.rotateVector(UNIT_X, vecHas);
         // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_X, vecHas) ), Quaternion.ALLOWED_DEVIANCE );
+        Assert.assertEquals( 0f, Math.abs( NEG_UNIT_X.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE );
 
-        quat.toMatrix(mat2, 0);
+        quat.toMatrix(mat2);
         // System.err.println(FloatUtil.matrixToString(null, "matr-rot", "%10.5f", mat1, 0, mat2, 0, 4, 4, false).toString());
-        Assert.assertArrayEquals(mat1, mat2, FloatUtil.EPSILON);
+        Assert.assertEquals(mat1, mat2);
 
-        quat.rotateVector(vecHas, 0, ONE, 0);
-        FloatUtil.multMatrixVec(mat2, ONE_v4, vecOut2);
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecHas, vecOut2) ), Quaternion.ALLOWED_DEVIANCE );
+        quat.rotateVector(ONE, vecHas);
+        mat2.mulVec4f(ONE_v4, vecOut4);
+        vecOut3.set(vecOut4);
+        Assert.assertEquals( 0f, Math.abs( vecHas.dist(vecOut3) ), Quaternion.ALLOWED_DEVIANCE );
 
         //
         // Test Matrix-Columns
         //
 
         a = FloatUtil.QUARTER_PI;
-        final float[] vecExp = new float[3];
-        final float[] vecCol = new float[3];
-        mat1 = new float[] { // Column Order
+        final Vec3f vecExp0 = new Vec3f( FloatUtil.cos(a), FloatUtil.sin(a), 0);
+        final Vec3f vecExp1 = new Vec3f(-FloatUtil.sin(a), FloatUtil.cos(a), 0);
+        final Vec3f vecExp2 = new Vec3f( 0,                0,                1);
+        final Vec3f vecCol = new Vec3f();
+        mat1.load( new float[] { // Column Order
                   FloatUtil.cos(a), FloatUtil.sin(a), 0, 0, //
                  -FloatUtil.sin(a), FloatUtil.cos(a), 0, 0,
                   0,                0,                1, 0,
-                  0,                0,                0, 1 };
-        quat.setFromMatrix(mat1, 0);
-        FloatUtil.copyMatrixColumn(mat1, 0, 0, vecExp, 0);
-        quat.copyMatrixColumn(0, vecCol, 0);
+                  0,                0,                0, 1 } );
+        mat1.getColumn(0, vecCol);
         // System.err.println("exp0 "+Arrays.toString(vecExp));
-        // System.err.println("has0 "+Arrays.toString(vecCol));
-        Assert.assertEquals(0f, Math.abs( VectorUtil.distVec3(vecExp, vecCol)), FloatUtil.EPSILON);
+        // System.err.println("has0 "+Arrays.toString(vecCol))
+        Assert.assertEquals(vecExp0, vecCol);
+        Assert.assertEquals(0f, Math.abs( vecExp0.dist(vecCol)), FloatUtil.EPSILON);
 
-        FloatUtil.copyMatrixColumn(mat1, 0, 1, vecExp, 0);
-        quat.copyMatrixColumn(1, vecCol, 0);
-        // System.err.println("exp1 "+Arrays.toString(vecExp));
-        // System.err.println("has1 "+Arrays.toString(vecCol));
-        Assert.assertEquals(0f, Math.abs( VectorUtil.distVec3(vecExp, vecCol)), FloatUtil.EPSILON);
-
-        FloatUtil.copyMatrixColumn(mat1, 0, 2, vecExp, 0);
-        quat.copyMatrixColumn(2, vecCol, 0);
-        // System.err.println("exp2 "+Arrays.toString(vecExp));
-        // System.err.println("has2 "+Arrays.toString(vecCol));
-        Assert.assertEquals(0f, Math.abs( VectorUtil.distVec3(vecExp, vecCol)), FloatUtil.EPSILON);
-
-        quat.set(0f, 0f, 0f, 0f);
-        Assert.assertArrayEquals(UNIT_X, quat.copyMatrixColumn(0, vecCol, 0), FloatUtil.EPSILON);
+        mat1.getColumn(1, vecCol);
+        Assert.assertEquals(vecExp1, vecCol);
+        Assert.assertEquals(0f, Math.abs( vecExp1.dist(vecCol)), FloatUtil.EPSILON);
 
+        mat1.getColumn(2, vecCol);
+        Assert.assertEquals(vecExp2, vecCol);
+        Assert.assertEquals(0f, Math.abs( vecExp2.dist(vecCol)), FloatUtil.EPSILON);
     }
 
     @Test
     public void test15aAxesAndMatrix() {
-        final float[] eulerExp = new float[] { 0f, FloatUtil.HALF_PI, 0f };
-        final float[] matExp = new float[4*4];
-        FloatUtil.makeRotationEuler(matExp, 0, eulerExp[0], eulerExp[1], eulerExp[2]); // 45 degr on X, 90 degr on Y
+        final Vec3f eulerExp = new Vec3f ( 0f, FloatUtil.HALF_PI, 0f ); // 45 degr on X, 90 degr on Y
+        final Matrix4f matExp1 = new Matrix4f();
+        matExp1.setToRotationEuler(eulerExp.x(), eulerExp.y(), eulerExp.z());
+        {
+            final float[] matExp0 = new float[4*4];
+            FloatUtil.makeRotationEuler(matExp0, 0, eulerExp.x(), eulerExp.y(), eulerExp.z());
+            final Matrix4f matExp0b = new Matrix4f();
+            matExp0b.load(matExp0);
+            Assert.assertEquals(matExp0b, matExp1);
+        }
 
-        final float[] matHas = new float[4*4];
+        final Matrix4f matHas = new Matrix4f();;
         final Quaternion quat1 = new Quaternion();
         quat1.setFromEuler(eulerExp);
-        quat1.toMatrix(matHas, 0);
+        quat1.toMatrix(matHas);
         // System.err.println(FloatUtil.matrixToString(null, "exp-has", "%10.5f", matExp, 0, matHas, 0, 4, 4, false).toString());
-        Assert.assertArrayEquals(matExp, matHas, FloatUtil.EPSILON);
+        Assert.assertEquals(matExp1, matHas);
 
-        final float[] eulerHas = new float[3];
+        final Vec3f eulerHas = new Vec3f();
         final Quaternion quat2 = new Quaternion();
-        quat2.setFromMatrix(matExp, 0);
+        quat2.setFromMatrix(matExp1);
         quat2.toEuler(eulerHas);
         // System.err.println("exp-euler "+Arrays.toString(eulerExp));
         // System.err.println("has-euler "+Arrays.toString(eulerHas));
-        Assert.assertArrayEquals(eulerExp, eulerHas, FloatUtil.EPSILON);
+        Assert.assertEquals(eulerExp, eulerHas);
 
         Assert.assertEquals(quat2, quat1);
 
-        final float[] angles = new float[3];
+        final Vec3f angles = new Vec3f();
         quat2.toEuler(angles);
         quat1.setFromEuler(angles);
         Assert.assertEquals(quat2, quat1);
@@ -464,28 +542,39 @@ public class TestQuaternion01NOUI extends JunitTracer {
 
     @Test
     public void test15bAxesAndMatrix() {
-        final float[] eulerExp = new float[] { FloatUtil.HALF_PI, 0f, 0f };
-        final float[] matExp = new float[4*4];
-        FloatUtil.makeRotationEuler(matExp, 0, eulerExp[0], eulerExp[1], eulerExp[2]); // 45 degr on X, 90 degr on Y
+        final Vec3f eulerExp = new Vec3f(FloatUtil.HALF_PI, 0f, 0f);
+        final Matrix4f matExp = new Matrix4f();
+        matExp.setToRotationEuler(eulerExp.x(), eulerExp.y(), eulerExp.z()); // 45 degr on X, 90 degr on Y (?)
+        {
+            final float[] matExp_b0 = new float[4*4];
+            FloatUtil.makeRotationEuler(matExp_b0, 0, eulerExp.x(), eulerExp.y(), eulerExp.z());
+            final Matrix4f matExp_b = new Matrix4f();
+            matExp_b.load(matExp_b0);
+            Assert.assertEquals(matExp_b, matExp);
+
+            final float[] matExp_b1 = new float[16];
+            matExp.get(matExp_b1);
+            Assert.assertArrayEquals(matExp_b0, matExp_b1, FloatUtil.EPSILON);
+        }
 
-        final float[] matHas = new float[4*4];
+        final Matrix4f matHas = new Matrix4f();
         final Quaternion quat1 = new Quaternion();
         quat1.setFromEuler(eulerExp);
-        quat1.toMatrix(matHas, 0);
+        quat1.toMatrix(matHas);
         // System.err.println(FloatUtil.matrixToString(null, "exp-has", "%10.5f", matExp, 0, matHas, 0, 4, 4, false).toString());
-        Assert.assertArrayEquals(matExp, matHas, FloatUtil.EPSILON);
+        Assert.assertEquals(matExp, matHas);
 
-        final float[] eulerHas = new float[3];
+        final Vec3f eulerHas = new Vec3f();
         final Quaternion quat2 = new Quaternion();
-        quat2.setFromMatrix(matExp, 0);
+        quat2.setFromMatrix(matExp);
         quat2.toEuler(eulerHas);
         // System.err.println("exp-euler "+Arrays.toString(eulerExp));
         // System.err.println("has-euler "+Arrays.toString(eulerHas));
-        Assert.assertArrayEquals(eulerExp, eulerHas, FloatUtil.EPSILON);
+        Assert.assertEquals(eulerExp, eulerHas);
 
         Assert.assertEquals(quat2, quat1);
 
-        final float[] angles = new float[3];
+        final Vec3f angles = new Vec3f();
         quat2.toEuler(angles);
         quat1.setFromEuler(angles);
         Assert.assertEquals(quat2, quat1);
@@ -493,28 +582,68 @@ public class TestQuaternion01NOUI extends JunitTracer {
 
     @Test
     public void test15cAxesAndMatrix() {
-        final float[] eulerExp = new float[] { FloatUtil.QUARTER_PI, FloatUtil.HALF_PI, 0f };
-        final float[] matExp = new float[4*4];
-        FloatUtil.makeRotationEuler(matExp, 0, eulerExp[0], eulerExp[1], eulerExp[2]); // 45 degr on X, 90 degr on Y
+        final Vec3f eulerExp1 = new Vec3f(FloatUtil.QUARTER_PI, FloatUtil.HALF_PI, 0f); // 45 degr on X, 90 degr on Y
+        final float[] eulerExp0 = new float[3];
+        eulerExp1.get(eulerExp0);
 
-        final float[] matHas = new float[4*4];
+        final Matrix4f matExp = new Matrix4f();
+        matExp.setToRotationEuler(eulerExp1.x(), eulerExp1.y(), eulerExp1.z());
+        {
+            final float[] matExp_b0 = new float[4*4];
+            FloatUtil.makeRotationEuler(matExp_b0, 0, eulerExp1.x(), eulerExp1.y(), eulerExp1.z());
+            final Matrix4f matExp_b = new Matrix4f();
+            matExp_b.load(matExp_b0);
+            Assert.assertEquals(matExp_b, matExp);
+
+            final float[] matExp_b1 = new float[16];
+            matExp.get(matExp_b1);
+            Assert.assertArrayEquals(matExp_b0, matExp_b1, FloatUtil.EPSILON);
+
+            matExp.get(matExp_b0);
+            final Quaternion quat2 = new Quaternion();
+            quat2.setFromMatrix(matExp);
+            quat2.toMatrix(matExp_b1, 0);
+            Assert.assertArrayEquals(matExp_b0, matExp_b1, FloatUtil.EPSILON);
+            quat2.toMatrix(matExp_b);
+            Assert.assertEquals(matExp, matExp_b);
+        }
+
+        final Matrix4f matHas = new Matrix4f();
         final Quaternion quat1 = new Quaternion();
-        quat1.setFromEuler(eulerExp);
-        quat1.toMatrix(matHas, 0);
+        quat1.setFromEuler(eulerExp1);
+        quat1.toMatrix(matHas);
         // System.err.println(FloatUtil.matrixToString(null, "exp-has", "%10.5f", matExp, 0, matHas, 0, 4, 4, false).toString());
-        Assert.assertArrayEquals(matExp, matHas, FloatUtil.EPSILON);
+        Assert.assertEquals(matExp, matHas);
 
-        final float[] eulerHas = new float[3];
+        final Vec3f eulerHas1 = new Vec3f();
         final Quaternion quat2 = new Quaternion();
-        quat2.setFromMatrix(matExp, 0);
-        quat2.toEuler(eulerHas);
-        // System.err.println("exp-euler "+Arrays.toString(eulerExp));
-        // System.err.println("has-euler "+Arrays.toString(eulerHas));
-        Assert.assertArrayEquals(eulerExp, eulerHas, FloatUtil.EPSILON);
+        quat2.setFromMatrix(matExp);
+        quat2.toEuler(eulerHas1); // Vec3f
+        if( DEBUG ) {
+            System.err.println("PI");
+            System.err.printf("  double %20.20f%n", Math.PI);
+            System.err.printf("   float %20.20f%n", FloatUtil.PI);
+            System.err.printf("    diff %20.20f%n", (Math.PI - FloatUtil.PI));
+            System.err.println("PI/2");
+            System.err.printf("  double %20.20f%n", Math.PI/2f);
+            System.err.printf("   float %20.20f%n", FloatUtil.HALF_PI);
+            System.err.printf("    diff %20.20f%n", (Math.PI/2f - FloatUtil.HALF_PI));
+
+            System.err.println("exp-euler "+eulerExp1);
+            System.err.println("has-euler1 "+eulerHas1);
+            System.err.println("dif-euler1 "+eulerExp1.minus(eulerHas1));
+        }
+        {
+            final float[] eulerHas0 = new float[3];
+            eulerHas1.get(eulerHas0);
+            Assert.assertArrayEquals(eulerExp0, eulerHas0, FloatUtil.EPSILON);
+        }
+        Assert.assertTrue(eulerExp1+" != "+eulerHas1, eulerExp1.isEqual(eulerHas1, Quaternion.ALLOWED_DEVIANCE));
+        // Assert.assertEquals(eulerExp1, eulerHas1); // `diff < EPSILON` criteria hits, while `Assert.assertArrayEquals(..)` uses `diff <= EPSILON`
 
         Assert.assertEquals(quat2, quat1);
 
-        final float[] angles = new float[3];
+        final Vec3f angles = new Vec3f();
         quat2.toEuler(angles);
         quat1.setFromEuler(angles);
         Assert.assertEquals(quat2, quat1);
@@ -562,20 +691,20 @@ public class TestQuaternion01NOUI extends JunitTracer {
         quat2.set(quat1);
         quat2.mult(quat1); // q2 = q1 * q1 -> 2 * 45 degr -> 90 degr on Y
 
-        final float[] vecOut = new float[3];
-        quat2.rotateVector(vecOut, 0, UNIT_Z, 0);
-        Assert.assertTrue( Math.abs( VectorUtil.distVec3(UNIT_X, vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
+        final Vec3f vecOut = new Vec3f();
+        quat2.rotateVector(UNIT_Z, vecOut);
+        Assert.assertTrue( Math.abs( UNIT_X.dist(vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
 
         quat2.setFromAngleNormalAxis(FloatUtil.HALF_PI, UNIT_Y); // 90 degr on Y
         quat1.mult(quat1); // q1 = q1 * q1 -> 2 * 45 degr ->  90 degr on Y
         quat1.mult(quat2); // q1 = q1 * q2 -> 2 * 90 degr -> 180 degr on Y
-        quat1.rotateVector(vecOut, 0, UNIT_Z, 0);
-        Assert.assertTrue( Math.abs( VectorUtil.distVec3(NEG_UNIT_Z, vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
+        quat1.rotateVector(UNIT_Z, vecOut);
+        Assert.assertTrue( Math.abs( NEG_UNIT_Z.dist(vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
 
         quat2.setFromEuler(0f, FloatUtil.HALF_PI, 0f);
         quat1.mult(quat2); // q1 = q1 * q2 = q1 * rotMat(0, 90degr, 0)
-        quat1.rotateVector(vecOut, 0, UNIT_Z, 0);
-        Assert.assertTrue( Math.abs( VectorUtil.distVec3(NEG_UNIT_X, vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
+        quat1.rotateVector(UNIT_Z, vecOut);
+        Assert.assertTrue( Math.abs( NEG_UNIT_X.dist(vecOut)) <= Quaternion.ALLOWED_DEVIANCE);
     }
 
     @Test
@@ -631,50 +760,54 @@ public class TestQuaternion01NOUI extends JunitTracer {
         }
 
 
-        float[] vecExp = new float[3];
-        float[] vecRot = new float[3];
+        final Vec3f vecExp = new Vec3f();
+        final Vec3f vecRot = new Vec3f();
         final Quaternion quat = new Quaternion();
 
         // Try a new way with new angles...
         quat.setFromEuler(FloatUtil.HALF_PI, FloatUtil.QUARTER_PI, FloatUtil.PI);
-        vecRot = new float[] { 1f, 1f, 1f };
-        quat.rotateVector(vecRot, 0, vecRot, 0);
+        vecRot.set(1f, 1f, 1f);
+        quat.rotateVector(vecRot, vecRot);
 
         // expected
-        vecExp = new float[] { 1f, 1f, 1f };
+        vecExp.set(1f, 1f, 1f);
         final Quaternion worker = new Quaternion();
         // put together matrix, then apply to vector, so YZX
         worker.rotateByAngleY(FloatUtil.QUARTER_PI).rotateByAngleZ(FloatUtil.PI).rotateByAngleX(FloatUtil.HALF_PI);
-        quat.rotateVector(vecExp, 0, vecExp, 0);
-        Assert.assertEquals(0f, VectorUtil.distVec3(vecExp, vecRot), FloatUtil.EPSILON);
+        quat.rotateVector(vecExp, vecExp);
+        Assert.assertEquals(0f, vecExp.dist(vecRot), FloatUtil.EPSILON);
+        Assert.assertEquals(vecExp, vecRot);
 
         // test axis rotation methods against general purpose
         // X AXIS
-        vecExp = new float[] { 1f, 1f, 1f };
-        vecRot = new float[] { 1f, 1f, 1f };
-        worker.setIdentity().rotateByAngleX(FloatUtil.QUARTER_PI).rotateVector(vecExp, 0, vecExp, 0);
-        worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 1f, 0f, 0f).rotateVector(vecRot, 0, vecRot, 0);
+        vecExp.set(1f, 1f, 1f);
+        vecRot.set(1f, 1f, 1f);
+        worker.setIdentity().rotateByAngleX(FloatUtil.QUARTER_PI).rotateVector(vecExp, vecExp);
+        worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 1f, 0f, 0f).rotateVector(vecRot, vecRot);
         // System.err.println("exp0 "+Arrays.toString(vecExp)+", len "+VectorUtil.length(vecExp));
         // System.err.println("has0 "+Arrays.toString(vecRot)+", len "+VectorUtil.length(vecRot));
-        Assert.assertEquals(0f, VectorUtil.distVec3(vecExp, vecRot), FloatUtil.EPSILON);
+        Assert.assertEquals(0f, vecExp.dist(vecRot), FloatUtil.EPSILON);
+        Assert.assertEquals(vecExp, vecRot);
 
         // Y AXIS
-        vecExp = new float[] { 1f, 1f, 1f };
-        vecRot = new float[] { 1f, 1f, 1f };
-        worker.setIdentity().rotateByAngleY(FloatUtil.QUARTER_PI).rotateVector(vecExp, 0, vecExp, 0);
-        worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 0f, 1f, 0f).rotateVector(vecRot, 0, vecRot, 0);
+        vecExp.set(1f, 1f, 1f);
+        vecRot.set(1f, 1f, 1f);
+        worker.setIdentity().rotateByAngleY(FloatUtil.QUARTER_PI).rotateVector(vecExp, vecExp);
+        worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 0f, 1f, 0f).rotateVector(vecRot, vecRot);
         // System.err.println("exp0 "+Arrays.toString(vecExp));
         // System.err.println("has0 "+Arrays.toString(vecRot));
-        Assert.assertEquals(0f, VectorUtil.distVec3(vecExp, vecRot), FloatUtil.EPSILON);
+        Assert.assertEquals(0f, vecExp.dist(vecRot), FloatUtil.EPSILON);
+        Assert.assertEquals(vecExp, vecRot);
 
         // Z AXIS
-        vecExp = new float[] { 1f, 1f, 1f };
-        vecRot = new float[] { 1f, 1f, 1f };
-        worker.setIdentity().rotateByAngleZ(FloatUtil.QUARTER_PI).rotateVector(vecExp, 0, vecExp, 0);
-        worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 0f, 0f, 1f).rotateVector(vecRot, 0, vecRot, 0);
+        vecExp.set(1f, 1f, 1f);
+        vecRot.set(1f, 1f, 1f);
+        worker.setIdentity().rotateByAngleZ(FloatUtil.QUARTER_PI).rotateVector(vecExp, vecExp);
+        worker.setIdentity().rotateByAngleNormalAxis(FloatUtil.QUARTER_PI, 0f, 0f, 1f).rotateVector(vecRot, vecRot);
         // System.err.println("exp0 "+Arrays.toString(vecExp));
         // System.err.println("has0 "+Arrays.toString(vecRot));
-        Assert.assertEquals(0f, VectorUtil.distVec3(vecExp, vecRot), FloatUtil.EPSILON);
+        Assert.assertEquals(0f, vecExp.dist(vecRot), FloatUtil.EPSILON);
+        Assert.assertEquals(vecExp, vecRot);
 
         quat.set(worker);
         worker.rotateByAngleNormalAxis(0f, 0f, 0f, 0f);
@@ -684,18 +817,18 @@ public class TestQuaternion01NOUI extends JunitTracer {
     @Test
     public void test24Axes() {
         final Quaternion quat0 = new Quaternion().rotateByAngleX(FloatUtil.QUARTER_PI).rotateByAngleY(FloatUtil.HALF_PI);
-        final float[] rotMat = new float[4*4];
-        quat0.toMatrix(rotMat, 0);
-        final float[] xAxis = new float[3];
-        final float[] yAxis = new float[3];
-        final float[] zAxis = new float[3];
-        FloatUtil.copyMatrixColumn(rotMat, 0, 0, xAxis, 0);
-        FloatUtil.copyMatrixColumn(rotMat, 0, 1, yAxis, 0);
-        FloatUtil.copyMatrixColumn(rotMat, 0, 2, zAxis, 0);
+        final Matrix4f rotMat = new Matrix4f();
+        quat0.toMatrix(rotMat);
+        final Vec3f xAxis = new Vec3f();
+        final Vec3f yAxis = new Vec3f();
+        final Vec3f zAxis = new Vec3f();
+        rotMat.getColumn(0, xAxis);
+        rotMat.getColumn(1, yAxis);
+        rotMat.getColumn(2, zAxis);
 
         final Quaternion quat1 = new Quaternion().setFromAxes(xAxis, yAxis, zAxis);
         Assert.assertEquals(quat0, quat1);
-        final Quaternion quat2 = new Quaternion().setFromMatrix(rotMat, 0);
+        final Quaternion quat2 = new Quaternion().setFromMatrix(rotMat);
         Assert.assertEquals(quat2, quat1);
 
         quat1.toAxes(xAxis, yAxis, zAxis, rotMat);
@@ -709,107 +842,126 @@ public class TestQuaternion01NOUI extends JunitTracer {
         final Quaternion quat1 = new Quaternion();     // angle: 0 degrees
         final Quaternion quat2 = new Quaternion().rotateByAngleY(FloatUtil.HALF_PI); // angle: 90 degrees, axis Y
 
-        float[] vecExp = new float[] { FloatUtil.sin(FloatUtil.QUARTER_PI), 0f, FloatUtil.sin(FloatUtil.QUARTER_PI) };
-        final float[] vecHas = new float[3];
+        final Vec3f vecExp = new Vec3f( FloatUtil.sin(FloatUtil.QUARTER_PI), 0f, FloatUtil.sin(FloatUtil.QUARTER_PI) );
+        final Vec3f vecHas = new Vec3f();
         final Quaternion quatS = new Quaternion();
         // System.err.println("Slerp #01: 1/2 * 90 degrees Y");
         quatS.setSlerp(quat1, quat2, 0.5f);
-        quatS.rotateVector(vecHas, 0, UNIT_Z, 0);
+        quatS.rotateVector(UNIT_Z, vecHas);
         // System.err.println("exp0 "+Arrays.toString(vecExp));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecExp, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals( 0f, Math.abs( vecExp.dist(vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        if( !vecExp.equals(vecHas) ) {
+            System.err.println("Deviance: "+vecExp+", "+vecHas+": "+vecExp.minus(vecHas)+", dist "+vecExp.dist(vecHas));
+        }
+        // Assert.assertEquals(vecExp, vecHas);
 
         // delta == 100%
         quat2.setIdentity().rotateByAngleZ(FloatUtil.PI); // angle: 180 degrees, axis Z
         // System.err.println("Slerp #02: 1 * 180 degrees Z");
         quatS.setSlerp(quat1, quat2, 1.0f);
-        quatS.rotateVector(vecHas, 0, UNIT_X, 0);
+        quatS.rotateVector(UNIT_X, vecHas);
         // System.err.println("exp0 "+Arrays.toString(NEG_UNIT_X));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(NEG_UNIT_X, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals( 0f, Math.abs( NEG_UNIT_X.dist(vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals(NEG_UNIT_X, vecHas);
 
         quat2.setIdentity().rotateByAngleZ(FloatUtil.PI); // angle: 180 degrees, axis Z
         // System.err.println("Slerp #03: 1/2 * 180 degrees Z");
         quatS.setSlerp(quat1, quat2, 0.5f);
-        quatS.rotateVector(vecHas, 0, UNIT_X, 0);
+        quatS.rotateVector(UNIT_X, vecHas);
         // System.err.println("exp0 "+Arrays.toString(UNIT_Y));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(UNIT_Y, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals( 0f, Math.abs( UNIT_Y.dist(vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        if( !UNIT_Y.equals(vecHas) ) {
+            System.err.println("Deviance: "+UNIT_Y+", "+vecHas+": "+UNIT_Y.minus(vecHas)+", dist "+UNIT_Y.dist(vecHas));
+        }
+        // Assert.assertEquals(UNIT_Y, vecHas);
 
         // delta == 0%
         quat2.setIdentity().rotateByAngleZ(FloatUtil.PI); // angle: 180 degrees, axis Z
         // System.err.println("Slerp #04: 0 * 180 degrees Z");
         quatS.setSlerp(quat1, quat2, 0.0f);
-        quatS.rotateVector(vecHas, 0, UNIT_X, 0);
+        quatS.rotateVector(UNIT_X, vecHas);
         // System.err.println("exp0 "+Arrays.toString(UNIT_X));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(UNIT_X, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals( 0f, Math.abs( UNIT_X.dist(vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals(UNIT_X, vecHas);
 
         // a==b
         quat2.setIdentity();
         // System.err.println("Slerp #05: 1/4 * 0 degrees");
         quatS.setSlerp(quat1, quat2, 0.25f); // 1/4 of identity .. NOP
-        quatS.rotateVector(vecHas, 0, UNIT_X, 0);
+        quatS.rotateVector(UNIT_X, vecHas);
         // System.err.println("exp0 "+Arrays.toString(UNIT_X));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(UNIT_X, vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals( 0f, Math.abs( UNIT_X.dist(vecHas)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals(UNIT_X, vecHas);
 
         // negative dot product
-        vecExp = new float[] { 0f, -FloatUtil.sin(FloatUtil.QUARTER_PI), FloatUtil.sin(FloatUtil.QUARTER_PI) };
+        vecExp.set(0f, -FloatUtil.sin(FloatUtil.QUARTER_PI), FloatUtil.sin(FloatUtil.QUARTER_PI));
         quat1.setIdentity().rotateByAngleX( -2f * FloatUtil.HALF_PI); // angle: -180 degrees, axis X
         quat2.setIdentity().rotateByAngleX(       FloatUtil.HALF_PI); // angle:   90 degrees, axis X
         // System.err.println("Slerp #06: 1/2 * 270 degrees");
         quatS.setSlerp(quat1, quat2, 0.5f);
-        quatS.rotateVector(vecHas, 0, UNIT_Y, 0);
+        quatS.rotateVector(UNIT_Y, vecHas);
         // System.err.println("exp0 "+Arrays.toString(vecExp));
         // System.err.println("has0 "+Arrays.toString(vecHas));
-        Assert.assertEquals( 0f, Math.abs( VectorUtil.distVec3(vecExp, vecHas)), Quaternion.ALLOWED_DEVIANCE);
-
-
+        Assert.assertEquals( 0f, Math.abs( vecExp.dist(vecHas) ), Quaternion.ALLOWED_DEVIANCE);
+        if( !vecExp.equals(vecHas) ) {
+            System.err.println("Deviance: "+vecExp+", "+vecHas+": "+vecExp.minus(vecHas)+", dist "+vecExp.dist(vecHas));
+        }
+        // Assert.assertEquals(vecExp, vecHas);
     }
 
     @Test
     public void test26LookAt() {
-        final float[] direction = new float[3];
-        final float[] xAxis = new float[3];
-        final float[] yAxis = new float[3];
-        final float[] zAxis = new float[3];
-        final float[] vecHas = new float[3];
+        final Vec3f direction = new Vec3f();
+        final Vec3f xAxis = new Vec3f();
+        final Vec3f yAxis = new Vec3f();
+        final Vec3f zAxis = new Vec3f();
+        final Vec3f vecHas = new Vec3f();
 
         if( DEBUG ) System.err.println("LookAt #01");
-        VectorUtil.copyVec3(direction, 0, NEG_UNIT_X, 0);
+        direction.set(NEG_UNIT_X);
         final Quaternion quat = new Quaternion().setLookAt(direction, UNIT_Y, xAxis, yAxis, zAxis);
-        Assert.assertEquals(0f, VectorUtil.distVec3(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals(0f, direction.dist( quat.rotateVector(UNIT_Z, vecHas) ), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals(direction, vecHas);
 
         if( DEBUG ) System.err.println("LookAt #02");
-        VectorUtil.normalizeVec3(VectorUtil.copyVec3(direction, 0, ONE, 0));
+        direction.set(ONE).normalize();
         quat.setLookAt(direction, UNIT_Y, xAxis, yAxis, zAxis);
         if( DEBUG )System.err.println("quat0 "+quat);
-        quat.rotateVector(vecHas, 0, UNIT_Z, 0);
+        quat.rotateVector(UNIT_Z, vecHas);
         if( DEBUG ) {
-            System.err.println("xAxis "+Arrays.toString(xAxis)+", len "+VectorUtil.normVec3(xAxis));
-            System.err.println("yAxis "+Arrays.toString(yAxis)+", len "+VectorUtil.normVec3(yAxis));
-            System.err.println("zAxis "+Arrays.toString(zAxis)+", len "+VectorUtil.normVec3(zAxis));
-            System.err.println("exp0 "+Arrays.toString(direction)+", len "+VectorUtil.normVec3(direction));
-            System.err.println("has0 "+Arrays.toString(vecHas)+", len "+VectorUtil.normVec3(vecHas));
+            System.err.println("xAxis "+xAxis+", len "+xAxis.length());
+            System.err.println("yAxis "+yAxis+", len "+yAxis.length());
+            System.err.println("zAxis "+zAxis+", len "+zAxis.length());
+            System.err.println("exp0 "+direction+", len "+direction.length());
+            System.err.println("has0 "+vecHas+", len "+vecHas.length());
         }
         // Assert.assertEquals(0f, VectorUtil.distance(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);
-        Assert.assertEquals(0f, VectorUtil.distVec3(direction, vecHas), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals(0f, direction.dist(vecHas), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals(direction, vecHas);
 
         if( DEBUG )System.err.println("LookAt #03");
-        VectorUtil.normalizeVec3(VectorUtil.copyVec3(direction, 0, new float[] { -1f, 2f, -1f }, 0));
+        direction.set(-1f, 2f, -1f).normalize();
         quat.setLookAt(direction, UNIT_Y, xAxis, yAxis, zAxis);
         if( DEBUG )System.err.println("quat0 "+quat);
-        quat.rotateVector(vecHas, 0, UNIT_Z, 0);
+        quat.rotateVector(UNIT_Z, vecHas);
         if( DEBUG ) {
-            System.err.println("xAxis "+Arrays.toString(xAxis)+", len "+VectorUtil.normVec3(xAxis));
-            System.err.println("yAxis "+Arrays.toString(yAxis)+", len "+VectorUtil.normVec3(yAxis));
-            System.err.println("zAxis "+Arrays.toString(zAxis)+", len "+VectorUtil.normVec3(zAxis));
-            System.err.println("exp0 "+Arrays.toString(direction)+", len "+VectorUtil.normVec3(direction));
-            System.err.println("has0 "+Arrays.toString(vecHas)+", len "+VectorUtil.normVec3(vecHas));
+            System.err.println("xAxis "+xAxis+", len "+xAxis.length());
+            System.err.println("yAxis "+yAxis+", len "+yAxis.length());
+            System.err.println("zAxis "+zAxis+", len "+zAxis.length());
+            System.err.println("exp0 "+direction+", len "+direction.length());
+            System.err.println("has0 "+vecHas+", len "+vecHas.length());
         }
         // Assert.assertEquals(0f, VectorUtil.distance(direction, quat.rotateVector(vecHas, 0, UNIT_Z, 0)), Quaternion.ALLOWED_DEVIANCE);
-        Assert.assertEquals(0f, VectorUtil.distVec3(direction, vecHas), Quaternion.ALLOWED_DEVIANCE);
+        Assert.assertEquals(0f, direction.dist(vecHas), Quaternion.ALLOWED_DEVIANCE);
+        if( !direction.equals(vecHas) ) {
+            System.err.println("Deviance: "+direction+", "+vecHas+": "+direction.minus(vecHas)+", dist "+direction.dist(vecHas));
+        }
+        // Assert.assertEquals(direction, vecHas);
     }
 
     public static void main(final String args[]) {
-- 
cgit v1.2.3