box_corner_jump.jl

function rot_SO2(theta)
   rot = [cos(theta) sin(theta); -sin(theta) cos(theta)]
   return rot
end

using JuMP
m = Model()

vertices_zero = zeros(4,2)
half_height = 2
vertices_zero[1,:] = [1,half_height]
vertices_zero[2,:] = [1,-half_height]
vertices_zero[3,:] = [-1,-half_height]
vertices_zero[4,:] = [-1,half_height]

(m1, m2) = (-1.0, -1.0)
(b1, b2) = (5.5, 5.0)


t0 = 0
angle_0 = deg2rad(-45.0)
starting_position = [m1*t0 + b1 m2*t0 + b2]

vertices_posed = vertices_zero * rot_SO2(angle_0) .+ starting_position #best-effort broadcast semantics?

c0 = cos(t0)
s0 = sin(t0)
x0 = starting_position[1]
y0 = starting_position[2]

vx0 = Float64[vertices_posed[i, 1] for i=1:4]
vy0 = Float64[vertices_posed[i, 2] for i=1:4]

@defVar(m, t, start=t0)

@setObjective(m, Max, t)	

@defVar(m, x, start=x0)
@defVar(m, y, start=y0)
@defVar(m, c, start=c0)
@defVar(m, s, start=s0)
@defVar(m, vx[1:4])
@defVar(m, vy[1:4])

setValue(vx, vx0)
setValue(vy, vy0)


@addConstraint(m, x-m1*t-b1==0)
@addConstraint(m, y-m2*t-b2==0)

@addNLConstraint(m, s^2 + c^2 == 1)

# first row of rotation matrix multiplication
for i in 1:4 @addConstraint(m, c*vertices_zero[i,1] + -s*vertices_zero[i,2] + x == vx[i] ) end

# second row of rotation matrix multiplication
for i in 1:4 @addConstraint(m, s*vertices_zero[i,1] +  c*vertices_zero[i,2] + y == vy[i] ) end

#y coordinates don't penetrate bottom
for i in 1:4 @addConstraint(m, vy[i]>=0) end

#x coordinates don't penetrate left 
for i in 1:4 @addConstraint(m, vx[i]>=0) end

solve(m)
rad2deg(atan2(getValue(s),getValue(c)))
getValue(x), getValue(y)
getValue(t)

using PyPlot

ax = PyPlot.gca()
ax[:set_aspect]("equal")

#plot starting position of box
ax[:plot]( vcat(vx0, vx0[1]),  vcat(vy0, vy0[1]), "--")

#center of starting position, and constraint axis/direction
ax[:plot](x0,  y0, "o")
tf = 10
ax[:plot]([x0, x0+m1*tf], [y0, y0+m2*tf], "--")

#plot resting position of box
let vx=getValue(vx), vy=getValue(vy)
	ax[:plot]( vcat(vx, vx[1]),  vcat(vy, vy[1]), "-")
end

#center of resting position
let x=getValue(x), y=getValue(y)
	ax[:plot](x,  y, "o")
end

ax[:plot]([0.0, 10.0], [0.0, 0.0], lw=2, color="k")
ax[:plot]([0.0, 0.0], [0.0, 10.0], lw=2, color="k")