From 0fa39164f744244f2cc1ac584216791a4bb996e1 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 19 Aug 2024 07:54:01 -0700 Subject: [PATCH] Speed up mesh support function with warmstarts. PiperOrigin-RevId: 664807396 Change-Id: I9124e65efd9140a8e858b7b6be0867c26961477b --- src/engine/engine_collision_convex.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index e74b29d3..ceba996b 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -219,11 +219,11 @@ void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { vert_globalid = m->mesh_graph + graphadr + 2 + numvert; edge_localid = m->mesh_graph + graphadr + 2 + 2*numvert; - // init with first vertex in convex hull - ibest = 0; - tmp = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[0]] + - local_dir[1] * (mjtNum)vertdata[3*vert_globalid[0]+1] + - local_dir[2] * (mjtNum)vertdata[3*vert_globalid[0]+2]; + // init with first vertex in convex hull or warmstart + ibest = obj->meshindex < 0 ? 0 : obj->meshindex; + tmp = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[ibest]+0] + + local_dir[1] * (mjtNum)vertdata[3*vert_globalid[ibest]+1] + + local_dir[2] * (mjtNum)vertdata[3*vert_globalid[ibest]+2]; // hill-climb until no change change = 1;