state equation for trapeze is now without allocation

control-toolbox · Nov 15, 2024 · 87d2b89 · 87d2b89
1 parent 6d850ca
commit 87d2b89
Showing 1 changed file with 2 additions and 6 deletions.
diff --git 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]))