Changing XYZ order when converting Euler-angles to quaternions - math

I'm using the following code to produce a quaternion from XYZ Euler-Angles in radians:
c1 = Math.cos( x / 2 )
c2 = Math.cos( y / 2 )
c3 = Math.cos( z / 2 )
s1 = Math.sin( x / 2 )
s2 = Math.sin( y / 2 )
s3 = Math.sin( z / 2 )
quaternion = [
c1 * c2 * c3 - s1 * s2 * s3,
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3,
c1 * c2 * s3 + s1 * s2 * c3,
]
from:
http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
This produces a quaternion that first rotates around the z, then y, then finally the x-axis - Z-Y-X. Is it possible to alter this formula so that it rotates around the axes in a different order? What I'm looking for is the opposite, so X-Y-Z.

If anyone's interested...
Yes you can, turns out on this occasion I just needed to swap the plus and minus signs around to get an X-Y-Z order (traditionally written ZYX).
Like so...
[
c1 * c2 * c3 + s1 * s2 * s3,
s1 * c2 * c3 - c1 * s2 * s3,
c1 * s2 * c3 + s1 * c2 * s3,
c1 * c2 * s3 - s1 * s2 * c3
]
Three.js has a full list of the different formulas for various xyz orders - in the function THREE.Quaternion.setFromEuler
https://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/steps/index.htm

Related

JuMP constraints involving matrix inverse

I'm attempting to solve for an n*n matrix U, which satisfies a variety of constraints, including some involving inverses of its sub-matrices. However, it seems that JuMP can't handle inverses, at least without some additional specification of invertibility. Here's an example of the problem with n=2.
using JuMP, Ipopt
m = Model(with_optimizer(Ipopt.Optimizer))
A = [5 7; 7 10]
B = [9 13; 13 19]
C = [3 4; 4 6]
nnodes = 2
#variable(m, U[1:nnodes, 1:nnodes])
A1 = U * A * U'
B1 = U * B * U'
C1 = U * C * U'
c1 = A1[1, 1] - 1
c2 = A1[2, 2] - 1
c3 = C1[1, 1] - 1
c4 = unmixed_iv2[1, 2]
a = A1[2, 2] - A1[2, 1] * inv(A1[1, 1]) * A1[2,1] # Schur complement
b = B1[2, 2] - B1[2, 1] * inv(B1[1, 1]) * B1[2,1] # Schur complement
c5 = a - b
#NLconstraint(m, c1 == 0)
#NLconstraint(m, c2 == 0)
#NLconstraint(m, c3 == 0)
#NLconstraint(m, c4 == 0)
#NLconstraint(m, c5 == 0)
solve(m)
This raises the following error:
ERROR: inv is not defined for type GenericQuadExpr. Are you trying to build a nonlinear problem? Make sure you use #NLconstraint/#NLobjective.
Any suggestions on how to solve this problem?
You cannot use inv outside the macros (or more generally, build up any nonlinear expression). Just put it inside like so:
using JuMP
model = Model()
#variable(model, x >= 0.5)
#NLconstraint(model, inv(x) <= 0.5)
p.s., I can't run your example because I don't know what unmixed_iv2 is.

Qbasic GOTO to a line with Apostophe

I'm trying to "convert" an old program in QBasic to Java.
I have a problem while debugging and I think is related to this GOTO statement.
I know that a line with an apostrophe is ignored but in this case the statement GOTO targets that line. Is ignored the first time and after with the GOTO statement is not?And the apostrophe concerns only the variable R12 and X12 or also R22,X22?
This is the .BAS file :)
*
4010 'R12 = R2MA + ((R2A * R2M * (R2A + R2M) + S ^ 2 * (R2A * X2M ^ 2 + R2M * X2A ^ 2)) / ((R2A + R2M) ^ 2 + S ^ 2 * (X2A + X2M) ^ 2))
4014 'X12 = X2D + ((R2A ^ 2 * X2M + R2M ^ 2 * X2A + S ^ 2 * X2A * X2M * (X2A + X2M)) / ((R2A + R2M) ^ 2 + S ^ 2 * (X2A + X2M) ^ 2))
R22 = ((R2A * XMMU ^ 2 + R2M * XAVVMU ^ 2) * S ^ 2 + R2A * R2M * RT2) / (RT2 ^ 2 + S ^ 2 * XT2 ^ 2)
X22 = (X2A * (R2M ^ 2 + S ^ 2 * X2M ^ 2) + X2M * (R2A ^ 2 + S ^ 2 * X2A ^ 2) + 2 * X2AVM * (R2A * R2M - S ^ 2 * X2AVM * XT2)) / (RT2 ^ 2 + S ^ 2 * XT2 ^ 2)
X22 = X22 + X2D
R22 = R22 / ZUN: X22 = X22 / ZUN
4016 A1 = R22 * E1 ^ 2 / (P2 * X22 ^ 2 + R22 * E1 ^ 2)
4020 S = .5 * A1 - SQR(A1 * (.25 * A1 - R22 * P2 / E1 ^ 2))
4030 G1 = (PFE / (3 * (VF * (FR / 100)) ^ 2)) * ZUN
4040 A2 = (R22 / S) ^ 2 + X22 ^ 2: B2 = R22 / (S * A2): C2 = X22 / A2: D2 = B2 + G1
4050 E2 = C2 + B1: F2 = D2 ^ 2 + E2 ^ 2: G2 = D2 / F2: H2 = E2 / F2: Z2 = SQR(G2 ^ 2 + H2 ^ 2)
4060 I2 = G2 + R11: L = H2 + X11: ZTOT = SQR(I2 ^ 2 + L ^ 2)
4070 I1 = 1 / ZTOT
4080 E3 = I1 * Z2
4090 IF ABS(E1 - E3) > P0 THEN 5800
.
.
.
5800 E1 = (E1 + E3) / 2
5810 GOTO 4010
*
This sure is an odd way to write a QBasic program, usually you either number all of the lines or none of them. You can run a small test program to see how QB handles GOTO in this case. So something like this:
10 I = 0
20 'PRINT "Test line 20"
30 'PRINT "Test line 30
PRINT "Test line 40"
PRINT "Test line 50"
60 PRINT "Test line 60"
70 I = I + 1
80 IF (I <= 1) GOTO 20
90 PRINT "THE END"
I tried this in QB4.5 and the output is this:
Test line 40
Test line 50
Test line 60
Test line 40
Test line 50
Test line 60
THE END
The GOTO statement works as normal but the contents of lines 20 and 30 are not executed, it is treated as comment. So to answer your question, in your case the lines with R12 = .. and X12 = .. will be ignored and not executed, and the lines with R22 = .. and X22 = .. etc. are processed as normal.

transfer the co-ordinates error in function

I am trying to transfer the coordinates of the points to a new generated system coordinates
the original points in the original system is in the top left corner ....
I wrote the following function to transfer the coordinates
I am using the formal that I got from this question
pre_question
this question has 2 photos that show what I mean and the sign for each part
The problem now is , I am getting negative value for w !
can anyone please check this function and let me know where is the problem
Thanks
{
CvPoint transfer_coordinate (CvPoint pt1 , CvPoint pt2 , CvPoint pt3 , CvPoint pt4 , CvPoint origin , CvPoint current)
{
// pt1 , pt2 ==> points in line Z
// pt3 , pt4 ==> points in line W
double a1 , a2 , b1 , b2 , d1 , d2;
d1= sqrt(pow((pt1.x - pt2.x),2.0)+ pow((pt1.y - pt2.y),2.0));
d2= sqrt(pow((pt3.x - pt4.x),2.0)+ pow((pt3.y - pt4.y),2.0));
a1 =(pt1.y-pt2.y)/d1;
b1 =(pt2.x-pt1.x)/d1;
a2 =(pt3.y-pt4.y)/d2;
b2 =(pt4.x-pt3.x)/d2;
CvPoint new_point;
//z = -sqrt(a1^2+b1^2)*(a2*(x-x0)+b2*(y-y0))/(a2*b1-a1*b2)
//w = sqrt(a2^2+b2^2)*(a1*(x-x0)+b1*(y-y0))/(a1*b2-a2*b1)
//z
new_point.x = -round(sqrt(pow(a1,2.0)+ pow(b1,2.0)) * (a2 * (current.x - origin.x) + b2 * (current.y - origin.y))/(a2 * b1 - a1 * b2));
// w
new_point.y = round(sqrt(pow(a2,2.0)+ pow(b2,2.0)) * (a1 * (current.x - origin.x) + b1 * (current.y - origin.y))/(a1 * b2 - a2 * b1));
CvPoint reverse_point;
//x = x0 - b1*z/sqrt(a1^2+b1^2) + b2*w/sqrt(a2^2+b2^2)
//y = y0 + a1*z/sqrt(a1^2+b1^2) - a2*w/sqrt(a2^2+b2^2)
//x
reverse_point.x = round (origin.x - b1 * new_point.x / sqrt(pow(a1,2.0) + pow(b1,2.0)) + b2 * new_point.y /sqrt(pow(a2,2)+ pow(b2,2)));
//y
reverse_point.y = round (origin.y + a1 * new_point.x / sqrt(pow(a1,2.0) + pow(b1,2.0)) - a2 * new_point.y /sqrt(pow(a2,2)+ pow(b2,2)));
//printf("\n points in Z line (%d,%d),(%d,%d) , points in W line (%d,%d),(%d,%d) , origin (%d,%d)",pt1.x,pt1.y,pt2.x,pt2.y,pt3.x,pt3.y,pt4.x,pt4.y,origin.x,origin.y);
//printf("\n current point = (%d,%d) , new point = (%d,%d) , reverse point = (%d,%d)" , current.x,current.y,new_point.x,new_point.y,reverse_point.x,reverse_point.y);
return new_point ;
}
}
If your W-axis corresponds to X-axis, then affine transformation matrix is M = [R]*[T], where R is rotation matrix by Phi angle and T is translation matrix by x0, y0
R = Cos(phi) -Sin(phi) 0
Sin(phi) Cos(phi) 0
0 0 1
and
T = 1 0 0
0 1 0
dx dy 1
You have to multiply these matrices to get M matrix and get inverse matrix MR = Inverse(M). Then you can use M and MR to transform coordinates from XY to WZ system and vice versa
[xnew, ynew, 1] = [xold, yold, 1] * [M]
More information here

Tangent calculation OpenGL

I am trying to calculate the tangent line (needed for bump mapping) for every vertex in my mesh. The v1, v2 and v3 are the vertices in the triangle and the t1, t2 and t3 are the respective texture coords. From what i understand this should output the tangent line for the three vertices of the triangle.
Vec3f va = Vec3f{vertexData[a * 3 + 0], vertexData[a * 3 + 1], vertexData[a * 3 + 2]};
Vec3f vb = Vec3f{vertexData[b * 3 + 0], vertexData[b * 3 + 1], vertexData[b * 3 + 2]};
Vec3f vc = Vec3f{vertexData[c * 3 + 0], vertexData[c * 3 + 1], vertexData[c * 3 + 2]};
Vec2f ta = (Vec2f){texcoordData[a * 2 + 0],texcoordData[a * 2 + 1]};
Vec2f tb = (Vec2f){texcoordData[b * 2 + 0],texcoordData[b * 2 + 1]};
Vec2f tc = (Vec2f){texcoordData[c * 2 + 0],texcoordData[c * 2 + 1]};
Vec3f v1 = subtractVec3f(vb, va);
Vec3f v2 = subtractVec3f(vc, va);
Vec2f t1 = subtractVec2f(tb, ta);
Vec2f t2 = subtractVec2f(tc, ta);
float coef = 1/(t1.u * t2.v - t1.v * t2.u);
Vec3f tangent = Vec3fMake((t2.v * v1.x - t1.v * v2.x) * coef,
(t2.v * v1.y - t1.v * v2.y) * coef,
(t2.v * v1.z - t1.v * v2.z) * coef);
My problem is that the coef variable is sometimes the nan (not a number) value causing the multiplication to be off. My mesh is not super complex, a simple cylinder, but i would like a universal formula to calculate the tangent line to enable bump mapping on all of my meshes.
coef becomming a NaN indicates some numerical problem with your input data, like degenerate triangles or texture coordinates. Make sure that the expression (t1.u * t2.v - t1.v * t2.u) doesn't (nearly) vanish, i.e. its absolute value is larger than some reasonable threshold value.
A good sanity check is |vb-va|>0 ^ |vc-va|>0, |tb-ta|>0 ^ |tc-ta|>0, |normalized(vb-va) . normalized(vc-va)| < 1 and |normalized(tb-ta) . normalized(tc-ta)| < 1.

Efficient 4x4 matrix inverse (affine transform)

I was hoping someone can point out an efficient formula for 4x4 affine matrix transform. Currently my code uses cofactor expansion and it allocates a temporary array for each cofactor. It's easy to read, but it's slower than it should be.
Note, this isn't homework and I know how to work it out manually using 4x4 co-factor expansion, it's just a pain and not really an interesting problem for me. Also I've googled and came up with a few sites that give you the formula already (http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm). However this one could probably be optimized further by pre-computing some of the products. I'm sure someone came up with the "best" formula for this at one point or another?
You should be able to exploit the fact that the matrix is affine to speed things up over a full inverse. Namely, if your matrix looks like this
A = [ M b ]
[ 0 1 ]
where A is 4x4, M is 3x3, b is 3x1, and the bottom row is (0,0,0,1), then
inv(A) = [ inv(M) -inv(M) * b ]
[ 0 1 ]
Depending on your situation, it may be faster to compute the result of inv(A) * x instead of actually forming inv(A). In that case, things simplify to
inv(A) * [x] = [ inv(M) * (x - b) ]
[1] = [ 1 ]
where x is a 3x1 vector (usually a 3D point).
Lastly, if M represents a rotation (i.e. its columns are orthonormal), then you can use the fact that inv(M) = transpose(M). Then computing the inverse of A is just a matter of subtracting the translation component, and multiplying by the transpose of the 3x3 part.
Note that whether or not the matrix is orthonormal is something that you should know from the analysis of the problem. Checking it during runtime would be fairly expensive; although you might want to do it in debug builds to check that your assumptions hold.
Hope all of that is clear...
Just in case someone would like to save some typing, here's an AS3 version I wrote based on page 9 (more efficient version of Laplace Expansion Theorem) of the link posted above by phkahler:
public function invert() : Matrix4 {
var m : Matrix4 = new Matrix4();
var s0 : Number = i00 * i11 - i10 * i01;
var s1 : Number = i00 * i12 - i10 * i02;
var s2 : Number = i00 * i13 - i10 * i03;
var s3 : Number = i01 * i12 - i11 * i02;
var s4 : Number = i01 * i13 - i11 * i03;
var s5 : Number = i02 * i13 - i12 * i03;
var c5 : Number = i22 * i33 - i32 * i23;
var c4 : Number = i21 * i33 - i31 * i23;
var c3 : Number = i21 * i32 - i31 * i22;
var c2 : Number = i20 * i33 - i30 * i23;
var c1 : Number = i20 * i32 - i30 * i22;
var c0 : Number = i20 * i31 - i30 * i21;
// Should check for 0 determinant
var invdet : Number = 1 / (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
m.i00 = (i11 * c5 - i12 * c4 + i13 * c3) * invdet;
m.i01 = (-i01 * c5 + i02 * c4 - i03 * c3) * invdet;
m.i02 = (i31 * s5 - i32 * s4 + i33 * s3) * invdet;
m.i03 = (-i21 * s5 + i22 * s4 - i23 * s3) * invdet;
m.i10 = (-i10 * c5 + i12 * c2 - i13 * c1) * invdet;
m.i11 = (i00 * c5 - i02 * c2 + i03 * c1) * invdet;
m.i12 = (-i30 * s5 + i32 * s2 - i33 * s1) * invdet;
m.i13 = (i20 * s5 - i22 * s2 + i23 * s1) * invdet;
m.i20 = (i10 * c4 - i11 * c2 + i13 * c0) * invdet;
m.i21 = (-i00 * c4 + i01 * c2 - i03 * c0) * invdet;
m.i22 = (i30 * s4 - i31 * s2 + i33 * s0) * invdet;
m.i23 = (-i20 * s4 + i21 * s2 - i23 * s0) * invdet;
m.i30 = (-i10 * c3 + i11 * c1 - i12 * c0) * invdet;
m.i31 = (i00 * c3 - i01 * c1 + i02 * c0) * invdet;
m.i32 = (-i30 * s3 + i31 * s1 - i32 * s0) * invdet;
m.i33 = (i20 * s3 - i21 * s1 + i22 * s0) * invdet;
return m;
}
This successfully produced an identity matrix when I multiplied various 3D transformation matrices by the inverse returned from this method. I'm sure you can search/replace to get this into whatever language you'd like.
To follow-up on pkhaler's and Robin Hilliard's excellent responses above, here is Robin's ActionScript 3 code converted into a C# method. Hopefully this can save some typing for other C# developers, as well as C/C++ and Java developers in need of a 4x4 matrix inversion function:
public static double[,] GetInverse(double[,] a)
{
var s0 = a[0, 0] * a[1, 1] - a[1, 0] * a[0, 1];
var s1 = a[0, 0] * a[1, 2] - a[1, 0] * a[0, 2];
var s2 = a[0, 0] * a[1, 3] - a[1, 0] * a[0, 3];
var s3 = a[0, 1] * a[1, 2] - a[1, 1] * a[0, 2];
var s4 = a[0, 1] * a[1, 3] - a[1, 1] * a[0, 3];
var s5 = a[0, 2] * a[1, 3] - a[1, 2] * a[0, 3];
var c5 = a[2, 2] * a[3, 3] - a[3, 2] * a[2, 3];
var c4 = a[2, 1] * a[3, 3] - a[3, 1] * a[2, 3];
var c3 = a[2, 1] * a[3, 2] - a[3, 1] * a[2, 2];
var c2 = a[2, 0] * a[3, 3] - a[3, 0] * a[2, 3];
var c1 = a[2, 0] * a[3, 2] - a[3, 0] * a[2, 2];
var c0 = a[2, 0] * a[3, 1] - a[3, 0] * a[2, 1];
// Should check for 0 determinant
var invdet = 1.0 / (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
var b = new double[4, 4];
b[0, 0] = ( a[1, 1] * c5 - a[1, 2] * c4 + a[1, 3] * c3) * invdet;
b[0, 1] = (-a[0, 1] * c5 + a[0, 2] * c4 - a[0, 3] * c3) * invdet;
b[0, 2] = ( a[3, 1] * s5 - a[3, 2] * s4 + a[3, 3] * s3) * invdet;
b[0, 3] = (-a[2, 1] * s5 + a[2, 2] * s4 - a[2, 3] * s3) * invdet;
b[1, 0] = (-a[1, 0] * c5 + a[1, 2] * c2 - a[1, 3] * c1) * invdet;
b[1, 1] = ( a[0, 0] * c5 - a[0, 2] * c2 + a[0, 3] * c1) * invdet;
b[1, 2] = (-a[3, 0] * s5 + a[3, 2] * s2 - a[3, 3] * s1) * invdet;
b[1, 3] = ( a[2, 0] * s5 - a[2, 2] * s2 + a[2, 3] * s1) * invdet;
b[2, 0] = ( a[1, 0] * c4 - a[1, 1] * c2 + a[1, 3] * c0) * invdet;
b[2, 1] = (-a[0, 0] * c4 + a[0, 1] * c2 - a[0, 3] * c0) * invdet;
b[2, 2] = ( a[3, 0] * s4 - a[3, 1] * s2 + a[3, 3] * s0) * invdet;
b[2, 3] = (-a[2, 0] * s4 + a[2, 1] * s2 - a[2, 3] * s0) * invdet;
b[3, 0] = (-a[1, 0] * c3 + a[1, 1] * c1 - a[1, 2] * c0) * invdet;
b[3, 1] = ( a[0, 0] * c3 - a[0, 1] * c1 + a[0, 2] * c0) * invdet;
b[3, 2] = (-a[3, 0] * s3 + a[3, 1] * s1 - a[3, 2] * s0) * invdet;
b[3, 3] = ( a[2, 0] * s3 - a[2, 1] * s1 + a[2, 2] * s0) * invdet;
return b;
}
IIRC you can greatly shrink the code and time by precomputing a bunch (12?) 2x2 determinants. Split the matrix in half vertically and compute every 2x2 in both the upper and lower half. One of these smaller determinants is used in every term you'll need for the bigger computation and they each get reused.
Also, don't use a separate determinant function - reuse the sub-determinants you computed for the adjoint to get the determinant.
Oh, just found
this.
There are some improvements you can make knowing its a certain kind of transform too.
I believe the only way to compute an inverse is to solve n times the equation: A x = y, where y spans the unit vectors, i.e., the first one is (1,0,0,0), the second is (0,1,0,0), etc.
(Using the cofactors (Cramer's rule) is a bad idea, unless you want a symbolic formula for the inverse.)
Most linear algebra libraries will allow you to solve those linear systems, and even to compute an inverse. Example in python (using numpy):
from numpy.linalg import inv
inv(A) # here you go

Resources