forked from Archive/PX4-Autopilot
20 lines
651 B
C
20 lines
651 B
C
t2 = q0*q0;
|
|
t3 = acos(q0);
|
|
t4 = -t2+1.0;
|
|
t5 = t2-1.0;
|
|
t6 = 1.0/t5;
|
|
t7 = q1*t6*2.0;
|
|
t8 = 1.0/pow(t4,3.0/2.0);
|
|
t9 = q0*q1*t3*t8*2.0;
|
|
t10 = t7+t9;
|
|
t11 = 1.0/sqrt(t4);
|
|
t12 = q2*t6*2.0;
|
|
t13 = q0*q2*t3*t8*2.0;
|
|
t14 = t12+t13;
|
|
t15 = q3*t6*2.0;
|
|
t16 = q0*q3*t3*t8*2.0;
|
|
t17 = t15+t16;
|
|
A0[0][0] = t10*(P_l_0_c_0_r_*t10+P_l_1_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_1_r_*t10+P_l_1_c_1_r_*t3*t11*2.0)*2.0;
|
|
A0[1][0] = t14*(P_l_0_c_0_r_*t14+P_l_2_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_2_r_*t14+P_l_2_c_2_r_*t3*t11*2.0)*2.0;
|
|
A0[2][0] = t17*(P_l_0_c_0_r_*t17+P_l_3_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_3_r_*t17+P_l_3_c_3_r_*t3*t11*2.0)*2.0;
|