public static Quaternion ToQuaternion(this Vector3 vect)
{
vect = new Vector3()
{
X = vect.X.Denormalize() * -1,
Y = vect.Y.Denormalize() - 180f,
Z = vect.Z.Denormalize() - 180f,
};
vect = vect.ToRadians();
float rollOver2 = vect.Z * 0.5f;
float sinRollOver2 = (float)Math.Sin((double)rollOver2);
float cosRollOver2 = (float)Math.Cos((double)rollOver2);
float pitchOver2 = vect.Y * 0.5f;
float sinPitchOver2 = (float)Math.Sin((double)pitchOver2);
float cosPitchOver2 = (float)Math.Cos((double)pitchOver2);
float yawOver2 = vect.X * 0.5f; // pitch
float sinYawOver2 = (float)Math.Sin((double)yawOver2);
float cosYawOver2 = (float)Math.Cos((double)yawOver2);
Quaternion result = new Quaternion()
{
X = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2,
Y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2,
Z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2,
W = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2
};
return(result);
}
public static Quaternion LerpQuaternion(Quaternion start, Quaternion end, float speed)
{
return(new Quaternion()
{
X = start.X + (end.X - start.X) * speed,
Y = start.Y + (end.Y - start.Y) * speed,
Z = start.Z + (end.Z - start.Z) * speed,
W = start.W + (end.W - start.W) * speed,
});
}
public static Vector3 ToEuler(this Quaternion q)
{
var pitchYawRoll = new Vector3();
double sqw = q.W * q.W;
double sqx = q.X * q.X;
double sqy = q.Y * q.Y;
double sqz = q.Z * q.Z;
pitchYawRoll.Y = (float)Math.Atan2(2f * q.X * q.W + 2f * q.Y * q.Z, 1 - 2f * (sqz + sqw)); // Yaw
pitchYawRoll.X = (float)Math.Asin(2f * (q.X * q.Z - q.W * q.Y)); // Pitch
pitchYawRoll.Z = (float)Math.Atan2(2f * q.X * q.Y + 2f * q.Z * q.W, 1 - 2f * (sqy + sqz));
return(pitchYawRoll);
}
public static Vector3 QuaternionToEuler(Quaternion quat)
{
//heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2) (yaw)
//attitude = asin(2 * qx * qy + 2 * qz * qw) (pitch)
//bank = atan2(2 * qx * qw - 2 * qy * qz, 1 - 2 * qx2 - 2 * qz2) (roll)
return(new Vector3()
{
X = (float)Math.Asin(2 * quat.X * quat.Y + 2 * quat.Z * quat.W),
Y = (float)Math.Atan2(2 * quat.X * quat.W - 2 * quat.Y * quat.Z, 1 - 2 * quat.X * quat.X - 2 * quat.Z * quat.Z),
Z = (float)Math.Atan2(2 * quat.Y * quat.W - 2 * quat.X * quat.Z, 1 - 2 * quat.Y * quat.Y - 2 * quat.Z * quat.Z),
});
/*except when qx*qy + qz*qw = 0.5 (north pole)
* which gives:
* heading = 2 * atan2(x,w)
* bank = 0
*
* and when qx*qy + qz*qw = -0.5 (south pole)
* which gives:
* heading = -2 * atan2(x,w)
* bank = 0 */
}
public static void SetEntityQuaternion(Entity ent, GTA.Math.Quaternion q)
{
Function.Call(Hash.SET_ENTITY_QUATERNION, ent.Handle, q.X, q.Y, q.Z, q.W);
}
