Math problem: rotating a point around an axis

Feb 13, 2018
3
0
10
i have a problem with a formula.

Java:
/*
 */

package be.manudahmen.growth.graphics;

import be.manudahmen.empty3.Point3D;

import javax.vecmath.Matrix4d;
import javax.vecmath.Vector3d;

public class Rotation {
    private Matrix4d matriceRotation;
    private Point3D origin;

    public Rotation(Point3D vecteur, Point3D origin, double angle) {
        this.origin = origin;
        double[] pointAngle = new double[]{0, 0, 0, 0};
        Point3D v2 = vecteur.moins(origin);
        rq(pointAngle, angle, v2.getX(), v2.getY(), v2.getZ());
        double[] mat44 = new double[16];
        qm(mat44, pointAngle);

        matriceRotation = new Matrix4d(mat44);


    }


    public static Point3D rotate(Point3D originVector, Point3D vector, double angle, Point3D pointToRotate) {
        Rotation rot = new Rotation(vector, originVector, angle);
        Point3D rotated = rot.rotate(pointToRotate);
        System.out.println("Axe              : " + originVector + " -> " + vector);
        System.out.println("Point de départ  : " + pointToRotate);
        System.out.println("Point transformé : " + rotated);
        return rotated;
    }
    public Point3D rotate(Point3D p) {
        p = p.moins(origin);
        Vector3d p2 = new Vector3d(p.getX(), p.getY(), p.getZ());
        matriceRotation.transform(p2);
        return new Point3D(p2.x, p2.y, p2.z).plus(origin);
    }

    private void rq(double[] _q, double a, double x, double y, double z) {
        double sin_a = Math.sin(a / 2);
        double cos_a = Math.cos(a / 2);

        _q[0] = x * sin_a;
        _q[1] = y * sin_a;
        _q[2] = z * sin_a;
        _q[3] = cos_a;

        nq(_q);
    }


    private void nq(double[] q_) {
        double l = Math.sqrt(q_[3] * q_[3] + q_[0] * q_[0] + q_[1] * q_[1] + q_[2] * q_[2]);
        l = 1 / l;
        q_[3] = q_[3] * l;
        q_[0] = q_[0] * l;
        q_[1] = q_[1] * l;
        q_[2] = q_[2] * l;
    }

    private void qm(double[] mat, double[] q) {
        double xx = q[0] * q[0];
        double xy = q[0] * q[1];
        double xz = q[0] * q[2];
        double xw = q[0] * q[3];

        double yy = q[1] * q[1];
        double yz = q[1] * q[2];
        double yw = q[1] * q[3];

        double zz = q[2] * q[2];
        double zw = q[2] * q[3];

        mat[0] = 1 - 2 * (yy + zz);
        mat[1] = 2 * (xy - zw);
        mat[2] = 2 * (xz + yw);

        mat[4] = 2 * (xy + zw);
        mat[5] = 1 - 2 * (xx + zz);
        mat[6] = 2 * (yz - xw);

        mat[8] = 2 * (xz - yw);
        mat[9] = 2 * (yz + xw);
        mat[10] = 1 - 2 * (xx + yy);

        mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0;
        mat[15] = 1;
    }

}
 
Feb 13, 2018
3
0
10
Here is a test which fails:
Java:
    public void testRotation30deg() {
        Point3D x = new Point3D(3, 5, 5);
        Point3D y = x;

        for (int i = 0; i < 12; i++) {
            x = rotate(Point3D.X, new Point3D(10, 0, 0),
                    Math.PI / 6, x);
        }


        assertEqualsPoint3D(x, y);

    }

Here are some tests which succeed:

Java:
    public void testRotationIdent1() {
        Point3D x = rotate(Point3D.O0, Point3D.X,
                2 * Math.PI, Point3D.Y);
        Point3D y = Point3D.Y;

        assertEqualsPoint3D(x, y);

    }

    public void testRotationIdent2() {
        Point3D x = rotate(Point3D.O0, Point3D.X,
                2 * Math.PI, Point3D.X);
        Point3D y = Point3D.X;

        assertEqualsPoint3D(x, y);

    }

    public void testRotationIdent3() {
        Point3D x = rotate(Point3D.O0, Point3D.X,
                2 * Math.PI, Point3D.Z);
        Point3D y = Point3D.Z;

        assertEqualsPoint3D(x, y);

    }

    public void testRotation90() {
        Point3D x = rotate(Point3D.O0, Point3D.X,
                Math.PI, Point3D.Z);
        Point3D y = Point3D.Z.mult(-1);

        assertEqualsPoint3D(x, y);


    }

    public void testRotationNonO() {
        Point3D x = rotate(Point3D.X, new Point3D(10, 0, 0),
                Math.PI, new Point3D(3, 5, 5));
        Point3D y = new Point3D(3, -5, -5);

        assertEqualsPoint3D(x, y);

    }

    public void testRotation180() {
        Point3D x = rotate(new Point3D(11, 0, 0), new Point3D(10, 0, 0),
                Math.PI, new Point3D(3, 5, 0));
        Point3D y = new Point3D(3, -5, 0);

        assertEqualsPoint3D(x, y);

    }

Java:
    public void assertEqualsPoint3D(Point3D x, Point3D y) {
        for (int i = 0; i < 3; i++) {
            assertEquals(y.get(i), x.get(i), 0.00000000001);
        }
    }

I don't understand why. It seems that rotations like 90°, 180°, 360° work well but if I try 12x 30° (360°) it doesn't work. But why?

Formula seems ok, but not totally true.

https://gitlab.com/Graphics3D/Plants