//https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
public static FixVec3 ToEuler(FixQuaternion q)
{
FixVec3 result;
Fix t0 = 2 * (q.w * q.z + q.x * q.y);
Fix t1 = Fix.One - (2 * (q.y * q.y + q.z * q.z));
result._z = FixMath.Atan2(t0, t1);
Fix t2 = 2 * (q.w * q.y - q.z * q.x);
if (t2 >= Fix.One)
{
result._y = FixMath.PI / 2;
}
else if (t2 <= -Fix.One)
{
result._y = -(FixMath.PI / 2);
}
else
{
result._y = FixMath.Asin(t2);
}
Fix t3 = 2 * (q.w * q.x + q.y * q.z);
Fix t4 = Fix.One - (2 * (q.x * q.x + q.y * q.y));
result._x = FixMath.Atan2(t3, t4);
return(result);
}
//https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
public FixQuaternion(FixVec3 e)
{
w = FixMath.Cos(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Cos(e._x / 2) + FixMath.Sin(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Sin(e._x / 2);
x = FixMath.Cos(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Sin(e._x / 2) - FixMath.Sin(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Cos(e._x / 2);
y = FixMath.Sin(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Sin(e._x / 2) + FixMath.Cos(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Cos(e._x / 2);
z = FixMath.Sin(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Cos(e._x / 2) - FixMath.Cos(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Sin(e._x / 2);
}
//https://math.stackexchange.com/questions/2975109/how-to-convert-euler-angles-to-quaternions-and-get-the-same-euler-angles-back-fr
public static FixVec3 ToEuler(FixQuaternion q)
{
FixVec3 result;
Fix t0 = 2 * (q.w * q.z + q.x * q.y);
Fix t1 = 1 - 2 * (q.y * q.y + q.z * q.z);
result.z = FixMath.Atan2(t0, t1);
Fix t2 = 2 * (q.w * q.y - q.z * q.x);
if (FixMath.Abs(t2) >= Fix.one)
{
result.y = FixMath.PI / 2;
//t2 = 1;
}
else if (t2 <= -Fix.one)
{
result.y = -(FixMath.PI / 2);
//t2 = -1;
}
else
{
result.y = FixMath.Asin(t2);
}
Fix t3 = 2 * (q.w * q.x + q.y * q.z);
Fix t4 = 1 - 2 * (q.x * q.x + q.y * q.y);
result.x = FixMath.Atan2(t3, t4);
return(result);
}
//https://math.stackexchange.com/questions/2975109/how-to-convert-euler-angles-to-quaternions-and-get-the-same-euler-angles-back-fr
public FixQuaternion(FixVec3 e)
{
x = FixMath.Cos(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Sin(e.x / 2) - FixMath.Sin(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Cos(e.x / 2);
y = FixMath.Sin(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Sin(e.x / 2) + FixMath.Cos(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Cos(e.x / 2);
z = FixMath.Sin(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Cos(e.x / 2) - FixMath.Cos(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Sin(e.x / 2);
w = FixMath.Cos(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Cos(e.x / 2) + FixMath.Sin(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Sin(e.x / 2);
}
public FixQuaternion(ref FixTrans3 matrix)
{
Fix determinant = (matrix.M11 * (matrix.M22 * matrix.M33 - matrix.M32 * matrix.M23)) -
(matrix.M12 * (matrix.M21 * matrix.M33 - matrix.M31 * matrix.M23)) +
(matrix.M13 * (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22));
Fix scale = FixMath.Pow(determinant, Fix.One / 3);
Fix x, y, z;
w = (FixMath.Sqrt(FixMath.Max(0, scale + matrix.M11 + matrix.M22 + matrix.M33)) / 2);
x = (FixMath.Sqrt(FixMath.Max(0, scale + matrix.M11 - matrix.M22 - matrix.M33)) / 2);
y = (FixMath.Sqrt(FixMath.Max(0, scale - matrix.M11 + matrix.M22 - matrix.M33)) / 2);
z = (FixMath.Sqrt(FixMath.Max(0, scale - matrix.M11 - matrix.M22 + matrix.M33)) / 2);
xyz = new FixVec3(x, y, z);
if (matrix.M32 - matrix.M23 < 0)
{
X = -X;
}
if (matrix.M13 - matrix.M31 < 0)
{
Y = -Y;
}
if (matrix.M21 - matrix.M12 < 0)
{
Z = -Z;
}
}
public FixTrans2(FixVec2 position, FixVec2 scale, Fix rotation)
{
Fix cos = FixMath.Cos(rotation);
Fix sin = FixMath.Sin(rotation);
_m11 = cos * scale.X; _m12 = -sin * scale.X; _m13 = position.X;
_m21 = sin * scale.Y; _m22 = cos * scale.Y; _m23 = position.Y;
}
public static FixTrans2 MakeRotation(Fix degrees)
{
Fix cos = FixMath.Cos(degrees);
Fix sin = FixMath.Sin(degrees);
return(new FixTrans2(
cos, -sin, 0,
sin, cos, 0
));
}
public Fix GetMagnitude()
{
ulong N = (ulong)((long)_x.Raw * (long)_x.Raw + (long)_y.Raw * (long)_y.Raw + (long)_z.Raw * (long)_z.Raw);
if (N == 0)
{
return(new Fix(0));
}
return(new Fix((int)(FixMath.SqrtULong(N << 2) + 1) >> 1));
}
public Fix GetMagnitude()
{
Fix sqrMagnitude = GetSqrMagnitude();
if (sqrMagnitude == Fix.Zero)
{
return(Fix.Zero);
}
return(FixMath.Sqrt(sqrMagnitude));
}
public Fix GetMagnitude()
{
if (X == Fix.Zero && Y == Fix.Zero)
{
return(Fix.Zero);
}
ulong N = (ulong)((long)_x.Raw * (long)_x.Raw + (long)_y.Raw * (long)_y.Raw);
return(new Fix((int)(FixMath.SqrtULong(N << 2) + 1) >> 1));
}
public static FixTrans3 MakeRotationX(Fix degrees)
{
Fix cos = FixMath.Cos(degrees);
Fix sin = FixMath.Sin(degrees);
return(new FixTrans3(
1, 0, 0, 0,
0, cos, -sin, 0,
0, sin, cos, 0,
0, 0, 0, 1
));
}
public Fix GetMagnitude()
{
ulong N = (ulong)(((long)x.raw * (long)x.raw) + ((long)y.raw * (long)y.raw) + ((long)z.raw * (long)z.raw));
if (N == 0)
{
return(Fix.zero);
}
else
{
return(new Fix((int)(FixMath.SqrtULong(N << 2) + 1) >> 1));
}
}
public static Fix Angle(FixVec2 from, FixVec2 to)
{
Fix denominator = FixMath.Sqrt(from.GetMagnitudeSquared() * to.GetMagnitudeSquared());
if (denominator < Fix.Epsilon)
{
return(Fix.zero);
}
Fix dot = FixMath.Clamp(FixVec2.Dot(from, to) / denominator, -Fix.one, Fix.one);
return(FixMath.Acos(dot) * FixMath.Rad2Deg);
}
public Fix GetMagnitude()
{
ulong N = (ulong)((long)_x.Raw * (long)_x.Raw + (long)_y.Raw * (long)_y.Raw + (long)_z.Raw * (long)_z.Raw);
// Debug.Log("N:::: " + N);
if (0 == N)
{
return(0);
}
return(new Fix((int)(FixMath.SqrtULong(N << 2) + 1) >> 1));
}
/// <summary>
/// Build a quaternion from the given axis and angle
/// </summary>
/// <param name="axis">The axis to rotate about</param>
/// <param name="angle">The rotation angle in radians</param>
/// <returns></returns>
public static FixQuaternion FromAxisAngle(FixVec3 axis, Fix angle)
{
if (axis.GetSqrMagnitude() == Fix.Zero)
{
return(Identity);
}
FixQuaternion result = Identity;
angle *= Fix.One / 2;
axis.Normalize();
result.Xyz = axis * FixMath.Sin(angle);
result.W = FixMath.Cos(angle);
return(Normalize(result));
}
//https://gamedev.stackexchange.com/questions/50963/how-to-extract-euler-angles-from-transformation-matrix
public FixVec3 EulerAngle()
{
FixVec3 ea = new FixVec3();
ea.x = FixMath.Atan2(-m[1, 2], m[2, 2]);
Fix cosYangle = FixMath.Sqrt(FixMath.Pow(m[0, 0], 2) + FixMath.Pow(m[0, 1], 2));
ea.y = FixMath.Atan2(m[0, 2], cosYangle);
Fix sinXangle = FixMath.Sin(ea.x);
Fix cosXangle = FixMath.Cos(ea.x);
ea.z = FixMath.Atan2((cosXangle * m[1, 0]) + (sinXangle * m[2, 0]), (cosXangle * m[1, 1]) + (sinXangle * m[2, 1]));
return(ea);
}
public static FixVec3 SmoothDamp(FixVec3 current, FixVec3 target, ref FixVec3 currentVelocity, Fix smoothTime, Fix maxSpeed, Fix deltaTime)
{
smoothTime = FixMath.Max(Fix.Ratio(1, 10000), smoothTime);
Fix num = Fix.Ratio(2, 1) / smoothTime;
Fix num2 = num * deltaTime;
Fix d = Fix.One / (Fix.One + num2 + Fix.Ratio(48, 100) * num2 * num2 + Fix.Ratio(235, 1000) * num2 * num2 * num2);
FixVec3 vector = current - target;
FixVec3 vector2 = target;
Fix maxLength = maxSpeed * smoothTime;
vector = FixVec3.ClampMagnitude(vector, maxLength);
target = current - vector;
FixVec3 vector3 = (currentVelocity + num * vector) * deltaTime;
currentVelocity = (currentVelocity - num * vector3) * d;
FixVec3 vector4 = target + (vector + vector3) * d;
if ((vector2 - current).Dot(vector4 - vector2) > Fix.Zero)
{
vector4 = vector2;
currentVelocity = (vector4 - vector2) / deltaTime;
}
return(vector4);
}
public static FixVec2 Max(FixVec2 pointA, FixVec2 t)
{
return(new FixVec2(FixMath.Max(pointA.x, t.x), FixMath.Max(pointA.y, t.y)));
}
public static FixVec2 Abs(FixVec2 v)
{
return(new FixVec2(FixMath.Abs(v.x), FixMath.Abs(v.y)));
}
/// <summary>
/// Do Spherical linear interpolation between two quaternions
/// </summary>
/// <param name="q1">The first quaternion</param>
/// <param name="q2">The second quaternion</param>
/// <param name="blend">The blend factor</param>
/// <returns>A smooth blend between the given quaternions</returns>
public static FixQuaternion Slerp(FixQuaternion q1, FixQuaternion q2, Fix blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == Fix.Zero)
{
if (q2.LengthSquared == Fix.Zero)
{
return(Identity);
}
return(q2);
}
else if (q2.LengthSquared == Fix.Zero)
{
return(q1);
}
Fix cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz);
if (cosHalfAngle >= Fix.One || cosHalfAngle <= -Fix.One)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < Fix.Zero)
{
q2.Xyz = -q2.Xyz;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
Fix blendA;
Fix blendB;
if (cosHalfAngle < (Fix.One / 100) * 99)
{
// do proper slerp for big angles
Fix halfAngle = FixMath.Acos(cosHalfAngle);
Fix sinHalfAngle = FixMath.Sin(halfAngle);
Fix oneOverSinHalfAngle = Fix.One / sinHalfAngle;
blendA = FixMath.Sin(halfAngle * (Fix.One - blend)) * oneOverSinHalfAngle;
blendB = FixMath.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = Fix.One - blend;
blendB = blend;
}
FixQuaternion result = new FixQuaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > Fix.Zero)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
public Fix Magnitude()
{
return(FixMath.Sqrt(x * x + y * y + z * z + w * w));
}
public Fix GetMagnitude()
{
ulong N = (ulong)((long)_x.raw * (long)_x.raw + (long)_y.raw * (long)_y.raw);
return(new Fix((int)(FixMath.SqrtULong(N << 2) + 1) >> 1));
}
internal static FixVec2 Min(FixVec2 pointA, FixVec2 t)
{
return(new FixVec2(FixMath.Min(pointA.x, t.x), FixMath.Min(pointA.y, t.y)));
}
