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);
}