From eed24245436a9b04a1896083077004c689a4ab6c Mon Sep 17 00:00:00 2001
From: ppongsa <81595384+ppongsa@users.noreply.github.com>
Date: Sun, 28 Nov 2021 12:24:44 -0500
Subject: [PATCH] Add files via upload

Added calculation for dg for "nonlcon" function; Added diff_g function to calculate dg_dz
---
 Main_Peter.m | 24 ++++++++++++++++++++++--
 1 file changed, 22 insertions(+), 2 deletions(-)

diff --git a/Main_Peter.m b/Main_Peter.m
index 0ce2f6a..7b66d3b 100644
--- a/Main_Peter.m
+++ b/Main_Peter.m
@@ -77,6 +77,25 @@ function cp = ontrack(pose, bl_prev, bl_next, br_prev, br_next)
     cp = c1*c2;
 end
 
+function dg = diff_g(pose, bl_prev, bl_next, br_prev, br_next)
+%     syms Lx Ly Rx Ry ly lx rx ry X Y;
+%     g = (Lx*(Y - ly)-Ly*(X-lx))*(Rx*(Y - ry)-Ry*(X-rx));
+%     diff(g,X);
+%     diff(g,Y);
+
+    dg = [0,0];
+    l_vec = bl_next - bl_prev; % l = l_i+1 - l_i-1
+    r_vec = br_next - br_prev; % r = r_i+1 - r_i-1
+    
+    pl = pose - bl_prev; % p_l = p - l_i-1
+    pr = pose - br_prev; % p_r = p - r_i-1
+    
+    dg(1) = r_vec(2)*(l_vec(2)*pl(1) - l_vec(1)*pl(2)) + ...
+            l_vec(2)*(r_vec(2)*pr(1) - r_vec(1)*pr(2));
+    dg(2) = - r_vec(1)*(l_vec(2)*pl(1) - l_vec(1)*pl(2)) - ...
+              l_vec(1)*(r_vec(2)*pr(1) - r_vec(1)*pr(2));
+end
+
 function [lb, ub] = bounds(TestTrack, StepsPerPoint)
 
     [m,nPts] = size(TestTrack.cline);
@@ -165,6 +184,9 @@ function [g,h,dg,dh]=nonlcon(z, TestTrack)
 
         g = ontrack(Pinit, bl_vec(:,1), bl_vec(:,2), br_vec(:,1), br_vec(:,2));
         
+        dgi_dzj = diff_g(pose, bl_prev, bl_next, br_prev, br_next);
+        dg(i,numState*i-numState+1) = dgi_dzj(1);
+        dg(i,numState*i-numState+2) = dgi_dzj(2);
         
 %         if i==1
 %             h(1:3) = z(1:3,:) ;
@@ -175,8 +197,6 @@ function [g,h,dg,dh]=nonlcon(z, TestTrack)
 %             dh(3*i-2:3*i,3*i-5:3*i) = [-eye(3)-dt*statepart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)),eye(3)] ;
 %             dh(3*i-2:3*i,3*nsteps+2*i-3:3*nsteps+2*i-2) = -dt*inputpart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)) ;
 %         end
-
-%         dg(i,3*i-2:3*i-1) = -2*[z(3*i-2)-3.5, z(3*i-1)+0.5] ;
     end
 end