public Kvaternion mult(TVector b)
{
Kvaternion C = new Kvaternion(new TVector(1, 1, 1),
Math.PI / 2);
//C.x = mas[0, 3] * b.x + mas[1, 3] * b.y + mas[2, 3] * b.z;
C.setx(mas[0, 0] * b.x + mas[0, 1] * b.y + mas[0, 2] * b.z);
//C.y = mas[0, 2] * b.x + mas[1, 2] * b.y + mas[2, 2] * b.z;
C.sety(mas[1, 0] * b.x + mas[1, 1] * b.y + mas[1, 2] * b.z);
C.setz(mas[2, 0] * b.x + mas[2, 1] * b.y + mas[2, 2] * b.z);
C.setw(mas[3, 0] * b.x + mas[3, 1] * b.y + mas[3, 2] * b.z);
return(C);
}
public Kvaternion quat_from_angles_rad()
{
Kvaternion q_h = new Kvaternion(new TVector(0, 0, 0), 0);
q_h.setz(Math.Sin(heading / 2));
q_h.setw(Math.Cos(heading / 2));
Kvaternion q_a = new Kvaternion(new TVector(0, 0, 0), 0);
q_a.sety(Math.Sin(altitude / 2));
q_a.setw(Math.Cos(altitude / 2));
Kvaternion q_b = new Kvaternion(new TVector(0, 0, 0), 0);
q_b.setx(Math.Sin(bank / 2));
q_b.setw(Math.Cos(bank / 2));
Kvaternion q_temp = q_h.mult(q_a);
return(q_temp.mult(q_b));
}