diff --git a/src/trapeze.jl b/src/trapeze.jl index 7fd9cf24..faeb797b 100644 --- a/src/trapeze.jl +++ b/src/trapeze.jl @@ -116,13 +116,9 @@ function setConstraintBlock!(docp::DOCP{Trapeze}, c, xu, v, time_grid, i, work) offset_dyn_i = (i-1)*docp.dim_NLP_x offset_dyn_ip1 = i*docp.dim_NLP_x - # trapeze rule - #@. c[offset+1:offset+docp.dim_OCP_x] = xip1 - (xi + 0.5 * (tip1 - ti) * (work[offset_dyn_i+1:offset_dyn_i+docp.dim_OCP_x] + work[offset_dyn_ip1+1:offset_dyn_ip1+docp.dim_OCP_x])) # STILL ALLOCATES -_- - + # trapeze rule (no allocations ^^) halfstep = 0.5 * (tip1 - ti) - @. c[offset+1:offset+docp.dim_OCP_x] = @views work[offset_dyn_i+1:offset_dyn_i+docp.dim_OCP_x] + work[offset_dyn_ip1+1:offset_dyn_ip1+docp.dim_OCP_x] - @views c[offset+1:offset+docp.dim_OCP_x] .*= -halfstep - @views c[offset+1:offset+docp.dim_OCP_x] .+= xip1 .- xi + @views @. c[offset+1:offset+docp.dim_OCP_x] = xip1 - (xi + halfstep * (work[offset_dyn_i+1:offset_dyn_i+docp.dim_OCP_x] + work[offset_dyn_ip1+1:offset_dyn_ip1+docp.dim_OCP_x])) if docp.is_lagrange c[offset+docp.dim_NLP_x] = get_lagrange_state_at_time_step(xu, docp, i+1) - (get_lagrange_state_at_time_step(xu, docp, i) + 0.5 * (tip1 - ti) * (work[offset_dyn_i+docp.dim_NLP_x] + work[offset_dyn_ip1+docp.dim_NLP_x]))