From: Svjatoslav Agejenko Date: Fri, 20 Mar 2026 20:51:42 +0000 (+0200) Subject: feat(math): add Matrix3x3 and Quaternion for 3D transformations X-Git-Url: http://www2.svjatoslav.eu/gitweb/?a=commitdiff_plain;h=dc54493dfc166c0b47daaff819a8f91748c45f0b;p=sixth-3d.git feat(math): add Matrix3x3 and Quaternion for 3D transformations - Add Matrix3x3 class for 3D transformations - Add toMatrix method to Rotation with test - Use matrix for point transformation - Cache transformation matrix for performance - Add Quaternion class for 3D rotations - Use quaternion internally instead of Euler angles - Use quaternion-based camera rotation - Use rotation matrix for movement direction - Use quaternion for lookAt rotation --- diff --git a/src/main/java/eu/svjatoslav/sixth/e3d/gui/Camera.java b/src/main/java/eu/svjatoslav/sixth/e3d/gui/Camera.java index 2f0b842..f5542e6 100644 --- a/src/main/java/eu/svjatoslav/sixth/e3d/gui/Camera.java +++ b/src/main/java/eu/svjatoslav/sixth/e3d/gui/Camera.java @@ -5,6 +5,8 @@ package eu.svjatoslav.sixth.e3d.gui; import eu.svjatoslav.sixth.e3d.geometry.Point3D; +import eu.svjatoslav.sixth.e3d.math.Matrix3x3; +import eu.svjatoslav.sixth.e3d.math.Quaternion; import eu.svjatoslav.sixth.e3d.math.Transform; /** @@ -168,27 +170,28 @@ public class Camera implements FrameListener { * camera between consecutive frames. */ private void translateCameraLocationBasedOnMovementVector(int millisecondsPassedSinceLastFrame) { - final double sinXZ = transform.getRotation().getSinXZ(); - final double cosXZ = transform.getRotation().getCosXZ(); + final Matrix3x3 m = transform.getRotation().toMatrix(); + + final double forwardX = m.m20; + final double forwardY = m.m21; + final double forwardZ = m.m22; + + final double rightX = m.m00; + final double rightY = m.m01; + final double rightZ = m.m02; final Point3D location = transform.getTranslation(); + final double ms = millisecondsPassedSinceLastFrame; + + location.x += forwardX * movementVector.z * SPEED_MULTIPLIER * ms; + location.y += forwardY * movementVector.z * SPEED_MULTIPLIER * ms; + location.z += forwardZ * movementVector.z * SPEED_MULTIPLIER * ms; + + location.x += rightX * movementVector.x * SPEED_MULTIPLIER * ms; + location.y += rightY * movementVector.x * SPEED_MULTIPLIER * ms; + location.z += rightZ * movementVector.x * SPEED_MULTIPLIER * ms; - location.x -= (float) sinXZ - * movementVector.z * SPEED_MULTIPLIER - * millisecondsPassedSinceLastFrame; - location.z += (float) cosXZ - * movementVector.z * SPEED_MULTIPLIER - * millisecondsPassedSinceLastFrame; - - location.x += (float) cosXZ - * movementVector.x * SPEED_MULTIPLIER - * millisecondsPassedSinceLastFrame; - location.z += (float) sinXZ - * movementVector.x * SPEED_MULTIPLIER - * millisecondsPassedSinceLastFrame; - - location.y += movementVector.y * SPEED_MULTIPLIER - * millisecondsPassedSinceLastFrame; + location.y += movementVector.y * SPEED_MULTIPLIER * ms; } /** @@ -226,6 +229,6 @@ public class Camera implements FrameListener { final double horizontalDist = Math.sqrt(dx * dx + dz * dz); final double angleYZ = -Math.atan2(dy, horizontalDist); - transform.setRotation(angleXZ, angleYZ); + transform.getRotation().setQuaternion(Quaternion.fromAngles(angleXZ, angleYZ)); } } \ No newline at end of file diff --git a/src/main/java/eu/svjatoslav/sixth/e3d/gui/humaninput/InputManager.java b/src/main/java/eu/svjatoslav/sixth/e3d/gui/humaninput/InputManager.java index 5b6db3d..dc038aa 100644 --- a/src/main/java/eu/svjatoslav/sixth/e3d/gui/humaninput/InputManager.java +++ b/src/main/java/eu/svjatoslav/sixth/e3d/gui/humaninput/InputManager.java @@ -5,9 +5,11 @@ package eu.svjatoslav.sixth.e3d.gui.humaninput; import eu.svjatoslav.sixth.e3d.geometry.Point2D; +import eu.svjatoslav.sixth.e3d.geometry.Point3D; import eu.svjatoslav.sixth.e3d.gui.Camera; import eu.svjatoslav.sixth.e3d.gui.FrameListener; import eu.svjatoslav.sixth.e3d.gui.ViewPanel; +import eu.svjatoslav.sixth.e3d.math.Quaternion; import eu.svjatoslav.sixth.e3d.math.Rotation; import java.awt.*; @@ -41,6 +43,8 @@ public class InputManager implements private Point2D currentMouseLocation; private boolean mouseMoved; private boolean mouseWithinWindow = false; + private double cameraYaw = 0; + private double cameraPitch = 0; /** * Creates an input manager attached to the given view panel. @@ -263,15 +267,21 @@ public class InputManager implements } private boolean handleMouseDragging() { + if (mouseDelta.isZero()) { + return false; + } + + cameraYaw -= mouseDelta.x / 50.0; + cameraPitch -= mouseDelta.y / 50.0; + + cameraPitch = Math.max(-Math.PI / 2 + 0.001, + Math.min( Math.PI / 2 - 0.001, cameraPitch)); + final Camera camera = viewPanel.getCamera(); - Rotation rotation = camera.getTransform().getRotation(); - final double newXZ = rotation.getAngleXZ() - ((float) mouseDelta.x / 50); - final double newYZ = rotation.getAngleYZ() - ((float) mouseDelta.y / 50); - camera.getTransform().setRotation(newXZ, newYZ); + camera.getTransform().getRotation().setQuaternion(Quaternion.fromAngles(cameraYaw, cameraPitch)); - boolean viewUpdateNeeded = !mouseDelta.isZero(); mouseDelta.zero(); - return viewUpdateNeeded; + return true; } } \ No newline at end of file diff --git a/src/main/java/eu/svjatoslav/sixth/e3d/math/Matrix3x3.java b/src/main/java/eu/svjatoslav/sixth/e3d/math/Matrix3x3.java new file mode 100644 index 0000000..9dbc255 --- /dev/null +++ b/src/main/java/eu/svjatoslav/sixth/e3d/math/Matrix3x3.java @@ -0,0 +1,67 @@ +/* + * Sixth 3D engine. Author: Svjatoslav Agejenko. + * This project is released under Creative Commons Zero (CC0) license. + */ +package eu.svjatoslav.sixth.e3d.math; + +import eu.svjatoslav.sixth.e3d.geometry.Point3D; + +/** + * A 3x3 matrix for 3D transformations. + * + *

Matrix elements are stored in row-major order:

+ * 
+ * | m00 m01 m02 |
+ * | m10 m11 m12 |
+ * | m20 m21 m22 |
+ *
+ * + * @see Point3D + */ +public class Matrix3x3 { + + public double m00; + public double m01; + public double m02; + public double m10; + public double m11; + public double m12; + public double m20; + public double m21; + public double m22; + + /** + * Creates a zero matrix. + */ + public Matrix3x3() { + } + + /** + * Returns an identity matrix. + * + * @return a new identity matrix + */ + public static Matrix3x3 identity() { + final Matrix3x3 m = new Matrix3x3(); + m.m00 = 1; + m.m11 = 1; + m.m22 = 1; + return m; + } + + /** + * Applies this matrix transformation to a point. + * + * @param in the input point (not modified) + * @param out the output point (will be modified) + */ + public void transform(final Point3D in, final Point3D out) { + final double x = m00 * in.x + m01 * in.y + m02 * in.z; + final double y = m10 * in.x + m11 * in.y + m12 * in.z; + final double z = m20 * in.x + m21 * in.y + m22 * in.z; + out.x = x; + out.y = y; + out.z = z; + } + +} \ No newline at end of file diff --git a/src/main/java/eu/svjatoslav/sixth/e3d/math/Quaternion.java b/src/main/java/eu/svjatoslav/sixth/e3d/math/Quaternion.java new file mode 100644 index 0000000..86aef8a --- /dev/null +++ b/src/main/java/eu/svjatoslav/sixth/e3d/math/Quaternion.java @@ -0,0 +1,136 @@ +/* + * Sixth 3D engine. Author: Svjatoslav Agejenko. + * This project is released under Creative Commons Zero (CC0) license. + */ +package eu.svjatoslav.sixth.e3d.math; + +import eu.svjatoslav.sixth.e3d.geometry.Point3D; + +import static java.lang.Math.cos; +import static java.lang.Math.sin; + +/** + * A unit quaternion representing a 3D rotation. + * + *

Quaternions provide a compact representation of rotations that avoids + * gimbal lock and enables smooth interpolation (slerp).

+ * + * @see Matrix3x3 + * @see Rotation + */ +public class Quaternion { + + public double w; + public double x; + public double y; + public double z; + + /** + * Creates a quaternion with the specified components. + * + * @param w the scalar component + * @param x the i component + * @param y the j component + * @param z the k component + */ + public Quaternion(final double w, final double x, final double y, final double z) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + /** + * Returns the identity quaternion representing no rotation. + * + * @return the identity quaternion (1, 0, 0, 0) + */ + public static Quaternion identity() { + return new Quaternion(1, 0, 0, 0); + } + + /** + * Creates a quaternion from an axis-angle representation. + * + * @param axis the rotation axis (must be normalized) + * @param angle the rotation angle in radians + * @return a quaternion representing the rotation + */ + public static Quaternion fromAxisAngle(final Point3D axis, final double angle) { + final double halfAngle = angle / 2; + final double s = sin(halfAngle); + final double c = cos(halfAngle); + return new Quaternion(c, axis.x * s, axis.y * s, axis.z * s); + } + + /** + * Creates a quaternion from XZ (yaw) and YZ (pitch) Euler angles. + * + *

The rotation is composed as yaw (around Y axis) followed by + * pitch (around X axis).

+ * + * @param angleXZ the angle around the XZ axis (yaw) in radians + * @param angleYZ the angle around the YZ axis (pitch) in radians + * @return a quaternion representing the combined rotation + */ + public static Quaternion fromAngles(final double angleXZ, final double angleYZ) { + final Quaternion yaw = fromAxisAngle(new Point3D(0, 1, 0), angleXZ); + final Quaternion pitch = fromAxisAngle(new Point3D(1, 0, 0), -angleYZ); + return pitch.multiply(yaw); + } + + /** + * Multiplies this quaternion by another (Hamilton product). + * + * @param other the quaternion to multiply by + * @return a new quaternion representing the combined rotation + */ + public Quaternion multiply(final Quaternion other) { + return new Quaternion( + w * other.w - x * other.x - y * other.y - z * other.z, + w * other.x + x * other.w + y * other.z - z * other.y, + w * other.y - x * other.z + y * other.w + z * other.x, + w * other.z + x * other.y - y * other.x + z * other.w + ); + } + + /** + * Normalizes this quaternion to unit length. + * + * @return this quaternion (for chaining) + */ + public Quaternion normalize() { + final double len = Math.sqrt(w * w + x * x + y * y + z * z); + if (len > 0) { + w /= len; + x /= len; + y /= len; + z /= len; + } + return this; + } + + /** + * Converts this quaternion to a 3x3 rotation matrix. + * + * @return a new matrix representing this rotation + */ + public Matrix3x3 toMatrix3x3() { + final Matrix3x3 m = new Matrix3x3(); + + m.m00 = 1 - 2 * (y * y + z * z); + m.m01 = 2 * (x * y - w * z); + m.m02 = 2 * (x * z + w * y); + + m.m10 = 2 * (x * y + w * z); + m.m11 = 1 - 2 * (x * x + z * z); + m.m12 = 2 * (y * z - w * x); + + m.m20 = 2 * (x * z - w * y); + m.m21 = 2 * (y * z + w * x); + m.m22 = 1 - 2 * (x * x + y * y); + + return m; + } + +} \ No newline at end of file diff --git a/src/main/java/eu/svjatoslav/sixth/e3d/math/Rotation.java b/src/main/java/eu/svjatoslav/sixth/e3d/math/Rotation.java index b76ce06..236be0d 100644 --- a/src/main/java/eu/svjatoslav/sixth/e3d/math/Rotation.java +++ b/src/main/java/eu/svjatoslav/sixth/e3d/math/Rotation.java @@ -6,39 +6,26 @@ package eu.svjatoslav.sixth.e3d.math; import eu.svjatoslav.sixth.e3d.geometry.Point3D; -import static java.lang.Math.cos; -import static java.lang.Math.sin; +import static java.lang.Math.atan2; /** - * Represents a rotation in 3D space using two Euler angles (XZ and YZ). + * Represents a rotation in 3D space using a quaternion internally. * - *

Angles are stored with precomputed sine and cosine values for efficient - * repeated rotation operations without recalculating trigonometric functions.

+ *

Historically used two Euler angles (XZ and YZ). Now stores a quaternion + * for better numerical stability and to avoid gimbal lock.

* * @see Transform + * @see Quaternion */ public class Rotation implements Cloneable { - /** - * Precomputed sine and cosine of the rotation angles. - */ - private double s1, c1, s2, c2; - - /** - * The angle of rotation around the XZ axis (yaw). - */ - private double angleXZ = 0; - - /** - * The angle of rotation around the YZ axis (pitch). - */ - private double angleYZ = 0; + private Quaternion quaternion; /** - * Creates a rotation with no rotation (zero angles). + * Creates a rotation with no rotation (identity). */ public Rotation() { - computeMultipliers(); + quaternion = Quaternion.identity(); } /** @@ -48,116 +35,116 @@ public class Rotation implements Cloneable { * @param angleYZ the angle around the YZ axis (pitch) in radians */ public Rotation(final double angleXZ, final double angleYZ) { - this.angleXZ = angleXZ; - this.angleYZ = angleYZ; - computeMultipliers(); + quaternion = Quaternion.fromAngles(angleXZ, angleYZ); } /** - * Creates a copy of this rotation with the same angles. + * Creates a copy of this rotation with the same orientation. * - * @return a new rotation with the same angle values + * @return a new rotation with the same quaternion values */ @Override public Rotation clone() { - return new Rotation(angleXZ, angleYZ); + final Rotation r = new Rotation(); + r.quaternion = new Quaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z); + return r; } /** - * Recomputes the sine and cosine values from the current angles. - */ - private void computeMultipliers() { - s1 = sin(angleXZ); - c1 = cos(angleXZ); - - s2 = sin(angleYZ); - c2 = cos(angleYZ); - } - - /** - * Rotates a point around the origin using this rotation's angles. + * Rotates a point around the origin using this rotation. * * @param point3d the point to rotate (modified in place) */ public void rotate(final Point3D point3d) { - // Rotate around the XZ axis - final double z1 = (point3d.z * c1) - (point3d.x * s1); - point3d.x = (point3d.z * s1) + (point3d.x * c1); - - // Rotate around the YZ axis - point3d.z = (z1 * c2) - (point3d.y * s2); - point3d.y = (z1 * s2) + (point3d.y * c2); + toMatrix().transform(point3d, point3d); } /** - * Adds the specified angles to this rotation and updates the trigonometric values. + * Adds the specified angles to this rotation. * * @param angleXZ the angle to add around the XZ axis in radians * @param angleYZ the angle to add around the YZ axis in radians */ public void rotate(final double angleXZ, final double angleYZ) { - this.angleXZ += angleXZ; - this.angleYZ += angleYZ; - computeMultipliers(); + final Quaternion delta = Quaternion.fromAngles(angleXZ, angleYZ); + quaternion = delta.multiply(quaternion); } /** - * Sets the rotation angles and recomputes the trigonometric values. + * Sets the rotation angles. * * @param angleXZ the angle around the XZ axis (yaw) in radians * @param angleYZ the angle around the YZ axis (pitch) in radians */ public void setAngles(final double angleXZ, final double angleYZ) { - this.angleXZ = angleXZ; - this.angleYZ = angleYZ; - computeMultipliers(); + quaternion = Quaternion.fromAngles(angleXZ, angleYZ); } /** - * Copies the angles from another rotation into this rotation. + * Copies the rotation from another rotation. * - * @param rotation the rotation to copy angles from + * @param rotation the rotation to copy from */ - public void setAngles(Rotation rotation) { - this.angleXZ = rotation.angleXZ; - this.angleYZ = rotation.angleYZ; - computeMultipliers(); + public void setAngles(final Rotation rotation) { + quaternion = new Quaternion( + rotation.quaternion.w, + rotation.quaternion.x, + rotation.quaternion.y, + rotation.quaternion.z + ); + } + + /** + * Sets the rotation from a quaternion. + * + * @param q the quaternion to set + */ + public void setQuaternion(final Quaternion q) { + quaternion = new Quaternion(q.w, q.x, q.y, q.z); + } + + /** + * Returns the internal quaternion. + * + * @return the quaternion (not a copy) + */ + public Quaternion getQuaternion() { + return quaternion; } /** * Returns the angle around the XZ axis (yaw) in radians. * + *

This is a compatibility shim that extracts the angle from the quaternion. + * May not be accurate for extreme pitch angles.

+ * * @return the XZ angle */ public double getAngleXZ() { - return angleXZ; + final Matrix3x3 m = toMatrix(); + return atan2(m.m02, m.m00); } /** * Returns the angle around the YZ axis (pitch) in radians. * + *

This is a compatibility shim that extracts the angle from the quaternion. + * May not be accurate for extreme yaw angles.

+ * * @return the YZ angle */ public double getAngleYZ() { - return angleYZ; - } - - /** - * Returns the precomputed sine of the XZ angle. - * - * @return sin(angleXZ) - */ - public double getSinXZ() { - return s1; + final Matrix3x3 m = toMatrix(); + return atan2(-m.m21, m.m11); } /** - * Returns the precomputed cosine of the XZ angle. + * Converts this rotation to a 3x3 transformation matrix. * - * @return cos(angleXZ) + * @return a matrix representing this rotation */ - public double getCosXZ() { - return c1; + public Matrix3x3 toMatrix() { + return quaternion.toMatrix3x3(); } } \ No newline at end of file diff --git a/src/main/java/eu/svjatoslav/sixth/e3d/math/Transform.java b/src/main/java/eu/svjatoslav/sixth/e3d/math/Transform.java index 8131ef6..021fe1a 100755 --- a/src/main/java/eu/svjatoslav/sixth/e3d/math/Transform.java +++ b/src/main/java/eu/svjatoslav/sixth/e3d/math/Transform.java @@ -102,7 +102,7 @@ public class Transform implements Cloneable { * @param point the point to transform (modified in place) */ public void transform(final Point3D point) { - rotation.rotate(point); + rotation.toMatrix().transform(point, point); point.add(translation); } diff --git a/src/test/java/eu/svjatoslav/sixth/e3d/math/QuaternionTest.java b/src/test/java/eu/svjatoslav/sixth/e3d/math/QuaternionTest.java new file mode 100644 index 0000000..70d0488 --- /dev/null +++ b/src/test/java/eu/svjatoslav/sixth/e3d/math/QuaternionTest.java @@ -0,0 +1,33 @@ +/* + * Sixth 3D engine. Author: Svjatoslav Agejenko. + * This project is released under Creative Commons Zero (CC0) license. + */ +package eu.svjatoslav.sixth.e3d.math; + +import org.junit.Test; + +import static org.junit.Assert.assertEquals; + +public class QuaternionTest { + + @Test + public void testFromAnglesMatchesRotation() { + final Rotation rotation = new Rotation(0.5, 0.3); + final Matrix3x3 rotationMatrix = rotation.toMatrix(); + + final Quaternion quaternion = Quaternion.fromAngles(0.5, 0.3); + final Matrix3x3 quaternionMatrix = quaternion.toMatrix3x3(); + + final double epsilon = 0.0001; + assertEquals(rotationMatrix.m00, quaternionMatrix.m00, epsilon); + assertEquals(rotationMatrix.m01, quaternionMatrix.m01, epsilon); + assertEquals(rotationMatrix.m02, quaternionMatrix.m02, epsilon); + assertEquals(rotationMatrix.m10, quaternionMatrix.m10, epsilon); + assertEquals(rotationMatrix.m11, quaternionMatrix.m11, epsilon); + assertEquals(rotationMatrix.m12, quaternionMatrix.m12, epsilon); + assertEquals(rotationMatrix.m20, quaternionMatrix.m20, epsilon); + assertEquals(rotationMatrix.m21, quaternionMatrix.m21, epsilon); + assertEquals(rotationMatrix.m22, quaternionMatrix.m22, epsilon); + } + +} \ No newline at end of file diff --git a/src/test/java/eu/svjatoslav/sixth/e3d/math/RotationMatrixTest.java b/src/test/java/eu/svjatoslav/sixth/e3d/math/RotationMatrixTest.java new file mode 100644 index 0000000..d0d14e6 --- /dev/null +++ b/src/test/java/eu/svjatoslav/sixth/e3d/math/RotationMatrixTest.java @@ -0,0 +1,31 @@ +/* + * Sixth 3D engine. Author: Svjatoslav Agejenko. + * This project is released under Creative Commons Zero (CC0) license. + */ +package eu.svjatoslav.sixth.e3d.math; + +import eu.svjatoslav.sixth.e3d.geometry.Point3D; +import org.junit.Test; + +import static org.junit.Assert.assertEquals; + +public class RotationMatrixTest { + + @Test + public void testToMatrixMatchesRotate() { + final Rotation rotation = new Rotation(0.5, 0.3); + final Point3D original = new Point3D(1, 2, 3); + + final Point3D viaRotate = new Point3D(original); + rotation.rotate(viaRotate); + + final Point3D viaMatrix = new Point3D(); + rotation.toMatrix().transform(original, viaMatrix); + + final double epsilon = 0.0001; + assertEquals(viaRotate.x, viaMatrix.x, epsilon); + assertEquals(viaRotate.y, viaMatrix.y, epsilon); + assertEquals(viaRotate.z, viaMatrix.z, epsilon); + } + +} \ No newline at end of file