@@ -84,7 +84,6 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
84
84
}
85
85
86
86
87
-
88
87
// ccd center function
89
88
void mjccd_center (const void * obj , ccd_vec3_t * center ) {
90
89
mjc_center (center -> v , (const mjCCDObj * ) obj );
@@ -114,7 +113,6 @@ void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
114
113
}
115
114
116
115
117
-
118
116
// prism center function
119
117
static void mjc_prism_center (mjtNum res [3 ], const mjCCDObj * obj ) {
120
118
// compute mean
@@ -126,7 +124,6 @@ static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) {
126
124
}
127
125
128
126
129
-
130
127
// ccd prism center function
131
128
static void mjccd_prism_center (const void * obj , ccd_vec3_t * center ) {
132
129
mjc_prism_center (center -> v , (const mjCCDObj * ) obj );
@@ -143,7 +140,6 @@ static inline void mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum
143
140
}
144
141
145
142
146
-
147
143
// transform a vector from local to global frame
148
144
static inline void localToGlobal (mjtNum res [3 ], const mjtNum mat [9 ], const mjtNum dir [3 ],
149
145
const mjtNum pos [3 ]) {
@@ -157,7 +153,6 @@ static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNu
157
153
}
158
154
159
155
160
-
161
156
// point support function
162
157
void mjc_pointSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
163
158
const mjtNum * pos = obj -> data -> geom_xpos + 3 * obj -> geom ;
@@ -167,7 +162,6 @@ void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
167
162
}
168
163
169
164
170
-
171
165
// sphere support function
172
166
static void mjc_sphereSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
173
167
const mjModel * m = obj -> model ;
@@ -183,7 +177,6 @@ static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3])
183
177
}
184
178
185
179
186
-
187
180
// line support function (capsule)
188
181
void mjc_lineSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
189
182
const mjModel * m = obj -> model ;
@@ -208,7 +201,6 @@ void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
208
201
}
209
202
210
203
211
-
212
204
// capsule support function
213
205
static void mjc_capsuleSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
214
206
const mjModel * m = obj -> model ;
@@ -238,7 +230,6 @@ static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]
238
230
}
239
231
240
232
241
-
242
233
// ellipsoid support function
243
234
static void mjc_ellipsoidSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
244
235
const mjModel * m = obj -> model ;
@@ -278,7 +269,6 @@ static void mjc_ellipsoidSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[
278
269
}
279
270
280
271
281
-
282
272
// cylinder support function
283
273
static void mjc_cylinderSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
284
274
const mjModel * m = obj -> model ;
@@ -311,7 +301,6 @@ static void mjc_cylinderSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3
311
301
}
312
302
313
303
314
-
315
304
// box support function
316
305
static void mjc_boxSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
317
306
const mjModel * m = obj -> model ;
@@ -343,14 +332,12 @@ static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
343
332
}
344
333
345
334
346
-
347
335
// dot product between mjtNum and float
348
336
static inline mjtNum dot3f (const mjtNum a [3 ], const float b [3 ]) {
349
337
return a [0 ]* (mjtNum )b [0 ] + a [1 ]* (mjtNum )b [1 ] + a [2 ]* (mjtNum )b [2 ];
350
338
}
351
339
352
340
353
-
354
341
// mesh support function via exhaustive search
355
342
static void mjc_meshSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
356
343
const mjModel * m = obj -> model ;
@@ -398,7 +385,6 @@ static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
398
385
}
399
386
400
387
401
-
402
388
// mesh support function via hill climbing
403
389
static void mjc_hillclimbSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
404
390
const mjModel * m = obj -> model ;
@@ -449,7 +435,6 @@ static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[
449
435
}
450
436
451
437
452
-
453
438
// prism support function
454
439
static void mjc_prism_support (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
455
440
int istart , ibest ;
@@ -471,7 +456,6 @@ static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3])
471
456
}
472
457
473
458
474
-
475
459
// flex support function
476
460
static void mjc_flexSupport (mjtNum res [3 ], mjCCDObj * obj , const mjtNum dir [3 ]) {
477
461
const mjModel * m = obj -> model ;
@@ -512,7 +496,6 @@ static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
512
496
}
513
497
514
498
515
-
516
499
// libccd support function
517
500
void mjccd_support (const void * _obj , const ccd_vec3_t * _dir , ccd_vec3_t * vec ) {
518
501
mjCCDObj * obj = (mjCCDObj * )_obj ;
@@ -720,7 +703,6 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
720
703
}
721
704
722
705
723
-
724
706
// libccd prism support function
725
707
static void mjccd_prism_support (const void * obj , const ccd_vec3_t * dir , ccd_vec3_t * vec ) {
726
708
mjc_prism_support (vec -> v , (mjCCDObj * ) obj , dir -> v );
@@ -784,7 +766,6 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt
784
766
}
785
767
786
768
787
-
788
769
// set flex data for CCD object
789
770
static void mjc_setCCDObjFlex (mjCCDObj * obj , int flex , int elem , int vert ) {
790
771
obj -> flex = flex ;
@@ -793,7 +774,6 @@ static void mjc_setCCDObjFlex(mjCCDObj* obj, int flex, int elem, int vert) {
793
774
}
794
775
795
776
796
-
797
777
// initialize CCD structure
798
778
static void mjc_initCCD (ccd_t * ccd , const mjModel * m ) {
799
779
CCD_INIT (ccd );
@@ -803,7 +783,6 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
803
783
}
804
784
805
785
806
-
807
786
// find convex-convex collision
808
787
static int mjc_CCDIteration (const mjModel * m , const mjData * d , mjCCDObj * obj1 , mjCCDObj * obj2 ,
809
788
mjContact * con , int max_contacts , mjtNum margin ) {
@@ -873,7 +852,6 @@ static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, m
873
852
}
874
853
875
854
876
-
877
855
// compare new contact to previous contacts, return 1 if it is far from all of them
878
856
static int mjc_isDistinctContact (mjContact * con , int ncon , mjtNum tolerance ) {
879
857
for (int i = 0 ; i < ncon - 1 ; i ++ ) {
@@ -885,7 +863,6 @@ static int mjc_isDistinctContact(mjContact* con, int ncon, mjtNum tolerance) {
885
863
}
886
864
887
865
888
-
889
866
// in-place rotation of spatial frame around given point of origin
890
867
static void mju_rotateFrame (const mjtNum origin [3 ], const mjtNum rot [9 ],
891
868
mjtNum xmat [9 ], mjtNum xpos [3 ]) {
@@ -907,7 +884,6 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
907
884
}
908
885
909
886
910
-
911
887
// return number of contacts supported by a single pass of narrowphase
912
888
static int maxContacts (const mjCCDObj * obj1 , const mjCCDObj * obj2 ) {
913
889
const mjModel * m = obj1 -> model ;
@@ -936,7 +912,6 @@ static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) {
936
912
}
937
913
938
914
939
-
940
915
// multi-point convex-convex collision, using libccd
941
916
int mjc_Convex (const mjModel * m , const mjData * d ,
942
917
mjContact * con , int g1 , int g2 , mjtNum margin ) {
@@ -1024,7 +999,6 @@ int mjc_Convex(const mjModel* m, const mjData* d,
1024
999
}
1025
1000
1026
1001
1027
-
1028
1002
// parameters for plane-mesh extra contacts
1029
1003
const int maxplanemesh = 3 ;
1030
1004
const mjtNum tolplanemesh = 0.3 ;
@@ -1063,7 +1037,6 @@ static int addplanemesh(mjContact* con, const float vertex[3],
1063
1037
}
1064
1038
1065
1039
1066
-
1067
1040
// plane-convex collision, using libccd
1068
1041
int mjc_PlaneConvex (const mjModel * m , const mjData * d ,
1069
1042
mjContact * con , int g1 , int g2 , mjtNum margin ) {
@@ -1164,7 +1137,6 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
1164
1137
}
1165
1138
1166
1139
1167
-
1168
1140
//---------------------------- heightfield collisions ---------------------------------------------
1169
1141
1170
1142
// ccd prism first dir
@@ -1191,7 +1163,6 @@ static void addVert(int* nvert, mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) {
1191
1163
}
1192
1164
1193
1165
1194
-
1195
1166
// entry point for heightfield collisions
1196
1167
int mjc_ConvexHField (const mjModel * m , const mjData * d ,
1197
1168
mjContact * con , int g1 , int g2 , mjtNum margin ) {
@@ -1379,7 +1350,6 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
1379
1350
}
1380
1351
1381
1352
1382
-
1383
1353
//--------------------------- fix contact frame normal ---------------------------------------------
1384
1354
1385
1355
// compute normal for point outside ellipsoid, using ray-projection SQP
@@ -1437,7 +1407,6 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum
1437
1407
}
1438
1408
1439
1409
1440
-
1441
1410
// compute normal for point inside ellipsoid, using diagonal QCQP
1442
1411
static int mjc_ellipsoidOutside (mjtNum nrm [3 ], const mjtNum pos [3 ], const mjtNum size [3 ]) {
1443
1412
// algorithm constants
@@ -1487,7 +1456,6 @@ static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum
1487
1456
}
1488
1457
1489
1458
1490
-
1491
1459
// entry point
1492
1460
void mjc_fixNormal (const mjModel * m , const mjData * d , mjContact * con , int g1 , int g2 ) {
1493
1461
mjtNum dst1 , dst2 ;
@@ -1637,7 +1605,6 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
1637
1605
}
1638
1606
1639
1607
1640
-
1641
1608
//---------------------------- flex collisions ---------------------------------------------
1642
1609
1643
1610
// geom-elem or elem-elem or vert-elem convex collision using ccd
@@ -1654,7 +1621,6 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
1654
1621
}
1655
1622
1656
1623
1657
-
1658
1624
// test a height field and a flex element for collision
1659
1625
int mjc_HFieldElem (const mjModel * m , const mjData * d , mjContact * con ,
1660
1626
int g , int f , int e , mjtNum margin ) {
0 commit comments