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;
}
}
//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 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 FixTrans3 Translate(FixVec3 delta)
{
return(new FixTrans3(
_m11, _m12, _m13, _m14 + delta.X,
_m21, _m22, _m23, _m24 + delta.Y,
_m31, _m32, _m33, _m34 + delta.Z
));
}
public static FixVec3 Cross(FixVec3 lhs, FixVec3 rhs)
{
return(new FixVec3(
lhs.y * rhs.z - lhs.z * rhs.y,
lhs.z * rhs.x - lhs.x * rhs.z,
lhs.x * rhs.y - lhs.y * rhs.x
));
}
public FixTrans3(FixVec3 position, FixVec3 scale, FixVec3 rotation)
{
this = MakeRotationX(rotation.X)
.RotateY(rotation.Y)
.RotateZ(rotation.Z)
.Scale(scale)
.Translate(position);
}
public FixVec3 Cross(FixVec3 rhs)
{
return(new FixVec3(
y * rhs.z - z * rhs.y,
z * rhs.x - x * rhs.z,
x * rhs.y - y * rhs.x
));
}
public static FixTrans3 MakeScale(FixVec3 scale)
{
return(new FixTrans3(
scale.X, 0, 0, 0,
0, scale.Y, 0, 0,
0, 0, scale.Z, 0
));
}
public static FixTrans3 MakeTranslation(FixVec3 delta)
{
return(new FixTrans3(
1, 0, 0, delta.X,
0, 1, 0, delta.Y,
0, 0, 1, delta.Z
));
}
public FixTrans3(FixVec3 position, FixVec3 rotation, FixVec3 scale)
{
this = MakeRotationX(rotation.x)
.RotateY(rotation.y)
.RotateZ(rotation.z)
.Scale(scale)
.Translate(position);
}
public FixVec3 Cross(FixVec3 rhs)
{
return(new FixVec3(
_y * rhs._z - _z * rhs._y,
_z * rhs._x - _x * rhs._z,
_x * rhs._y - _y * rhs._x
));
}
public FixTrans3 Scale(FixVec3 scale)
{
return(new FixTrans3(
m[0, 0] * scale.X, m[0, 1] * scale.X, m[0, 2] * scale.X, m[0, 3] * scale.X,
m[1, 0] * scale.Y, m[1, 1] * scale.Y, m[1, 2] * scale.Y, m[1, 3] * scale.Y,
m[2, 0] * scale.Z, m[2, 1] * scale.Z, m[2, 2] * scale.Z, m[2, 3] * scale.Z
));
}
public FixTrans3 Scale(FixVec3 scale)
{
return(new FixTrans3(
_m11 * scale.X, _m12 * scale.X, _m13 * scale.X, _m14 * scale.X,
_m21 * scale.Y, _m22 * scale.Y, _m23 * scale.Y, _m24 * scale.Y,
_m31 * scale.Z, _m32 * scale.Z, _m33 * scale.Z, _m34 * scale.Z
));
}
public FixTrans3(FixVec3 position, FixVec3 scale, FixVec3 rotation)
{
this = MakeRotationX(rotation.X)
.RotateY(rotation.Y)
.RotateZ(rotation.Z)
.Scale(scale)
.Translate(position);
}
public static FixTrans3 MakeTranslation(FixVec3 delta)
{
return(new FixTrans3(
1, 0, 0, delta.x,
0, 1, 0, delta.y,
0, 0, 1, delta.z,
0, 0, 0, 1
));
}
public FixTrans3 Scale(FixVec3 scale)
{
return(new FixTrans3(
m[0, 0] * scale.x, m[0, 1] * scale.x, m[0, 2] * scale.x, m[0, 3] * scale.x,
m[1, 0] * scale.y, m[1, 1] * scale.y, m[1, 2] * scale.y, m[1, 3] * scale.y,
m[2, 0] * scale.z, m[2, 1] * scale.z, m[2, 2] * scale.z, m[2, 3] * scale.z,
0, 0, 0, 1
));
}
public FixTrans3 Translate(FixVec3 delta)
{
FixTrans3 ft = new FixTrans3(m);
ft.m[0, 3] += delta.x;
ft.m[1, 3] += delta.y;
ft.m[2, 3] += delta.z;
return(ft);
}
public static FixVec3 ClampMagnitude(FixVec3 vector, Fix maxLength)
{
if (vector.GetSqrMagnitude() > maxLength * maxLength)
{
return(vector.Normalize() * maxLength);
}
return(vector);
}
public static FixTrans3 MakeScale(FixVec3 scale)
{
return(new FixTrans3(
scale.x, 0, 0, 0,
0, scale.y, 0, 0,
0, 0, scale.z, 0,
0, 0, 0, 1
));
}
public static FixVec3 MoveTowards(FixVec3 current, FixVec3 target, Fix maxDistanceDelta)
{
FixVec3 vector = target - current;
Fix magnitude = vector.GetMagnitude();
if ((magnitude > maxDistanceDelta) && (magnitude != Fix.Zero))
{
return(current + ((FixVec3)((vector / magnitude) * maxDistanceDelta)));
}
return(target);
}
public override bool Equals(Object obj)
{
//Check for null and compare run-time types.
if ((obj == null) || !this.GetType().Equals(obj.GetType()))
{
return(false);
}
else
{
FixVec3 fv = (FixVec3)obj;
return(x == fv.x.raw && y.raw == fv.y.raw && z.raw == fv.z.raw);
}
}
//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);
}
/// <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));
}
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 FixVec3 Apply(FixVec3 vec)
{
return this * vec;
}
public static FixTrans3 MakeTranslation(FixVec3 delta)
{
return new FixTrans3(
1, 0, 0, delta.X,
0, 1, 0, delta.Y,
0, 0, 1, delta.Z
);
}
public static FixTrans3 MakeScale(FixVec3 scale)
{
return new FixTrans3(
scale.X, 0, 0, 0,
0, scale.Y, 0, 0,
0, 0, scale.Z, 0
);
}
public static FixTrans3 MakeRotation(FixVec3 degrees)
{
return MakeRotationX(degrees.X)
.RotateY(degrees.Y)
.RotateZ(degrees.Z);
}
public FixVec3 Cross(FixVec3 rhs)
{
return new FixVec3(
_y * rhs._z - _z * rhs._y,
_z * rhs._x - _x * rhs._z,
_x * rhs._y - _y * rhs._x
);
}
public static Fix Dot(FixVec3 lhs, FixVec3 rhs)
{
return(lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z);
}
public FixTrans3 Translate(FixVec3 delta)
{
return new FixTrans3(
_m11, _m12, _m13, _m14 + delta.X,
_m21, _m22, _m23, _m24 + delta.Y,
_m31, _m32, _m33, _m34 + delta.Z
);
}
public Fix Dot(FixVec3 rhs)
{
return(_x * rhs._x + _y * rhs._y + _z * rhs._z);
}
public FixTrans3 Rotate(FixVec3 degrees)
{
return(MakeRotation(degrees));
}
public static FixTrans3 MakeRotation(FixVec3 degrees)
{
return(MakeRotationX(degrees.X)
.RotateY(degrees.Y)
.RotateZ(degrees.Z));
}
public Fix Dot(FixVec3 rhs)
{
return _x * rhs._x + _y * rhs._y + _z * rhs._z;
}
public FixTrans3 Rotate(FixVec3 degrees)
{
return MakeRotation(degrees);
}
public FixTrans3 Scale(FixVec3 scale)
{
return new FixTrans3(
_m11 * scale.X, _m12 * scale.X, _m13 * scale.X, _m14 * scale.X,
_m21 * scale.Y, _m22 * scale.Y, _m23 * scale.Y, _m24 * scale.Y,
_m31 * scale.Z, _m32 * scale.Z, _m33 * scale.Z, _m34 * scale.Z
);
}
public FixVec3 Apply(FixVec3 vec)
{
return(this * vec);
}
public static FixTrans3 MakeRotation(FixVec3 degrees)
{
return(MakeRotationX(degrees.x)
.RotateY(degrees.y)
.RotateZ(degrees.z));
}
public Fix Dot(FixVec3 rhs)
{
return(x * rhs.x + y * rhs.y + z * rhs.z);
}
