require 'matrix'
require 'mathn'
def clamp(f, min, max)
return (f > max ? max : (f < min ? min : f))
end
# Colums of the matrix are the thruster's force components (fx, fy, torque)
# The main forward thruster is the first one (it's big)
A = Matrix.columns([
Vector[ 0.0, 4.0, 0.0],
Vector[ 1.0, 0.0, 1.0],
Vector[ 1.0, 0.0, -1.0],
Vector[-1.0, 0.0, 1.0],
Vector[-1.0, 0.0, -1.0],
Vector[ 0.0, -1.0, 0.0],
])
# This is a 3x3 matrix that iteratively calculates the solution.
# Calculated sort of like least squares, but then multiplied by the transpose again.
M = A.transpose*(A*A.transpose).inverse
puts "M: #{M}"
# The force we want to apply.
desired = Vector[1.5, 1.0, 0.0]
puts "desired: #{desired}"
# The solution vector (how hard to fire the thrusters from [0-1])
# Start out with 0 and iterate to converge on the solution.
solution = Vector[*Array.new(A.column_size, 0.0)]
10.times do
# Figure out the error that needs to be corrected
err = desired - A*solution
# Calculate the approximate solution for the error
# Add that to the current solution then clamp
solution = (M*err + solution).map{|elt| clamp(elt, 0.0, 1.0)}
puts "solution: #{solution}"
end
puts "A*solution: #{A*solution}"