rotVarVec.c

Go to the documentation of this file.

  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;