1
$\begingroup$

Friends and I are building a "simple" physics engine, but we are running into trouble. All of our objects' angles are defined by a vector which represents an Euler intrinsic rotation of type z-y'-x''.

Problem: we have a system of points which represent an object, each point have a mass and a relative position to the center of mass (barycenter) of the object. Because we have all informations about forces on each point, we can easily calculate the moment (or torque, or couple), we obtain a vector which represents the axis of rotation and the torque value (given by the length of the vector).

After a lot of try and retry, looking at forums and applying different transformations (see here), we can't make this work.

Because the torque is defined by a vector which is the axis of "rotation force" of the system and everything else in our work is defined by Euler angles, we have to use some tricks. There is an example :

(0 multiply rotation speed by the delta time) - 1/ convert the rotation speed (quaternion) to a rotation matrix 3x3 - 2/ convert the current object rotation (Euler intrinsic angles) to rotation matrix - 3/ multiply the two matrix //This is the only way to "add" a rotation - 4/ convert this new matrix to an Euler intrinsic rotation vector //Then we have our new position

Question: Do you agree with this procedure ? And do you know if there is a faster way to do what we want to do ?

To be more precise, the precedent procedure may be correct, but it is a big source of error, and we have currently somme errors that we can't identify. The system for instance is rotating with in the good way but after 45° of rotation it bounce / jump back and "fall" again.

(In the gif bellow, the instantaneous change of orientation is due to the gif loop, it's not a bug, the bug is only the bounce effect) Bounce effect GIF White vectors represent forces (inverted), cubes are the "points" and blues links are the links between points.

I think the error is here, we use this documentation4 to get the intrinsic rotation from the rotation matrix, we have theses equations : $$ w = tan^{-1}(R_{21} / R_{11}) = atan2(R_{21},R_{11})\\ v = -sin^{-1}(R_{31})\\ u = tan^{-1}(R_{32} / R_{33}) = atan2(R_{32},R_{33}) $$

But when we tests the invert conversion of the conversion ("matrix to intrinsic" of the "intrinsic to matrix" operation) given a arbitrary vector (a,b,c) ; we obtain : $$ (a \equiv \pi,b \equiv \pi,c \equiv \pi)\\ or \\(a \equiv \pi, - b \equiv \pi,c \equiv \pi) $$ And I think this "or" is our current problem, but I am not sure.

UPDATE : Finally it seems to be a limitation to the conversions, I working on it tomorrow. It's because I use the quaternion length to save the angle of rotation or the angular velocity. So when I compute angular acceleration to the angular velocity to have a new angular velocity, it's via rotations matrix that limit angle of rotation to PI radians, that is ok for orienting objects, but not with speed, because if PI and 0 are the same as angle, their are not exactly the same as angular velocity. For now I found the solution to divide the size of the vectors by a number in order to have every time something between 0 and PI, then after operation I multiply the result vector by this same number, not very beautiful...

Thank you a lot, I'm going to have a headache soon... Sorry for my english, don't hesitate to ask if you don't understand something !

Context: We use openGL for visualization, and Java as language. You can find the code here. The main test is in main.TestsCentripete for those who want to test the code !

Below is my current code, I figured out that the wikipedia article had a mistake, so I found another solution: In the part Rotation matrix ↔ quaternion, qr was given by

qr = arccos( 0.5 * (A11+A22+A33-1));

According to this document: I can have the rotation matrix from quaternion and the invert function (quaternion from matrix). I also have the conversion from Euler angles to rotation matrix and the invert function.

/**
     * Convert a quaternion axis to matrix of rotation (VERIFIED - OK)
     * @param quaternion
     * @return
     */
    public static Matrix quaternionToMatrix(Vect3D quaternion){

        double theta = quaternion.size();

        double uUn = quaternion.x;
        double vUn = quaternion.y;
        double wUn = quaternion.z;

        //Normalisation
        double u = uUn/theta;
        double v = vUn/theta;
        double w = wUn/theta;

        double u2 = u*u;
        double v2 = v*v;
        double w2 = w*w;
        double cosT = Math.cos(theta);
        double oneMinusCosT = 1-cosT;
        double sinT = Math.sin(theta);

        Matrix res = new Matrix();

        if(Math.abs(theta)<=1E-9){
            res.setCoef(1, 1, 1);
            res.setCoef(2, 2, 1);
            res.setCoef(3, 3, 1);
            return res;
        }

        // Build the matrix entries element by element.
        res.setCoef(1, 1, u2 + (v2 + w2) * cosT);
        res.setCoef(1, 2, u*v*oneMinusCosT - w*sinT);
        res.setCoef(1, 3, u*w*oneMinusCosT + v*sinT);

        res.setCoef(2, 1, u*v * oneMinusCosT + w*sinT);
        res.setCoef(2, 2, v2 + (u2 + w2) * cosT);
        res.setCoef(2, 3, v*w * oneMinusCosT - u*sinT);

        res.setCoef(3, 1, u*w * oneMinusCosT - v*sinT);
        res.setCoef(3, 2, v*w * oneMinusCosT + u*sinT);
        res.setCoef(3, 3, w2 + (u2 + v2) * cosT);


        return res;
    }

    /**
     * Convert an intrinsec axis to matrix of rotation (VERIFIED - OK)
     * @param quaternion
     * @return
     */
    public static Matrix intrinsecToMatrix(Vect3D intrinsec){
        return rotMatrixModule_Space(intrinsec);
    }

    /**
     * Convert a rotation matrix to quaternion (VERIFIED - OK) 
     * @param quaternion
     * @return
     */
    public static Vect3D matrixToQuaternion(Matrix matrix){

        double qr = Math.acos(0.5 * (matrix.getCoef(1, 1)+matrix.getCoef(2, 2)+matrix.getCoef(3, 3)-1));

        double qi = (matrix.getCoef(3, 2)-matrix.getCoef(2, 3))/(4*qr);
        double qj = (matrix.getCoef(1, 3)-matrix.getCoef(3, 1))/(4*qr);
        double qk = (matrix.getCoef(2, 1)-matrix.getCoef(1, 2))/(4*qr);

        double l = Math.sqrt(qi*qi + qj*qj + qk*qk);

        if(l<=1E-9 || qr<=1E-9){
            return new Vect3D();
        }

        double x = qi * qr / l;
        double y = qj * qr / l;
        double z = qk * qr / l;

        return new Vect3D(x,y,z);
    }

    /**
     * Convert a rotation matrix to intrinsec angles
     * @param quaternion
     * @return
     */
    public static Vect3D matrixToIntrinsec(Matrix matrix){

        double z = Math.atan2(
                matrix.getCoef(2,1),
                matrix.getCoef(1,1)
                );

        double y = -Math.asin(
                matrix.getCoef(3,1)
                );

        double x = Math.atan2(
                matrix.getCoef(3,2),
                matrix.getCoef(3,3)
                );

        if(y>=Math.PI/2-(1E-9)){
            z = 0;
            x = Math.atan2(
                    matrix.getCoef(1,2),
                    matrix.getCoef(1,3)
                    );
        }

        if(y<=-Math.PI/2+(1E-9)){
            z = 0;
            x = Math.atan2(
                    -matrix.getCoef(1,2),
                    -matrix.getCoef(1,3)
                    );
        }

        return new Vect3D(x,y,z);

    }


    /**
     * Compute the rotation matrix between the spaceship and the space
     * 
     * @return The rotation matrix between the spaceship and the space
     */
    public static Matrix rotMatrixModule_Space(Vect3D orientation) {
        Matrix rotx = new Matrix();
        rotx.setCoef(1, 1, 1);
        rotx.setCoef(2, 2, Math.cos(orientation.x));
        rotx.setCoef(3, 3, Math.cos(orientation.x));
        rotx.setCoef(2, 3, -Math.sin(orientation.x));
        rotx.setCoef(3, 2, Math.sin(orientation.x));

        Matrix roty = new Matrix();
        roty.setCoef(2, 2, 1);
        roty.setCoef(1, 1, Math.cos(orientation.y));
        roty.setCoef(3, 3, Math.cos(orientation.y));
        roty.setCoef(1, 3, Math.sin(orientation.y));
        roty.setCoef(3, 1, -Math.sin(orientation.y));

        Matrix rotz = new Matrix();
        rotz.setCoef(3, 3, 1);
        rotz.setCoef(1, 1, Math.cos(orientation.z));
        rotz.setCoef(2, 2, Math.cos(orientation.z));
        rotz.setCoef(1, 2, -Math.sin(orientation.z));
        rotz.setCoef(2, 1, Math.sin(orientation.z));

        return Matrix.multiply(Matrix.multiply(rotz, roty), rotx);
    }
$\endgroup$
  • $\begingroup$ I edited the post, thanks for your time, sorry for the bad presentation, I tried to improve it. What I call a moment is the torque of our system at a specific time according to the current forces present on each point of the system. $\endgroup$ – Romaric Mollard Jan 9 '17 at 22:45
1
$\begingroup$

This is the normal procedure:

1) Compute body-fixed torques $\bf\tau$.

2) Compute body-fixed rotational accelerations, $\alpha = \bf I^{-1}\bf \tau$

3) Integrate accelerations to body-fixed rotational velocity, $\bf\omega$.

4) Convert $\bf\omega$ to quaternion derivative, $\dot{\bf q} = \frac{1}{2}\bf q \left[ \begin{array}{c} 0 \\ \bf\omega \end{array} \right] $.

5) Integrate $\dot{\bf q}$ to update $\bf q$

6) Normalize $\bf q$ as $\bf q \leftarrow \bf q/\|\bf q\|$

7) Convert $\bf q$ to Euler angles as needed (not necessary).

$\endgroup$
  • $\begingroup$ Ok, thank you, but in the case of the simulator, we are not in a continuous space/time. So I obtain the new velocity with $$ v = a * \Delta t $$ For linear acceleration. So for angular acceleration I do the same thing, but I was not able to add the two vectors (acceleration and current velocity), it was needed to convert the two vectors (which are actually quaternions) into matrix and multiply them. I don't know if this make a difference with your procedure I'll update my post in a couple of minutes. But thank you, my problem is finely resolved ! $\endgroup$ – Romaric Mollard Jan 10 '17 at 8:10
  • $\begingroup$ In discrete time your update law would be: $\bf\omega_{k+1} = \bf\omega_{k} + \bf\alpha \Delta t$ (i.e. both $\bf\alpha$ and $\bf\omega$ are vectors. Same law applies for $\bf q$, namely $\bf q_{k+1} = \bf q_{k}+ \dot{\bf q}\Delta t$. Also, you do not need to convert quaternions into matrices to multiply them. There are laws for direct multiplication. $\endgroup$ – Tpofofn Jan 11 '17 at 1:25
  • $\begingroup$ Ok, interesting, but in the case where my quaternion's length is equal to my quaternion angle, I have to normalize the vectors, then add them, re-normalize and what is the new rotation ? The sum of the angles ? (quaternion is defined by $$(qi, qj, qk, qr)$$ where qr is an angle usely, and $$(qi, qj, qk)$$ a normalized vector, but I have only a vector). $\endgroup$ – Romaric Mollard Jan 11 '17 at 7:02
  • $\begingroup$ I am not sure what you mean by quaternion angle. The quaternion should always have unit length. $\dot{\bf q}$ should always be perpendicular to $\bf q$ so the renormalization is a small adjustment to keep it on the unit sphere. Also, to combine rotations represented by quaternions, you multiply them, not add them. If you are properly using quaternions, should not need to use angles at all. That is the beauty of them. $\endgroup$ – Tpofofn Jan 11 '17 at 16:28
  • $\begingroup$ Also, note that the whole quaternion is normalized, not just the vector part, so $\| (qi, qj, qk, qr) \| = 1$ $\endgroup$ – Tpofofn Jan 11 '17 at 16:29

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.