Обновить Quaternion.lua

Quaternion.lua

@@ -159,4 +159,48 @@ function Quaternion:conjugate()
   return Quaternion.new(-self.x, -self.y, -self.z, self.w)
 end
 
+function Quaternion:inverse()
+  local lenSq = self:length()^2
+  if lenSq == 0 then return Quaternion.new(0, 0, 0, 1) end
+  return Quaternion.new(-self.x / lenSq, -self.y / lenSq, -self.z / lenSq, self.w / lenSq)
+end
+
+function Quaternion.fromMatrix(matrix)
+  if #matrix ~= 3 or #matrix[1] ~= 3 or #matrix[2] ~= 3 or #matrix[3] ~= 3 then
+    error("Invalid matrix size, must be 3x3")
+  end
+
+  local trace = matrix[1][1] + matrix[2][2] + matrix[3][3]
+  local q = Quaternion.new()
+
+  if trace > 0 then
+    local s = math.sqrt(trace + 1) * 2
+    q.w = s / 4
+    q.x = (matrix[3][2] - matrix[2][3]) / s
+    q.y = (matrix[1][3] - matrix[3][1]) / s
+    q.z = (matrix[2][1] - matrix[1][2]) / s
+  else
+    if matrix[1][1] > matrix[2][2] and matrix[1][1] > matrix[3][3] then
+      local s = math.sqrt(1 + matrix[1][1] - matrix[2][2] - matrix[3][3]) * 2
+      q.w = (matrix[3][2] - matrix[2][3]) / s
+      q.x = s / 4
+      q.y = (matrix[1][2] + matrix[2][1]) / s
+      q.z = (matrix[1][3] + matrix[3][1]) / s
+    elseif matrix[2][2] > matrix[3][3] then
+      local s = math.sqrt(1 + matrix[2][2] - matrix[1][1] - matrix[3][3]) * 2
+      q.w = (matrix[1][3] - matrix[3][1]) / s
+      q.x = (matrix[1][2] + matrix[2][1]) / s
+      q.y = s / 4
+      q.z = (matrix[2][3] + matrix[3][2]) / s
+    else
+      local s = math.sqrt(1 + matrix[3][3] - matrix[1][1] - matrix[2][2]) * 2
+      q.w = (matrix[2][1] - matrix[1][2]) / s
+      q.x = (matrix[1][3] + matrix[3][1]) / s
+      q.y = (matrix[2][3] + matrix[3][2]) / s
+      q.z = s / 4
+    end
+  end
+  return q:normalize()
+end 
+
 return Quaternion