@@ -212,6 +212,165 @@ def __init__(self, f_batch):
212212 self .simplex_size = ti .field (gs .ti_int , shape = f_batch ())
213213
214214
215+ # =========================================== GJK ===========================================
216+ @ti .data_oriented
217+ class GJKState :
218+ def __init__ (self , solver , static_rigid_sim_config , gjk_static_config ):
219+ _B = solver ._B
220+ polytope_max_faces = gjk_static_config .polytope_max_faces
221+ max_contacts_per_pair = gjk_static_config .max_contacts_per_pair
222+ max_contact_polygon_verts = gjk_static_config .max_contact_polygon_verts
223+
224+ # Cache to store the previous support points for support mesh function.
225+ self .support_mesh_prev_vertex_id = ti .field (dtype = gs .ti_int , shape = (_B , 2 ))
226+
227+ ### GJK simplex
228+ struct_simplex_vertex = ti .types .struct (
229+ # Support points on the two objects
230+ obj1 = gs .ti_vec3 ,
231+ obj2 = gs .ti_vec3 ,
232+ # Support point IDs on the two objects
233+ id1 = gs .ti_int ,
234+ id2 = gs .ti_int ,
235+ # Vertex on Minkowski difference
236+ mink = gs .ti_vec3 ,
237+ )
238+ struct_simplex = ti .types .struct (
239+ # Number of vertices in the simplex
240+ nverts = gs .ti_int ,
241+ # Distance from the origin to the simplex
242+ dist = gs .ti_float ,
243+ )
244+ struct_simplex_buffer = ti .types .struct (
245+ # Normals of the simplex faces
246+ normal = gs .ti_vec3 ,
247+ # Signed distances of the simplex faces from the origin
248+ sdist = gs .ti_float ,
249+ )
250+ self .simplex_vertex = struct_simplex_vertex .field (shape = (_B , 4 ))
251+ self .simplex_buffer = struct_simplex_buffer .field (shape = (_B , 4 ))
252+ self .simplex = struct_simplex .field (shape = (_B ,))
253+
254+ # Only when we enable MuJoCo compatibility, we use the simplex vertex and buffer for intersection checks.
255+ if static_rigid_sim_config .enable_mujoco_compatibility :
256+ self .simplex_vertex_intersect = struct_simplex_vertex .field (shape = (_B , 4 ))
257+ self .simplex_buffer_intersect = struct_simplex_buffer .field (shape = (_B , 4 ))
258+ self .nsimplex = ti .field (dtype = gs .ti_int , shape = (_B ,))
259+
260+ # In safe GJK, if the initial simplex is degenerate and the geometries are discrete, we go through vertices
261+ # on the Minkowski difference to find a vertex that would make a valid simplex. To prevent iterating through
262+ # the same vertices again during initial simplex construction, we keep the vertex ID of the last vertex that
263+ # we searched, so that we can start searching from the next vertex.
264+ self .last_searched_simplex_vertex_id = ti .field (dtype = gs .ti_int , shape = (_B ,))
265+
266+ ### EPA polytope
267+ struct_polytope_vertex = struct_simplex_vertex
268+ struct_polytope_face = ti .types .struct (
269+ # Indices of the vertices forming the face on the polytope
270+ verts_idx = gs .ti_ivec3 ,
271+ # Indices of adjacent faces, one for each edge: [v1,v2], [v2,v3], [v3,v1]
272+ adj_idx = gs .ti_ivec3 ,
273+ # Projection of the origin onto the face, can be used as face normal
274+ normal = gs .ti_vec3 ,
275+ # Square of 2-norm of the normal vector, negative means deleted face
276+ dist2 = gs .ti_float ,
277+ # Index of the face in the polytope map, -1 for not in the map, -2 for deleted
278+ map_idx = gs .ti_int ,
279+ )
280+ # Horizon is used for representing the faces to delete when the polytope is expanded by inserting a new vertex.
281+ struct_polytope_horizon_data = ti .types .struct (
282+ # Indices of faces on horizon
283+ face_idx = gs .ti_int ,
284+ # Corresponding edge of each face on the horizon
285+ edge_idx = gs .ti_int ,
286+ )
287+ struct_polytope = ti .types .struct (
288+ # Number of vertices in the polytope
289+ nverts = gs .ti_int ,
290+ # Number of faces in the polytope (it could include deleted faces)
291+ nfaces = gs .ti_int ,
292+ # Number of faces in the polytope map (only valid faces on polytope)
293+ nfaces_map = gs .ti_int ,
294+ # Number of edges in the horizon
295+ horizon_nedges = gs .ti_int ,
296+ # Support point on the Minkowski difference where the horizon is created
297+ horizon_w = gs .ti_vec3 ,
298+ )
299+
300+ self .polytope = struct_polytope .field (shape = (_B ,))
301+ self .polytope_verts = struct_polytope_vertex .field (shape = (_B , 5 + gjk_static_config .epa_max_iterations ))
302+ self .polytope_faces = struct_polytope_face .field (shape = (_B , polytope_max_faces ))
303+ self .polytope_horizon_data = struct_polytope_horizon_data .field (
304+ shape = (_B , 6 + gjk_static_config .epa_max_iterations )
305+ )
306+
307+ # Face indices that form the polytope. The first [nfaces_map] indices are the faces that form the polytope.
308+ self .polytope_faces_map = ti .Vector .field (n = polytope_max_faces , dtype = gs .ti_int , shape = (_B ,))
309+
310+ # Stack to use for visiting faces during the horizon construction. The size is (# max faces * 3),
311+ # because a face has 3 edges.
312+ self .polytope_horizon_stack = struct_polytope_horizon_data .field (shape = (_B , polytope_max_faces * 3 ))
313+
314+ # Data structures for multi-contact detection based on MuJoCo's implementation.
315+ if gjk_static_config .enable_mujoco_multi_contact :
316+ struct_contact_face = ti .types .struct (
317+ # Vertices from the two colliding faces
318+ vert1 = gs .ti_vec3 ,
319+ vert2 = gs .ti_vec3 ,
320+ endverts = gs .ti_vec3 ,
321+ # Normals of the two colliding faces
322+ normal1 = gs .ti_vec3 ,
323+ normal2 = gs .ti_vec3 ,
324+ # Face ID of the two colliding faces
325+ id1 = gs .ti_int ,
326+ id2 = gs .ti_int ,
327+ )
328+ # Struct for storing temp. contact normals
329+ struct_contact_normal = ti .types .struct (
330+ endverts = gs .ti_vec3 ,
331+ # Normal vector of the contact point
332+ normal = gs .ti_vec3 ,
333+ # Face ID
334+ id = gs .ti_int ,
335+ )
336+ struct_contact_halfspace = ti .types .struct (
337+ # Halfspace normal
338+ normal = gs .ti_vec3 ,
339+ # Halfspace distance from the origin
340+ dist = gs .ti_float ,
341+ )
342+ self .contact_faces = struct_contact_face .field (shape = (_B , max_contact_polygon_verts ))
343+ self .contact_normals = struct_contact_normal .field (shape = (_B , max_contact_polygon_verts ))
344+ self .contact_halfspaces = struct_contact_halfspace .field (shape = (_B , max_contact_polygon_verts ))
345+ self .contact_clipped_polygons = gs .ti_vec3 .field (shape = (_B , 2 , max_contact_polygon_verts ))
346+
347+ # Whether or not the MuJoCo's contact manifold detection algorithm was used for the current pair.
348+ self .multi_contact_flag = ti .field (dtype = gs .ti_int , shape = (_B ,))
349+
350+ ### Final results
351+ # Witness information
352+ struct_witness = ti .types .struct (
353+ # Witness points on the two objects
354+ point_obj1 = gs .ti_vec3 ,
355+ point_obj2 = gs .ti_vec3 ,
356+ )
357+ self .witness = struct_witness .field (shape = (_B , max_contacts_per_pair ))
358+ self .n_witness = ti .field (dtype = gs .ti_int , shape = (_B ,))
359+
360+ # Contact information, the namings are the same as those from the calling function. Even if they could be
361+ # redundant, we keep them for easier use from the calling function.
362+ self .n_contacts = ti .field (dtype = gs .ti_int , shape = (_B ,))
363+ self .contact_pos = gs .ti_vec3 .field (shape = (_B , max_contacts_per_pair ))
364+ self .normal = gs .ti_vec3 .field (shape = (_B , max_contacts_per_pair ))
365+ self .is_col = ti .field (dtype = gs .ti_int , shape = (_B ,))
366+ self .penetration = ti .field (dtype = gs .ti_float , shape = (_B ,))
367+
368+ # Distance between the two objects.
369+ # If the objects are separated, the distance is positive.
370+ # If the objects are intersecting, the distance is negative (depth).
371+ self .distance = ti .field (dtype = gs .ti_float , shape = (_B ,))
372+
373+
215374# =========================================== SupportField ===========================================
216375@ti .data_oriented
217376class SupportFieldInfo :
0 commit comments