float SF[9][1];
SF[0] = 0;
SF[1] = 0;
SF[2] = 0;
SF[3] = -SF[2];
SF[4] = SF[2];
SF[5] = 0;
SF[6] = 0;
SF[7] = sq(q3);
SF[8] = sq(q2);
float SG[3][1];
SG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
SG[1] = 2*q0*q2 - 2*q1*q3;
SG[2] = 2*q0*q1 + 2*q2*q3;
float SQ[4][1];
SQ[0] = (q1*SG[0])/2 - (q0*SG[2])/2 + (q3*SG[1])/2;
SQ[1] = (q0*SG[1])/2 - (q2*SG[0])/2 + (q3*SG[2])/2;
SQ[2] = (q3*SG[0])/2 - (q1*SG[1])/2 + (q2*SG[2])/2;
SQ[3] = (q0*SG[0])/2 + (q1*SG[2])/2 + (q2*SG[1])/2;
float SPP[4][1];
SPP[0] = SF[5] - SF[0] + SF[6];
SPP[1] = SF[0] - SF[5];
SPP[2] = SF[0] + SF[5] + SF[6];
SPP[3] = SF[1];
float nextP[4][4];
nextP[0][0] = P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1] + SF[3]*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) + SPP[3]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) + SPP[1]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) + daYawVar*sq(SQ[2]);
nextP[0][1] = P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1] - (SF[0] + SF[6])*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) + SF[3]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) - SPP[3]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + daYawVar*SQ[1]*SQ[2];
nextP[1][1] = P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] + SF[3]*(P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] - P[2][3]*(SF[0] + SF[6])) - SPP[3]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + daYawVar*sq(SQ[1]) - P[2][1]*(SF[0] + SF[6]) - (SF[0] + SF[6])*(P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] - P[2][2]*(SF[0] + SF[6]));
nextP[0][2] = P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1] + SF[4]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + SPP[2]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) - SPP[3]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) + daYawVar*SQ[0]*SQ[2];
nextP[1][2] = P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] + SF[4]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + SPP[2]*(P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] - P[2][1]*(SF[0] + SF[6])) - SPP[3]*(P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] - P[2][3]*(SF[0] + SF[6])) - P[2][2]*(SF[0] + SF[6]) + daYawVar*SQ[0]*SQ[1];
nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SPP[2] - P[3][2]*SPP[3] + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SPP[2] - P[3][0]*SPP[3]) + SPP[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SPP[2] - P[3][1]*SPP[3]) - SPP[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SPP[2] - P[3][3]*SPP[3]) + daYawVar*sq(SQ[0]);
nextP[0][3] = P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1] + SF[4]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) + SPP[0]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + SPP[3]*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) - daYawVar*SQ[2]*SQ[3];
nextP[1][3] = P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] + SF[4]*(P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] - P[2][1]*(SF[0] + SF[6])) + SPP[0]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + SPP[3]*(P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] - P[2][2]*(SF[0] + SF[6])) - P[2][3]*(SF[0] + SF[6]) - daYawVar*SQ[1]*SQ[3];
nextP[2][3] = P[2][3] + P[0][3]*SF[4] + P[1][3]*SPP[2] - P[3][3]*SPP[3] + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SPP[2] - P[3][1]*SPP[3]) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SPP[2] - P[3][0]*SPP[3]) + SPP[3]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SPP[2] - P[3][2]*SPP[3]) - daYawVar*SQ[0]*SQ[3];
nextP[3][3] = P[3][3] + P[1][3]*SF[4] + P[0][3]*SPP[0] + P[2][3]*SPP[3] + SF[4]*(P[3][1] + P[1][1]*SF[4] + P[0][1]*SPP[0] + P[2][1]*SPP[3]) + SPP[0]*(P[3][0] + P[1][0]*SF[4] + P[0][0]*SPP[0] + P[2][0]*SPP[3]) + SPP[3]*(P[3][2] + P[1][2]*SF[4] + P[0][2]*SPP[0] + P[2][2]*SPP[3]) + daYawVar*sq(SQ[3]);