diff --git a/data/paper_collision.vtk b/data/paper_collision.vtk new file mode 100644 index 000000000..430a20d44 --- /dev/null +++ b/data/paper_collision.vtk @@ -0,0 +1,3225 @@ +# vtk DataFile Version 2.0 +paper_collision, Created by Gmsh +ASCII +DATASET UNSTRUCTURED_GRID +POINTS 412 double +-0.0798514932 0.0149937477 0.0206544828 +-0.0790450945 -0.0127552943 -0.0124013424 +-0.0780825317 0.012215009 -0.0277125854 +-0.0720056817 -0.00502119027 0.0456755236 +-0.0705661923 0.0299513582 -0.00211349502 +-0.0638125464 -0.0273741614 0.00410486199 +-0.06380074469999999 0.0300440472 -0.0348700248 +-0.0577872805 0.00418848079 -0.0575864427 +-0.0571920946 0.0365523808 -0.0186748393 +-0.0529749095 0.0257498082 -0.0529402308 +-0.0525738671 0.0404502563 0.0388024524 +-0.0512716472 0.0467252024 0.00880652107 +-0.0507368259 -0.0201681964 -0.0492436476 +-0.04762247039192807 0.0348754824291312 -0.03618575143528616 +-0.0463904887 -0.0185252633 0.0573576167 +-0.0452418029 0.0268723201 0.06923313439999999 +-0.0450602584 0.0438725837 -0.0120340809 +-0.0447889306 0.0386486165 -0.0291038658 +-0.04469918114274272 0.03219772793715973 -0.0443236964930268 +-0.03949963403817546 0.04160078153781051 -0.02520521171874138 +-0.03788957058852005 -0.03704054523703147 0.01856849384882313 +-0.0351954959 -0.0310790949 -0.0341904089 +-0.035087917 0.0342393704 -0.0468971319 +-0.0350660831 0.0287204832 -0.062914148 +-0.0346768871 0.0386576392 -0.0358594432 +-0.03335163 0.0433749259 -0.0263383593 +-0.0306712184 0.0477556437 -0.0132296858 +-0.0283984877 0.00816077925 -0.07867389919999999 +-0.0280937441 0.0354234576 -0.0453123078 +-0.0278285872 0.0435199551 -0.0299467482 +-0.0252116152879978 0.05007118199265022 0.007147923030108534 +-0.0245310478 0.0418390818 -0.0354041904 +-0.0245076139 0.0389184207 -0.0410808884 +-0.0241638459 0.00673970766 0.0837060139 +-0.0215230156 -0.0245913863 0.0599329472 +-0.0187296886 0.0324856527 -0.0582891628 +-0.0182702225 0.0450742766 -0.0206493642 +-0.0156984404 0.0450861901 0.0339377411 +-0.0154369362 -0.0363751464 -0.0170974582 +-0.0125727467 -0.0139743583 -0.0719676316 +-0.0123336269 -0.0379464515 0.0287727714 +-0.0110792266 0.0414306782 -0.0328307189 +-0.0107581913 0.0389411226 -0.0471989214 +-0.0040699942 0.0449028611 -0.0063264221 +-0.00343501847 0.0338849798 0.0555836074 +-0.0020239111 -0.0110258143 0.0776162446 +-0.00133447163 0.0242523961 -0.0753385127 +0.00359241385 -0.0269939303 -0.058288794 +0.00378023973 -0.038893804 0.00127462856 +0.00624911999 -0.0312372576 0.0508415736 +0.01023677983853266 -0.03452968920213689 0.03071116911332998 +0.009557361091678016 0.03961807275536097 -0.0288460447565983 +0.0102930171 0.0362066813 -0.0532082133 +0.013878433 0.0211894587 0.0740507394 +0.0157535449 -0.0236616768 0.0642987117 +0.0171408225 -0.0334864408 -0.0408184454 +0.01781126725617366 0.045593637712402 0.01441078717400669 +0.01799145583235717 -0.03622036691695094 0.01652025190088073 +0.0209705569 0.004584422340000002 -0.07808747890000001 +0.0234485306 -0.0305266604 0.0415627584 +0.02384892295413105 -0.03284768867376705 0.03090358439249017 +0.0260607488 -0.0368426815 -0.0182624366 +0.0275926273 -0.0325779244 0.0484817363 +0.0282474572310186 -0.03395914921722401 0.02525583297340204 +0.0289282408 -0.0007367823269999999 0.0806203187 +0.031354107 -0.0363190025 0.00483915396 +0.0325140618 0.0404513963 -0.0177632421 +0.03294919011917687 -0.03462225739272083 0.01923186236554258 +0.034409482 0.0360297002 0.0353381783 +0.0354992114 -0.0326281935 0.0346817896 +0.0360291973 -0.0207777955 0.0688243285 +0.0367511101 -0.0310608968 0.0500535183 +0.0377072059 0.0294647515 -0.0535913929 +0.04196216680583513 -0.03460246735068836 0.01980535378198632 +0.0426275395 -0.0193124302 -0.06686683 +0.0443605855 -0.0308691077 0.0442918092 +0.04599969706800438 -0.03611389674373018 0.004939726099039117 +0.0463987328 -0.0347424112 0.0267554447 +0.04835685817915118 -0.03280913865861572 0.03546956097808678 +0.05088925877785338 -0.03582673578559618 -0.01660896332459125 +0.0514672622 0.0218239184 0.0507143959 +0.0523538366 -0.0352241397 -0.0422832295 +0.0533739589 -0.0234434083 0.0552398637 +0.0587589331 -0.00368985347 0.0605345443 +0.0589167625 -0.0355811752 0.0166702513 +0.0605825968 0.033381898 -0.00229938631 +0.06259462168878385 -0.02972315392262693 0.03582999071974086 +0.06796488169999999 0.0106940679 -0.0443939082 +0.07272417840000001 -0.0304218382 0.00172252394 +0.0797980353 -0.0175584685 0.0329943858 +0.0814706311 -0.0154668596 -0.0208948776 +0.085020557 0.0121538425 0.0183929037 +-0.06440967879999999 -0.0039765937 -0.0384781165 +0.04708863259141125 0.0072638345606248 0.04002223496867525 +-0.02550266508395307 0.03675074191485136 0.008332238564828159 +-0.001232388053492784 0.02910463115856776 0.03707572411368061 +-0.008872780938184218 0.04504905817689032 0.009441296470238903 +-0.02283763820492265 -0.01644208071484967 -0.002736485537906843 +0.02678478826865979 0.02633771574671151 -0.02852596616144323 +0.04384358695 -0.0022133178985 0.0705774315 +-0.0198532402713027 0.01917953873102862 0.055137738804073 +-0.0607940126 0.017116263995 -0.04622823375 +0.006513086504498574 0.01925897788383476 0.0006462358071251809 +-0.04961838379872203 -0.0141491478629199 0.011255226543059 +-0.05841229495926056 0.009650919342751163 -0.02319312750161279 +-0.05726353291830732 0.02044795639688842 0.006216795859208799 +-0.05255337144864108 0.01961760067283625 0.02364330764820511 +-0.003179517738717174 0.03016400150585359 0.02067163830222432 +0.05381569870242817 0.008820442416570829 0.02157664146814734 +-0.01206187228836779 0.002858870263164327 0.06582851703842778 +-0.03638144350198703 -0.01665694083231668 0.02816003928622308 +-0.03838891821871309 -0.008653519096488933 0.04393426584867614 +-0.0497979447766648 0.02108843638923008 -0.0329748495623907 +-0.04417977456100224 0.03461679191907537 0.003170778982737624 +-0.02642699049902959 0.03682849146277208 -0.005773648600618578 +-0.03820976358660369 -0.008103173013089174 -0.04248035900030774 +0.02234089285 -0.0121992295635 0.07245951519999999 +-0.03099182373943785 -0.00830223158537695 0.05790017300770441 +-0.02015305159322866 -0.01095070967915897 0.07118514359259004 +-0.03051694169516979 0.02023303325255642 -0.04165016574147468 +0.0067267851 0.0452907756 0.004139097850000001 +-0.007636947805000001 -0.02791432195 0.0553872604 +0.009818042635 0.01441840922 -0.0767129958 +-0.002835824724094421 -0.03435704577133345 0.04045678587151159 +0.0473940652 -0.012233824485 0.0646794364 +-0.01177346335 -0.0178086003 0.0687745959 +-0.03467896243280839 0.0308259823644902 -0.01912963449039079 +-0.01341158352977247 -0.01206728532838348 0.05525697929546225 +-0.02465474568953999 -0.01419273634703311 -0.02912362856858058 +0.02994695375113347 -0.01196355843103094 0.05761542527021488 +-0.01503704757487927 -0.01550729616795531 0.02132808962691623 +-0.02039597089181929 0.04761927583092467 0.02012041278788892 +-0.02093357316477443 0.006739555239740947 -0.05799620622908853 +-0.0173915776676657 0.02991943144049706 -0.03009891471989972 +-0.01485229695892883 0.02796549549302726 0.04100815873978625 +-0.02032262755142458 0.01614583898473746 0.00582997432333292 +-0.008354501024313435 0.008850887490102891 0.03606955081919066 +-0.02825848775799346 0.02795648719049383 0.03053938379783445 +-0.05910389192952736 -0.01187488941186818 -0.004519901715159439 +-0.009406663429384884 -0.01578278954732582 -0.04486080133369526 +-0.01294689688709042 -0.01562054692216451 0.03841683485684572 +-0.02911800125669257 0.005668768017641893 0.05228723656666625 +-0.03989785316638383 0.02116598653258632 0.01285566667694565 +0.03194608784987116 0.01929324586116168 0.02382714897089537 +0.00504073793608764 -0.01973450286465057 -0.03429005432448522 +0.07709740475 -0.0229443489 -0.009586176829999999 +0.01237931556715626 -0.0145922539202271 0.02580171241595575 +-0.006631372844254883 -0.02707101223967118 0.01166718207824982 +0.004977737352040157 -0.002826370850484633 -0.04823579111540864 +0.007980341169320627 -0.004306340930299821 0.05963953117313219 +0.05592462581874569 0.007389165364904833 0.05588161793829394 +0.002469664189190436 0.01422885233545212 -0.05957259946778131 +0.0006838718775333826 0.01325734567809007 0.05977607818335286 +0.005540966352247815 0.0248218443676448 -0.04084294949017964 +0.008614768092306907 0.01989373872674494 -0.02479868882499121 +0.01535440215457498 -0.02625009607037252 -0.01213673484097101 +0.007875667913274887 -0.01824372298211635 0.004219455818116129 +0.01850933083832114 -0.02035267778131103 0.05100492040664616 +0.01392186680450316 0.02555370261738941 0.02390211440144889 +0.04261779051236137 0.0215109887033561 0.05632577428718499 +0.014848825295 -0.030881959 0.046202166 +0.0191573623053501 -0.008766916476091628 -0.02162034649109006 +0.03068606198800013 -0.01684625000277214 -0.005516786994831136 +0.03079465607411283 -0.02308901348213519 0.01952765890042058 +0.02506387296575567 -0.002992438115703611 -0.05796222268191205 +0.02670350794888364 0.003173196648407219 0.06182292448829645 +0.02286816315566406 0.01772561657507569 -0.04702496142785054 +-0.01694268344272156 0.03141561812913862 0.06017742288896554 +-0.007110927765676912 0.04279517074814305 -0.02154191273008757 +0.03518637206974213 -0.02178607500077647 -0.02164402487135563 +-0.02007631944092758 0.04709863890626199 -0.007447870927269422 +0.03821967004724587 -0.0005845519229350944 -0.03289894294393091 +0.03525000960843244 -0.01548190627084462 0.03579622069915436 +-0.001418404826363697 0.04006858124647553 -0.0341038767233955 +0.02989285116071679 0.0421462710175448 0.009748715040770201 +0.04092988260971678 0.006439943085916525 0.05623706439883703 +0.0461358203480557 0.02246371587735773 0.004221531286442199 +-0.008725139020094782 0.03887929636296936 0.04583682097268568 +0.0585545800323929 -0.03347540941368696 0.003653922165957472 +0.04798627644379225 -0.02427762270094794 -0.03268053120352386 +0.04973175490362818 -0.01928641967264436 -0.008235530753633718 +0.04317768381929517 0.02849887247328322 0.04365162297011538 +0.05738446819607463 -0.009577601719384205 -0.02539568410179475 +0.05438513471014508 0.00402072290007981 -0.03724617922544714 +-0.07142882045 -0.02006472785 -0.004148240205 +0.08403505412091195 0.004485997534001262 0.007486134749095442 +-0.07075682033441857 0.009317778098312175 -0.03849576092218164 +-0.0542620532 -0.007989857805 -0.05341504515 +-0.007683687168981991 -0.02818153563902627 -0.05128311035957309 +0.06398560548781473 -0.009933464084220975 0.02802029567581429 +0.03857791536060261 0.01749938920360838 0.04553805366977341 +0.06056839409079504 0.0117013984846115 0.001838161726312439 +-0.03822128195 0.0484250505 0.007864281535 +-0.009603780791032768 0.02580672585452767 0.0639526500860757 +0.06469986543685828 -0.00835881736257745 0.002693780772320538 +0.07111021614943515 0.008113853747233621 0.01626509164827248 +0.04861685394968992 -0.008733834279459137 0.04801783247212076 +0.02703034808057249 0.009180888797726846 -0.02907512201716447 +-0.04796199783704574 0.004382465192127371 -0.04039348374813459 +-0.07682887333786656 0.007281396047909488 -0.0169797499971639 +-0.07843250697037191 0.0008667160145014758 -0.02088068468457309 +0.04766890459345382 0.03004669779414725 0.03111255313139311 +-0.0005190715796935631 -0.02549760320667836 0.02833160452606243 +-0.03395675215000001 -0.0215583248 0.05864528195 +-0.06964389700186344 0.02654146902740202 0.01554987488324272 +-0.06149625569543053 0.03540545217979534 0.01291347579516942 +-0.010032080115 0.0283690244 -0.06681383774999999 +-0.03844787800515936 0.03867668398121143 0.04462500916961673 +-0.03202237894672007 -0.02593699763383079 0.01550905499735985 +-0.01188034438168971 0.007508831578862176 -0.03655284952092894 +0.01777616697958617 0.01103652995040624 0.04357745275450222 +-0.03223367727068446 0.03409484477330691 0.05523454862154298 +-0.0498881129693368 -0.02490911801098579 -0.02704557527839091 +-0.03297785685810964 -0.03229153432264384 0.03369604177995317 +-0.05455374719489401 0.02582515954439725 -0.008138318249702653 +-0.005254663761041034 0.001330375248046492 -0.01587518626766316 +0.03365694999467939 -0.003988251091728133 0.009819747296229453 +-0.04581457286214598 0.003143533651469941 0.005912013434616695 +-0.0279241635369057 0.04585498187677119 0.02508855607385626 +-0.01230122682804845 0.0309984686801739 -0.009277651831152206 +-0.004598491573457099 -0.03749782513888252 -0.009182313667286539 +-0.004110033557737366 -0.03843577233767086 0.01421442836728655 +0.002584531899999996 0.0417737671820584 -0.02088068468457309 +0.01698863006692109 0.01637716896833792 -0.06880936641945117 +-0.01462045545 0.04751387985 0.0002978099499999997 +-0.006475487551343567 -0.0191197784278758 -0.02241083244479167 +0.002584531899999996 0.02838850200809574 -0.06767416875371926 +0.00781982453621918 0.02184416560388452 -0.07045704776990502 +0.002497728224060198 0.0168666800593106 0.07693920538554219 +-0.004122836811381985 -0.03020089096966569 -0.04396314047119201 +-0.009269597686036548 -0.03760586424707128 -0.004517059423662833 +0.01443866148603654 -0.03777736596053802 -0.009182313667286539 +0.01443866148603654 -0.03790773586068868 -0.004361918330626558 +0.003050161618693379 -0.02328715964717258 0.06138827902227414 +0.03996381807986044 -0.03626726362162234 -0.01764642744769537 +-0.0220614092764473 -0.03657247758690013 -0.006569111304948137 +0.01884846972843202 -0.03508194391681873 -0.02982468850251231 +0.03247871905 -0.0107572889135 0.0747223236 +-0.04296616089999999 -0.02562364565 -0.04171702825 +0.04278187316067771 0.03191781837041682 -0.02528760514142604 +-0.07094163819999999 0.0211295281 -0.0312913051 +0.02313821502226782 0.03465192079582589 -0.04129470919699357 +-0.0582668631108887 -0.01132693723624152 -0.04327243673891308 +0.03971536324375124 -0.03602168142568844 -0.02980871474412777 +0.05989777239606679 -0.01303446515957696 -0.04330320122495041 +0.04290102888320442 0.01474448438150914 0.06100791243643273 +0.03002663670965119 -0.03487794101208151 -0.03541361229288448 +0.03003898237635336 -0.03655279477830145 -0.008529483226216317 +0.06186351555067436 0.027471389269134 0.01374627560547406 +-0.05744295666054394 0.0123326628666145 0.05849365849108015 +0.07656284847556355 -0.01714699785531694 0.006957220515617358 +0.0688041291192521 -0.01346414948606746 -0.0342727567138516 +0.08197762100285788 0.00164630150925909 0.01421442836728655 +0.04777457715957969 0.007060389808403735 -0.01324187057743612 +-0.06385119725426772 -0.01929621067438623 -0.0193920317255624 +-0.06171629079905992 0.01961231984950423 0.04163355480571918 +-0.06319499289099126 0.009349785596562295 -0.003424000787464948 +0.06251308314873243 0.00423761396024175 -0.04922937363851743 +-0.03836423783771358 0.005862893284340195 0.07051218646853498 +-0.03297785685810964 -0.03315401337238517 -0.009182313667286539 +-0.009269597686036548 -0.03174534971968886 -0.03677488536513469 +-0.004924332846527723 -0.03354174477738605 -0.03150692524166987 +-0.03312521376279697 0.003326863163405222 0.02564288795104442 +0.007223317756090405 0.04111907869894519 0.03162762140584171 +-0.0292904394403482 0.009461769419941032 -0.02171586519551934 +0.01762141360211033 0.03958249783129445 0.03167755816754441 +0.02676681234007878 0.04245897746898478 -0.005256411204766833 +0.03222548025237385 0.002452696736918076 -0.06850073633845881 +0.03467960561169835 0.02172008228773655 0.06100791243643273 +0.03175968497740926 -0.02145105340696965 -0.06447860027650873 +0.06123482634117013 -0.02905839018467858 -0.02621845241263986 +0.01829896488735721 -0.00427013794209119 0.07958868986288112 +0.06940773823664777 0.01066454364162999 -0.02377173285992882 +0.05539104750173866 0.02876239086514983 0.02161659831666779 +0.05459744890213009 0.02412206570367614 0.03771974228303754 +0.0528732571111118 -0.02120589667233259 0.01897271519905563 +0.06485398552200618 0.02199513816080538 0.02525463998273107 +0.07498871711392366 0.01434023678932089 0.002185092325328595 +-0.07423237200232713 0.0001504778086822458 0.01485645090510531 +-0.07484833886171807 0.02305328552966866 0.008386522048826076 +-0.06791377869808701 0.02406161237001387 -0.023313318532343 +-0.06931605965502657 -0.01283627732150107 0.009783605029006701 +-0.04971410740658944 -0.03259253628538329 0.01176427925200991 +-0.06283183396047112 0.03087713506706036 0.03197776501484527 +-0.03490258986143595 0.01699681987765848 0.07633240803605337 +-0.04775729925116332 0.04511121638174299 -0.002984904188356279 +-0.03979103217756753 -0.03288719143014153 0.02727851536711103 +-0.02112372727207309 -0.03763584780936095 0.02526342110916589 +1.703839366907003e-05 -0.03663990896511465 0.02461565728088817 +-0.03126405152263417 0.01699681987765848 -0.07190075683259789 +-0.02340371665129625 0.02072668039396341 -0.06814341964890688 +-0.03382601473111062 0.04747469022672979 -0.0009339699068723652 +-0.02108365163909142 -0.002070331088777286 -0.07557418664470272 +-0.05533871537344098 -0.0138078478783344 0.05327667859896595 +-0.01353741141591473 0.01699681987765848 -0.07684241073630696 +-0.07311472296598133 0.02393757576772273 -0.01079326097823463 +-0.006813514686095794 0.005615547300000006 -0.07369509299645652 +-0.002458696833335402 -0.03597377018026675 0.03211267515226621 +0.01325387452664447 0.04261028915282256 -0.01268815809011487 +0.006190722327310291 0.005615547300000006 0.0757744206514685 +0.002584531899999996 0.02947099401170889 0.06200427005389421 +0.02192159395435578 0.009471283421764218 0.07756175990036174 +0.002584531899999996 -0.03477716305497967 -0.03021950929203256 +0.006653504506280831 -0.03658501750246723 0.01724339217899212 +-0.01333573284347896 -0.03810127221815265 0.007868046062929313 +0.03229229266389898 -0.007908262123196553 -0.07222160070765737 +0.01277125596883076 -0.03665342731516862 -0.01816624664201431 +0.04114410326421095 -0.03608764134900063 -0.006675701018548724 +0.02454253500511211 0.03270231901151569 -0.05340738478053908 +0.04468625431562927 -0.02268037914838642 -0.0616633469014978 +0.05180026604690564 -0.0354519046185668 -0.03257905570185964 +0.016920873645 -0.031907591 0.04966165495 +0.03507976758670732 -0.03437169546907792 -0.04156466614268223 +0.0662034784614703 -0.0207508719690663 0.04421288306070644 +0.05000105024414618 0.03640377449933828 0.001854690001067065 +0.05763362973779469 0.01710317423904124 -0.04753431899963677 +0.05479060587136566 0.02174317482082844 -0.03454241523712405 +0.07253074713194216 0.001849888210141741 -0.03644963140920664 +0.05943581585965055 -0.03371277284260114 -0.009253786225884337 +0.07554991179325635 -0.02528341741697682 0.01421442836728655 +0.06939355107206913 -0.02691726356259793 0.02341826803397235 +0.08115809300464213 0.009823609401295025 0.02459094271474697 +0.08233091096780581 -0.003148267089792522 0.0259127993845731 +0.07665384676983616 0.01942159265855602 0.01130858106851524 +-0.07934749427770738 -0.002349394423930156 -5.397078642010306e-06 +-0.07539778055381582 0.005615547300000006 -0.007975886327952323 +-0.06484904868679336 -0.01647272044525057 -0.03087704149250264 +-0.06644602117189324 -0.0201893678634264 0.01746669291214356 +-0.06929717753830653 -0.01241068319486594 0.03193300552208705 +-0.0759192343498041 0.004962418631151679 0.03319483112813823 +-0.05576794206897456 -0.02032417554679415 0.03262266040609629 +0.0117775444 -0.0005018716172916137 -0.07641024897065604 +-0.06170935607477208 0.03765110909730875 0.002899154595977919 +0.06242518181632952 0.02771911793101486 -0.01280598912399721 +-0.0504598064325939 -0.02910288650470748 -0.01376374379715287 +-0.04561950502510134 -0.003125038514046861 0.06173482120578366 +-0.04558587556604816 0.01271917011550879 0.06554366271665787 +-0.04483198644414618 0.005939566483690531 -0.06688230598049261 +-0.05386494849405762 0.04226925682070182 -0.003230989373887241 +-0.05152111535776578 0.04552310186103066 0.0145528852050928 +-0.05195194265734374 0.04344709451138089 0.02447675911704077 +-0.04904676012763485 0.03391855770394172 0.05344120456150273 +-0.04205805162434058 -0.02795363815139563 0.03761117040185964 +-0.03863504233906451 -0.005754704031187694 -0.06457783331768507 +-0.03740973710122079 -0.008316864740495984 0.0680037773644063 +-0.02868588235289461 -0.03676980877380026 0.003959235590103725 +-0.001639330908125673 0.04533625506464609 0.02569851152984644 +-0.02787701517449225 -0.01645816409741431 -0.06285503254779842 +-0.02355383709696941 -0.02334003946053038 -0.05191766196154549 +-0.02615680815886347 -0.02811879438484947 0.04821686179086862 +-0.03059730819365439 -0.0005731933248231838 0.07607951711370042 +-0.0295649095148289 0.02950195832648118 0.06411477424734822 +-0.03074370566251915 0.04319471764795436 0.03592255533766344 +-0.01895978818796398 -0.02936914097805254 -0.04427742671914618 +-0.01457764916393067 -0.03258278443825574 0.04168734215669274 +-0.01454354264204897 -0.0009798141152397625 0.08105987117346586 +-0.01619701993672906 0.01753862694463543 0.07256732636551176 +0.04788227947878621 0.01366529607931802 0.05818645560348379 +-0.02792106755 0.04894027115000001 -0.0031538219 +-0.01001344925479164 0.01211450944771604 0.08011458962564177 +0.0258913711 -0.02221973615 0.06656152009999999 +0.01443866148603654 -0.007284883614972959 -0.07064577821145507 +-0.01279132885 0.04504035785 0.0238717003 +0.01443866148603654 0.02392907402465014 -0.06286779684595428 +0.01269952131454278 0.034413069521206 0.04773540117478236 +0.02089183045481852 -0.0235896766139393 -0.06209037055301747 +0.01987959376974858 0.02655246772270903 0.06065650348975797 +0.02421917187535293 0.03545219578759581 0.04078962299007056 +0.03085841037193918 -0.02568287170999331 -0.05522851315390962 +0.03572913130850908 0.03364957720599493 -0.03994440883930635 +0.04107403377897505 -0.003911923448501759 -0.06267535251888837 +0.03954791699565416 0.01121706610526808 -0.05855777249052793 +0.04000552562858405 0.009776803103216209 0.06626365770819551 +0.04626136983546995 0.0241580813592869 -0.05099116701226751 +0.04404913979563478 0.03754610700433467 -0.01140819782530242 +0.05000105024414618 0.03445237976971249 0.01291713053230075 +0.06556450545348644 -0.02626000441901371 -0.03257905570185964 +0.001129308999166869 0.0373777886692671 0.04432945634235316 +0.05490104728391226 -0.004777165013492048 -0.0559808586750537 +0.06488861409107713 0.02014830026827059 -0.0268527246833219 +0.06751324003857917 0.01717389552781753 -0.008469364495888757 +0.06185517983018272 0.01883011548062723 0.04070783765707617 +0.06612829257522199 -0.0256791319975884 -0.01860424178576708 +0.07120586701896678 0.008657650425378947 -0.01161570144864867 +0.07721407098727412 0.01440367557020411 0.0259127993845731 +0.0758203504425707 0.006380931167603607 0.03320842924855582 +0.08304955124182389 -0.003181847431997476 -0.003420634201809117 +0.06091120487950562 -0.03251356377508723 0.02666319404794949 +0.03307292491493365 0.004635722513953975 0.07424291459608723 +0.03975207980279129 0.01984682465584152 -0.02480747727853163 +0.04050474564283451 -0.0102972357040172 -0.04917258925973515 +-0.04644043150065263 -0.007892863521945306 -0.01939240894308155 +-0.04324534102017329 0.01287233593832124 -0.006938803371354456 +0.006832999353931824 -0.01769809296596773 0.04466665705040609 +0.02297214486853728 -0.004920123968817177 -0.04136005828020861 +-0.06295664227115268 -0.002481645448306645 -0.01391959251132069 +-0.02284299657354101 0.04120001870900005 0.04135046897622834 +0.03388289807694254 0.03755547859108796 -0.02720701481387632 +0.06425871651154605 -0.0003718219345097296 0.05170912296315664 +-0.01820317320008445 -0.02849234370065569 0.05100696052622863 +-0.07966960943421517 0.008734932729338809 0.0131987193315055 +0.02514537989973427 0.01079062110882924 -0.071977126508893 +0.02444788116528306 0.01592333545774584 -0.01358743789455489 +-0.04147287806520608 0.04626307488610527 0.01661338915223439 +-0.03957411127085653 0.034947187244489 0.01836115920516453 +0.06392189410147329 -0.007093188738951565 0.05377623479809106 +0.03035113653603654 0.03309624917294107 0.04299043935780533 +0.02179559015083519 -0.03722984384333465 -0.01034001575268428 +-0.05699640239534023 -0.003247969844155047 0.02805614552281249 +-0.04520717483396506 -0.03033519187607014 0.02399413105771692 +0.03179622781023555 -0.02148905924305097 -0.03859630354041377 +0.07445639177154403 -0.01403734877638145 0.03998659050653811 + +CELLS 1402 7010 +4 65 76 162 216 +4 102 402 266 216 +4 101 198 92 7 +4 59 62 69 172 +4 140 146 202 130 +4 215 156 161 102 +4 182 183 253 272 +4 9 119 23 22 +4 211 249 255 141 +4 270 179 79 180 +4 115 12 326 242 +4 242 115 12 187 +4 134 44 152 95 +4 99 129 165 196 +4 14 111 342 330 +4 266 66 402 154 +4 43 168 219 36 +4 399 121 127 34 +4 128 264 97 215 +4 258 100 141 336 +4 126 135 114 113 +4 303 57 156 146 +4 389 166 369 373 +4 107 134 137 136 +4 149 165 152 301 +4 60 69 163 172 +4 271 116 45 149 +4 225 148 215 144 +4 69 275 78 77 +4 202 146 303 156 +4 243 410 169 179 +4 136 210 149 140 +4 157 62 129 360 +4 205 404 113 339 +4 113 332 105 205 +4 143 176 273 108 +4 383 90 145 382 +4 70 129 124 82 +4 275 194 189 250 +4 105 4 256 279 +4 341 255 207 211 +4 341 255 10 207 +4 37 134 362 95 +4 346 37 362 95 +4 222 154 133 173 +4 9 112 119 18 +4 96 102 43 120 +4 13 18 24 112 +4 18 112 119 24 +4 19 126 24 25 +4 80 357 190 93 +4 22 24 119 28 +4 104 395 326 391 +4 25 126 24 29 +4 254 326 395 391 +4 28 32 24 133 +4 28 133 24 119 +4 29 31 36 133 +4 29 133 24 31 +4 29 24 133 126 +4 154 402 161 197 +4 31 32 41 133 +4 31 133 24 32 +4 31 133 41 36 +4 32 133 42 41 +4 110 103 97 208 +4 110 130 208 97 +4 35 119 28 23 +4 35 23 290 132 +4 36 168 133 41 +4 37 134 137 362 +4 358 94 114 224 +4 40 287 130 147 +4 326 198 115 242 +4 41 173 133 42 +4 226 151 206 227 +4 68 174 375 143 +4 323 248 195 276 +4 373 316 369 389 +4 171 371 389 316 +4 411 405 398 196 +4 282 259 208 345 +4 49 157 311 160 +4 258 117 141 350 +4 49 123 393 160 +4 49 157 160 393 +4 325 256 395 199 +4 105 142 205 106 +4 50 59 60 146 +4 158 120 56 266 +4 128 215 225 209 +4 52 166 151 363 +4 52 153 151 166 +4 128 225 97 38 +4 58 223 122 331 +4 58 223 361 401 +4 61 247 169 155 +4 63 163 69 67 +4 100 211 336 351 +4 258 284 356 351 +4 299 109 355 359 +4 294 296 132 292 +4 69 71 75 172 +4 69 75 78 172 +4 69 275 77 73 +4 392 97 264 135 +4 71 196 82 75 +4 71 75 172 196 +4 71 196 172 129 +4 72 241 166 369 +4 371 389 166 197 +4 73 275 84 76 +4 73 77 84 275 +4 75 86 196 82 +4 77 275 387 84 +4 344 34 117 118 +4 80 357 93 150 +4 211 15 249 336 +4 263 346 95 377 +4 84 76 275 178 +4 85 176 191 248 +4 86 89 189 313 +4 87 379 316 183 +4 91 252 322 195 +4 280 2 240 104 +4 117 349 203 111 +4 266 389 253 402 +4 197 253 389 402 +4 98 402 197 389 +4 30 94 192 291 +4 45 149 299 301 +4 85 176 333 191 +4 374 239 333 389 +4 374 239 389 66 +4 66 239 389 98 +4 139 47 39 148 +4 335 111 117 14 +4 136 152 109 149 +4 304 97 345 208 +4 148 154 209 215 +4 347 348 115 238 +4 34 118 127 117 +4 353 260 128 38 +4 85 176 375 314 +4 325 324 1 395 +4 395 324 1 138 +4 152 359 228 193 +4 260 225 128 38 +4 281 256 138 324 +4 313 196 82 405 +4 205 340 106 404 +4 230 38 235 97 +4 256 103 138 217 +4 190 268 245 159 +4 190 181 268 159 +4 26 291 114 358 +4 113 291 94 114 +4 291 114 126 26 +4 166 98 197 389 +4 8 16 338 214 +4 8 16 214 126 +4 30 224 94 358 +4 138 395 217 391 +4 104 395 391 392 +4 395 324 138 256 +4 329 106 255 408 +4 374 266 176 174 +4 64 116 271 165 +4 302 155 225 220 +4 55 365 148 368 +4 383 380 272 253 +4 355 45 118 109 +4 96 135 102 107 +4 39 148 151 132 +4 56 120 158 107 +4 395 392 217 391 +4 97 392 391 217 +4 262 110 103 97 +4 46 206 296 227 +4 54 360 311 157 +4 104 126 214 392 +4 1 395 199 325 +4 113 105 392 142 +4 105 217 392 142 +4 210 406 364 366 +4 92 198 187 7 +4 39 347 132 292 +4 182 383 317 272 +4 165 268 372 175 +4 391 115 264 128 +4 212 115 128 21 +4 131 37 137 362 +4 131 218 137 37 +4 201 375 273 143 +4 97 135 217 262 +4 126 24 133 119 +4 217 262 106 408 +4 102 136 158 107 +4 255 137 207 211 +4 56 107 346 120 +4 146 393 59 172 +4 160 146 393 59 +4 110 130 262 140 +4 111 141 117 127 +4 117 127 141 118 +4 291 94 114 358 +4 30 358 94 291 +4 312 309 368 410 +4 243 312 246 410 +4 262 137 142 106 +4 137 100 211 141 +4 137 141 136 100 +4 152 356 359 193 +4 359 109 356 152 +4 143 93 108 201 +4 274 201 108 93 +4 136 262 137 135 +4 276 381 274 108 +4 115 132 128 348 +4 279 204 105 205 +4 366 165 210 268 +4 115 23 119 132 +4 68 406 143 190 +4 354 123 40 140 +4 281 103 138 256 +4 281 256 278 103 +4 88 318 178 194 +4 143 406 210 190 +4 382 194 182 180 +4 290 132 294 206 +4 294 132 296 206 +4 126 219 264 133 +4 394 164 371 390 +4 66 402 389 266 +4 147 48 220 156 +4 130 146 156 135 +4 140 202 146 393 +4 140 130 136 146 +4 202 50 146 160 +4 139 148 39 132 +4 393 140 210 146 +4 144 154 148 215 +4 109 149 299 45 +4 215 161 225 144 +4 30 218 94 131 +4 6 13 8 112 +4 132 151 296 206 +4 97 391 264 128 +4 217 135 97 392 +4 215 102 219 135 +4 215 102 154 219 +4 42 209 148 153 +4 141 350 109 356 +4 356 350 109 33 +4 302 155 144 225 +4 369 98 389 239 +4 139 144 47 148 +4 225 161 155 144 +4 225 155 161 156 +4 210 93 172 143 +4 139 225 148 209 +4 144 215 161 154 +4 215 161 154 102 +4 140 136 210 146 +4 144 394 148 154 +4 342 110 409 330 +4 148 153 154 166 +4 135 136 102 107 +4 94 135 362 137 +4 344 34 203 117 +4 255 3 408 249 +4 53 366 165 152 +4 152 193 53 300 +4 300 53 152 366 +4 144 148 394 55 +4 144 161 55 394 +4 152 228 53 193 +4 393 146 210 172 +4 156 162 102 216 +4 326 200 1 395 +4 157 129 210 149 +4 223 164 166 148 +4 213 130 140 40 +4 154 197 161 394 +4 213 140 349 354 +4 102 146 216 143 +4 107 136 158 95 +4 158 95 210 364 +4 348 139 128 353 +4 348 139 353 188 +4 141 211 336 100 +4 258 284 351 336 +4 351 100 211 167 +4 158 68 143 56 +4 158 210 146 143 +4 149 129 210 165 +4 239 316 333 389 +4 104 198 326 92 +4 101 9 112 198 +4 162 253 161 402 +4 216 176 253 266 +4 150 93 381 398 +4 7 9 101 198 +4 118 127 125 34 +4 57 60 163 146 +4 57 60 146 50 +4 36 170 219 43 +4 283 329 106 255 +4 247 307 169 162 +4 169 162 161 155 +4 155 161 156 162 +4 55 410 394 161 +4 161 162 169 253 +4 307 180 79 169 +4 161 171 394 197 +4 161 171 169 410 +4 172 163 216 275 +4 299 53 152 228 +4 237 64 165 116 +4 360 237 129 116 +4 129 237 165 116 +4 351 100 356 258 +4 121 393 127 233 +4 210 129 172 196 +4 233 121 393 49 +4 129 210 165 196 +4 300 152 210 366 +4 364 210 300 44 +4 247 307 162 65 +4 65 76 307 162 +4 138 97 391 217 +4 142 113 94 135 +4 149 129 165 116 +4 88 382 318 194 +4 273 201 108 274 +4 57 156 163 65 +4 104 198 92 112 +4 169 182 171 253 +4 169 182 180 179 +4 253 162 180 216 +4 65 76 216 163 +4 390 183 371 171 +4 171 169 390 182 +4 190 93 274 80 +4 80 181 274 190 +4 258 100 356 141 +4 171 182 183 253 +4 166 389 369 98 +4 195 185 252 194 +4 158 136 210 95 +4 168 173 133 41 +4 92 186 112 101 +4 92 104 112 186 +4 256 214 392 104 +4 256 199 104 395 +4 38 21 259 128 +4 229 353 139 188 +4 362 107 137 135 +4 107 136 137 135 +4 362 135 96 107 +4 99 83 196 150 +4 99 124 196 83 +4 108 189 172 275 +4 319 322 250 189 +4 196 83 398 150 +4 176 108 191 248 +4 17 19 126 24 +4 112 126 119 24 +4 112 17 126 24 +4 368 164 394 390 +4 196 210 93 172 +4 152 109 299 359 +4 275 194 180 108 +4 405 82 83 196 +4 264 215 209 154 +4 264 215 154 219 +4 210 152 95 136 +4 136 134 137 100 +4 141 109 127 136 +4 104 280 2 199 +4 208 97 345 259 +4 115 128 132 119 +4 133 28 42 119 +4 100 356 109 152 +4 126 219 135 264 +4 108 194 189 275 +4 10 106 340 404 +4 42 132 151 148 +4 42 148 151 153 +4 165 268 301 372 +4 352 207 137 396 +4 158 210 367 364 +4 99 129 196 124 +4 211 396 137 207 +4 237 70 129 99 +4 285 338 16 113 +4 338 214 16 113 +4 7 337 23 198 +4 8 126 17 16 +4 20 208 287 304 +4 111 140 141 127 +4 152 299 109 149 +4 262 136 137 141 +4 40 130 287 213 +4 364 300 210 366 +4 94 142 135 137 +4 142 137 262 135 +4 97 130 147 156 +4 97 215 156 225 +4 135 215 102 156 +4 151 166 153 148 +4 52 363 308 166 +4 146 172 216 143 +4 53 268 301 165 +4 64 165 301 388 +4 158 210 143 367 +4 68 367 143 406 +4 74 390 368 309 +4 116 54 149 129 +4 118 127 141 109 +4 136 127 149 109 +4 385 398 189 411 +4 103 282 409 330 +4 409 208 282 103 +4 205 106 142 404 +4 409 20 282 208 +4 191 194 253 383 +4 113 339 403 11 +4 162 402 161 102 +4 383 380 253 191 +4 102 266 120 158 +4 137 10 207 352 +4 158 120 102 107 +4 109 355 33 118 +4 51 66 98 397 +4 268 190 406 181 +4 165 210 268 190 +4 249 330 293 3 +4 52 241 51 153 +4 143 172 216 108 +4 110 103 409 330 +4 182 253 180 194 +4 48 220 156 155 +4 231 155 220 48 +4 232 156 155 48 +4 232 48 155 231 +4 110 130 97 262 +4 93 385 381 398 +4 133 154 209 153 +4 308 241 52 166 +4 391 326 115 212 +4 212 254 334 391 +4 156 146 57 163 +4 23 7 198 9 +4 114 94 135 219 +4 216 275 180 108 +4 256 217 392 105 +4 97 392 264 391 +4 298 51 154 222 +4 155 247 162 65 +4 13 112 24 17 +4 12 347 115 238 +4 364 44 95 210 +4 68 190 143 201 +4 68 190 201 181 +4 116 54 45 149 +4 368 410 394 55 +4 278 106 329 408 +4 369 98 397 241 +4 395 325 324 256 +4 260 128 139 353 +4 334 97 138 103 +4 318 180 76 178 +4 108 189 93 172 +4 39 47 361 148 +4 53 152 301 299 +4 361 223 148 164 +4 241 98 166 369 +4 182 383 194 382 +4 121 140 127 393 +4 399 349 127 140 +4 140 123 40 202 +4 123 202 140 393 +4 170 114 36 219 +4 84 178 275 88 +4 178 194 180 275 +4 392 214 256 105 +4 172 189 93 196 +4 280 295 2 199 +4 214 295 280 256 +4 256 295 280 199 +4 5 184 138 334 +4 254 334 138 184 +4 133 153 209 42 +4 209 153 154 148 +4 35 206 151 132 +4 271 149 301 165 +4 197 371 171 394 +4 244 378 390 183 +4 219 102 222 43 +4 216 176 108 253 +4 133 219 222 168 +4 257 371 378 183 +4 22 23 28 119 +4 331 39 361 151 +4 236 306 155 302 +4 196 124 82 83 +4 86 172 196 189 +4 47 365 361 148 +4 365 148 368 164 +4 151 39 361 148 +4 280 240 6 112 +4 101 240 112 6 +4 411 196 313 405 +4 269 164 368 390 +4 283 329 278 106 +4 283 0 278 329 +4 115 12 187 343 +4 2 104 92 186 +4 92 187 198 242 +4 146 102 216 156 +4 69 163 172 275 +4 218 404 403 30 +4 192 403 404 30 +4 35 42 28 119 +4 160 157 311 59 +4 75 172 86 78 +4 75 86 172 196 +4 386 185 383 194 +4 137 134 211 100 +4 134 44 95 177 +4 210 44 95 152 +4 107 362 134 95 +4 107 134 136 95 +4 395 256 138 217 +4 88 178 275 194 +4 141 118 109 350 +4 326 391 115 198 +4 102 154 402 161 +4 266 374 176 253 +4 278 105 106 217 +4 217 392 142 135 +4 234 79 169 307 +4 135 219 215 264 +4 96 94 219 135 +4 102 96 219 135 +4 136 100 152 134 +4 395 256 217 392 +4 298 266 120 102 +4 298 66 266 154 +4 357 175 190 93 +4 245 357 175 190 +4 372 357 175 245 +4 268 406 210 366 +4 244 378 183 257 +4 277 85 191 248 +4 195 248 191 108 +4 259 128 97 38 +4 93 196 189 398 +4 411 196 189 313 +4 93 196 398 150 +4 411 398 189 196 +4 235 38 259 97 +4 117 349 111 127 +4 117 34 349 127 +4 4 295 8 214 +4 249 293 330 111 +4 335 249 111 293 +4 348 39 132 139 +4 348 132 128 139 +4 222 219 154 102 +4 219 154 133 222 +4 221 303 147 288 +4 198 391 115 264 +4 401 223 361 164 +4 267 401 361 164 +4 244 183 390 182 +4 244 317 183 182 +4 201 273 108 143 +4 158 263 364 265 +4 265 367 364 158 +4 263 107 158 95 +4 189 108 195 194 +4 408 262 255 111 +4 217 262 142 106 +4 104 92 326 200 +4 104 92 200 2 +4 255 262 106 137 +4 130 136 135 262 +4 253 176 333 374 +4 135 130 97 156 +4 147 156 303 48 +4 221 48 147 303 +4 51 241 154 153 +4 96 107 120 346 +4 172 108 143 93 +4 99 150 175 372 +4 196 150 93 175 +4 213 349 110 342 +4 263 346 107 95 +4 111 262 141 140 +4 349 111 127 140 +4 213 140 110 349 +4 377 346 95 177 +4 372 99 165 175 +4 99 175 196 165 +4 388 165 372 99 +4 218 404 94 137 +4 218 404 30 94 +4 10 404 403 218 +4 10 404 218 137 +4 147 287 208 304 +4 147 304 221 287 +4 147 208 97 304 +4 322 194 189 195 +4 322 195 189 385 +4 73 76 163 275 +4 158 367 143 68 +4 134 396 211 167 +4 265 367 158 68 +4 65 73 163 67 +4 76 275 180 216 +4 302 220 225 38 +4 248 176 375 85 +4 151 361 223 148 +4 173 222 154 51 +4 173 154 153 51 +4 168 222 133 173 +4 100 167 193 44 +4 213 208 110 130 +4 110 286 409 208 +4 110 208 409 103 +4 294 292 132 27 +4 76 79 307 180 +4 263 95 158 364 +4 165 237 129 99 +4 99 70 129 124 +4 7 186 92 101 +4 348 353 128 21 +4 348 128 115 21 +4 281 184 324 138 +4 318 76 180 79 +4 146 136 102 135 +4 51 153 173 52 +4 6 13 112 9 +4 226 151 227 363 +4 100 109 356 141 +4 214 280 104 256 +4 113 94 135 114 +4 146 102 158 143 +4 282 103 259 5 +4 282 259 103 208 +4 235 259 345 97 +4 230 48 147 304 +4 104 392 264 126 +4 245 190 175 268 +4 165 268 175 190 +4 268 372 175 245 +4 243 179 169 234 +4 100 152 44 193 +4 176 375 314 174 +4 288 50 303 202 +4 97 220 156 147 +4 250 185 194 252 +4 408 255 262 106 +4 191 176 333 253 +4 134 100 152 44 +4 333 191 253 380 +4 335 14 117 344 +4 273 176 375 248 +4 335 293 111 14 +4 126 219 114 135 +4 128 97 225 215 +4 315 371 183 316 +4 373 316 371 315 +4 157 129 172 210 +4 157 393 210 172 +4 323 195 248 277 +4 171 316 183 371 +4 171 253 183 316 +4 49 54 311 157 +4 54 360 157 129 +4 188 47 39 139 +4 202 123 297 50 +4 202 50 303 146 +4 99 150 196 175 +4 140 262 141 136 +4 205 404 142 113 +4 252 194 322 195 +4 228 299 359 152 +4 18 22 24 119 +4 212 115 391 128 +4 128 212 334 391 +4 216 253 108 180 +4 353 38 128 21 +4 186 240 104 112 +4 182 90 317 383 +4 229 188 139 47 +4 229 47 139 144 +4 71 129 62 70 +4 71 129 82 196 +4 279 324 278 400 +4 71 62 129 172 +4 304 235 345 97 +4 230 235 304 97 +4 259 97 334 103 +4 259 103 334 5 +4 233 45 127 121 +4 127 45 125 121 +4 119 132 35 42 +4 393 127 233 149 +4 101 186 112 240 +4 130 40 202 140 +4 130 147 202 40 +4 138 217 103 97 +4 331 223 122 151 +4 331 122 296 151 +4 260 225 139 128 +4 260 225 38 261 +4 212 326 115 238 +4 177 44 95 377 +4 263 377 95 364 +4 362 134 137 107 +4 275 84 88 319 +4 160 146 59 50 +4 187 198 115 337 +4 96 94 224 219 +4 102 96 43 219 +4 131 362 94 96 +4 92 198 326 242 +4 279 204 278 105 +4 210 190 93 143 +4 78 275 172 86 +4 76 307 162 180 +4 320 86 189 275 +4 9 112 18 13 +4 192 285 291 113 +4 144 55 236 302 +4 9 119 22 18 +4 153 241 154 166 +4 52 241 153 166 +4 56 263 158 265 +4 257 315 371 183 +4 127 233 149 45 +4 233 149 45 54 +4 37 396 352 137 +4 37 352 218 137 +4 98 166 197 154 +4 113 192 94 291 +4 113 142 94 404 +4 47 55 148 144 +4 328 278 329 408 +4 296 206 151 227 +4 296 227 151 122 +4 320 86 275 387 +4 77 275 78 86 +4 69 172 78 275 +4 86 275 172 189 +4 149 54 157 129 +4 280 240 112 104 +4 382 318 180 79 +4 180 178 318 194 +4 126 17 112 8 +4 13 8 112 17 +4 256 325 295 199 +4 102 43 120 298 +4 154 222 102 298 +4 105 256 278 279 +4 154 51 98 241 +4 65 48 57 156 +4 61 234 169 247 +4 247 234 169 307 +4 45 149 301 271 +4 130 287 213 208 +4 161 394 171 410 +4 394 390 171 410 +4 161 253 169 171 +4 72 371 373 166 +4 139 144 148 225 +4 128 225 139 209 +4 128 132 209 139 +4 319 322 189 89 +4 194 195 191 108 +4 108 248 276 195 +4 277 195 248 191 +4 43 102 222 298 +4 162 76 180 216 +4 55 410 161 169 +4 402 162 253 216 +4 374 314 176 85 +4 85 333 176 374 +4 394 371 171 390 +4 196 93 165 175 +4 190 175 165 93 +4 350 33 356 284 +4 133 264 209 154 +4 133 264 154 219 +4 128 215 209 264 +4 126 119 133 264 +4 119 133 264 209 +4 16 17 19 126 +4 201 190 143 93 +4 201 190 274 181 +4 209 128 119 132 +4 198 9 119 23 +4 23 132 35 119 +4 133 42 209 119 +4 169 180 79 179 +4 132 139 148 209 +4 391 104 392 264 +4 389 316 371 373 +4 237 129 70 360 +4 173 133 153 154 +4 302 144 155 236 +4 144 155 236 161 +4 225 209 215 148 +4 144 236 55 161 +4 161 171 197 253 +4 253 182 180 169 +4 162 180 307 169 +4 378 371 390 183 +4 198 104 264 112 +4 198 264 119 112 +4 110 408 111 262 +4 105 142 106 217 +4 169 179 390 182 +4 182 383 272 253 +4 183 379 272 317 +4 253 108 194 191 +4 330 110 111 342 +4 335 141 117 111 +4 255 141 262 137 +4 130 146 135 136 +4 209 148 132 42 +4 35 151 42 132 +4 135 146 156 102 +4 140 127 393 149 +4 140 127 149 136 +4 220 225 156 155 +4 225 156 161 215 +4 147 303 156 202 +4 130 146 202 156 +4 146 216 163 156 +4 136 146 102 158 +4 152 165 210 366 +4 152 165 149 210 +4 152 210 149 136 +4 61 236 155 169 +4 155 247 169 162 +4 162 161 156 102 +4 148 55 368 394 +4 156 216 65 162 +4 311 59 157 62 +4 166 148 164 394 +4 148 154 394 166 +4 154 166 197 394 +4 278 408 217 106 +4 102 266 143 216 +4 402 253 266 216 +4 10 137 255 106 +4 279 0 400 278 +4 184 324 138 1 +4 186 2 104 240 +4 328 3 408 329 +4 205 4 105 279 +4 332 4 8 214 +4 136 210 146 158 +4 163 146 172 216 +4 146 172 143 210 +4 339 11 113 205 +4 326 12 115 238 +4 293 14 330 111 +4 111 203 117 14 +4 343 27 132 292 +4 289 27 132 337 +4 34 349 127 399 +4 26 358 114 170 +4 220 38 97 225 +4 39 296 292 132 +4 151 166 148 223 +4 365 361 148 164 +4 37 134 95 177 +4 346 37 95 177 +4 304 48 147 221 +4 232 48 65 156 +4 393 121 123 49 +4 302 55 229 144 +4 229 55 47 144 +4 174 56 143 68 +4 46 227 296 122 +4 267 58 361 401 +4 267 305 361 58 +4 231 61 306 155 +4 246 169 236 61 +4 271 64 165 301 +4 269 305 390 74 +4 113 105 142 205 +4 4 105 214 332 +4 113 332 338 214 +4 137 404 106 10 +4 378 74 390 370 +4 32 133 28 42 +4 68 406 190 181 +4 312 309 179 81 +4 57 163 60 63 +4 60 163 69 63 +4 94 362 135 96 +4 114 358 224 170 +4 53 165 301 152 +4 137 396 211 134 +4 320 387 275 84 +4 71 129 70 82 +4 360 70 62 129 +4 183 87 317 244 +4 244 257 183 87 +4 315 316 183 87 +4 379 253 316 183 +4 317 87 183 379 +4 21 212 334 128 +4 131 137 94 362 +4 131 218 94 137 +4 51 397 98 241 +4 320 89 189 86 +4 275 189 320 319 +4 411 89 189 322 +4 382 145 194 88 +4 182 90 383 382 +4 169 236 155 161 +4 192 30 404 94 +4 354 140 349 399 +4 354 121 140 399 +4 281 327 5 103 +4 277 195 191 185 +4 277 191 383 185 +4 194 185 191 195 +4 194 185 383 191 +4 48 57 156 303 +4 256 4 295 325 +4 107 346 362 95 +4 190 159 245 80 +4 190 181 159 80 +4 383 194 145 386 +4 376 90 182 382 +4 8 112 126 104 +4 104 126 8 214 +4 220 147 97 230 +4 200 2 199 104 +4 342 110 111 349 +4 163 73 69 67 +4 50 146 57 303 +4 211 15 255 249 +4 255 3 329 408 +4 214 295 256 4 +4 76 178 180 275 +4 4 105 256 214 +4 173 42 52 153 +4 73 163 69 275 +4 62 59 157 172 +4 311 62 157 360 +4 287 208 130 147 +4 184 1 138 254 +4 5 103 334 138 +4 77 275 86 387 +4 126 25 26 36 +4 126 114 36 26 +4 105 204 278 106 +4 98 197 402 154 +4 154 98 166 241 +4 332 338 214 8 +4 326 200 395 104 +4 253 379 272 183 +4 339 404 403 340 +4 391 138 254 334 +4 194 386 250 145 +4 81 376 179 270 +4 208 286 409 20 +4 138 395 391 254 +4 110 408 103 330 +4 143 367 210 406 +4 202 160 123 50 +4 342 110 286 409 +4 334 97 391 138 +4 281 103 408 327 +4 202 160 146 393 +4 123 160 202 393 +4 279 256 324 325 +4 279 4 256 325 +4 328 278 408 281 +4 249 255 111 408 +4 154 51 66 98 +4 298 51 66 154 +4 335 141 111 249 +4 141 255 111 249 +4 336 335 141 258 +4 211 336 249 141 +4 393 140 123 121 +4 313 189 86 196 +4 188 39 348 139 +4 8 280 214 295 +4 256 280 104 199 +4 306 61 236 155 +4 155 306 231 220 +4 259 97 103 208 +4 282 345 208 20 +4 278 103 217 408 +4 334 391 97 128 +4 65 76 163 73 +4 334 259 128 97 +4 335 117 141 258 +4 371 401 166 72 +4 11 285 192 113 +4 140 210 149 393 +4 115 132 348 347 +4 345 208 20 304 +4 118 344 350 117 +4 363 227 223 151 +4 71 69 62 172 +4 347 343 132 292 +4 271 116 149 165 +4 12 347 343 115 +4 132 289 23 290 +4 380 333 191 85 +4 298 266 102 154 +4 258 351 100 336 +4 385 384 195 108 +4 385 195 189 108 +4 125 127 121 34 +4 104 256 395 392 +4 254 1 138 395 +4 388 165 301 372 +4 331 39 151 296 +4 348 132 39 347 +4 44 152 300 193 +4 72 373 369 166 +4 171 197 253 389 +4 399 121 140 127 +4 45 355 299 109 +4 39 296 132 151 +4 100 167 351 193 +4 251 317 244 182 +4 244 376 179 81 +4 85 277 191 380 +4 353 229 139 260 +4 144 261 225 260 +4 144 225 139 260 +4 144 260 139 229 +4 144 229 261 260 +4 30 94 224 96 +4 263 107 56 158 +4 346 56 263 107 +4 105 204 106 205 +4 151 52 35 42 +4 182 244 376 179 +4 151 35 52 226 +4 226 151 35 206 +4 52 42 151 153 +4 113 404 403 339 +4 383 145 90 386 +4 302 225 261 38 +4 310 234 179 243 +4 278 103 256 217 +4 281 324 278 256 +4 408 278 103 281 +4 264 126 119 112 +4 171 389 371 197 +4 389 371 166 373 +4 391 104 264 198 +4 114 219 224 94 +4 404 142 94 137 +4 106 137 142 404 +4 192 404 113 94 +4 289 132 23 337 +4 134 152 136 95 +4 15 255 341 211 +4 243 410 246 169 +4 312 179 309 410 +4 232 155 156 65 +4 100 136 109 141 +4 152 136 109 100 +4 407 61 155 247 +4 361 331 223 58 +4 209 132 119 42 +4 269 365 368 164 +4 223 166 308 363 +4 102 158 143 266 +4 392 113 214 105 +4 392 135 113 142 +4 66 98 402 154 +4 66 402 98 389 +4 406 210 364 367 +4 226 46 227 206 +4 182 244 251 376 +4 281 327 408 328 +4 368 269 390 74 +4 244 179 390 309 +4 350 141 258 356 +4 164 371 267 401 +4 250 386 194 185 +4 109 33 359 356 +4 355 33 359 109 +4 60 172 146 59 +4 126 36 219 133 +4 265 68 158 56 +4 10 255 137 207 +4 109 127 149 45 +4 390 164 371 370 +4 390 370 371 378 +4 40 123 297 202 +4 226 151 363 52 +4 232 407 155 247 +4 232 407 231 155 +4 283 10 106 340 +4 357 175 93 150 +4 372 150 175 357 +4 201 190 93 274 +4 239 316 389 369 +4 333 316 253 389 +4 374 389 333 253 +4 49 54 157 233 +4 49 157 393 233 +4 233 157 393 149 +4 405 196 83 398 +4 374 176 314 174 +4 273 108 176 248 +4 382 180 182 270 +4 244 179 309 81 +4 217 135 142 262 +4 140 136 141 127 +4 323 384 276 195 +4 103 327 282 330 +4 274 381 93 108 +4 382 180 270 79 +4 382 182 376 270 +4 250 322 194 189 +4 392 135 264 126 +4 383 253 182 194 +4 194 383 145 382 +4 205 340 404 339 +4 261 225 302 144 +4 302 144 229 261 +4 44 134 100 167 +4 44 177 134 167 +4 122 227 151 223 +4 223 363 151 166 +4 170 114 219 224 +4 43 170 219 224 +4 384 108 276 195 +4 10 283 106 255 +4 164 305 370 390 +4 270 179 180 182 +4 250 322 252 194 +4 374 389 266 66 +4 204 279 278 0 +4 204 0 278 283 +4 200 1 395 199 +4 4 105 332 205 +4 310 79 179 234 +4 310 270 179 79 +4 34 349 203 117 +4 285 11 338 113 +4 361 331 151 223 +4 351 211 336 15 +4 117 203 344 14 +4 16 113 291 285 +4 390 309 378 244 +4 21 128 334 259 +4 212 115 21 238 +4 343 337 132 27 +4 27 132 290 289 +4 35 206 132 290 +4 36 26 114 170 +4 291 16 26 126 +4 37 396 137 134 +4 230 38 97 220 +4 336 284 351 15 +4 25 29 36 126 +4 335 344 117 258 +4 294 206 296 46 +4 220 48 147 230 +4 103 262 97 217 +4 366 53 165 268 +4 100 351 356 193 +4 100 152 193 356 +4 43 224 219 96 +4 168 219 222 43 +4 402 197 253 161 +4 166 308 241 72 +4 378 309 390 74 +4 61 243 169 234 +4 61 243 246 169 +4 245 80 357 190 +4 93 80 150 381 +4 243 312 179 81 +4 179 390 309 410 +4 310 81 243 179 +4 30 131 94 96 +4 313 82 196 86 +4 64 165 388 99 +4 129 196 124 82 +4 230 147 97 304 +4 213 40 140 354 +4 313 89 189 411 +4 112 198 92 101 +4 6 101 9 112 +4 90 251 182 317 +4 90 251 376 182 +4 88 145 194 250 +4 300 152 44 210 +4 91 384 195 321 +4 91 195 322 321 +4 365 361 164 305 +4 267 305 164 361 +4 269 305 164 390 +4 365 305 164 269 +4 364 377 95 44 +4 185 323 195 91 +4 384 195 323 91 +4 395 199 104 200 +4 210 165 93 190 +4 196 210 165 93 +4 395 326 254 1 +4 5 327 282 103 +4 330 408 103 327 +4 330 328 408 327 +4 408 330 3 328 +4 277 191 380 383 +4 286 208 110 213 +4 286 213 110 342 +4 157 62 172 129 +4 283 205 106 204 +4 283 204 106 278 +4 346 362 96 107 +4 283 205 340 106 +4 290 294 132 27 +4 11 332 113 205 +4 11 332 338 113 +4 126 113 214 392 +4 126 135 113 392 +4 7 198 187 337 +4 216 143 108 176 +4 108 176 191 253 +4 198 264 115 119 +4 262 140 111 110 +4 111 342 203 14 +4 208 130 147 97 +4 215 135 264 97 +4 16 126 113 214 +4 115 187 337 343 +4 118 350 141 117 +4 156 135 215 97 +4 115 337 132 343 +4 115 198 23 337 +4 23 132 115 337 +4 256 217 105 278 +4 356 284 258 350 +4 140 213 110 130 +4 348 21 115 238 +4 288 202 303 147 +4 258 117 350 344 +4 297 50 288 202 +4 64 99 237 165 +4 360 116 129 54 +4 40 202 288 147 +4 104 391 326 198 +4 254 212 326 391 +4 133 173 153 42 +4 371 267 370 164 +4 57 163 67 65 +4 11 113 192 403 +4 134 37 396 177 +4 111 342 349 203 +4 47 55 365 148 +4 275 250 319 88 +4 194 250 275 88 +4 297 202 288 40 +4 118 45 125 127 +4 249 330 408 111 +4 3 330 408 249 +4 8 104 214 280 +4 280 112 8 104 +4 280 6 8 112 +4 155 306 220 302 +4 210 190 406 268 +4 149 301 152 299 +4 407 61 231 155 +4 103 262 217 408 +4 243 312 410 179 +4 410 55 368 312 +4 410 55 246 169 +4 410 55 312 246 +4 309 390 368 410 +4 166 223 401 164 +4 166 223 308 401 +4 401 166 164 371 +4 72 401 166 308 +4 60 69 172 59 +4 146 60 163 172 +4 98 397 66 239 +4 324 256 279 278 +4 164 305 267 370 +4 390 305 370 74 +4 408 110 103 262 +4 408 330 110 111 +4 113 332 214 105 +4 68 375 201 143 +4 270 182 376 179 +4 354 123 140 121 +4 100 134 211 167 +4 134 396 167 177 +4 87 315 257 183 +4 310 179 270 81 +4 379 380 253 272 +4 333 253 316 379 +4 333 380 253 379 +4 143 176 375 273 +4 273 276 108 248 +4 276 273 108 274 +4 93 80 381 274 +4 320 319 84 275 +4 242 198 115 187 +4 9 112 198 119 +4 127 45 109 118 +4 182 183 272 317 +4 385 189 93 108 +4 113 404 192 403 +4 108 384 381 385 +4 108 381 93 385 +4 29 126 133 36 +4 108 276 381 384 +4 404 403 340 10 +4 385 195 321 322 +4 385 322 411 189 +4 385 93 189 398 +4 147 287 221 40 +4 221 288 147 40 +4 323 185 195 277 +4 382 180 318 194 +4 56 158 266 143 +4 174 56 266 143 +4 323 85 277 248 +4 126 26 19 16 +4 126 26 25 19 +4 137 10 352 218 +4 195 91 252 185 +4 149 233 157 54 +4 350 118 109 33 +4 385 321 195 384 +4 266 389 374 253 +4 216 176 266 143 +4 266 143 176 174 +4 174 143 176 375 +4 394 371 166 197 +4 171 390 169 410 +4 368 390 394 410 +4 164 394 371 166 +4 171 253 316 389 +4 115 264 128 119 +4 144 154 161 394 +4 104 264 112 126 +4 79 234 169 179 +4 102 266 402 154 +4 128 209 119 264 +4 262 135 130 97 +4 169 161 55 236 +4 163 76 216 275 +4 23 198 115 119 +4 57 63 67 163 +4 183 171 390 182 +4 141 249 335 336 +4 162 253 180 169 +4 130 147 156 202 +4 393 157 210 149 +4 111 255 141 262 +4 255 211 141 137 +4 275 250 189 319 +4 97 156 220 225 +4 180 194 253 108 +4 156 216 163 65 +4 148 368 164 394 +4 102 162 402 216 +4 126 113 114 291 +4 126 113 291 16 +4 244 182 390 179 +4 410 169 179 390 +4 155 162 156 65 +4 232 155 65 247 +4 110 349 140 111 +4 275 108 216 172 +4 140 130 262 136 +4 347 343 115 132 +4 120 96 102 107 +4 59 393 157 172 +4 160 59 393 157 +4 208 287 213 20 +4 286 20 208 213 +4 89 319 320 189 +4 36 114 126 219 +4 219 168 133 36 +4 98 369 397 239 +4 246 169 55 236 +4 281 5 138 103 +4 281 184 138 5 + +CELL_TYPES 1402 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 +10 diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp new file mode 100644 index 000000000..ef9c8da47 --- /dev/null +++ b/examples/DeformableDemo/ClothFriction.cpp @@ -0,0 +1,250 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "ClothFriction.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The ClothFriction shows the use of deformable friction. +class ClothFriction : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + ClothFriction(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~ClothFriction() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 12; + float pitch = -50; + float yaw = 120; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void ClothFriction::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150), btScalar(25.), btScalar(150))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -32, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(3); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + btScalar s = 4; + btScalar h = 0; + + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 10,10, + 0, true); + + psb->getCollisionShape()->setMargin(0.05); + psb->generateBendingConstraints(2); + psb->setTotalMass(1); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 3; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,1, true); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); + + + h = 2; + s = 2; + btSoftBody* psb2 = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 5,5, + 0, true); + psb2->getCollisionShape()->setMargin(0.05); + psb2->generateBendingConstraints(2); + psb2->setTotalMass(1); + psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb2->m_cfg.kCHR = 1; // collision hardness with rigid body + psb2->m_cfg.kDF = 20; + psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->translate(btVector3(0,0,0)); + getDeformableDynamicsWorld()->addSoftBody(psb2); + + btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(10,1, true); + getDeformableDynamicsWorld()->addForce(psb2, mass_spring2); + m_forces.push_back(mass_spring2); + + btDeformableGravityForce* gravity_force2 = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb2, gravity_force2); + m_forces.push_back(gravity_force2); + } + getDeformableDynamicsWorld()->setImplicit(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void ClothFriction::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + +class CommonExampleInterface* ClothFrictionCreateFunc(struct CommonExampleOptions& options) +{ + return new ClothFriction(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/ClothFriction.h b/examples/DeformableDemo/ClothFriction.h new file mode 100644 index 000000000..228ec659c --- /dev/null +++ b/examples/DeformableDemo/ClothFriction.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef CLOTH_FRICTION_H +#define CLOTH_FRICTION_H + +class CommonExampleInterface* ClothFrictionCreateFunc(struct CommonExampleOptions& options); + +#endif //CLOTH_FRICTION_H diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp new file mode 100644 index 000000000..12a81543b --- /dev/null +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -0,0 +1,252 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "DeformableContact.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The DeformableContact shows the contact between deformable objects + +class DeformableContact : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + DeformableContact(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableContact() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 12; + float pitch = -50; + float yaw = 120; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void DeformableContact::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150), btScalar(25.), btScalar(150))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -32, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(2); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + btScalar s = 4; + btScalar h = 0; + + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 20,20, + 1 + 2 + 4 + 8, true); + + psb->getCollisionShape()->setMargin(0.1); + psb->generateBendingConstraints(2); + psb->setTotalMass(1); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,1, true); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); + + + h = 2; + s = 2; + btSoftBody* psb2 = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 10,10, + 0, true); + psb2->getCollisionShape()->setMargin(0.1); + psb2->generateBendingConstraints(2); + psb2->setTotalMass(1); + psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb2->m_cfg.kCHR = 1; // collision hardness with rigid body + psb2->m_cfg.kDF = 0.1; + psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->translate(btVector3(3.5,0,0)); + getDeformableDynamicsWorld()->addSoftBody(psb2); + + btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(10,1, true); + getDeformableDynamicsWorld()->addForce(psb2, mass_spring2); + m_forces.push_back(mass_spring2); + + btDeformableGravityForce* gravity_force2 = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb2, gravity_force2); + m_forces.push_back(gravity_force2); + } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableContact::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + +class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableContact(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableContact.h b/examples/DeformableDemo/DeformableContact.h new file mode 100644 index 000000000..1e1fe5676 --- /dev/null +++ b/examples/DeformableDemo/DeformableContact.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef DEFORMABLE_CONTACT_H +#define DEFORMABLE_CONTACT_H + +class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_CONTACT_H diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index 5ffaefc36..d7f254960 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -11,20 +11,6 @@ 3. This notice may not be removed or altered from any source distribution. */ -///create 125 (5x5x5) dynamic object -#define ARRAY_SIZE_X 5 -#define ARRAY_SIZE_Y 5 -#define ARRAY_SIZE_Z 5 - -//maximum number of objects (and allow user to shoot additional boxes) -#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024) - -///scaling of the objects (0.1 = 20 centimeter boxes ) -#define SCALING 1. -#define START_POS_X -5 -#define START_POS_Y -5 -#define START_POS_Z -3 - #include "DeformableMultibody.h" ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. #include "btBulletDynamicsCommon.h" @@ -235,7 +221,7 @@ void DeformableMultibody::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); } - + getDeformableDynamicsWorld()->setImplicit(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } @@ -360,26 +346,23 @@ void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btM local_origin[0] = pMultiBody->getBasePos(); { - // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; - if (1) - { - btCollisionShape* box = new btBoxShape(baseHalfExtents); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); - col->setCollisionShape(box); - - btTransform tr; - tr.setIdentity(); - tr.setOrigin(local_origin[0]); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - col->setWorldTransform(tr); - - pWorld->addCollisionObject(col, 2, 1 + 2); - - col->setFriction(friction); - pMultiBody->setBaseCollider(col); - } + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2, 1 + 2); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); } for (int i = 0; i < pMultiBody->getNumLinks(); ++i) @@ -392,7 +375,6 @@ void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btM for (int i = 0; i < pMultiBody->getNumLinks(); ++i) { btVector3 posr = local_origin[i + 1]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 850f72600..1bd437a49 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -10,21 +10,6 @@ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ - -///create 125 (5x5x5) dynamic object -#define ARRAY_SIZE_X 5 -#define ARRAY_SIZE_Y 5 -#define ARRAY_SIZE_Z 5 - -//maximum number of objects (and allow user to shoot additional boxes) -#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024) - -///scaling of the objects (0.1 = 20 centimeter boxes ) -#define SCALING 1. -#define START_POS_X -5 -#define START_POS_Y -5 -#define START_POS_Z -3 - #include "DeformableRigid.h" ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. #include "btBulletDynamicsCommon.h" @@ -39,9 +24,7 @@ #include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../Utils/b3ResourcePath.h" -///The DeformableRigid shows the use of rolling friction. -///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. -///Generally it is best to leave the rolling friction coefficient zero (or close to zero). +///The DeformableRigid shows contact between deformable objects and rigid objects. class DeformableRigid : public CommonRigidBodyBase { btAlignedObjectArray m_forces; @@ -238,7 +221,7 @@ void DeformableRigid::initPhysics() psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.1, true); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); @@ -248,6 +231,8 @@ void DeformableRigid::initPhysics() // add a few rigid bodies Ctor_RbUpStack(1); } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp new file mode 100644 index 000000000..be09af17c --- /dev/null +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -0,0 +1,233 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ +#include "DeformableSelfCollision.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The DeformableSelfCollision shows deformable self collisions +class DeformableSelfCollision : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + DeformableSelfCollision(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableSelfCollision() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 10; + float pitch = -8; + float yaw = 100; + float targetPos[3] = {0, -10, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void DeformableSelfCollision::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + // deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + btVector3 gravity = btVector3(0, -100, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + // getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -35, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(40); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + const btScalar s = 2; + const btScalar h = 0; + + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -4*s), + btVector3(+s, h, -4*s), + btVector3(-s, h, +4*s), + btVector3(+s, h, +4*s), + 10,40, + 0, true, 0.01); + + + psb->getCollisionShape()->setMargin(0.2); + psb->generateBendingConstraints(2); + psb->setTotalMass(1); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0.2; + psb->rotate(btQuaternion(0,SIMD_PI / 2, 0)); + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,0.2, true); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); + } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableSelfCollision::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* DeformableSelfCollisionCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableSelfCollision(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableSelfCollision.h b/examples/DeformableDemo/DeformableSelfCollision.h new file mode 100644 index 000000000..36359c0d9 --- /dev/null +++ b/examples/DeformableDemo/DeformableSelfCollision.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef DEFORMABLE_SELF_COLLISION_H +#define DEFORMABLE_SELF_COLLISION_H + +class CommonExampleInterface* DeformableSelfCollisionCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_SELF_COLLISION_H diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 34127774d..41f75a4cf 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -34,9 +34,7 @@ #include "../CommonInterfaces/CommonFileIOInterface.h" #include "Bullet3Common/b3FileUtils.h" -///The GraspDeformable shows the use of rolling friction. -///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. -///Generally it is best to leave the rolling friction coefficient zero (or close to zero). +///The GraspDeformable shows grasping a volumetric deformable objects with multibody gripper with moter constraints. static btScalar sGripperVerticalVelocity = 0.f; static btScalar sGripperClosingTargetVelocity = 0.f; static float friction = 1.; @@ -103,11 +101,6 @@ public: btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; if (motor) { -// if (dofIndex == 10 || dofIndex == 11) -// { -// motor->setVelocityTarget(fingerTargetVelocities[1], 1); -// motor->setMaxAppliedImpulse(1); -// } if (dofIndex == 6) { motor->setVelocityTarget(-fingerTargetVelocities[1], 1); @@ -118,7 +111,6 @@ public: motor->setVelocityTarget(fingerTargetVelocities[1], 1); motor->setMaxAppliedImpulse(2); } -// motor->setRhsClamp(SIMD_INFINITY); motor->setMaxAppliedImpulse(1); } } @@ -127,7 +119,7 @@ public: } //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 250.f; + float internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } @@ -192,54 +184,11 @@ void GraspDeformable::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); -// deformableBodySolver->setWorld(getDeformableDynamicsWorld()); btVector3 gravity = btVector3(0, -9.81, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); -// // load a gripper -// { -// btTransform rootTrans; -// rootTrans.setIdentity(); -// BulletURDFImporter u2b(m_guiHelper,0,0,50,0); -// bool forceFixedBase = false; -// bool loadOk = u2b.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", forceFixedBase); -// if (loadOk) -// { -// for (int m = 0; m < u2b.getNumModels(); m++) -// { -// u2b.activateModel(m); -// -// btMultiBody* mb = 0; -// -// MyMultiBodyCreator creation(m_guiHelper); -// -// u2b.getRootTransformInWorld(rootTrans); -// ConvertURDF2Bullet(u2b, creation, rootTrans, getDeformableDynamicsWorld(), true, u2b.getPathPrefix(), CUF_USE_SDF+CUF_RESERVED); -// mb = creation.getBulletMultiBody(); -// -// int numLinks = mb->getNumLinks(); -// for (int i = 0; i < numLinks; i++) -// { -// int mbLinkIndex = i; -// float maxMotorImpulse = 1.f; -// -// if (supportsJointMotor(mb, mbLinkIndex)) -// { -// int dof = 0; -// btScalar desiredVelocity = 0.f; -// btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse); -// motor->setPositionTarget(0, 0); -// motor->setVelocityTarget(0, 1); -// mb->getLink(mbLinkIndex).m_userPtr = motor; -// getDeformableDynamicsWorld()->addMultiBodyConstraint(motor); -// motor->finalizeMultiDof(); -// } -// } -// } -// } -// } // build a gripper { bool damping = true; @@ -273,7 +222,6 @@ void GraspDeformable::initPhysics() } } - if (!damping) { mbC->setLinearDamping(0.0f); @@ -287,10 +235,9 @@ void GraspDeformable::initPhysics() btScalar q0 = 0.f * SIMD_PI / 180.f; if (numLinks > 0) mbC->setJointPosMultiDof(0, &q0); - /// addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents); - } + //create a ground { btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); @@ -325,12 +272,13 @@ void GraspDeformable::initPhysics() { char relative_path[1024]; // b3FileUtils::findFile("banana.vtk", relative_path, 1024); - b3FileUtils::findFile("ball.vtk", relative_path, 1024); +// b3FileUtils::findFile("ball.vtk", relative_path, 1024); +// b3FileUtils::findFile("paper_collision.vtk", relative_path, 1024); // b3FileUtils::findFile("single_tet.vtk", relative_path, 1024); -// b3FileUtils::findFile("tube.vtk", relative_path, 1024); -// b3FileUtils::findFile("torus.vtk", relative_path, 1024); -// b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024); -// b3FileUtils::findFile("bread.vtk", relative_path, 1024); + b3FileUtils::findFile("tube.vtk", relative_path, 1024); +// b3FileUtils::findFile("torus.vtk", relative_path, 1024); +// b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024); +// b3FileUtils::findFile("bread.vtk", relative_path, 1024); // b3FileUtils::findFile("ditto.vtk", relative_path, 1024); // b3FileUtils::findFile("boot.vtk", relative_path, 1024); // btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), @@ -341,27 +289,24 @@ void GraspDeformable::initPhysics() btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), relative_path); // psb->scale(btVector3(30, 30, 30)); // for banana - psb->scale(btVector3(.25, .25, .25)); -// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot -// psb->scale(btVector3(1, 1, 1)); // for ditto - psb->translate(btVector3(.25, 0, 0.4)); - psb->getCollisionShape()->setMargin(0.02); - psb->setTotalMass(.1); +// psb->scale(btVector3(.2, .2, .2)); +// psb->scale(btVector3(2, 2, 2)); + psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot +// psb->scale(btVector3(.1, .1, .1)); // for ditto +// psb->translate(btVector3(.25, 2, 0.4)); + psb->getCollisionShape()->setMargin(0.01); + psb->setTotalMass(.01); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 2; + psb->m_cfg.kDF = 20; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); - - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,.04, true); - getDeformableDynamicsWorld()->addForce(psb, mass_spring); - m_forces.push_back(mass_spring); - + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10); + + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(10,20, 0.01); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } @@ -369,7 +314,7 @@ void GraspDeformable::initPhysics() // // create a piece of cloth // { // bool onGround = false; -// const btScalar s = 4; +// const btScalar s = .4; // btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), // btVector3(+s, 0, -s), // btVector3(-s, 0, +s), @@ -386,17 +331,17 @@ void GraspDeformable::initPhysics() // 2,2, // 0, true); // -// psb->getCollisionShape()->setMargin(0.1); +// psb->getCollisionShape()->setMargin(0.02); // psb->generateBendingConstraints(2); -// psb->setTotalMass(1); -// psb->setSpringStiffness(2); -// psb->setDampingCoefficient(0.03); +// psb->setTotalMass(.01); +// psb->setSpringStiffness(5); +// psb->setDampingCoefficient(0.05); // psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects // psb->m_cfg.kCHR = 1; // collision hardness with rigid body // psb->m_cfg.kDF = 1; // psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; // getDeformableDynamicsWorld()->addSoftBody(psb); -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); +// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.2,0.02, true)); // getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); // } diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index 661698a06..7c966cc4d 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -11,20 +11,6 @@ 3. This notice may not be removed or altered from any source distribution. */ -///create 125 (5x5x5) dynamic object -#define ARRAY_SIZE_X 5 -#define ARRAY_SIZE_Y 5 -#define ARRAY_SIZE_Z 5 - -//maximum number of objects (and allow user to shoot additional boxes) -#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024) - -///scaling of the objects (0.1 = 20 centimeter boxes ) -#define SCALING 1. -#define START_POS_X -5 -#define START_POS_Y -5 -#define START_POS_Z -3 - #include "Pinch.h" ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. #include "btBulletDynamicsCommon.h" @@ -39,21 +25,13 @@ #include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../Utils/b3ResourcePath.h" -///The Pinch shows the use of rolling friction. -///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. -///Generally it is best to leave the rolling friction coefficient zero (or close to zero). +///The Pinch shows the frictional contact between kinematic rigid objects with deformable objects struct TetraCube { #include "../SoftDemo/cube.inl" }; -struct TetraBunny -{ -#include "../SoftDemo/bunny.inl" -}; - - class Pinch : public CommonRigidBodyBase { btAlignedObjectArray m_forces; @@ -351,6 +329,7 @@ void Pinch::initPhysics() // add a grippers createGrip(); } + getDeformableDynamicsWorld()->setImplicit(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp new file mode 100644 index 000000000..72e864ac3 --- /dev/null +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -0,0 +1,409 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ +#include "PinchFriction.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The PinchFriction shows the frictional contacts among volumetric deformable objects + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +class PinchFriction : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + PinchFriction(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~PinchFriction() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 25; + float pitch = -30; + float yaw = 100; + float targetPos[3] = {0, -0, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + void createGrip() + { + int count = 2; + float mass = 1e6; + btCollisionShape* shape[] = { + new btBoxShape(btVector3(3, 3, 0.5)), + }; + static const int nshapes = sizeof(shape) / sizeof(shape[0]); + for (int i = 0; i < count; ++i) + { + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(10, 0, 0)); + startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + createRigidBody(mass, startTransform, shape[i % nshapes]); + } + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); +// btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); +// +// for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) +// { +// btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; +// { +// btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); +// btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); +// } +// } + } +}; + +void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world) +{ + btAlignedObjectArray& rbs = world->getNonStaticRigidBodies(); + if (rbs.size()<2) + return; + btRigidBody* rb0 = rbs[0]; + btScalar pressTime = 0.9; + btScalar liftTime = 10; + btScalar shiftTime = 3.5; + btScalar holdTime = 4.5*1000; + btScalar dropTime = 5.3*1000; + btTransform rbTransform; + rbTransform.setIdentity(); + btVector3 translation; + btVector3 velocity; + + btVector3 initialTranslationLeft = btVector3(0.5,3,4); + btVector3 initialTranslationRight = btVector3(0.5,3,-4); + btVector3 PinchFrictionVelocityLeft = btVector3(0,0,-1); + btVector3 PinchFrictionVelocityRight = btVector3(0,0,1); + btVector3 liftVelocity = btVector3(0,1,0); + btVector3 shiftVelocity = btVector3(0,0,0); + btVector3 holdVelocity = btVector3(0,0,0); + btVector3 openVelocityLeft = btVector3(0,0,4); + btVector3 openVelocityRight = btVector3(0,0,-4); + + if (time < pressTime) + { + velocity = PinchFrictionVelocityLeft; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityLeft; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime); + } + else + { + velocity = holdVelocity; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime); + } + rbTransform.setOrigin(translation); + rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + rb0->setCenterOfMassTransform(rbTransform); + rb0->setAngularVelocity(btVector3(0,0,0)); + rb0->setLinearVelocity(velocity); + + btRigidBody* rb1 = rbs[1]; + if (time < pressTime) + { + velocity = PinchFrictionVelocityRight; + translation = initialTranslationRight + PinchFrictionVelocityRight * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityRight; + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime); + } + else + { + velocity = holdVelocity; + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime); + } + rbTransform.setOrigin(translation); + rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + rb1->setCenterOfMassTransform(rbTransform); + rb1->setAngularVelocity(btVector3(0,0,0)); + rb1->setLinearVelocity(velocity); + + rb0->setFriction(200); + rb1->setFriction(200); +} + +void PinchFriction::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + getDeformableDynamicsWorld()->setSolverCallback(dynamics2); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + //create a ground + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -25, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(0); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a soft block + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + + psb->scale(btVector3(2, 2, 1)); + psb->translate(btVector3(0, 2.1, 2.2)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(.6); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 20; + btSoftBodyHelpers::generateBoundaryFaces(psb); + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); + + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); + getDeformableDynamicsWorld()->addForce(psb, neohookean); + m_forces.push_back(neohookean); + } + + // create a second soft block + { + btSoftBody* psb2 = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + + psb2->scale(btVector3(2, 2, 1)); + psb2->translate(btVector3(0, 2.1, -2.2)); + psb2->getCollisionShape()->setMargin(0.1); + psb2->setTotalMass(.6); + psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb2->m_cfg.kCHR = 1; // collision hardness with rigid body + psb2->m_cfg.kDF = 20; + psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + btSoftBodyHelpers::generateBoundaryFaces(psb2); + getDeformableDynamicsWorld()->addSoftBody(psb2); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb2, gravity_force); + m_forces.push_back(gravity_force); + + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); + getDeformableDynamicsWorld()->addForce(psb2, neohookean); + m_forces.push_back(neohookean); + } + + // create a third soft block + { + btSoftBody* psb3 = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + + psb3->scale(btVector3(2, 2, 1)); + psb3->translate(btVector3(0, 2.1, 0)); + psb3->getCollisionShape()->setMargin(0.1); + psb3->setTotalMass(.6); + psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb3->m_cfg.kCHR = 1; // collision hardness with rigid body + psb3->m_cfg.kDF = 20; + psb3->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb3->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + btSoftBodyHelpers::generateBoundaryFaces(psb3); + getDeformableDynamicsWorld()->addSoftBody(psb3); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb3, gravity_force); + m_forces.push_back(gravity_force); + + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); + getDeformableDynamicsWorld()->addForce(psb3, neohookean); + m_forces.push_back(neohookean); + } + + // add a pair of grippers + createGrip(); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void PinchFriction::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* PinchFrictionCreateFunc(struct CommonExampleOptions& options) +{ + return new PinchFriction(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/PinchFriction.h b/examples/DeformableDemo/PinchFriction.h new file mode 100644 index 000000000..b397ba342 --- /dev/null +++ b/examples/DeformableDemo/PinchFriction.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _PINCH_FRICTION_H +#define _PINCH_FRICTION_H + +class CommonExampleInterface* PinchFrictionCreateFunc(struct CommonExampleOptions& options); + +#endif //_PINCH_FRICTION_H diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index cff07a175..c1213d520 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -11,20 +11,6 @@ 3. This notice may not be removed or altered from any source distribution. */ -///create 125 (5x5x5) dynamic object -#define ARRAY_SIZE_X 5 -#define ARRAY_SIZE_Y 5 -#define ARRAY_SIZE_Z 5 - -//maximum number of objects (and allow user to shoot additional boxes) -#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024) - -///scaling of the objects (0.1 = 20 centimeter boxes ) -#define SCALING 1. -#define START_POS_X -5 -#define START_POS_Y -5 -#define START_POS_Z -3 - #include "VolumetricDeformable.h" ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. #include "btBulletDynamicsCommon.h" @@ -39,10 +25,7 @@ #include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../Utils/b3ResourcePath.h" -///The VolumetricDeformable shows the use of rolling friction. -///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. -///Generally it is best to leave the rolling friction coefficient zero (or close to zero). - +///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects. struct TetraCube { @@ -108,7 +91,7 @@ public: void Ctor_RbUpStack(int count) { - float mass = 0.02; + float mass = 1; btCompoundShape* cylinderCompound = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); @@ -183,7 +166,7 @@ void VolumetricDeformable::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); - btVector3 gravity = btVector3(0, -10, 0); + btVector3 gravity = btVector3(0, -100, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -236,19 +219,17 @@ void VolumetricDeformable::initPhysics() psb->m_cfg.kDF = 0.5; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(0,0.03); - getDeformableDynamicsWorld()->addForce(psb, mass_spring); - m_forces.push_back(mass_spring); - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.5,2.5); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.02); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); // add a few rigid bodies Ctor_RbUpStack(4); diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 87f47f162..7c31c9d84 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -353,10 +353,18 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/MultiBodyConstraintFeedback.cpp ../SoftDemo/SoftDemo.cpp ../SoftDemo/SoftDemo.h + ../DeformableDemo/DeformableContact.cpp + ../DeformableDemo/DeformableContact.h ../DeformableDemo/GraspDeformable.cpp ../DeformableDemo/GraspDeformable.h ../DeformableDemo/Pinch.cpp ../DeformableDemo/Pinch.h + ../DeformableDemo/DeformableSelfCollision.cpp + ../DeformableDemo/DeformableSelfCollision.h + ../DeformableDemo/PinchFriction.cpp + ../DeformableDemo/PinchFriction.h + ../DeformableDemo/ClothFriction.cpp + ../DeformableDemo/ClothFriction.h ../DeformableDemo/DeformableMultibody.cpp ../DeformableDemo/DeformableMultibody.h ../DeformableDemo/DeformableRigid.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 1c8fa7921..e705fd075 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -45,10 +45,14 @@ #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../DeformableDemo/DeformableRigid.h" +#include "../DeformableDemo/ClothFriction.h" #include "../DeformableDemo/Pinch.h" +#include "../DeformableDemo/DeformableSelfCollision.h" +#include "../DeformableDemo/PinchFriction.h" #include "../DeformableDemo/DeformableMultibody.h" #include "../DeformableDemo/VolumetricDeformable.h" #include "../DeformableDemo/GraspDeformable.h" +#include "../DeformableDemo/DeformableContact.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -186,6 +190,10 @@ static ExampleEntry gDefaultExamples[] = //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3), ExampleEntry(0, "Deformabe Body"), + ExampleEntry(1, "Deformable Self Collision", "Deformable Self Collision", DeformableSelfCollisionCreateFunc), + ExampleEntry(1, "Deformable-Deformable Contact", "Deformable contact", DeformableContactCreateFunc), + ExampleEntry(1, "Cloth Friction", "Cloth friction contact", ClothFrictionCreateFunc), + ExampleEntry(1, "Deformable-Deformable Friction Contact", "Deformable friction contact", PinchFrictionCreateFunc), ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc), ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 2db272fa8..1963c3473 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -376,7 +376,7 @@ B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle comm return 0; } -B3_SHARED_API int b3LoadSoftBodySetCollsionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness) +B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 886e7a6ea..64ef41bc7 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -635,7 +635,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda); B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); - B3_SHARED_API int b3LoadSoftBodySetCollsionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); + B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 62a0e3ac7..78087a686 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5055,15 +5055,24 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S { btSoftBody* psb = bodyHandle->m_softBody; int totalBytesPerVertex = sizeof(btVector3); - int numVertices = psb->m_nodes.size(); + bool separateRenderMesh = (psb->m_renderNodes.size() != 0); + int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); btVector3* verticesOut = (btVector3*)bufferServerToClient; for (int i = 0; i < verticesCopied; ++i) { - const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; - verticesOut[i] = n.m_x; + if (separateRenderMesh) + { + const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; + verticesOut[i] = n.m_x; + } + else + { + const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; + verticesOut[i] = n.m_x; + } } serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; @@ -7989,200 +7998,230 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar } { - CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); - char relativeFileName[1024]; - char pathPrefix[1024]; - pathPrefix[0] = 0; - if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024)) - { - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); - } - const std::string& error_message_prefix = ""; - std::string out_found_filename; - int out_type=0; + btSoftBody* psb = NULL; + - bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - btSoftBody* psb = NULL; - if (foundFile) - { - btScalar spring_elastic_stiffness, spring_damping_stiffness; - if (out_type == UrdfGeometry::FILE_OBJ) - { - std::vector shapes; - tinyobj::attrib_t attribute; - std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); - if (!shapes.empty()) - { - const tinyobj::shape_t& shape = shapes[0]; - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - for (int i = 0; i < attribute.vertices.size(); i++) - { - vertices.push_back(attribute.vertices[i]); - } - for (int i = 0; i < shape.mesh.indices.size(); i++) - { - indices.push_back(shape.mesh.indices[i].vertex_index); - } - int numTris = shape.mesh.indices.size() / 3; - if (numTris > 0) - { - psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); - } - } + CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + // add _sim.vtk postfix + char relativeSimFileName[1024]; + char sim_file_ending[9] = "_sim.vtk"; + strncpy(relativeSimFileName, relativeFileName, strlen(relativeFileName)); + strncpy(relativeSimFileName + strlen(relativeFileName)-4, sim_file_ending, sizeof(sim_file_ending)); + + const std::string& error_message_prefix = ""; + std::string out_found_filename; + std::string out_found_sim_filename; + int out_type, out_sim_type; + + bool render_mesh_is_sim_mesh = true; + + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + if (out_type == UrdfGeometry::FILE_OBJ) + { + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeSimFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type); + render_mesh_is_sim_mesh = !foundFile; + } + + if (render_mesh_is_sim_mesh) + { + out_sim_type = out_type; + out_found_sim_filename = out_found_filename; + } + + if (out_sim_type == UrdfGeometry::FILE_OBJ) + { + std::vector shapes; + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO); + if (!shapes.empty()) + { + const tinyobj::shape_t& shape = shapes[0]; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + for (int i = 0; i < attribute.vertices.size(); i++) + { + vertices.push_back(attribute.vertices[i]); + } + for (int i = 0; i < shape.mesh.indices.size(); i++) + { + indices.push_back(shape.mesh.indices[i].vertex_index); + } + int numTris = shape.mesh.indices.size() / 3; + if (numTris > 0) + { + psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + } + } #ifndef SKIP_DEFORMABLE_BODY - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) - { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false)); - } + btScalar spring_elastic_stiffness, spring_damping_stiffness; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) + { + spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; + spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false)); + } +#endif + } + else if (out_sim_type == UrdfGeometry::FILE_VTK) + { +#ifndef SKIP_DEFORMABLE_BODY + psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_sim_filename.c_str()); + btScalar corotated_mu, corotated_lambda; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) + { + corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; + corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda)); + } + btScalar neohookean_mu, neohookean_lambda; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) + { + neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; + neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda)); + } + btScalar spring_elastic_stiffness, spring_damping_stiffness; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) + { + spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; + spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true)); + } #endif } - else if (out_type == UrdfGeometry::FILE_VTK) - { -#ifndef SKIP_DEFORMABLE_BODY - psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_filename.c_str()); - btScalar corotated_mu, corotated_lambda; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) - { - corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; - corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda)); - } - btScalar neohookean_mu, neohookean_lambda; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) - { - neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; - neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda)); - } - btScalar spring_elastic_stiffness, spring_damping_stiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) - { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true)); - } -#endif - } - } + if (psb != NULL) { #ifndef SKIP_DEFORMABLE_BODY - btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE) - { - m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity)); - } - btScalar collision_hardness = 1; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_COLLISION_HARDNESS) - { - collision_hardness = loadSoftBodyArgs.m_collisionHardness; - } - psb->m_cfg.kKHR = collision_hardness; - psb->m_cfg.kCHR = collision_hardness; + if (!render_mesh_is_sim_mesh) + { + // load render mesh + btSoftBodyHelpers::readRenderMeshFromObj(out_found_filename.c_str(), psb); + btSoftBodyHelpers::interpolateBarycentricWeights(psb); + } + else + { + psb->m_renderNodes.resize(0); + } + btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE) + { + m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity)); + } + btScalar collision_hardness = 1; + psb->m_cfg.kKHR = collision_hardness; + psb->m_cfg.kCHR = collision_hardness; - btScalar friction_coeff = 0; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT) - { - friction_coeff = loadSoftBodyArgs.m_frictionCoeff; - } - psb->m_cfg.kDF = friction_coeff; + btScalar friction_coeff = 0; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT) + { + friction_coeff = loadSoftBodyArgs.m_frictionCoeff; + } + psb->m_cfg.kDF = friction_coeff; + + bool use_bending_spring = true; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) + { + use_bending_spring = loadSoftBodyArgs.m_useBendingSprings; + } + btSoftBody::Material* pm = psb->appendMaterial(); + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + if (use_bending_spring) + { + psb->generateBendingConstraints(2,pm); + } - bool use_bending_spring = true; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) - { - use_bending_spring = loadSoftBodyArgs.m_useBendingSprings; - } - btSoftBody::Material* pm = psb->appendMaterial(); - pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - if (use_bending_spring) - { - psb->generateBendingConstraints(2, pm); - } - - // turn on the collision flag for deformable - // collision - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - psb->setCollisionFlags(0); - psb->setTotalMass(mass); + // turn on the collision flag for deformable + // collision between deformable and rigid + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + // collion between deformable and deformable and self-collision + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->setCollisionFlags(0); + psb->setTotalMass(mass); #else - btSoftBody::Material* pm = psb->appendMaterial(); - pm->m_kLST = 0.5; - pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - psb->generateBendingConstraints(2, pm); - psb->m_cfg.piterations = 20; - psb->m_cfg.kDF = 0.5; - //turn on softbody vs softbody collision - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; - psb->randomizeConstraints(); - psb->setTotalMass(mass, true); + btSoftBody::Material* pm = psb->appendMaterial(); + pm->m_kLST = 0.5; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2, pm); + psb->m_cfg.piterations = 20; + psb->m_cfg.kDF = 0.5; + //turn on softbody vs softbody collision + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; + psb->randomizeConstraints(); + psb->setTotalMass(mass, true); #endif - psb->scale(btVector3(scale, scale, scale)); - psb->rotate(initialOrn); - psb->translate(initialPos); + psb->scale(btVector3(scale, scale, scale)); + psb->rotate(initialOrn); + psb->translate(initialPos); - psb->getCollisionShape()->setMargin(collisionMargin); - psb->getCollisionShape()->setUserPointer(psb); - m_data->m_dynamicsWorld->addSoftBody(psb); - m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape()); - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - bodyHandle->m_softBody = psb; + psb->getCollisionShape()->setMargin(collisionMargin); + psb->getCollisionShape()->setUserPointer(psb); + m_data->m_dynamicsWorld->addSoftBody(psb); + m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape()); + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + bodyHandle->m_softBody = psb; - b3VisualShapeData visualShape; + b3VisualShapeData visualShape; - visualShape.m_objectUniqueId = bodyUniqueId; - visualShape.m_linkIndex = -1; - visualShape.m_visualGeometryType = URDF_GEOM_MESH; - //dimensions just contains the scale - visualShape.m_dimensions[0] = scale; - visualShape.m_dimensions[1] = scale; - visualShape.m_dimensions[2] = scale; - //filename - strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN); - visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; - //position and orientation - visualShape.m_localVisualFrame[0] = initialPos[0]; - visualShape.m_localVisualFrame[1] = initialPos[1]; - visualShape.m_localVisualFrame[2] = initialPos[2]; - visualShape.m_localVisualFrame[3] = initialOrn[0]; - visualShape.m_localVisualFrame[4] = initialOrn[1]; - visualShape.m_localVisualFrame[5] = initialOrn[2]; - visualShape.m_localVisualFrame[6] = initialOrn[3]; - //color and ids to be set by the renderer - visualShape.m_rgbaColor[0] = 0; - visualShape.m_rgbaColor[1] = 0; - visualShape.m_rgbaColor[2] = 0; - visualShape.m_rgbaColor[3] = 1; - visualShape.m_tinyRendererTextureId = -1; - visualShape.m_textureUniqueId = -1; - visualShape.m_openglTextureId = -1; + visualShape.m_objectUniqueId = bodyUniqueId; + visualShape.m_linkIndex = -1; + visualShape.m_visualGeometryType = URDF_GEOM_MESH; + //dimensions just contains the scale + visualShape.m_dimensions[0] = scale; + visualShape.m_dimensions[1] = scale; + visualShape.m_dimensions[2] = scale; + //filename + strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN); + visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; + //position and orientation + visualShape.m_localVisualFrame[0] = initialPos[0]; + visualShape.m_localVisualFrame[1] = initialPos[1]; + visualShape.m_localVisualFrame[2] = initialPos[2]; + visualShape.m_localVisualFrame[3] = initialOrn[0]; + visualShape.m_localVisualFrame[4] = initialOrn[1]; + visualShape.m_localVisualFrame[5] = initialOrn[2]; + visualShape.m_localVisualFrame[6] = initialOrn[3]; + //color and ids to be set by the renderer + visualShape.m_rgbaColor[0] = 0; + visualShape.m_rgbaColor[1] = 0; + visualShape.m_rgbaColor[2] = 0; + visualShape.m_rgbaColor[3] = 1; + visualShape.m_tinyRendererTextureId = -1; + visualShape.m_textureUniqueId =-1; + visualShape.m_openglTextureId = -1; - m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO); + m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO); - serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; - serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; + serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; + serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; - int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; #ifdef ENABLE_LINK_MAPPER - if (m_data->m_urdfLinkNameMapper.size()) - { - serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize(); - } + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize(); + } #endif - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); - b3Notification notification; - notification.m_notificationType = BODY_ADDED; - notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId; - m_data->m_pluginManager.addNotification(notification); + b3Notification notification; + notification.m_notificationType = BODY_ADDED; + notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId; + m_data->m_pluginManager.addNotification(notification); } } #endif @@ -8879,8 +8918,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S { SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; - serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY; - + serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY; + btMultiBody* mb = body->m_multiBody; if (linkIndex == -1) { @@ -8994,7 +9033,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S { SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; - serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY; + serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY; btRigidBody* rb = body->m_rigidBody; serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = rb->getFriction(); @@ -9005,8 +9044,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S serverCmd.m_dynamicsInfo.m_mass = rb->getMass(); } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - else if (body && body->m_softBody) - { + else if (body && body->m_softBody){ SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY; diff --git a/examples/pybullet/examples/load_soft_body.py b/examples/pybullet/examples/load_soft_body.py index 401c25a92..1d1052c88 100644 --- a/examples/pybullet/examples/load_soft_body.py +++ b/examples/pybullet/examples/load_soft_body.py @@ -5,6 +5,7 @@ physicsClient = p.connect(p.DIRECT) p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf") +boxId = p.loadURDF("cube.urdf", useMaximalCoordinates = True) bunnyId = p.loadSoftBody("bunny.obj") #meshData = p.getMeshData(bunnyId) #print("meshData=",meshData) @@ -14,6 +15,10 @@ useRealTimeSimulation = 1 if (useRealTimeSimulation): p.setRealTimeSimulation(1) +print(p.getDynamicsInfo(planeId, -1)) +print(p.getDynamicsInfo(bunnyId, 0)) +print(p.getDynamicsInfo(boxId, -1)) + while p.isConnected(): p.setGravity(0, 0, -10) if (useRealTimeSimulation): diff --git a/setup.py b/setup.py index 88d3e47c1..b661d632a 100644 --- a/setup.py +++ b/setup.py @@ -250,6 +250,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp"]\ +["src/BulletSoftBody/btDeformableBodySolver.cpp"]\ +["src/BulletSoftBody/btDeformableContactProjection.cpp"]\ ++["src/BulletSoftBody/btDeformableContactConstraint.cpp"]\ +["src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp"]\ +["src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp"]\ +["src/BulletInverseDynamics/IDMath.cpp"]\ diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h index a316dbf20..324c499e2 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -21,7 +21,6 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btAabbUtil2.h" - // // Compile time configuration // @@ -131,6 +130,8 @@ subject to the following restrictions: /* btDbvtAabbMm */ struct btDbvtAabbMm { + DBVT_INLINE btDbvtAabbMm(){} + DBVT_INLINE btDbvtAabbMm(const btDbvtAabbMm& other): mi(other.mi), mx(other.mx){} DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); } DBVT_INLINE btVector3 Lengths() const { return (mx - mi); } DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); } @@ -190,6 +191,36 @@ struct btDbvtNode }; }; +/* btDbv(normal)tNode */ +struct btDbvntNode +{ + btDbvtVolume volume; + btVector3 normal; + btScalar angle; + DBVT_INLINE bool isleaf() const { return (childs[1] == 0); } + DBVT_INLINE bool isinternal() const { return (!isleaf()); } + btDbvntNode* childs[2]; + void* data; + + btDbvntNode(const btDbvtNode* n) + : volume(n->volume) + , angle(0) + , normal(0,0,0) + , data(n->data) + { + childs[0] = 0; + childs[1] = 0; + } + + ~btDbvntNode() + { + if (childs[0]) + delete childs[0]; + if (childs[1]) + delete childs[1]; + } +}; + typedef btAlignedObjectArray btNodeStack; ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). @@ -225,6 +256,14 @@ struct btDbvt btDbvtNode* parent; sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {} }; + + struct sStknNN + { + const btDbvntNode* a; + const btDbvntNode* b; + sStknNN() {} + sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {} + }; // Policies/Interfaces /* ICollide */ @@ -234,6 +273,7 @@ struct btDbvt DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {} DBVT_VIRTUAL void Process(const btDbvtNode*) {} DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); } + DBVT_VIRTUAL void Process(const btDbvntNode*, const btDbvntNode*) {} DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); } DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); } }; @@ -306,6 +346,9 @@ struct btDbvt void collideTT(const btDbvtNode* root0, const btDbvtNode* root1, DBVT_IPOLICY); + DBVT_PREFIX + void selfCollideT(const btDbvntNode* root, + DBVT_IPOLICY); DBVT_PREFIX void collideTTpersistentStack(const btDbvtNode* root0, @@ -837,6 +880,71 @@ inline void btDbvt::collideTT(const btDbvtNode* root0, } } +// +DBVT_PREFIX +inline void btDbvt::selfCollideT(const btDbvntNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if (root) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0] = sStknNN(root, root); + do + { + sStknNN p = stkStack[--depth]; + if (depth > treshold) + { + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal() && p.a->angle > SIMD_PI) + { + stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.a->childs[1]); + stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[1]); + } + } + else if (Intersect(p.a->volume, p.b->volume)) + { + if (p.a->isinternal()) + { + if (p.b->isinternal()) + { + stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[1]); + } + else + { + stkStack[depth++] = sStknNN(p.a->childs[0], p.b); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b); + } + } + else + { + if (p.b->isinternal()) + { + stkStack[depth++] = sStknNN(p.a, p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a, p.b->childs[1]); + } + else + { + policy.Process(p.a, p.b); + } + } + } + } while (depth); + } +} + + DBVT_PREFIX inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0, const btDbvtNode* root1, diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 8c1e9c022..408d5b5d8 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -22,6 +22,7 @@ SET(BulletSoftBody_SRCS btDeformableMultiBodyConstraintSolver.cpp btDeformableContactProjection.cpp btDeformableMultiBodyDynamicsWorld.cpp + btDeformableContactConstraint.cpp ) @@ -54,6 +55,7 @@ SET(BulletSoftBody_HDRS btDeformableMultiBodyConstraintSolver.h btDeformableContactProjection.h btDeformableMultiBodyDynamicsWorld.h + btDeformableContactConstraint.h btSoftBodySolverVertexBuffer.h ) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 9055ad5ce..d047e6d3d 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -20,8 +20,6 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" -//class btDeformableMultiBodyDynamicsWorld; - struct DeformableContactConstraint { const btSoftBody::Node* m_node; @@ -73,6 +71,8 @@ public: typedef btAlignedObjectArray > TArrayStack; btAlignedObjectArray& m_softBodies; const btScalar& m_dt; + // map from node indices to node pointers + const btAlignedObjectArray* m_nodes; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) @@ -95,6 +95,11 @@ public: virtual void reinitialize(bool nodeUpdated) { } + + virtual void setIndices(const btAlignedObjectArray* nodes) + { + m_nodes = nodes; + } }; diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index eee3ea52e..a73199f90 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -17,6 +17,7 @@ #define BT_CONJUGATE_GRADIENT_H #include #include +#include #include #include #include "LinearMath/btQuickprof.h" @@ -26,16 +27,18 @@ class btConjugateGradient typedef btAlignedObjectArray TVStack; TVStack r,p,z,temp; int max_iterations; + btScalar tolerance; public: btConjugateGradient(const int max_it_in) : max_iterations(max_it_in) { + tolerance = 1024 * std::numeric_limits::epsilon(); } virtual ~btConjugateGradient(){} // return the number of iterations taken - int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance, bool verbose = false) + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) { BT_PROFILE("CGSolve"); btAssert(x.size() == b.size()); @@ -48,7 +51,8 @@ public: A.precondition(r, z); A.project(z); btScalar r_dot_z = dot(z,r); - if (r_dot_z < tolerance) { + btScalar local_tolerance = tolerance; + if (std::sqrt(r_dot_z) <= local_tolerance) { if (verbose) { std::cout << "Iteration = 0" << std::endl; @@ -58,11 +62,21 @@ public: } p = z; btScalar r_dot_z_new = r_dot_z; - for (int k = 1; k < max_iterations; k++) { + for (int k = 1; k <= max_iterations; k++) { // temp = A*p A.multiply(p, temp); A.project(temp); // alpha = r^T * z / (p^T * A * p) + if (dot(p,temp) < SIMD_EPSILON) + { + if (verbose) + std::cout << "Encountered negative direction in CG!" << std::endl; + if (k == 1) + { + x = b; + } + return k; + } btScalar alpha = r_dot_z_new / dot(p, temp); // x += alpha * p; multAndAddTo(alpha, p, x); @@ -72,14 +86,15 @@ public: A.precondition(r, z); r_dot_z = r_dot_z_new; r_dot_z_new = dot(r,z); - if (r_dot_z_new < tolerance) { + if (std::sqrt(r_dot_z_new) < local_tolerance) { if (verbose) { std::cout << "ConjugateGradient iterations " << k << std::endl; } return k; } - btScalar beta = r_dot_z_new/ r_dot_z; + + btScalar beta = r_dot_z_new/r_dot_z; p = multAndAdd(beta, p, z); } if (verbose) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 4ca5187c4..49232725a 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -19,7 +19,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : m_softBodies(softBodies) -, projection(m_softBodies, m_dt) +, m_projection(softBodies) , m_backupVelocity(backup_v) , m_implicit(false) { @@ -46,7 +46,7 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar { m_lf[i]->reinitialize(nodeUpdated); } - projection.reinitialize(nodeUpdated); + m_projection.reinitialize(nodeUpdated); m_preconditioner->reinitialize(nodeUpdated); } @@ -84,11 +84,14 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { - // only the velocity of the constrained nodes needs to be updated during contact solve - for (int i = 0; i < projection.m_constraints.size(); ++i) + for (int i = 0; i < m_softBodies.size(); ++i) { - int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); - m_nodes[index]->m_v = m_backupVelocity[index] + dv[index]; + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + node.m_v = m_backupVelocity[node.index] + dv[node.index]; + } } } @@ -126,24 +129,34 @@ void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &r m_lf[i]->addScaledDampingForce(dt, residual); } } - projection.project(residual); + m_projection.project(residual); } btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const { - btScalar norm_squared = 0; + btScalar mag = 0; for (int i = 0; i < residual.size(); ++i) { - norm_squared += residual[i].length2(); + mag += residual[i].length2(); } - return std::sqrt(norm_squared); + return std::sqrt(mag); +} + +btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt) +{ + btScalar e = 0; + for (int i = 0; i < m_lf.size(); ++i) + { + e += m_lf[i]->totalEnergy(dt); + } + return e; } void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) { for (int i = 0; i < m_softBodies.size(); ++i) { - m_softBodies[i]->updateDeformation(); + m_softBodies[i]->advanceDeformation(); } for (int i = 0; i < m_lf.size(); ++i) @@ -170,10 +183,10 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack //set constraints as projections void btDeformableBackwardEulerObjective::setConstraints() { - projection.setConstraints(); + m_projection.setConstraints(); } void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) { - projection.applyDynamicFriction(r); + m_projection.applyDynamicFriction(r); } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index e96738557..7699a195e 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -34,7 +34,7 @@ public: btAlignedObjectArray m_lf; btAlignedObjectArray& m_softBodies; Preconditioner* m_preconditioner; - btDeformableContactProjection projection; + btDeformableContactProjection m_projection; const TVStack& m_backupVelocity; btAlignedObjectArray m_nodes; bool m_implicit; @@ -71,13 +71,7 @@ public: void setDt(btScalar dt); - // enforce constraints in CG solve - void enforceConstraint(TVStack& x) - { - BT_PROFILE("enforceConstraint"); - projection.enforceConstraint(x); - } - + // add friction force to residual void applyDynamicFriction(TVStack& r); // add dv to velocity @@ -90,7 +84,7 @@ public: void project(TVStack& r) { BT_PROFILE("project"); - projection.project(r); + m_projection.project(r); } // perform precondition M^(-1) x = b @@ -99,18 +93,25 @@ public: m_preconditioner->operator()(x,b); } + // reindex all the vertices virtual void updateId() { - size_t id = 0; + size_t node_id = 0; + size_t face_id = 0; m_nodes.clear(); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - psb->m_nodes[j].index = id; + psb->m_nodes[j].index = node_id; m_nodes.push_back(&psb->m_nodes[j]); - ++id; + ++node_id; + } + for (int j = 0; j < psb->m_faces.size(); ++j) + { + psb->m_faces[j].m_index = face_id; + ++face_id; } } } @@ -124,6 +125,9 @@ public: { m_implicit = implicit; } + + // Calculate the total potential energy in the system + btScalar totalEnergy(btScalar dt); }; #endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index a902322ce..10d3775d3 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -16,6 +16,7 @@ #include #include #include "btDeformableBodySolver.h" +#include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" btDeformableBodySolver::btDeformableBodySolver() @@ -23,8 +24,9 @@ btDeformableBodySolver::btDeformableBodySolver() , m_cg(20) , m_maxNewtonIterations(5) , m_newtonTolerance(1e-4) +, m_lineSearch(true) { - m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity); + m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); } btDeformableBodySolver::~btDeformableBodySolver() @@ -49,9 +51,9 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) updateState(); // add the inertia term in the residual int counter = 0; - for (int k = 0; k < m_softBodySet.size(); ++k) + for (int k = 0; k < m_softBodies.size(); ++k) { - btSoftBody* psb = m_softBodySet[k]; + btSoftBody* psb = m_softBodies[k]; for (int j = 0; j < psb->m_nodes.size(); ++j) { if (psb->m_nodes[j].m_im > 0) @@ -63,13 +65,36 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) } m_objective->computeResidual(solverdt, m_residual); - if (m_objective->computeNorm(m_residual) < m_newtonTolerance) + if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) { break; } + // todo xuchenhan@: this really only needs to be calculated once m_objective->applyDynamicFriction(m_residual); - computeStep(m_ddv, m_residual); - updateDv(); + if (m_lineSearch) + { + btScalar inner_product = computeDescentStep(m_ddv,m_residual); + btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 + btScalar scale = 2; + btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2; + backupDv(); + do { + scale *= beta; + if (scale < 1e-8) { + return; + } + updateEnergy(scale); + f1 = m_objective->totalEnergy(solverdt)+kineticEnergy(); + f2 = f0 - alpha * scale * inner_product; + } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues + revertDv(); + updateDv(scale); + } + else + { + computeStep(m_ddv, m_residual); + updateDv(); + } for (int j = 0; j < m_numNodes; ++j) { m_ddv[j].setZero(); @@ -79,31 +104,107 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) } } +btScalar btDeformableBodySolver::kineticEnergy() +{ + btScalar ke = 0; + for (int i = 0; i < m_softBodies.size();++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size();++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + ke += m_dv[node.index].length2() * 0.5 / node.m_im; + } + } + } + return ke; +} + +void btDeformableBodySolver::backupDv() +{ + m_backup_dv.resize(m_dv.size()); + for (int i = 0; icomputeNorm(residual); + btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); + if (inner_product < -tol) + { + if (verbose) + { + std::cout << "Looking backwards!" << std::endl; + } + for (int i = 0; i < m_ddv.size();++i) + { + m_ddv[i] = -m_ddv[i]; + } + inner_product = -inner_product; + } + else if (std::abs(inner_product) < tol) + { + if (verbose) + { + std::cout << "Gradient Descent!" << std::endl; + } + btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; + for (int i = 0; i < m_ddv.size();++i) + { + m_ddv[i] = scale * residual[i]; + } + inner_product = scale * res_norm * res_norm; + } + return inner_product; +} + void btDeformableBodySolver::updateState() { updateVelocity(); updateTempPosition(); - } -void btDeformableBodySolver::updateDv() +void btDeformableBodySolver::updateDv(btScalar scale) { for (int i = 0; i < m_numNodes; ++i) { - m_dv[i] += m_ddv[i]; + m_dv[i] += scale * m_ddv[i]; } } void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - //btScalar tolerance = std::numeric_limits::epsilon() * m_objective->computeNorm(residual); - btScalar tolerance = std::numeric_limits::epsilon(); - m_cg.solve(*m_objective, ddv, residual, tolerance); + m_cg.solve(*m_objective, ddv, residual, false); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) { - m_softBodySet.copyFromArray(softBodies); + m_softBodies.copyFromArray(softBodies); bool nodeUpdated = updateNodes(); if (nodeUpdated) @@ -134,10 +235,8 @@ void btDeformableBodySolver::setConstraints() btScalar btDeformableBodySolver::solveContactConstraints() { - BT_PROFILE("setConstraint"); - btScalar maxSquaredResidual = m_objective->projection.update(); - m_objective->enforceConstraint(m_dv); - m_objective->updateVelocity(m_dv); + BT_PROFILE("solveContactConstraints"); + btScalar maxSquaredResidual = m_objective->m_projection.update(); return maxSquaredResidual; } @@ -145,9 +244,9 @@ btScalar btDeformableBodySolver::solveContactConstraints() void btDeformableBodySolver::updateVelocity() { int counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodies.size(); ++i) { - btSoftBody* psb = m_softBodySet[i]; + btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { // set NaN to zero; @@ -164,9 +263,9 @@ void btDeformableBodySolver::updateVelocity() void btDeformableBodySolver::updateTempPosition() { int counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodies.size(); ++i) { - btSoftBody* psb = m_softBodySet[i]; + btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v; @@ -179,9 +278,9 @@ void btDeformableBodySolver::updateTempPosition() void btDeformableBodySolver::backupVelocity() { int counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodies.size(); ++i) { - btSoftBody* psb = m_softBodySet[i]; + btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { m_backupVelocity[counter++] = psb->m_nodes[j].m_v; @@ -189,25 +288,20 @@ void btDeformableBodySolver::backupVelocity() } } -void btDeformableBodySolver::backupVn() +void btDeformableBodySolver::setupDeformableSolve(bool implicit) { int counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodies.size(); ++i) { - btSoftBody* psb = m_softBodySet[i]; + btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - // Here: - // dv = 0 for nodes not in constraints - // dv = v_{n+1} - v_{n+1}^* for nodes in constraints - if (m_objective->projection.m_constraints.find(psb->m_nodes[j].index)!=NULL) + if (implicit) { - m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn; + m_backupVelocity[counter] = psb->m_nodes[j].m_vn; } - // Now: - // dv = 0 for nodes not in constraints - // dv = v_{n+1} - v_n for nodes in constraints - m_backupVelocity[counter++] = psb->m_nodes[j].m_vn; + m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; + ++counter; } } } @@ -215,9 +309,9 @@ void btDeformableBodySolver::backupVn() void btDeformableBodySolver::revertVelocity() { int counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodies.size(); ++i) { - btSoftBody* psb = m_softBodySet[i]; + btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { psb->m_nodes[j].m_v = m_backupVelocity[counter++]; @@ -228,8 +322,8 @@ void btDeformableBodySolver::revertVelocity() bool btDeformableBodySolver::updateNodes() { int numNodes = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) - numNodes += m_softBodySet[i]->m_nodes.size(); + for (int i = 0; i < m_softBodies.size(); ++i) + numNodes += m_softBodies[i]->m_nodes.size(); if (numNodes != m_numNodes) { m_numNodes = numNodes; @@ -241,9 +335,9 @@ bool btDeformableBodySolver::updateNodes() void btDeformableBodySolver::predictMotion(btScalar solverdt) { - for (int i = 0; i < m_softBodySet.size(); ++i) + for (int i = 0; i < m_softBodies.size(); ++i) { - btSoftBody *psb = m_softBodySet[i]; + btSoftBody *psb = m_softBodies[i]; if (psb->isActive()) { @@ -259,6 +353,18 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d { int i, ni; + /* Update */ + if (psb->m_bUpdateRtCst) + { + psb->m_bUpdateRtCst = false; + psb->updateConstants(); + psb->m_fdbvt.clear(); + if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD) + { + psb->initializeFaceTree(); + } + } + /* Prepare */ psb->m_sst.sdt = dt * psb->m_cfg.timescale; psb->m_sst.isdt = 1 / psb->m_sst.sdt; @@ -286,21 +392,41 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d psb->m_sst.updmrg); } + if (!psb->m_fdbvt.empty()) + { + for (int i = 0; i < psb->m_faces.size(); ++i) + { + btSoftBody::Face& f = psb->m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, psb->m_sst.radmrg); + psb->m_fdbvt.update(f.m_leaf, + vol, + v * psb->m_sst.velmrg, + psb->m_sst.updmrg); + } + } /* Clear contacts */ - psb->m_rcontacts.resize(0); - psb->m_scontacts.resize(0); + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); + psb->m_faceNodeContacts.resize(0); /* Optimize dbvt's */ psb->m_ndbvt.optimizeIncremental(1); + psb->m_fdbvt.optimizeIncremental(1); } + void btDeformableBodySolver::updateSoftBodies() { - for (int i = 0; i < m_softBodySet.size(); i++) + BT_PROFILE("updateSoftBodies"); + for (int i = 0; i < m_softBodies.size(); i++) { - btSoftBody *psb = (btSoftBody *)m_softBodySet[i]; + btSoftBody *psb = (btSoftBody *)m_softBodies[i]; if (psb->isActive()) { - psb->updateNormals(); // normal is updated here + psb->updateNormals(); } } } @@ -310,3 +436,8 @@ void btDeformableBodySolver::setImplicit(bool implicit) m_implicit = implicit; m_objective->setImplicit(implicit); } + +void btDeformableBodySolver::setLineSearch(bool lineSearch) +{ + m_lineSearch = lineSearch; +} diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index fed15b2b2..bc56b3736 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -29,24 +29,24 @@ class btDeformableMultiBodyDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { -// using TVStack = btAlignedObjectArray; typedef btAlignedObjectArray TVStack; protected: - int m_numNodes; - TVStack m_dv; - TVStack m_ddv; - TVStack m_residual; - btAlignedObjectArray m_softBodySet; - - btAlignedObjectArray m_backupVelocity; - btScalar m_dt; - btScalar m_contact_iterations; - btConjugateGradient m_cg; - bool m_implicit; - int m_maxNewtonIterations; - btScalar m_newtonTolerance; + int m_numNodes; // total number of deformable body nodes + TVStack m_dv; // v_{n+1} - v_n + TVStack m_backup_dv; // backed up dv + TVStack m_ddv; // incremental dv + TVStack m_residual; // rhs of the linear solve + btAlignedObjectArray m_softBodies; // all deformable bodies + TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit + btScalar m_dt; // dt + btConjugateGradient m_cg; // CG solver + bool m_implicit; // use implicit scheme if true, explicit scheme if false + int m_maxNewtonIterations; // max number of newton iterations + btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance + bool m_lineSearch; // If true, use newton's method with line search under implicit scheme public: + // handles data related to objective function btDeformableBackwardEulerObjective* m_objective; btDeformableBodySolver(); @@ -58,54 +58,101 @@ public: return DEFORMABLE_SOLVER; } + // update soft body normals virtual void updateSoftBodies(); - - virtual void copyBackToSoftBodies(bool bMove = true) {} + // solve the momentum equation virtual void solveDeformableConstraints(btScalar solverdt); + // solve the contact between deformable and rigid as well as among deformables btScalar solveContactConstraints(); - - virtual void solveConstraints(btScalar dt){} - + + // resize/clear data structures void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); + // set up contact constraints void setConstraints(); + // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion + virtual void predictMotion(btScalar solverdt); + + // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* + // x_{n+1}^* is stored in m_q void predictDeformableMotion(btSoftBody* psb, btScalar dt); + // save the current velocity to m_backupVelocity void backupVelocity(); - void backupVn(); + + // set m_dv and m_backupVelocity to desired value to prepare for momentum solve + void setupDeformableSolve(bool implicit); + + // set the current velocity to that backed up in m_backupVelocity void revertVelocity(); + + // set velocity to m_dv + m_backupVelocity void updateVelocity(); + // update the node count bool updateNodes(); - void computeStep(TVStack& dv, const TVStack& residual); - - virtual void predictMotion(btScalar solverdt); + // calculate the change in dv resulting from the momentum solve + void computeStep(TVStack& ddv, const TVStack& residual); + + // calculate the change in dv resulting from the momentum solve when line search is turned on + btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose=false); virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} + // process collision between deformable and rigid virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap) { softBody->defaultCollisionHandler(collisionObjectWrap); } - + + // process collision between deformable and deformable virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { softBody->defaultCollisionHandler(otherSoftBody); } - virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false){} - - virtual bool checkInitialized(){return true;} - + + // If true, implicit time stepping scheme is used. + // Otherwise, explicit time stepping scheme is used void setImplicit(bool implicit); + // If true, newton's method with line search is used when implicit time stepping scheme is turned on + void setLineSearch(bool lineSearch); + + // set temporary position x^* = x_n + dt * v + // update the deformation gradient at position x^* void updateState(); - void updateDv(); + // set dv = dv + scale * ddv + void updateDv(btScalar scale = 1); + // set temporary position x^* = x_n + dt * v^* void updateTempPosition(); + + // save the current dv to m_backup_dv; + void backupDv(); + + // set dv to the backed-up value + void revertDv(); + + // set dv = dv + scale * ddv + // set v^* = v_n + dv + // set temporary position x^* = x_n + dt * v^* + // update the deformation gradient at position x^* + void updateEnergy(btScalar scale); + + // calculates the appropriately scaled kinetic energy in the system, which is + // 1/2 * dv^T * M * dv + // used in line search + btScalar kineticEnergy(); + + // unused functions + virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false){} + virtual void solveConstraints(btScalar dt){} + virtual bool checkInitialized(){return true;} + virtual void copyBackToSoftBodies(bool bMove = true) {} }; #endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp new file mode 100644 index 000000000..ce5018f2f --- /dev/null +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -0,0 +1,378 @@ +/* + Written by Xuchen Han + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "btDeformableContactConstraint.h" + +/* ================ Deformable vs. Rigid =================== */ +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c) +: m_contact(&c) +, btDeformableContactConstraint(c.m_cti.m_normal) +{ + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); +} + +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) +: m_contact(other.m_contact) +, btDeformableContactConstraint(other) +{ + m_total_normal_dv = other.m_total_normal_dv; + m_total_tangent_dv = other.m_total_tangent_dv; +} + + +btVector3 btDeformableRigidContactConstraint::getVa() const +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; +} + +btScalar btDeformableRigidContactConstraint::solveConstraint() +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn*dn; + btVector3 impulse = m_contact->m_c0 * vr; + const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c2 is the inverse mass of the deformable node/face + m_total_normal_dv -= impulse_normal * m_contact->m_c2; + m_total_tangent_dv -= impulse_tangent * m_contact->m_c2; + + if (m_total_normal_dv.dot(cti.m_normal) < 0) + { + // separating in the normal direction + m_static = false; + m_total_tangent_dv = btVector3(0,0,0); + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.norm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0,0,0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_c3; + } + impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + if (impulse_tangent.norm() > SIMD_EPSILON) + { + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); + const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); + } + } + } + return residualSquare; +} + +/* ================ Node vs. Rigid =================== */ +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact) + : m_node(contact.m_node) + , btDeformableRigidContactConstraint(contact) + { + } + +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other) +: m_node(other.m_node) +, btDeformableRigidContactConstraint(other) +{ +} + +btVector3 btDeformableNodeRigidContactConstraint::getVb() const +{ + return m_node->m_v; +} + + +btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const +{ + return m_total_normal_dv + m_total_tangent_dv; +} + +void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + contact->m_node->m_v -= dv; +} + +/* ================ Face vs. Rigid =================== */ +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact) +: m_face(contact.m_face) +, btDeformableRigidContactConstraint(contact) +{ +} + +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other) +: m_face(other.m_face) +, btDeformableRigidContactConstraint(other) +{ +} + +btVector3 btDeformableFaceRigidContactConstraint::getVb() const +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; +} + + +btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const +{ + btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + if (m_face->m_n[0] == node) + { + return face_dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == node) + { + return face_dv * contact->m_weights[1]; + } + btAssert(node == m_face->m_n[2]); + return face_dv * contact->m_weights[2]; +} + +void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + if (face->m_n[0]->m_im > 0) + face->m_n[0]->m_v -= dv * contact->m_weights[0]; + if (face->m_n[1]->m_im > 0) + face->m_n[1]->m_v -= dv * contact->m_weights[1]; + if (face->m_n[2]->m_im > 0) + face->m_n[2]->m_v -= dv * contact->m_weights[2]; +} + +/* ================ Face vs. Node =================== */ +btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact) +: m_node(contact.m_node) +, m_face(contact.m_face) +, m_contact(&contact) +, btDeformableContactConstraint(contact.m_normal) +{ + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); +} + +btVector3 btDeformableFaceNodeContactConstraint::getVa() const +{ + return m_node->m_v; +} + +btVector3 btDeformableFaceNodeContactConstraint::getVb() const +{ + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; +} + +btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const +{ + btVector3 dv = m_total_normal_dv + m_total_tangent_dv; + if (n == m_node) + return dv; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + if (m_face->m_n[0] == n) + { + return dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == n) + { + return dv * contact->m_weights[1]; + } + btAssert(n == m_face->m_n[2]); + return dv * contact->m_weights[2]; +} + +btScalar btDeformableFaceNodeContactConstraint::solveConstraint() +{ + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + const btScalar dn = btDot(vr, m_contact->m_normal); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn*dn; + btVector3 impulse = m_contact->m_c0 * vr; + const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c2 is the inverse mass of the deformable node/face + if (m_node->m_im > 0) + { + m_total_normal_dv -= impulse_normal * m_node->m_im; + m_total_tangent_dv -= impulse_tangent * m_node->m_im; + } + else + { + m_total_normal_dv -= impulse_normal * m_contact->m_imf; + m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; + } + + if (m_total_normal_dv.dot(m_contact->m_normal) < 0) + { + // separating in the normal direction + m_static = false; + m_total_tangent_dv = btVector3(0,0,0); + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.norm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0,0,0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_friction; + } + impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + return residualSquare; +} + +void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 dva = impulse * contact->m_node->m_im; + btVector3 dvb = impulse * contact->m_imf; + if (contact->m_node->m_im > 0) + { + contact->m_node->m_v += dva; + } + + btSoftBody::Face* face = contact->m_face; + if (face->m_n[0]->m_im > 0) + { + face->m_n[0]->m_v -= dvb * contact->m_weights[0]; + } + if (face->m_n[1]->m_im > 0) + { + face->m_n[1]->m_v -= dvb * contact->m_weights[1]; + } + if (face->m_n[2]->m_im > 0) + { + face->m_n[2]->m_v -= dvb * contact->m_weights[2]; + } +} diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h new file mode 100644 index 000000000..f79bc6065 --- /dev/null +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -0,0 +1,229 @@ +/* + Written by Xuchen Han + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_DEFORMABLE_CONTACT_CONSTRAINT_H +#define BT_DEFORMABLE_CONTACT_CONSTRAINT_H +#include "btSoftBody.h" + +// btDeformableContactConstraint is an abstract class specifying the method that each type of contact constraint needs to implement +class btDeformableContactConstraint +{ +public: + // True if the friction is static + // False if the friction is dynamic + bool m_static; + + // normal of the contact + btVector3 m_normal; + + btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal) + { + } + + btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal) + { + } + + btDeformableContactConstraint(const btDeformableContactConstraint& other) + : m_static(other.m_static) + , m_normal(other.m_normal) + { + + } + btDeformableContactConstraint(){} + + virtual ~btDeformableContactConstraint(){} + + // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence + // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact + virtual btScalar solveConstraint() = 0; + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const = 0; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const = 0; + + // get the velocity change of the soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const = 0; + + // apply impulse to the soft body node and/or face involved + virtual void applyImpulse(const btVector3& impulse) = 0; +}; + +// +// Constraint that a certain node in the deformable objects cannot move +class btDeformableStaticConstraint : public btDeformableContactConstraint +{ +public: + const btSoftBody::Node* m_node; + + btDeformableStaticConstraint(){} + + btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0)) + { + } + + btDeformableStaticConstraint(const btDeformableStaticConstraint& other) + : m_node(other.m_node) + , btDeformableContactConstraint(other) + { + + } + + virtual ~btDeformableStaticConstraint(){} + + virtual btScalar solveConstraint() + { + return 0; + } + + virtual btVector3 getVa() const + { + return btVector3(0,0,0); + } + + virtual btVector3 getVb() const + { + return btVector3(0,0,0); + } + + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0,0,0); + } + + virtual void applyImpulse(const btVector3& impulse){} +}; + +// +// Constraint between rigid/multi body and deformable objects +class btDeformableRigidContactConstraint : public btDeformableContactConstraint +{ +public: + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + const btSoftBody::DeformableRigidContact* m_contact; + + btDeformableRigidContactConstraint(){} + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c); + btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + virtual ~btDeformableRigidContactConstraint() + { + } + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + + virtual btScalar solveConstraint(); +}; + +// +// Constraint between rigid/multi body and deformable objects nodes +class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint +{ +public: + // the deformable node in contact + const btSoftBody::Node* m_node; + + btDeformableNodeRigidContactConstraint(){} + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact); + btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); + + virtual ~btDeformableNodeRigidContactConstraint() + { + } + + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableNodeRigidContact* getContact() const + { + return static_cast(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); +}; + +// +// Constraint between rigid/multi body and deformable objects faces +class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint +{ +public: + const btSoftBody::Face* m_face; + btDeformableFaceRigidContactConstraint(){} + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact); + btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); + + virtual ~btDeformableFaceRigidContactConstraint() + { + } + + // get the velocity of the deformable face at the contact point + virtual btVector3 getVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceRigidContact* getContact() const + { + return static_cast(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); +}; + +// +// Constraint between deformable objects faces and deformable objects nodes +class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint +{ +public: + btSoftBody::Node* m_node; + btSoftBody::Face* m_face; + const btSoftBody::DeformableFaceNodeContact* m_contact; + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + + btDeformableFaceNodeContactConstraint(){} + + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact); + + virtual ~btDeformableFaceNodeContactConstraint(){} + + virtual btScalar solveConstraint(); + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceNodeContact* getContact() const + { + return static_cast(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); +}; +#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 404eda494..e340a3fd5 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -20,148 +20,30 @@ btScalar btDeformableContactProjection::update() { btScalar residualSquare = 0; - btScalar max_impulse = 0; - // loop through constraints to set constrained values - for (int index = 0; index < m_constraints.size(); ++index) + + // node constraints + for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) { - DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index); - const btSoftBody::Node* node = constraint.m_node; - for (int j = 0; j < constraint.m_contact.size(); ++j) + btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); + for (int i = 0; i < constraints.size(); ++i) { - if (constraint.m_contact[j] == NULL) - { - // nothing needs to be done for dirichelet constraints - continue; - } - const btSoftBody::RContact* c = constraint.m_contact[j]; - const btSoftBody::sCti& cti = c->m_cti; - - if (cti.m_colObj->hasContactResponse()) - { - btVector3 va(0, 0, 0); - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - const btScalar* deltaV_normal; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; - const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_n[k]; - } - va = cti.m_normal * vel * m_dt; - // add in the tangential components of the va - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t1[k]; - } - va += c->t1 * vel * m_dt; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t2[k]; - } - va += c->t2 * vel * m_dt; - } - } - - const btVector3 vb = c->m_node->m_v * m_dt; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); - btVector3 impulse = c->m_c0 * vr; - const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - - btVector3 old_total_tangent_dv = constraint.m_total_tangent_dv[j]; - constraint.m_total_normal_dv[j] -= impulse_normal * node->m_im; - constraint.m_total_tangent_dv[j] -= impulse_tangent * node->m_im; - - if (constraint.m_total_normal_dv[j].dot(cti.m_normal) < 0) - { - // separating in the normal direction - constraint.m_static[j] = false; - constraint.m_can_be_dynamic[j] = false; - constraint.m_total_tangent_dv[j] = btVector3(0,0,0); - impulse_tangent.setZero(); - } - else - { - if (constraint.m_can_be_dynamic[j] && constraint.m_total_normal_dv[j].norm() * c->m_c3 < constraint.m_total_tangent_dv[j].norm()) - { - // dynamic friction - // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. - constraint.m_static[j] = false; - constraint.m_can_be_dynamic[j] = true; - if (constraint.m_total_tangent_dv[j].norm() < SIMD_EPSILON) - { - constraint.m_total_tangent_dv[j] = btVector3(0,0,0); - } - else - { - constraint.m_total_tangent_dv[j] = constraint.m_total_tangent_dv[j].normalized() * constraint.m_total_normal_dv[j].norm() * c->m_c3; - } - impulse_tangent = -btScalar(1)/node->m_im * (constraint.m_total_tangent_dv[j] - old_total_tangent_dv); - } - else - { - // static friction - constraint.m_static[j] = true; - constraint.m_can_be_dynamic[j] = false; - } - } - impulse = impulse_normal + impulse_tangent; - max_impulse = btMax(impulse.length2(), max_impulse); - - // dn is the normal component of velocity diffrerence. Approximates the residual. - residualSquare = btMax(residualSquare, dn*dn); - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - if (rigidCol) - { - rigidCol->applyImpulse(impulse, c->m_c1); - } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - if (multibodyLinkCol) - { - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - if (impulse_tangent.norm() > SIMD_EPSILON) - { - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(c->t1)); - const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(c->t2)); - } - } - } - } + btScalar localResidualSquare = constraints[i].solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); } } + + // face constraints + for (int index = 0; index < m_allFaceConstraints.size(); ++index) + { + btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; + btScalar localResidualSquare = constraint->solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + return residualSquare; } + void btDeformableContactProjection::setConstraints() { BT_PROFILE("setConstraints"); @@ -173,96 +55,146 @@ void btDeformableContactProjection::setConstraints() { if (psb->m_nodes[j].m_im == 0) { - m_constraints.insert(psb->m_nodes[j].index, DeformableContactConstraint()); + btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); + m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint); } } } + + // set Deformable Node vs. Rigid constraint for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; - btMultiBodyJacobianData jacobianData_normal; - btMultiBodyJacobianData jacobianData_complementary; - for (int j = 0; j < psb->m_rcontacts.size(); ++j) + for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { - const btSoftBody::RContact& c = psb->m_rcontacts[j]; - // skip anchor points - if (c.m_node->m_im == 0) + const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; + // skip fixed points + if (contact.m_node->m_im == 0) { continue; } - - const btSoftBody::sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) + btDeformableNodeRigidContactConstraint constraint(contact); + btVector3 va = constraint.getVa(); + btVector3 vb = constraint.getVb(); + const btVector3 vr = vb - va; + const btSoftBody::sCti& cti = contact.m_cti; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn < SIMD_EPSILON) { - btVector3 va(0, 0, 0); - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL) { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0); + btAlignedObjectArray constraintsList; + constraintsList.push_back(constraint); + m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList); } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + else { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - btScalar vel = 0.0; - const btScalar* jac = &c.jacobianData_normal.m_jacobians[0]; - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - for (int j = 0; j < ndof; ++j) - { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; - } - va = cti.m_normal * vel * m_dt; - } + btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[contact.m_node->index]; + constraintsList.push_back(constraint); } - - const btVector3 vb = c.m_node->m_v * m_dt; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) + } + } + + // set Deformable Face vs. Rigid constraint + for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) + { + const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j]; + // skip fixed faces + if (contact.m_c2 == 0) + { + continue; + } + btDeformableFaceRigidContactConstraint* constraint = new btDeformableFaceRigidContactConstraint(contact); + btVector3 va = constraint->getVa(); + btVector3 vb = constraint->getVb(); + const btVector3 vr = vb - va; + const btSoftBody::sCti& cti = contact.m_cti; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn < SIMD_EPSILON) + { + m_allFaceConstraints.push_back(constraint); + // add face constraints to each of the nodes + for (int k = 0; k < 3; ++k) { - - if (m_constraints.find(c.m_node->index) == NULL) + btSoftBody::Node* node = contact.m_face->m_n[k]; + // static node does not need to own face/rigid constraint + if (node->m_im != 0) { - m_constraints.insert(c.m_node->index, DeformableContactConstraint(c)); - } - else - { - DeformableContactConstraint& constraints = *m_constraints[c.m_node->index]; - bool single_contact = true; - if (single_contact) + if (m_faceRigidConstraints.find(node->index) == NULL) { - if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset) - { - constraints.replace(c); - } + btAlignedObjectArray constraintsList; + constraintsList.push_back(constraint); + m_faceRigidConstraints.insert(node->index, constraintsList); } else { - constraints.append(c); + btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[node->index]; + constraintsList.push_back(constraint); } } } } + else + { + delete constraint; + } } - } -} - -void btDeformableContactProjection::enforceConstraint(TVStack& x) -{ - for (int index = 0; index < m_constraints.size(); ++index) - { - const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index); - size_t i = m_constraints.getKeyAtIndex(index).getUid1(); - x[i].setZero(); - for (int j = 0; j < constraints.m_total_normal_dv.size(); ++j) + + // set Deformable Face vs. Deformable Node constraint + for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) { - x[i] += constraints.m_total_normal_dv[j]; - x[i] += constraints.m_total_tangent_dv[j]; + const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; + + btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact); + btVector3 va = constraint->getVa(); + btVector3 vb = constraint->getVb(); + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, contact.m_normal); + if (dn > -SIMD_EPSILON) + { + btSoftBody::Node* node = contact.m_node; + btSoftBody::Face* face = contact.m_face; + m_allFaceConstraints.push_back(constraint); + if (node->m_im != 0) + { + if (m_deformableConstraints.find(node->index) == NULL) + { + btAlignedObjectArray constraintsList; + constraintsList.push_back(constraint); + m_deformableConstraints.insert(node->index, constraintsList); + } + else + { + btAlignedObjectArray& constraintsList = *m_deformableConstraints[node->index]; + constraintsList.push_back(constraint); + } + } + + // add face constraints to each of the nodes + for (int k = 0; k < 3; ++k) + { + btSoftBody::Node* node = face->m_n[k]; + // static node does not need to own face/rigid constraint + if (node->m_im != 0) + { + if (m_deformableConstraints.find(node->index) == NULL) + { + btAlignedObjectArray constraintsList; + constraintsList.push_back(constraint); + m_deformableConstraints.insert(node->index, constraintsList); + } + else + { + btAlignedObjectArray& constraintsList = *m_deformableConstraints[node->index]; + constraintsList.push_back(constraint); + } + } + } + } + else + { + delete constraint; + } } } } @@ -270,35 +202,20 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) void btDeformableContactProjection::project(TVStack& x) { const int dim = 3; - for (int index = 0; index < m_constraints.size(); ++index) + for (int index = 0; index < m_projectionsDict.size(); ++index) { - const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index); - size_t i = m_constraints.getKeyAtIndex(index).getUid1(); - if (constraints.m_contact[0] == NULL) + btAlignedObjectArray& projectionDirs = *m_projectionsDict.getAtIndex(index); + size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); + if (projectionDirs.size() >= dim) { // static node x[i].setZero(); continue; } - bool has_static = false; - for (int j = 0; j < constraints.m_static.size(); ++j) + else if (projectionDirs.size() == 2) { - has_static = has_static || constraints.m_static[j]; - } - // static friction => fully constrained - if (has_static) - { - x[i].setZero(); - } - else if (constraints.m_total_normal_dv.size() >= dim) - { - x[i].setZero(); - } - else if (constraints.m_total_normal_dv.size() == 2) - { - - btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0); - btVector3 dir1 = (constraints.m_total_normal_dv[1].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[1].normalized() : btVector3(0,0,0); + btVector3 dir0 = projectionDirs[0]; + btVector3 dir1 = projectionDirs[1]; btVector3 free_dir = btCross(dir0, dir1); if (free_dir.norm() < SIMD_EPSILON) { @@ -313,39 +230,206 @@ void btDeformableContactProjection::project(TVStack& x) } else { - btAssert(constraints.m_total_normal_dv.size() == 1); - btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0); + btAssert(projectionDirs.size() == 1); + btVector3 dir0 = projectionDirs[0]; x[i] -= x[i].dot(dir0) * dir0; } } } +void btDeformableContactProjection::setProjection() +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + int index = psb->m_nodes[j].index; + bool hasConstraint = false; + bool existStaticConstraint = false; + btVector3 averagedNormal(0,0,0); + btAlignedObjectArray normals; + if (m_staticConstraints.find(index) != NULL) + { + existStaticConstraint = true; + hasConstraint = true; + } + + // accumulate normals from Deformable Node vs. Rigid constraints + if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL) + { + hasConstraint = true; + btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[index]; + for (int k = 0; k < constraintsList.size(); ++k) + { + if (constraintsList[k].m_static) + { + existStaticConstraint = true; + break; + } + const btVector3& local_normal = constraintsList[k].m_normal; + normals.push_back(local_normal); + averagedNormal += local_normal; + } + } + + // accumulate normals from Deformable Face vs. Rigid constraints + if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL) + { + hasConstraint = true; + btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[index]; + for (int k = 0; k < constraintsList.size(); ++k) + { + if (constraintsList[k]->m_static) + { + existStaticConstraint = true; + break; + } + const btVector3& local_normal = constraintsList[k]->m_normal; + normals.push_back(local_normal); + averagedNormal += local_normal; + } + } + + // accumulate normals from Deformable Node vs. Deformable Face constraints + if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL) + { + hasConstraint = true; + btAlignedObjectArray& constraintsList = *m_deformableConstraints[index]; + for (int k = 0; k < constraintsList.size(); ++k) + { + if (constraintsList[k]->m_static) + { + existStaticConstraint = true; + break; + } + const btVector3& local_normal = constraintsList[k]->m_normal; + normals.push_back(local_normal); + averagedNormal += local_normal; + } + } + + + // build projections + if (!hasConstraint) + { + continue; + } + btAlignedObjectArray projections; + if (existStaticConstraint) + { + projections.push_back(btVector3(1,0,0)); + projections.push_back(btVector3(0,1,0)); + projections.push_back(btVector3(0,0,1)); + } + else + { + bool averageExists = (averagedNormal.length2() > SIMD_EPSILON); + averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0); + if (averageExists) + { + projections.push_back(averagedNormal); + } + for (int k = 0; k < normals.size(); ++k) + { + const btVector3& local_normal = normals[k]; + // add another projection direction if it deviates from the average by more than about 15 degrees + if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25) + { + projections.push_back(local_normal); + } + } + } + m_projectionsDict.insert(index, projections); + } + } +} + + void btDeformableContactProjection::applyDynamicFriction(TVStack& f) { - for (int index = 0; index < m_constraints.size(); ++index) + // loop over constraints + for (int i = 0; i < f.size(); ++i) { - const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index); - const btSoftBody::Node* node = constraint.m_node; - if (node == NULL) - continue; - size_t i = m_constraints.getKeyAtIndex(index).getUid1(); - bool has_static_constraint = false; - - // apply dynamic friction force (scaled by dt) if the node does not have static friction constraint - for (int j = 0; j < constraint.m_static.size(); ++j) + if (m_projectionsDict.find(i) != NULL) { - if (constraint.m_static[j]) + // doesn't need to add friction force for fully constrained vertices + btAlignedObjectArray& projectionDirs = *m_projectionsDict[i]; + if (projectionDirs.size() >= 3) { - has_static_constraint = true; - break; + continue; } } - for (int j = 0; j < constraint.m_total_tangent_dv.size(); ++j) + + // add friction contribution from Face vs. Node + if (m_nodeRigidConstraints.find(i) != NULL) { - btVector3 friction_force = constraint.m_total_tangent_dv[j] * (1./node->m_im); - if (!has_static_constraint) + btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[i]; + for (int j = 0; j < constraintsList.size(); ++j) { - f[i] += friction_force; + const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j]; + btSoftBody::Node* node = constraint.getContact()->m_node; + + // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. + f[i] += constraint.getDv(node)* (1./node->m_im); + } + } + + // add friction contribution from Face vs. Rigid + if (m_faceRigidConstraints.find(i) != NULL) + { + btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[i]; + for (int j = 0; j < constraintsList.size(); ++j) + { + const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j]; + btSoftBody::Face* face = constraint->getContact()->m_face; + + // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. + for (int k = 0; k < 3; ++k) + { + if (face->m_n[k]->index == i) + { + if (face->m_n[k]->m_im != 0) + { + f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im); + } + break; + } + } + } + } + + if (m_deformableConstraints.find(i) != NULL) + { + btAlignedObjectArray& constraintsList = *m_deformableConstraints[i]; + for (int j = 0; j < constraintsList.size(); ++j) + { + const btDeformableFaceNodeContactConstraint* constraint = constraintsList[j]; + btSoftBody::Face* face = constraint->getContact()->m_face; + btSoftBody::Node* node = constraint->getContact()->m_node; + + // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. + if (node->index == i) + { + if (node->m_im != 0) + { + f[i] += constraint->getDv(node)*(1./node->m_im); + } + } + else + { + for (int k = 0; k < 3; ++k) + { + if (face->m_n[k]->index == i) + { + if (face->m_n[k]->m_im != 0) + { + f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im); + } + break; + } + } + } } } } @@ -353,8 +437,16 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) void btDeformableContactProjection::reinitialize(bool nodeUpdated) { - btCGProjection::reinitialize(nodeUpdated); - m_constraints.clear(); + m_staticConstraints.clear(); + m_nodeRigidConstraints.clear(); + m_faceRigidConstraints.clear(); + m_deformableConstraints.clear(); + m_projectionsDict.clear(); + for (int i = 0; i < m_allFaceConstraints.size(); ++i) + { + delete m_allFaceConstraints[i]; + } + m_allFaceConstraints.clear(); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 774da1ae3..fa5eb662b 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -19,15 +19,32 @@ #include "btSoftBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "btDeformableContactConstraint.h" #include "LinearMath/btHashMap.h" -class btDeformableContactProjection : public btCGProjection +#include +class btDeformableContactProjection { public: - // map from node index to constraints - btHashMap m_constraints; + typedef btAlignedObjectArray TVStack; + btAlignedObjectArray& m_softBodies; - btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) - : btCGProjection(softBodies, dt) + // map from node index to static constraint + btHashMap m_staticConstraints; + // map from node index to node rigid constraint + btHashMap > m_nodeRigidConstraints; + // map from node index to face rigid constraint + btHashMap > m_faceRigidConstraints; + // map from node index to deformable constraint + btHashMap > m_deformableConstraints; + + // all constraints involving face + btAlignedObjectArray m_allFaceConstraints; + + // map from node index to projection directions + btHashMap > m_projectionsDict; + + btDeformableContactProjection(btAlignedObjectArray& softBodies) + : m_softBodies(softBodies) { } @@ -35,19 +52,21 @@ public: { } - // apply the constraints to the rhs + // apply the constraints to the rhs of the linear solve virtual void project(TVStack& x); - // add to friction - virtual void applyDynamicFriction(TVStack& f); - // apply constraints to x in Ax=b - virtual void enforceConstraint(TVStack& x); + // add friction force to the rhs of the linear solve + virtual void applyDynamicFriction(TVStack& f); // update the constraints virtual btScalar update(); + // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. virtual void setConstraints(); + // Set up projections for each vertex by adding the projection direction to + virtual void setProjection(); + virtual void reinitialize(bool nodeUpdated); }; #endif /* btDeformableContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableCorotatedForce.h b/src/BulletSoftBody/btDeformableCorotatedForce.h index 13f64ccfa..c2a26338e 100644 --- a/src/BulletSoftBody/btDeformableCorotatedForce.h +++ b/src/BulletSoftBody/btDeformableCorotatedForce.h @@ -19,7 +19,7 @@ #include "btDeformableLagrangianForce.h" #include "LinearMath/btPolarDecomposition.h" -static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) +static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) { static const btPolarDecomposition polar; return polar.decompose(m, q, s); @@ -100,7 +100,7 @@ public: if (J < 1024 * SIMD_EPSILON) R.setIdentity(); else - PolarDecompose(F, R, S); // this QR is not robust, consider using implicit shift svd + PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ P += (F-R) * 2 * m_mu; } diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 6fd53144a..d9bef8b07 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -72,6 +72,25 @@ public: { return BT_GRAVITY_FORCE; } + + // the gravitational potential energy + virtual double totalEnergy(btScalar dt) + { + double e = 0; + for (int i = 0; im_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + e -= m_gravity.dot(node.m_q)/node.m_im; + } + } + } + return e; + } }; diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index 7ce72a7fd..cab48837d 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -28,6 +28,11 @@ enum btDeformableLagrangianForceType BT_NEOHOOKEAN_FORCE = 4 }; +static inline double randomDouble(double low, double high) +{ + return low + static_cast(rand()) / RAND_MAX * (high - low); +} + class btDeformableLagrangianForce { public: @@ -62,6 +67,7 @@ public: { } + // get number of nodes that have the force virtual int getNumNodes() { int numNodes = 0; @@ -72,6 +78,7 @@ public: return numNodes; } + // add a soft body to be affected by the particular lagrangian force virtual void addSoftBody(btSoftBody* psb) { m_softBodies.push_back(psb); @@ -82,28 +89,25 @@ public: m_nodes = nodes; } + // Calculate the incremental deformable generated from the input dx virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) { btVector3 c1 = dx[id1] - dx[id0]; btVector3 c2 = dx[id2] - dx[id0]; btVector3 c3 = dx[id3] - dx[id0]; - btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - return dF; + return btMatrix3x3(c1,c2,c3).transpose(); } + // Calculate the incremental deformable generated from the current velocity virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) { btVector3 c1 = n1->m_v - n0->m_v; btVector3 c2 = n2->m_v - n0->m_v; btVector3 c3 = n3->m_v - n0->m_v; - btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - return dF; + return btMatrix3x3(c1,c2,c3).transpose(); } + // test for addScaledElasticForce function virtual void testDerivative() { for (int i = 0; iupdateDeformation(); } counter = 0; - double f1 = totalElasticEnergy(); + double f1 = totalElasticEnergy(0); for (int i = 0; i(rand()) / RAND_MAX * (high - low); + return 0; + } + + // total Energy takes dt as input because certain energies depend on dt + virtual double totalEnergy(btScalar dt) + { + return totalElasticEnergy(dt) + totalDampingEnergy(dt); } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 73d7efb42..9e7b3594c 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -20,6 +20,8 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce { + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity bool m_momentum_conserving; btScalar m_elasticStiffness, m_dampingStiffness; public: @@ -62,9 +64,9 @@ public: btVector3 scaled_force = scale * m_dampingStiffness * v_diff; if (m_momentum_conserving) { - if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON) + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) { - btVector3 dir = (node2->m_q - node1->m_q).normalized(); + btVector3 dir = (node2->m_x - node1->m_x).normalized(); scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; } } @@ -118,9 +120,9 @@ public: btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); if (m_momentum_conserving) { - if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON) + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) { - btVector3 dir = (node2->m_q - node1->m_q).normalized(); + btVector3 dir = (node2->m_x - node1->m_x).normalized(); local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; } } @@ -129,7 +131,8 @@ public: } } } - virtual double totalElasticEnergy() + + virtual double totalElasticEnergy(btScalar dt) { double energy = 0; for (int i = 0; i < m_softBodies.size(); ++i) @@ -144,7 +147,36 @@ public: // elastic force btVector3 dir = (node2->m_q - node1->m_q); - energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r ); + energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r); + } + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz+1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; } } return energy; diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 65ba43e04..362451c0e 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -23,13 +23,21 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + m_maxOverrideNumSolverIterations = 50; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { - m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body + // solve rigid/rigid in solver body + m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + // solver body velocity -> rigid body velocity solverBodyWriteBack(infoGlobal); + + // update rigid body velocity in rigid/deformable contact m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints()); + + // solver body velocity <- rigid body velocity writeToSolverBody(bodies, numBodies, infoGlobal); if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) @@ -51,3 +59,50 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration } return 0.f; } + +void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +{ + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // overriden + solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlyFinish(bodies, numBodies, info); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; +} + +void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + for (int i = 0; i < numBodies; i++) + { + int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; + solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; + } + } +} + +void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) +{ + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) + { + btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; + if (body) + { + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity); + } + } +} diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 80aafe03f..cac82f98b 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -22,6 +22,13 @@ class btDeformableBodySolver; +// btDeformableMultiBodyConstraintSolver extendsn btMultiBodyConstraintSolver to solve for the contact among rigid/multibody and deformable bodies. Notice that the following constraints +// 1. rigid/multibody against rigid/multibody +// 2. rigid/multibody against deforamble +// 3. deformable against deformable +// 4. deformable self collision +// 5. joint constraints +// are all coupled in this solve. ATTRIBUTE_ALIGNED16(class) btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver { @@ -31,34 +38,11 @@ protected: // override the iterations method to include deformable/multibody contact virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - void solverBodyWriteBack(const btContactSolverInfo& infoGlobal) - { - for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) - { - btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; - if (body) - { - m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity); - } - } - } - - void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) - { - for (int i = 0; i < numBodies; i++) - { - int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); - - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body && body->getInvMass()) - { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; - solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; - } - } - } + // write the velocity of the the solver body to the underlying rigid body + void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); + + // write the velocity of the underlying rigid body to the the the solver body + void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -68,23 +52,7 @@ public: m_deformableSolver = deformableSolver; } - virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) - { - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // overriden - solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlyFinish(bodies, numBodies, info); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; - } + virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); }; #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 3fff883c9..595ed737c 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -37,6 +37,10 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { BT_PROFILE("internalSingleStepSimulation"); + if (0 != m_internalPreTickCallback) + { + (*m_internalPreTickCallback)(this, timeStep); + } reinitialize(timeStep); // add gravity to velocity of rigid and multi bodys applyRigidBodyGravity(timeStep); @@ -47,11 +51,16 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t ///perform collision detection btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + if (m_selfCollision) + { + softBodySelfCollision(); + } + btMultiBodyDynamicsWorld::calculateSimulationIslands(); beforeSolverCallbacks(timeStep); - ///solve deformable bodies constraints + ///solve contact constraints and then deformable bodies momemtum equation solveConstraints(timeStep); afterSolverCallbacks(timeStep); @@ -66,86 +75,21 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t // /////////////////////////////// } -void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep) +void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() { - // perform position correction for all constraints - BT_PROFILE("positionCorrection"); - for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) + m_deformableBodySolver->updateSoftBodies(); + for (int i = 0; i < m_softBodies.size(); i++) { - DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); - for (int j = 0; j < constraint.m_contact.size(); ++j) - { - const btSoftBody::RContact* c = constraint.m_contact[j]; - // skip anchor points - if (c == NULL || c->m_node->m_im == 0) - continue; - const btSoftBody::sCti& cti = c->m_cti; - btVector3 va(0, 0, 0); - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; - const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += local_v[k] * J_n[k]; - } - va = cti.m_normal * vel; - - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += local_v[k] * J_t1[k]; - } - va += c->t1 * vel; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += local_v[k] * J_t2[k]; - } - va += c->t2 * vel; - } - } - else - { - // The object interacting with deformable node is not supported for position correction - btAssert(false); - } - - if (cti.m_colObj->hasContactResponse()) - { - btScalar dp = cti.m_offset; - - // only perform position correction when penetrating - if (dp < 0) - { - c->m_node->m_v -= dp * cti.m_normal / timeStep; - } - } - } + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; + psb->defaultCollisionHandler(psb); } } - void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); - m_deformableBodySolver->backupVelocity(); - positionCorrection(timeStep); + //m_deformableBodySolver->backupVelocity(); + //positionCorrection(timeStep); // looks like position correction is no longer necessary btMultiBodyDynamicsWorld::integrateTransforms(timeStep); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -170,28 +114,35 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) node.m_q = node.m_x; node.m_vn = node.m_v; } + psb->interpolateRenderMesh(); } - m_deformableBodySolver->revertVelocity(); + //m_deformableBodySolver->revertVelocity(); } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); + if (!m_implicit) + { + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); + } // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); - solveMultiBodyRelatedConstraints(); - if (m_implicit) - { - // at this point dv = v_{n+1} - v_{n+1}^* - // modify dv such that dv = v_{n+1} - v_n - // modify m_backupVelocity so that it stores v_n instead of v_{n+1}^* as needed by Newton - m_deformableBodySolver->backupVn(); - } + // solve contact constraints + solveContactConstraints(); + + // set up the directions in which the velocity does not change in the momentum solve + m_deformableBodySolver->m_objective->m_projection.setProjection(); + + // for explicit scheme, m_backupVelocity = v_{n+1}^* + // for implicit scheme, m_backupVelocity = v_n + // Here, set dv = v_{n+1} - v_n for nodes in contact + m_deformableBodySolver->setupDeformableSolve(m_implicit); // At this point, dv should be golden for nodes in contact + // proceed to solve deformable momentum equation m_deformableBodySolver->solveDeformableConstraints(timeStep); } @@ -207,7 +158,6 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints() btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; m_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); // build islands m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); @@ -233,7 +183,7 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints() } -void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints() +void btDeformableMultiBodyDynamicsWorld::solveContactConstraints() { // process constraints on each island m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); @@ -315,12 +265,8 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) { m_internalTime += timeStep; m_deformableBodySolver->setImplicit(m_implicit); + m_deformableBodySolver->setLineSearch(m_lineSearch); m_deformableBodySolver->reinitialize(m_softBodies, timeStep); -// if (m_implicit) -// { -// // todo: backup v_n velocity somewhere else -// m_deformableBodySolver->backupVelocity(); -// } btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 972f05664..e66e060f2 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -13,8 +13,8 @@ 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H -#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H +#ifndef BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H +#define BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H #include "btSoftMultiBodyDynamicsWorld.h" #include "btDeformableLagrangianForce.h" @@ -36,7 +36,6 @@ typedef btAlignedObjectArray btSoftBodyArray; class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld { typedef btAlignedObjectArray TVStack; -// using TVStack = btAlignedObjectArray; ///Solver classes that encapsulate multiple deformable bodies for solving btDeformableBodySolver* m_deformableBodySolver; btSoftBodyArray m_softBodies; @@ -48,6 +47,8 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld btScalar m_internalTime; int m_contact_iterations; bool m_implicit; + bool m_lineSearch; + bool m_selfCollision; typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); btSolverCallback m_solverCallback; @@ -73,7 +74,7 @@ public: m_sbi.m_broadphase = pairCache; m_sbi.m_dispatcher = dispatcher; m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025); + m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005); m_sbi.m_sparsesdf.Reset(); m_sbi.air_density = (btScalar)1.2; @@ -83,6 +84,7 @@ public: m_sbi.m_gravity.setValue(0, -10, 0); m_internalTime = 0.0; m_implicit = true; + m_selfCollision = true; } void setSolverCallback(btSolverCallback cb) @@ -152,14 +154,21 @@ public: void solveMultiBodyConstraints(); - void solveMultiBodyRelatedConstraints(); + void solveContactConstraints(); void sortConstraints(); + void softBodySelfCollision(); + void setImplicit(bool implicit) { m_implicit = implicit; } + + void setLineSearch(bool lineSearch) + { + m_lineSearch = lineSearch; + } }; -#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H +#endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index fe80b372d..c40aaf4a7 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -17,7 +17,8 @@ subject to the following restrictions: #define BT_NEOHOOKEAN_H #include "btDeformableLagrangianForce.h" - +#include "LinearMath/btQuickprof.h" +// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { public: @@ -26,12 +27,12 @@ public: btScalar m_mu_damp, m_lambda_damp; btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) { - btScalar damping = 0.005; + btScalar damping = 0.05; m_mu_damp = damping * m_mu; m_lambda_damp = damping * m_lambda; } - btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.005): m_mu(mu), m_lambda(lambda) + btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0): m_mu(mu), m_lambda(lambda) { m_mu_damp = damping * m_mu; m_lambda_damp = damping * m_lambda; @@ -48,8 +49,11 @@ public: addScaledElasticForce(scale, force); } + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search virtual void addScaledDampingForce(btScalar scale, TVStack& force) { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; int numNodes = getNumNodes(); btAssert(numNodes <= force.size()); btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); @@ -69,10 +73,10 @@ public: size_t id3 = node3->index; btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; btMatrix3x3 dP; - firstPiolaDampingDifferential(tetra.m_F, dF, dP); + firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - + // damping force differential btScalar scale1 = scale * tetra.m_element_measure; force[id0] -= scale1 * df_on_node0; @@ -83,31 +87,58 @@ public: } } - virtual double totalElasticEnergy() + virtual double totalElasticEnergy(btScalar dt) { double energy = 0; for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_tetras.size(); ++j) + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; - energy += tetra.m_element_measure * elasticEnergyDensity(tetra); + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); } } return energy; } - double elasticEnergyDensity(const btSoftBody::Tetra& t) + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz+1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) { double density = 0; - btMatrix3x3 F = t.m_F; - btMatrix3x3 C = F.transpose()*F; - double J = F.determinant(); - double trace = C[0].getX() + C[1].getY() + C[2].getZ(); - density += m_mu * 0.5 * (trace - 3.); - density += m_lambda * 0.5 * (J - 1. - 0.75 * m_mu / m_lambda)* (J - 1. - 0.75 * m_mu / m_lambda); - density -= m_mu * 0.5 * log(trace+1); + density += m_mu * 0.5 * (s.m_trace - 3.); + density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda); + density -= m_mu * 0.5 * log(s.m_trace+1); return density; } @@ -123,9 +154,10 @@ public: { btSoftBody::Tetra& tetra = psb->m_tetras[j]; btMatrix3x3 P; - firstPiola(tetra.m_F,P); - btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + firstPiola(psb->m_tetraScratches[j],P); +// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node1 = tetra.m_n[1]; @@ -146,8 +178,11 @@ public: } } + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; int numNodes = getNumNodes(); btAssert(numNodes <= df.size()); btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); @@ -167,10 +202,11 @@ public: size_t id3 = node3->index; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; btMatrix3x3 dP; - firstPiolaDampingDifferential(tetra.m_F, dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); +// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + // damping force differential btScalar scale1 = scale * tetra.m_element_measure; df[id0] -= scale1 * df_on_node0; @@ -202,9 +238,10 @@ public: size_t id3 = node3->index; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; btMatrix3x3 dP; - firstPiolaDifferential(tetra.m_F, dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); +// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; // elastic force differential btScalar scale1 = scale * tetra.m_element_measure; @@ -216,34 +253,35 @@ public: } } - void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) { - btMatrix3x3 C = F.transpose()*F; - btScalar J = F.determinant(); - btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); - P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1.) - 0.75 * m_mu); + btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.))); + btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); + P = s.m_F * c1 + s.m_cofF * c2; } - void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP) + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { - btScalar J = F.determinant(); - btMatrix3x3 C = F.transpose()*F; - btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); - dP = dF * m_mu * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace))); - - addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1.) - 0.75*m_mu, dP); - dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF); + btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.)); + btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); + btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP); + dP += s.m_cofF * c3; } - void firstPiolaDampingDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP) + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { - btScalar J = F.determinant(); - btMatrix3x3 C = F.transpose()*F; - btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); - dP = dF * m_mu_damp * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu_damp) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace))); - - addScaledCofactorMatrixDifferential(F, dF, m_lambda_damp*(J-1.) - 0.75*m_mu_damp, dP); - dP += F.adjoint().transpose() * m_lambda_damp * DotProduct(F.adjoint().transpose(), dF); + btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.))); + btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace)))); + btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP); + dP += s.m_cofF * c3; } btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) @@ -256,6 +294,9 @@ public: return ans; } + // Let C(A) be the cofactor of the matrix A + // Let H = the derivative of C(A) with respect to A evaluated at F = A + // This function calculates H*dF void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) { M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index eb630cbfc..0da85ff43 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -18,8 +18,12 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBodyData.h" #include "LinearMath/btSerializer.h" +#include "LinearMath/btAlignedAllocator.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include // btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) : m_softBodySolver(0), m_worldInfo(worldInfo) @@ -86,6 +90,7 @@ void btSoftBody::initDefaults() m_cfg.diterations = 0; m_cfg.citerations = 4; m_cfg.collisions = fCollision::Default; + m_cfg.collisions |= fCollision::VF_DD; m_pose.m_bvolume = false; m_pose.m_bframe = false; m_pose.m_volume = 0; @@ -2289,6 +2294,7 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, } return (false); } + // bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, @@ -2303,6 +2309,7 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) : colObjWrap->getWorldTransform(); + //const btTransform& wtr = colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), @@ -2320,6 +2327,63 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr return (false); } +// +// Compute barycentric coordinates (u, v, w) for +// point p with respect to triangle (a, b, c) +static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary) +{ + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = v0.dot(v0); + btScalar d01 = v0.dot(v1); + btScalar d11 = v1.dot(v1); + btScalar d20 = v2.dot(v0); + btScalar d21 = v2.dot(v1); + btScalar denom = d00 * d11 - d01 * d01; + bary.setY((d11 * d20 - d01 * d21) / denom); + bary.setZ((d00 * d21 - d01 * d20) / denom); + bary.setX(btScalar(1) - bary.getY() - bary.getZ()); +} + +// +bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, + const Face& f, + btVector3& contact_point, + btVector3& bary, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? + (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + + btGjkEpaSolver2::sResults results; + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); + btVector3 guess(0,0,0); + const btConvexShape* csh = static_cast(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + btScalar dst = results.distance - margin; + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); +// cti.m_normal = wtr.getBasis() * results.normal; + cti.m_normal = results.normal; + cti.m_offset = dst; + } + if (dst < 0) + return true; + return (false); +} + // void btSoftBody::updateNormals() { @@ -2842,6 +2906,22 @@ void btSoftBody::updateDeformation() c1.getY(), c2.getY(), c3.getY(), c1.getZ(), c2.getZ(), c3.getZ()); t.m_F = Ds * t.m_Dm_inverse; + + btSoftBody::TetraScratch& s = m_tetraScratches[i]; + s.m_F = t.m_F; + s.m_J = t.m_F.determinant(); + btMatrix3x3 C = t.m_F.transpose()*t.m_F; + s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); + s.m_cofF = t.m_F.adjoint().transpose(); + } +} + +void btSoftBody::advanceDeformation() +{ + updateDeformation(); + for (int i = 0; i < m_tetras.size(); ++i) + { + m_tetraScratchesTn[i] = m_tetraScratches[i]; } } // @@ -3082,6 +3162,20 @@ void btSoftBody::applyForces() } } +// +void btSoftBody::interpolateRenderMesh() +{ + for (int i = 0; i < m_renderNodes.size(); ++i) + { + Node& n = m_renderNodes[i]; + n.m_x.setZero(); + for (int j = 0; j < 4; ++j) + { + n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; + } + } +} + // void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti) { @@ -3324,7 +3418,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap break; case fCollision::SDF_RD: { - btSoftColliders::CollideSDF_RD docollide; + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); btTransform wtr = pcoWrap->getWorldTransform(); @@ -3340,21 +3434,75 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap maxs); volume = btDbvtVolume::FromMM(mins, maxs); volume.Expand(btVector3(basemargin, basemargin, basemargin)); - docollide.psb = this; - docollide.m_colObj1Wrap = pcoWrap; - docollide.m_rigidBody = prb1; + btSoftColliders::CollideSDF_RD docollideNode; + docollideNode.psb = this; + docollideNode.m_colObj1Wrap = pcoWrap; + docollideNode.m_rigidBody = prb1; + docollideNode.dynmargin = basemargin + timemargin; + docollideNode.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); - docollide.dynmargin = basemargin + timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide); + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; + docollideFace.dynmargin = basemargin + timemargin; + docollideFace.stamargin = basemargin; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); } break; } } +static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) +{ + if (n == 0) + return 0; + btDbvntNode* root = new btDbvntNode(n); + if (n->isinternal()) + { + btDbvntNode* c0 = copyToDbvnt(n->childs[0]); + root->childs[0] = c0; + btDbvntNode* c1 = copyToDbvnt(n->childs[1]); + root->childs[1] = c1; + } + return root; +} + +static inline void calculateNormalCone(btDbvntNode* root) +{ + if (!root) + return; + if (root->isleaf()) + { + const btSoftBody::Face* face = (btSoftBody::Face*)root->data; + root->normal = face->m_normal; + root->angle = 0; + } + else + { + btVector3 n0(0,0,0), n1(0,0,0); + btScalar a0 = 0, a1 = 0; + if (root->childs[0]) + { + calculateNormalCone(root->childs[0]); + n0 = root->childs[0]->normal; + a0 = root->childs[0]->angle; + } + if (root->childs[1]) + { + calculateNormalCone(root->childs[1]); + n1 = root->childs[1]->normal; + a1 = root->childs[1]->angle; + } + root->normal = (n0+n1).safeNormalize(); + root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5; + } +} // void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { + BT_PROFILE("Deformable Collision"); const int cf = m_cfg.collisions & psb->m_cfg.collisions; switch (cf & fCollision::SVSmask) { @@ -3392,6 +3540,53 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } } break; + case fCollision::VF_DD: + { + if (this != psb) + { + btSoftColliders::CollideVF_DD docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + /* psb0 faces vs psb0 faces */ + btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); + calculateNormalCone(root); + this->m_fdbvt.selfCollideT(root,docollide); + delete root; + +// btSoftColliders::CollideFF_DD docollide; +// /* common */ +// docollide.mrg = getCollisionShape()->getMargin() + +// psb->getCollisionShape()->getMargin(); +// /* psb0 nodes vs psb1 faces */ +// docollide.psb[0] = this; +// docollide.psb[1] = psb; +// docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_fdbvt.m_root, +// docollide.psb[1]->m_fdbvt.m_root, +// docollide); + } + } + break; default: { } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 5371e172d..1016fe9f6 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -161,11 +161,13 @@ public: SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft SDF_RD = 0x0003, ///DF based rigid vs deformable + SDF_RDF = 0x0004, ///DF based rigid vs deformable faces - SVSmask = 0x0030, ///Rigid versus soft mask + SVSmask = 0x00F0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling CL_SELF = 0x0040, ///Cluster soft body self collision + VF_DD = 0x0050, ///Vertex vs face soft vs soft handling /* presets */ Default = SDF_RS, END @@ -217,6 +219,7 @@ public: const btCollisionObject* m_colObj; /* Rigid body */ btVector3 m_normal; /* Outward normal */ btScalar m_offset; /* Offset from origin */ + btVector3 m_bary; /* Barycentric weights for faces */ }; /* sMedium */ @@ -283,6 +286,7 @@ public: btVector3 m_normal; // Normal btScalar m_ra; // Rest area btDbvtNode* m_leaf; // Leaf data + int m_index; }; /* Tetra */ struct Tetra : Feature @@ -297,6 +301,16 @@ public: btMatrix3x3 m_F; btScalar m_element_measure; }; + + /* TetraScratch */ + struct TetraScratch + { + btMatrix3x3 m_F; // deformation gradient F + btScalar m_trace; // trace of F^T * F + btScalar m_J; // det(F) + btMatrix3x3 m_cofF; // cofactor of F + }; + /* RContact */ struct RContact { @@ -315,6 +329,53 @@ public: btVector3 t1; btVector3 t2; }; + + class DeformableRigidContact + { + public: + sCti m_cti; // Contact infos + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // inverse mass of node/face + btScalar m_c3; // Friction + btScalar m_c4; // Hardness + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; + }; + + class DeformableNodeRigidContact : public DeformableRigidContact + { + public: + Node* m_node; // Owner node + }; + + class DeformableFaceRigidContact : public DeformableRigidContact + { + public: + Face* m_face; // Owner face + btVector3 m_contactPoint; // Contact point + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + }; + + struct DeformableFaceNodeContact + { + Node* m_node; // Node + Face* m_face; // Face + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + btVector3 m_normal; // Normal + btScalar m_margin; // Margin + btScalar m_friction; // Friction + btScalar m_imf; // inverse mass of the face at contact point + btScalar m_c0; // scale of the impulse matrix; + }; + /* SContact */ struct SContact { @@ -703,11 +764,18 @@ public: btSoftBodyWorldInfo* m_worldInfo; // World info tNoteArray m_notes; // Notes tNodeArray m_nodes; // Nodes + tNodeArray m_renderNodes; // Nodes tLinkArray m_links; // Links tFaceArray m_faces; // Faces + tFaceArray m_renderFaces; // Faces tTetraArray m_tetras; // Tetras + btAlignedObjectArray m_tetraScratches; + btAlignedObjectArray m_tetraScratchesTn; tAnchorArray m_anchors; // Anchors tRContactArray m_rcontacts; // Rigid contacts + btAlignedObjectArray m_nodeRigidContacts; + btAlignedObjectArray m_faceNodeContacts; + btAlignedObjectArray m_faceRigidContacts; tSContactArray m_scontacts; // Soft contacts tJointArray m_joints; // Joints tMaterialArray m_materials; // Materials @@ -719,6 +787,9 @@ public: btDbvt m_cdbvt; // Clusters tree tClusterArray m_clusters; // Clusters btScalar m_dampingCoefficient; // Damping Coefficient + + btAlignedObjectArray m_renderNodesInterpolationWeights; + btAlignedObjectArray > m_renderNodesParents; btAlignedObjectArray m_clusterConnectivity; //cluster connectivity, for self-collision @@ -1012,6 +1083,7 @@ public: void initializeFaceTree(); btVector3 evaluateCom() const; bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, const Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); @@ -1029,7 +1101,9 @@ public: void setSpringStiffness(btScalar k); void initializeDmInverse(); void updateDeformation(); + void advanceDeformation(); void applyForces(); + void interpolateRenderMesh(); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti); @@ -1042,8 +1116,6 @@ public: ///fills the dataBuffer and returns the struct name (and 0 on failure) virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; - - //virtual void serializeSingleObject(class btSerializer* serializer) const; }; #endif //_BT_SOFT_BODY_H diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index d9bd513ad..3aafe23d2 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -20,11 +20,13 @@ subject to the following restrictions: #include #include #include +#include #include "btSoftBodyHelpers.h" #include "LinearMath/btConvexHull.h" #include "LinearMath/btConvexHullComputer.h" +#include +#include -// static void drawVertex(btIDebugDraw* idraw, const btVector3& x, btScalar s, const btVector3& c) { @@ -1232,6 +1234,8 @@ if(face&&face[0]) } } psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Links: %u\r\n", psb->m_links.size()); printf("Faces: %u\r\n", psb->m_faces.size()); @@ -1321,7 +1325,12 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, psb->appendLink(ni[2], ni[3], 0, true); } } + + + generateBoundaryFaces(psb); psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Links: %u\r\n", psb->m_links.size()); printf("Faces: %u\r\n", psb->m_faces.size()); @@ -1330,3 +1339,243 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, fs.close(); return psb; } + +void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb) +{ + int counter = 0; + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + psb->m_nodes[i].index = counter++; + } + typedef btAlignedObjectArray Index; + btAlignedObjectArray indices; + indices.resize(psb->m_tetras.size()); + for (int i = 0; i < indices.size(); ++i) + { + Index index; + index.push_back(psb->m_tetras[i].m_n[0]->index); + index.push_back(psb->m_tetras[i].m_n[1]->index); + index.push_back(psb->m_tetras[i].m_n[2]->index); + index.push_back(psb->m_tetras[i].m_n[3]->index); + indices[i] = index; + } + + std::map, std::vector > dict; + for (int i = 0; i < indices.size(); ++i) + { + for (int j = 0; j < 4; ++j) + { + std::vector f; + if (j == 0) + { + f.push_back(indices[i][1]); + f.push_back(indices[i][0]); + f.push_back(indices[i][2]); + } + if (j == 1) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][0]); + f.push_back(indices[i][1]); + } + if (j == 2) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][1]); + f.push_back(indices[i][2]); + } + if (j == 3) + { + f.push_back(indices[i][2]); + f.push_back(indices[i][0]); + f.push_back(indices[i][3]); + } + std::vector f_sorted = f; + std::sort(f_sorted.begin(), f_sorted.end()); + if (dict.find(f_sorted) != dict.end()) + { + dict.erase(f_sorted); + } + else + { + dict.insert(std::make_pair(f_sorted, f)); + } + } + } + + for (std::map, std::vector >::iterator it = dict.begin(); it != dict.end(); ++it) + { + std::vector f = it->second; + psb->appendFace(f[0], f[1], f[2]); + } +} + +void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) +{ + std::ofstream fs; + fs.open(filename); + btAssert(fs); + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_x[d]; + } + fs << "\n"; + } + + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << psb->m_faces[i].m_n[n]->index + 1; + } + fs << "\n"; + } + fs.close(); +} + +void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) +{ + std::ifstream fs_read; + fs_read.open(filename); + std::string line; + btVector3 pos; + btAlignedObjectArray > additional_faces; + while (std::getline(fs_read, line)) + { + std::stringstream ss(line); + if (line[0] == 'v') + { + } + else if (line[0] == 'f') + { + ss.ignore(); + int id0, id1, id2; + ss >> id0; + ss >> id1; + ss >> id2; + btAlignedObjectArray new_face; + new_face.push_back(id1); + new_face.push_back(id0); + new_face.push_back(id2); + additional_faces.push_back(new_face); + } + } + fs_read.close(); + + std::ofstream fs_write; + fs_write.open(filename, std::ios_base::app); + for (int i = 0; i < additional_faces.size(); ++i) + { + fs_write << "f"; + for (int n = 0; n < 3; n++) + { + fs_write << " " << additional_faces[i][n]; + } + fs_write << "\n"; + } + fs_write.close(); +} + +// Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex +void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary) +{ + btVector3 vap = p - a; + btVector3 vbp = p - b; + + btVector3 vab = b - a; + btVector3 vac = c - a; + btVector3 vad = d - a; + + btVector3 vbc = c - b; + btVector3 vbd = d - b; + btScalar va6 = (vbp.cross(vbd)).dot(vbc); + btScalar vb6 = (vap.cross(vac)).dot(vad); + btScalar vc6 = (vap.cross(vad)).dot(vab); + btScalar vd6 = (vap.cross(vab)).dot(vac); + btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); + bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6); +} + +void btSoftBodyHelpers::readRenderMeshFromObj(const char* file, btSoftBody* psb) +{ + std::ifstream fs; + fs.open(file); + std::string line; + btVector3 pos; + while (std::getline(fs, line)) + { + std::stringstream ss(line); + if (line[0] == 'v' && line[1] != 't' && line[1] != 'n') + { + ss.ignore(); + for (size_t i = 0; i < 3; i++) + ss >> pos[i]; + btSoftBody::Node n; + n.m_x = pos; + psb->m_renderNodes.push_back(n); + } + else if (line[0] == 'f') + { + ss.ignore(); + int id0, id1, id2; + ss >> id0; + ss >> id1; + ss >> id2; + btSoftBody::Face f; + f.m_n[0] = &psb->m_renderNodes[id0-1]; + f.m_n[1] = &psb->m_renderNodes[id1-1]; + f.m_n[2] = &psb->m_renderNodes[id2-1]; + psb->m_renderFaces.push_back(f); + } + } + fs.close(); +} + +// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights +// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight +void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) +{ + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -1e3; + btAlignedObjectArray optimal_parents; + bool found = false; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + const btSoftBody::Tetra& t = psb->m_tetras[j]; + getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 4; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + if (new_min_bary_weight > min_bary_weight) + { + btAlignedObjectArray parents; + parents.push_back(t.m_n[0]); + parents.push_back(t.m_n[1]); + parents.push_back(t.m_n[2]); + parents.push_back(t.m_n[3]); + optimal_parents = parents; + optimal_bary = bary; + min_bary_weight = new_min_bary_weight; + // stop searching if p is inside the tetrahedron at hand + if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.) + { + break; + } + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + } +} diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index 60e0d6133..d5d7c3648 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -144,8 +144,17 @@ struct btSoftBodyHelpers bool bfacesfromtetras); static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); + static void writeObj(const char* file, const btSoftBody* psb); + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); + + static void readRenderMeshFromObj(const char* file, btSoftBody* psb); + static void interpolateBarycentricWeights(btSoftBody* psb); + + static void generateBoundaryFaces(btSoftBody* psb); + + static void duplicateFaces(const char* filename, const btSoftBody* psb); /// Sort the list of links to move link calculations that are dependent upon earlier /// ones as far as possible away from the calculation of those values /// This tends to make adjacent loop iterations not dependent upon one another, diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 8ce114fa8..ff074eb43 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -29,6 +29,8 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include //for memset #include + +// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, @@ -501,6 +503,77 @@ static inline void ProjectOrigin(const btVector3& a, } } +// +static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t) +{ + btScalar a, f, u, v; + + btVector3 e1 = v1 - v0; + btVector3 e2 = v2 - v0; + btVector3 h = dir.cross(e2); + a = e1.dot(h); + + if (a > -0.00001 && a < 0.00001) + return (false); + + f = btScalar(1) / a; + btVector3 s = origin - v0; + u = f * s.dot(h); + + if (u < 0.0 || u > 1.0) + return (false); + + btVector3 q = s.cross(e1); + v = f * dir.dot(q); + if (v < 0.0 || u + v > 1.0) + return (false); + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * e2.dot(q); + if (t > 0) // ray intersection + return (true); + else // this means that there is a line intersection + // but not a ray intersection + return (false); +} + +static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal) +{ + btVector3 dir = rayEnd - rayStart; + btScalar dir_norm = dir.norm(); + if (dir_norm < SIMD_EPSILON) + return false; + dir.normalize(); + + btScalar t; + + bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); + + if (ret) + { + if (t <= dir_norm) + { + sect = rayStart + dir * t; + } + else + { + ret = false; + } + } + + if (ret) + { + btVector3 n = (p3-p1).cross(p2-p1); + n.safeNormalize(); + if (n.dot(dir) < 0) + normal = n; + else + normal = -n; + } + return ret; +} + + // template static inline T BaryEval(const T& a, @@ -993,11 +1066,11 @@ struct btSoftColliders void DoNode(btSoftBody::Node& n) const { const btScalar m = n.m_im > 0 ? dynmargin : stamargin; - btSoftBody::RContact c; + btSoftBody::DeformableNodeRigidContact c; if (!n.m_battach) { - // check for collision at x_{n+1}^* + // check for collision at x_{n+1}^* as well at x_n if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) { const btScalar ima = n.m_im; @@ -1010,7 +1083,7 @@ struct btSoftColliders btSoftBody::sCti& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_c2 = ima * psb->m_sst.sdt; + c.m_c2 = ima; c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; @@ -1021,7 +1094,7 @@ struct btSoftColliders const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; const btVector3 ra = n.m_x - wtr.getOrigin(); - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; if (m_rigidBody) m_rigidBody->activate(); @@ -1051,8 +1124,7 @@ struct btSoftColliders t1.getX(), t1.getY(), t1.getZ(), t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btScalar dt = psb->m_sst.sdt; - btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); c.m_c0 = rot.transpose() * local_impulse_matrix * rot; c.jacobianData_normal = jacobianData_normal; c.jacobianData_t1 = jacobianData_t1; @@ -1061,7 +1133,7 @@ struct btSoftColliders c.t2 = t2; } } - psb->m_rcontacts.push_back(c); + psb->m_nodeRigidContacts.push_back(c); } } } @@ -1072,6 +1144,107 @@ struct btSoftColliders btScalar dynmargin; btScalar stamargin; }; + + // + // CollideSDF_RDF + // + struct CollideSDF_RDF : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; + DoNode(*face); + } + void DoNode(btSoftBody::Face& f) const + { + btSoftBody::Node* n0 = f.m_n[0]; + btSoftBody::Node* n1 = f.m_n[1]; + btSoftBody::Node* n2 = f.m_n[2]; + + const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; + btSoftBody::DeformableFaceRigidContact c; + btVector3 contact_point; + btVector3 bary; + if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) + { + btScalar ima = n0->m_im + n1->m_im + n2->m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_contactPoint = contact_point; + c.m_bary = bary; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices + c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary; + c.m_face = &f; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf + ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im; + + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = contact_point - wtr.getOrigin(); + + // we do not scale the impulse matrix by dt + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + if (m_rigidBody) + m_rigidBody->activate(); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_faceRigidContacts.push_back(c); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; // // CollideVF_SS // @@ -1082,6 +1255,12 @@ struct btSoftColliders { btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + for (int i = 0; i < 3; ++i) + { + if (face->m_n[i] == node) + continue; + } + btVector3 o = node->m_x; btVector3 p; btScalar d = SIMD_INFINITY; @@ -1121,6 +1300,182 @@ struct btSoftColliders btSoftBody* psb[2]; btScalar mrg; }; + + // + // CollideVF_DD + // + struct CollideVF_DD : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 o = node->m_x; + btVector3 p, normal; + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + btVector3 dir = node->m_q - o; + btScalar l = dir.length(); + if (l < SIMD_EPSILON) + return; + btVector3 rayEnd = dir.normalized() * (l + 2*mrg); + // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face + btVector3 v0 = face->m_n[0]->m_x; + btVector3 v1 = face->m_n[1]->m_x; + btVector3 v2 = face->m_n[2]->m_x; + btVector3 vc = (v0+v1+v2)/3.; + btScalar scale = 1.5; + // enlarge the triangle to catch collision on the edge + btVector3 u0 = vc + (v0-vc)*scale; + btVector3 u1 = vc + (v1-vc)*scale; + btVector3 u2 = vc + (v2-vc)*scale; + bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal); + + if (intersect) + { + p += o; + const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p); + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + const btScalar ms = ma + mb; + if (ms > 0) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = w; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices + c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w; + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf + c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; + c.m_c0 = btScalar(1)/(ma + c.m_imf); + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + }; + + // + // CollideFF_DD + // + struct CollideFF_DD : btDbvt::ICollide + { + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface2->data; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f->m_n[node_id]; + btVector3 o = node->m_x; + btVector3 p, normal; + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + btVector3 dir = node->m_q - o; + btScalar l = dir.length(); + if (l < SIMD_EPSILON) + return; + btVector3 rayEnd = dir.normalized() * (l + 2*mrg); + // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face + btVector3 v0 = face->m_n[0]->m_x; + btVector3 v1 = face->m_n[1]->m_x; + btVector3 v2 = face->m_n[2]->m_x; + btVector3 vc = (v0+v1+v2)/3.; + btScalar scale = 1.5; + // enlarge the triangle to catch collision on the edge + btVector3 u0 = vc + (v0-vc)*scale; + btVector3 u1 = vc + (v1-vc)*scale; + btVector3 u2 = vc + (v2-vc)*scale; + bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal); + + if (intersect) + { + p += o; + const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p); + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + const btScalar ms = ma + mb; + if (ms > 0) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = w; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices + c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w; + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf + c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; + c.m_c0 = btScalar(1)/(ma + c.m_imf); + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + } + void Process(const btDbvtNode* lface1, + const btDbvtNode* lface2) + { + btSoftBody::Face* f = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface2->data; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f->m_n[node_id]; + btVector3 o = node->m_x; + btVector3 p, normal; + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + btVector3 dir = node->m_q - o; + btScalar l = dir.length(); + if (l < SIMD_EPSILON) + return; + btVector3 rayEnd = dir.normalized() * (l + 2*mrg); + // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face + btVector3 v0 = face->m_n[0]->m_x; + btVector3 v1 = face->m_n[1]->m_x; + btVector3 v2 = face->m_n[2]->m_x; + btVector3 vc = (v0+v1+v2)/3.; + btScalar scale = 1.5; + // enlarge the triangle to catch collision on the edge + btVector3 u0 = vc + (v0-vc)*scale; + btVector3 u1 = vc + (v1-vc)*scale; + btVector3 u2 = vc + (v2-vc)*scale; + bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal); + + if (intersect) + { + p += o; + const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p); + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + const btScalar ms = ma + mb; + if (ms > 0) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = w; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices + c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w; + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf + c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; + c.m_c0 = btScalar(1)/(ma + c.m_imf); + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + }; }; #endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 0a08ae409..cc33a6866 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -125,6 +125,13 @@ public: m_el[2] = other.m_el[2]; return *this; } + + SIMD_FORCE_INLINE btMatrix3x3(const btVector3& v0, const btVector3& v1, const btVector3& v2) + { + m_el[0] = v0; + m_el[1] = v1; + m_el[2] = v2; + } #endif