function [dud_dq,ddvec_dq,dd_dq,dv_dq] = getudderiv(oc, vert, lam, ...
                  lcmax, lc1, lc2, linkn, pos, dvec, d, nj, dJvdq, vel)

%function way
  p1 = vert(:,1)*lam(1) + vert(:,2)*lam(2) ...
     + vert(:,3)*lam(3) + vert(:,4)*lam(4);  
  p2 = vert(:,5)*lam(5) + vert(:,6)*lam(6) ...
     + vert(:,7)*lam(7) + vert(:,8)*lam(8);  
  pvec = p1 - p2;
  p = norm(p1-p2);
        
  fill = [];
  lag1 = 0;
  output_vec = [];
  dp1_term1 = 0;
  for j=1:lcmax
    pjointframe = vector3d(vert(1,j),vert(2,j),vert(3,j));
    J(:,:,j) = jacobian(oc,linkn,pjointframe,pos);  %J2 same as J1
          
    vdiff(:,j) = vert(:,j) - vert(:,j+4);
    fill(j) = 1;
    D(:,j) = vdiff(:,j);
    %lag1 = lag1 - vdiff(:,1)'*vdiff(:,j)*lam(j);
    output_vec = [output_vec  lam(j)];
    dD_dq(:,j,:) = J(4:6,:,j);
          
    dp1_term1 = dp1_term1 + lam(j)*J(4:6,:,j);
  end
  Kappa = [D'*D fill'; fill 0];
  Kappa_inv = inv(Kappa);   
  lag1 = Kappa_inv(lcmax+1,lcmax+1);
  output_vec = [output_vec  lag1]';
                
  dp1_dq = dp1_term1;
  dp2_dq = dp1_term1*0;
  for j=1:nj
    dDtD_dq(:,:,j) = D' * dD_dq(:,:,j);
        
    dDtD_dq(:,:,j) = dDtD_dq(:,:,j) + (dDtD_dq(:,:,j))';
          
    dKappa_dq(:,:,j) = [dDtD_dq(:,:,j)   (fill*0)'; (fill*0) 0];
    dans_dq(:,j) = -1* Kappa_inv * (dKappa_dq(:,:,j)*output_vec);
    %pause
          
    for b=1:lcmax
      dp1_dq(:,j) = dp1_dq(:,j) + vert(:,b)*dans_dq(b,j);
      dp2_dq(:,j) = dp2_dq(:,j) + vert(:,b+4)*dans_dq(b,j);
      %dp2_dq(:,j) = vert5*dans_dq(1,j) + vert6*dans_dq(2,j) ...
      %           +vert7*dans_dq(3,j);
    end
    dpvec_dq(:,j) = dp1_dq(:,j) - dp2_dq(:,j);
          
     %p = p + eps;
    dud_dq(:,j) = ((1/p)*dpvec_dq(:,j)) - (pvec/(p^3))*pvec'*dpvec_dq(:,j);
    ddvec_dq(:,j) = dpvec_dq(:,j) - (p-d)*dud_dq(:,j);
    %dvec'
    %ddvec_dq(:,j)
    %(1/(d+eps))
    %pause
    dd_dq(:,j) = (1/(d+eps))*dvec*ddvec_dq(:,j);
    
    %commented out while testing, to not have to deal with
    %generating a vel for the test.  Deal with this when doing
    %full test with splines and everything
    dv_dq(:,j) = dJvdq(:,:,j)*vel';
    %dv_dq = [];
          
  end
%test sphere/sphere
%dpvec_dq
%J(4:6,:,1);

%diff_matrix = dpvec_dq - J(4:6,:,1)
%pause