Custom constraint solver

constraint.rb

require 'matrix'

Constraints = Matrix[
	[ 1.0, -1.0,  0.0,  1.0, -1.0],
	[ 1.0,  1.0, -1.0,  0.0,  0.0],
	[ 1.0, -1.0,  0.0, -1.0,  1.0],
]

puts "Constraints: #{Constraints}"

# Reduce the constraints to a 3x3 linear system.
Reduced = Constraints*Constraints.transpose
puts "Reduced: #{Reduced}"

# Add a small epsilon value on to ensure the system is invertible.
Epsilon = 0.1*(Reduced[0,0] + Reduced[1,1] + Reduced[2,2])
puts "Epsilon: #{Epsilon}"

# Solution matrix, multiply by error
Solution = Constraints.transpose*(Reduced + Matrix.scalar(3, Epsilon)).inverse
puts "Solution: #{Solution}"

# Desired solution
Desired = Vector[0, 1, 0.5]

# Clamp a solution. Could be more or less complicated than this.
def clamp(x)
	x.map{|elt| elt > 0.0 ? elt : 0.0}
end

# Solution and error vectors that are iterated on.
err = Desired
x = Vector[*Array.new(Constraints.column_size, 0.0)]

10.times do|i|
	# Solve, accumulate and clamp
	x = clamp(x + Solution*err)
	
	# Update the error using the clamped solution.
	err = Desired - Constraints*x
end

puts "------------"
puts "Desired: #{Desired}"
puts "Solved: #{Constraints*x}"
puts "Applied: #{x}"
puts "Error: #{err}"

# OUTPUT:

# Constraints: Matrix[[1.0, -1.0, 0.0, 1.0, -1.0], [1.0, 1.0, -1.0, 0.0, 0.0], [1.0, -1.0, 0.0, -1.0, 1.0]]
# Reduced: Matrix[[4.0, 0.0, 0.0], [0.0, 3.0, 0.0], [0.0, 0.0, 4.0]]
# Epsilon: 1.1
# Solution: Matrix[[0.19607843137254904, 0.24390243902439027, 0.19607843137254904], [-0.19607843137254904, 0.24390243902439027, -0.19607843137254904], [0.0, -0.24390243902439027, 0.0], [0.19607843137254904, 0.0, -0.19607843137254904], [-0.19607843137254904, 0.0, 0.19607843137254904]]
# ------------
# Desired: Vector[0, 1, 0.5]
# Solved: Vector[0.0017212400487222335, 0.9987573341025214, 0.4982786510093403]
# Applied: Vector[0.6243786398157763, 0.3743786942867451, 0.0, 0.0, 0.24827870548030903]
# Error: Vector[-0.0017212400487222335, 0.0012426658974785676, 0.0017213489906596768]