Distance between ellipsoids with GJK

1.352229930501766
3.759316469308127
0.12168404052298859
1.1683181142055468
1.3021829253073798
1.642507803529878
2.2850369516813087
1.8503855213884803
2.4227848525069007
2.797771003502515
2.523065760240443
2.984908815451367
3.1265244733938995
1.7058466310205096
2.154944395330565
1.377450644210651
2.5347151273953137
2.7433257546719676
1.374410678320646
2.0792114872752205
0.6010865646900021
1.943897409839882
1.1310587067851883
3.0219165339834286
0.9468961118317129
1.8134985766752239
1.6829483389433686
2.581611644307836
1.9844917074265234
2.0401170605400525
2.3557780167782587
1.025573443660311
1.1965899444225907
2.2971904464733663
1.0363108898377416
1.246712065385128
1.7561557270435277
1.9130742943706496
2.0871069017658255
1.0140131371212662
0.7814090759501973
2.172807370463525
0.9751936690713466
2.3014137578517704
3.005878556824738
0.9002046642439038
1.6742808970732153
0.6584555502869798
1.218789929600684
2.8105133663544546
0.34038989085339283
2.003137671614124
2.0162900674292046
2.9703446805475995
2.8399672934033346
1.1469075560189792
1.9517872945017927
1.398656601552406
3.0987303324944184
0.9573386421989311
2.795467108089688
1.752968549225944
0.42131610208060066
1.3690903422549883
1.838348207272637
3.7597957618375597
2.7398192765192917
3.255020590955924
1.508035662373805
3.487356180354514
2.096530787331767
1.3300333592046658
1.8434289452159214
2.5404004680719736
2.9564815661693213
0.6251625267065533
2.081415577354956
0.5477036696285041
1.8886441979918405
2.1259752754609713
4.220180878298143
0.4737346550815934
1.1438004982592354
1.658798536852585
2.55344771971857
1.9745710020062677
2.1835759193914703
2.117394234515123
3.2213683765214682
2.905070579313443
0.7987815773268431
0.7570689831086002
0.7924032982743079
0.0
2.69667000979644
1.029762864068788
2.791808622428965
0.943274196735492
1.4780335696102385
0.887384843607486
1.9649569050243885
2.033044960714868
1.2962654551975386
2.8623570523921984
1.5598224480890859
1.8728981260079591
1.9318683188890902
1.6244965227566692
0.9736871392859735
3.2867978567363023
2.0229060073463176
0.0
0.9447664682453037
1.9250871035603436
1.0969158811950555
1.7760851785533638
0.2714074863933493
1.3537356831062768
1.6849839279598877
2.7058042797952946
1.0269472235430819
2.0999960270403726
1.9607280209155107
0.774146374310119
1.7282648099323727
2.184900632104038
1.2521263191532892
1.42682240931724
2.1671679565464834
2.4183397687694144
1.9126992819452409
2.2882993554944817
0.19494054859869286
0.6681170995353858
2.3663483051175564
1.4348224655287436
2.5173557795299755
0.8611579478137396
2.6037744630244744
1.0834963007351377
1.7814567512210293
1.8717392058671087
1.3939995691534777
1.4117081698279808
1.005130291989355
2.2073605449486546
0.9143414725436301
0.47435641884335816
2.6148168193292416
2.55892262234356
3.0852577956903047
2.5731638183751806
3.3428301415434944
2.7358156610638797
1.0364604657296792
1.524443617598256
1.0887420058770305
1.6637168807657576
1.4660641790742739
0.45079073813785625
1.0324996824308086
3.029442810841856
0.8071995244812971
3.0492738978952576
1.5780228716814744
1.77764590761158
1.8379033977240804
2.1542683829281897
2.523692731087188
0.4123143754026582
1.2216796661343634
2.5218991441684384
1.362923822390875
1.5738494645999141
1.9638899037477753
1.3682329197230838
2.5166221205213466
1.2812984140808812
1.0051037556883788
1.673194331087156
2.7152246418361785
2.411946119743828
1.4744964186023497
1.7371656850121378
2.11603218022989
1.6810619389663153
0.6336450099333366
2.322713258583022
1.5289020307443941
0.6489574326886636
2.231716529764403
2.8648281993517934
2.6200272745658384
0.9233588951095139
0.9957627695598655
1.0036971836609918
2.0226633902718034
1.109034363254715
1.1878807889493745
1.1442921346354886
0.841162311421399
2.334707680736545
3.2484823396833287
2.991116822493881
0.8292632346076821
1.9657328618349008
1.938844871051829
1.124158122624007
0.4477299268607114
1.9393555438519976
1.852621142428191
2.312955496648937
0.0
2.893827092005361
1.4121803518696319
3.019341626311815
1.999154699999538
1.4372294598717248
2.0197217223834447
2.016538098788361
1.4445414427514334
0.6574922942558399
1.2749140037633666
2.2320225832705956
1.2864836475926908
2.2075750483246663
1.5316654330670052
0.7704685086308949
1.7540645711667526
2.4918323411174734
1.4327725977437091
1.7397818434172418
1.5804793392579555
2.3708703351862828
2.886256147435345
0.9289590082331378
3.7155850530286094
2.440701965709638
2.038653941817293
0.8421021906615304
0.5960719967214502
2.2343330049361088
1.1729619878988906
2.2286515743458035
1.861931871056486
1.0190232008260183
2.560721134963946
0.0
0.0
0.3718197046599311
2.441760878486876
0.8237438543643077
1.780280765063437
0.8088633950704688
1.8896200004052806
0.4308448357695765
3.1931106498151896
1.9191657124014188
3.0303888360808053
2.037089896129313
2.414204509344261
1.838589594070528
2.212451937223886
0.8527358865804618
3.590978793702016
1.3522472777329697
1.9153109305758065
1.5391604631452185
2.0886771947059937
0.9705163747779956
1.2456159382041547
1.2338956176486202
1.2016166472662115
1.2680370428244496
1.2591989820442948
1.4509209001505403
2.342215516668618
1.4316741586220185
1.563308181066332
2.216719198910922
1.3148118778666522
1.4377865131106382
2.878017277279205
0.3733503289077481
1.2884328558961453
1.0093268881545159
2.3043921616057523
1.7983661125614796
1.6211034743821682
1.1692058947837614
1.9074441901790176
1.62477684532865
1.5917179129095143
2.8723924774051377
0.33988603709868725
2.406847023935764
0.08257670001233404
1.0683891620508112
2.19447706344273
2.343251384074424
1.927057322635417
0.8743072072956501
1.3906090602314278
2.4312690990301857
1.7543379854604595
0.7934936118012331
2.4701367404431664
3.622636617676942
1.3810716816552213
3.2618422192047216
2.5205483085103806
2.524034867592262
2.3822345964058944
1.1830382972870392
2.1635377748596625
2.036693477235165
1.3999435788008077
3.032788520450711
1.6267684676313097
1.9579538608713145
0.3393006017040406
1.479739774937092
1.331763537183008
2.068569947185855
1.4476133862708183
2.5925531421157655
1.3161438195892827
1.1559975648813823
3.149535266279761
1.2153968707421754
2.9399175274826432
1.1917836397549253
0.4566128221022787
1.1661463025069867
0.0
0.4420551962312983
0.6150471418928879
1.3682712653399642
1.1918789013746642
2.5590253502401845
1.1982689268657083
2.0019313662509908
1.8257862000818113
0.41133184263847933
1.438219151595322
1.1543973677323727
0.0
1.604263367456795
1.803909173931443
0.10668146954099927
1.0322981745439421
1.197145052959775
0.0
2.2072471859540372
2.6081638640518303
1.7650554383442845
1.4772742377746664
1.1284020263178365
3.100392466465766
2.5194707016084315
2.0308261555078064
3.2818277278376766
1.5339355882934413
3.242723621011958
1.7945079839482179
0.963302776521739
2.4482970361661227
1.5593117797362663
2.082779674512381
1.6070804157283352
2.222260997474239
2.359117271951932
1.300153954397228
3.060420259419347
1.7993318225943091
1.3772654201228458
0.0
2.0416911343527957
1.1963433362725109
1.6500439555058728
2.9834445216634413
3.2022130879479693
1.520760611755225
1.6889088740019447
0.5572472095872585
2.087270535340162
2.6383428236799142
3.056994302736131
2.2889635925750187
0.2329822278915269
1.76147509273692
2.4257012170931977
2.171959849229493
2.8973473424605083
0.49209660340945655
2.195010208403686
1.961816800193874
1.013954000081113
1.9791390386712848
0.0
1.75381004413373
0.5897414704650584
1.5936856014172065
1.8254403744968264
0.940037347839962
1.1891257837304
2.0845117514408003
0.5182633327455295
1.6755550442634939
2.0898318932050324
2.828099218046479
2.7321008295694424
1.1265085868928715
4.165937259918252
3.237414147449475
1.1642090219992278
2.6946834590824458
2.2522732071843836
1.616664907767297
1.2669300170845967
2.531594423475801
3.211043999183848
2.0846478212908695
2.044638340521446
1.1378480918240454
1.5437110652745742
3.036376273814751
1.6370632869507924
1.4849762338182122
0.2067141416938325
0.9048110553274215
0.9500839737922068
2.000842300359423
3.964291527148839
0.2506866828449744
1.2667682965423654
2.0913719555036665
0.3418500491730953
2.261659180764788
4.149933606314709
1.3897358033800398
2.3924936082074626
0.5089339873583383
2.009776209403919
1.6561571376466477
0.7974831037419026
2.3404924469805515
2.3002708428087275
1.811521554481632
2.109987192881168
0.9302327613502323
1.2217598525439153
0.6553467012326
1.7978283486251456
2.31136262135099
0.9557419232036658
1.0880955558619132
2.2858511947724867
1.990701616691945
1.4706247137604052
1.1287507149777651
1.5080925321047922
1.9512930643744588
1.6126350057274512
3.52784249860838
2.5759490956336637
2.2452833043803526
0.35069391868784205
1.5421764892775955
1.171469920516051
2.1389338227056998
2.4727821912204684
1.127917138417532
1.12286204709107
1.7785184095799071
0.3555766715401959
1.6457974493558416
2.614757901574749
1.9293828570348432
1.4307239717296252
1.1321038740354383
3.0585323472058974
1.2403859356969669
1.7052104226469091
2.0728634235613623
2.6277613412519476
0.7136514791049976
1.5638406139148764
2.083621410791661
3.4577957077532124
3.360232554054202
2.844918614193717
2.37564044172072
2.085291697940784
1.3893569234554795
0.12516412485968928
2.3664832128033324
1.8299309208164287
0.4748208871121076
2.4660193499539553
1.4460318102541883
1.7609626156603775
1.2961529475926257
1.109968179365341
4.470401610827176
2.382734855941745
2.4063959039238036
2.358532313119885
0.6548684031538156
2.774378551681272
2.012648374924627
2.6087403776628735
0.3404854489804471
1.256012565417311
1.9942064194031364
3.0142416777613024
1.156998049333158
0.5166411810159404
2.005834971218023
1.627280619903616
2.412538177748294
2.083430251454856
2.544780647749043
2.145269237378766
1.3184588771205064
2.372302321910374
1.0138825085191205
2.830677113709716
1.1036923442824436
0.6072635772151836
0.0
2.7190351991967114
1.7905605065478167
3.1171267940078535
2.138136239956829
0.7936080602977285
0.0
2.183584716862579
1.2993299775162341
1.678150901170364
1.889600793551687
1.1057425079291583
1.2434234488065323
2.449798732473779
1.2460302167771653
1.9472922652938638
2.4271137569910173
3.697973745065138
1.6492004395936328
1.4988845796824595
3.3225101300834785
1.7575478546161347
1.5456755933284443
1.2735001473320313
1.9865027700503866
1.9660525313512447
2.6471015352305485
2.4400418670546453
3.3718865037390744
2.6794039159839835
1.7732858936280644
1.4388856091285656
0.4198263225175851
1.106547398004372
3.437003639344553
3.5537586550524773
2.5622067857393045
0.5862819160017194
1.2301812407600266
1.9199846230712696
1.8149678113625431
2.939163549346316
2.9248219063895857
2.460067785050186
3.1792581028447477
2.132405375705974
0.7098134978157025
2.0454607185109457
1.4944928381667495
2.805328837416319
2.40432927256394
2.3176424305004666
1.7781150278342053
1.2306873131172786
0.26881963720809415
1.4523224162559483
3.111910243787066
2.882311119314066
0.6778442675945299
2.9227834089977702
0.8287566678452147
1.2758273099860469
2.0151474243113077
3.107527522951178
1.906487449831835
1.5099009355679553
2.560003666136515
0.31841354923153586
0.6680145235374639
1.1777083353387214
0.9482481111550445
0.9008510585757693
0.8430684856956429
0.8028683259182768
0.4993817272993435
1.6731749030948806
1.1870411483011958
0.6925300352345377
2.2751881009837063
1.950953182200207
2.1387558391525756
2.17344924648091
2.4770468474570504
1.30081690973782
2.818292261216971
4.2901400148450195
1.1998556373170541
2.320100643810896
0.8400954439361837
1.8886009348496313
1.9617676519554472
1.0349710797684648
0.9685783149984445
1.863540012314957
0.5261540428835765
2.5771173764772968
1.9518107772572433
2.074131043208006
1.9663706571597852
0.5202443687884106
1.6358862124404183
2.6948509243169605
1.358197352498408
2.736877675762007
0.8488883128333217
1.6482586170723441
1.777334258214363
1.1743797815453865
1.6484859920900412
2.6040330685852426
0.3618018811025928
2.123978772379483
0.4713883522773268
1.305891152905718
1.3103110558740052
2.21458266075921
0.31067322373027995
1.6460095891476076
1.436555288737034
3.6265524700783627
2.646550308015185
2.99634280129843
0.3812753173873282
0.9617992026236497
1.1174084370063542
2.061926625423428
2.7877493986849706
0.661324472381191
0.39738201941834694
2.1948817338141495
2.5889132954237266
2.103618356421809
2.414136704542483
2.6119857206106722
0.6970822104609231
2.4382799769202332
2.878558597234556
1.099362250804814
1.121360731899838
1.898406022028858
1.7866071742957754
1.3710094475492638
2.3628624502519013
0.9885494368091713
2.0390547660392895
2.2653618074822566
3.769594345351315
1.7885999727926785
1.0858399229966929
1.9503658345758241
3.851310223913691
2.5775979542791956
1.4435977648743417
0.43694873616314694
1.12855016786621
1.1966215820742976
0.8140986163377143
0.32824392547465836
1.6443772881466279
2.0172433364421165
2.5041141316469875
3.340277900171072
0.4397773546010074
1.9428884721466526
1.1652332737660906
0.34328880217997343
3.426120452019834
0.9290610815688629
1.0364247313468165
2.2397924050735596
accumulated_time=0.12277603149414062

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


random_state = np.random.RandomState(0)
ellipsoid2origin, radii = random.rand_ellipsoid(random_state, min_radius=0.2)

ax = ppu.make_3d_axis(ax_s=2)

accumulated_time = 0.0
for i in range(700):
    ellipsoid2origin2, radii2 = random.rand_ellipsoid(random_state, min_radius=0.2)
    start = time.time()
    c1 = colliders.Ellipsoid(ellipsoid2origin, radii)
    c2 = colliders.Ellipsoid(ellipsoid2origin2, radii2)
    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)
    ppu.plot_ellipsoid(
        ax=ax, A2B=ellipsoid2origin2, radii=radii2, wireframe=False, alpha=0.5)
print(f"{accumulated_time=}")

ppu.plot_ellipsoid(
    ax=ax, A2B=ellipsoid2origin, radii=radii, wireframe=False, alpha=0.5,
    color="yellow")
plt.show()