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 .. --- .../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 +++++--- 13 files changed, 3218 insertions(+), 408 deletions(-) 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 (limited to 'src/test/com') 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: + *

+ *

+ * @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: + *

+ *

+ * + * @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: + *

+ * @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: + *

+ * @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: + *

+ *

+ * @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