public static GTA.Math.Quaternion ToQuaternion(this Vector3 vect)
{
vect = new Vector3()
{
X = vect.X.Denormalize() * -1f,
Y = vect.Y.Denormalize() - 180f,
Z = vect.Z.Denormalize() - 180f,
};
vect = vect.TransformVector(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);
GTA.Math.Quaternion result = new GTA.Math.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 Vector3 ToEuler(this GTA.Math.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));
pitchYawRoll = pitchYawRoll.TransformVector(ToDegrees);
pitchYawRoll = pitchYawRoll.Denormalize();
pitchYawRoll = new Vector3()
{
Y = pitchYawRoll.Y * -1f + 180f,
X = pitchYawRoll.X,
Z = pitchYawRoll.Z,
};
return(pitchYawRoll);
}
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 CherryMPShared.Quaternion ToLQuaternion(this GTA.Math.Quaternion vec)
{
return(new CherryMPShared.Quaternion()
{
X = vec.X,
Y = vec.Y,
Z = vec.Z,
W = vec.W,
});
}
/// <summary>
/// Rotate the rectangle by a quaternion
/// </summary>
/// <param name="rot">The quaternion to rotate by</param>
/// <returns>The current rectangle instance</returns>
public Rectangle3D Rotate(Quaternion rot)
{
foreach (var k in new List <string>(Corners.Keys))
{
var q = new QuaternionRotation3D(new System.Windows.Media.Media3D.Quaternion(rot.X, rot.Y, rot.Z, rot.W));
var r = new RotateTransform3D(q, ToPoint3D(Center));
Corners[k] = ToVector3(r.Transform(ToPoint3D(Corners[k])));
}
Position = Corners["000"];
GenerateEdges();
GenerateFaces();
return(this);
}
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 */
}
/// <summary>
/// Rotate the rectangle by a quaternion
/// </summary>
/// <param name="rot">The quaternion to rotate by</param>
/// <returns>The current rectangle instance</returns>
public Rectangle3D Rotate(Quaternion rot)
{
foreach (var k in new List<string>(Corners.Keys)) {
var q = new QuaternionRotation3D(new System.Windows.Media.Media3D.Quaternion(rot.X, rot.Y, rot.Z, rot.W));
var r = new RotateTransform3D(q, ToPoint3D(Center));
Corners[k] = ToVector3(r.Transform(ToPoint3D(Corners[k])));
}
Position = Corners["000"];
GenerateEdges();
GenerateFaces();
return this;
}
