Distance between capsules with GJK

0.7402845970292322
0.6899528988171484
1.8722143295474376
1.701608875672425
1.5805315050616324
1.4584705080882492
0.9280829435360647
1.752718927828596
0.13955418695181787
0.6835375402457197
1.9871249594849303
1.3922690022534436
1.1145649317397284
1.8700808018296013
1.5189759723734837
1.232621498043899
0.6980132269993252
0.7930465506705408
0.37882023815715665
0.5381729743936885
1.5808038088378744
1.2005632717526376
1.1762680331424595
2.4401824166572297
1.5500676264179636
1.794933965432824
1.128351540350999
0.0
1.3931095010087522
0.0
0.8807008451270784
0.362442169029888
0.7192805343894937
2.024463268269886
0.04518883641225439
0.17627636242806893
1.2219493663347796
1.3929740161887085
1.014020874978383
1.4645791822418002
0.4322759310469517
0.161437931381424
1.544108285383706
1.4441074486670011
1.541726380580072
0.6498958608038538
0.8930889922893182
0.3556540648490598
0.9722871813631464
0.9566296707249667
1.5443967602774105
1.0235232071554339
1.1675976754295672
0.4555707568572217
1.0242946756791755
1.7659792223936193
0.9085446768913222
0.38786930203774234
0.0
0.4324131915910023
0.873625827752726
2.1580377226708234
1.1259242385693629
2.2026887228585537
1.000439204945847
0.831679111054146
2.2321334183247146
1.6560078604040684
1.9612435574028846
2.4341084743388226
0.23573256248050325
0.9999061698730415
0.10832490181587517
1.0823434457523344
0.28639338477857834
0.0
2.193355661020803
0.10311018514976136
0.7187557091378648
1.582901222308271
1.6305479060391475
0.0
1.1385046059356179
0.0
0.8904541762414122
0.1344013797296231
0.12427712546935517
0.6848308707222273
1.5604303493991907
1.0041185928193805
2.755250179611987
1.3647366831116359
1.776236836221232
0.4807139549322095
0.543261696406851
0.2976438084052161
0.8070211452642202
0.5638513755351592
0.5937630345664017
1.9057145776597257
1.607942470554766
0.7698911221184166
1.4025700190176953
1.3477888131189681
0.9743943554485546
0.7275687725162051
0.3801777561028484
0.973023113646331
0.016302225535756658
1.3197864795998209
0.1948893291897454
1.6704434925302356
2.7381343085185765
0.625518821775254
0.21162067349998992
0.7207341029869959
1.4418500921531499
0.5777421723330497
1.9488541157229635
0.7718250224351983
2.019669938881417
2.2586567066487104
0.9242756253332924
0.6663471028208399
1.3156185499240274
0.31202381231145393
0.8305025305065149
0.056307177379073176
0.8104356976336993
1.0073396064210507
0.41654744988559506
1.0041585003873201
0.0
0.022820323468639995
1.747244854825795
1.6111516116306575
1.3261037133439948
0.6913530326090089
0.7648458404629621
1.502421656058984
0.2987363551285769
0.309996824545668
1.7604209978478804
1.2064577128636573
1.2444134170061312
1.848365858174893
1.0817638208295637
0.5538257573408474
0.7846937897414481
1.1218643343726802
2.317384978828612
0.5341235602165636
0.0
1.4666104187138638
1.8958922430167124
0.0
0.35327738959747035
1.6727981506864802
0.9560261215545366
1.9162145792720933
0.588542299796228
0.24669886982643982
0.0
0.6826668798568117
1.0289339706674048
1.6424268128282247
1.4217723161143543
3.4078820616274315
2.022599257702819
2.7366393994895715
1.1790562135813412
0.6168885029593726
1.0164801332579187
1.4431961811867644
0.0
0.7541189987850605
0.9294400081779599
0.9381919270232113
0.6719284857118679
1.6302096490758806
0.6237763514512394
2.7794599675207694
0.6185985726603466
0.5131182906610953
1.120047338793527
1.0554048126802125
0.770950294159239
1.8432763799166805
1.8862859386594697
1.1642695462843435
0.5985173196774268
1.3901794539987102
1.4544450684403283
0.17099697714426923
0.1953600168548149
0.6983897106119737
0.18431682639517633
0.47717225653869616
1.31888272130682
1.6439123193598126
2.0262898177202673
0.41695613602616644
1.5155141138701975
0.17457123548503872
1.4711702561507216
0.952884674330747
1.0176169722463941
0.4544829851313124
0.8531236711075791
0.9525786147842579
1.350354434637688
1.1636903702400034
1.7539370189830659
0.7323903867955905
0.0
1.291847855150363
0.7153525840295123
0.961962122816061
0.0
0.6652835573502643
0.7854849274009237
1.3232525444983272
2.2162190724279007
1.4566221728939917
0.8904583823939006
1.512681805237175
2.378240429981777
0.13811916398611407
1.656950558689386
0.28236812680653006
0.6329961317897546
1.0972115579420132
2.408006945398817
1.2082525300907623
1.1828539424503188
0.15566705142837112
0.9264732503657332
0.6023232687136347
1.1598089369208422
1.8191313616190237
0.09978945403138434
0.693960596318023
1.2269266018322578
0.7336280307786005
1.0207136106839503
1.0430420650287613
1.057518246323743
0.972685122949321
0.21007252205970003
0.5362551134019043
1.1934890679487646
1.2251990209079586
2.0155639334246778
1.4741792745080593
0.0
0.7400422382604844
1.6816628869471177
1.1657026391558287
0.6321310707323603
1.438271750005818
0.35008578739652674
1.5593009549294792
0.4751498204566611
0.4123771055244901
0.5317440993488057
0.5962984818819167
1.3554280270569141
1.3449385060121206
0.1823001987256488
0.3321181647921169
0.005074183626617116
1.2401379061871967
0.7691843632264559
0.3573560335708849
2.0046961274930095
1.4201122629282106
0.21233397735446247
1.1455288068161162
0.13808912759981226
0.7415232415297772
0.7449560863977515
0.4610112158802144
1.7259094441786866
1.0202057298575697
0.5333655290237387
0.3853834813968412
1.4158934478934746
0.9803152503747353
1.5339995153248616
1.2312122169762711
0.7872500820536228
1.5292345068117061
0.6640950462020193
1.6570515781433397
1.9975797592516933
0.8187993461662754
2.146359759277757
1.9362909037748006
0.38816984280101335
2.6956379877408394
1.7517850662254946
0.29386419847768247
1.2077850724299966
1.0314170277967212
1.5368817460592128
0.542440572544119
0.8273331331229056
1.170433316375224
0.5013995524117055
0.6597453511856858
0.0
1.3453425402519101
2.1256729515582378
0.7099761914516385
1.1961265022148604
0.9845629616524385
1.731832243896008
0.7264823246475416
0.5868896619361433
0.8556925652187828
0.9980161886614182
1.0187564232361486
1.4936749597602983
0.36634498840114305
0.6744523861888994
0.8858148128150035
2.4228966529249254
1.9257331366432702
1.213747379860306
0.35909225496276026
0.6518208972768098
0.7202838220409119
0.38232170427708595
1.5920091665864562
0.48078088309411793
1.4424750025297357
0.8644475012141817
0.15113433429527148
0.4097424388915593
1.4848787966452304
1.717241833816778
2.3400983820085486
1.8533107076020445
1.1546242279383196
1.4251629994572266
1.592372071399173
0.8444259503056145
1.1852691392965928
1.2098987201760638
1.3291267017102173
0.35447160624931995
1.7228107120222527
1.0869407411345866
1.952180020589275
1.4463735083431604
0.3782649543959794
0.2557706714201729
1.912553201052779
0.2214748083395675
1.5257060571402719
1.2179183463532506
1.130487179648514
1.1199217631199045
1.798823508568519
0.641766939793432
1.6355987516466521
0.4765623235779339
0.9306120749847173
0.8190000504662268
0.916613298058171
0.06890412772635565
1.115765378001537
0.0
0.2733492139482952
0.4481433803067477
1.1298142290712032
0.2985872458071974
1.244576631750883
1.6468207027320845
0.6629809437139895
1.7587950177406477
0.04870709667520183
1.0190941460084286
1.4162882178259677
1.016825300555525
0.9724821517503298
0.31738125110527343
0.5673393905735634
1.2781333863710769
1.0444145529360098
0.6193501404141951
0.36959318716061124
1.1880757514444058
2.2793314283215484
1.4950117219179133
1.2790333724458358
0.1622865420948929
1.2442543407678945
2.3798931354166055
1.3850135635949417
1.015958372877245
0.0
1.1698989781077798
0.7089390861854938
1.4333890391995157
0.9048953955301163
2.0103790519649696
0.9094613938701592
0.7568784592787591
1.1256257865120358
0.5251607602840122
0.6802012420420912
0.6982140572638409
0.9346634270384067
0.5696257476608115
0.05422754644168572
1.6253755452267395
1.3991364043140004
0.8321981978180456
0.8079902476206658
0.785252903720796
2.441375470086921
0.9677049715532141
0.8635198243724624
1.21169702707886
2.1377889923770943
1.6762064806737726
0.9775494393983511
0.43653298701528226
1.3666133243919962
1.9722785237299072
1.3070624834774214
1.7549266195235105
1.0553172659585301
2.1732217256274398
0.5211934916481125
3.2297888109400494
1.5055509946626655
0.4858873224483856
0.46744437752253537
1.287004328497033
0.22528150794831373
0.8214366797181407
2.077172549055627
1.8745067175950811
0.5840004252443258
1.1226011465521344
0.3824543637480974
1.0112923862536543
1.0075879494716153
1.4089285075601063
1.57252588328281
1.3166236735836916
0.677938480270403
0.8126166569758385
1.5126479951306069
1.4511967681003495
0.09663183453987867
0.5620473803499468
1.0889822484749083
1.6043926639305122
1.4601407553789576
1.9388485230055637
1.5113014834115681
1.8211006384942385
1.7448085409553973
1.7026108464010172
0.5832426311361433
0.4095889288323308
1.241197292466812
0.4254873273585797
2.2663914635380165
0.7293747293345593
2.1042119888806545
1.4140514770388475
1.9839334463850087
1.2865297996458562
2.2509799325485953
1.1948757453493533
0.0
0.21005724092926212
0.6833195202546768
0.9415709192950079
3.39945199863648
1.2523963338899604
0.3460331517893734
1.4079360457433852
0.51095830308995
0.9780987935681968
0.7404194897245091
2.4933618591799798
1.3975668957787284
0.7594401875393284
0.31209627597845796
0.6137542722393199
0.752124070133284
1.2850732556059496
2.13476035782286
1.4881168481886007
0.6217151296609911
0.5826215282361878
0.5461294375183872
0.49072552757927745
0.12999407545708005
0.8541892216613599
2.9341628164333793
1.6453541536371874
1.0889651142197745
2.43080000545082
0.04868039661276321
0.5646208869594589
1.3313480418963564
1.5307231287729965
2.462746445695573
1.0236570679537647
0.0
1.9452384473749027
0.0
1.051073824064001
1.3581098353820658
0.437954178080042
0.7235405888140628
1.1225522096744114
0.8088629614945407
0.8429463788520635
1.5359848351217986
2.1485252956778957
0.8732059816336541
1.306129058301149
2.220725903700367
0.6105878835920282
1.6876022208762025
0.8443964614173598
0.7328461463879681
1.7436294853207701
1.5769932644434697
0.6236851973319362
1.3620720715568493
2.2665166305441304
1.8162929109403565
1.8894374964416452
1.410134473456218
0.6152662213604102
1.1219100337913928
1.1039695896438395
2.049142818498531
0.6050549305215581
0.4962013508460702
1.243570613460129
0.6699437482216472
1.0919592994948417
0.977746773230277
1.0855322421238758
1.6367236154019444
0.2868968653549263
1.0210647279364873
0.6559952523258693
1.173706975069909
1.441471424004649
1.0551049780328092
0.27606728657718216
1.592346263775144
0.0
1.2758990906230308
0.0
1.7951702023763827
0.43445433421029556
0.5866408910232866
0.5486806147916741
1.7367661047763936
0.1783672181630963
0.6204560693893423
0.8079613979116131
1.743124518302217
1.5450783295697068
0.2974971019373213
0.0
1.9015867225216856
1.786789408158634
0.06279376727529883
1.0058807042521272
1.124824322067207
0.7857804972436754
1.215195609281008
2.834597659556243
0.21964221493332142
0.2731281750472263
0.0
0.6680013783338051
0.710661712269119
0.6994343545565306
1.3904851048160127
0.46824843653652237
0.1453273971156568
1.3243153879378724
1.150056858784734
0.4599698354150138
0.5634444816095554
0.9493157677488766
0.6062728413722986
0.5592310447396156
0.8844861054377695
2.3452915077801952
0.7259877359954009
1.1457078526149007
0.9013296032033729
1.0427141376279734
1.140747943468164
1.9370761786730375
0.3934292485775954
0.42734629900407095
1.9007893598247438
1.5646653559534647
1.4502805252601412
1.2762189600536071
1.5706632152080737
1.4721429869548044
1.4492455114614922
1.1818052094490923
1.1002348695150268
1.3321919575875714
0.0
0.4265114679414013
2.1681514774992854
0.7903326266008228
1.0427866532825838
0.15330948834318228
2.054963993022826
0.666485772019076
1.0866242832780537
1.8004712855910148
1.302185824167551
0.9463024035332936
1.092924416774369
0.5724573685660178
0.9974516713832675
1.1405212664215085
0.7057025828179676
2.1520061850003103
0.5309970866340274
0.5512018465237916
0.6473775288609241
0.7547111109402272
0.7467355369915648
1.5953641106206642
1.1385056540604312
1.2254080382818078
0.0
0.8676996374408308
0.6806599774304035
1.6578182335846234
0.7182054935211525
1.0681676031324596
0.6512840291657039
2.10281223987869
1.901388941795664
0.6782016633486296
0.26092017757708746
0.2797257658437407
0.3594980385013211
1.8907130622834254
1.8861973967429615
0.9754653179411121
1.0691330981956875
2.215633039533811
0.7064552817112896
0.43674101442586144
0.31234017849182455
1.220457350288604
0.0
0.5887583771869179
0.9981259396863559
1.5239161885466843
1.6838500152480205
0.836082930324385
2.3654292977550253
1.162997128218803
0.785157039887018
1.0302720246429171
0.8249392582710091
1.448483385923939
0.9894079836597477
2.370265666491182
0.6330533216046811
0.7333955356370304
0.0
0.7408398763231725
2.219686473192943
0.9226144590704869
0.8169814323817349
0.6275537467948237
2.103829698298982
1.6356873932976264
1.1620901409178455
1.7746191984524986
1.4641385087959813
1.5554279903502988
1.5401198953241249
0.7190320446978254
1.2567840367135763
accumulated_time=0.10025691986083984

print(__doc__)
import time
import numpy as np
import matplotlib.pyplot as plt
import pytransform3d.plot_utils as ppu
import pytransform3d.transformations as pt
from distance3d import gjk, colliders
from distance3d import random
from distance3d import plotting


random_state = np.random.RandomState(0)
capsule2origin, radius, height = random.rand_capsule(random_state, 0.2, 0.4, 1.0)

ax = ppu.make_3d_axis(ax_s=2)

accumulated_time = 0.0
for i in range(700):
    capsule2origin2, radius2, height2 = random.rand_capsule(random_state, 1.0, 0.3, 1.0)
    start = time.time()
    c1 = colliders.Capsule(capsule2origin, radius, height)
    c2 = colliders.Capsule(capsule2origin2, radius2, height2)
    dist, closest_point_capsule, closest_point_capsule2, _ = gjk.gjk(c1, c2)
    end = time.time()
    accumulated_time += end - start
    print(dist)
    if i > 5:
        continue
    plotting.plot_segment(
        ax, closest_point_capsule, closest_point_capsule2, c="k", lw=1)
    pt.plot_transform(ax=ax, A2B=capsule2origin2, s=0.1)
    ppu.plot_capsule(
        ax=ax, A2B=capsule2origin2, radius=radius2, height=height2,
        wireframe=False, alpha=0.5)
print(f"{accumulated_time=}")

pt.plot_transform(ax=ax, A2B=capsule2origin, s=0.1)
ppu.plot_capsule(
    ax=ax, A2B=capsule2origin, radius=radius, height=height, wireframe=False,
    alpha=0.5, color="yellow")
plt.show()