From 5e52292b7194b32e181e776903c0e2ab06a08cde Mon Sep 17 00:00:00 2001 From: Kevin D Date: Fri, 27 Feb 2026 16:06:10 -0800 Subject: [PATCH] added slack in collision avoidance constraint --- @miSim/constrainMotion.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index b325d50..1d9da7c 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -44,12 +44,12 @@ function [obj] = constrainMotion(obj) A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos); A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii)); - % Floor derived from existing params: recovery velocity = max gradient approach velocity. - % Correction splits between 2 agents, so |A| = 2*r_sum → floor = (4*r_sum*v_max/gain)^(1/p) + % Slack derived from existing params: recovery velocity = max gradient approach velocity. + % Correction splits between 2 agents, so |A| = 2*r_sum r_sum_ij = obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius; v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep; - h_floor_ij = -(4 * r_sum_ij * v_max_ij / obj.barrierGain)^(1 / obj.barrierExponent); - b(kk) = obj.barrierGain * max(h_floor_ij, h(ii, jj))^obj.barrierExponent; + slack = -(4 * r_sum_ij * v_max_ij / obj.barrierGain)^(1 / obj.barrierExponent); + b(kk) = obj.barrierGain * max(slack, h(ii, jj))^obj.barrierExponent; kk = kk + 1; end end