forked from Archive/PX4-Autopilot
18 lines
369 B
C
18 lines
369 B
C
float t2 = q1*q2*2.0;
|
|
float t3 = q0*q0;
|
|
float t4 = q1*q1;
|
|
float t5 = q2*q2;
|
|
float t6 = q3*q3;
|
|
float t7 = q0*q2*2.0;
|
|
float t8 = q1*q3*2.0;
|
|
float t9 = q2*q3*2.0;
|
|
A0[0][0] = t3+t4-t5-t6;
|
|
A0[0][1] = t2-q0*q3*2.0;
|
|
A0[0][2] = t7+t8;
|
|
A0[1][0] = t2+q0*q3*2.0;
|
|
A0[1][1] = t3-t4+t5-t6;
|
|
A0[1][2] = t9-q0*q1*2.0;
|
|
A0[2][0] = -t7+t8;
|
|
A0[2][1] = t9+q0*q1*2.0;
|
|
A0[2][2] = t3-t4-t5+t6;
|