@@ -36,15 +36,18 @@ class Geom:
36
36
size : wp .vec3
37
37
vertadr : int
38
38
vertnum : int
39
+ vert : wp .array (dtype = wp .vec3 )
39
40
40
41
41
42
@wp .func
42
43
def _geom (
43
44
# Model:
45
+ geom_type : wp .array (dtype = int ),
44
46
geom_dataid : wp .array (dtype = int ),
45
47
geom_size : wp .array (dtype = wp .vec3 ),
46
48
mesh_vertadr : wp .array (dtype = int ),
47
49
mesh_vertnum : wp .array (dtype = int ),
50
+ mesh_vert : wp .array (dtype = wp .vec3 ),
48
51
# Data in:
49
52
geom_xpos_in : wp .array2d (dtype = wp .vec3 ),
50
53
geom_xmat_in : wp .array2d (dtype = wp .mat33 ),
@@ -67,6 +70,9 @@ def _geom(
67
70
geom .vertadr = - 1
68
71
geom .vertnum = - 1
69
72
73
+ if geom_type [gid ] == int (GeomType .MESH .value ):
74
+ geom .vert = mesh_vert
75
+
70
76
return geom
71
77
72
78
@@ -724,6 +730,152 @@ def plane_box(
724
730
break
725
731
726
732
733
+ _HUGE_VAL = 1e6
734
+
735
+
736
+ @wp .func
737
+ def plane_convex (
738
+ # Data in:
739
+ nconmax_in : int ,
740
+ # In:
741
+ plane : Geom ,
742
+ convex : Geom ,
743
+ worldid : int ,
744
+ margin : float ,
745
+ gap : float ,
746
+ condim : int ,
747
+ friction : vec5 ,
748
+ solref : wp .vec2f ,
749
+ solreffriction : wp .vec2f ,
750
+ solimp : vec5 ,
751
+ geoms : wp .vec2i ,
752
+ # Data out:
753
+ ncon_out : wp .array (dtype = int ),
754
+ contact_dist_out : wp .array (dtype = float ),
755
+ contact_pos_out : wp .array (dtype = wp .vec3 ),
756
+ contact_frame_out : wp .array (dtype = wp .mat33 ),
757
+ contact_includemargin_out : wp .array (dtype = float ),
758
+ contact_friction_out : wp .array (dtype = vec5 ),
759
+ contact_solref_out : wp .array (dtype = wp .vec2 ),
760
+ contact_solreffriction_out : wp .array (dtype = wp .vec2 ),
761
+ contact_solimp_out : wp .array (dtype = vec5 ),
762
+ contact_dim_out : wp .array (dtype = int ),
763
+ contact_geom_out : wp .array (dtype = wp .vec2i ),
764
+ contact_worldid_out : wp .array (dtype = int ),
765
+ ):
766
+ """Calculates contacts between a plane and a convex object."""
767
+
768
+ # get points in the convex frame
769
+ plane_pos = wp .transpose (convex .rot ) @ (plane .pos - convex .pos )
770
+ n = wp .transpose (convex .rot ) @ plane .normal
771
+
772
+ # Find support points
773
+ max_support = wp .float32 (- _HUGE_VAL )
774
+ for i in range (convex .vertnum ):
775
+ support = wp .dot (plane_pos - convex .vert [convex .vertadr + i ], n )
776
+
777
+ max_support = wp .max (support , max_support )
778
+
779
+ threshold = wp .max (0.0 , max_support - 1e-3 )
780
+
781
+ # Store indices in vec4
782
+ indices = wp .vec4i (- 1 , - 1 , - 1 , - 1 )
783
+
784
+ # TODO(team): Explore faster methods like tile_min or even fast pass kernels if the upper bound of vertices in all convexes is small enough such that all vertices fit into shared memory
785
+ # Find point a (first support point)
786
+ a_dist = wp .float32 (- _HUGE_VAL )
787
+ for i in range (convex .vertnum ):
788
+ support = wp .dot (plane_pos - convex .vert [convex .vertadr + i ], n )
789
+ dist = wp .where (support > threshold , 0.0 , - _HUGE_VAL )
790
+ if dist > a_dist :
791
+ indices [0 ] = i
792
+ a_dist = dist
793
+ a = convex .vert [convex .vertadr + indices [0 ]]
794
+
795
+ # Find point b (furthest from a)
796
+ b_dist = wp .float32 (- _HUGE_VAL )
797
+ for i in range (convex .vertnum ):
798
+ support = wp .dot (plane_pos - convex .vert [convex .vertadr + i ], n )
799
+ dist_mask = wp .where (support > threshold , 0.0 , - _HUGE_VAL )
800
+ dist = wp .length_sq (a - convex .vert [convex .vertadr + i ]) + dist_mask
801
+ if dist > b_dist :
802
+ indices [1 ] = i
803
+ b_dist = dist
804
+ b = convex .vert [convex .vertadr + indices [1 ]]
805
+
806
+ # Find point c (furthest along axis orthogonal to a-b)
807
+ ab = wp .cross (n , a - b )
808
+ c_dist = wp .float32 (- _HUGE_VAL )
809
+ for i in range (convex .vertnum ):
810
+ support = wp .dot (plane_pos - convex .vert [convex .vertadr + i ], n )
811
+ dist_mask = wp .where (support > threshold , 0.0 , - _HUGE_VAL )
812
+ ap = a - convex .vert [convex .vertadr + i ]
813
+ dist = wp .abs (wp .dot (ap , ab )) + dist_mask
814
+ if dist > c_dist :
815
+ indices [2 ] = i
816
+ c_dist = dist
817
+ c = convex .vert [convex .vertadr + indices [2 ]]
818
+
819
+ # Find point d (furthest from other triangle edges)
820
+ ac = wp .cross (n , a - c )
821
+ bc = wp .cross (n , b - c )
822
+ d_dist = wp .float32 (- _HUGE_VAL )
823
+ for i in range (convex .vertnum ):
824
+ support = wp .dot (plane_pos - convex .vert [convex .vertadr + i ], n )
825
+ dist_mask = wp .where (support > threshold , 0.0 , - _HUGE_VAL )
826
+ ap = a - convex .vert [convex .vertadr + i ]
827
+ bp = b - convex .vert [convex .vertadr + i ]
828
+ dist_ap = wp .abs (wp .dot (ap , ac )) + dist_mask
829
+ dist_bp = wp .abs (wp .dot (bp , bc )) + dist_mask
830
+ if dist_ap + dist_bp > d_dist :
831
+ indices [3 ] = i
832
+ d_dist = dist_ap + dist_bp
833
+
834
+ # Write contacts
835
+ frame = make_frame (plane .normal )
836
+ for i in range (3 , - 1 , - 1 ):
837
+ idx = indices [i ]
838
+ count = int (0 )
839
+ for j in range (i + 1 ):
840
+ if indices [j ] == idx :
841
+ count = count + 1
842
+
843
+ # Check if the index is unique (appears exactly once)
844
+ if count == 1 :
845
+ pos = convex .vert [convex .vertadr + idx ]
846
+ pos = convex .pos + convex .rot @ pos
847
+ support = wp .dot (plane_pos - convex .vert [convex .vertadr + idx ], n )
848
+ dist = - support
849
+ pos = pos - 0.5 * dist * plane .normal
850
+ write_contact (
851
+ nconmax_in ,
852
+ dist ,
853
+ pos ,
854
+ frame ,
855
+ margin ,
856
+ gap ,
857
+ condim ,
858
+ friction ,
859
+ solref ,
860
+ solreffriction ,
861
+ solimp ,
862
+ geoms ,
863
+ worldid ,
864
+ ncon_out ,
865
+ contact_dist_out ,
866
+ contact_pos_out ,
867
+ contact_frame_out ,
868
+ contact_includemargin_out ,
869
+ contact_friction_out ,
870
+ contact_solref_out ,
871
+ contact_solreffriction_out ,
872
+ contact_solimp_out ,
873
+ contact_dim_out ,
874
+ contact_geom_out ,
875
+ contact_worldid_out ,
876
+ )
877
+
878
+
727
879
@wp .func
728
880
def sphere_cylinder (
729
881
# Data in:
@@ -1734,6 +1886,7 @@ def _primitive_narrowphase(
1734
1886
geom_gap : wp .array (dtype = float ),
1735
1887
mesh_vertadr : wp .array (dtype = int ),
1736
1888
mesh_vertnum : wp .array (dtype = int ),
1889
+ mesh_vert : wp .array (dtype = wp .vec3 ),
1737
1890
pair_dim : wp .array (dtype = int ),
1738
1891
pair_solref : wp .array (dtype = wp .vec2 ),
1739
1892
pair_solreffriction : wp .array (dtype = wp .vec2 ),
@@ -1794,20 +1947,24 @@ def _primitive_narrowphase(
1794
1947
worldid = collision_worldid_in [tid ]
1795
1948
1796
1949
geom1 = _geom (
1950
+ geom_type ,
1797
1951
geom_dataid ,
1798
1952
geom_size ,
1799
1953
mesh_vertadr ,
1800
1954
mesh_vertnum ,
1955
+ mesh_vert ,
1801
1956
geom_xpos_in ,
1802
1957
geom_xmat_in ,
1803
1958
worldid ,
1804
1959
g1 ,
1805
1960
)
1806
1961
geom2 = _geom (
1962
+ geom_type ,
1807
1963
geom_dataid ,
1808
1964
geom_size ,
1809
1965
mesh_vertadr ,
1810
1966
mesh_vertnum ,
1967
+ mesh_vert ,
1811
1968
geom_xpos_in ,
1812
1969
geom_xmat_in ,
1813
1970
worldid ,
@@ -1953,6 +2110,33 @@ def _primitive_narrowphase(
1953
2110
contact_geom_out ,
1954
2111
contact_worldid_out ,
1955
2112
)
2113
+ elif type1 == int (GeomType .PLANE .value ) and type2 == int (GeomType .MESH .value ):
2114
+ plane_convex (
2115
+ nconmax_in ,
2116
+ geom1 ,
2117
+ geom2 ,
2118
+ worldid ,
2119
+ margin ,
2120
+ gap ,
2121
+ condim ,
2122
+ friction ,
2123
+ solref ,
2124
+ solreffriction ,
2125
+ solimp ,
2126
+ geoms ,
2127
+ ncon_out ,
2128
+ contact_dist_out ,
2129
+ contact_pos_out ,
2130
+ contact_frame_out ,
2131
+ contact_includemargin_out ,
2132
+ contact_friction_out ,
2133
+ contact_solref_out ,
2134
+ contact_solreffriction_out ,
2135
+ contact_solimp_out ,
2136
+ contact_dim_out ,
2137
+ contact_geom_out ,
2138
+ contact_worldid_out ,
2139
+ )
1956
2140
elif type1 == int (GeomType .SPHERE .value ) and type2 == int (GeomType .CAPSULE .value ):
1957
2141
sphere_capsule (
1958
2142
nconmax_in ,
@@ -2110,6 +2294,7 @@ def primitive_narrowphase(m: Model, d: Data):
2110
2294
m .geom_gap ,
2111
2295
m .mesh_vertadr ,
2112
2296
m .mesh_vertnum ,
2297
+ m .mesh_vert ,
2113
2298
m .pair_dim ,
2114
2299
m .pair_solref ,
2115
2300
m .pair_solreffriction ,
0 commit comments