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 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))); }