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;